#include "traffic_agent.h"
#include <list>

class GreenWaveTrafficAgent : public BaseTrafficAgent {
protected:
	double rating_change;
	int negot_offset;

	RV rv_change_request;
	vec change_request;

	RV rv_recieved_exps;
	vec recieved_exps;

	RV rv_recieved_changes;
	vec recieved_changes;

	//! name of agent, whose change we have accepted
	string accepted_from;

	list<string> seznam;
	
	double car_leaving; //s; how long is 1 car leaving queue

	bool need_exps;
	bool new_stable_state;
	bool send_requests;
	bool final_state;
	//bool reset_negot_offset;

	//! counts all expected cars going from each lane, saves to outputs and rv_outputs
	void expected_cars(/*const string dest*/) {
		double start_time;

		for (int i=0;i<lanes.length();i++) {
			for (int j=0;j<lanes(i).outputs.length();j++) {
				string group = name+"_"+lanes(i).sg;		//e.g. 495_VA
				ivec index=rv_inputs.dataind(RV(group,1));
				vec green_time=inputs(index);

				rv_outputs.add(RV(lanes(i).outputs(j)+"-"+group,3));
				outputs.set_size(rv_outputs._dsize());

				ivec ind = rv_outputs.dataind(RV(lanes(i).outputs(j)+"-"+group,3));

				//Number of cars
				outputs(ind(0))=lanehs(i)->expected_output(green_time(0));	

				start_time = green_starts(sg_index(lanes(i).sg)) + lanes(i).output_distances(j)/VP + planned_offset;
				//first car arrive time
				outputs(ind(1))=start_time;									
				//last car arrive time
				outputs(ind(2))= start_time + green_time(0);
			}
		}
	};

	//! counts planned rating using offset and recieved_exps
	double count_rating(const int offset) {
		double virtual_queue;
		double t_emptiyng;
		double t_green_begin;
		double t_green_end;
		double t_cars_begin;
		double t_cars_end;
//		vec t_cars_begin;
//		vec t_cars_end;
		double t_mixing;
		bool found;

		double rating=0.0;

		for (int i=0;i<lanes.length();i++) {

			//Finding, if we have some expectations
			found=false;
			for (int k=0;k<lanes(i).inputs.length();k++) { 
//				t_cars_begin.set_size(lanes(i).inputs.length());
//				t_cars_end.set_size(lanes(i).inputs.length());

				for (int j=0;j<rv_recieved_exps.length();j++) {
					int result=rv_recieved_exps.name(j).find(lanes(i).inputs(k)+"-");
					if (result>=0) {
						ivec ind = rv_recieved_exps.dataind(RV(rv_recieved_exps.name(j),3));

						t_cars_begin=recieved_exps(ind(1));		//TODO now using only last found exp.
						t_cars_end=recieved_exps(ind(2));
						found=true;
					}
				}
			}

			if (!found) {
				t_cars_begin=0;
				t_cars_end=cycle_length;
			}
			//counting rating
			t_green_begin=green_starts(sg_index(lanes(i).sg)) + offset;
			ivec ind = rv_inputs.dataind(RV(name+"_"+lanes(i).sg,1));
			t_green_end=t_green_begin+inputs(ind(0));

			//TODO use real queues lenghts
			if (t_green_begin<t_cars_begin) {
				//virtual_queue=lanehs(i)->queue-(t_cars_begin-t_green_begin)/car_leaving;
				//virtual_queue=(rand()%8)-(t_cars_begin-t_green_begin)/car_leaving;
				virtual_queue=5-(t_cars_begin-t_green_begin)/car_leaving;
			} 
			else if (t_green_begin>t_cars_begin) {
				//virtual_queue=lanehs(i)->queue+(t_green_begin-t_cars_begin)*lanehs(i)->expected_density();
				//virtual_queue=(rand()%9)+(t_green_begin-t_cars_begin)*lanehs(i)->expected_density();
				virtual_queue=4+(t_green_begin-t_cars_begin)*lanehs(i)->expected_density();
			}

			t_emptiyng=virtual_queue/(1/car_leaving - lanehs(i)->expected_density());
			t_mixing=min(t_green_end,t_cars_end)-max(t_green_begin,t_cars_begin);

			rating+=max((t_mixing-t_emptiyng)*lanehs(i)->expected_density(),0.0);  
		}
		return rating;
	}

