#include "messages.h"

#include "mbed.h"
#include "serial_irq.h"
#include "serial_dma.h"
#include "timer_dma.h"

/**
 * Mode of operation:
 *   Devices 1 and 2 synchronize clocks using serial messages.
 *
 *   1. Each serial message timestamped using the hardware timer capture
 *      registers in both the sender and receiver.
 *   2. The sender transmits the send timestamp during the next time-sync
 *      message.
 *   3. The receiver then compares the senders timestamp with it's own
 *      timestamp for the corresponding messages and calculates an offset.
 *   4. The offset is used to compensate the receivers local clock.
 *
 *   Time synchronization is performed in both directions.
 */

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

#define NSEC_PER_SEC 1000000000ULL

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

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

/**
 * 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 0
	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)
{
	uint64_t world = time_to_world(local);
	sirq_printf("%s -- %d.%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));
}

/*********************
 * Signal generation *
 *********************/

static uint32_t *emit_pcr    = 0; // transmit pin name

static uint64_t  emit_start  = 0; // transmit start time (world time)
static uint64_t  emit_period = 0; // transmit period
static uint64_t  emit_due    = 0; // next transmit (world time)

static uint32_t  emit_slack  = 0; // how far ahead we need to schedule, in us
static uint32_t  emit_worst  = 0; // worst-case latency in task table

void emit_init(int alt, PinName pin, PinMode mode)
{
	// Find pin
	emit_pcr = (uint32_t*)(PORTA_BASE + pin);

	// Enable clocks
	SIM->SCGC6            |= SIM_SCGC6_TPM1_MASK;
	SIM->SOPT2            |= SIM_SOPT2_TPMSRC(1);

	// Set pin mode
	emit_pcr[0]            = PORT_PCR_ISF_MASK
	                       | PORT_PCR_MUX(alt)
	                       | mode;

	// Setup Timer/PWM Module
	TPM1->SC               = TPM_SC_TOF_MASK;
	TPM1->CNT              = TPM_CNT_COUNT(0);
	TPM1->MOD              = TPM_MOD_MOD(0xFFFF);

	TPM1->CONTROLS[0].CnSC = TPM_CnSC_CHF_MASK    // clear flag
	                       | TPM_CnSC_MSB_MASK    // set output highon match,
	                       | TPM_CnSC_ELSB_MASK   // cleared on overflow
	                       | TPM_CnSC_ELSA_MASK;  // ..

	TPM1->STATUS           = TPM_STATUS_CH0F_MASK
                               | TPM_STATUS_TOF_MASK;

	TPM1->CONF             = TPM_CONF_CSOO_MASK;
}

void emit_enable(uint64_t start, uint64_t period)
{
	const int slack_tick = 0x8000; // tune based on emit_worst

	emit_start  = start;
	emit_period = period;
	emit_due    = start + period;

	emit_slack  = slack_tick * 1000 / 24;

	time_printf("emit scheduled", emit_due);
}

#define CLOCKS(x) ((uint16_t)((x) * 24 / 1000))

void emit_schedule(uint64_t when)
{
	uint64_t now    = time_to_world(tdma_time());
	uint64_t start  = when - now;
	uint64_t stop   = start + 100000;

	// Disable timer
	TPM1->SC               = TPM_SC_TOF_MASK;

	// Set transmit time
	TPM1->CONTROLS[0].CnV  = CLOCKS(start);
	TPM1->MOD              = TPM_MOD_MOD(CLOCKS(stop));

	// Start the timer
	TPM1->SC               = TPM_SC_TOF_MASK
	                       | TPM_SC_PS(1)
	                       | TPM_SC_CMOD(1);

	// Debug
	//sirq_printf("emitting event\r\n");
}

void emit_transmit(uint64_t local, uint64_t world)
{
	static uint64_t prev = 0;

	// Record how how much time we have to reschedule
  	if (prev && (local-prev) > emit_worst)
  		emit_worst = (local-prev);
	prev = local;

	// Schedule task if needed
	if (emit_due && emit_period &&
	    world+emit_slack > emit_due) {
		emit_schedule(emit_due);
		emit_due += emit_period;
	}
}

