#pragma once
#include "traffic_agent.h"
#include "QueueData.h"

/**
* Zakladni trida agenta pro nastavovani delky cyklu
*/
class BaseTrafficAgentCt : public BaseTrafficAgent {
protected:
	Array<QueueData> queueData;
	int looper;
	mat A, B, Q, R, I0;
	static const int T = 90;
	double L;
	int nOfBroadcast;
	
	vec computedQueues;
	// intendsities of inputs
	vec inputs_i;

	double Tc_sum; // suma Tc*w
	double Tc_w_sum; // suma vah
	double Tc_computed; // vypoctena hodnota pro Tc
	double Tc_avg; // prumer s exponencialnim zapominanim
	double Tc_w; // vaha pro prumer s exponencialnim zapominanim
	static int const Tc_min = 40;
	static int const Tc_max = 200;

	// pomocne logovaci soubory
	ofstream Tc_log;
	ofstream Q_log;
public:
	// FUNKCE VOLANE V main_loop NA ZACATKU
	
	void from_setting(const Setting& set) {
		BaseTrafficAgent::from_setting(set);
		UI::get(L, set, "lost_time", UI::compulsory);
	}

	void validate (){
		BaseTrafficAgent::validate();
		
		queueData.set_length(queues.length());
		looper = 0;
		Tc_avg = 80;
		Tc_w = 0.2;				
		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 );
		}	
		
		// logovaci soubory
		stringstream Tc_log_name;
		Tc_log_name << name << "_tc_log.dat";
		Tc_log.open(Tc_log_name.str().c_str());

		stringstream Q_log_name;
		Q_log_name << name << "_q_log.dat";
		Q_log.open(Q_log_name.str().c_str());
	}

	// FUNKCE VOLANE V main_loop V KAZDEM CYKLU

	void initQueueData() {
		for ( int i=0; i < queues.length(); i ++ ) {
			queueData(i).id = rv_queues.name(i);
			queueData(i).idInput = lanehs(i)->rv_inputs.name(0);
			if ( queues(i) > 0 )
				queueData(i).queue = queues(i);
			queueData(i).green = lanehs(i)->green_time_ratio;
			
			queueData(i).outputs.set_length( lanehs(i)->rv_outputs.length() );
			for ( int j = 0; j < lanehs(i)->rv_outputs.length(); j ++ ) {
				queueData(i).outputs(j) = lanehs(i)->rv_outputs.name(j);
			}
			queueData(i).alphas.set_length( lanehs(i)->getLane().alpha.length() );
			for ( int j = 0; j < lanehs(i)->getLane().alpha.length(); j ++ ) {
				queueData(i).alphas(j) = lanehs(i)->getLane().alpha(j);
			}
		}
		for ( int i=0; i < queueData.length(); i ++ ) {
			for ( int j = 0; j < rv_inputs.length(); j ++ ) {
				if ( queueData(i).idInput == rv_inputs.name(j) && inputs(2*j) > 0 ) {
					queueData(i).input = inputs(2*j);
				}
			}
		}
	}

	void adapt (const vec &glob_dt) {
		BaseTrafficAgent::adapt(glob_dt);
		

		// compute inputs intensities
		inputs_i = zeros( queues.length() );
		for ( int i = 0; i < lanehs.length(); i++ ) {
			inputs_i(i) = lanehs(i)->inputs(0);
		}

		Tc_sum = 0;
		Tc_w_sum = 0;
		nOfBroadcast = 0;
		
		for ( int i=0; i<queues.length(); i ++ ) {
			// nevim jestli sedej indexy
			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();
			Q_log << lanehs(i)->queue << " "; 
		}

		Q_log << endl;
		computedQueues = getComputedQueues();	

		initQueueData();
	}

	void act (vec &glob_ut){			
		looper ++;
	}


// POMOCNE FCE
	// saturovany tok pruhu i
	double s_flow(int i) {
		double min_flow = 0.5;
		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;
	}

	void setCycleTime( int Tc, vec &glob_ut ) {
		Tc_avg += Tc_w * (Tc - Tc_avg);		
		Tc_log << Tc << "\t" << Tc_avg << endl;
		if ( looper % 5 == 0) {
			Tc = (int)round(Tc_avg);
			cout << endl << endl << "Nastavuji Tc = "<< Tc << endl << endl;
			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 = (int)round(((double)(stage_times(i)*(double)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);
		}
	}

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

	int findQueueDataById( string id ) {
		for ( int i = 0; i < queueData.length(); i ++ ) {
			if ( queueData(i).id == id )
				return i;
		}
		return -1;
	}

	void sendQueueData( Setting &set, QueueData data) {
		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");
			string msgCode = data.getMessageCode();
			UI::save( msgCode, msg, "what" );
			vec value = data.getVec();
			UI::save( value, msg, "value" );
		}
	}

	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 ( 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 ( int i = 0; i < arr.length(); i ++) {
			if ( arr(i) == index_string )
				return i;
		}
		return -1;
	}

	/**
	* Destructor
	*/
	~BaseTrafficAgentCt() {
		Tc_log.close();
		Q_log.close();
	}
};
