#include "BaseTrafficAgentCt.h"
#include "QuadraticMinimalizator.h"

class TrafficAgentLQ : public BaseTrafficAgentCt {
protected:
	QuadraticMinimalizator * minimizer;
	
public:
	// FUNKCE VOLANE V main_loop NA ZACATKU
	
	void from_setting(const Setting& set) {
		BaseTrafficAgentCt::from_setting(set);		
	}

	void validate (){
		BaseTrafficAgentCt::validate();					
		
		cout << "HELLO! " << name << " IS HERE!" << endl;
	}

	// FUNKCE VOLANE V main_loop V KAZDEM CYKLU
	void adapt (const vec &glob_dt) {	
		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;
		}

		
	}

	
	void broadcast( Setting &set ) {
		string dummyDet = "DUMMY_DET";
		if ( nOfBroadcast == 0 ) {			
			for ( int i = 0; i < lanehs.length(); i++ ) {		
				for ( int j = 0; j < lanehs(i)->rv_outputs.length(); j ++ ) {
					if ( dummyDet.compare(lanehs(i)->rv_outputs.name(j)) != 0 ) {						
						stringstream coefStr;
						coefStr << "coef" << lanehs(i)->rv_outputs.name(j);
						double val = s_flow(i) * lanehs(i)->green_time_ratio * lanehs(i)->getLane().alpha(j);
						sendToAll<double>(set, coefStr.str(), val);
					}
				}				
			}
		}


		if ( nOfBroadcast == 1 ) {
			broadcastTc(set);
		}
		
		nOfBroadcast ++;
	}

	double votingWeight() {
		//vec computedQueues = getComputedQueues();
		double qsum = 0;
		for ( int i = 0; i < computedQueues.length(); i++ ) {
			qsum += computedQueues(i);
		}
		if ( qsum > 1 )
			return qsum;
		else
			return 1;
	}

	void broadcastTc( Setting & set) {
		// posli idealni tc
		Tc_computed = computeTc();
		stringstream tcstr;
		tcstr << "timecycle" << name;
		
		// posli vahu hlasu (suma front)
		//vec computedQueues = getComputedQueues();	
		
		vec tc_vec = "0.0 0.0";
		tc_vec(0) = Tc_computed;
		tc_vec(1) = votingWeight();
		// posilam vektor [tc, w]
		sendToAll<vec>(set, tcstr.str(), tc_vec);
	}

	void receive(const Setting& msg){
		string what;
		UI::get(what, msg, "what", UI::compulsory);		
		if ( what.substr(0,4) == "coef" ) {
			string inputName = what.substr(4,what.length()-4);
			int i = find_index(rv_inputs, inputName);
			double val;
			UI::get(val, msg, "value");
			B(i,0) = B(i,0) + val;
		}

		if ( what.substr(0,9) == "timecycle" ) {
			vec val;
			UI::get(val, msg, "value");
			cout << "receiving Tc " << val << endl;
			Tc_sum += val(0) * val(1);
			Tc_w_sum += val(1);			
		}

	}

	void act (vec &glob_ut){		
		//vec computedQueues = getComputedQueues();
		cout << endl << name << " ACT" << endl;
		cout << "Tc computed = " << Tc_computed << endl;
		cout << "queues = " << round( computedQueues )<< endl;			
		double w = votingWeight();
		Tc_sum += Tc_computed * w;
		Tc_w_sum += w;
		int Tc = (int)round( Tc_sum / Tc_w_sum );		
		setCycleTime(Tc, glob_ut);
		looper ++;
	}
// VYPOCETNI FUNKCE

	// saturovany tok pruhu i
	double s_flow(int i) {
		double min_flow = 0.3;
		double max_flow = 0.5;
		double input = 0;
		if ( 2*i < inputs.length() )
			input = inputs(2*i);
		
		vec q = getComputedQueues();
		double flow = q(i);
		if ( input > 0 )
			flow += input;
		flow = flow/T;
		if ( flow > max_flow )
			return max_flow;
		if ( flow < min_flow )
			return min_flow;
		else
			return flow;
	}

	double computeTc() {
		B = B * T;
		//vec computedQueues = getComputedQueues();
		minimizer = new QuadraticMinimalizator(A, B, Q, R);		
		mat L_mat = minimizer->L( 100);
		vec u_vec = L_mat * computedQueues;
		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;
	}

	



	


// KONEC
	~TrafficAgentLQ() {
		
	}

};

UIREGISTER(TrafficAgentLQ);