#ifdef PC_BUILD
#include "../gps-test-tool/main.h"
#else
#include "main.h"
#endif

#define KALMAN_Q	8.5e-6
#define KALMAN_R	4e-5
#define KALMAN_ERR_MAX	6e-4

__flash const char xml_header[] = "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
		"<gpx xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns=\"http://www.topografix.com/GPX/1/1\" xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\" version=\"1.1\" creator=\"k4be\">\n";
__flash const char xml_trk_start[] = "\t<trk>\n";
__flash const char xml_trkseg_end[] = "\t\t</trkseg>\n";
__flash const char xml_trkseg_start[] = "\t\t<trkseg>\n";

FIL gpx_file;
static char buf[sizeof(xml_header)+sizeof(xml_trk_start)+2];

struct kalman_s {
	unsigned char initialized;
	float x_est_last;
	float P_last;
	float Q;
	float R;
	float K;
};


#define PREV_POINTS_LENGTH	4
#define AVG_COUNT	3
#define MIN_DIST_DELTA	2.0

struct prev_points_s {
	struct location_s data[PREV_POINTS_LENGTH];
	unsigned char start;
	unsigned char count;
};

struct avg_store_s {
	float lat;
	float lon;
	time_t time;
};

static struct gpx_s {
	struct prev_points_s prev_points;
	unsigned char avg_count;
	unsigned char paused;
	unsigned char point_count;
	struct avg_store_s avg_store;
	struct location_s last_saved;
	struct location_s last_distance_point; /* Last accepted point for distance calculation */
	struct kalman_s kalman[2];
} gpx;

float kalman_predict(struct kalman_s *k, float data);
void kalman_init(struct kalman_s *k);
float distance(struct location_s *pos1, struct location_s *pos2);

void prev_points_append(struct location_s *new){
	gpx.prev_points.data[(gpx.prev_points.start + gpx.prev_points.count)%PREV_POINTS_LENGTH] = *new;
	if(++gpx.prev_points.count > PREV_POINTS_LENGTH){
		gpx.prev_points.count--;
		gpx.prev_points.start++;
		gpx.prev_points.start %= PREV_POINTS_LENGTH;
	}
}

struct location_s *prev_points_get(unsigned char index){
	unsigned char i, addr = gpx.prev_points.start;
	for(i=0; i<index; i++){
		addr++;
	}
	addr %= PREV_POINTS_LENGTH;
	return &gpx.prev_points.data[addr];
}


unsigned char gpx_init(FIL *file) {
	unsigned int bw;

	kalman_init(&gpx.kalman[0]);
	kalman_init(&gpx.kalman[1]);
	gpx.prev_points.count = 0;
	gpx.avg_count = 0;
	gpx.last_saved.lon = 0;
	gpx.last_saved.lat = 0;
	gpx.last_saved.time = 0;
	gpx.last_distance_point.lon = 0;
	gpx.last_distance_point.lat = 0;
	gpx.last_distance_point.time = 0;

	gpx.paused = 1; /* make it add a <trkseg> tag */

	strcpy_P(buf, xml_header);
	strcat_P(buf, xml_trk_start);
	return f_write(file, buf, strlen(buf), &bw);
}

void gpx_save_single_point(struct location_s *loc) {
	FIL gpx;
	UINT bw;
	unsigned char err = 0;
	char *time = get_iso_time(loc->time, 1);
	iso_time_to_filename(time);
	xsprintf(buf, PSTR("%s-POINT.GPX"), time);
	xprintf(PSTR("Writing single point in %s\r\n"), buf);
	if ((err = f_open(&gpx, buf, FA_WRITE | FA_OPEN_ALWAYS))) {
		f_close(&gpx);
//		System.status = STATUS_FILE_OPEN_ERROR;
		xputs_P(PSTR("File open error\r\n"));
		return;	/* Failed to open file */
	}
	strcpy_P(buf, xml_header);
	err |= f_write(&gpx, buf, strlen(buf), &bw);
	xsprintf(buf, PSTR("\t<wpt lat=\"%.8f\" lon=\"%.8f\"></wpt>\n</gpx>\n"), loc->lat, loc->lon);
	err |= f_write(&gpx, buf, strlen(buf), &bw);
	err |= f_close(&gpx);
	if (err) {
		/* TODO */
	}
}

