#include <stdint.h>

#include "serial_irq.h"
#include "main_time.h"

/*******************
 * Timer functions *
 *******************/

uint64_t time_last_local; // timestamp at last time sync
uint64_t time_last_world; // offset at last time sync

/* Initialization */
void time_init(void)
{
	// nothing for now
}

/**
 * Generate time stamp for an async event:
 *   time:  drift compensated wall-clock time
 *   stamp: event timestamp from PIT Module
 */
uint64_t time_to_world(uint64_t local)
{
	uint64_t elapsed = local - time_last_local;
	return time_last_world + elapsed;
}

uint64_t time_to_local(uint64_t world)
{
	uint64_t elapsed = world - time_last_world;
	return time_last_local + elapsed;
}

/**
 * Synchronize the timer internal state with updates
 * from an external time sync message.
 *   local: our internal timestamp for the event
 *   world: reference timestamp from the other device
 */
void time_ext_init(uint64_t local, uint64_t world)
{
	sirq_printf("initialize clocks: %d -> %d\r\n",
			(int)(local/NSEC_PER_SEC),
			(int)(world/NSEC_PER_SEC));

	time_last_local = local;
	time_last_world = world;
}

/**
 * Synchronize the timer internal state with updates
 * from an external time sync message.
 *   local: our internal timestamp for the event
 *   world: reference timestamp from the other device
 */
void time_ext_sync(uint64_t local, uint64_t world)
{
	uint64_t guess = time_to_world(local);

	time_last_local = local;
	time_last_world = (guess/2) + (world/2);
	//time_last_world = (guess * 3 / 4) + (world * 1 / 4);
	//time_last_world =
	//	(guess - (        guess / 2)) +
	//	(world - (world - world / 2));
	//time_last_world =
	//	(guess - (guess - guess / 4)) +
	//	(world - (        world / 4));

	world = time_last_world;

//#ifdef VERBOSE
#if 1
	uint64_t error = world > guess ? world - guess :
	                 guess > world ? guess - world : 0;
	int      ahead = guess > world;
	sirq_printf("syncing clocks: %6d=%d.%04u -> %d.%04u (err: %s%ld.%09lu)\r\n",
			(int)((local / NSEC_PER_SEC)),
			(int)((guess / NSEC_PER_SEC)),
			(int)((guess % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)),
			(int)((world / NSEC_PER_SEC)),
			(int)((world % NSEC_PER_SEC)/(NSEC_PER_SEC/10000)),
			ahead ? "-" : " ",
			(int32_t )(error / (int64_t)NSEC_PER_SEC),
			(uint32_t)(error % (int64_t)NSEC_PER_SEC));
#endif
//#endif
}

void time_printf(const char *label, uint64_t local)
{
	if (local > 1000000 * NSEC_PER_SEC) {
		sirq_printf("%s -- %14s -> %d.%09u\r\n",
				label, "",
				(int)(local / NSEC_PER_SEC),
				(int)(local % NSEC_PER_SEC));
	} else {
		uint64_t world = time_to_world(local);
		sirq_printf("%s -- %4d.%09u -> %d.%09u\r\n",
				label,
				(int)(local / NSEC_PER_SEC),
				(int)(local % NSEC_PER_SEC),
				(int)(world / NSEC_PER_SEC),
				(int)(world % NSEC_PER_SEC));
	}
}