/*
* Copyright (c) 2017 Texas Instruments Incorporated
*
* All rights reserved not granted herein.
* Limited License.
*
* Texas Instruments Incorporated grants a world-wide, royalty-free,
* non-exclusive license under copyrights and patents it now or hereafter
* owns or controls to make, have made, use, import, offer to sell and sell ("Utilize")
* this software subject to the terms herein.  With respect to the foregoing patent
* license, such license is granted  solely to the extent that any such patent is necessary
* to Utilize the software alone.  The patent license shall not apply to any combinations which
* include this software, other than combinations with devices manufactured by or for TI ("TI Devices").
* No hardware patent is licensed hereunder.
*
* Redistributions must preserve existing copyright notices and reproduce this license (including the
* above copyright notice and the disclaimer and (if applicable) source code license limitations below)
* in the documentation and/or other materials provided with the distribution
*
* Redistribution and use in binary form, without modification, are permitted provided that the following
* conditions are met:
*
*             * No reverse engineering, decompilation, or disassembly of this software is permitted with respect to any
*               software provided in binary form.
*             * any redistribution and use are licensed by TI for use only with TI Devices.
*             * Nothing shall obligate TI to provide you with source code for the software licensed and provided to you in object code.
*
* If software source code is provided to you, modification and redistribution of the source code are permitted
* provided that the following conditions are met:
*
*   * any redistribution and use of the source code, including any resulting derivative works, are licensed by
*     TI for use only with TI Devices.
*   * any redistribution and use of any object code compiled from the source code and any resulting derivative
*     works, are licensed by TI for use only with TI Devices.
*
* Neither the name of Texas Instruments Incorporated nor the names of its suppliers may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* DISCLAIMER.
*
* THIS SOFTWARE IS PROVIDED BY TI AND TI'S LICENSORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
* BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL TI AND TI'S LICENSORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

/**************************************************************************
 *************************** Include Files ********************************
 **************************************************************************/

/* Standard Include Files. */
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.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/spi/SPI.h>
#include <ti/drivers/pinmux/pinmux.h>
#include <ti/drivers/osal/HwiP.h>
#include <ti/drivers/esm/esm.h>
#include <ti/drivers/soc/include/reg_toprcm.h>
#include <ti/drivers/soc/soc.h>
#include <ti/drivers/qspiflash/qspiflash.h>
#include <ti/drivers/uart/UART.h>

#include <ti/drivers/pinmux/include/reg_pinmux.h>
#include <ti/utils/cycleprofiler/cycle_profiler.h>

#include "demo.h"
#include "iwr_gpio.h"
#include "math.h"

/**************************************************************************
 *************************** Local Definitions ****************************
 **************************************************************************/
#define TOPRCM_BASE_ADDRESS 0xFFFFE100U

#define INCLUDE_QSPIFLASH 1
#define INCLUDE_UART 1
#define INCLUDE_CYCLE_PROFILER 0

#define IR_SPI_MASTER 1
#define SPI_BITRATE 1000000

#define SWAP_BYTES(x) (((x & 0xff) << 8) | ((x >> 8) & 0xff))

#define L3_SRAM 0x51020000 /* L3 RAM */

/**************************************************************************
 *************************** Global Definitions ***************************
 **************************************************************************/
SOC_Handle socHandle;
DMA_Handle dmaHandle;
SPI_Handle spiHandle;

#ifdef INCLUDE_QSPIFLASH
QSPI_Handle qspiHandle;
QSPIFlash_Handle qspiflashHandle;
#endif

#ifdef INCLUDE_UART
UART_Handle uartHandle;
#endif

float *const ptrTime_vary_gain_alpha = MY_OWN_BASE_ADDRESS(TIME_VARY_GAIN_OFFSET);

uint32_t demo_mode;
ParamNeedSave_t *ParamNeedSave = MY_OWN_BASE_ADDRESS(PARAM_NEED_TO_SAVE_OFFSET);
RF_Param_t *RF_Param = MY_OWN_BASE_ADDRESS(RF_PARAM_OFFSET);

FlagAboutMCU_t FlagAboutMcu;
// RF_Param_t RF_Param;

//Эñ
uint8_t receive_num;  //ָֽ
uint8_t receive[256]; //ָջ
uint8_t send_num;     //ָֽ
uint8_t send[256];    //ָͻ
uint8_t data1;        //ֽڽջ