	//! finds best offset using recieved_exps. Returns found offset
	int find_best_offset(const int center, int interval) {
		//! rating if offset is rised
		double rating_p;
		//! rating if offset is unchaged (=center)
		double rating_c;
		//! rating if offset is lowered
		double rating_n;

		int new_center;

		rating_p=count_rating(center+interval);
		rating_c=count_rating(center);
		rating_n=count_rating(center-interval);

		new_center=center;
		int max_index=max_of_three(rating_p,rating_c,rating_n);
		switch (max_index) {
			case 0:
				new_center+=interval;
				break;
			case 1:
				break;
			case 2:
				new_center-=interval;
				break;
		}

		if (interval>2) {
			interval/=2;
			new_center=find_best_offset(new_center,interval);
		}

		return new_center;
	}

	//! finds if changing neighbour's offset could have positive effect, returns found offset change
	int find_best_exps(const int offset_change, const string neighbour, double &rating_change) {
		//! expectations recieved from neighbour
		vec original_exps;
		//! expactations after positve change of neighbour's offset
		vec positive_exps;
		//! expactations after negative change of neighbour's offset
		vec negative_exps;
		//! rating if offset is rised
		double rating_p;
		//! rating if offset is unchaged
		double rating_c;
		//! rating if offset is lowered
		double rating_n;		
		original_exps.set_size(recieved_exps.length());
		
		original_exps=recieved_exps;
		positive_exps=recieved_exps;
		negative_exps=recieved_exps;

		for (int j=0;j<rv_recieved_exps.length();j++) {
			int res = rv_recieved_exps.name(j).find("-"+neighbour);
			if (res>0) {
				ivec ind = rv_recieved_exps.dataind(RV(rv_recieved_exps.name(j),3));
				rating_n=count_rating(planned_offset);

				positive_exps(ind(1))+=offset_change;
				positive_exps(ind(2))+=offset_change;

				negative_exps(ind(1))-=offset_change;
				negative_exps(ind(2))-=offset_change;
			}
		}

		rating_c=count_rating(planned_offset);

		recieved_exps=positive_exps;
		rating_p=count_rating(planned_offset);

		recieved_exps=negative_exps;
		rating_n=count_rating(planned_offset);

		recieved_exps=original_exps;

		int max_index=max_of_three(rating_p,rating_c,rating_n);
		switch (max_index) {
			case 0:
				rating_change=rating_p-rating_c;
				return offset_change;
				break;
			case 1:
				rating_change=0;
				return 0;
				break;
			case 2:
				rating_change=rating_n-rating_c;
				return -offset_change;
				break;
		}
		rating_change=NULL;
		return NULL;
	}

	//! returns index of signal group sg
	int sg_index(const string sg) {
		for (int i=0;i<sgs.length();i++) {
			if (sgs(i)==sg) {
				return i;
			}
		}
		return -1;
	}
	
	//! returns offset value shifted to fit interval <0;cycle_length>
	int normalize_offset(int offset) {
		while (offset<0 && offset<cycle_length) {
			if (offset<0) {
				offset+=cycle_length;
			}
			else {
				offset-=cycle_length;
			}
		}
		return offset;
	}


	//! returns index of maximum of entered values
	int max_of_three(const double a, const double b, const double c) {
		int index = a > b ? 0 : 1;

		if (index == 0) {
			index = a > c ? 0 : 2;
		} 
		else {
			index = b > c ? 1 : 2;
		}
		return index;
	}

public:
	//! offset set in last simulation step
	int last_offset;
	//! actual planned offset to set for next simulation step
	int planned_offset;
	//! rating of actual planned offset
	double planned_rating;
	
	//! array of existing signal groups
	Array<string> sgs;
	//! relative starts of green for signal groups
	ivec green_starts;

	//! avarage speed of cars
	int VP;

	void validate() {
		rv_action = RV(name+"_offset", 1); // <======= example

		for (int i=0; i<sgs.length();i++) {
			rv_inputs.add(RV(name+"_"+sgs(i),1));
		}
		inputs.set_size(rv_inputs._dsize());
		
		BaseTrafficAgent::validate();

		for (int i=0;i<lanehs.length();i++) {
			ivec index = rv_queues.dataind(RV(lanes(i).queue,1));
			lanehs(i)->queue_index=index(0);
		}


	}

	void adapt(const vec &glob_dt) {
		BaseTrafficAgent::adapt(glob_dt);
			
		for (int i=0;i<lanehs.length();i++) {
			lanehs(i)->queue=queues(lanehs(i)->queue_index);		
		}

		planned_offset=last_offset;
		
		//set state variables to default values
		final_state=false;
		new_stable_state=false;
		send_requests=false;
		need_exps=true;
		negot_offset=8;
	}

	void broadcast(Setting& set){

		//ask neighbours for exptected arrive times
		if (need_exps) {
			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 ( (string)"expected_times_request", msg, "what");
			}
			need_exps=false;
		}

