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

class GreenWaveTrafficAgent : public BaseTrafficAgent {
LOG_LEVEL(GreenWaveTrafficAgent,logoffset);
protected:
	double rating_change;
	int negot_start;
	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;

	//! 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;
	
	double car_leaving_time; //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;
	//! Finding of best own offsset starts on this offset change
	int find_best_start;
	//! minimal value of offsset change tested during looking for best offset
	int find_best_limit;

	//! 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);

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

					start_time = green_starts(group_index(name+"_"+lanes(i).sg)) + (3.6*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;
					
					exp2outputs.pushup(outputs,exp);
				}			
			}
		}
	};
	
	/*! creates periodic expansion of gb (green begin], ge (green end), using 
		expectations cb (cars begin), ce (cars end). Result is stored in gbv 
		(green begin vector) and gev (green end vector) */	
	void expand_greens(const double gb, const double ge, const vec cb, const vec ce, vec &gbv, vec &gev) {
		gbv.set_size(1);
		gev.set_size(1);
		gbv(0)=gb;
		gev(0)=ge;
		if (ge>cycle_length) {
			gbv.ins(gbv.length(),0);
			gev.ins(gev.length(),ge-cycle_length);
		}
		int i=1;
		while (gev(gev.length()-1)<ce(max_i(ce))) {
			gbv.ins(gbv.length(),gb+i*cycle_length);
			gev.ins(gev.length(),ge+i*cycle_length);
			i++;
		}
	};
	
	//! returns true if what is in interval <left;right>
	bool in_interval(double what, const double left, const double right) {
		return ((what>=left && what<=right) ? true : false);
	}

	//! 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_density;
		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_density.set_size(l+1,true);

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

						cars_density(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;

				vec t_gb_vec;
				vec t_ge_vec;

				expand_greens(t_green_begin, t_green_end, t_cars_begin, t_cars_end, t_gb_vec, t_ge_vec);
				
				//! index for t_cars_begin
				int k=min_i(t_cars_begin);
				//! index for t_ggb_vec
				int l;
				//! indicator of actual position in whole interval
				double t_act=0;
				//! end of counting for actual line
				double t_limit=t_ge_vec(max_i(t_ge_vec));
				//! end of closest future interval
				double t_end;

				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);
					l=min_i(t_gb_vec);
					if (k!=-1) {
						//cars are entering queue
						if (in_interval(t_act,t_cars_begin(k), t_cars_end(k))) {
							//cars leaving and entering queue
							if (in_interval(t_act,t_gb_vec(l), t_ge_vec(l))) {
								t_end = min(t_cars_end(k),t_ge_vec(l));
			 					virtual_queue+=(t_end - t_act)*(cars_density(k)-(1/car_leaving_time));
								t_cars_begin(k)=t_end;
								t_gb_vec(l)=t_end;
							}
							//cars only entering queue
							else {
								t_end=min(t_cars_end(k),t_ge_vec(l));
								virtual_queue+=(t_end-t_act)*cars_density(k);
								t_cars_begin(k)=t_end;
							}
						}
						//cars are not entering queue
						else {
							//cars are only leaving queue
							if (in_interval(t_act,t_gb_vec(l), t_ge_vec(l))) {
								t_end = min(t_ge_vec(l),t_cars_begin(k));
								virtual_queue-=(t_end-t_act)/car_leaving_time;
								t_gb_vec(l)=t_end;
								
								//in case we emptied whole queue
								virtual_queue=max(virtual_queue,0.0);
							}
							//no cars entering, no cars leaving
							else {
								t_end=min(t_gb_vec(l),t_cars_begin(k));
							}
						}
						t_act=t_end;

						//raising rating
						if (virtual_queue<0) {
							rating-=virtual_queue;
							virtual_queue=0;
						}

						//deleting used intervals
						if (t_cars_begin(k)==t_cars_end(k)) {
							t_cars_begin.del(k);
							t_cars_end.del(k);
							cars_density.del(k);
						}
						if (t_gb_vec(l)==t_ge_vec(l)) {
							t_gb_vec.del(l);
							t_ge_vec.del(l);
						}
					}
					//if no other expectations found
					else {
						virtual_queue-=( t_ge_vec(l)-t_act)/car_leaving_time;
						t_act=t_ge_vec(l);
						t_gb_vec.del(l);
						t_ge_vec.del(l);
					}
				} while (t_act<t_limit);
			}
		}
		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;
		//! center point for next cycle
		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>find_best_limit) {
			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) {
		//! 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;		

		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;
	}

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

public:
	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;
		planned_rating=0;
		
		//set state variables to default values
		final_state=false;
		new_stable_state=false;
		send_requests=false;
		need_exps=true;
		negot_offset=negot_start;

		if (actual_time/cycle_length<2) {
			need_exps=false;
			final_state=true;		
		}
	}

	void broadcast(Setting& set){

		//ask neighbours for expetcted 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()) {
			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();
			} 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,find_best_start);
				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;
			
			planned_rating=count_rating(planned_offset, recieved_exps, rv_recieved_exps);

			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);

		total_offset=0;
		actual_time=0;
		negot_cycle=1;

		car_leaving_time=2;
		VP=40;
				
		cycle_count=5;

		negot_start=4;
		negot_limit=1;

		find_best_start=8;
		find_best_limit=2;

		passive=0;
		
		UI::get(last_offset, set, "offset", UI::compulsory);
		
		//optional reading parameters from config
		UI::get(passive, set, "passive", UI::optional);
		UI::get(car_leaving_time, set, "car_leaving_time", UI::optional);
		UI::get(VP, set, "VP", UI::optional);
		UI::get(cycle_count, set, "cycle_count", UI::optional);
		UI::get(negot_start, set, "negot_start", UI::optional);
		UI::get(negot_limit, set, "negot_limit", UI::optional);
		UI::get(find_best_start, set, "find_best_start", UI::optional);
		UI::get(find_best_limit, set, "find_best_limit", 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("a",2), "x"+prefix );
		}
	}
	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);