#ifdef INCLUDE_CYCLE_PROFILER
unsigned int cptime[12];
#endif

extern LSDemo_MCB gMmwMCB;

extern int32_t demo_eventCallbackFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload);
extern void Get_DataFromFlash(uint8_t *dstDataAddr, uint16_t DataLen, uint32_t flashAddrOffset);
extern void Save_Data2Flash(uint8_t *const srcDataAddr, uint16_t DataLen, uint32_t flashAddrOffset);
int32_t initialize(void);

const RF_Param_t RF_Param_10m = {
    40,   //  profileCfg->rxGain
    5000, //  profileCfg->digOutSampleRate
    1515, //  profileCfg->freqSlopeConst
    700,  //  profileCfg->adcStartTimeConst
    223,  //  profileCfg->numAdcSamples
    4096, //  dataPathObj->numRangeBins
    // 1,                 //  frameCfg->numLoops
    5468,               //  profileCfg->rampEndTime
    1435384036,         //  profileCfg->startFreqConst
    2916,               //  profileCfg->idleTimeConst
    0.002503349120641f, //  dataPathObj->rangeResolution
    10.253717998146127f //max
};
const RF_Param_t RF_Param_150m = {
    30,    //  profileCfg->rxGain
    18750, //  profileCfg->digOutSampleRate
    387,   //  profileCfg->freqSlopeConst
    700,   //  profileCfg->adcStartTimeConst
    3826,  //  profileCfg->numAdcSamples
    4096,  //  dataPathObj->numRangeBins
    // 1,                 //  frameCfg->numLoops
    21408,              //  profileCfg->rampEndTime
    1435384036,         //  profileCfg->startFreqConst
    2916,               //  profileCfg->idleTimeConst
    0.036749747265226f, //  dataPathObj->rangeResolution
    150.5269647983661f};

/**
 *  @b Description
 *  @n
 *      System Initialization Task which initializes the various
 *      components in the system.
 *
 *   @param[in] arg0               Task Arg0
 *   @param[in] arg1               Task Arg1
 *
 *  @retval
 *      Not Applicable.
 */
