#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. POS_JUMP_THRESHOLD is informational
 * logging only - the distance-diff spike check above already rejects
 * inconsistent position jumps regardless of size. ALT_JUMP_THRESHOLD both
 * logs and (combined with the same spike shape) rejects an inconsistent
 * altitude jump - unlike position there was previously no altitude
 * consistency check at all, letting a poor-geometry fix's bogus altitude
 * (right sats/HDOP, wrong solution - e.g. just after reacquiring signal)
 * straight into the accepted track. */
#define POS_JUMP_THRESHOLD	100.0	/* meters between consecutive raw fixes */
#define ALT_JUMP_THRESHOLD	50.0	/* meters between consecutive raw fixes */

/* Post-gap settling: a freshly reacquired fix's position/altitude solution
 * can keep drifting for several seconds after reacquisition even while
 * reported quality (satellite count, HDOP) already looks good throughout -
 * seen on a real recorded track exiting an underground space: HDOP was
 * already 0.81/17 sats just 7s after reacquiring, yet elevation kept
 * changing by several meters per sample for another ~10s after that before
 * settling to normal (sub-meter) noise. Sat count/HDOP were tried as a
 * "has it converged" signal and rejected: both fluctuate (sat count can
 * drop as easily as rise while the receiver's tracking set changes) in a
 * way that gave false "stable" readings well before the position had
 * actually settled. A fixed wall-clock window matches what the real data
 * showed converging by, without depending on a quality signal that proved
 * unreliable for this specific purpose in practice.
 * Withhold distance/elevation/GPX credit for points within this window
 * after a gap, while still updating the Kalman/last-position state normally
 * so a genuine tunnel-with-real-displacement case isn't penalized beyond
 * losing GAP_SETTLE_SECONDS worth of distance right at the exit. */
#define GAP_SETTLE_SECONDS	18	/* real data showed convergence taking ~15-17s after reacquisition */

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;
	time_t gap_settle_until; /* points before this time are held back post-gap; 0 = not settling */
} gpx;

float kalman_predict(struct kalman_s *k, float data);
void kalman_init(struct kalman_s *k);
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 = NAN; /* sentinel: no prior point yet, distinct from a real lat=0 (equator) fix */
	gpx.last_distance_point.time = 0;
	gpx.elevation.initialized = 0;
	gpx.point_count = 0;
	gpx.gap_settle_until = 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 tracking_is_paused();
}

unsigned char gpx_write(struct location_s *loc, FIL *file) {
	unsigned int bw;
	const char *time;
	unsigned char paused = tracking_is_paused();

	if (paused) {
		if (!gpx.paused) {
			strcpy_P(buf, xml_trkseg_end);
			gpx.paused = 1;
			gpx.point_count = 0;
		} 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;
		}
		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"));
	}

	{
		unsigned char ret;
		unsigned long int t0 = get_uptime_ms();
		ret = f_write(file, buf, strlen(buf), &bw);
		io_mark(t0);
		return ret;
	}
}

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

/* Call once, whenever fix_gap_detected() reports a gap on the point about to
 * be passed to gpx_process_point() - see fix_gap_detected()'s comment for why
 * a gap (regardless of cause: fix loss, a 2D fix, poor HDOP, or the receiver
 * simply not producing lines) needs handling at all. Reset the Kalman/window
 * state that would otherwise splice the old and new fixes together, and
 * re-run the existing skip_points warm-up: a freshly reacquired fix can carry
 * a wildly wrong position/altitude for a few seconds while it converges (seen
 * after a real signal loss - a nominally-3D, in-range-HDOP fix still a couple
 * hundred meters off in altitude until more satellites came in), the same way
 * a fix has never been trusted right after power-on. */
