Index: /library/bdm/design/lq_ctrl.h
===================================================================
--- /library/bdm/design/lq_ctrl.h (revision 1218)
+++ /library/bdm/design/lq_ctrl.h (revision 1221)
@@ -7,34 +7,36 @@
 //! left matrix division with upper triangular matrix using Gauss elimination
 //! fast variant of inv(A) * B where A is UT matrix, returns result in C and true if no error
-bool ldutg(mat &utA, mat &mB, mat &mC){
-	int utsize = utA.rows();
-	
-	if(utsize != utA.cols()) return false; //utA not square
-	if(utsize != mB.rows()) return false; //incorrect mB size
-	
-	int mbcol = mB.cols();
-	
-	mC.set_size(utsize, mbcol);
-	
+inline bool ldutg(mat &_utA, mat &_mB, mat &_mC){
+	int utsize = _utA.rows();
+	
+	if(utsize != _utA.cols()) return false; //utA not square
+	if(utsize != _mB.rows()) return false; //incorrect mB size
+	
+	int mbcol = _mB.cols();	
 	int totsize = utsize + mbcol;
 	int sqs = utsize*utsize;
-	int i, j, k;
-	
-	double pvt;
-	
+	int i, j, k;	
+	
+	double pvt;	
 	double tmp[utsize*totsize];
+	double *utA = _utA._data();
+	double *mB = _mB._data();
+			
+	_mC.set_size(utsize, mbcol);
+	
+	double *mC = _mC._data();
 	
 	//fill tmp array
 	for(i = 0; i < sqs; i++) 
-		tmp[i] = utA._data()[i];
+		tmp[i] = utA[i];
 	for(i = sqs; i < utsize*totsize; i++)   			
-		tmp[i] = mB._data()[i-sqs];		    
+		tmp[i] = mB[i-sqs];		    
 		   
 	for(i = utsize-1; i >= 0; i--){  //Gauss elimination
 		pvt = tmp[i + utsize*i];    //get pivot 
-		for(j = i; j < totsize; j++) tmp[i + utsize*j] /= pvt;	//normalize row 			           
+		for(j = utsize; j < totsize; j++) tmp[i + utsize*j] /= pvt;	//normalize row - only part on the right 			           
 		for(j = 0;j < i; j++){ //subtract normalized row from above ones
 			pvt = tmp[j + utsize*i]; //get pivot
-			for(k = 0; k < totsize; k++)	
+			for(k = utsize; k < totsize; k++) //goes from utsize - do not need make matrix on the left = I	
 				tmp[j + utsize*k] -= pvt * tmp[i + utsize*k]; //create zero col above           				  
 		}
@@ -43,5 +45,5 @@
 		    
 	for(i = 0; i < utsize*mbcol; i++)
-		mC._data()[i] = tmp[i+sqs];		
+		mC[i] = tmp[i+sqs];		
 	
 	return true;
@@ -69,5 +71,4 @@
 		//! model of evolutin
 		Array<linfnEx> Models;
-//		RV model_rv_ret;
 
 		//! control law rv is public member in Controller class	 
@@ -106,8 +107,5 @@
 			curtime--;
 			generateLmat(curtime);
-		}
-		//cout << "time " << curtime << endl;
-		//report time 0 reached - LQG designing complete
-		//if(curtime == 0) cout << "time 0 reached" << endl;
+		}		
 	}
     //! returns designed control action
@@ -170,22 +168,5 @@
 				if(finding1(j) <= (-1) ) return; //rv element is not part of admissible rvs => error
 			}			
-		}		
-		
-		//NOT!!! (2) test Models array rv_ret - each array element's rv_ret must be unique (except const 1)
-		//RV unique_rv_ret;
-		//unique_rv_ret = Models(0).rv_ret;
-
-		//for(i = 1; i < Models.length(); i++){
-		//	finding1 = Models(i).rv_ret.findself(unique_rv_ret);
-
-		//	for(j = 0; j < finding1.size(); j++){								
-		//		if(Models(i).rv_ret.name(j) == "1") continue; // except const 1
-
-		//		bdm_assert((finding1(j) == (-1) ), "VALIDATION FAILED! Models functions result RV (rv_ret) must be unique.");
-		//		if(finding1(j) != (-1) ) return; //rv_ret element not unique
-		//	}
-
-		//	unique_rv_ret.add(Models(i).rv_ret);
-		//}		
+		}			
 		
 		// (3) test Losses array - acceptable rv is only part/composition of LQG_universal::rv, LQG_universal::rvc, Models rv_ret and const 1
@@ -215,21 +196,5 @@
 
 private:
