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

class GreenWaveTrafficAgent : public BaseTrafficAgent {
LOG_LEVEL(GreenWaveTrafficAgent,logoffset);
protected:
	double rating_change;
	int negot_offset;
	int negot_limit;

	int actual_time;

	RV rv_outputs;
	vec outputs;

	RV rv_change_request;
	vec change_request;

	RV rv_recieved_exps;
	vec recieved_exps;

	RV rv_next_exps;
	vec next_exps;

	//! expectations recieved from neighbours
	RV rv_recieved_changes;
	vec recieved_changes;

	//! list of agents, who request expected cars
	list<string> requesters;
	
	double car_leaving; //s; how long is 1 car leaving queue

	// some state variables
	bool need_exps;
	bool new_stable_state;
	bool send_requests;
	bool final_state;
	
	//! determines wheteher agent actively communicates
	int passive;

	//! sum of final planned_offset values since last reach of cycle_count
	int total_offset;
	//! number of finished cycles since last reach of cycle_count
	int negot_cycle;
	//! after cycle_count cycles, we count avarege planned_offseta send it to Aimsun
	int cycle_count;

	//! counts all expected cars going from each lane, saves to outputs and rv_outputs
	void expected_cars() {
		double start_time;
		ivec ind;
		RV rv_exp;
		datalink exp2outputs;
		vec exp;
		string group_name;
		double green_time;
		int index;

		for (int i=0;i<lanes.length();i++) {
			for (int j=0;j<lanes(i).outputs.length();j++) {
				if (lanes(i).outputs(j)!="DUMMY_DET") {
					group_name = name+"_"+lanes(i).sg;		//e.g. 495_VA
					
					index=group_index(group_name);
					green_time=green_times(index)*cycle_length;

					rv_exp=RV(lanes(i).outputs(j)+"-"+name+"_"+to_string(i),3);
					exp.set_size(rv_exp._dsize());
					exp2outputs.set_connection(rv_exp,rv_outputs);

					//Number of cars
					exp(0)=lanehs(i)->expected_output(green_time)*lanes(i).alpha(j);	

					start_time = green_starts(group_index(name+"_"+lanes(i).sg)) + lanes(i).output_distances(j)/VP + planned_offset;
					//first car arrive time
					exp(1)=start_time;
					//last car arrive time
					exp(2)=start_time + green_time;
					//TODO pushup az na konec
					exp2outputs.pushup(outputs,exp);
				}			
			}
		}
	};

