/*!
\file
\brief Traffic Light Agents 
\author Vaclav Smidl.
*/

#ifndef TRAGE_H
#define TRAGE_H


#include <base/participants.h>

using namespace bdm;

class BaseTrafficAgent;

//! detector of traffic variables
class Lane{
public:
	Array<string> inputs;
	Array<string> outputs;
	vec input_distances;
	vec output_distances;
	vec alpha; //size of outputs
	//! percent of queue lenght (for "multiline queues")
	double beta;
	string queue;
	string sg;

	//! function loading info from Setting
	void from_setting(const Setting &set){
		UI::get(inputs,set,"inputs",UI::compulsory);
		UI::get(outputs,set,"outputs",UI::compulsory);
		UI::get(input_distances,set,"input_distances",UI::compulsory);
		UI::get(output_distances,set,"output_distances",UI::compulsory);
		UI::get(alpha,set,"alpha",UI::compulsory);
		UI::get(queue,set,"queue",UI::compulsory);
		UI::get(sg,set,"sg",UI::compulsory);
		UI::get(beta, set, "beta");
	}
};

//! class that operates on a signal group
class LaneHandler {
protected:
	//! pointer to physical lane
	const Lane &lane;
	//! agent pointer
	BaseTrafficAgent *agent;

public:
	//! actual data from the relevant signal group
	vec inputs;
	//! queue length
	double queue; 
	//! description of det_data
	RV rv_inputs;
	//! description of det_data
	RV rv_outputs;
	//! description of det_data
	RV rv_queue;
	//! link from global measured data 
	datalink agentin2input;
	//!
	datalink output2agentout;
	//!
	int queue_index;

public:
	LaneHandler(const Lane &lane0): lane(lane0){
		for (int i=0;i<lane0.inputs.length();i++){
			rv_inputs.add(RV(lane.inputs(i), 2));
		}
		for (int i=0;i<lane0.outputs.length();i++){
			rv_outputs.add(RV(lane.outputs(i), 2)); 
		}
		rv_queue.add(RV(lane.queue, 1)); 
		inputs.set_size(rv_inputs._dsize());
		// KOD PRO TrafficAgentCycleTime
		init();
	}


	void connect_data(BaseTrafficAgent &agent0);

	//! computes expected density in cars/s
	double expected_density();

	//! arbitrary function that computes the need of the signal group for green light in common units (number of waiting cars?)
	double expected_output(double green_time);

	

/********************************
* KOD PRO TrafficAgentCycleTime *
*********************************/
	double last_queue;
	double sum_queue_length;
	double last_sum_queue_length;
	double queue_diff_sum;
	double queue_variance_sum;
	int n_of_queues_in_length;
	int last_n_of_queues_in_length;
	double last_variance;
	double green_time_ratio;
	double Tc;
	double last_Tc;
	double saturated_stream;
	double delta; // ztratovy cas pri prechodu na zelenou
	// udaje opro kalmana
	double queue_avg; //  pruner
	double queue_w;   //  vaha
	double queue_d;   //  rozptyl
	double queue_dd;  //  snizeni verohodnosti odhadu 
	double queue_r;   //  neduveryhodnost mereni = 1

	double Ro_avg;	//  prumer
	double Ro_w;	//  vaha K
	double Ro_d;		//  rozptyl P
	double Ro_dd;		//  snizeni verohodnosti odhadu Q
	double Ro_r;		//  neduveryhodnost mereni = 1 R

	double last_queue_avg;

// pomocne funkce

	

	void init () {
		last_Tc = 0;
		saturated_stream = 0.5;
		queue = 0;
		sum_queue_length = 0;
		last_sum_queue_length = 0;
		n_of_queues_in_length = 0;
		last_n_of_queues_in_length = 0;
		queue_diff_sum = 0;
		queue_variance_sum = 0;
		last_variance = 0;
		delta = 5;
		// KALMAN
		queue_avg = 0;	//  prumer
		queue_w = 1;	//  vaha K
		queue_d = 0.9;		//  rozptyl P
		queue_dd =0.1;		//  snizeni verohodnosti odhadu Q
		queue_r = 9;		//  neduveryhodnost mereni = 1 R

		Ro_avg = 0;	//  prumer
		Ro_w = 1;	//  vaha K
		Ro_d = 0.9;		//  rozptyl P
		Ro_dd =0.1;		//  snizeni verohodnosti odhadu Q
		Ro_r = 9;		//  neduveryhodnost mereni = 1 R

		last_queue_avg = 0;
	}

