#pragma once
#include "BaseTrafficAgentCt.h"
#include "QuadraticMinimalizator.h"

class TrafficAgentLQMaster :public BaseTrafficAgentCt {
protected:
	vec x;
	QuadraticMinimalizator * minimizer;
public:
	void adapt(const vec &glob_dt) {
		BaseTrafficAgentCt::adapt(glob_dt);
		
		// FUNKCE VOLANE V main_loop V KAZDEM CYKLU
		
		BaseTrafficAgentCt::adapt(glob_dt);

		Q = 100 * diag( ones( queues.length()) ) + diag(computedQueues);
		R = "0.0000001";
		A = diag( ones( queues.length()) );
		B = zeros( queues.length(), 1);	

		// jeden clen matice B
		for ( int i=0; i<queues.length(); i ++ ) {
			B(i,0) = - s_flow(i) * lanehs(i)->green_time_ratio;
		}
		
		x = getComputedQueues();		
		I0 = (mat)inputs;

	}

	void receive(const Setting& msg){
		string what;
		UI::get(what, msg, "what", UI::compulsory);	

		cout << endl << endl;		

		if ( what.substr(0,9) == "QueueData" ) {
			vec val;
			UI::get(val, msg, "value");
			QueueData qd;
			qd.fromMessageCode(what);
			qd.fromVec(val);			
			int ind = findQueueDataById( qd.id );
			if ( ind >= 0 ) {
				queueData(ind) = qd;
			} else {
				ind = queueData.length();
				queueData.set_length( ind+1, true );
				queueData(ind) = qd;
			}				
		}

		
	}

	void setDummy() {
		for ( int i = 0; i < queueData.length(); i ++ ) {
			for ( int j = 0; j < queueData(i).outputs.length(); j ++ ) {
				for ( int k = 0; k < queueData.length(); k ++ ) {
					if ( queueData(k).idInput == queueData(i).outputs(j) ) {
						queueData(k).dummy = false;
					}
				}
			}
		}
	}

	void setx() {
		x = ones( queueData.length() + 1 );
		for ( int i = 0; i < queueData.length(); i ++ ) {
			x(i) = queueData(i).queue;
		}
	}

	// az po setDummy !!!
	void setI0(){
		I0 = zeros(queueData.length(),1);
		for ( int i = 0; i < queueData.length(); i ++ ) {
			if ( queueData(i).dummy )
				I0(i,0) = queueData(i).input;
		}
	}

	// az po setI0 !!!
	void setA() {
		A = concat_vertical(
				concat_horizontal( diag( ones( queueData.length() ) ), I0	),
				concat_horizontal( zeros( 1, queueData.length() ) , ones(1,1) )
			);
	}


	void setB() {
		B = zeros(x.length(), 1);
		//cout << "B' 1 [";
		for ( int i = 0; i < queueData.length(); i ++ ) {
			B(i,0) = B(i,0) - queueData(i).ss() * queueData(i).green;
			//cout << B(i,0) << " ";
		}
		for ( int i = 0; i < queueData.length(); i ++ ) {
			//B(i,0) = B(i,0) - queueData(i).ss() * queueData(i).green;			
			for ( int j = 0; j < queueData(i).outputs.length(); j ++ ) {
				for ( int k = 0; k < queueData.length(); k ++ ) {
					if ( queueData(k).idInput == queueData(i).outputs(j) ) {
						B(k,0) = B(k,0) + queueData(i).alphas(j) * queueData(i).ss() * queueData(i).green;
					}
				}
			}
			
		}
		//cout << endl << "B' 2 " << endl << B.transpose() << endl;
	}

	void setQ() {
		Q = 1000 * diag( ones(x.length()) );
		for ( int i = 0; i < queueData.length(); i ++ ) {
			Q(i,i) += queueData(i).queue * 100;
			Q(i,i) += queueData(i).input * 100;
		}
	}

	double computeTc() {
		B = B * T;
		//vec computedQueues = getComputedQueues();
		minimizer = new QuadraticMinimalizator(A, B, Q, R);		
		mat L_mat = minimizer->L( 200);
		vec u_vec = L_mat * x;
		double u = u_vec(0);
		double Tc_computed = L / (1 - u);
		delete minimizer;
		/*
		if ( u >= 1 || Tc_computed > Tc_max ) {
			cout << endl << "Tc error : " << Tc_computed << " setting to max" << endl;
			Tc_computed = Tc_max;
		}
		if ( Tc_computed < Tc_min ) {
			cout << endl << "Tc error : " << Tc_computed << " setting to min" << endl;
			Tc_computed = Tc_min;	
		}
		*/
		return Tc_computed;
	}


	

	void act (vec &glob_ut){		
		BaseTrafficAgentCt::act(glob_ut);
		cout << endl;
		cout << endl;
		cout << "queues ";
		for ( int i = 0; i < queueData.length(); i ++ ) {
			cout << queueData(i).queue << " ";
		}	
		cout << endl;
		cout << endl;
		cout << "inputs ";
		for ( int i = 0; i < queueData.length(); i ++ ) {
			cout << queueData(i).input << " ";
		}	
		cout << endl;
		

		setDummy();
		setx();
		setI0();
		setA();
		setB();
		setQ();
		
		R = "0.001";
	
		cout << endl << endl;

		Tc_computed = computeTc();
		cout << "L  = " << L << endl;
		cout << "TC = " << Tc_computed << endl;

		//cout << "A" << endl << A << endl << endl;
		//cout << "I0'" << endl << I0.transpose() << endl << endl;
		

		cout << endl << endl;

		
		
		
		cout << endl << name << " ACT" << endl;

		looper ++;
	}

};
UIREGISTER(TrafficAgentLQMaster);