#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

/* Elevation dead-band + smoothing, applied to the filtered ascent/descent totals */
#define ELEV_SMOOTH_TAU		30.0	/* seconds */
#define ELEV_DEADBAND		5.0		/* meters */

/* Large-jump detection thresholds (informational logging only, points are not rejected because of these) */
#define POS_JUMP_THRESHOLD	100.0	/* meters between consecutive raw fixes */
#define ALT_JUMP_THRESHOLD	50.0	/* meters between consecutive raw fixes */

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

struct elevation_s {
	unsigned char initialized;
	float smoothed_alt;
	float baseline;
	time_t last_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];
	struct elevation_s elevation;
} 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);
float elevation_process(float alt, time_t time);
void add_elevation_filtered(float amount, unsigned char is_gain);

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.elevation.initialized = 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 always calculate distance/elevation from filtered data */
		if (get_flag(CONFFLAG_VERBOSE_LOG))
			xputs_P(PSTR("Write with filters disabled\r\n"));

		gpx_write(loc, file);
		System.points_written++;

		/* 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);
			if (dist > POS_JUMP_THRESHOLD)
				log_jump(0, dist);
			add_distance(dist);
			ele_change = filtered_loc.alt - gpx.last_distance_point.alt;
			if (fabs(ele_change) > ALT_JUMP_THRESHOLD)
				log_jump(1, ele_change);
			add_elevation(ele_change);
		}
		elevation_process(filtered_loc.alt, filtered_loc.time);
		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){
			if (get_flag(CONFFLAG_VERBOSE_LOG))
				xputs_P(PSTR("KALMAN REJECT\r\n"));
			log_reject(REJECT_REASON_KALMAN);
			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));
			if (get_flag(CONFFLAG_VERBOSE_LOG))
				xprintf(PSTR("New distance: %fm\r\n"), dist32);
			if(dist34 > dist12 && dist32 > dist12){
				if (get_flag(CONFFLAG_VERBOSE_LOG))
					xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
				log_reject(REJECT_REASON_DISTDIFF);
				return;
			}
			if (dist32 > POS_JUMP_THRESHOLD)
				log_jump(0, dist32);
			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);
				if (get_flag(CONFFLAG_VERBOSE_LOG))
					xputs_P(PSTR("NEW\r\n"));
			} else {
				return;
			}
		}

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

		if (get_flag(CONFFLAG_VERBOSE_LOG))
			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;
			if (fabs(ele_change) > ALT_JUMP_THRESHOLD)
				log_jump(1, ele_change);
			add_elevation(ele_change);
		}
		elevation_process(ptr->alt, ptr->time);
		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;
			nloc.alt = gpx.elevation.smoothed_alt; /* filtered (smoothed) altitude, for the filtered GPX write */
			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);
			System.points_written++;
		}
	}
	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;
	if (get_flag(CONFFLAG_VERBOSE_LOG))
		xprintf(PSTR("Distance: %.2f m; sum: %.2f m\r\n"), (double)dist, (double)System.distance/100.0);
}

/* Unfiltered (raw), per-point gain/loss - kept only for comparison against the
 * filtered (dead-band+smoothed) totals in the periodic status line/session summary. */
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_raw += (ele_change+0.05)*10.0;
		} else if (ele_change < 0) {
			System.elevation_loss_raw += (-ele_change+0.05)*10.0;
		}
	}
	if (get_flag(CONFFLAG_VERBOSE_LOG))
		xprintf(PSTR("Elevation change: %.1f m; raw gain: %.1f m, raw loss: %.1f m\r\n"),
			(double)ele_change, (double)System.elevation_gain_raw/10.0, (double)System.elevation_loss_raw/10.0);
}

/* Filtered (dead-band + smoothed) gain/loss, used for display, GPX ascent stats
 * and the periodic status line/session summary. */
void add_elevation_filtered(float amount, unsigned char is_gain) {
	unsigned long int dm = (unsigned long int)(amount*10.0 + 0.5);
	if (is_gain)
		System.elevation_gain += dm;
	else
		System.elevation_loss += dm;
}

/* Exponential smoothing (~30s time constant) followed by a 5m dead-band on the
 * result, so that a step in the smoothed altitude only counts once it clears
 * the dead-band, and only the amount past the dead-band edge is credited. */
float elevation_process(float alt, time_t time) {
	unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
	unsigned long int alt_dm;
	float delta;

	if (!gpx.elevation.initialized) {
		gpx.elevation.smoothed_alt = alt;
		gpx.elevation.baseline = alt;
		gpx.elevation.last_time = time;
		gpx.elevation.initialized = 1;
	} else {
		float dt = (float)(time - gpx.elevation.last_time);
		float alpha;
		if (dt <= 0)
			dt = 1.0;
		gpx.elevation.last_time = time;
		alpha = dt / (ELEV_SMOOTH_TAU + dt);
		gpx.elevation.smoothed_alt += alpha * (alt - gpx.elevation.smoothed_alt);
	}

	if (gpx.elevation.smoothed_alt > 0) {
		alt_dm = (unsigned long int)(gpx.elevation.smoothed_alt*10.0 + 0.5);
		if (alt_dm > System.alt_max)
			System.alt_max = alt_dm;
	}

	if (paused) {
		/* Discard the dead-band reference drift accumulated while paused,
		 * the same way distance/raw elevation data is discarded when paused. */
		gpx.elevation.baseline = gpx.elevation.smoothed_alt;
	} else {
		delta = gpx.elevation.smoothed_alt - gpx.elevation.baseline;
		if (delta > ELEV_DEADBAND) {
			add_elevation_filtered(delta - ELEV_DEADBAND, 1);
			gpx.elevation.baseline = gpx.elevation.smoothed_alt - ELEV_DEADBAND;
		} else if (delta < -ELEV_DEADBAND) {
			add_elevation_filtered(-delta - ELEV_DEADBAND, 0);
			gpx.elevation.baseline = gpx.elevation.smoothed_alt + ELEV_DEADBAND;
		}
	}
	return gpx.elevation.smoothed_alt;
}