#include <stdint.h>

#include "serial_irq.h"
#include "timer_dma.h"
#include "main_time.h"
#include "main_emit.h"

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

// for 50 MHz clock  50/1000 = 1/20  (PLL/2)

// for 48 MHz clock  48/1000 = 6/125 (FLL)
// for 24 MHz clock, 24/1000 = 3/125
// for 12 MHz clock, 12/1000 = 3/250
// for  6 MHz clock,  6/1000 = 3/500
// for  3 MHz clock,  3/1000 = 3/1000

#define EMIT_PS 1

//#if EMIT_PS == 0
//#define EMIT_CLOCKS(nsec) ((uint16_t)((nsec)   / 20))
//#define EMIT_NSEC(clocks) ((uint16_t)((clocks) * 20))

#if EMIT_PS == 0
#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 6 / 125))
#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 125 / 6))
#elif EMIT_PS == 1
#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 125))
#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 125 / 3))
#elif EMIT_PS == 2
#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 250))
#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 250 / 3))
#elif EMIT_PS == 3
#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 500))
#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 500 / 3))
#elif EMIT_PS == 4
#define EMIT_CLOCKS(nsec) ((uint32_t)((nsec)   * 3 / 1000))
#define EMIT_NSEC(clocks) ((uint32_t)((clocks) * 1000 / 3))
#endif

#define EMIT_SLACK_CLOCKS 0x4000  // tune based on emit_worst

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

static void emit_setup(void)
{
	//        start-.  .-phase-.--period--.
	//      |       @  |       @  |       @  |
	//  utc-^                last-^ now-^
	if (emit_period) {
		uint64_t now   = time_to_world(tdma_time());
		uint64_t phase = emit_start % emit_period;
		uint64_t last  = (now / emit_period) * emit_period;
		emit_due = last + phase;
		time_printf("emit scheduled", emit_due);
	} else {
		emit_due = 0;
	}
}

void emit_init(int alt, PinName pin, PinMode mode)
{
	// Calculate slack time
	emit_slack  = EMIT_NSEC(EMIT_SLACK_CLOCKS);

	// Find pin
	emit_pcr = (uint32_t*)(PORTA_BASE + pin);

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

	// Reset PLL Source
	//SIM->SOPT2 &= ~SIM_SOPT2_PLLFLLSEL_MASK;

	// Debug print on SOPT2
	// -- mbed may set PLLFLL when configuring UART0
	// SOPT2: u0src=1 tpmsrc=1 USBSRC PLL/2 clkos=0 rtcos
	sirq_printf("SOPT2: u0src=%d tpmsrc=%d %s %s clkos=%d %s\r\n",
		(SIM->SOPT2 & SIM_SOPT2_UART0SRC_MASK)     >> SIM_SOPT2_UART0SRC_SHIFT,
		(SIM->SOPT2 & SIM_SOPT2_TPMSRC_MASK)       >> SIM_SOPT2_TPMSRC_SHIFT,
		(SIM->SOPT2 & SIM_SOPT2_UART0SRC_MASK)     ? "USBSRC" : "usbsrc",
		(SIM->SOPT2 & SIM_SOPT2_PLLFLLSEL_MASK)    ? "PLL/2"  : "FLL",
		(SIM->SOPT2 & SIM_SOPT2_CLKOUTSEL_MASK)    >> SIM_SOPT2_CLKOUTSEL_SHIFT,
		(SIM->SOPT2 & SIM_SOPT2_RTCCLKOUTSEL_MASK) ? "RTCOS"  : "rtcos");

	// 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 high on 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_set_start(uint64_t start)
{
	emit_start  = start;
	emit_setup();
}

void emit_set_period(uint64_t period)
{
	emit_period = period;
	emit_setup();
}

void emit_schedule(uint64_t when)
{
	uint64_t local  = time_to_local(when) * 3 / 125;
	uint32_t width  = EMIT_CLOCKS(10000);

	// Disable timer
	TPM1->SC               = TPM_SC_TOF_MASK;

	__disable_irq();

	uint64_t now    = ((uint64_t)~PIT->LTMR64H << 32)
	                | ((uint64_t)~PIT->LTMR64L);
	uint32_t delta  = local - now;
	uint32_t start  = delta >> (EMIT_PS-1); // convert to clocks
	uint32_t stop   = start + width;        // end time

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

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

	__enable_irq();

	// Test
	//int64_t  cnv = TPM1->CONTROLS[0].CnV;
	//int64_t  mod = TPM1->MOD;
	//int64_t  due = local - tdma_time();
	//sirq_printf("%6d -- cnv=%04x mod=%04x due=%04x start=%04x\r\n",
	//		(int)(cnv - EMIT_CLOCKS(due)),
	//		(int)cnv, (int)mod,
	//		(int)EMIT_CLOCKS(due), EMIT_CLOCKS(start));

	// Clock testing
	//uint32_t test_tpm0 =  TPM1->CNT;
	//uint32_t test_pit0 = ~PIT->CHANNEL[0].CVAL;
	//for (int i = 0; i < 100; i++)
	//	asm("nop");
	//uint32_t test_tpm1 =  TPM1->CNT;
	//uint32_t test_pit1 = ~PIT->CHANNEL[0].CVAL;

	//uint32_t test_tpm  = test_tpm1 - test_tpm0;
	//uint32_t test_pit  = test_pit1 - test_pit0;
	//sirq_printf("pit/tpm: %d - tpm=%08x/%08x=%d pit=%08x/%08x=%d\r\n",
	//		test_tpm - test_pit,
	//		test_tpm0, test_tpm1, test_tpm,
	//		test_pit0, test_pit1, test_pit);

	// Debug output
	//time_printf("emitting event", when);
}

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_period && world+emit_slack > emit_due) {
		emit_schedule(emit_due);
		emit_due += emit_period;
	}
	if (emit_period && emit_due < world) {
		sirq_printf("missed emit deadline\r\n");
		time_printf("  due   ", emit_due);
		time_printf("  period", emit_period);
		time_printf("  world ", world);
		emit_setup();
	}
}