//
// tracker.c
// pete february 2006
// a first pass at code to implement the tracking filter described in Essay 2- Radar Autotracking
//

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

typedef struct {
	float r, 				// measured range
		theta, 				// measured angle
		t,					// time of measurement
		e_r;				// std dev of error at range 1
		} Measure;
		
typedef struct {
	int id,					// target number
		init;				// state 0, 1, 2
	long measurements;		// number of measurements on this target	
	float x, 				// estimated position			
		y, 
		t,					// at time
		s, 					// estimated speed
		phi, 				// course
		v_x,				// error in along-track position ('std dev')
		v_y,				// in across
		v_s,				// in speed
		v_phi,				// in course
		t0;					// time of first measurement
		} Track;

FILE * logfile;
float speed, course;		// globals holding current real speed and course
		
// ------------------------- get_alpha ------------------------

static float get_alpha(float v_e, float v_m, float ta, float p, float m) {
	// v_e is the std deviation of the estimate, v_m of the measurement
	// ta is the ratio of this measurement interval to the average
	// p is predicted position, m is measured position
	// compute the weighting factor
	// when the error is within one nominal std deviation of the sensor's accuracy
	// we do simple variance-combining.
	// when it's bigger, we first compute alpha normally, and then jack it up by an amount
	// dependent on the error size
	float alpha, err, a;
	// first compute alpha, the weighting factor
	// a bigger alpha indicates a larger confidence in the measurement
	
	v_e = v_e * ta;							// shrink v_e if shorter interval than usual
	alpha = v_e/sqrt((v_e * v_e) + (v_m * v_m));

	// now compute err, the relative positional error 
	err = (m - p)/(1.25 * v_m);
	if (err < 0) err = -err;
	printf("\n\talpha=%.2f; m=%.2f p=%.2f v_e=%.2f v_m=%.2f; error is %.2f sensor bars", 
		alpha, m, p, v_e, v_m, err);
	if (err > 1.0) {
		// our measurement is further away than we can account for by sensor accuracy...
		alpha = 1.0 - ((1.0 - alpha)/err);	// move alpha closer to 1 as error gets larger
		printf("\n\t::alpha corrected to %.2f", alpha);
		}
	return alpha;
	}

// ------------------------- tracker --------------------------