void gpx_reset_gap(void) {
	if (get_flag(CONFFLAG_VERBOSE_LOG))
		xputs_P(PSTR("FIX GAP: resetting warm-up/filter state\r\n"));
	kalman_init(&gpx.kalman[0]);
	kalman_init(&gpx.kalman[1]);
	gpx.prev_points.count = 0;
	gpx.avg_count = 0;
	gpx.avg_store.lat = 0;
	gpx.avg_store.lon = 0;
	gpx.avg_store.time = 0;
	gpx.point_count = 0;
	/* Without this, the first point(s) accepted after the gap (still inside
	 * the prev_points warm-up above, so exempt from the distdiff/altdiff
	 * spike checks until that window refills) would add one uncontrolled
	 * distance/elevation segment straight from the pre-gap position - which
	 * may be stale by an arbitrary, unknown amount of real movement during
	 * the gap - with none of the usual spike protection. Clearing it forces
	 * distance/elevation accumulation to (re)start fresh from the first
	 * post-gap point actually reached once warm-up completes, the same way
	 * it starts fresh after power-on. */
	gpx.last_distance_point.lat = NAN;
	/* -1 (all-ones): "settling, but the deadline isn't computed yet" - the
	 * real deadline needs the first post-gap point's own timestamp, which
	 * isn't available here (this runs before gpx_process_point() sees that
	 * point), so gpx_process_point() computes gap_settle_until = that
	 * point's time + GAP_SETTLE_SECONDS on its first point after this. */
	gpx.gap_settle_until = (time_t)-1;
}

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

	if (tracking_is_paused())
		System.points_paused++;

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

	if (gpx.gap_settle_until) {
		if (gpx.gap_settle_until == (time_t)-1)
			gpx.gap_settle_until = loc->time + GAP_SETTLE_SECONDS;
		if (loc->time < gpx.gap_settle_until) {
			if (get_flag(CONFFLAG_VERBOSE_LOG))
				xputs_P(PSTR("GAP SETTLE: waiting for fix to stabilize\r\n"));
			System.points_skipped++;
			return;
		}
		gpx.gap_settle_until = 0; /* window elapsed - resume normal processing below */
	}

	/* 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++;
		System.points_accepted++;

		/* Calculate distance and elevation from filtered points */
		if (!isnan(gpx.last_distance_point.lat)) {
			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. KALMAN_ERR_MAX is a threshold in degrees
		 * of latitude - a degree of longitude covers less ground the further
		 * from the equator, by a factor of cos(latitude), so lon_err is
		 * scaled up here to the equivalent latitude-degree error before the
		 * same threshold is applied to both. Without this, the effective
		 * east-west tolerance in meters would shrink at higher latitudes
		 * (e.g. roughly half as permissive at 60 degrees as at the equator),
		 * causing spurious rejects that have nothing to do with fix quality. */
		lat_err = fabs(loc->lat - lat_est);
		lon_err = fabs(loc->lon - lon_est) * cosf(loc->lat * M_PI / 180.0);
	//	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));
			/* Same spike test as the distance check below, applied to altitude:
			 * a poor-geometry fix (e.g. right after reacquiring signal) can pass
			 * the satellite-count/HDOP gate in nmea.c yet still carry a wildly
			 * wrong altitude, so catch it here the same way a position spike is
			 * caught - a jump surrounded by two much smaller ones on both sides.
			 * TODO: GSA's VDOP would be a more direct altitude-quality gate than
			 * this shape heuristic, but VDOP is only in GSA, which is parsed
			 * after GGA within the epoch (see nmea.c's gp_gga_parse HDOP
			 * comment) - using it here would need buffering a point across the
			 * epoch boundary until VDOP arrives. Left as a future refinement. */
			float alt12 = fabs(prev_points_get(0)->alt - prev_points_get(1)->alt);
			float alt34 = fabs(prev_points_get(2)->alt - prev_points_get(3)->alt);
			float alt32 = fabs(prev_points_get(2)->alt - prev_points_get(1)->alt);
			if (get_flag(CONFFLAG_VERBOSE_LOG))
				xprintf(PSTR("New distance: %fm\r\n"), dist32);
			/* A real spike must stand out on BOTH sides - bigger than the
			 * older segment AND bigger than the newer one. The previous
			 * "dist34 > dist12 && dist32 > dist12" shape only checked dist32
			 * against dist12 twice from different angles, so it stayed true
			 * for every step of a sustained, monotonic acceleration (dist12 <
			 * dist32 < dist34, completely normal e.g. when picking up pace
			 * while hiking) - rejecting real point after real point until the
			 * trend broke, then crediting all of that accumulated real
			 * distance to whichever single point finally got through, as one
			 * artificially fast-looking GPX segment. Comparing dist32 against
			 * both neighbors instead lets a genuine acceleration/deceleration
			 * ramp through cleanly while still catching an isolated outlier
			 * surrounded by two much smaller segments. */
			if(dist32 > dist12 && dist32 > dist34){
				if (get_flag(CONFFLAG_VERBOSE_LOG))
					xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
				log_reject(REJECT_REASON_DISTDIFF);
				return;
			}
			if(alt32 > ALT_JUMP_THRESHOLD && alt32 > alt12 && alt32 > alt34){
				if (get_flag(CONFFLAG_VERBOSE_LOG))
					xputs_P(PSTR("ALTITUDE DIFF REJECT\r\n"));
				log_reject(REJECT_REASON_ALTDIFF);
				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"));
		log_reject_flush(); /* close out any pending reject burst now that good data has resumed */
		System.points_accepted++;

		/* Calculate distance and elevation for accepted point */
		if (!isnan(gpx.last_distance_point.lat)) {
			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;
		/* Use the newest accepted point's time, not e.g. the midpoint's: the
		 * averaged lat/lon reflects real movement up through this instant, so
		 * timestamping it any earlier claims that movement happened faster
		 * than it did. This matters most exactly when the accepted points
		 * feeding this average were NOT evenly spaced in real time (e.g. a
		 * reject streak between two of them) - an earlier timestamp there
		 * would understate the elapsed time for the position delta and
		 * inflate the speed an external tool derives from consecutive GPX
		 * points. */
		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;
}

/* Initial bearing (degrees, 0-360, 0=north) from pos1 to pos2 - used by the
 * auto-pause resume-by-distance check (autopause.c) to tell a real, directed
 * departure from a smooth-but-directionless position drift (e.g. a satellite
 * geometry change reshaping the fix while the receiver doesn't move at all). */
float bearing(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 dlon = (pos2->lon - pos1->lon) * M_PI / 180.0;
	float y = sinf(dlon) * cosf(lat2);
	float x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(dlon);
	float deg = atan2f(y, x) * 180.0 / M_PI;
	if (deg < 0)
		deg += 360.0;
	return deg;
}

void add_distance(float dist) {
	unsigned char paused = tracking_is_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 = tracking_is_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 = tracking_is_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;
}