	//! counts planned rating using offset and recieved_exps
	double count_rating(const int offset, const vec recieved_exps, const RV rv_recieved_exps) {
		double virtual_queue;
		double t_green_begin;
		double t_green_end;
		vec cars_count;
		vec t_cars_begin;
		vec t_cars_end;
		bool found;
		ivec ind;
		string group_name;

		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++) { 
				int l=0;		
				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) {

						t_cars_begin.set_size(l+1,true);
						t_cars_end.set_size(l+1,true);
						cars_count.set_size(l+1,true);

						ind = RV(rv_recieved_exps.name(j),3).dataind(rv_recieved_exps);

						cars_count(l)=recieved_exps(ind(0));
						t_cars_begin(l)=recieved_exps(ind(1));
						t_cars_end(l)=recieved_exps(ind(2));
						l++;

						found=true;
					}
				}
			}
			if (found) {			

				//counting rating
				group_name = name+"_"+lanes(i).sg;		//e.g. 495_VA
				int index=group_index(group_name);

				t_green_begin=green_starts(index) + offset;
				t_green_end=t_green_begin + green_times(index)*cycle_length;

				/************** counting with all exps ****************/

				int k;
				double t_act=t_green_begin;
				virtual_queue=lanehs(i)->queue;

				//cycle goes through all "stopping" points and counts queue lenght at these points
				do {
					k=min_i(t_cars_begin);
					if (k!=-1) {						
						//in case there are cars comming before t_green begin
						if (t_cars_begin(k)<t_act) {
							if (t_cars_end(k)<=t_act) {
								virtual_queue+=cars_count(k);

								cars_count.del(k);
								t_cars_begin.del(k);
								t_cars_end.del(k);

							}
							else {
								double frac=(t_cars_begin(k)-t_act)/(t_cars_end(k)-t_cars_begin(k));
								virtual_queue+=cars_count(k)*frac;
								t_cars_begin(k)=t_act;
							}
						}
						else if (t_cars_begin(k)==t_act) {
							if (t_cars_end(k)<t_green_end) {
								virtual_queue+=cars_count(k)-(t_cars_end(k)-t_act)/car_leaving;
								t_act=t_cars_end(k);
							}
							//if t_cars_end>=t_green_end
							else {
								virtual_queue+=cars_count(k)-(t_green_end-t_act)/car_leaving;
								t_act=t_green_end;
							}
							//more cars than cars_count(k) couldn't pass without stopping
							if (virtual_queue < -cars_count(k)) {
								virtual_queue = -cars_count(k);
							}
							cars_count.del(k);
							t_cars_begin.del(k);
							t_cars_end.del(k);
						}
						//if t_cars_begin(k)>=t_act
						else {
							if (t_cars_begin(k)<t_green_end) {
								virtual_queue-=(t_cars_begin(k)-t_act)/car_leaving;
								t_act=t_cars_begin(k);
							}
							else {
								virtual_queue-=(t_green_end-t_act)/car_leaving;
								t_act=t_green_end;
							}
							// in case we managed to empty whole queue
							if (virtual_queue<0) {
								virtual_queue=0;
							}
						}
					}
					//if no other expectations found
					else {
						virtual_queue-=(t_green_end-t_act)/car_leaving;
						t_act=t_green_end;
					}
					if (virtual_queue<0) {
						rating-=virtual_queue;
						virtual_queue=0;
					}
				} while (t_act<t_green_end);
			}
		}
		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, recieved_exps, rv_recieved_exps);
		rating_c=count_rating(center, recieved_exps, rv_recieved_exps);
		rating_n=count_rating(center-interval, recieved_exps, rv_recieved_exps);

		new_center=center;
		int max_index=max_i_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 and stores chage of rating to rating_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 raised
		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(rv_recieved_exps.name(j),3).dataind(rv_recieved_exps);

				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, rv_recieved_exps);
		rating_p=count_rating(planned_offset, positive_exps,  rv_recieved_exps);
		rating_n=count_rating(planned_offset, negative_exps,  rv_recieved_exps);

		int max_index=max_i_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 "group"
	int group_index(const string group) {
		for (int i=0;i<green_names.length();i++) {
			if (green_names(i)==group) {
				return i;
			}
		}
		return -1;
	}
	
	/*! 
	returns offset value shifted to fit interval <-cycle_length/2;cycle_length/2> 
	or (when second parameter is false)) <0;cycle_length>
	*/
	int normalize_offset(int offset, bool zero=true) {
		if (zero) {
			while ((offset<(-cycle_length/2)) || (offset>(cycle_length/2))) {
				if (offset<0) {
					offset+=cycle_length;
				}
				else {
					offset-=cycle_length;
				}
			}
			return offset;
		}
		else {
			while (offset<0 || offset>cycle_length) {
				if (offset<0) {
					offset+=cycle_length;
				}
				else {
					offset-=cycle_length;
				}
			}
			return offset;
		}
	}

	//! converts t to string
	template <class T> inline string to_string (const T& t)
	{
		std::stringstream ss;
		ss << t;
		return ss.str();
	}

	//! returns index of maximum of entered values
	int max_i_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;
	}

	//! returns index of smallest element in vector
	int min_i(vec vector) {
		if (vector.length()>0) {
			double min=vector(0);
			int index=0;
			for (int i=1;i<vector.length();i++) {
				if (vector(i)<min) {
					min=vector(i);
					index=i;
				}
			}
			return index;
		}
		return -1;
	}

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;	
	//! avarage speed of cars
	int VP;

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

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

		for (int i=0;i<lanehs.length();i++) {
			ivec index = RV(lanes(i).queue,1).dataind(rv_queues);
			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=4;
	}

	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 (!requesters.empty()) {
			double a;
			expected_cars();
			do {
				Setting &msg =set.add(Setting::TypeGroup);
				UI::save ( requesters.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");
				requesters.pop_back();
				a=outputs (10);
			} while (!requesters.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)"stable_state", 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;
		}
	
		// reached final offset.
		if (final_state) {
			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"){ 
			requesters.push_back(from);
		} 
		else if (what=="new_expected_cars") {
			rv_recieved_exps=*rv;
			recieved_exps=value;
			
			if (!passive) {
				planned_offset=find_best_offset(planned_offset,8);
				planned_offset=normalize_offset(planned_offset);
			}

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

			planned_rating=count_rating(planned_offset, recieved_exps, rv_recieved_exps);

			if (!passive) {
				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(neighbours(i)+"_change",2).dataind(rv_change_request);
					
					// 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>=negot_limit) { 
					negot_offset/=2;
					send_requests=true;
				}
				else {
					final_state=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(rv_recieved_changes.name(i),2).dataind(rv_recieved_changes);

				final_rating_diff=-planned_rating+count_rating(planned_offset+(int)recieved_changes(ind(0)), recieved_exps, rv_recieved_exps)+recieved_changes(ind(1));
				if (final_rating_diff>=0) {
					planned_offset+=(int)recieved_changes(ind(0));
					planned_offset=normalize_offset(planned_offset);
					planned_rating+=final_rating_diff;
				}
			}

			new_stable_state=true;
		}
		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) {
		RV rv_exp;

		BaseTrafficAgent::from_setting(set);

		car_leaving=2;
		VP=45;
		actual_time=0;
		
		negot_cycle=1;
		cycle_count=5;
		total_offset=0;

		negot_offset=4;
		negot_limit=1;

		passive=0;
		
		UI::get(last_offset, set, "offset", UI::compulsory);
		UI::get(passive, set, "passive", UI::optional);

		for (int i=0;i<lanes.length();i++) {
			for (int j=0;j<lanes(i).outputs.length();j++) {
				if (lanes(i).outputs(j)!="DUMMY_DET") {
					rv_exp=RV(lanes(i).outputs(j)+"-"+name+"_"+to_string(i),3);
					rv_outputs.add(rv_exp);
				}
			}
		}
		outputs.set_size(rv_outputs._dsize());	

		log_level[logoffset]=true;
	}

	void act(vec &glob_ut){
		if (negot_cycle==cycle_count) {
			vec action;
			action.set_size(rv_action._dsize());
		
			ivec index = RV(name+"_offset",1).dataind(rv_action);

			action(index(0))=normalize_offset(total_offset/cycle_count, false);
			action2ds.filldown(action,glob_ut);

			total_offset=0;
			negot_cycle=1;
		}
		else {
			total_offset+=planned_offset;
			negot_cycle++;
		}
		last_offset=planned_offset;
	}

	void step() {
		actual_time+=step_length;
	}

	void log_register(logger &l, const string &prefix){
		if ( log_level[logoffset]){
			l.add_vector ( log_level, logoffset, RV("1",2), "x"+prefix );	//TODO 
		}
	}
	void log_write() const {
		if (log_level[logoffset]){
			vec offset_vec(2);
			offset_vec(0)=planned_offset;
			offset_vec(1)=planned_rating;
			log_level.store(logoffset, offset_vec);			
		}
	} 
};
UIREGISTER(GreenWaveTrafficAgent);