#include "messages.h"

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

#include "main_time.h"
#include "main_emit.h"
#include "main_comm.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.
 */

/*******************
 * Tasks Scheduler *
 *******************/

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

// Task entry
typedef struct {
	void (*task)(uint64_t, uint64_t);
	uint64_t period;
	uint64_t due;
} task_t;

// Task functions
void task_serial(uint64_t local, uint64_t world);
void task_events(uint64_t local, uint64_t world);
void task_sync(uint64_t local, uint64_t world);
void task_leds(uint64_t local, uint64_t world);
void task_emit(uint64_t local, uint64_t world);
void task_debug(uint64_t local, uint64_t world);

// Task table
task_t 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
};

/********************
 * 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");
		msg_receive(&parser_dbg,  sirq_getc(sirq_dbg));
	}

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

	while (sirq_ready(sirq_mbed)) {
		//sirq_printf("serial recv - mbed\r\n");
		msg_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)) {
		comm_send_event(0, event);
		tdma_stop(tdma_evt, 0);
		tdma_start(tdma_evt);
	}
}

void task_sync(uint64_t local, uint64_t world)
{
	comm_send_sync(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);

	comm_send_event(1, local);

#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 *
 ********/

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)
{
	// Open serial ports
	sirq_dbg   = sirq_open(SIRQ_UART0, USBTX, USBRX, 115200, 0); // to pc
	sirq_bbb   = sirq_open(SIRQ_UART1, PTE0,  PTE1,  115200, 0); // to bbb
	sirq_mbed  = sirq_open(SIRQ_UART2, PTD3,  PTD2,  115200, 1); // to mbed

	// Setup timers
	tdma_evt   = tdma_open(TDMA_CHAN0, 3, PTC9,  PullDown); // async event
	tdma_rcv   = tdma_open(TDMA_CHAN2, 3, PTD2,  PullUp);   // time sync rcv
	tdma_xmt   = tdma_open(TDMA_CHAN3, 3, PTD3,  PullUp);   // time sync xmt

	// Setup event generation
	time_init();
	emit_init(3, PTE20, PullDown);
	comm_init(sirq_dbg, sirq_bbb, sirq_mbed,
			tdma_rcv, tdma_xmt);

	// Register messages
	msg_register(&parser_dbg,  MSG_ID_INIT,  (handler_t)comm_handle_init);
	msg_register(&parser_dbg,  MSG_ID_SYNC,  (handler_t)comm_handle_sync);
	msg_register(&parser_dbg,  MSG_ID_EVENT, (handler_t)comm_handle_event);

	msg_register(&parser_bbb,  MSG_ID_INIT,  (handler_t)comm_handle_init);

	msg_register(&parser_mbed, MSG_ID_INIT,  (handler_t)comm_handle_init);
	msg_register(&parser_mbed, MSG_ID_SYNC,  (handler_t)comm_handle_sync);
	msg_register(&parser_mbed, MSG_ID_EVENT, (handler_t)comm_handle_event);

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

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

	return 0;
}