#include <stdint.h>

#include "messages.h"

#include "serial_irq.h"
#include "timer_dma.h"

#include "main_time.h"
#include "main_emit.h"

/**
 * Communication overview:
 *
 *     Initialization:
 *        bbb --init1-->  mbed1
 *        bbb --init2-->  mbed1 --init2---> mbed2
 *
 *     Time sync:
 *        bbb             mbed1 ---sync---> mbed2
 *        bbb             mbed1 <--sync---- mbed2
 *
 *     Event Receive:
 *        bbb <--event1-- mbed1             mbed2
 *        bbb <--event2-- mbed1 <--event2-- mbed2
 *
 * Initialization:
 *     Each mbed is initialized by the BBB by receiving an initialization
 *     message.  The Device ID must be non-zero, and is saved for future
 *     messages. If the device is already initialized and a received Device ID
 *     does not match the configured Device ID, the messages is relayed to the
 *     second mbed.
 *
 * Event receive:
 *     When receiving events, an event message is sent from the mbed to the
 *     bbb. If the mbed receiving the event is not Device 1, the message is
 *     sent to mbed1 instead of the bbb.
 *
 * Debug ports:
 *     1. Init messages may be received from the host instead of the bbb
 *     2. Event messages may be sent to the host in addition to the bbb
 */

/***************************
 * Communication functions *
 ***************************/

static int      comm_device_id   = 0;
static int      comm_relay_mode  = 0;

const  uint64_t comm_sync_delay  = NSEC_PER_SEC / 100;
static uint64_t comm_sync_due    = 0;

static sirq_t  *comm_sirq_dbg    = 0;
static sirq_t  *comm_sirq_bbb    = 0;
static sirq_t  *comm_sirq_mbed   = 0;

static tdma_t  *comm_tdma_rcv    = 0;
static tdma_t  *comm_tdma_xmt    = 0;

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

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

static int comm_time_stamp(tdma_t *port, uint64_t *local, uint64_t *world,
		const char *msg)
{
	int valid = tdma_stamp(port, local);
	*world = time_to_world(*local);

	if (!valid)
		sirq_printf("%s -- missing\r\n", msg);
	//else
	//	time_printf(msg, current);

	return valid;
}

/**
 * Initialization
 */
void comm_init(sirq_t *dbg, sirq_t *bbb, sirq_t *mbed,
               tdma_t *rcv, tdma_t *xmt)
{
	comm_sirq_dbg  = dbg;
	comm_sirq_bbb  = bbb;
	comm_sirq_mbed = mbed;

	comm_tdma_rcv  = rcv;
	comm_tdma_xmt  = xmt;
}

/**
 * Output time sync message
 */
void comm_send_sync(uint64_t local)
{
	if (comm_sync_due == 0 || local < comm_sync_due)
		return; // not ready

	// Message data
	header_t   head;
	sync_msg_t body;

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

	sirq_write(comm_sirq_mbed, &head, sizeof(head));

	// Capture transmit time
	tdma_stop(comm_tdma_rcv, 0);
	tdma_start(comm_tdma_xmt);

	sirq_transmit(comm_sirq_mbed);

	tdma_stop(comm_tdma_xmt, 100);
	tdma_start(comm_tdma_rcv);

	// Save transmit time
	uint64_t xmt_local = 0, xmt_world = 0;
	comm_time_stamp(comm_tdma_xmt, &xmt_local, &xmt_world,
			"sync time transmit");

	// Debug output
	//sirq_printf("sync time transmit\r\n");
	//time_printf("  local", xmt_local);
	//time_printf("  world", xmt_world);

	// Write body with updated time and send
	body.time = comm_write_time(xmt_world);

	sirq_write(comm_sirq_mbed, &body, sizeof(body));

	sirq_transmit(comm_sirq_mbed);

	// Queue next transmit time
	comm_sync_due  = 0;
}

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

	// Convert timestamp
	uint64_t world = time_to_world(local);
	ntime_t  ltime = comm_write_time(local);
	ntime_t  wtime = comm_write_time(world);

	// Message data
	header_t    head = {};
	event_msg_t body = {};

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

	body.device = comm_device_id;
	body.event  = event;
	body.world  = wtime;
	body.local  = ltime;

	// Transmit message to BBB
	if (comm_relay_mode) {
		sirq_write(comm_sirq_mbed, &head, sizeof(head));
		sirq_write(comm_sirq_mbed, &body, sizeof(body));
	} else {
		sirq_write(comm_sirq_bbb,  &head, sizeof(head));
		sirq_write(comm_sirq_bbb,  &body, sizeof(body));
	}
}

