// Input/output support functions for pingpong

#include "io.h"
#include "ti_drivers_config.h"
#include <stdio.h>
#include <string.h>
#include <unistd.h>

// Driver header files
#include <ti/drivers/GPIO.h>
#include <ti/drivers/UART.h>
#include <ti/drivers/Timer.h>
#include <ti/drivers/PWM.h>

// tick variable is set to true every sample time.
volatile bool tick = false;
// distance variable contains the measured distance in mm.
volatile int32_t distance;
// Kc variable can be set by typing a float value to the terminal (which is connected to the UART).
//volatile float Kc = 0;

static Timer_Handle timer1;

static void gpioButtonFxn0(uint_least8_t index)
{
    int32_t start = Timer_getCount(timer1); // Start time of timer1.
    while(GPIO_read(Board_ECHO)); // Wait until ECHO becomes low. Time of pulse equals distance measured.
    int32_t stop = Timer_getCount(timer1); // Stop time of timer1.
    if (start < stop)
    {
        distance = (stop - start) / 450; // Calculates distances in mm.
    }
}

void initPort()
{
    GPIO_init();
    GPIO_setConfig(Board_TRIGGER, GPIO_CFG_OUT_STD | GPIO_CFG_OUT_HIGH);
    GPIO_setConfig(Board_ECHO, GPIO_CFG_IN_PD | GPIO_CFG_IN_INT_RISING);
    GPIO_setCallback(Board_ECHO, gpioButtonFxn0);
    GPIO_enableInt(Board_ECHO);
}

static UART_Handle uart;
static char readbuffer[128] = "";
static volatile bool UART_is_read = false;

void UART_read_callback(UART_Handle handle, void *buf, size_t count)
{
    float f;
    int res = sscanf(buf, "%f", &f);
    //if (res == 1)
    {
    //    Kc = f;
    }
    //sendStringUART("Kc = ");
    //sendFloatUART(Kc);
    //sendStringUART("Kc = ");
    //UART_is_read = true;
}

bool initUART(void) {
    UART_init();
    UART_Params uartParams;
    UART_Params_init(&uartParams);
    uartParams.writeDataMode = UART_DATA_TEXT;
    uartParams.readDataMode = UART_DATA_TEXT;
    uartParams.readReturnMode = UART_RETURN_NEWLINE;
    uartParams.readEcho = UART_ECHO_ON;
    uartParams.readMode = UART_MODE_CALLBACK;
    uartParams.baudRate = 115200;
    uartParams.readCallback = UART_read_callback;
    uart = UART_open(CONFIG_UART_0, &uartParams);
    if (uart != NULL)
    {
        sendStringUART("Kc = ");
        UART_read(uart, readbuffer, 128);
    }
    return uart != NULL;
}

static void timerTick(Timer_Handle handle, int_fast16_t status)
{
    tick = true;
    if (UART_is_read)
    {
        UART_is_read = false;
        UART_read(uart, readbuffer, 128);
   }
}

bool initTimer(uint32_t sampleFrequency)
{
    Timer_init();

    Timer_Handle timer0;
    Timer_Params params;
    Timer_Params_init(&params);
    params.periodUnits = Timer_PERIOD_HZ;
    params.period = sampleFrequency;
    params.timerMode  = Timer_CONTINUOUS_CALLBACK;
    params.timerCallback = timerTick;
    timer0 = Timer_open(CONFIG_TIMER_0, &params);
    if (timer0 == NULL)
    {
        return false;
    }
    int32_t status = Timer_start(timer0);
    if (status == Timer_STATUS_ERROR)
    {
        return false;
    }

    Timer_Params tparams;
    Timer_Params_init(&tparams); //Free running timer for measuring length of the echo pulse.
    params.periodUnits = Timer_PERIOD_US;
    tparams.period = 0xFFFFFFFF;
    tparams.timerMode = Timer_FREE_RUNNING;
    timer1 = Timer_open(CONFIG_TIMER_1, &tparams);
    if(Timer_start(timer1) == Timer_STATUS_ERROR)
    {
        return false;
    }
    return true;
}

static PWM_Handle pwm0;

bool initPWM(void)
{
    PWM_Params pwmParams;

    PWM_init();

    // PWM params init
    PWM_Params_init(&pwmParams);
    pwmParams.dutyUnits = PWM_DUTY_US;
    // PWM duty cycle = 0
    pwmParams.dutyValue = 0;
    pwmParams.periodUnits = PWM_PERIOD_US;
    // PWM frequency = 1 / 1000 us = 1 / 1 ms = 1 kHz.
    pwmParams.periodValue = 1000;
    pwmParams.idleLevel = PWM_IDLE_LOW;

    /* Open PWM0 */
    pwm0 = PWM_open(CONFIG_PWM_0, &pwmParams);

    if (!pwm0)
    {
        return false;
    }
    PWM_start(pwm0);
    return true;
}

bool setPWM(uint16_t dutyCycle)
{
    if (PWM_setDuty(pwm0, dutyCycle) < 0)
    {
        return false;
    }
    return true;
}

void trigger()
{
    GPIO_write(Board_TRIGGER, 1);
    usleep(10);
    GPIO_write(Board_TRIGGER, 0);
}

void sendStringUART(char *s)
{
    UART_writePolling(uart, s, strlen(s));
}

void sendIntUART(int i)
{
    char buffer[14];
    int res = sprintf(buffer, "%d\n", i);
    if (res > 0)
    {
        UART_writePolling(uart, buffer, res);
    }
}

void sendIntIntUART(int i1, int i2)
{
    char buffer[28];
    int res = sprintf(buffer, "%d, %d\n", i1, i2);
    if (res > 0)
    {
        UART_writePolling(uart, buffer, res);
    }
}

void sendFloatUART(float f)
{
    char buffer[100];
    int res = sprintf(buffer, "%f\n", f);
    if (res > 0)
    {
        UART_writePolling(uart, buffer, res);
    }
}