static void tracker(Track * t, Measure * m) {
	// updates the provided Track with the provided Measurements

	float xm, ym, tm, dx, dy, dt, s, phi, e_x;
	float ta, psi, delta, D, Xm, Ym, Xp, Yp, Xe, Ye, Sm, Phi_m, Se, Phi_e, dPhi, De,
			alpha_x, alpha_y, beta_x, beta_y;	// first transform from polar to cartesian
	xm = (m->r * cos(m->theta));
	ym = (m->r * sin(m->theta));
	tm = m->t;
	e_x = m->e_r;					// measurement error scaled already
	printf("\n\nmeasure:r=%.2f theta=%.2f", m->r, m->theta);
	printf("\n\tcart:xm=%.2f y=%.2f t=%.2f", xm, ym, tm);
	switch (t->init) {
		case 0:
			// new track
			printf("\n\t**new track.");
			t->init = 1;
			t->x = xm;				// start off at measured position
			t->y = ym;
			t->t = tm;
			t->s = 0;
			t->phi = 0;
			t->v_x = 0;			// set initial variances to be large
			t->v_y = 0;
			t->v_s = 0;
			t->v_phi = 0;
			t->t0 = tm;
			break;
		case 1:
			// second measurement - compute initial course and speed
			printf("\n\t**estimate velocity.");
			t->init = 2;
			(t->measurements)++;
		  	dx = xm - t->x;
		  	dy = ym - t->y;
			dt = tm - t->t;
			// course = arctan dy/dx
			t->x = xm;
			t->y = ym;
			t->phi = atan(dy/dx);
			t->s= dx/(cos(t->phi) * dt);
			t->t = tm;
			t->v_x= e_x;
			t->v_y = e_x;
			t->v_s = e_x/dt;
			t->v_phi = e_x/dt;
			t->measurements=2;
			break;
		case 2:
		  	// declare simply - compiler sorts it out..
		  	
		  	dx = xm - t->x;
		  	dy = ym - t->y;
			dt = tm - t->t;
			printf("\n\tdx=%.2f dy=%.2f dt=%.2f", dx, dy, dt);
			psi = atan(dy/dx);				// apparent course since last measurement
			delta =  psi - t->phi;			// difference in course
			printf("\n\tpsi=%.2f old_phi=%.2f delta=%.2f", psi, t->phi, delta);
			D = sqrt((dx * dx) + (dy * dy));// distance gone between measurements
			Xm = D * cos(delta);			// distance gone along track
			Ym = D * sin(delta);			// distance gone across track
			printf("\n\tXm=%.2f Ym=%.2f", Xm, Ym);
			Xp = t->s * dt;					// predicted distance along track
			// compute ratio of this measurement interval to average time between measurements
			ta = ((tm - t->t0)/t->measurements);
			ta = (tm - t->t)/ta;
			// compute new estimate of along-track position Xe
			// (we'll do the adpative stuff later)
			alpha_x = get_alpha(t->v_x, e_x, ta, Xp, Xm);
			Xe = (1.0 - alpha_x) * Xp +  (alpha_x * Xm);
			printf("\n\talpha_x=%.3f Xp=%.2f Xe=%.2f ta=%.2f", alpha_x, Xp, Xe, ta);
			t->v_x = alpha_x * e_x;	// compute new estimate of along-track positional error
			// compute new estimate of across-track position Ye
			alpha_y = get_alpha(t->v_y, e_x, ta, 0, Ym);
			Ye = alpha_y * Ym;
			printf("\n\talpha_y=%.3f Ye=%.2f", alpha_y, Ye);
			t->v_y = (1.0 - alpha_y) * e_x;	// compute new estimate of across-track positional error
			// compute 'measured' course from Ye, Xe
			dPhi = atan(Ye/Xe);
			Phi_m = t->phi + dPhi;	// course = old course + error in course
			printf("\n\tdPhi=%.2f phi=%2.f Phi_m= %.2f", dPhi, t->phi, Phi_m); 
			// compute 'measured' speed from Xe and course error
			Sm = (Xe/cos(dPhi))/dt;
			printf("\n\tphi_m=%.2f s_m=%.2f", Phi_m, Sm);
			// compute weighting factors for speed, course
			beta_x = alpha_x * sqrt(alpha_x);
			beta_y = alpha_y * sqrt(alpha_y);
			t->s = (1.0 - beta_x) * t->s + beta_x * Sm;
			t->phi = ((1.0 - beta_y) * t->phi) +  beta_y * Phi_m;
			t->v_s = beta_x * (e_x/dt);
			t->v_phi = beta_y * (e_x/dt);
			// compute new cartesian coordinates of target
			De = sqrt((Xe * Xe) + (Ye * Ye));		// estimated distance gone
			t->x = t->x + De * cos(Phi_m);
			t->y = t->y + De * sin(Phi_m);
			t->t = tm;
			printf("\n\tDe=%.2f, X=%.2f Y=%.2f", De, t->x, t->y);
			(t->measurements)++;
			break;
		}
	printf("\n%.1f::t[%d] (%.2f, %.2f) s=%.2f c=%.2f\n\t\tdX=%.2f dY=%.2f dS=%.2f dC=%.2f",
		t->t, t->id, t->x, t->y, t->s, t->phi,
		t->v_x, t->v_y, t -> v_s, t->v_phi);
	}

// -------------------------- init_target ------------------------- 

static void init_target(Track * target, int id) {
	target -> id = id;
	target -> init = 0;
	target -> x = 0;
	target -> y = 0;
	target -> t = 0;
	target -> s = 0;
	target -> phi = 0;
	target -> v_x = 0;
	target -> v_y = 0;
	target -> v_s = 0;
	target -> v_phi = 0;
	}

typedef struct {
	float x, y, s, c, t, dt, v_e;
	} Sense;
	
// -------------------------- position ------------------------- 

