/* Standard Include Files. */
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
/* BIOS/XDC Include Files. */
#include <xdc/std.h>
#include <xdc/cfg/global.h>
#include <xdc/runtime/IHeap.h>
#include <xdc/runtime/System.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/Memory.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Clock.h>
#include <ti/sysbios/heaps/HeapBuf.h>
#include <ti/sysbios/heaps/HeapMem.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/family/arm/v7a/Pmu.h>
/* mmWave SDK Include Files: */
#include <ti/common/sys_common.h>
#include <ti/drivers/soc/soc.h>
#include <ti/drivers/esm/esm.h>
#include <ti/drivers/crc/crc.h>
#include <ti/drivers/uart/UART.h>
#include <ti/drivers/gpio/gpio.h>
#include <ti/drivers/mailbox/mailbox.h>
#include <ti/control/mmwave/mmwave.h>
#include <ti/control/mmwavelink/mmwavelink.h>
#include <ti/utils/cli/cli.h>
#include <ti/drivers/osal/DebugP.h>
#include <ti/drivers/osal/HwiP.h>
#include <ti/utils/cycleprofiler/cycle_profiler.h>
#include <ti/drivers/pinmux/pinmux.h>
#include <ti/drivers/osal/MemoryP.h>
#include <ti/utils/testlogger/logger.h>
/* Global Variable: The variable detects if the configuration has been
 * detected or not. */
uint32_t        gConfigDetected = 0U;

/* Global Variable: The variable detects if the start has been
 * detected or not. */
uint32_t        gStartDetected = 0U;

/* Global Variable: The variable detects if the start has been
 * detected or not. */
uint32_t        gStopDetected = 0U;

/* Global Variable: The variable detects if the open has been
 * detected or not. */
uint32_t         gOpenDetected = 0U;

/* Global Variable: The variable detects if the close has been
 * detected or not. */
uint32_t         gCloseDetected = 0U;

/* Global Variable: This is the handle to the mmWave module */
MMWave_Handle   gMMWaveHandle;

/* Global Variable: The variable detects the test selection as entered on the MSS */
int32_t         gTestSelection;

SOC_Handle          socHandle;
Event_Handle        event;
uint8_t period = 0;
uint8_t buffer = 0;
uint8_t id_buf = 0;
uint8_t value_buf = 0;
uint8_t value = 0;
uint8_t id = 0;
uint8_t metka = 0;

UART_Handle     handle;
MMWave_Handle handleMMwave;

UInt eventMask1 = Event_Id_00;
UInt eventMask2 = Event_Id_01;
UInt eventMask3 = Event_Id_02;

uint32_t BAUDRATE = 115200;
uint8_t STOPBITS = 1;
uint8_t DATALEN = 8;
uint8_t PARITY_VALUE = 4;
uint8_t gMCPIUARTPollingModeEnable = 0;

Event_Handle event1;

uint32_t gCPUClockFrequency = (200 * 1000000);

uint8_t uartDataLenths [] = {
                             0, // Padding
                             0,
                             0,
                             0,
                             0,
                             (uint8_t) UART_LEN_5,
                             (uint8_t) UART_LEN_6,
                             (uint8_t) UART_LEN_7,
                             (uint8_t) UART_LEN_8
};

uint8_t uartParityTypes [] = {
                              (uint8_t) UART_PAR_ZERO,
                              (uint8_t) UART_PAR_ONE,
                              (uint8_t) UART_PAR_EVEN,
                              (uint8_t) UART_PAR_ODD,
                              (uint8_t) UART_PAR_NONE
};

uint8_t uartStopBits [] = {
                           0,  // Padding
                           (uint8_t) UART_STOP_ONE,
                           (uint8_t) UART_STOP_TWO
};

Mbox_Handle peerMailbox;

#define BIG_MSG_SIZE 3

volatile uint8_t mboxProcToken = 0;
extern void Mmwave_populateDefaultOpenCfg (MMWave_OpenCfg* ptrOpenCfg);
extern int32_t Mmwave_testLinkAPI (void);
extern int32_t Mmwave_eventFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload);
extern void Mmwave_cfgFxn(MMWave_CtrlCfg* ptrCtrlCfg);
extern void Mmwave_startFxn(MMWave_CalibrationCfg* ptrCalibrationCfg);
extern void Mmwave_stopFxn(void);
extern void Mmwave_openFxn (MMWave_OpenCfg* ptrOpenCfg);
extern void Mmwave_closeFxn (void);
extern void Mmwave_ctrlTask(UArg arg0, UArg arg1);
extern int32_t MonitoringEnable();