	/*double abs ( double x ) {
		if ( x > 0 ) return x;
		else return -x;
	}*/

	void echo () {
		//cout << "n:" << n_of_queues_in_length << " " << getQueueName() << " q:" << getQueue() << " aq:" << getAverageQueueLength() << " diff:" << getAverageQueueGrow() << " D:" << getQueueVariance() << endl;
		/*cout << "n:" << n_of_queues_in_length 
		     << " " << getQueueName() 
			 << "LAST aq:" << last_sum_queue_length/last_n_of_queues_in_length 
			 << " aq:" << getAverageQueueLength()
			 << " diff" <<  getAverageQueueLength() - last_sum_queue_length/last_n_of_queues_in_length 
			 << endl;*/
		cout << getQueueName();
		/*if ( last_n_of_queues_in_length> 0 && n_of_queues_in_length > 0 )
			cout << " DIFF: " << getAverageQueueLength() - last_sum_queue_length/last_n_of_queues_in_length;
		else
			cout << " no diff available";*/
		//cout << "\tn " << n_of_queues_in_length;
		//cout << " prijizdejicich aut za 90: " << getRo()*90;
		cout << "\tgtr: " << green_time_ratio;
		cout << "\tRo: " << getRo();
		cout << "\tC: " << green_time_ratio*Tc*saturated_stream - getRo()*Tc << endl; 
		//cout << "\tWT:";
		for ( int i = -2; i <= 2; i++ ) {
			double tc = Tc + i*8;
			cout << tc << "\t";
		}
		cout << endl;
		for ( int i = -2; i <= 2; i++ ) {
			double tc = Tc + i*8;
			cout << (int)(100*getWT(tc)) << "\t";
		}
		//cout << " diff: " << getRo()*Tc - saturated_stream*getGreenTime();
		/*if ( saturated_stream*getGreenTime() >= getAverageQueueLength() )
			cout << " projede vsechno ";
		else
			cout << " zustanou auta ";*/
		cout << endl << endl;
	}
	

	Lane getLane () {
		return lane;
	}

	string getSG () {
		return lane.sg;
	}

	string getQueueName () {
		return lane.queue;
	}

	double getQueue() {
		return queue_avg;
	}

	void addQueueLength ( int ql ) {
		// pricita pouze namerene udaje
		if ( ql >= 0 ) {
			last_queue = queue;
			queue = ql;
			queue_diff_sum += queue - last_queue;
			//queue_variance_sum += abs(queue - last_queue);
		
			sum_queue_length += ql;
			n_of_queues_in_length ++;
			// KALMAN			
			cout << endl << "pridani fronty\t" << queue_avg << "\t" << ql << endl;
			queue_avg += queue_w*( ql - queue_avg );
			queue_d += queue_dd;
			queue_w = queue_d/(queue_d+1);
			queue_d = (1-queue_w)*queue_d;
	
			double Ro = getActualRo();
			Ro_avg += Ro_w*( Ro - Ro_avg );
			Ro_d += Ro_dd;
			Ro_w = Ro_d/(Ro_d+1);
			Ro_d = (1-Ro_w)*Ro_d;
		}
	}

	void resetQueue () {
		last_Tc = Tc;
		//last_variance = getQueueVariance();
		last_n_of_queues_in_length = n_of_queues_in_length;
		last_sum_queue_length = sum_queue_length;
		n_of_queues_in_length = 0;
		sum_queue_length = 0;
		queue_variance_sum = 0;
		queue_diff_sum = 0;
		// kalman
		last_queue_avg = getAverageQueueLength();
	}

	double getAverageQueueLength () {
		// kalmam
		return queue_avg;

		if( n_of_queues_in_length > 0 )
			return sum_queue_length / n_of_queues_in_length;
		else
			return 0.0;
	}

	double getLastAverageQueueLength () {
		if( last_n_of_queues_in_length > 0 )
			return last_sum_queue_length / last_n_of_queues_in_length;
		else
			return 0.0;
	}

	/*double getAverageQueueGrow () {
		return queue_diff_sum / n_of_queues_in_length;
	}*/
	
	/*double getQueueVariance () {
		return queue_variance_sum / n_of_queues_in_length;
	}*/

	double getGreenTime ( double tc ) { return green_time_ratio * tc - delta; }	
	double getGreenTime () { return getGreenTime( Tc ); }	
	