static void init_Task(UArg arg0, UArg arg1)
{
    int32_t errCode;
    MMWave_InitCfg initCfg;
    Task_Params taskParams;

    //Read GPIO_1 to determine what to run:
    //  0 = run a minimum calibration and a normal measurement cycle.
    //  1 = run full calibration, save to flash, and no measurement.
    gpio_set_direction(0, 0, 0);
    // demo_mode = gpio_read(0, 1);

    if (initialize())
    {
    }
    else
    {
        demo_printf("Error: Unable to initialize\n");
        return;
    }

    /* Initialize the Mailbox */
    Mailbox_init(MAILBOX_TYPE_MSS);

    gpio_led_on();

    memset(ParamNeedSave, 0, sizeof(ParamNeedSave_t));
    memset((void *)&FlagAboutMcu, 0, sizeof(FlagAboutMCU_t));
    FlagAboutMcu.Working_mode = DEMO_RUN_MEASUREMENT;
    FlagAboutMcu.Environment_param.mode = ENVIRONMENT_MODE_DEFULT;
    Get_DataFromFlash(ParamNeedSave, sizeof(ParamNeedSave_t), CONFIG_FROM_MCU_SAVE2FLASH_OFFSET);
    Get_DataFromFlash(ParamNeedSave, sizeof(ParamNeedSave_t), CONFIG_FROM_MCU_SAVE2FLASH_OFFSET);

    if (0xff == ParamNeedSave->ParamFromMcu.Medium)
    {
        memset(ParamNeedSave, 0, sizeof(ParamNeedSave_t));
        FlagAboutMcu.Save_param_flag = 1;
    }
    memcpy(RF_Param, &RF_Param_35m, sizeof(RF_Param_t));

    if (ParamNeedSave->ParamFromMcu.Curve_Threshold < CURVE_THRESHOLD_MIN)
    {
        ParamNeedSave->ParamFromMcu.Curve_Threshold = CURVE_THRESHOLD_MIN;
    }
    else if (ParamNeedSave->ParamFromMcu.Curve_Threshold > CURVE_THRESHOLD_MAX - 1)
    {
        ParamNeedSave->ParamFromMcu.Curve_Threshold = CURVE_THRESHOLD_MAX - 1;
    }
    if (ParamNeedSave->ParamFromMcu.Peak_Threshold < PEAK_THRESHOLD_MIN)
    {
        ParamNeedSave->ParamFromMcu.Peak_Threshold = PEAK_THRESHOLD_MIN;
    }
    else if (ParamNeedSave->ParamFromMcu.Peak_Threshold > PEAK_THRESHOLD_MAX - 1)
    {
        ParamNeedSave->ParamFromMcu.Peak_Threshold = PEAK_THRESHOLD_MAX - 1;
    }

    Get_DataFromFlash(ptrTime_vary_gain_alpha, sizeof(float), TIME_VARY_GAIN_SAVE2FLASH_OFFSET);

#if 1
    int32_t read_flag = 0;

    FlagAboutMcu.Start_Measuring_flag = !gpio_read(0, 0);

    if (!FlagAboutMcu.Start_Measuring_flag)
    {
        uint8_t init_finish_buf[9] = {0xfe, 0x54, 0x55, 0x09, 0x00, 0x01, 0x0f, 0x0f, 0xf7};
        UART_write(uartHandle, init_finish_buf, 9);
    }

    while (!FlagAboutMcu.Start_Measuring_flag)
    {
        read_flag = UART_readPolling(uartHandle, &data1, 1);
        if (read_flag > 0)
        {
            receive[receive_num++] = data1;
        }
        else
        {
            Task_sleep(1);
        }
    }
#endif

    memcpy(RF_Param, &RF_Param_10m, sizeof(RF_Param_t));

    if (demo_mode == DEMO_RUN_MEASUREMENT)
    {
    }
    else //run calibration only
    {
    }

    if (demo_mode == DEMO_RUN_MEASUREMENT)
    {
        /* Initialize the Data Path: */
        demo_dataPathInit(&gMmwMCB.dataPathObj);
    }

    /* Initialize the mmWave control init configuration */
    memset((void *)&initCfg, 0, sizeof(MMWave_InitCfg));

    /* Populate the init configuration: */
    initCfg.domain = MMWave_Domain_MSS;
    initCfg.socHandle = gMmwMCB.socHandle;
    initCfg.eventFxn = demo_eventCallbackFxn;
    initCfg.linkCRCCfg.useCRCDriver = 0;
    initCfg.cfgMode = MMWave_ConfigurationMode_FULL;

    /* Initialize and setup the mmWave Control module */
    gMmwMCB.ctrlHandle = MMWave_init(&initCfg, &errCode);
    if (gMmwMCB.ctrlHandle == NULL)
    {
        /* Error: Unable to initialize the mmWave control module */
        demo_printf("Error: mmWave Control Initialization failed [Error code %d]\n", errCode);
        return;
    }
    demo_printf("Debug: mmWave Control Initialization was successful\n");

    /* Synchronization: This will synchronize the execution of the control module
   * between the domains. This is a prerequiste and always needs to be invoked. */
    if (MMWave_sync(gMmwMCB.ctrlHandle, &errCode) < 0)
    {
        /* Error: Unable to synchronize the mmWave control module */
        demo_printf("Error: mmWave Control Synchronization failed [Error code %d]\n", errCode);
        return;
    }
    demo_printf("Debug: mmWave Control Synchronization was successful\n");

    if (demo_mode == DEMO_RUN_MEASUREMENT)
        demo_dataPathOpen(&gMmwMCB.dataPathObj);

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

    if (demo_mode == DEMO_RUN_MEASUREMENT)
    {
        /*****************************************************************************
    * Launch the Main task
    * - The demo's data path task
    *****************************************************************************/
        Task_Params_init(&taskParams);
        taskParams.priority = 4;
        taskParams.stackSize = 6 * 1024;
        Task_create(demo_dataPathTask, &taskParams, NULL);
    }
    else //run calibration only
    {
        /*****************************************************************************
    * Launch the calibration task
    * - The main calibration task
    *****************************************************************************/
        Task_Params_init(&taskParams);
        taskParams.priority = 4;
        taskParams.stackSize = 6 * 1024;
        Task_create(demo_calibrationTask, &taskParams, NULL);
    }

    return;
}

void Signal_Process_Initial(void)
{
}

void Empty_array(void) //պ
{
    receive_num = 0;
    send_num = 0;
    for (int i = 0; i < 30; i++)
    {
        receive[i] = 0;
        send[i] = 0;
    }
}