static int32_t mboxWrite_ch0(uint8_t *message, int32_t len)
{
    int32_t retVal = -1;
    retVal = Mailbox_write(peerMailbox, message, len);
    if (retVal == len)
    {
        retVal = 0;
    }
    return retVal;
}

static void mboxReadProc_ch0()
{
    uint8_t   mes;
    int32_t           retVal = 0;
    int32_t           ret = 0;


    /* Read the message from the peer mailbox: We are not trying to protect the read
     * from the peer mailbox because this is only being invoked from a single thread */
    retVal = Mailbox_read(peerMailbox, &mes, 1);
    if (retVal < 0)
    {
        /* Error: Unable to read the message. Setup the error code and return values */
        System_printf("Error read dss\n");
        return;
    }
    else
    {
        Mailbox_readFlush (peerMailbox);
        int i;
        if(mes/100 > 0) {
            i = 3;
        } else if((mes - (mes / 100) * 100) / 10 > 0) {
            i = 2;
        } else {
            i = 1;
        }
        char str[i];
        if(i == 3) {
            str[0] = '0' + mes / 100;
            mes = mes - (mes / 100) * 100;
            str[1] = '0' + mes / 10;
            mes = mes - (mes / 10) * 10;
            str[2] = '0' + mes;
        } else if(i == 2) {
            str[0] = '0' + mes / 10;
            mes = mes - (mes / 10) * 10;
            str[1] = '0' + mes;
        } else {
            str[0] = '0' + mes;
        }
        CLI_write(str);
        CLI_write("\n");
        if (ret != 0)
        {
            System_printf ("Error: Mailbox send message failed \n");
        }
        /* We are done: There are no messages available from the peer execution domain. */
        return;
    }

}

void mboxCallback_ch0 (Mbox_Handle handle, Mailbox_Type peer)
{
    /* Message has been received from the peer endpoint. */
    //    mboxReadProc_ch0();
    mboxProcToken = 1;
}

void mBox(UArg a0, UArg a1)
{

    while(true){
        if (mboxProcToken == 1)
        {
            mboxProcToken = 0;
            /* If the mailbox has a message and the frame processing task has finished. */
            mboxReadProc_ch0();
        }
    }
}

int32_t SetVal(int32_t argc, char* argv[])
{
    uint8_t   message[BIG_MSG_SIZE];
    message[0] = 0;
    int id = atoi(argv[1]);
    int value = atoi(argv[2]);
    if((value > 255) || (id > 128)) return -1;
    message[1] = (uint8_t)atoi(argv[1]);
    message[2] = (uint8_t)atoi(argv[2]);
    mboxWrite_ch0(message, BIG_MSG_SIZE);
    return 0;
}

int32_t Get(int32_t argc, char* argv[])
{
    uint8_t   message[BIG_MSG_SIZE];
    message[0] = 1;
    int id = atoi(argv[1]);
    if(id > 128) return -1;
    message[1] = (uint8_t)atoi(argv[1]);
    mboxWrite_ch0(message, BIG_MSG_SIZE);
    return 0;
}

void Work_UART(UArg arg0, UArg arg1)
{
    UART_Params     params;

    /* Setup the default UART Parameters */
    UART_Params_init(&params);
    params.writeDataMode = UART_DATA_TEXT;
    params.readDataMode = UART_DATA_TEXT;
    params.readReturnMode = UART_RETURN_NEWLINE;
    params.isPinMuxDone   = 1;
    params.baudRate = BAUDRATE;
    params.dataLength = (UART_LEN) uartDataLenths[DATALEN];
    handle = UART_open(0, &params);

    if (handle == NULL)
    {
        printf("Error: Unable to open the UART Instance\n");
        return;
    }
    return;
}

