brought some infrastructure for better simulation in place

src/lookahead.c

@@ -65,6 +65,42 @@ void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
 	m->aw = 2*(w2 - w1)/(dt1 + dt2);
 }
 
+
+void radar_dgl(trial* t, timestamp ts){
+	timestamp step = ts- t->sim.tm.ts;
+	if (step > 0){
+		dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,1);
+		t->sim.tm.ts += step;
+		t->sim.steps++;
+	}
+}
+
+/*updates the simulation state when new telemetry arrives*/
+void sim_update(trial* t){
+
+	telemetry *tm,*tmp;
+	tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
+	/*run simulation up to new telemetry timestamp*/
+	radar_dgl(t,tm->ts);
+	
+	/*calculate misspredict*/
+	fprintf(stderr, "Miss predict: dx=%f, dy=%f, ddir=%f, steps=%u\n",
+		t->sim.tm.vehicle.x - t->vehicle.x,
+		t->sim.tm.vehicle.y - t->vehicle.y,
+		t->sim.tm.vehicle.dir - t->vehicle.dir,
+		t->sim.steps);
+
+	/*update simulation vehicle from telemetry: everything and calculate w*/
+	t->sim.tm.vehicle = tm->vehicle;
+	g_array_set_size(t->sim.tm.objects, 0);
+	g_array_append_vals(t->sim.tm.objects, tm->objects->data, tm->objects->len);
+	t->sim.steps = 0;
+	if(t->telemetry.length > 1){
+		tmp = (telemetry*) g_queue_peek_tail_link(&t->telemetry)->prev->data;
+		t->sim.tm.vehicle.w = (tm->vehicle.dir - tmp->vehicle.dir)/(tm->ts - tmp->ts);
+	}
+}
+
 int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){
 	double a,k,aw;
 	double wsoll;

src/lookahead.h

@@ -7,4 +7,8 @@ double turn2w(trial* t, turn_t turn);
 void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m);
 int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat);
 
+void sim_update(trial* t);
+
+void radar_dgl(trial* t, timestamp ts);
 #endif
+

src/radar_main.c

@@ -35,7 +35,7 @@ void trial_loop(trial *t) {
 			timestamp step=getcurts(t) + LATENCY - t->sim.tm.ts;
 			if (step > 0) {
 				t->sim.tm.vehicle.w = turn2w(t, t->sim.tm.vehicle.turn);
-				dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,step);
+				dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,1);
 				t->sim.tm.ts += step;
 				t->sim.steps++;
 			}