-	RV trs_crv;
-
-	//! compute complete RV from all of RVs used in Losses array
-	/*RV getCompleteRV() {
-		RV cRv; //complete RV
-
-		//cRv has form [rv, "other", 1]
-		cRv = rv; //add rv
-
-		//add "other"
-		for(int i = 0; i < Losses.size(); i++)
-			cRv.add(Losses(i).rv);
-
-		cRv.add(rvOne); //add 1
-
-		return cRv;
-	}*/
+	RV trs_crv;	
 
 	mat getMatRow (quadraticfn sourceQfn){ //returns row of matrixes crated from quadratic function
@@ -241,5 +206,5 @@
 		//set data in tmpMatRow - other times then current replace using model
 		RV tmpQrv = sourceQfn.rv;
-//cout << "Qrv " << tmpQrv << endl << "total crv " << trs_crv << endl;
+
 		ivec j_vec(1);
 		vec copycol;
@@ -247,9 +212,8 @@
 		for(int j = 0; j < tmpQrv.length(); j++){			
 			j_vec(0) = j;
-//cout << "matrow\n" << tmpMatRow << endl;
-
-			if( /*(tmpQrv.time(j) == 0) &&*/ (sum(tmpQrv(j_vec).findself(trs_crv)) > (-1)) ) {//sum is only formal, summed vector is in fact scalar
+
+			if( (sum(tmpQrv(j_vec).findself(trs_crv)) > (-1)) ) {//sum is only formal, summed vector is in fact scalar
 				//jth element of tmpQrv is also element of trs_crv with a proper time
-//cout << "copy" << endl;
+
 				ivec copytarget = (tmpQrv(j_vec)).dataind(trs_crv); //get target column position in tmpMatRow
 				ivec copysource = (tmpQrv(j_vec)).dataind(tmpQrv); //get source column position in Losses(i).Q
@@ -261,31 +225,7 @@
 				}					
 			}
-			else {
-//cout << "USING MODEL on " << tmpQrv(j,j) << endl;
-//cout << "model" << endl;
-				//jth tmpQrv element is not in trs_crv -> using Model to teplace it
-				// = (tmpQrv(j_vec)).findself(tmpQrv); //get source column position in Losses(i).Q
-
-				//int selectedModel = -1;
-
-				//int k;
-				////find first usable replacement in Model				
-				//for(k = 0; k < Models.size(); k++){							
-				//	if( sum((tmpQrv(j_vec)).findself(Models(k).rv_ret)) > (-1) ){
-				//	//tTODO is tmpQrv(j) in kth Models RV
-				//		//if( (Models(k)).rv_ret.findself_ids(trs_crv) ){//???????????????????
-				//		// is kth Models rv_ret subset of trs_crv
-
-				//		selectedModel = k;
-				//		break;
-
-				//		//}
-				//	}
-				//}
+			else {				
+				//!model is model_rv_ret = sum(Array<linfn>) = sum( A1*rv + B1 + ... + An*rv + Bn)				
 				
-				//!model is model_rv_ret = sum(Array<linfn>) = sum( A1*rv + B1 + ... + An*rv + Bn)
-				
-				//if(selectedModel == -1) {cout << "NO MODEL" << endl;return mat("0");}//ERROR - inconsistent model data;				
-			//!!tTODO!!if(NOT((tmpQrv(j_vec)).findself(model_rv_ret))) {cout << "NO MODEL" << endl;return mat("0");}//ERROR - inconsistent model data;				
 				//use kth Model to convert tmpQrv memeber to trs_crv memeber
 
@@ -319,6 +259,5 @@
 						ivec copytarget = (Models(k)).rv.dataind(trs_crv); //get target column position in tmpMatRow
 												
-						//copy transsubmat into tmpMatRow with adding to current one
-						//	tmpMatRow(new) = tmpMatRow(old) + transsubmat /all in proper indices
+						//copy transsubmat into tmpMatRow with adding to current one						
 						int l;
 						for(l = 0; l < copytarget.size(); l++){					
@@ -352,7 +291,5 @@
 		int i;
 		//used fake quadratic function from tolC matrix
-		quadraticfn fakeQfn;
-
-		//RV pretrs_crv = getCompleteRV(); // crv before transformation based on Losses array
+		quadraticfn fakeQfn;		
 
 		//set proper size of pre_qr matrix
@@ -370,10 +307,10 @@
 			fakeM1.t_plus(1); //RV in time t+1 => necessary use of Model to get RV in time t
 			fakeM1.set_time((RV("1", 1, 1).findself(fakeM1))(0) , 0);
-//cout << fakeM1 << endl;
+
 			fakeQfn.rv = fakeM1;
 			
 			rows += fakeQfn.Q.rows();
 		}
-//cout << "buildpreqr trscrv: " << trs_crv << " of size " << trs_crv.countsize() << endl;
+
 		pre_qr.set_size(rows, trs_crv.countsize()); //(rows, cols)
 		pre_qr.zeros();
@@ -387,12 +324,6 @@
 			//compute row matrix and insert it on proper place in pre_qr
 			tmpMatRow = getMatRow(Losses(i));
-//cout << "tmpMatRow no " << i << endl << tmpMatRow << endl;
+
 			//copy tmpMatRow in pre_qr
-
-		/*cout << "submatrix ( " << rowIndex << ", " << 
-			(rowIndex + rows - 1) << ", 0, " << (trs_crv.countsize() - 1) << ")" << endl;
-		cout << "seting with submatrix of rows " << tmpMatRow.rows() << " and cols " << tmpMatRow.cols() <<
-			"and data " << endl << tmpMatRow << endl;*/
-
 			pre_qr.set_submatrix(rowIndex, (rowIndex + rows - 1), 0, (trs_crv.countsize() - 1), tmpMatRow);  //(int r1, int r2, int c1, int c2, const Mat<  Num_T > &m)
 			rowIndex += rows;
@@ -406,16 +337,9 @@
 			//based on tolC but time must be shifted by one - all implemented in getMatRow method
 				
-			//get matrix row via getMatRow method
-			//cout << "XXXXXXXXXXXXXXX" << endl;
+			//get matrix row via getMatRow method			
 			tmpMatRow = getMatRow(fakeQfn);
-			//cout << tmpMatRow << endl;
-		/*cout << "submatrix ( " << rowIndex << ", " << 
-			(rowIndex + fakeQfn.Q.rows() - 1) << ", 0, " << (trs_crv.countsize() - 1) << ")" << endl;
-		cout << "seting with submatrix of rows " << tmpMatRow.rows() << " and cols " << tmpMatRow.cols() <<
-			"and data " << endl << tmpMatRow << endl;*/
+			
 			pre_qr.set_submatrix(rowIndex, (rowIndex + fakeQfn.Q.rows() - 1), 0, (trs_crv.countsize() - 1), tmpMatRow);  //(int r1, int r2, int c1, int c2, const Mat<  Num_T > &m)		
-		//cout << "NEXT 3" << endl;
-		}
-//cout << "last tmpMatRow  " << endl << tmpMatRow << endl;
+		}
 	}	
 
@@ -488,27 +412,13 @@
 		mat tmpQ;		
 		qr(pre_qr, tmpQ, post_qr);
-//cout << "preQR " << pre_qr << endl << "postQR" << post_qr << endl;
+
 		mat qrA = -get_qr_submatrix(0);		
 		mat qrB = get_qr_submatrix(1);
 		mat qrC = get_qr_submatrix(2);
-//cout << "A " << qrA << "\n B " << qrB << "\n C " << qrC << endl;
-
-		//mat L1 = - inv(qrA)*qrB; 
-		//mat L2;
+
+		//mat L = inv(qrA)*qrB; 		
 		bool invmult = ldutg(qrA, qrB, L);
 		bdm_assert(invmult, "Matrix inversion error!");
-		
-		/*if(L1 != (-L2)){
-			 cout << "qrA" << qrA << endl;
-			 int l;
-			 for(l = 0; l < qrA.rows()*qrA.cols();l++) cout << qrA._data()[l] << " ";
-			 cout << endl << "qrB" << qrB << endl;
-			 for(l = 0; l < qrB.rows()*qrB.cols();l++) cout << qrB._data()[l] << " ";
-			 cout << endl << L1 << endl << (-L2) << endl;
-			}
-		
-		bdm_assert(L1 == (-L2), "Matrix compare error!");
-		L = L1;*/
-				
+						
 		// ldutg is faster matrix inv&mult (like Matlab's \ operator) function than inv
 		// it uses Gauss elimination
@@ -521,36 +431,3 @@
 };
 
-class LQG_recedinghorizon : public LQG_universal {
-protected:
-	//!total_curtime is curtime for total_horizon
-	int total_curtime;
-public:
-	//! LQG_universal::horizon means shorter receding horizon for designing control strategy
-	//! total_horizon is longer total horizon
-	int total_horizon;
-	
-	//!constructor
-	LQG_recedinghorizon() : LQG_universal() {
-			total_horizon = 0;
-			total_curtime = 0;
-		}
-
-	virtual void redesign() {
-		if (total_curtime < total_horizon){
-			for(int i = 0; i < horizon - 1; i++) LQG_universal::redesign();
-			total_curtime++;
-		}		
-	}
-
-	virtual vec ctrlaction ( const vec &cond ) const {
-        //return L * concat(cond, 1.0);
-        return empty_vec;
-    }
-
-    //! register this controller with given datasource under name "name"
-    virtual void log_register ( logger &L, const string &prefix ) { }
-    //! write requested values into the logger
-    virtual void log_write ( ) const { }	
-};
-
 } // namespace