int32_t init_dma(void)
{
    int32_t errCode;
    DMA_Params dmaParams;

    /* Init SYSDMA params */
    DMA_Params_init(&dmaParams);

    /* Open DMA driver instance 0 for SPI test */
    dmaHandle = DMA_open(0, &dmaParams, &errCode);

    if (dmaHandle == NULL)
    {
        demo_printf("Open DMA driver failed with error=%d\n", errCode);
        return (0);
    }

    return (1);
}

int32_t init_soc(void)
{
    int32_t errCode;
    SOC_Cfg socCfg;

    ESM_init(0); //don't clear errors as TI RTOS does it

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

    socCfg.clockCfg = SOC_SysClock_INIT;

    /* Enable SPI signal output from TOPRCM */
    socHandle = SOC_init(&socCfg, &errCode);
    if (socHandle == NULL)
    {
        /* Debug Message: */
        demo_printf("Debug: Soc_init failed with Error [%d]\n", errCode);
        return (0);
    }

    return (1);
}

/* AR1443 to MSP432 pin connections:
*  Pin Assignments:
*   |         |  AR1443   |   MSP432  |
*   |SPI_CLK  |   J6.13   |   P1.5    |
*   |SPI_MOSI |   J5.12   |   P1.6    |
*   |SPI_MISO |   J5.14   |   P1.7    |
*   |GPIO 1   |   J5.8    |   P5.7    |
*/
int32_t init_spi(void)
{
    int32_t errCode;
    SPI_Params params;

    /* Setup the PINMUX to bring out the MibSpiA */
    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINR8_PADAD, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINR8_PADAD, SOC_XWR14XX_PINR8_PADAD_SPIA_MOSI);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINP5_PADAE, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINP5_PADAE, SOC_XWR14XX_PINP5_PADAE_SPIA_MISO);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINR9_PADAF, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINR9_PADAF, SOC_XWR14XX_PINR9_PADAF_SPIA_CLK);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINR7_PADAG, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINR7_PADAG, SOC_XWR14XX_PINR7_PADAG_SPIA_CS);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINP6_PADAA, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINP6_PADAA, SOC_XWR14XX_PINP6_PADAA_SPI_HOST1_INTR);

    /* Initialize the SPI */
    SPI_init();

    if (SOC_SPIOutputCtrl(socHandle, 0, 1, &errCode) < 0)
    {
        /* Debug Message: */
        demo_printf("Debug: SOC_SPIOutputCtrl failed with Error [%d]\n", errCode);
        return (0);
    }

    /* Setup the default SPI Parameters */
    SPI_Params_init(&params);

#ifdef IR_SPI_MASTER
    params.mode = SPI_MASTER;
    params.u.masterParams.bitRate = SPI_BITRATE;
    params.u.masterParams.numSlaves = 1;
    params.u.masterParams.slaveProf[0].chipSelect = 0;
    // params.u.masterParams.slaveProf[0].ramBufLen = sizeof(DataAndIndex_t) * 2; //MIBSPI_RAM_MAX_ELEM;
    params.u.masterParams.slaveProf[0].ramBufLen = SPIRAMBUFLEN; //MIBSPI_RAM_MAX_ELEM;  20210811
    params.u.masterParams.slaveProf[0].dmaCfg.txDmaChanNum = 1U;
    params.u.masterParams.slaveProf[0].dmaCfg.rxDmaChanNum = 0U;
    params.frameFormat = SPI_POL0_PHA0;
    params.shiftFormat = SPI_MSB_FIRST;
#else
    params.mode = SPI_SLAVE;
    params.frameFormat = SPI_POL0_PHA0;
    params.u.slaveParams.dmaCfg.txDmaChanNum = 1U;
    params.u.slaveParams.dmaCfg.rxDmaChanNum = 0U;
#endif

    /* Enable DMA and set DMA channels */
    params.dmaEnable = 1;
    params.dmaHandle = dmaHandle;
    params.shiftFormat = SPI_MSB_FIRST;
    params.pinMode = SPI_PINMODE_4PIN_CS; //20210809
    params.eccEnable = 1;
    // params.dataSize = 16;

    //default datasize (set in SPI_Params_init) is 16 bits
    //    params.dataSize = 8;

    spiHandle = SPI_open(0, &params);
    if (spiHandle == NULL)
    {
        demo_printf("Error: Unable to open the SPI Instance\n");
        return (0);
    }

    return (1);
}