void init_mmWave() {
    MMWave_InitCfg          initCfg;
    Task_Params             taskParams;
    MMWave_CalibrationCfg   calibrationCfg;
    MMWave_OpenCfg          openCfg;
    int32_t errCode;
    /* Initialize the configuration: */
    memset ((void *)&initCfg, 0, sizeof(MMWave_InitCfg));
    memset ((void *)&calibrationCfg, 0, sizeof(MMWave_CalibrationCfg));

    /***********************************************************************
     * [Isolation Mode]: MSS executes the mmWave link
     ***********************************************************************/
    initCfg.domain                  = MMWave_Domain_MSS;
    initCfg.socHandle               = socHandle;
    initCfg.eventFxn                = Mmwave_eventFxn;
    initCfg.cfgMode                 = MMWave_ConfigurationMode_MINIMAL;
    initCfg.executionMode           = MMWave_ExecutionMode_ISOLATION;
    initCfg.linkCRCCfg.useCRCDriver = 1U;
    initCfg.linkCRCCfg.crcChannel   = CRC_Channel_CH1;

    /* Initialize and setup the mmWave Control module */
    gMMWaveHandle = MMWave_init (&initCfg, &errCode);
    if (gMMWaveHandle == NULL)
    {
        /* Error: Unable to initialize the mmWave control module */
        System_printf ("Error: mmWave Control Initialization failed [Error code %d]\n", errCode);

        /* Log into the MCPI Test Logger: */
        return;
    }
    /******************************************************************************
     * TEST: Synchronization
     * - The synchronization API always needs to be invoked [Irrespective of the
     *   mode]
     ******************************************************************************/
    while (1)
    {
        int32_t syncStatus;

        /* Get the synchronization status: */
        syncStatus = MMWave_sync (gMMWaveHandle, &errCode);
        if (syncStatus < 0)
        {
            /* Error: Unable to synchronize the mmWave control module */
            System_printf ("Error: mmWave Control Synchronization failed [Error code %d]\n", errCode);
            return;
        }
        if (syncStatus == 1)
        {
            /* Synchronization acheived: */
            break;
        }
        /* Sleep and poll again: */
        Task_sleep(1);
    }


    /*****************************************************************************
     * Launch the mmWave control execution task
     * - This should have a higher priroity than any other task which uses the
     *   mmWave control API
     *****************************************************************************/
    Task_Params_init(&taskParams);
    taskParams.priority  = 6;
    taskParams.stackSize = 4*1024;
    Task_create(Mmwave_ctrlTask, &taskParams, NULL);


    /***********************************************************************
     * [Isolation Mode]: MSS executes the mmWave link
     ***********************************************************************/
    Mmwave_populateDefaultOpenCfg (&openCfg);

    /************************************************************************
     * Open the mmWave:
     ************************************************************************/
    if (MMWave_open (gMMWaveHandle, &openCfg, NULL, &errCode) < 0)
    {
        /* Error: Unable to configure the mmWave control module */
        System_printf ("Error: mmWave open failed [Error code %d]\n", errCode);
        int16_t mmWaveError;
        int16_t subSysError;
        MMWave_ErrorLevel       errorLevel;
        MMWave_decodeError(errCode, &errorLevel, &mmWaveError, &subSysError);
        System_printf ("Error Level: %s mmWave: %d Subsys: %d\n",
                             (errorLevel == MMWave_ErrorLevel_ERROR) ? "Error" : "Warning",
                                     mmWaveError, subSysError);
        return;
    }

    /************************************************************************
     * Minimal Mode: We can now call the mmWave Link API to perform the
     * profile/chirp/frame configuration. This is just an illustration
     ************************************************************************/
    if (Mmwave_testLinkAPI () < 0){
        System_printf("Error: Link Api failed\n");
        return;
    }


    if (MonitoringEnable() != 0){
        System_printf("Error: temperature monitoring failed\n");
        return;
    }

    /*if (GPADCMonitoringEnable() != 0){
            System_printf("Error: Gpadc monitoring failed\n");
            return;
        }*/
    /* Populate the calibration configuration: */
    calibrationCfg.dfeDataOutputMode                          = MMWave_DFEDataOutputMode_FRAME;
    calibrationCfg.u.chirpCalibrationCfg.enableCalibration    = true;
    calibrationCfg.u.chirpCalibrationCfg.enablePeriodicity    = true;
    calibrationCfg.u.chirpCalibrationCfg.periodicTimeInFrames = 10U;

    /************************************************************************
     * Start the mmWave:
     ************************************************************************/
    if (MMWave_start (gMMWaveHandle, &calibrationCfg, &errCode) < 0)
    {
        /* Error: Unable to configure the mmWave control module */
        System_printf ("Error: mmWave start failed [Error code %d]\n", errCode);
        return;
    }
    /**************************************************************************
     ************************** Extern Definitions ****************************
     **************************************************************************/
    return;
}
int32_t MmwDemo_mssMmwaveEventCallbackFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload){
    return 0;
}