	double getRedTime ( double tc ) { return (1-green_time_ratio) * tc;	}
	double getRedTime () { return getRedTime( Tc ); }	

	double getRelativeQueueDiff () {
		if ( last_n_of_queues_in_length > 0 && n_of_queues_in_length > 0 ) {
			
			return getAverageQueueLength()/(Tc*(1-green_time_ratio)) - (last_sum_queue_length/last_n_of_queues_in_length)/(last_Tc*(1-green_time_ratio));
		}
		else
			return 0;
	}

	// hodnotici funkce - suma cekaciho casu aut za 10h
	double getWT ( double  Tc ) {
	  double T = 36000; // celkovy cas 10h
	  double Ro = getRo();
	  double Gr = green_time_ratio;
	  double ss = saturated_stream;
	  double WT = 0;
	  double q = 0;  // zacina s nulovou frontou ?
	  double ti = 0;
	  double sumq = q;
	  while ( ti < T ) {
		// ve fronte stoji vic aut nez je schopno odjet za zelenou
		if ( q > 0.5*ss*Tc ) {
		  WT += 0.5*ss*(Tc*Gr - delta)*(Tc*Gr - delta);
		}
		else {

		  WT += 0.5*q*(Tc*Gr - delta);
		}

		if ( (Tc*Gr - delta)*ss < q )
		  q -= (Tc*Gr - delta)*ss;
		else
		  q = 0;

		WT += q * Tc; // zbytek fronty ceka cely cyklus
		// cekani vozidel, ktera prijela za pocitany cyklus
		if ( q > 0 ) {
		  //pokud je fronta > 0, auta cekaji prumerne polovinu delky cyklu
		  WT += Ro*Tc*0.5*Tc;
		}
		else {
		  // pokud je fronta = 0 cekaji pouze auta, ktera prijela na cervenou (p=(1-Gr)) 0.5Tc(1-Gr)
		  WT += Ro*(1-Gr)*Tc*0.5*(1-Gr)*Tc;
		}
		// fronta se zvetsi o Ro*Tc
		q += Ro*Tc;
		sumq += Ro*Tc;
		ti += Tc;
	  }

	  //return WT;
	  if ( sumq > 0 )
		return 100*(WT/(0.5*T*sumq));
	  else
	  return 0;
	}

	//double getWT ( double  tc ) {
	//	double T = ;
	//	double WT = 0;
	//	double cWT = 0;
	//	double q = 0;
	//	double cq = 0;
	//	double sum_t;
	//	double car_part = 0.1;
	//	double Ro = getRo();
	//	double dt = car_part/Ro;
	//	while ( sum_t < T ) {
	//		if ( q < 0 )
	//			q = 0;
	//		WT += q * dt;
	//		// prirustek fronty za dt
	//		q += car_part;
	//		// zelena - auta projedou
	//		if ( (sum_t - tc*(int)(sum_t/tc)) > delta && )
	//			q -= saturated_stream*dt;
	//		sum_t += dt;
	//	}
	//}
	// Odhad hustoty ( auto/sec )
	double getActualRo () {
		// pokud delka fronty mensi nez pocet aut ktere odjedou za zelenou
		if ( saturated_stream*getGreenTime() >= getAverageQueueLength() ) {
			// pocet aut, ktera prijela pri cervene
			return getAverageQueueLength()/getRedTime();
		}
		else {
			//fronta se nevynuluje
			// pocet aut = to co stihlo projet na zelenou + o co narostla fronta
			// doba pocet_cyklu*90
			//double T = n_of_queues_in_length*90;
			//double cars_per_green = T*(getGreenTime()/Tc)*saturated_stream;
			//return (cars_per_green + queue_diff_sum)/T;
			double T = 90;
			double cars_per_green = T*(getGreenTime()/Tc)*saturated_stream;
			return (cars_per_green + (getLastAverageQueueLength()-getAverageQueueLength()))/T;
		}
	}

	double getRo() {
		return Ro_avg;
	}
	
	

	/***************************************
	* KONEC KODU PRO TrafficAgentCycleTime *
	****************************************/
};

/*!
\brief  Basic Traffic Agent

*/
class BaseTrafficAgent : public Participant {

LOG_LEVEL(BaseTrafficAgent,logdata);
public:
	//! Signal Groups
	Array<Lane> lanes;

	Array<LaneHandler*> lanehs;

	//!data from messages
	vec inputs;

	//! decription of msg_data
	RV rv_inputs;