unsigned char is_paused(void) {
	return gpx.paused;
}

unsigned char gpx_write(struct location_s *loc, FIL *file) {
	unsigned int bw;
	const char *time;
	unsigned char paused = System.tracking_paused || System.tracking_auto_paused;

	if (paused) {
		if (!gpx.paused) {
			strcpy_P(buf, xml_trkseg_end);
			gpx.paused = 1;
			gpx.point_count = 0;
			System.current_pause_start = utc;
		} else {
			return 0; /* nothing to store */
		}
	} else {
		if (gpx.paused) {
			strcpy_P(buf, xml_trkseg_start);
			f_write(file, buf, strlen(buf), &bw);
			gpx.paused = 0;
			if (System.current_pause_start)
				System.pause_time += utc - System.current_pause_start;
		}
		time = get_iso_time(loc->time, 0);
		xsprintf(buf, PSTR("\t\t\t<trkpt lat=\"%.8f\" lon=\"%.8f\">\n\t\t\t\t<ele>%.2f</ele>\n\t\t\t\t<time>%s</time>\n"), loc->lat, loc->lon, loc->alt, time);
		strcat_P(buf, PSTR("\t\t\t</trkpt>\n"));
	}

	return f_write(file, buf, strlen(buf), &bw);
}

unsigned char gpx_close(FIL *file) {
	unsigned int bw;
	buf[0] = '\0';
	if (!gpx.paused)
		strcpy_P(buf, xml_trkseg_end);
	strcat_P(buf, PSTR("\t</trk>\n</gpx>\n"));
	f_write(file, buf, strlen(buf), &bw);
	return f_close(file);
}

void gpx_process_point(struct location_s *loc, FIL *file){
	float lon_est, lon_err, lat_est, lat_err, dist;
	struct location_s *ptr;
	struct location_s nloc;
	struct location_s filtered_loc;

	if (gpx.point_count < System.conf.skip_points) { /* Skipping initial points */
		gpx.point_count++;
		return;
	}

	/* Always apply Kalman filtering for distance calculation */
	lat_est = kalman_predict(&gpx.kalman[0], loc->lat);
	lon_est = kalman_predict(&gpx.kalman[1], loc->lon);

	filtered_loc.lat = lat_est;
	filtered_loc.lon = lon_est;
	filtered_loc.time = loc->time;
	filtered_loc.alt = loc->alt;

	if (get_flag(CONFFLAG_DISABLE_FILTERS)) {
		/* Write unfiltered data to GPX, but calculate distance from filtered data */
		xputs_P(PSTR("Write with filters disabled\r\n"));

		gpx_write(loc, file);

		/* Calculate distance and elevation from filtered points */
		if (gpx.last_distance_point.lat != 0) {
			float ele_change;
			dist = distance(&gpx.last_distance_point, &filtered_loc);
			add_distance(dist);
			ele_change = filtered_loc.alt - gpx.last_distance_point.alt;
			add_elevation(ele_change);
		}
		gpx.last_distance_point = filtered_loc;

	} else {
		/* Apply Kalman error check */
		lat_err = fabs(loc->lat - lat_est);
		lon_err = fabs(loc->lon - lon_est);
	//	xprintf(PSTR("lat_err: %e, lon_err: %e, limit: %e\r\n"), lat_err, lon_err, (float)KALMAN_ERR_MAX);
		if(lat_err > KALMAN_ERR_MAX || lon_err > KALMAN_ERR_MAX){
			xputs_P(PSTR("KALMAN REJECT\r\n"));
			return;
		}

		prev_points_append(&filtered_loc);

		if(gpx.prev_points.count == PREV_POINTS_LENGTH){
			float dist12 = distance(prev_points_get(0), prev_points_get(1));
			float dist34 = distance(prev_points_get(2), prev_points_get(3));
			float dist32 = distance(prev_points_get(2), prev_points_get(1));
			xprintf(PSTR("New distance: %fm\r\n"), dist32);
			if(dist34 > dist12 && dist32 > dist12){
				xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
				return;
			}
			ptr = prev_points_get(PREV_POINTS_LENGTH - 2);
		} else {
			if(gpx.prev_points.count >= PREV_POINTS_LENGTH-2){
				ptr = prev_points_get(gpx.prev_points.count - 2);
				xputs_P(PSTR("NEW\r\n"));
			} else {
				return;
			}
		}

		if(distance(&gpx.last_saved, ptr) < MIN_DIST_DELTA){
			xputs_P(PSTR("Too small position change REJECT\r\n"));
			return;
		}

		xputs_P(PSTR("ACCEPT\r\n"));

		/* Calculate distance and elevation for accepted point */
		if (gpx.last_distance_point.lat != 0) {
			float ele_change;
			dist = distance(&gpx.last_distance_point, ptr);
			add_distance(dist);
			ele_change = ptr->alt - gpx.last_distance_point.alt;
			add_elevation(ele_change);
		}
		gpx.last_distance_point = *ptr;

		gpx.avg_store.lat += ptr->lat;
		gpx.avg_store.lon += ptr->lon;
		if(gpx.avg_count == AVG_COUNT/2)
			gpx.avg_store.time = ptr->time;

		if(++gpx.avg_count == AVG_COUNT){
			nloc.lat = gpx.avg_store.lat / AVG_COUNT;
			nloc.lon = gpx.avg_store.lon / AVG_COUNT;
			nloc.time = gpx.avg_store.time;
			gpx.avg_count = 0;
			gpx.avg_store.lat = 0;
			gpx.avg_store.lon = 0;
			gpx.avg_store.time = 0;
			gpx.last_saved = nloc;
			gpx_write(&nloc, file);
		}
	}
	if (System.time_start == 0)
		System.time_start = utc;
}

