#include "gps_time_anchor.h"

#include <math.h>
#include <stddef.h>

static double gps_time_normalize_week(double tow_sec, int* week)
{
	while(tow_sec >= GPS_TIME_WEEK_SECONDS)
	{
		tow_sec -= GPS_TIME_WEEK_SECONDS;
		(*week)++;
	}
	while(tow_sec < 0.0)
	{
		tow_sec += GPS_TIME_WEEK_SECONDS;
		(*week)--;
	}
	return tow_sec;
}

int gps_time_resolve_how_week(bool previous_valid,
                              int previous_week,
                              double previous_tow_sec,
                              int fallback_week,
                              double how_tow_sec)
{
	if(!previous_valid || !isfinite(previous_tow_sec) || !isfinite(how_tow_sec))
	{
		return fallback_week;
	}

	int week = previous_week;
	double delta_sec = how_tow_sec - previous_tow_sec;
	if(delta_sec < -GPS_TIME_HALF_WEEK_SECONDS)
	{
		week++;
	}
	else if(delta_sec > GPS_TIME_HALF_WEEK_SECONDS)
	{
		week--;
	}
	return week;
}

bool gps_time_from_sample_anchor(int anchor_week,
                                 double anchor_tow_sec,
                                 uint32_t anchor_raw_sample,
                                 double anchor_corrected_sample,
                                 uint32_t event_raw_sample,
                                 double event_corrected_sample,
                                 double samples_per_second,
                                 double max_age_sec,
                                 int* event_week_out,
                                 double* event_tow_sec_out,
                                 double* elapsed_sec_out)
{
	if((event_week_out == NULL) || (event_tow_sec_out == NULL) ||
	   !(samples_per_second > 0.0) || !(max_age_sec > 0.0) ||
	   !isfinite(anchor_tow_sec) || !isfinite(anchor_corrected_sample) ||
	   !isfinite(event_corrected_sample))
	{
		return false;
	}

	/* The integer part is unwrapped modulo 2^32.  Preserve the code-phase
	 * fractions from both simultaneous ABS_SAMPLE/REM_CODE_PHASE latches. */
	int32_t raw_delta = (int32_t)(event_raw_sample - anchor_raw_sample);
	double anchor_fraction = anchor_corrected_sample - (double)anchor_raw_sample;
	double event_fraction = event_corrected_sample - (double)event_raw_sample;
	double sample_delta = (double)raw_delta + event_fraction - anchor_fraction;
	double elapsed_sec = sample_delta / samples_per_second;
	if(!isfinite(elapsed_sec) || (elapsed_sec < -0.001) ||
	   (elapsed_sec > max_age_sec))
	{
		return false;
	}

	int event_week = anchor_week;
	double event_tow_sec = gps_time_normalize_week(anchor_tow_sec + elapsed_sec,
	                                                &event_week);
	*event_week_out = event_week;
	*event_tow_sec_out = event_tow_sec;
	if(elapsed_sec_out != NULL)
	{
		*elapsed_sec_out = elapsed_sec;
	}
	return true;
}