Index: /applications/doprava/traffic_agent_offset.h
===================================================================
--- /applications/doprava/traffic_agent_offset.h (revision 1127)
+++ /applications/doprava/traffic_agent_offset.h (revision 1133)
@@ -30,4 +30,13 @@
 	//! 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; //s; how long is 1 car leaving queue
@@ -48,4 +57,8 @@
 	//! 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
@@ -72,6 +85,6 @@
 					exp2outputs.set_connection(rv_exp,rv_outputs);
 
-					//Number of cars
-					exp(0)=lanehs(i)->expected_output(green_time)*lanes(i).alpha(j);	
+					//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)) + lanes(i).output_distances(j)/VP + planned_offset;
@@ -80,5 +93,5 @@
 					//last car arrive time
 					exp(2)=start_time + green_time;
-					//TODO pushup az na konec
+					
 					exp2outputs.pushup(outputs,exp);
 				}			
@@ -86,4 +99,29 @@
 		}
 	};
+	
+	/*! 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
@@ -92,5 +130,5 @@
 		double t_green_begin;
 		double t_green_end;
-		vec cars_count;
+		vec cars_density;
 		vec t_cars_begin;
 		vec t_cars_end;
@@ -108,15 +146,13 @@
 				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);
+						cars_density.set_size(l+1,true);
 
 						ind = RV(rv_recieved_exps.name(j),3).dataind(rv_recieved_exps);
 
-						cars_count(l)=recieved_exps(ind(0));
+						cars_density(l)=recieved_exps(ind(0));
 						t_cars_begin(l)=recieved_exps(ind(1));
 						t_cars_end(l)=recieved_exps(ind(2));
@@ -128,5 +164,4 @@
 			}
 			if (found) {			
-
 				//counting rating
 				group_name = name+"_"+lanes(i).sg;		//e.g. 495_VA
@@ -136,74 +171,86 @@
 				t_green_end=t_green_begin + green_times(index)*cycle_length;
 
-				/************** counting with all exps ****************/
-
-				int k;
-				double t_act=t_green_begin;
+				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 {
+				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);
-
+					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));
+								t_cars_begin(k)=t_end;
+								t_gb_vec(l)=t_end;
 							}
+							//cars only entering queue
 							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;
+								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;
 							}
 						}
-						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);
+						//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;
+								t_gb_vec(l)=t_end;
+								
+								//in case we emptied whole queue
+								virtual_queue=max(virtual_queue,0.0);
 							}
-							//if t_cars_end>=t_green_end
+							//no cars entering, no cars leaving
 							else {
-								virtual_queue+=cars_count(k)-(t_green_end-t_act)/car_leaving;
-								t_act=t_green_end;
+								t_end=min(t_gb_vec(l),t_cars_begin(k));
 							}
-							//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_act=t_endl
+						}
+						//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_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 (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_green_end-t_act)/car_leaving;
-						t_act=t_green_end;
+						virtual_queue-=( t_ge_vec(l)-t_act)/car_leaving;
+						t_act=t_ge_vec(l);
+						t_gb_vec.del(l);
+						t_ge_vec.del(l);
 					}
-					if (virtual_queue<0) {
-						rating-=virtual_queue;
-						virtual_queue=0;
-					}
-				} while (t_act<t_green_end);
+				} while (t_act<t_limit);
 			}
 		}
@@ -219,5 +266,5 @@
 		//! rating if offset is lowered
 		double rating_n;
-
+		//! center point for next cycle
 		int new_center;
 
@@ -239,5 +286,5 @@
 		}
 
-		if (interval>2) {
+		if (interval>find_best_limit) {
 			interval/=2;
 			new_center=find_best_offset(new_center,interval);
@@ -249,6 +296,4 @@
 	//! 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;
@@ -261,7 +306,5 @@
 		//! 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;
@@ -379,14 +422,22 @@
 	}
 
+	//! 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:
-	//! 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);
@@ -424,5 +475,5 @@
 	void broadcast(Setting& set){
 
-		//ask neighbours for exptected arrive times
+		//ask neighbours for expetcted arrive times
 		if (need_exps) {
 			for (int i=0; i<neighbours.length(); i++){
@@ -478,5 +529,5 @@
 		}
 	
-		// reached final offset.
+		// reached final offset
 		if (final_state) {
 			final_state=false;
@@ -505,5 +556,5 @@
 			
 			if (!passive) {
-				planned_offset=find_best_offset(planned_offset,8);
+				planned_offset=find_best_offset(planned_offset,find_best_start);
 				planned_offset=normalize_offset(planned_offset);
 			}
@@ -580,13 +631,16 @@
 
 		car_leaving=2;
-		VP=45;
+		VP=40;
 		actual_time=0;
 		
 		negot_cycle=1;
+		total_offset=0;
 		cycle_count=5;
-		total_offset=0;
 
 		negot_offset=4;
 		negot_limit=1;
+
+		find_best_start=8;
+		find_best_limit=2;
 
 		passive=0;