/************************
 * Serial I/O functions *
 ************************/

typedef struct {
	int      index;
	int      state;
	uint8_t  buffer[256];
} parser_t;

static uint32_t serial_device_id   = 0;

const  uint64_t serial_sync_delay  = NSEC_PER_SEC / 100; // 1hz
static uint64_t serial_sync_due    = 0;

static tdma_t  *serial_tdma_rcv    = NULL;
static tdma_t  *serial_tdma_xmt    = NULL;

static uint64_t serial_prev_local  = 0;
static uint64_t serial_prev_seq    = 0;

static uint64_t serial_xmt_local   = 0;
static uint64_t serial_xmt_seq     = 0;

/**
 * Convert world to local time
 */
uint64_t serial_read_time(ntime_t time)
{
	return ((uint64_t)time.seconds) * NSEC_PER_SEC
	     + ((uint64_t)time.nanosec);
}

ntime_t serial_write_time(uint64_t time)
{
	ntime_t buf = {};
	buf.seconds = time / NSEC_PER_SEC;
	buf.nanosec = time % NSEC_PER_SEC;
	return buf;
}

/**
 * Output initialization message init message
 */
void serial_send_init(uint16_t device, uint64_t local)
{
}

/**
 * Output time sync message
 */
void serial_send_sync(sirq_t *port, uint64_t now)
{
	if (serial_sync_due == 0 || now < serial_sync_due)
		return; // not ready

	//sirq_printf("sending sync\r\n");

	// Calculate world time
	uint64_t local = 0;
	uint64_t world = time_to_world(serial_xmt_local);

	// Message data
	header_t   head;
	sync_msg_t body;

	// Transmit sync message
	head.header = MSG_HEADER;
	head.msgid  = MSG_ID_SYNC;
	head.length = sizeof(body);
	head.cksum  = 0; // todo

	body.seq          = serial_xmt_seq;
	body.time.seconds = world / NSEC_PER_SEC;
	body.time.nanosec = world % NSEC_PER_SEC;

	tdma_stop(serial_tdma_rcv);

	tdma_start(serial_tdma_xmt);
	sirq_write(port, &head, sizeof(head));
	sirq_write(port, &body, sizeof(body));
	tdma_stop(serial_tdma_xmt);

	// save transmit time
	int valid = tdma_stamp(serial_tdma_xmt, &local);
	if (!valid)
		sirq_printf("sync transmit time -- missed\r\n");
	else
		//time_printf("sync transmit time ", local);

	tdma_start(serial_tdma_rcv);

	serial_xmt_seq  += 1;
	serial_sync_due  = 0;
	serial_xmt_local = local;
}

/**
 * Output external event received message
 *   event: id of the received event
 *   time:  compensated timestamp of the event
 */
void serial_send_event(uint16_t event, uint64_t local)
{
	time_printf("event received", local);

#if 0
	// Message data
	header_t    head;
	event_msg_t body;

	uint64_t world = time_to_world(local);

	ntime_t time = {};
	time.seconds = (uint32_t)(world / NSEC_PER_SEC);
	time.nanosec = (uint32_t)(world % NSEC_PER_SEC);

	// Transmit sync message
	head.header = MSG_HEADER;
	head.msgid  = MSG_ID_SYNC;
	head.length = sizeof(body);
	head.cksum  = 0; // todo

	body.seq          = serial_xmt_seq;
	body.time.seconds = world / NSEC_PER_SEC;
	body.time.nanosec = world % NSEC_PER_SEC;

	tdma_stop(serial_tdma_rcv);

	tdma_start(serial_tdma_xmt);
	sirq_write(port, &head, sizeof(head));
	sirq_write(port, &body, sizeof(body));
	tdma_stop(serial_tdma_xmt);
#endif
}

/**
 * Handle init message
 */