/**
 * Handle init message
 */
void comm_handle_init(header_t *head, init_msg_t *body)
{
	// Relay initialization from bbb to mbed
	if (comm_device_id && body->device != comm_device_id) {
		//sirq_printf("relaying init\r\n");

		sirq_write(comm_sirq_mbed, head, sizeof(*head));
		sirq_write(comm_sirq_mbed, body, sizeof(*body));

		// Normally we transmit during the time sync but
		// if we haven't started syncing yet, we need to
		// push out the message now.
		if (!comm_sync_due)
			sirq_transmit(comm_sirq_mbed);
		return;
	}

	// Debug output
	sirq_printf("initialize: %s %s %s %s %s\r\n",
		body->control & MSG_CTL_VALID_DEVICE ? "DEV"    : "dev",
		body->control & MSG_CTL_VALID_START  ? "START"  : "start",
		body->control & MSG_CTL_VALID_PERIOD ? "PERIOD" : "period",
		body->control & MSG_CTL_VALID_WORLD  ? "WORLD"  : "world",
		body->control & MSG_CTL_RELAY_MODE   ? "RELAY"  : "relay",
		body->control & MSG_CTL_BEGIN_SYNC   ? "SYNC"   : "sync");
	sirq_printf("  dev    -- %d\r\n", body->device);
	time_printf("  start ", comm_read_time(body->start));
	time_printf("  period", comm_read_time(body->period));
	time_printf("  world ", comm_read_time(body->world));

	// Validate message parts and initialize
	if (body->control & MSG_CTL_VALID_DEVICE)
		comm_device_id = body->device;

	if (body->control & MSG_CTL_VALID_WORLD)
		time_ext_init(tdma_time(), comm_read_time(body->world));

	if (body->control & MSG_CTL_RELAY_MODE)
		comm_relay_mode = 1;
	else
		comm_relay_mode = 0;

	if (body->control & MSG_CTL_BEGIN_SYNC)
		comm_sync_due = tdma_time() + comm_sync_delay;

	// Run these after world time is valid
	if (body->control & MSG_CTL_VALID_START)
		emit_set_start(comm_read_time(body->start));

	if (body->control & MSG_CTL_VALID_PERIOD)
		emit_set_period(comm_read_time(body->period));
}

/**
 * Handle sync message
 */
void comm_handle_sync(header_t *head, sync_msg_t *body)
{
	// Read receive timestamp
	uint64_t local = 0, world = 0;
	comm_time_stamp(comm_tdma_rcv, &local, &world,
			"sync time receive ");
	tdma_stop(comm_tdma_rcv, 0);

	// Lookup reference time from message
	uint64_t reference = comm_read_time(body->time);

	// Debug output
	//sirq_printf("sync time receive\r\n");
	//time_printf("  local", local);
	//time_printf("  world", world);
	//time_printf("  ref  ", reference);

	// Synchronize the clocks
	time_ext_sync(local, reference);

	// Queue transmit to other board
	comm_sync_due   = tdma_time() + comm_sync_delay;
}

/**
 * Handle event message
 */
void comm_handle_event(header_t *head, event_msg_t *body)
{
	// Relay events from other mbeds
	sirq_write(comm_sirq_bbb, head, sizeof(*head));
	sirq_write(comm_sirq_bbb, body, sizeof(*body));
}