		// broadcast expected cars
		if (!seznam.empty()) {
			expected_cars();
			do {
				Setting &msg =set.add(Setting::TypeGroup);
				UI::save ( seznam.back(), msg, "to");
				UI::save ( name, msg, "from");
				UI::save ( (string)"new_expected_cars", msg, "what");
				UI::save ( &(rv_outputs), msg, "rv");
				UI::save ( outputs, msg, "value");
				seznam.pop_back();
			} while (!seznam.empty());			
		}

		// broadcast new stable state (new stable expectations)
		if (new_stable_state) {
			expected_cars();
			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 ( (string)"new_stable_state2", msg, "what");
				UI::save ( &(rv_outputs), msg, "rv");
				UI::save ( outputs, msg, "value");
			}
			new_stable_state=false;
		}

		// broadcast requests to change offset(s)
		if (send_requests) {
			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 ( (string)"offset_change_request", msg, "what");
				UI::save ( &(rv_change_request), msg, "rv");
				UI::save ( change_request, msg, "value");
			}
			send_requests=false;
		}

		/*if (reset_negot_offset) {
			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 ( (string)"reset_negot_offset", msg, "what");
			}
		}*/


		
		// reached final offset. Log value?
		if (final_state) {
			
			//cout << "Jmenuji se "<<name<< " a skoncil jsem na offsetu " << planned_offset << " s hodnocenim " << planned_rating <<endl;
			final_state=false;
		}
	}

	void receive(const Setting &msg){
		string what;
		string to;

		string from;
		vec value;
		RV *rv;
		
		UI::get(what, msg, "what", UI::compulsory);
		UI::get(to, msg, "to", UI::compulsory);
		UI::get(from, msg, "from");
		UI::get(rv, msg, "rv");
		UI::get(value, msg, "value");
		
		if (what=="expected_times_request"){ 
			seznam.push_back(from);
		} 
		else if (what=="new_expected_cars") {
			rv_recieved_exps=*rv;
			recieved_exps=value;
			
			last_offset=planned_offset;

			planned_offset=find_best_offset(planned_offset,8);
			planned_offset=normalize_offset(planned_offset);
			

			/*if (planned_offset!=last_offset) {
				reset_negot_offset=true;
			}*/
			planned_rating=count_rating(planned_offset);
			// we have new stable state to broadcast
			new_stable_state=true;
		}
		else if (what=="new_stable_state2") {
			rv_recieved_exps=*rv;
			recieved_exps=value;
			planned_rating=count_rating(planned_offset);

			for (int i=0;i<neighbours.length();i++) {
				rv_change_request.add(RV(neighbours(i)+"_change",2));
				change_request.set_size(rv_change_request._dsize()); 
				ivec ind=rv_change_request.dataind(RV(neighbours(i)+"_change",2));
				// offset change
				change_request(ind(0))=find_best_exps(negot_offset,neighbours(i),rating_change);
				// rating change
				change_request(ind(1))=rating_change;
			}

			if (negot_offset>2) { 
				negot_offset/=2;
					send_requests=true;
			}
			else {
				final_state=true;
			}
		}
		else if (what=="offset_change_request") {
			double final_rating_diff;

			rv_recieved_changes=*rv;
			recieved_changes=value;

			for (int i=0;i<rv_recieved_changes.length();i++) {

				ivec ind=rv_recieved_changes.dataind(RV(rv_recieved_changes.name(i),2));

				final_rating_diff=-planned_rating+count_rating(planned_offset+(int)recieved_changes(ind(0)))-recieved_changes(ind(0));
				if (final_rating_diff>0) {
					planned_offset+=(int)recieved_changes(ind(0));
					planned_rating+=final_rating_diff;
					accepted_from=from;
				}
			}
			//need_exps=true;s
			new_stable_state=true;
		}
		/*else if (what=="reset_negot_offset") {
			negot_offset=8;
		}*/
		else {
			BaseTrafficAgent::receive(msg);
		}
	}
	
	void ds_register(const DS &ds) {
		BaseTrafficAgent::ds_register(ds);
		action2ds.set_connection( ds._urv(), rv_action);
	}

	void from_setting(const Setting &set) {
		BaseTrafficAgent::from_setting(set);
		
		srand(time(NULL));

		car_leaving=2;
		VP=45;

		negot_offset=8;
		
		// load from file
		UI::get(sgs, set, "sgs", UI::compulsory);	
		UI::get(green_starts, set, "green_starts", UI::compulsory);
		UI::get(last_offset, set, "offset", UI::compulsory);
	
	}
	void act(vec &glob_ut){
		vec action;
		action.set_size(rv_action._dsize());

		ivec index = rv_action.dataind(RV(name+"_offset",1));
		action(index(0))=planned_offset;
		last_offset=planned_offset;

		action2ds.filldown(action,glob_ut);
	}

};
UIREGISTER(GreenWaveTrafficAgent);