#ifdef INCLUDE_QSPIFLASH
int32_t init_qspiflash(void)
{
    int32_t errCode;
    QSPI_Params QSPIParams;

    /* Setup the PINMUX to bring out the QSPI */
    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINR10_PADAP, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINR10_PADAP, SOC_XWR14XX_PINR10_PADAP_QSPI_CLK);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINP8_PADAQ, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINP8_PADAQ, SOC_XWR14XX_PINP8_PADAQ_QSPI_CS);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINR11_PADAL, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINR11_PADAL, SOC_XWR14XX_PINR11_PADAL_QSPI_D0);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINP9_PADAM, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINP9_PADAM, SOC_XWR14XX_PINP9_PADAM_QSPI_D1);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINR12_PADAN, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINR12_PADAN, SOC_XWR14XX_PINR12_PADAN_QSPI_D2);

    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINP10_PADAO, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINP10_PADAO, SOC_XWR14XX_PINP10_PADAO_QSPI_D3);

    /* Initialize the QSPI Driver (currently this call does nothing) */
    // QSPI_init();

    /* Initialize the QSPI Flash (currently this call does nothing) */
    // QSPIFlash_init();

    /* Open QSPI driver */
    QSPI_Params_init(&QSPIParams);

    /* Set the QSPI peripheral clock to 200MHz  */
    QSPIParams.qspiClk = 200 * 1000000U;

    QSPIParams.clkMode = QSPI_CLOCK_MODE_0;

    /* Running at 40MHz QSPI bit rate
   * QSPI bit clock rate derives from QSPI peripheral clock(qspiClk)
     and divide clock internally down to bit clock rate
     BitClockRate = qspiClk/divisor(=5, setup by QSPI driver internally)
   */
    QSPIParams.bitRate = 40 * 1000000U;

    qspiHandle = QSPI_open(&QSPIParams, &errCode);

    if (qspiHandle == NULL)
    {
        demo_printf("Error: Unable to open the QSPI Instance\n");
        return (0);
    }

    qspiflashHandle = QSPIFlash_open(qspiHandle, &errCode);
    if (qspiflashHandle == NULL)
    {
        demo_printf("Error: Unable to open the QSPIflash Instance\n");
        return (0);
    }

    return (1);
}
#endif

#ifdef INCLUDE_UART
void init_uart(void)
{
    UART_Params uartParams;
    UART_init();

    /* Setup the PINMUX to bring out the UART-1 */
    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINN6_PADBE, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINN6_PADBE, SOC_XWR14XX_PINN6_PADBE_MSS_UARTA_TX);
    Pinmux_Set_OverrideCtrl(SOC_XWR14XX_PINN5_PADBD, PINMUX_OUTEN_RETAIN_HW_CTRL, PINMUX_INPEN_RETAIN_HW_CTRL);
    Pinmux_Set_FuncSel(SOC_XWR14XX_PINN5_PADBD, SOC_XWR14XX_PINN5_PADBD_MSS_UARTA_RX);

    /* Setup the default UART Parameters */
    UART_Params_init(&uartParams);
    uartParams.clockFrequency = (200 * 1000000);
    uartParams.baudRate = 115200;
    uartParams.isPinMuxDone = 1;
    uartParams.readEcho = UART_ECHO_OFF;
    // uartParams.readReturnMode = UART_RETURN_FULL;
    uartParams.readDataMode = UART_DATA_BINARY;
    uartParams.writeDataMode = UART_DATA_BINARY;

    /* Open the UART Instance */
    uartHandle = UART_open(0, &uartParams);
    if (uartHandle == NULL)
    {
        demo_printf("Error: Unable to open the UART.\n");
        return;
    }
}
#endif