	//! data to broadcast
	vec outputs;

	//! description of broadcast dataind
	RV rv_outputs;

	vec queues;

	//! description of queues
	RV rv_queues;

	//! 
	vec green_starts;

	//!
	vec green_times;

	//!
	Array<string> green_names;

	//!
	Array<string> stage_names;

	//!
	vec stage_times;





	//! datalink from DS to input variables
	datalink ds2inputs;

	//! datalink from DS to output variables
	datalink ds2queues;

	//! action description
	RV rv_action;

	datalink_part action2ds;

	Array<string> neighbours;

	Array<RV> rv_neighbours_out;

	Array<datalink> output2neighbour;

	//! simulator's step length in seconds
	static const int step_length=90;

	//! lenght of cycle in seconds
	static const int cycle_length=80;



public:
	void validate(){

		lanehs.set_length(lanes.length());
		for (int l=0; l<lanes.length(); l++){
			lanehs(l) = new LaneHandler(lanes(l));

			rv_inputs.add(lanehs(l)->rv_inputs);
			rv_outputs.add(lanehs(l)->rv_outputs);
			rv_queues.add(lanehs(l)->rv_queue);
		}
		inputs.set_size(rv_inputs._dsize());
		outputs.set_size(rv_outputs._dsize());
		queues.set_size(rv_queues._dsize());

		for (int l=0; l<lanes.length(); l++){
			lanehs(l)->connect_data(*this);
		}

		//for -- rv_outputs -- 
		// TODO vybrat rv pro sousedy 
		rv_neighbours_out.set_length(neighbours.length());
		output2neighbour.set_length(neighbours.length());

		for (int i=0; i<neighbours.length(); i++){
			for (int r=0; r<rv_outputs.length(); r++){
				int str_pos = rv_outputs.name(r).compare(neighbours(i));
				if (str_pos>(int)neighbours(i).length()){
					rv_neighbours_out(i).add(rv_outputs.subselect(vec_1(r)));
				}
			}
			// connect datasource
			output2neighbour(i).set_connection(rv_neighbours_out(i), rv_outputs);
		}

		// lanehs knows RVS
		// write internal checks if all was loaded OK

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

		if (what=="new_stable_state"){ // 
			// field data 
			// extract decription of teh received datavector
			shared_ptr<RV> rv=UI::build<RV>(msg,"rv",UI::compulsory);
			// find if it is needed
			ivec ind=rv->dataind(rv_inputs); // position of rv in in_rv;
			if (ind.length()>0){ //data are interesting
				vec dt;
				UI::get(dt, msg, "value",UI::compulsory); // get data
				set_subvector(inputs, ind,  dt); //check size?
			}				
		} else {
			Participant::receive(msg);
		}
	}
	void log_register(logger &L, const string &prefix){
		root::log_register ( L, prefix );
		if ( log_level[logdata]){
			L.add_vector ( log_level, logdata, RV ( 1 ), prefix );	
		}
	}
	void log_write() const {
		if (log_level[logdata]){
			log_level.store(logdata, inputs);
		}
	}

	void broadcast(Setting& set){
		// broadcast data to all neighbours
		for (int i=0; i<neighbours.length(); i++){
			Setting &msg =set.add(Setting::TypeGroup);

			// if...
			// copy from create message
			// create msg with fields {to=..., what=data, rv=..., value = ...}
			UI::save ( neighbours(i), msg, "to");
			UI::save ( (string)"new_stable_state", msg, "what");
			UI::save ( &(rv_neighbours_out(i)), msg, "rv");
			UI::save( output2neighbour(i).pushdown(outputs), msg, "value");
		}

	}
	void adapt(const vec &glob_dt){
		// copy data from global vector to sSGHandlers
		ds2inputs.filldown(glob_dt, inputs);
		//copy data from neighbours
		ds2queues.filldown(glob_dt, queues);
		// copy sg_length ... and others...
	}
	void act(vec &glob_ut){
		vec action; // trivial stuff
		action2ds.filldown(action,glob_ut);
	}

	void ds_register(const DS &ds){
		//register ds2output
		ds2inputs.set_connection(rv_inputs, ds._drv());
		ds2queues.set_connection(rv_queues, ds._drv());

		inputs.set_size(rv_inputs._dsize());
		action2ds.set_connection( ds._urv(), rv_action);

	}

	void from_setting(const Setting &set);
};
UIREGISTER(BaseTrafficAgent);

#endif //TRAGE_H