static void InitTask(UArg arg0, UArg arg1)
{
    Task_Params taskParams;
    int32_t      errCode;
    Mailbox_Config  cfg;

    CLI_Cfg     cliCfg;

    /* Initialize the UART */
    UART_init();

    Mailbox_init(MAILBOX_TYPE_MSS);

    if(Mailbox_Config_init(&cfg) < 0)
    {
        System_printf ("Error: Mailbox init failed\n");
        return;
    }

    /* Setup the configuration: */
    cfg.chType     = MAILBOX_CHTYPE_MULTI;
    cfg.chId         = MAILBOX_CH_ID_0;
    cfg.writeMode   = MAILBOX_MODE_BLOCKING;
    cfg.readMode     = MAILBOX_MODE_CALLBACK;
    cfg.readCallback = &mboxCallback_ch0;

    peerMailbox = Mailbox_open(MAILBOX_TYPE_DSS, &cfg, &errCode);
    if (peerMailbox == NULL)
    {
        System_printf ("Error: Mailbox open failed 0\n");
        return;
    }

    /* Setup the PINMUX to bring out the MSS UART-1 */
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINN5_PADBE, SOC_XWR16XX_PINN5_PADBE_MSS_UARTA_TX);
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINN5_PADBE, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR16XX_PINN4_PADBD, SOC_XWR16XX_PINN4_PADBD_MSS_UARTA_RX);
    Pinmux_Set_OverrideCtrl(SOC_XWR16XX_PINN4_PADBD, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);

    Work_UART(0,0);

    /* Initialize the CLI configuration: */
    memset ((void *)&cliCfg, 0, sizeof(CLI_Cfg));

    init_mmWave();

    cliCfg.cliPrompt                    = "mmwDemo:/>";
    cliCfg.cliUartHandle                = handle;
    cliCfg.taskPriority                 = 3;
    cliCfg.socHandle                    = socHandle;
    cliCfg.mmWaveHandle                 = gMMWaveHandle;
    cliCfg.enableMMWaveExtension        = 0U;
    cliCfg.usePolledMode                = true;

    cliCfg.tableEntry[0].cmd            = "set";
    cliCfg.tableEntry[0].helpString     = "Set val and ID to buffer";
    cliCfg.tableEntry[0].cmdHandlerFxn  = SetVal;

    cliCfg.tableEntry[1].cmd            = "get";
    cliCfg.tableEntry[1].helpString     = "Get ID from buffer";
    cliCfg.tableEntry[1].cmdHandlerFxn  = Get;

    //  Open the CLI:

    errCode = CLI_open (&cliCfg);
    if (errCode  < 0)
    {
        System_printf ("Error: Unable to open the CLI: %d\n", errCode);
        return;
    }
    System_printf ("Debug: CLI is operational\n");

    /* Initialize the Task Parameters Mailbox. */
    Task_Params_init(&taskParams);
    taskParams.priority = 2;
    taskParams.stackSize = 1 * 1024;
    Task_create(mBox, &taskParams, NULL);

    return;
}


int main (void)
{

    int32_t     errCode;
    Task_Params   taskParams;
    SOC_Cfg     socCfg;
    Error_Block eb;

    /* Initialize the ESM: Dont clear errors as TI RTOS does it */
    ESM_init(0U);

    /* Initialize the SOC confiugration: */
    memset ((void *)&socCfg, 0, sizeof(SOC_Cfg));

    /* Populate the SOC configuration: */
    socCfg.clockCfg = SOC_SysClock_INIT;
    /* Initialize the SOC Module: This is done as soon as the application is started
     * to ensure that the MPU is correctly configured. */
    socHandle = SOC_init (&socCfg, &errCode);
    if (socHandle == NULL)
    {
        System_printf ("Error: SOC Module Initialization failed [Error code %d]\n", errCode);
        return -1;
    }

    event = Event_create(NULL, &eb);
    if (event == NULL) {
        System_printf ("Error: Event not create\n");
        return -1;
    }

    /* Initialize the Task Parameters. */
    Task_Params_init(&taskParams);
    taskParams.priority = 3;
    taskParams.stackSize = 5*1024;
    Task_create(InitTask, &taskParams, NULL);


    /* Start BIOS */
    BIOS_start();
    return 0;
}