void serial_handle_init(init_msg_t *msg)
{
	sirq_printf("initialize: %s %s %s %s %s\r\n",
		msg->valid & MSG_VALID_DEVICE ? "DEV"    : "dev",
		msg->valid & MSG_VALID_START  ? "START"  : "start",
		msg->valid & MSG_VALID_PERIOD ? "PERIOD" : "period",
		msg->valid & MSG_VALID_WORLD  ? "WORLD"  : "world",
		msg->valid & MSG_VALID_SYNC   ? "SYNC"   : "sync");
	sirq_printf("  dev    -- %d\r\n", msg->device);
	time_printf("  start ", serial_read_time(msg->start));
	time_printf("  period", serial_read_time(msg->period));
	time_printf("  world ", serial_read_time(msg->world));

	if (msg->valid & MSG_VALID_DEVICE)
		serial_device_id = msg->device;

	if (msg->valid & MSG_VALID_START ||
	    msg->valid & MSG_VALID_PERIOD) {
		uint64_t start  = serial_read_time(msg->start);
		uint64_t period = serial_read_time(msg->period);
		emit_enable(start, period);
	}

	if (msg->valid & MSG_VALID_WORLD) {
		uint64_t world = serial_read_time(msg->world);
		uint64_t local = tdma_time();
		time_ext_init(local, world);
	}

	if (msg->valid & MSG_VALID_SYNC)
		serial_sync_due = tdma_time() + serial_sync_delay;
}

/**
 * Handle sync message
 */
void serial_handle_sync(sync_msg_t *msg)
{
	// Read receive timestamp for next time sync message
	uint64_t current = 0;
	int valid = tdma_stamp(serial_tdma_rcv, &current);
	if (!valid)
		sirq_printf("sync receive time  -- missing\r\n");
	//else
	//	time_printf("sync receive time ", current);
	tdma_stop(serial_tdma_rcv);

	// Lookup times
	uint64_t world = ((uint64_t)msg->time.seconds) * NSEC_PER_SEC
		       + ((uint64_t)msg->time.nanosec);

	// Valid times timestamp
	if (serial_prev_seq == (msg->seq-1)) {
		uint64_t local = serial_prev_local;
		time_ext_sync(local, world);
	}

	// Queue transmit to other board
	serial_sync_due   = tdma_time() + serial_sync_delay;

	// Update states
	serial_prev_local = current;
	serial_prev_seq   = msg->seq;
}

/**
 * Handle event message
 */
void serial_handle_event(event_msg_t *msg)
{
}

/**
 * Deliver message
 */
void serial_deliver(int msgid, void *body)
{
	switch (msgid) {
		case MSG_ID_INIT:
			//sirq_printf("received init msg\r\n");
			serial_handle_init((init_msg_t*)body);
			break;
		case MSG_ID_SYNC:
			//sirq_printf("received sync msg\r\n");
			serial_handle_sync((sync_msg_t*)body);
			break;
		case MSG_ID_EVENT:
			//sirq_printf("received event msg\r\n");
			serial_handle_event((event_msg_t*)body);
			break;
	}
}

/**
 * Process serial receive messages
 */
void serial_receive(parser_t *parser, int byte)
{
	//sirq_printf("serial_receive - %02x\r\n", byte);

	// Lookup pointers
	header_t *head = (header_t*)parser->buffer;
	void     *body = (void*)(head+1);
	const int max_length = sizeof(parser->buffer)-sizeof(header_t);

	// Process uart messages
	parser->buffer[parser->index++] = byte;
	switch (parser->state) {
		case 0: // Search
			if (parser->index == sizeof(uint16_t)) {
				if (head->header == MSG_HEADER) {
					parser->state = 1;
				} else {
					parser->buffer[0] = parser->buffer[1];
					parser->index = 1;
				}
			}
			break;
		case 1: // Header
			if (parser->index == sizeof(header_t)) {
				if (head->length <= max_length &&
				    head->msgid  <= MSG_MAX_ID) {
					parser->state = 2;
				} else {
					parser->index = 0;
					parser->state = 0;
				}
			}
			break;
		case 2: // Data
			if (parser->index == (int)sizeof(header_t)+head->length) {
				serial_deliver(head->msgid, body);
				parser->index = 0;
				parser->state = 0;
			}
			break;
	}
}

/********************
 * Data definitions *
 ********************/

// LEDs
DigitalOut led1(LED1);
DigitalOut led2(LED2);