void kalman_init(struct kalman_s *k){
	k->initialized = 0;
	k->P_last = 0;
	//the noise in the system
	k->Q = KALMAN_Q; // process variance
	k->R = KALMAN_R; // measurement variance
	k->K = 0;
}

float kalman_predict(struct kalman_s *k, float data){
	if(!k->initialized){
		//initial values for the kalman filter
		k->x_est_last = data;
		k->initialized = 1;
		return data;
	}
	//do a prediction
	float x_temp_est = k->x_est_last;
	float P_temp = k->P_last + k->Q;
	//calculate the Kalman gain
	k->K = P_temp * (1.0/(P_temp + k->R));
	//correct
	float x_est = x_temp_est + k->K * (data - x_temp_est);
	k->P_last = (1 - k->K) * P_temp;
	k->x_est_last = x_est;
	return x_est;
}

#define R_EARTH	6371e3 // m

float distance(struct location_s *pos1, struct location_s *pos2){
	float lat1 = pos1->lat * M_PI / 180.0;
	float lat2 = pos2->lat * M_PI / 180.0;
	float dlat = (pos2->lat - pos1->lat) * M_PI / 180.0;
	float dlon = (pos2->lon - pos1->lon) * M_PI / 180.0;
	float a = sinf(dlat/2.0) * sinf(dlat/2.0) + cosf(lat1) * cosf(lat2) * sinf(dlon/2.0) * sinf(dlon/2.0);
	float c = 2 * atan2f(sqrtf(a), sqrtf(1-a));
	float ret = R_EARTH * c;
	return ret;
}

void add_distance(float dist) {
	unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
	if (!paused)
		System.distance += (dist+0.005)*100.0;
	xprintf(PSTR("Distance: %f m; sum: %f m\r\n"), dist, System.distance/100.0);
}

void add_elevation(float ele_change) {
	unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
	if (!paused) {
		if (ele_change > 0) {
			System.elevation_gain += (ele_change+0.05)*10.0;
		} else if (ele_change < 0) {
			System.elevation_loss += (-ele_change+0.05)*10.0;
		}
	}
	xprintf(PSTR("Elevation change: %f m; gain: %f m, loss: %f m\r\n"),
		ele_change, System.elevation_gain/10.0, System.elevation_loss/10.0);
}