static void position(int n, Track * t, Measure * m, Sense * s) {
	// compute n measurements for track t starting with given position and velocity
	float x, y, sp, c, xm, ym, xdot, ydot, r, theta, T, dt, end, v_e;
	x = s -> x;
	y = s -> y;
	sp = s -> s;
	c = s -> c;
	xdot = sp * cos(c);
	ydot = sp * sin(c);
	T = s -> t;
	dt = s -> dt;
	v_e = s -> v_e;
	while (n-- > 0) {
		float ex = 0.0, ey= 0.0;	// error fractions
		// compute new position
		x = x + (xdot * dt);
		y = y + (ydot * dt);
		T = T + dt;
		r = sqrt((x * x) + (y * y));
		m->e_r = r * v_e;
		// ok, we have an error square; perturb the measurements within that chunk
		ex = ((rand() % 100) - 50)/100.0;		// a signed fraction of the errbox
		ey = ((rand() % 100) - 50)/100.0;
		xm = x + (ex * m->e_r);
		ym = y + (ey * m->e_r);
		printf("\n\t::T=%.2f x=%.2f y=%.2f", T, x, y);
		// write the measurements
		m->r = sqrt((xm * xm) + (ym * ym));
		m->theta = atan(ym/xm);
		m->t = T;
		fprintf(logfile, "\n%0.2f\t%.2f\t%.2f\t%.2f\t%0.2f\t%.2f\t%.2f\t%.2f\t%0.2f\t%.2f\t%.2f\t%.2f\t",
			T, xm, T, ym, T, t->s, T, t->phi, T, speed, T, course);
		tracker(t, m);
		}
	// update sensor with current data
	s -> x = xm;
	s -> y = ym;
	s -> t = T;
	}

// ------------------------- main -------------------------------
			
int main(void) {
	Track target;
	Measure m;
	Sense s;
	
	float x, y, sp, c;
	printf ("\n autotracker 1.0");
	
	logfile = fopen("1.track", "w");
 	fprintf(logfile, "xm\t\tym\t\tspeed est\t\tcourse est\t\tspeed\t\tcourse");
	init_target(&target, 0);
	
	speed = 10;
	course = 1.3;
	
	s.t = 0;
	s.x = 100;
	s.y = 50;
	s.c = course;
	s.s =  speed;
	s.dt = 5;
	s.v_e = 0.0003;			// measurement error of 0.03 at range 1
	printf("\nnow at x=%.2f y=%.2f time = %.2f", s.x, s.y, s.t);
	
	printf("\n\n********** speed=%.2f course=%.2f ******************\n\n", s.s, s.c);
	position(10, &target, &m, &s);
	printf("\nnow at x=%.2f y=%.2f time = %.2f", s.x, s.y, s.t);
	
	// ok, now change his course while keeping speed the same
	course = .90;
	s.c = course;
	printf("\n\n********** speed=%.2f course=%.2f ******************\n\n", s.s, s.c);
	position(5, &target, &m, &s);

	printf("\nnow at x=%.2f y=%.2f time = %.2f", s.x, s.y, s.t);
	course = .75;
	s.c = course;
	printf("\n\n********** speed=%.2f course=%.2f ******************\n\n", s.s, s.c);
	position(5, &target, &m, &s);

	printf("\nnow at x=%.2f y=%.2f time = %.2f", s.x, s.y, s.t);
	course = 0.95;
	s.c = course;
	printf("\n\n********** speed=%.2f course=%.2f ******************\n\n", s.s, s.c);
	position(5, &target, &m, &s);

	// ok, now change his speed while keeping course the same
	printf("\nnow at x=%.2f y=%.2f time = %.2f", s.x, s.y, s.t);
	speed = 12; 
	s.s = speed;
	printf("\n\n********** speed=%.2f course=%.2f ******************\n\n", s.s, s.c);
	position(5, &target, &m, &s);
	
	// ok, now change his speed while keeping course the same
	printf("\nnow at x=%.2f y=%.2f time = %.2f", s.x, s.y, s.t);
	speed = 15; 
	s.s = speed;
	printf("\n\n********** speed=%.2f course=%.2f ******************\n\n", s.s, s.c);
	position(5, &target, &m, &s);

	// ok, now change his speed and course
	printf("\nnow at x=%.2f y=%.2f time = %.2f", s.x, s.y, s.t);
	speed = 10; 
	course = 1.25;
	s.s = speed;
	s.c = course;
	printf("\n\n********** speed=%.2f course=%.2f ******************\n\n", s.s, s.c);
	position(5, &target, &m, &s);

	fclose(logfile);
	printf("\n\n************ autotracker done ****************\n");
	return 0;
	}