// Message Parsers
parser_t   parser_dbg;
parser_t   parser_bbb;
parser_t   parser_mbed;

// Serial IRQ
sirq_t    *sirq_dbg;
sirq_t    *sirq_bbb;
sirq_t    *sirq_mbed;

// Timer DMA
tdma_t    *tdma_evt;
tdma_t    *tdma_rcv;
tdma_t    *tdma_xmt;

/*********
 * Tasks *
 *********/

void task_serial(uint64_t local, uint64_t world)
{
	while (sirq_ready(sirq_dbg)) {
		//sirq_printf("serial recv - dbg\r\n");
		serial_receive(&parser_dbg,  sirq_getc(sirq_dbg));
	}

	while (sirq_ready(sirq_bbb)) {
		//sirq_printf("serial recv - bbb\r\n");
		serial_receive(&parser_bbb,  sirq_getc(sirq_bbb));
	}

	while (sirq_ready(sirq_mbed)) {
		//sirq_printf("serial recv - mbed\r\n");
		serial_receive(&parser_mbed, sirq_getc(sirq_mbed));
	}
}

void task_events(uint64_t local, uint64_t world)
{
	uint64_t event = 0;

#ifdef VERBOSE
	if (tdma_stamp(tdma_evt, &event)) {
		sirq_printf("event received - evt\r\n");
	if (tdma_stamp(tdma_rcv, &event))
		sirq_printf("event received - rcv\r\n");
	if (tdma_stamp(tdma_xmt, &event))
		sirq_printf("event received - xmt\r\n");
#endif

	if (tdma_stamp(tdma_evt, &event))
		serial_send_event(0, event);
	tdma_stop(tdma_evt);
	tdma_start(tdma_evt);
}

void task_sync(uint64_t local, uint64_t world)
{
	serial_send_sync(sirq_mbed, local);
}

void task_leds(uint64_t local, uint64_t world)
{
	static uint32_t which = 0;
	led1 = (which == 0);
	led2 = (which == 1);
	which ^= 1;
}

void task_emit(uint64_t local, uint64_t world)
{
	emit_transmit(local, world);
}

void task_debug(uint64_t local, uint64_t world)
{
	//tdma_debug(tdma_rcv);
	//tdma_debug(tdma_xmt);

	//sirq_debug(sirq_mbed);

#ifdef VERBOSE
	sirq_printf("background - %6u.%02u -> %u.%02u\r\n",
			(uint32_t)(local / NSEC_PER_SEC),
			(uint32_t)(local % NSEC_PER_SEC / 10000000),
			(uint32_t)(world / NSEC_PER_SEC),
			(uint32_t)(world % NSEC_PER_SEC / 10000000));
#endif
}

/********
 * Main *
 ********/

#define N_ELEM(x) (sizeof(x) / sizeof((x)[0]))

extern void test_main(void);
extern serial_t stdio_uart;

static struct {
	void (*task)(uint64_t, uint64_t);
	uint64_t period;
	uint64_t due;
} tasks[] = {
	{ task_serial, 0          }, // always
	{ task_events, 0          }, // always -- testing
	{ task_sync,   0          }, // always
	{ task_emit,   0          }, // always
	{ task_leds,   100000000  }, // 10hz
	{ task_debug,  1000000000 }, // 1hz
};

void background(void)
{
	// Debugging
	uint64_t local = tdma_time();
	uint64_t world = time_to_world(local);

	// Run the scheduler
	for (unsigned i = 0; i < N_ELEM(tasks); i++) {
		if (local >= tasks[i].due) {
			tasks[i].task(local, world);
			tasks[i].due += tasks[i].period;
		}
	}
}

int main(int argc, char **argv)
{
	tdma_init();
	emit_init(3, PTE20, PullDown);

	//pin = 1;

	// Open serial ports
	sirq_dbg   = sirq_open(SIRQ_UART0, USBTX, USBRX, 115200); // to pc
	sirq_bbb   = sirq_open(SIRQ_UART1, PTE0,  PTE1,  115200); // to bbb
	sirq_mbed  = sirq_open(SIRQ_UART2, PTD3,  PTD2,  115200); // to mbed

	// Setup timers
	tdma_evt   = tdma_open(TDMA_CHAN0, 3, PTC9,  PullDown); // async event

	// mbed time sync
	tdma_rcv   = tdma_open(TDMA_CHAN2, 3, PTD2,  PullUp);   // time sync rcv
	tdma_xmt   = tdma_open(TDMA_CHAN3, 3, PTD3,  PullUp);   // time sync xmt

	// host time sync
	//tdma_rcv   = tdma_open(TDMA_CHAN2, 2, USBRX, PullUp); // time sync rcv
	//tdma_xmt   = tdma_open(TDMA_CHAN3, 2, USBTX, PullUp); // time sync xmt

	// start timers
	tdma_start(tdma_evt);
	tdma_start(tdma_rcv);
	tdma_start(tdma_xmt);

	// Serial timestamping
	serial_tdma_rcv = tdma_rcv;
	serial_tdma_xmt = tdma_xmt;

	// Test clocks
	//MCG->C1    = 0x05; // was 0x1A
	//MCG->C2    = 0x2C; // was 0x24
	//MCG->C3    = 0x91; // was 0x91
	//MCG->C4    = 0x10; // was 0x10
	//MCG->C5    = 0x01; // was 0x01
	//MCG->C6    = 0x40; // was 0x40
	//MCG->S     = 0x6E; // was 0x6E
	//MCG->SC    = 0x02; // was 0x02
	//MCG->ATCVH = 0x00; // was 0x00
	//MCG->ATCVL = 0x00; // was 0x00
	//MCG->C7    = 0x00; // was 0x00
	//MCG->C8    = 0x80; // was 0x80
	//MCG->C9    = 0x00; // was 0x00
	//MCG->C10   = 0x00; // was 0x00

	//sirq_printf("MGC - C1    %02hx\r\n", MCG->C1);     // 1A
	//sirq_printf("MGC - C2    %02hx\r\n", MCG->C2);     // 24
	//sirq_printf("MGC - C3    %02hx\r\n", MCG->C3);     // 91
	//sirq_printf("MGC - C4    %02hx\r\n", MCG->C4);     // 10
	//sirq_printf("MGC - C5    %02hx\r\n", MCG->C5);     // 01
	//sirq_printf("MGC - C6    %02hx\r\n", MCG->C6);     // 40
	//sirq_printf("MGC - S     %02hx\r\n", MCG->S);      // 6E
	//sirq_printf("MGC - SC    %02hx\r\n", MCG->SC);     // 02
	//sirq_printf("MGC - ATCVH %02hx\r\n", MCG->ATCVH);  // 00
	//sirq_printf("MGC - ATCVL %02hx\r\n", MCG->ATCVL);  // 00
	//sirq_printf("MGC - C7    %02hx\r\n", MCG->C7);     // 00
	//sirq_printf("MGC - C8    %02hx\r\n", MCG->C8);     // 80
	//sirq_printf("MGC - C9    %02hx\r\n", MCG->C9);     // 00
	//sirq_printf("MGC - C10   %02hx\r\n", MCG->C10);    // 00

	// Run background loop
	while (true)
		background();

	// Performance testing
	//uint64_t prev = 0, due = 0;
	//uint64_t worst[10] = {};
	//int      count = 0;
	//while (true) {
	//	uint64_t local = tdma_time();
	//	if (prev && (local-prev) > worst[count])
	//		worst[count] = (local-prev);
	//	prev = local;
	//	if (local > due) {
	//		if (count == 5) {
	//			static char str[] = "background background background\r\n";
	//			sirq_write(sirq_dbg, str, sizeof(str));
	//		}
	//		if (count == 9) {
	//			sirq_printf("background\r\n");
	//			for (int i = 0; i < 10; i++) {
	//				sirq_printf("  worst[%d] = 0.%09u\r\n",
	//						i, worst[i]);
	//				worst[i] = 0;
	//			}
	//		}
	//		due += NSEC_PER_SEC;
	//		count = (count + 1) % 10;
	//	}
	//}

	// Run tests
	//test_main();

	return 0;
}