//This is a "reset" function, specified in mss.cfg to execute directly following
//the branch to _c_int00.  It will set GPIO_0 high as an indication that IWR Boot
//has completed. The difference between setting GPIO_0 here vs the top of main()
//was measured to be 3.5ms.
//Since this function will run prior to the C runtime being initialized, it must
//not use global variables, and use a minimal stack.
void setGpio0atReset(void)
{
    volatile uint32_t *reg1;
    volatile uint32_t *reg2;
    uint32_t temp;
    volatile PINMUXRegs *pinmuxRegs = (volatile PINMUXRegs *)SOC_XWR14XX_MSS_PINMUX_BASE_ADDRESS;

    //Bring the GIO module out of reset
    reg1 = (uint32_t *)GPIO_GIOGCR0_REG;
    *reg1 = 1;

    //Make sure interrupts are cleared
    reg1 = (uint32_t *)GPIO_GIOENACLR_REG;
    *reg1 = 0xffffffff;

    // Pinmux_Set_OverrideCtrl(1, 2, 2) code:

    //Unlock write access
    pinmuxRegs->IOCFGKICK0 = 0x83E70B13U;
    pinmuxRegs->IOCFGKICK1 = 0x95A4F1E0U;

    pinmuxRegs->PADxx_CFG_REG[1] = CSL_FINSR(pinmuxRegs->PADxx_CFG_REG[1],
                                             PADS_CFG_REG_OE_OVERRIDE_CTRL_BIT_START,
                                             PADS_CFG_REG_OE_OVERRIDE_CTRL_BIT_END, 0U);

    pinmuxRegs->PADxx_CFG_REG[1] = CSL_FINSR(pinmuxRegs->PADxx_CFG_REG[1],
                                             PADS_CFG_REG_IE_OVERRIDE_CTRL_BIT_START,
                                             PADS_CFG_REG_IE_OVERRIDE_CTRL_BIT_END, 0U);

    // Pinmux_Set_FuncSel(1, 1) code:

    /* Mask the existing functionality of the pin */
    pinmuxRegs->PADxx_CFG_REG[1] = CSL_FINSR(pinmuxRegs->PADxx_CFG_REG[1],
                                             PADS_CFG_REG_FUNC_SEL_BIT_END,
                                             PADS_CFG_REG_FUNC_SEL_BIT_START, 0);

    /* Setup the required fucntionality */
    pinmuxRegs->PADxx_CFG_REG[1] = CSL_FINSR(pinmuxRegs->PADxx_CFG_REG[1],
                                             PADS_CFG_REG_FUNC_SEL_BIT_END,
                                             PADS_CFG_REG_FUNC_SEL_BIT_START, 1);

    //Lock write access
    pinmuxRegs->IOCFGKICK1 = 0;
    pinmuxRegs->IOCFGKICK0 = 0;

    reg1 = (uint32_t *)GPIO_GIODIR_REG; //direction register

    /* Now set GPIO_0 for output, to allow measurement timing. */
    temp = *reg1;
    *reg1 = temp | 0x00000001; //set the bit to set output mode

    // gpio_set_direction(0, 0, GPIO_OUTPUT);
    reg2 = (uint32_t *)GPIO_GIOPDR_REG; //open drain register (for output)
    temp = *reg2;
    *reg2 = temp & 0xfffffffe; //clear the corresponding bit for normal GPIO mode

    // gpio_write(0, 0, 1);
    reg1 = (uint32_t *)GPIO_GIODSET_REG; //set GPIO_0 to 1 to mark boot is done
    *reg1 = 0x00000001;
}

int32_t initialize(void)
{
    int32_t init_pass;

    // gpio_init();
    gpio_reinit();

#ifdef INCLUDE_CYCLE_PROFILER
    Cycleprofiler_init();
    cptime[0] = Cycleprofiler_getTimeStamp();
#endif

    /* Initialize and populate the demo MCB */
    memset((void *)&gMmwMCB, 0, sizeof(LSDemo_MCB));

    gMmwMCB.socHandle = socHandle;

    init_pass = init_dma();

    if (init_pass)
    {
        init_pass = init_spi();

#ifdef INCLUDE_UART
        init_uart();
#endif

#ifdef INCLUDE_QSPIFLASH
        init_pass = init_qspiflash();
#endif
    }

    return (init_pass);
}

/**
 *  @b Description
 *  @n
 *      Entry point into the level sensing demo code.
 *
 *  @retval
 *      Not Applicable.
 */
int main(void)
{
    Task_Params taskParams;

    if (init_soc())
    {
        /* Initialize the Task Parameters. */
        Task_Params_init(&taskParams);
        taskParams.stackSize = 8 * 1024;
        Task_create(init_Task, &taskParams, NULL);

        /* Start BIOS */
        BIOS_start();
    }

    return 0;
}
