#include "QuadraticMinimalizator.h"

class TrafficAgentLQ : public BaseTrafficAgent {
protected:
	mat A, B, Q, R;
	static const int T = 90;
	int L;
	int nOfBroadcast;
	QuadraticMinimalizator * minimizer;
	//map<string, int> queueIndex;
public:
	// FUNKCE VOLANE V main_loop NA ZACATKU
	
	void validate (){
		L = 90;
		BaseTrafficAgent::validate();

		rv_action = RV("Tc",1);
		rv_action.add( RV( stage_names, ones_i(stage_names.length()) ) );

		for ( int i=0; i<lanehs.length(); i ++ ) {
			unsigned int index = find_index( green_names, name + "_" + lanehs(i)->getSG() );
			lanehs(i)->green_time_ratio = green_times( index );
		}
		
		Q = diag( ones( queues.length()) );
		R = "1";
		A = diag( ones( queues.length()) );
		B = ones( queues.length(), 1);

		minimizer = new QuadraticMinimalizator(A,B,Q,R);
		
		cout << "HELLO! " << name << " IS HERE!" << endl;
		/*
		cout << "inputs " << inputs.length() << endl;		
		cout << "queues " << queues.length() << endl;
		cout << "rv_inputs" << rv_inputs.length() << endl << rv_inputs << endl;
		cout << "rv_queues" << rv_queues.length() << endl << rv_queues << endl;
		cout << "lanehs " << lanehs.length() << endl << endl;
		cout << "A B Q R " << endl << A << endl << B << endl << Q << endl << R << endl << endl;
		*/
	}

	// FUNKCE VOLANE V main_loop V KAZDEM CYKLU
	void adapt (const vec &glob_dt) {
		for ( int i=0; i<lanehs.length(); i ++ ) {
			//lanehs(i)->inputs( 0 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) );
			//lanehs(i)->inputs( 1 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) + 1 );
			lanehs(i)->queue = queues( find_index( rv_queues, lanehs(i)->getQueueName() ) );
			lanehs(i)->countAvgs();
		}
		
		// jeden clen matice B
		for ( int i=0; i<queues.length(); i ++ ) {
			B(i,0) = 0.5 * lanehs(i)->green_time_ratio;
		}

		
		
		BaseTrafficAgent::adapt(glob_dt);
	}

	
	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 = 0.5 * lanehs(i)->green_time_ratio * lanehs(i)->getLane().alpha(j);
						sendToAll<double>(set, coefStr.str(), val);
					}
				}				
			}
		}
		nOfBroadcast ++;
	}

	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;
			//cout << name << " received coefs " << inputName << " " << what << endl << val <<endl;
		}
	}

	void act (vec &glob_ut){
		//cout <<"A act"<<endl<<A<<endl;
		B = B * T * L;
		minimizer = new QuadraticMinimalizator(A, B, Q, R);
		vec computedQueues = getComputedQueues();
		vec res = minimizer->minimize(computedQueues, 100);
		//cout << "minimalization res " << name << endl << res << endl << endl;
		delete minimizer;
		int Tc = 80;
		vec action;
		action.set_size(rv_action._dsize());
		action(find_index(rv_action, "Tc")) = Tc;	
		int st;
		int stage_time_sum = 0;  // soucet delky fazi
		action(find_index(rv_action, "Tc")) = Tc;		
		for ( int i =0; i < stage_names.length(); i ++) {			
			if ( (i+1) < stage_names.length() ) {
				st = round(((double)(stage_times(i)*Tc))/80.0);
				stage_time_sum += st;
				action(find_index(rv_action, stage_names(i))) = st;
			}
			else { // dopocitani posledni faze - oprava zaokrouhlovaci chyby
				action(find_index(rv_action, stage_names(i))) = Tc - stage_time_sum;
			}
		}
		action2ds.filldown(action,glob_ut);
	}

	void setCycleTime( int Tc, vec &glob_ut ) {
		
	}


	
// POMOCNE FCE
	vec getComputedQueues() {
		vec q = zeros( queues.length() );
		for ( int i = 0; i < lanehs.length(); i++ ) {
			q(i) = lanehs(i)->queue_avg;
		}
		return q;
	}

	template <class T> void sendToAll( Setting &set, string msgCode, T value) {
		for ( int i = 0; i < neighbours.length(); i ++  ) {	
			Setting & msg = set.add(Setting::TypeGroup);
			UI::save( neighbours(i), msg, "to" );
			UI::save (name,msg,"from");
			UI::save( msgCode, msg, "what" );
			UI::save( value, msg, "value" );
		}
	}

	void printVector ( RV rv_vector, vec vector, string description ) {
		cout << endl << description << " " << name << endl;
		int k = 0;
		for ( int i = 0; i < rv_vector.length(); i ++ ) {
			cout << rv_vector.name(i) << " : ";
			for ( int j = 0; j < rv_vector.size(i); j ++ ) {				
				cout << vector(k) << " ";
				k ++;
			}
			cout << endl;
		}
		cout << endl;
	}

	unsigned int find_index ( RV rv_vector, string index_string ) {
		for ( unsigned int i = 0; i < rv_vector.length(); i ++) {
			if ( rv_vector.name(i) == index_string )
				return i;
		}
		return -1;
	}

	unsigned int find_index ( Array <string> arr, string index_string ) {
		for ( unsigned int i = 0; i < arr.length(); i ++) {
			if ( arr(i) == index_string )
				return i;
		}
		return -1;
	}

};

UIREGISTER(TrafficAgentLQ);