/* main.cpp
 * mbed project: Simple_Square_Wave
 * mbed board: FRDM-KL46Z
 * Generate square waves using DigitalOut.
 */

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

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

// Push-button IRQ handler
void Control_T(void);

// Task modes
void Empty_Task(void);
void Receive_Event(void);
void Generate_Event(void);

// Local variables
int START_STOP_FLAG = 0;
float period_s = 0.0001;
DigitalOut  Square_Wave(PTD3);      // Used PWM output is PTD3
InterruptIn Control_Testing(PTC12); // SW3

tdma_t     *Tdma_Mbed1;
tdma_t     *Tdma_Mbed2;

sirq_t     *Sirq_Bbb;

parser_t    Parser_Bbb;

// Extern stuff...
extern serial_t stdio_uart;

// Taks tables
static struct {
	const char *mode;
	void (*func)(void);
} task_table[] = {
	{ "null",     Empty_Task },
	{ "generate", Generate_Event },
	{ "receive",  Receive_Event },
};

int task_index = 0;

// Main
int main(int arc, char **argv)
{
	// Set debug baud rate
	serial_init(&stdio_uart, USBTX, USBRX);
	serial_baud(&stdio_uart, 115200);

	// Setup BBB uart port
	Sirq_Bbb   = sirq_open(SIRQ_UART1, PTE0,  PTE1,  115200, 0);

	// Setup time tags
	Tdma_Mbed1 = tdma_open(TDMA_CHAN2, 3, PTD2, PullUp); // TODO -- Should be on PTC
	Tdma_Mbed2 = tdma_open(TDMA_CHAN3, 3, PTD3, PullUp); // TODO -- Should be on PTD

	// Register messages
	//msg_register(&parser_bbb, MSG_ID_INIT, (handler_t)serial_handle_init);

	// Start timers
	tdma_start(Tdma_Mbed1);
	tdma_start(Tdma_Mbed2);

	// Enable push-button controlling
	Control_Testing.rise(&Control_T);    // Enable getMode interrupt handler

	// Main loop
	__enable_irq();
	while (1) {
		// Recieve serial data
		while (sirq_ready(Sirq_Bbb)) {
			int byte = sirq_getc(Sirq_Bbb);
			msg_receive(&Parser_Bbb, byte);
		}

		// Run task function
		task_table[task_index].func();
	}
}

// Task functions
void Empty_Task(void)
{
	// do nothing
}

void Receive_Event(void)
{
	uint64_t event;
	if (tdma_stamp(Tdma_Mbed1, &event))
		printf("event received - rcv\r\n");
	if (tdma_stamp(Tdma_Mbed2, &event))
		printf("event received - xmt\r\n");
}

void Generate_Event(void)
{
	if (START_STOP_FLAG == 1)
	{
		Square_Wave.write(0);
		wait(period_s/2);
		Square_Wave.write(1);
		wait(period_s/2);
	}
	else
		Square_Wave.write(0);
}

// Interrupt handlers
void Control_T(void)
{
	if(START_STOP_FLAG == 0)
	{
		period_s = 1;
		START_STOP_FLAG = 1;
	}
	else
	{
		START_STOP_FLAG = 0;
		Square_Wave.write(0);
	}

	// Implement task cycling
	// task_index = (task_index + 1) % N_ELEM(task_table);
}