IWR6843AOP: Sensor Initialization Successful, but No Chirp Transmission or Processing

/*
 *   @file  mmw_cli.c
 *
 *   @brief
 *      Mmw (Milli-meter wave) DEMO CLI Implementation
 *
 *  \par
 *  NOTE:
 *      (C) Copyright 2018 Texas Instruments, Inc.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "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 THE COPYRIGHT
 *  OWNER OR CONTRIBUTORS 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/runtime/System.h>

/* mmWave SDK Include Files: */
#include <ti/common/sys_common.h>
#include <ti/common/mmwave_sdk_version.h>
#include <ti/drivers/uart/UART.h>
#include <ti/control/mmwavelink/mmwavelink.h>
#include <ti/utils/cli/cli.h>
#include <ti/utils/mathutils/mathutils.h>

/* Demo Include Files */
#include <ti/demo/xwr68xx/mmw/include/mmw_config.h>
#include <ti/demo/xwr68xx/mmw/mss/mmw_mss.h>
#include <ti/demo/utils/mmwdemo_adcconfig.h>
#include <ti/demo/utils/mmwdemo_rfparser.h>

/**************************************************************************
 *************************** Local function prototype****************************
 **************************************************************************/

/* CLI Extended Command Functions */
static int32_t MmwDemo_CLICfarCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIMultiObjBeamForming (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLICalibDcRangeSig (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIClutterRemoval (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLISensorStart (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLISensorStop (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIGuiMonSel (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIADCBufCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLICompRangeBiasAndRxChanPhaseCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIMeasureRangeBiasAndRxChanPhaseCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLICfarFovCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIAoAFovCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIExtendedMaxVelocity (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIBpmCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIChirpQualityRxSatMonCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIChirpQualitySigImgMonCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIAnalogMonitorCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLILvdsStreamCfg (int32_t argc, char* argv[]);
static int32_t MmwDemo_CLIConfigDataPort (int32_t argc, char* argv[]);

/**************************************************************************
 *************************** Extern Definitions *******************************
 **************************************************************************/

extern MmwDemo_MSS_MCB    gMmwMssMCB;

/**************************************************************************
 *************************** Local Definitions ****************************
 **************************************************************************/
#define MMWDEMO_DATAUART_MAX_BAUDRATE_SUPPORTED 3125000

/**************************************************************************
 *************************** CLI  Function Definitions **************************
 **************************************************************************/
/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for the sensor start command
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLISensorStart (int32_t argc, char* argv[])
{
    bool        doReconfig = true;
    int32_t     retVal = 0;

    /*  Only following command syntax will be supported 
        sensorStart
        sensorStart 0
    */
   CLI_write ("CLI Sensor Start called.\n");
    if (argc == 2)
    {
        doReconfig = (bool) atoi (argv[1]);

        if (doReconfig == true)
        {
            CLI_write ("Error: Reconfig is not supported, only argument of 0 is\n"
                       "(do not reconfig, just re-start the sensor) valid\n");
            return -1;
        }
    }
    else
    {
        /* In case there is no argument for sensorStart, always do reconfig */
        doReconfig = true;
    }

    /***********************************************************************************
     * Do sensor state management to influence the sensor actions
     ***********************************************************************************/

    /* Error checking initial state: no partial config is allowed 
       until the first sucessful sensor start state */
    if ((gMmwMssMCB.sensorState == MmwDemo_SensorState_INIT) || 
         (gMmwMssMCB.sensorState == MmwDemo_SensorState_OPENED))
    {
        MMWave_CtrlCfg ctrlCfg;

        /* need to get number of sub-frames so that next function to check
         * pending state can work */
        CLI_getMMWaveExtensionConfig (&ctrlCfg);
        gMmwMssMCB.objDetCommonCfg.preStartCommonCfg.numSubFrames =
            MmwDemo_RFParser_getNumSubFrames(&ctrlCfg);

        if (MmwDemo_isAllCfgInPendingState() == 0)
        {
            CLI_write ("Error: Full configuration must be provided before sensor can be started "
                       "the first time\n");

            /* Although not strictly needed, bring back to the initial value since we
             * are rejecting this first time configuration, prevents misleading debug. */
            gMmwMssMCB.objDetCommonCfg.preStartCommonCfg.numSubFrames = 0;

            return -1;
        }
    }

    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: Sensor is already started\n");
        return 0;
    }

    if (doReconfig == false)
    {
         /* User intends to issue sensor start without config, check if no
            config was issued after stop and generate error if this is the case. */
         if (MmwDemo_isAllCfgInNonPendingState() == 0)
         {
             /* Message user differently if all config was issued or partial config was
                issued. */
             if (MmwDemo_isAllCfgInPendingState())
             {
                 CLI_write ("Error: You have provided complete new configuration, "
                            "issue \"sensorStart\" (without argument) if you want it to "
                            "take effect\n");
             }
             else
             {
                 CLI_write ("Error: You have provided partial configuration between stop and this "
                            "command and partial configuration cannot be undone."
                            "Issue the full configuration and do \"sensorStart\" \n");
             }
             return -1;
         }
    }
    else
    {
        /* User intends to issue sensor start with full config, check if all config
           was issued after stop and generate error if  is the case. */
        MMWave_CtrlCfg ctrlCfg;

        /* need to get number of sub-frames so that next function to check
         * pending state can work */
        CLI_getMMWaveExtensionConfig (&ctrlCfg);
        gMmwMssMCB.objDetCommonCfg.preStartCommonCfg.numSubFrames =
            MmwDemo_RFParser_getNumSubFrames(&ctrlCfg);
        
        if (MmwDemo_isAllCfgInPendingState() == 0)
        {
            /* Message user differently if no config was issued or partial config was
               issued. */
            if (MmwDemo_isAllCfgInNonPendingState())
            {
                CLI_write ("Error: You have provided no configuration, "
                           "issue \"sensorStart 0\" OR provide "
                           "full configuration and issue \"sensorStart\"\n");
            }
            else
            {
                CLI_write ("Error: You have provided partial configuration between stop and this "
                           "command and partial configuration cannot be undone."
                           "Issue the full configuration and do \"sensorStart\" \n");
            }
            /* Although not strictly needed, bring back to the initial value since we
             * are rejecting this first time configuration, prevents misleading debug. */
            gMmwMssMCB.objDetCommonCfg.preStartCommonCfg.numSubFrames = 0;
            return -1;
        }
    }

    /***********************************************************************************
     * Retreive and check mmwave Open related config before calling openSensor
     ***********************************************************************************/
    CLI_write ("Before Init\n");
    /*  Fill demo's MCB mmWave openCfg structure from the CLI configs*/
    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_INIT)
    {
        /* Get the open configuration: */
        CLI_getMMWaveExtensionOpenConfig (&gMmwMssMCB.cfg.openCfg);
        /* call sensor open */
        CLI_write ("open sensor call\n");
        retVal = MmwDemo_openSensor(true);
        if(retVal != 0)
        {
            CLI_write ("Error: Failed to open sensor. Error code %d\n", retVal);
            return -1;
        }
        CLI_write ("Sensor opened successfully\n");
        gMmwMssMCB.sensorState = MmwDemo_SensorState_OPENED;    
    }
    else
    {
        /* openCfg related configurations like chCfg, lowPowerMode, adcCfg
         * are only used on the first sensor start. If they are different
         * on a subsequent sensor start, then generate a fatal error
         * so the user does not think that the new (changed) configuration
         * takes effect, the board needs to be reboot for the new
         * configuration to be applied.
         */
        MMWave_OpenCfg openCfg;
        CLI_getMMWaveExtensionOpenConfig (&openCfg);
        /* Compare openCfg->chCfg*/
        if(memcmp((void *)&gMmwMssMCB.cfg.openCfg.chCfg, (void *)&openCfg.chCfg,
                          sizeof(rlChanCfg_t)) != 0)
        {
            MmwDemo_debugAssert(0);
        }
        
        /* Compare openCfg->lowPowerMode*/
        if(memcmp((void *)&gMmwMssMCB.cfg.openCfg.lowPowerMode, (void *)&openCfg.lowPowerMode,
                          sizeof(rlLowPowerModeCfg_t)) != 0)
        {
            MmwDemo_debugAssert(0);
        }
        /* Compare openCfg->adcOutCfg*/
        if(memcmp((void *)&gMmwMssMCB.cfg.openCfg.adcOutCfg, (void *)&openCfg.adcOutCfg,
                          sizeof(rlAdcOutCfg_t)) != 0)
        {
            MmwDemo_debugAssert(0);
        }
    }

    

    /***********************************************************************************
     * Retrieve mmwave Control related config before calling startSensor
     ***********************************************************************************/
    /* Get the mmWave ctrlCfg from the CLI mmWave Extension */
    if(doReconfig)
    {
        /* Save the entire ctrlCfg structure to preserve settings from openSensor */
        MMWave_CtrlCfg savedCtrlCfg;
        memcpy(&savedCtrlCfg, &gMmwMssMCB.cfg.ctrlCfg, sizeof(MMWave_CtrlCfg));
        
        /* Get CLI configuration into a temporary structure */
        MMWave_CtrlCfg tempCtrlCfg;
        CLI_getMMWaveExtensionConfig(&tempCtrlCfg);
        
        /* Restore the original control config from openSensor */
        memcpy(&gMmwMssMCB.cfg.ctrlCfg, &savedCtrlCfg, sizeof(MMWave_CtrlCfg));
        
        
        retVal = MmwDemo_configSensor();
        if(retVal != 0)
        {
            return -1;
        }
        CLI_write ("config sensor success\n");
    }
    CLI_write ("Starting sensor...\n");
    retVal = MmwDemo_startSensor();
    if(retVal != 0)
    {
        CLI_write("Error: MmwDemo_startSensor failed with error code %d\n", retVal);
        return -1;
    }
    CLI_write ("Sensor start command issued successfully\n");

    /***********************************************************************************
     * Set the state
     ***********************************************************************************/
    gMmwMssMCB.sensorState = MmwDemo_SensorState_STARTED;
    CLI_write ("Sensor started successfully\n");

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for the sensor stop command
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLISensorStop (int32_t argc, char* argv[])
{
    if ((gMmwMssMCB.sensorState == MmwDemo_SensorState_STOPPED) ||
        (gMmwMssMCB.sensorState == MmwDemo_SensorState_INIT) ||
        (gMmwMssMCB.sensorState == MmwDemo_SensorState_OPENED))
    {
        CLI_write ("Ignored: Sensor is already stopped\n");
        return 0;
    }

    MmwDemo_stopSensor();

    MmwDemo_resetStaticCfgPendingState();

    gMmwMssMCB.sensorState = MmwDemo_SensorState_STOPPED;
    return 0;
}

/**
 *  @b Description
 *  @n
 *      Utility function to get sub-frame number
 *
 *  @param[in] argc  Number of arguments
 *  @param[in] argv  Arguments
 *  @param[in] expectedArgc Expected number of arguments
 *  @param[out] subFrameNum Sub-frame Number (0 based)
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIGetSubframe (int32_t argc, char* argv[], int32_t expectedArgc,
                                       int8_t* subFrameNum)
{
    int8_t subframe;
    
    /* Sanity Check: Minimum argument check */
    if (argc != expectedArgc)
    {
        CLI_write ("Error: Invalid usage of the CLI command\n");
        return -1;
    }

    /*Subframe info is always in position 1*/
    subframe = (int8_t) atoi(argv[1]);

    if(subframe >= (int8_t)RL_MAX_SUBFRAMES)
    {
        CLI_write ("Error: Subframe number is invalid\n");
        return -1;
    }

    *subFrameNum = (int8_t)subframe;

    return 0;
}



/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for gui monitoring configuration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIGuiMonSel (int32_t argc, char* argv[])
{
    MmwDemo_GuiMonSel   guiMonSel;
    int8_t              subFrameNum;

    if(MmwDemo_CLIGetSubframe(argc, argv, 8, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize the guiMonSel configuration: */
    memset ((void *)&guiMonSel, 0, sizeof(MmwDemo_GuiMonSel));

    /* Populate configuration: */
    guiMonSel.detectedObjects           = atoi (argv[2]);
    guiMonSel.logMagRange               = atoi (argv[3]);
    guiMonSel.noiseProfile              = atoi (argv[4]);
    guiMonSel.rangeAzimuthHeatMap       = atoi (argv[5]);
    guiMonSel.rangeDopplerHeatMap       = atoi (argv[6]);
    guiMonSel.statsInfo                 = atoi (argv[7]);

    MmwDemo_CfgUpdate((void *)&guiMonSel, MMWDEMO_GUIMONSEL_OFFSET,
        sizeof(MmwDemo_GuiMonSel), subFrameNum);

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for CFAR configuration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLICfarCfg (int32_t argc, char* argv[])
{
    DPU_CFARCAProc_CfarCfg   cfarCfg;
    uint32_t            procDirection;
    int8_t              subFrameNum;
    float               threshold;

    if(MmwDemo_CLIGetSubframe(argc, argv, 10, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&cfarCfg, 0, sizeof(cfarCfg));

    /* Populate configuration: */
    procDirection             = (uint32_t) atoi (argv[2]);
    cfarCfg.averageMode       = (uint8_t) atoi (argv[3]);
    cfarCfg.winLen            = (uint8_t) atoi (argv[4]);
    cfarCfg.guardLen          = (uint8_t) atoi (argv[5]);
    cfarCfg.noiseDivShift     = (uint8_t) atoi (argv[6]);
    cfarCfg.cyclicMode        = (uint8_t) atoi (argv[7]);
    threshold                 = (float) atof (argv[8]);
    cfarCfg.peakGroupingEn    = (uint8_t) atoi (argv[9]);

    if (threshold > 100.0)
    {
        CLI_write("Error: Maximum value for CFAR thresholdScale is 100.0 dB.\n");
        return -1;
    }   
    
    /* threshold is a float value from 0-100dB. It needs to
       be later converted to linear scale (conversion can only be done
       when the number of virtual antennas is known) before passing it
       to CFAR DPU.
       For now, the threshold will be coded in a 16bit integer in the following
       way:
       suppose threshold is a float represented as XYZ.ABC
       it will be saved as a 16bit integer XYZAB       
       that is, 2 decimal cases are saved.*/
    threshold = threshold * MMWDEMO_CFAR_THRESHOLD_ENCODING_FACTOR;   
    cfarCfg.thresholdScale    = (uint16_t) threshold;
    
    /* Save Configuration to use later */     
    if (procDirection == 0)
    {
        MmwDemo_CfgUpdate((void *)&cfarCfg, MMWDEMO_CFARCFGRANGE_OFFSET,
                          sizeof(cfarCfg), subFrameNum);
    }
    else
    {
        MmwDemo_CfgUpdate((void *)&cfarCfg, MMWDEMO_CFARCFGDOPPLER_OFFSET,
                          sizeof(cfarCfg), subFrameNum);
    }
    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for CFAR FOV (Field Of View) configuration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLICfarFovCfg (int32_t argc, char* argv[])
{
    DPU_CFARCAProc_FovCfg   fovCfg;
    uint32_t            procDirection;
    int8_t              subFrameNum;

    if(MmwDemo_CLIGetSubframe(argc, argv, 5, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&fovCfg, 0, sizeof(fovCfg));

    /* Populate configuration: */
    procDirection             = (uint32_t) atoi (argv[2]);
    fovCfg.min                = (float) atof (argv[3]);
    fovCfg.max                = (float) atof (argv[4]);

    /* Save Configuration to use later */
    if (procDirection == 0)
    {
        MmwDemo_CfgUpdate((void *)&fovCfg, MMWDEMO_FOVRANGE_OFFSET,
                          sizeof(fovCfg), subFrameNum);
    }
    else
    {
        MmwDemo_CfgUpdate((void *)&fovCfg, MMWDEMO_FOVDOPPLER_OFFSET,
                          sizeof(fovCfg), subFrameNum);
    }
    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for AoA FOV (Field Of View) configuration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIAoAFovCfg (int32_t argc, char* argv[])
{
    DPU_AoAProc_FovAoaCfg   fovCfg;
    int8_t              subFrameNum;

    if(MmwDemo_CLIGetSubframe(argc, argv, 6, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&fovCfg, 0, sizeof(fovCfg));

    /* Populate configuration: */
    fovCfg.minAzimuthDeg      = (float) atoi (argv[2]);
    fovCfg.maxAzimuthDeg      = (float) atoi (argv[3]);
    fovCfg.minElevationDeg    = (float) atoi (argv[4]);
    fovCfg.maxElevationDeg    = (float) atoi (argv[5]);

    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&fovCfg, MMWDEMO_FOVAOA_OFFSET,
                      sizeof(fovCfg), subFrameNum);
    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for extended maximum velocity configuration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIExtendedMaxVelocity (int32_t argc, char* argv[])
{

    DPU_AoAProc_ExtendedMaxVelocityCfg   cfg;
    int8_t              subFrameNum;

    if(MmwDemo_CLIGetSubframe(argc, argv, 3, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&cfg, 0, sizeof(cfg));

    /* Populate configuration: */
    cfg.enabled      = (uint8_t) atoi (argv[2]);

    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&cfg, MMWDEMO_EXTMAXVEL_OFFSET,
                      sizeof(cfg), subFrameNum);

    return 0;

}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for multi object beam forming configuration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIMultiObjBeamForming (int32_t argc, char* argv[])
{
    DPU_AoAProc_MultiObjBeamFormingCfg cfg;
    int8_t              subFrameNum;

    if(MmwDemo_CLIGetSubframe(argc, argv, 4, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&cfg, 0, sizeof(cfg));

    /* Populate configuration: */
    cfg.enabled                     = (uint8_t) atoi (argv[2]);
    cfg.multiPeakThrsScal           = (float) atof (argv[3]);

    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&cfg, MMWDEMO_MULTIOBJBEAMFORMING_OFFSET,
                      sizeof(cfg), subFrameNum);

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for DC range calibration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLICalibDcRangeSig (int32_t argc, char* argv[])
{
    DPU_RangeProc_CalibDcRangeSigCfg cfg;
    uint32_t                   log2NumAvgChirps;
    int8_t                     subFrameNum;

    if(MmwDemo_CLIGetSubframe(argc, argv, 6, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration for DC range signature calibration */
    memset ((void *)&cfg, 0, sizeof(cfg));

    /* Populate configuration: */
    cfg.enabled          = (uint16_t) atoi (argv[2]);
    cfg.negativeBinIdx   = (int16_t)  atoi (argv[3]);
    cfg.positiveBinIdx   = (int16_t)  atoi (argv[4]);
    cfg.numAvgChirps     = (uint16_t) atoi (argv[5]);

    if (cfg.negativeBinIdx > 0)
    {
        CLI_write ("Error: Invalid negative bin index\n");
        return -1;
    }

    if (cfg.positiveBinIdx < 0)
    {
        CLI_write ("Error: Invalid positive bin index\n");
        return -1;
    }

    if ((cfg.positiveBinIdx - cfg.negativeBinIdx + 1) > DPU_RANGEPROC_SIGNATURE_COMP_MAX_BIN_SIZE)
    {
        CLI_write ("Error: Number of bins exceeds the limit\n");
        return -1;
    }
    log2NumAvgChirps = (uint32_t) mathUtils_ceilLog2(cfg.numAvgChirps);
    if (cfg.numAvgChirps != (1U << log2NumAvgChirps))
    {
        CLI_write ("Error: Number of averaged chirps is not power of two\n");
        return -1;
    }

    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&cfg, MMWDEMO_CALIBDCRANGESIG_OFFSET,
                      sizeof(cfg), subFrameNum);

    return 0;
}

/**
 *  @b Description
 *  @n
 *      Clutter removal Configuration
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIClutterRemoval (int32_t argc, char* argv[])
{
    DPC_ObjectDetection_StaticClutterRemovalCfg_Base cfg;
    int8_t              subFrameNum;

    if(MmwDemo_CLIGetSubframe(argc, argv, 3, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration for clutter removal */
    memset ((void *)&cfg, 0, sizeof(cfg));

    /* Populate configuration: */
    cfg.enabled          = (uint16_t) atoi (argv[2]);

    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&cfg, MMWDEMO_STATICCLUTTERREMOFVAL_OFFSET,
                      sizeof(cfg), subFrameNum);

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for data logger set command
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIADCBufCfg (int32_t argc, char* argv[])
{
    MmwDemo_ADCBufCfg   adcBufCfg;
    int8_t              subFrameNum;

    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    if(MmwDemo_CLIGetSubframe(argc, argv, 6, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize the ADC Output configuration: */
    memset ((void *)&adcBufCfg, 0, sizeof(adcBufCfg));

    /* Populate configuration: */
    adcBufCfg.adcFmt          = (uint8_t) atoi (argv[2]);
    adcBufCfg.iqSwapSel       = (uint8_t) atoi (argv[3]);
    adcBufCfg.chInterleave    = (uint8_t) atoi (argv[4]);
    adcBufCfg.chirpThreshold  = (uint8_t) atoi (argv[5]);

    /* This demo is using HWA for 1D processing which does not allow multi-chirp
     * processing */
    if (adcBufCfg.chirpThreshold != 1)
    {
        CLI_write("Error: chirpThreshold must be 1, multi-chirp is not allowed\n");
        return -1;
    }
    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&adcBufCfg,
                      MMWDEMO_ADCBUFCFG_OFFSET,
                      sizeof(MmwDemo_ADCBufCfg), subFrameNum);
    return 0;
}


/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for compensation of range bias and channel phase offsets
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLICompRangeBiasAndRxChanPhaseCfg (int32_t argc, char* argv[])
{
    DPU_AoAProc_compRxChannelBiasCfg   cfg;
    int32_t Re, Im;
    int32_t argInd;
    int32_t i;

    /* Sanity Check: Minimum argument check */
    if (argc != (1+1+SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL*2))
    {
        CLI_write ("Error: Invalid usage of the CLI command\n");
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&cfg, 0, sizeof(cfg));

    /* Populate configuration: */
    cfg.rangeBias          = (float) atof (argv[1]);

    argInd = 2;
    for (i=0; i < SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL; i++)
    {
        Re = (int32_t) (atof (argv[argInd++]) * 32768.);
        MATHUTILS_SATURATE16(Re);
        cfg.rxChPhaseComp[i].real = (int16_t) Re;

        Im = (int32_t) (atof (argv[argInd++]) * 32768.);
        MATHUTILS_SATURATE16(Im);
        cfg.rxChPhaseComp[i].imag = (int16_t) Im;

    }
    /* Save Configuration to use later */
    memcpy((void *) &gMmwMssMCB.objDetCommonCfg.preStartCommonCfg.compRxChanCfg,
           &cfg, sizeof(cfg));

    gMmwMssMCB.objDetCommonCfg.isCompRxChannelBiasCfgPending = 1;

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for measurement configuration of range bias
 *      and channel phase offsets
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIMeasureRangeBiasAndRxChanPhaseCfg (int32_t argc, char* argv[])
{
    DPC_ObjectDetection_MeasureRxChannelBiasCfg   cfg;

    /* Sanity Check: Minimum argument check */
    if (argc != 4)
    {
        CLI_write ("Error: Invalid usage of the CLI command\n");
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&cfg, 0, sizeof(cfg));

    /* Populate configuration: */
    cfg.enabled          = (uint8_t) atoi (argv[1]);
    cfg.targetDistance   = (float) atof (argv[2]);
    cfg.searchWinSize   = (float) atof (argv[3]);

    /* Save Configuration to use later */
    memcpy((void *) &gMmwMssMCB.objDetCommonCfg.preStartCommonCfg.measureRxChannelBiasCfg,
           &cfg, sizeof(cfg));

    gMmwMssMCB.objDetCommonCfg.isMeasureRxChannelBiasCfgPending = 1;

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for BPM configuration supported by the mmw Demo
 *      Note that there is a generic BPM configuration command supported by
 *      utils/cli and mmwave. The generic BPM command is not supported by the
 *      demo as the mmw demo assumes a specific BPM pattern for the TX antennas.
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIBpmCfg (int32_t argc, char* argv[])
{
    int8_t              subFrameNum;
    MmwDemo_BpmCfg      bpmCfg;

    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    /* Sanity Check: Minimum argument check */
    if (argc != 5)
    {
        CLI_write ("Error: Invalid usage of the CLI command\n");
        return -1;
    }

    if(MmwDemo_CLIGetSubframe(argc, argv, 5, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration for DC range signature calibration */
    memset ((void *)&bpmCfg, 0, sizeof(MmwDemo_BpmCfg));

    /* Populate configuration: */
    bpmCfg.isEnabled = (bool) atoi(argv[2]) ;
    bpmCfg.chirp0Idx = (uint16_t) atoi(argv[3]) ;
    bpmCfg.chirp1Idx = (uint16_t) atoi(argv[4]) ;

    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&bpmCfg, MMWDEMO_BPMCFG_OFFSET,
                      sizeof(MmwDemo_BpmCfg), subFrameNum);

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for configuring CQ RX Saturation monitor
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIChirpQualityRxSatMonCfg (int32_t argc, char* argv[])
{
    rlRxSatMonConf_t        cqSatMonCfg;

    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    /* Sanity Check: Minimum argument check */
    if (argc != 6)
    {
        CLI_write ("Error: Invalid usage of the CLI command\n");
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&cqSatMonCfg, 0, sizeof(rlRxSatMonConf_t));

    /* Populate configuration: */
    cqSatMonCfg.profileIndx                 = (uint8_t) atoi (argv[1]);

    if(cqSatMonCfg.profileIndx < RL_MAX_PROFILES_CNT)
    {

        cqSatMonCfg.satMonSel                   = (uint8_t) atoi (argv[2]);
        cqSatMonCfg.primarySliceDuration        = (uint16_t) atoi (argv[3]);
        cqSatMonCfg.numSlices                   = (uint16_t) atoi (argv[4]);
        cqSatMonCfg.rxChannelMask               = (uint8_t) atoi (argv[5]);

        /* Save Configuration to use later */
        gMmwMssMCB.cqSatMonCfg[cqSatMonCfg.profileIndx] = cqSatMonCfg;

        return 0;
    }
    else
    {
        return -1;
    }
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for configuring CQ Signal & Image band monitor
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIChirpQualitySigImgMonCfg (int32_t argc, char* argv[])
{
    rlSigImgMonConf_t       cqSigImgMonCfg;

    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    /* Sanity Check: Minimum argument check */
    if (argc != 4)
    {
        CLI_write ("Error: Invalid usage of the CLI command\n");
        return -1;
    }

    /* Initialize configuration: */
    memset ((void *)&cqSigImgMonCfg, 0, sizeof(rlSigImgMonConf_t));

    /* Populate configuration: */
    cqSigImgMonCfg.profileIndx              = (uint8_t) atoi (argv[1]);

    if(cqSigImgMonCfg.profileIndx < RL_MAX_PROFILES_CNT)
    {
        cqSigImgMonCfg.numSlices            = (uint8_t) atoi (argv[2]);
        cqSigImgMonCfg.timeSliceNumSamples  = (uint16_t) atoi (argv[3]);

        /* Save Configuration to use later */
        gMmwMssMCB.cqSigImgMonCfg[cqSigImgMonCfg.profileIndx] = cqSigImgMonCfg;

        return 0;
    }
    else
    {
        return -1;
    }
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for enabling analog monitors
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIAnalogMonitorCfg (int32_t argc, char* argv[])
{
    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    /* Sanity Check: Minimum argument check */
    if (argc != 3)
    {
        CLI_write ("Error: Invalid usage of the CLI command\n");
        return -1;
    }

    /* Save Configuration to use later */
    gMmwMssMCB.anaMonCfg.rxSatMonEn = atoi (argv[1]);
    gMmwMssMCB.anaMonCfg.sigImgMonEn = atoi (argv[2]);

    gMmwMssMCB.isAnaMonCfgPending = 1;

    return 0;
}


/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for the High Speed Interface
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLILvdsStreamCfg (int32_t argc, char* argv[])
{
    MmwDemo_LvdsStreamCfg   cfg;
    int8_t                  subFrameNum;

    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    if(MmwDemo_CLIGetSubframe(argc, argv, 5, &subFrameNum) < 0)
    {
        return -1;
    }

    /* Initialize configuration for DC range signature calibration */
    memset ((void *)&cfg, 0, sizeof(MmwDemo_LvdsStreamCfg));

    /* Populate configuration: */
    cfg.isHeaderEnabled = (bool)    atoi(argv[2]);
    cfg.dataFmt         = (uint8_t) atoi(argv[3]);
    cfg.isSwEnabled     = (bool)    atoi(argv[4]);

    /* If both h/w and s/w are enabled, HSI header must be enabled, because
     * we don't allow mixed h/w session without HSI header
     * simultaneously with s/w session with HSI header (s/w session always
     * streams HSI header) */
    if ((cfg.isSwEnabled == true) && (cfg.dataFmt != MMW_DEMO_LVDS_STREAM_CFG_DATAFMT_DISABLED))
    {
        if (cfg.isHeaderEnabled == false)
        {
            CLI_write("Error: header must be enabled when both h/w and s/w streaming are enabled\n");
            return -1;
        }
    }

    /* Save Configuration to use later */
    MmwDemo_CfgUpdate((void *)&cfg,
                      MMWDEMO_LVDSSTREAMCFG_OFFSET,
                      sizeof(MmwDemo_LvdsStreamCfg), subFrameNum);

    return 0;
}


/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for configuring the data port
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIConfigDataPort (int32_t argc, char* argv[])
{
    uint32_t baudrate;
    bool  ackPing;
    UART_Params uartParams;
    uint8_t ackData[16];
    

    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    /* Populate configuration: */
    baudrate = (uint32_t) atoi(argv[1]);
    ackPing = (bool) atoi(argv[2]);

    /* check if requested value is less than max supported value */
    if (baudrate > MMWDEMO_DATAUART_MAX_BAUDRATE_SUPPORTED)
    {
        CLI_write ("Ignored: Invalid baud rate (%d) specified\n",baudrate);
        return 0;
    }

    /* re-open UART port if requested baud rate is different than current */
    if (gMmwMssMCB.cfg.platformCfg.loggingBaudRate != baudrate)
    {
        /* close previous opened handle */
        /* since the sensor is not running at this time, it is safe to close
           the existing port */
        if (gMmwMssMCB.loggingUartHandle != NULL)
        {
            UART_close(gMmwMssMCB.loggingUartHandle);
            gMmwMssMCB.loggingUartHandle = NULL;
        }   
        
        /* Setup the default UART Parameters */
        UART_Params_init(&uartParams);
        uartParams.writeDataMode  = UART_DATA_BINARY;
        uartParams.readDataMode   = UART_DATA_BINARY;
        uartParams.clockFrequency = gMmwMssMCB.cfg.platformCfg.sysClockFrequency;
        uartParams.baudRate       = baudrate;
        uartParams.isPinMuxDone   = 1U;

        /* Open the Logging UART Instance: */
        gMmwMssMCB.loggingUartHandle = UART_open(1, &uartParams);
        if (gMmwMssMCB.loggingUartHandle == NULL)
        {
            CLI_write ("Error: Unable to open the Logging UART Instance\n");
            return -1;
        }
        gMmwMssMCB.cfg.platformCfg.loggingBaudRate = baudrate;
        CLI_write ("Data port baud rate changed to %d\n",baudrate);
    }

    /* regardless of baud rate update, ack back to the host over this UART 
       port if handle is valid and user has requested the ack back */
    if ((gMmwMssMCB.loggingUartHandle != NULL) && (ackPing == true))
    {
        memset(ackData,0xFF,sizeof(ackData));
        UART_writePolling (gMmwMssMCB.loggingUartHandle,
                           (uint8_t*)ackData,
                           sizeof(ackData));
    }

    return 0;
}


/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for querying Demo status
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLIQueryDemoStatus (int32_t argc, char* argv[])
{
    CLI_write ("Sensor State: %d\n",gMmwMssMCB.sensorState);
    CLI_write ("Data port baud rate: %d\n",gMmwMssMCB.cfg.platformCfg.loggingBaudRate);

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Handler for save/restore calibration data to/from flash
 *
 *  @param[in] argc
 *      Number of arguments
 *  @param[in] argv
 *      Arguments
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_CLICalibDataSaveRestore(int32_t argc, char* argv[])
{
    if (gMmwMssMCB.sensorState == MmwDemo_SensorState_STARTED)
    {
        CLI_write ("Ignored: This command is not allowed after sensor has started\n");
        return 0;
    }

    /* Validate inputs */
    if ( ((uint32_t) atoi(argv[1]) == 1) && ((uint32_t) atoi(argv[2] ) == 1))
    {
        CLI_write ("Error: Save and Restore can be enabled only one at a time\n");
        return -1;
    }

    /* Populate configuration: */
    gMmwMssMCB.calibCfg.saveEnable = (uint32_t) atoi(argv[1]);
    gMmwMssMCB.calibCfg.restoreEnable = (uint32_t) atoi(argv[2]);
    sscanf(argv[3], "0x%x", &gMmwMssMCB.calibCfg.flashOffset);

    gMmwMssMCB.isCalibCfgPending = 1;

    return 0;
}

/**
 *  @b Description
 *  @n
 *      This is the CLI Execution Task
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_CLIInit (uint8_t taskPriority)
{
    CLI_Cfg     cliCfg;
    char        demoBanner[256];
    uint32_t    cnt;

    /* Create Demo Banner to be printed out by CLI */
    sprintf(&demoBanner[0], 
                       "******************************************\n" \
                       "xWR68xx MMW Demo %02d.%02d.%02d.%02d\n"  \
                       "******************************************\n", 
                        MMWAVE_SDK_VERSION_MAJOR,
                        MMWAVE_SDK_VERSION_MINOR,
                        MMWAVE_SDK_VERSION_BUGFIX,
                        MMWAVE_SDK_VERSION_BUILD
            );

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

    /* Populate the CLI configuration: */
    cliCfg.cliPrompt                    = "mmwDemo:/>";
    cliCfg.cliBanner                    = demoBanner;
    cliCfg.cliUartHandle                = gMmwMssMCB.commandUartHandle;
    cliCfg.taskPriority                 = taskPriority;
    cliCfg.socHandle                    = gMmwMssMCB.socHandle;
    cliCfg.mmWaveHandle                 = gMmwMssMCB.ctrlHandle;
    cliCfg.enableMMWaveExtension        = 1U;
    cliCfg.usePolledMode                = true;
    cliCfg.overridePlatform             = false;
    cliCfg.overridePlatformString       = NULL;    
    
    cnt=0;
    cliCfg.tableEntry[cnt].cmd            = "sensorStart";
    cliCfg.tableEntry[cnt].helpString     = "[doReconfig(optional, default:enabled)]";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLISensorStart;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "sensorStop";
    cliCfg.tableEntry[cnt].helpString     = "No arguments";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLISensorStop;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "guiMonitor";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <detectedObjects> <logMagRange> <noiseProfile> <rangeAzimuthHeatMap> <rangeDopplerHeatMap> <statsInfo>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIGuiMonSel;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "cfarCfg";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <procDirection> <averageMode> <winLen> <guardLen> <noiseDiv> <cyclicMode> <thresholdScale> <peakGroupingEn>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLICfarCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "multiObjBeamForming";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <enabled> <threshold>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIMultiObjBeamForming;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "calibDcRangeSig";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <enabled> <negativeBinIdx> <positiveBinIdx> <numAvgFrames>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLICalibDcRangeSig;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "clutterRemoval";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <enabled>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIClutterRemoval;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "adcbufCfg";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <adcOutputFmt> <SampleSwap> <ChanInterleave> <ChirpThreshold>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIADCBufCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "compRangeBiasAndRxChanPhase";
    cliCfg.tableEntry[cnt].helpString     = "<rangeBias> <Re00> <Im00> <Re01> <Im01> <Re02> <Im02> <Re03> <Im03> <Re10> <Im10> <Re11> <Im11> <Re12> <Im12> <Re13> <Im13> ";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLICompRangeBiasAndRxChanPhaseCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "measureRangeBiasAndRxChanPhase";
    cliCfg.tableEntry[cnt].helpString     = "<enabled> <targetDistance> <searchWin>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIMeasureRangeBiasAndRxChanPhaseCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "aoaFovCfg";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <minAzimuthDeg> <maxAzimuthDeg> <minElevationDeg> <maxElevationDeg>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIAoAFovCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "cfarFovCfg";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <procDirection> <min (meters or m/s)> <max (meters or m/s)>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLICfarFovCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "extendedMaxVelocity";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <enabled>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIExtendedMaxVelocity;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "bpmCfg";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <enabled> <chirp0Idx> <chirp1Idx>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIBpmCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "CQRxSatMonitor";
    cliCfg.tableEntry[cnt].helpString     = "<profile> <satMonSel> <priSliceDuration> <numSlices> <rxChanMask>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIChirpQualityRxSatMonCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "CQSigImgMonitor";
    cliCfg.tableEntry[cnt].helpString     = "<profile> <numSlices> <numSamplePerSlice>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIChirpQualitySigImgMonCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "analogMonitor";
    cliCfg.tableEntry[cnt].helpString     = "<rxSaturation> <sigImgBand>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIAnalogMonitorCfg;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "lvdsStreamCfg";
    cliCfg.tableEntry[cnt].helpString     = "<subFrameIdx> <enableHeader> <dataFmt> <enableSW>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLILvdsStreamCfg;
    cnt++;
 
    cliCfg.tableEntry[cnt].cmd            = "configDataPort";
    cliCfg.tableEntry[cnt].helpString     = "<baudrate> <ackPing>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIConfigDataPort;
    cnt++;
    
    cliCfg.tableEntry[cnt].cmd            = "queryDemoStatus";
    cliCfg.tableEntry[cnt].helpString     = "";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLIQueryDemoStatus;
    cnt++;

    cliCfg.tableEntry[cnt].cmd            = "calibData";
    cliCfg.tableEntry[cnt].helpString    = "<save enable> <restore enable> <Flash offset>";
    cliCfg.tableEntry[cnt].cmdHandlerFxn  = MmwDemo_CLICalibDataSaveRestore;
    cnt++;

    /* Open the CLI: */
    if (CLI_open (&cliCfg) < 0)
    {
        System_printf ("Error: Unable to open the CLI\n");
        return;
    }
    System_printf ("Debug: CLI is operational\n");
    return;
}

/**
 *   @file  dss_main.c
 *
 *   @brief
 *      This is the main file which implements the millimeter wave Demo
 *
 *  \par
 *  NOTE:
 *      (C) Copyright 2018 Texas Instruments, Inc.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "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 THE COPYRIGHT
 *  OWNER OR CONTRIBUTORS 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>
#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/utils/Load.h>
#include <ti/sysbios/family/c64p/Hwi.h>

/* mmWave SDK Include Files: */
#include <ti/common/sys_common.h>
#include <ti/common/mmwave_sdk_version.h>
#include <ti/control/dpm/dpm.h>
#include <ti/drivers/soc/soc.h>
#include <ti/drivers/esm/esm.h>
#include <ti/drivers/crc/crc.h>
#include <ti/drivers/osal/DebugP.h>

/* Data path Include Files */
#include <ti/datapath/dpc/objectdetection/objdetdsp/objectdetection.h>
#include <ti/datapath/dpu/rangeproc/rangeprocdsp.h>

/* Demo Include Files */
#include <ti/demo/xwr68xx/mmw/include/mmw_output.h>
#include <ti/demo/xwr68xx/mmw/dss/mmw_dss.h>
#include <ti/demo/xwr68xx/mmw/mmw_res.h>

/* Demo Profiling Include Files */
#include <ti/utils/cycleprofiler/cycle_profiler.h>

/**
 * @brief Task Priority settings:
 */
#define MMWDEMO_DPC_OBJDET_DPM_TASK_PRIORITY      5

/*! L3 RAM buffer for object detection DPC */
uint8_t gMmwL3[SOC_L3RAM_SIZE];
#pragma DATA_SECTION(gMmwL3, ".l3ram");

 /*! L2 RAM buffer for object detection DPC */
#define MMWDEMO_OBJDET_L2RAM_SIZE (49U * 1024U)
uint8_t gDPC_ObjDetL2Heap[MMWDEMO_OBJDET_L2RAM_SIZE];
#pragma DATA_SECTION(gDPC_ObjDetL2Heap, ".dpc_l2Heap");

 /*! L1 RAM buffer for object detection DPC */
#define MMWDEMO_OBJDET_L1RAM_SIZE (16U * 1024U)
uint8_t gDPC_ObjDetL1Heap[MMWDEMO_OBJDET_L1RAM_SIZE];
#pragma DATA_SECTION(gDPC_ObjDetL1Heap, ".dpc_l1Heap");

 /*! HSRAM for processing results */
#pragma DATA_SECTION(gHSRAM, ".demoSharedMem");
#pragma DATA_ALIGN(gHSRAM, 4);

#define DPC_OBJDET_DSP_INSTANCEID       (0xDEEDDEED)

/**************************************************************************
 *************************** Global Definitions ***************************
 **************************************************************************/

/**
 * @brief
 *  Global Variable for tracking information required by the mmw Demo
 */
MmwDemo_DSS_MCB    gMmwDssMCB;

/**
 * @brief
 *  Global Variable for DPM result buffer
 */
DPM_Buffer  resultBuffer;

/**
 * @brief
 *  Global Variable for HSRAM buffer used to share results to remote
 */
MmwDemo_HSRAM gHSRAM;

/**************************************************************************
 ******************* Millimeter Wave Demo Functions Prototype *******************
 **************************************************************************/
static void MmwDemo_dssInitTask(UArg arg0, UArg arg1);
static void MmwDemo_DPC_ObjectDetection_reportFxn
(
    DPM_Report  reportType,
    uint32_t    instanceId,
    int32_t     errCode,
    uint32_t    arg0,
    uint32_t    arg1
);
static void MmwDemo_DPC_ObjectDetection_processFrameBeginCallBackFxn(uint8_t subFrameIndx);
static void MmwDemo_DPC_ObjectDetection_processInterFrameBeginCallBackFxn(uint8_t subFrameIndx);
static void MmwDemo_updateObjectDetStats
(
    DPC_ObjectDetection_Stats       *currDpcStats,
    MmwDemo_output_message_stats    *outputMsgStats
);

static int32_t MmwDemo_copyResultToHSRAM
(
    MmwDemo_HSRAM           *ptrHsramBuffer,
    DPC_ObjectDetection_ExecuteResult *result,
    MmwDemo_output_message_stats *outStats
);
static void MmwDemo_DPC_ObjectDetection_dpmTask(UArg arg0, UArg arg1);
static void MmwDemo_sensorStopEpilog(void);

/**************************************************************************
 ************************* Millimeter Wave Demo Functions **********************
 **************************************************************************/
/**
 *  @b Description
 *  @n
 *      EDMA driver init
 *
 *  @param[in] obj      Pointer to data path object
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_edmaInit(MmwDemo_DataPathObj *obj, uint8_t instance)
{
    int32_t errorCode;

    errorCode = EDMA_init(instance);
    if (errorCode != EDMA_NO_ERROR)
    {
        //System_printf ("Debug: EDMA instance %d initialization returned error %d\n", errorCode);
        MmwDemo_debugAssert (0);
        return;
    }

    memset(&obj->EDMA_errorInfo, 0, sizeof(obj->EDMA_errorInfo));
    memset(&obj->EDMA_transferControllerErrorInfo, 0, sizeof(obj->EDMA_transferControllerErrorInfo));
}

/**
 *  @b Description
 *  @n
 *      Call back function for EDMA CC (Channel controller) error as per EDMA API.
 *      Declare fatal error if happens, the output errorInfo can be examined if code
 *      gets trapped here.
 */
void MmwDemo_EDMA_errorCallbackFxn(EDMA_Handle handle, EDMA_errorInfo_t *errorInfo)
{
    gMmwDssMCB.dataPathObj.EDMA_errorInfo = *errorInfo;
    MmwDemo_debugAssert(0);
}

/**
 *  @b Description
 *  @n
 *      Call back function for EDMA transfer controller error as per EDMA API.
 *      Declare fatal error if happens, the output errorInfo can be examined if code
 *      gets trapped here.
 */
void MmwDemo_EDMA_transferControllerErrorCallbackFxn(EDMA_Handle handle,
                EDMA_transferControllerErrorInfo_t *errorInfo)
{
    gMmwDssMCB.dataPathObj.EDMA_transferControllerErrorInfo = *errorInfo;
    MmwDemo_debugAssert(0);
}

/**
 *  @b Description
 *  @n
 *      Open EDMA driver instance
 *
 *  @param[in] obj          Pointer to data path object
 *  @param[in] instance     EDMA instance
 *
 *  @retval
 *      Not Applicable.
 */
static void MmwDemo_edmaOpen(MmwDemo_DataPathObj *obj, uint8_t instance)
{
    int32_t              errCode;
    EDMA_instanceInfo_t  edmaInstanceInfo;
    EDMA_errorConfig_t   errorConfig;

    obj->edmaHandle = EDMA_open(
        instance,
        &errCode, 
        &edmaInstanceInfo);
    if (obj->edmaHandle == NULL)
    {
        MmwDemo_debugAssert (0);
        return;
    }

    errorConfig.isConfigAllEventQueues = true;
    errorConfig.isConfigAllTransferControllers = true;
    errorConfig.isEventQueueThresholdingEnabled = true;
    errorConfig.eventQueueThreshold = EDMA_EVENT_QUEUE_THRESHOLD_MAX;
    errorConfig.isEnableAllTransferControllerErrors = true;
    errorConfig.callbackFxn = MmwDemo_EDMA_errorCallbackFxn;
    errorConfig.transferControllerCallbackFxn = MmwDemo_EDMA_transferControllerErrorCallbackFxn;
    if ((errCode = EDMA_configErrorMonitoring(obj->edmaHandle, &errorConfig)) != EDMA_NO_ERROR)
    {
        //System_printf("Error: EDMA_configErrorMonitoring() failed with errorCode = %d\n", errCode);
        MmwDemo_debugAssert (0);
        return;
    }
}

/**
 *  @b Description
 *  @n
 *      Close EDMA driver instance
 *
 *  @param[in] obj      Pointer to data path object
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_edmaClose(MmwDemo_DataPathObj *obj)
{
    EDMA_close(obj->edmaHandle);
}

/**
 *  @b Description
 *  @n
 *      Epilog processing after sensor has stopped
 *
 *  @retval None
 */
static void MmwDemo_sensorStopEpilog(void)
{
    Hwi_StackInfo   stackInfo;
    Task_Stat       stat;
    bool            hwiStackOverflow;

    System_printf("Data Path Stopped (last frame processing done)\n");

    /* Print DSS task statistics */
    System_printf("DSS Task Stack Usage (Note: Task Stack Usage) ==========\n");

    Task_stat(gMmwDssMCB.initTaskHandle, &stat);
    System_printf("%20s %12d %12d %12d\n", "initTask",
                  stat.stackSize,
                  stat.used,
                  stat.stackSize - stat.used);

    Task_stat(gMmwDssMCB.objDetDpmTaskHandle, &stat);
    System_printf("%20s %12s %12s %12s\n", "Task Name", "Size", "Used", "Free");
    System_printf("%20s %12d %12d %12d\n", "ObjDet DPM",
                  stat.stackSize,
                  stat.used,
                  stat.stackSize - stat.used);

    System_printf("HWI Stack (same as System Stack) Usage ============\n");
    hwiStackOverflow = Hwi_getStackInfo(&stackInfo, TRUE);
    if (hwiStackOverflow == TRUE)
    {
        System_printf("DSS HWI Stack overflowed\n");
        MmwDemo_debugAssert(0);
    }
    else
    {
        System_printf("%20s %12s %12s %12s\n", " ", "Size", "Used", "Free");
        System_printf("%20s %12d %12d %12d\n", " ",
                      stackInfo.hwiStackSize,
                      stackInfo.hwiStackPeak,
                      stackInfo.hwiStackSize - stackInfo.hwiStackPeak);
    }
}

/**
 *  @b Description
 *  @n
 *      DPM Registered Report Handler. The DPM Module uses this registered function to notify
 *      the application about DPM reports.
 *
 *  @param[in]  reportType
 *      Report Type
 *  @param[in]  instanceId
 *      Instance Identifier which generated the report
 *  @param[in]  errCode
 *      Error code if any.
 *  @param[in] arg0
 *      Argument 0 interpreted with the report type
 *  @param[in] arg1
 *      Argument 1 interpreted with the report type
 *
 *  @retval
 *      Not Applicable.
 */
static void MmwDemo_DPC_ObjectDetection_reportFxn
(
    DPM_Report  reportType,
    uint32_t    instanceId,
    int32_t     errCode,
    uint32_t    arg0,
    uint32_t    arg1
)
{

    /* Only errors are logged on the console: */
    if (errCode != 0)
    {
        /* Error: Detected log on the console and die all errors are FATAL currently. */
        System_printf ("Error: DPM Report %d received with error:%d arg0:0x%x arg1:0x%x\n",
                        reportType, errCode, arg0, arg1);
        DebugP_assert (0);
    }

    /* Processing further is based on the reports received: This is the control of the profile
     * state machine: */
    switch (reportType)
    {
        case DPM_Report_IOCTL:
        {
            /*****************************************************************
             * DPC has been configured without an error:
             * - This is an indication that the profile configuration commands
             *   went through without any issues.
             *****************************************************************/
            DebugP_log1("DSSApp: DPM Report IOCTL, command = %d\n", arg0);
            System_printf("DSSApp: DPM Report IOCTL, command = %d\n", arg0);
            break;
        }
        case DPM_Report_DPC_STARTED:
        {
            /*****************************************************************
             * DPC has been started without an error:
             * - notify sensor management task that DPC is started.
             *****************************************************************/
            DebugP_log0("DSSApp: DPM Report start\n");
            gMmwDssMCB.dpmStartEvents++;
            System_printf("DSSApp: DPM Report start\n");
            /* every sensor start should cause 2 DPM start events due to distributed domain */
            break;
        }
        case DPM_Report_NOTIFY_DPC_RESULT:
        {
            /*****************************************************************
             * DPC Results have been passed:
             * - This implies that we have valid profile results which have
             *   been received from the profile.
             *****************************************************************/
            System_printf("DSSApp: DPM Report Notify DPC Result\n");
            break;
        }
        case DPM_Report_NOTIFY_DPC_RESULT_ACKED:
        {
            /*****************************************************************
             * DPC Results have been acked:
             * - This implies that MSS received the results.
             *****************************************************************/
            System_printf("DSSApp: DPM Report Notify DPC Result Acked\n");  
            break;
        }
        case DPM_Report_DPC_ASSERT:
        {
            DPM_DPCAssert*  ptrAssert;

            /*****************************************************************
             * DPC Fault has been detected:
             * - This implies that the DPC has crashed.
             * - The argument0 points to the DPC assertion information
             *****************************************************************/
            ptrAssert = (DPM_DPCAssert*)arg0;
            System_printf ("DSS Exception: %s, line %d.\n", ptrAssert->fileName,
                       ptrAssert->lineNum);
            break;
        }
        case DPM_Report_DPC_STOPPED:
        {
            /*****************************************************************
             * DPC has been stopped without an error:
             * - This implies that the DPC can either be reconfigured or
             *   restarted.
             *****************************************************************/
            DebugP_log0("DSSApp: DPM Report stop\n");
            gMmwDssMCB.dpmStopEvents++;
            /* every sensor stop should cause 2 DPM stop events due to distributed domain 
               Its safe to start printing logs on the first event as RF/RadarSS chirp/frame are already stopped */
            if (gMmwDssMCB.dpmStopEvents % 2 == 1) 
            {
                MmwDemo_sensorStopEpilog();
            }
            break;
        }
        case DPM_Report_DPC_INFO:
        {
            /* Currently chain does not use this feature. */
            break;
        }
        default:
        {
            DebugP_assert (0);
            break;
        }
    }
    return;
}

/**
 *  @b Description
 *  @n
 *      Call back function that was registered during config time and is going
 *      to be called in DPC processing at the beginning of frame/sub-frame processing.
 *      Note: In this demo objdetdsp DPC only have inter-frame processing, hence this 
 *      callback function won't be called.
 *
 *  @param[in] subFrameIndx     Sub-frame index of the sub-frame during which processing
 *                              this function was called.
 *
 *  @retval None
 */
static void MmwDemo_DPC_ObjectDetection_processFrameBeginCallBackFxn(uint8_t subFrameIndx)
{
    /* Adding debug output */
    System_printf("DSS: Frame Begin callback called for subframe %d\n", subFrameIndx);
}

/**
 *  @b Description
 *  @n
 *      Call back function that was registered during config time and is going
 *      to be called in DPC processing at the beginning of inter-frame/inter-sub-frame processing,
 *      we use this to issue BIOS calls for computing CPU load during active frame (chirping)
 *
 *  @param[in] subFrameIndx     Sub-frame index of the sub-frame during which processing
 *                              this function was called.
 *
 *  @retval None
 */
static void MmwDemo_DPC_ObjectDetection_processInterFrameBeginCallBackFxn(uint8_t subFrameIndx)
{
    /* Adding debug output */
    System_printf("DSS: Inter-Frame Begin callback called for subframe %d\n", subFrameIndx);
    
    Load_update();
    gMmwDssMCB.dataPathObj.subFrameStats[subFrameIndx].interFrameCPULoad = Load_getCPULoad();
}


/**
 *  @b Description
 *  @n
 *      Update stats based on the stats from DPC
 *
 *  @param[in]  currDpcStats        Pointer to DPC status
 *  @param[in]  outputMsgStats      Pointer to Output message stats 
 *
 *  @retval
 *      Not Applicable.
 */
 void MmwDemo_updateObjectDetStats
(
    DPC_ObjectDetection_Stats       *currDpcStats,
    MmwDemo_output_message_stats    *outputMsgStats
)
{
    static uint32_t prevInterFrameEndTimeStamp = 0U;

    /* Calculate interframe proc time */
    outputMsgStats->interFrameProcessingTime =
            (currDpcStats->interFrameEndTimeStamp - currDpcStats->interFrameStartTimeStamp)/DSP_CLOCK_MHZ; /* In micro seconds */

    outputMsgStats->interChirpProcessingMargin = currDpcStats->interChirpProcessingMargin/DSP_CLOCK_MHZ;

    /* Calculate interFrame processing Margin for previous frame, but saved to current frame */
    outputMsgStats->interFrameProcessingMargin =
        (currDpcStats->frameStartTimeStamp - prevInterFrameEndTimeStamp - currDpcStats->subFramePreparationCycles)/DSP_CLOCK_MHZ;

    prevInterFrameEndTimeStamp = currDpcStats->interFrameEndTimeStamp;
}


/**
 *  @b Description
 *  @n
 *      Copy DPC results and output stats to HSRAM to share with MSS
 *
 *  @param[in]  ptrHsramBuffer      Pointer to HSRAM buffer memory
 *  @param[in]  result              Pointer to DPC results
 *  @param[in]  outStats            Pointer to Output message stats
 *
 *  @retval
 *      Not Applicable.
 */
static int32_t MmwDemo_copyResultToHSRAM
(
    MmwDemo_HSRAM           *ptrHsramBuffer,
    DPC_ObjectDetection_ExecuteResult *result,
    MmwDemo_output_message_stats *outStats
)
{
    uint8_t             *ptrCurrBuffer;
    uint32_t            totalHsramSize;
    uint32_t            itemPayloadLen;

    /* Save result in HSRAM */
    if(ptrHsramBuffer == NULL)
    {
        return -1;
    }

    /* Save result in HSRAM */
    if(result != NULL)
    {
        itemPayloadLen = sizeof(DPC_ObjectDetection_ExecuteResult);
        memcpy((void *)&ptrHsramBuffer->result, (void *)result, itemPayloadLen);
    }
    else
    {
        return -1;
    }

    /* Save output Stats in HSRAM */
    if(outStats != NULL)
    {
        itemPayloadLen = sizeof(MmwDemo_output_message_stats);
        memcpy((void *)&ptrHsramBuffer->outStats, (void *)outStats, itemPayloadLen);
    }

    /* Set payload pointer to HSM buffer */
    ptrCurrBuffer = &ptrHsramBuffer->payload[0];
    totalHsramSize = MMWDEMO_HSRAM_PAYLOAD_SIZE;

    /* Save ObjOut in HSRAM */
    if(result->objOut != NULL)
    {
        itemPayloadLen = sizeof(DPIF_PointCloudCartesian) * result->numObjOut;
        if((totalHsramSize- itemPayloadLen) > 0)
        {
            memcpy(ptrCurrBuffer, (void *)result->objOut, itemPayloadLen);

            ptrHsramBuffer->result.objOut = (DPIF_PointCloudCartesian *)ptrCurrBuffer;
            ptrCurrBuffer+= itemPayloadLen;
            totalHsramSize -=itemPayloadLen;
        }
        else
        {
            return -1;
        }
    }

    /* Save ObjOutSideInfo in HSRAM */
    if(result->objOutSideInfo != NULL)
    {
        itemPayloadLen = sizeof(DPIF_PointCloudSideInfo) * result->numObjOut;
        if((totalHsramSize- itemPayloadLen) > 0)
        {
            memcpy(ptrCurrBuffer, (void *)result->objOutSideInfo, itemPayloadLen);
            ptrHsramBuffer->result.objOutSideInfo = (DPIF_PointCloudSideInfo *)ptrCurrBuffer;
            ptrCurrBuffer+= itemPayloadLen;
            totalHsramSize -=itemPayloadLen;
        }
        else
        {
            return -1;
        }
    }

    /* Save DPC_ObjectDetection_Stats in HSRAM */
    if(result->stats != NULL)
    {
        itemPayloadLen = sizeof(DPC_ObjectDetection_Stats);
        if((totalHsramSize- itemPayloadLen) > 0)
        {
            memcpy(ptrCurrBuffer, (void *)result->stats, itemPayloadLen);
            ptrHsramBuffer->result.stats = (DPC_ObjectDetection_Stats *)ptrCurrBuffer;
            ptrCurrBuffer+= itemPayloadLen;
            totalHsramSize -=itemPayloadLen;
        }
        else
        {
            return -1;
        }
    }

    /* Save compRxChanBiasMeasurement in HSRAM */
    if(result->compRxChanBiasMeasurement != NULL)
    {
        itemPayloadLen = sizeof(DPU_AoAProc_compRxChannelBiasCfg);
        if((totalHsramSize- itemPayloadLen) > 0)
        {
            memcpy(ptrCurrBuffer, (void *)result->compRxChanBiasMeasurement, itemPayloadLen);
            ptrHsramBuffer->result.compRxChanBiasMeasurement = (DPU_AoAProc_compRxChannelBiasCfg *)ptrCurrBuffer;
            ptrCurrBuffer+= itemPayloadLen;
            totalHsramSize -=itemPayloadLen;
        }
        else
        {
            return -1;
        }
    }

    return totalHsramSize;
}

/**
 *  @b Description
 *  @n
 *      DPM Execution Task. DPM execute results are processed here:
 *      a) Update states based on timestamp from DPC.
 *      b) Copy results to shared memory to be shared with MSS.
 *      c) Send Results to MSS by calling DPM_sendResult()
 *
 *  @retval
 *      Not Applicable.
 */
static void MmwDemo_DPC_ObjectDetection_dpmTask(UArg arg0, UArg arg1)
{
    int32_t     retVal;
    DPC_ObjectDetection_ExecuteResult *result;
    volatile uint32_t              startTime;

    while (1)
    {
        /* Execute the DPM module: */
        retVal = DPM_execute (gMmwDssMCB.dataPathObj.objDetDpmHandle, &resultBuffer);
        if (retVal < 0) {
            System_printf ("Error: DPM execution failed [Error code %d]\n", retVal);
            MmwDemo_debugAssert (0);
        }
        else
        {
            if ((resultBuffer.size[0] == sizeof(DPC_ObjectDetection_ExecuteResult)))
            {
                result = (DPC_ObjectDetection_ExecuteResult *)resultBuffer.ptrBuffer[0];

                /* Get the time stamp before copy data to HSRAM */
                startTime = Cycleprofiler_getTimeStamp();

                /* Update processing stats and added it to buffer 1*/
                MmwDemo_updateObjectDetStats(result->stats,
                                                &gMmwDssMCB.dataPathObj.subFrameStats[result->subFrameIdx]);

                /* Copy result data to HSRAM */
                if ((retVal = MmwDemo_copyResultToHSRAM(&gHSRAM, result, &gMmwDssMCB.dataPathObj.subFrameStats[result->subFrameIdx])) >= 0)
                {
                    /* Update interframe margin with HSRAM copy time */
                    gHSRAM.outStats.interFrameProcessingMargin -= ((Cycleprofiler_getTimeStamp() - startTime)/DSP_CLOCK_MHZ);

                    /* Update DPM buffer */
                    resultBuffer.ptrBuffer[0] = (uint8_t *)&gHSRAM.result;
                    resultBuffer.ptrBuffer[1] = (uint8_t *)&gHSRAM.outStats;
                    resultBuffer.size[1] = sizeof(MmwDemo_output_message_stats);

                    /* YES: Results are available send them. */
                    retVal = DPM_sendResult (gMmwDssMCB.dataPathObj.objDetDpmHandle, true, &resultBuffer);
                    if (retVal < 0)
                    {
                        System_printf ("Error: Failed to send results [Error: %d] to remote\n", retVal);
                    }
                }
                else
                {
                    System_printf ("Error: Failed to copy processing results to HSRAM, error=%d\n", retVal);
                    MmwDemo_debugAssert (0);
                }
            }
        }
    }
}

/**
 *  @b Description
 *  @n
 *      System Initialization Task which initializes the various
 *      components in the system.
 *
 *  @retval
 *      Not Applicable.
 */
static void MmwDemo_dssInitTask(UArg arg0, UArg arg1)
{
    int32_t             errCode;
    Task_Params         taskParams;
    DPM_InitCfg         dpmInitCfg;
    DPC_ObjectDetection_InitParams      objDetInitParams;

    /*****************************************************************************
     * Driver Init:
     *****************************************************************************/

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

    /*****************************************************************************
     * Driver Open/Configuraiton:
     *****************************************************************************/
    /* Initialize EDMA */
    MmwDemo_edmaInit(&gMmwDssMCB.dataPathObj, DPC_OBJDET_DSP_EDMA_INSTANCE);

    /* Use instance 1 on DSS */
    MmwDemo_edmaOpen(&gMmwDssMCB.dataPathObj, DPC_OBJDET_DSP_EDMA_INSTANCE);

    /*****************************************************************************
     * Initialization of the DPM Module:
     *****************************************************************************/
    memset ((void *)&objDetInitParams, 0, sizeof(DPC_ObjectDetection_InitParams));

    objDetInitParams.L3ramCfg.addr = (void *)&gMmwL3[0];
    objDetInitParams.L3ramCfg.size = sizeof(gMmwL3);
    objDetInitParams.CoreL2RamCfg.addr = &gDPC_ObjDetL2Heap[0];
    objDetInitParams.CoreL2RamCfg.size = sizeof(gDPC_ObjDetL2Heap);
    objDetInitParams.CoreL1RamCfg.addr = &gDPC_ObjDetL1Heap[0];
    objDetInitParams.CoreL1RamCfg.size = sizeof(gDPC_ObjDetL1Heap);

    /* initialize edma handles, unused handles will remain NULL to to memset above */
    objDetInitParams.edmaHandle[DPC_OBJDET_DSP_EDMA_INSTANCE] = gMmwDssMCB.dataPathObj.edmaHandle;

    /* DPC Call-back config */
    objDetInitParams.processCallBackCfg.processFrameBeginCallBackFxn =
        MmwDemo_DPC_ObjectDetection_processFrameBeginCallBackFxn;
    objDetInitParams.processCallBackCfg.processInterFrameBeginCallBackFxn =
        MmwDemo_DPC_ObjectDetection_processInterFrameBeginCallBackFxn;

    memset ((void *)&dpmInitCfg, 0, sizeof(DPM_InitCfg));

    /* Setup the configuration: */
    dpmInitCfg.socHandle        = gMmwDssMCB.socHandle;
    dpmInitCfg.ptrProcChainCfg  = &gDPC_ObjectDetectionCfg;
    dpmInitCfg.instanceId       = DPC_OBJDET_DSP_INSTANCEID;
    dpmInitCfg.domain           = DPM_Domain_REMOTE;
    dpmInitCfg.reportFxn        = MmwDemo_DPC_ObjectDetection_reportFxn;
    dpmInitCfg.arg              = &objDetInitParams;
    dpmInitCfg.argSize          = sizeof(DPC_ObjectDetection_InitParams);

    /* Initialize the DPM Module: */
    gMmwDssMCB.dataPathObj.objDetDpmHandle = DPM_init (&dpmInitCfg, &errCode);
    if (gMmwDssMCB.dataPathObj.objDetDpmHandle == NULL)
    {
        System_printf ("Error: Unable to initialize the DPM Module [Error: %d]\n", errCode);
        MmwDemo_debugAssert (0);
        return;
    }

    /* Synchronization: This will synchronize the execution of the control module
     * between the domains. This is a prerequiste and always needs to be invoked. */
    while (1)
    {
        int32_t syncStatus;

        /* Get the synchronization status: */
        syncStatus = DPM_synch (gMmwDssMCB.dataPathObj.objDetDpmHandle, &errCode);
        if (syncStatus < 0)
        {
            /* Error: Unable to synchronize the framework */
            System_printf ("Error: DPM Synchronization failed [Error code %d]\n", errCode);
            MmwDemo_debugAssert (0);
            return;
        }
        if (syncStatus == 1)
        {
            /* Synchronization acheived: */
            break;
        }
        /* Sleep and poll again: */
        Task_sleep(1);
    }
    System_printf ("Debug: DPM Module Sync is done\n");

    /* Launch the DPM Task */
    Task_Params_init(&taskParams);
    taskParams.priority = MMWDEMO_DPC_OBJDET_DPM_TASK_PRIORITY;
    taskParams.stackSize = 4*1024;
    gMmwDssMCB.objDetDpmTaskHandle = Task_create(MmwDemo_DPC_ObjectDetection_dpmTask, &taskParams, NULL);

    return;
}

/**
 *  @b Description
 *  @n
 *     Function to sleep the DSP using IDLE instruction.
 *     When DSP has no work left to do,
 *     the BIOS will be in Idle thread and will call this function.
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_sleep(void)
{
    /* issue WFI (Wait For Interrupt) instruction */
    asm(" IDLE ");
}

/**
 *  @b Description
 *  @n
 *      Entry point into the Millimeter Wave Demo
 *
 *  @retval
 *      Not Applicable.
 */
int main (void)
{
    Task_Params     taskParams;
    int32_t         errCode;
    SOC_Handle      socHandle;
    SOC_Cfg         socCfg;

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

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

    /* Populate the SOC configuration: */
    socCfg.clockCfg = SOC_SysClock_BYPASS_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);
        MmwDemo_debugAssert (0);
        return -1;
    }

    gMmwDssMCB.socHandle = socHandle;

    /* Initialize the Task Parameters. */
    Task_Params_init(&taskParams);
    taskParams.stackSize = 4*1024;
    gMmwDssMCB.initTaskHandle = Task_create(MmwDemo_dssInitTask, &taskParams, NULL);

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

 /* 
 *  NOTE:
 *      (C) Copyright 2018 Texas Instruments, Inc.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "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 THE COPYRIGHT
 *  OWNER OR CONTRIBUTORS 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.
 */
/**
 *   @file  rangeprocdsp.c
 *
 *   @brief
 *      Implements Data path range FFT processing functionality on DSP.
 */

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

/* Standard Include Files. */
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <math.h>

/* mmwave SDK include files */
#include <ti/drivers/edma/edma.h>
#include <ti/drivers/soc/soc.h>
#include <ti/drivers/osal/MemoryP.h>
#include <ti/utils/cycleprofiler/cycle_profiler.h>

#include <ti/alg/mmwavelib/mmwavelib.h>

/* C64P dsplib (fixed point part for C674X) */
#include "DSP_fft16x16_imre.h"
#include "gen_twiddle_fft16x16_imre.h"

/* Internal include Files */
#include <ti/datapath/dpu/rangeproc/include/rangeprocdsp_internal.h>
#include <ti/datapath/dpu/rangeproc/rangeprocdsp.h>

/* MATH utils library Include files */
#include <ti/utils/mathutils/mathutils.h>
#include <xdc/runtime/System.h>
#include <ti/drivers/uart/UART.h>
#include <ti/utils/cli/cli.h>   

#define  DEBUG_CHECK_PARAMS 1

/* Macros to determine pingpong index */
#define pingPongId(x) ((x) & 0x1U)
#define isPong(x) (pingPongId(x) == 1U)
#define BYTES_PER_SAMP_1D   sizeof(cmplx16ImRe_t)

/* Define missing error codes */
#define DPU_RANGEPROCDSP_EDMA_FAIL              (DP_ERRNO_RANGE_PROC_BASE-9)
#define DPU_RANGEPROCDSP_EBUSY                  (DP_ERRNO_RANGE_PROC_BASE-10)

/* Error code string macros for better debugging */
#define ERROR_CODE_STRING(x) ((x) == DPU_RANGEPROCDSP_EINVAL ? "EINVAL" : \
                             ((x) == DPU_RANGEPROCDSP_ENOMEM ? "ENOMEM" : \
                             ((x) == DPU_RANGEPROCDSP_EDMA_FAIL ? "EDMA_FAIL" : \
                             ((x) == DPU_RANGEPROCDSP_EBUSY ? "EBUSY" : \
                             ((x) == DPU_RANGEPROCDSP_EINPROGRESS ? "EINPROGRESS" : \
                             ((x) == DPU_RANGEPROCDSP_ECMD ? "ECMD" : \
                             ((x) == DPU_RANGEPROCDSP_EBUFFER_POINTER ? "EBUFFER_POINTER" : \
                             ((x) == DPU_RANGEPROCDSP_EBUFFER_SIZE ? "EBUFFER_SIZE" : "UNKNOWN"))))))))

/******************************************************************
 *                      Internal Function prototype
 ******************************************************************/
static void rangeProcDSP_WaitEDMAComplete
(
    EDMA_Handle         edmaHandle,
    uint8_t             chId
);

static int32_t rangeProcDSP_ConfigDataInEDMA
(
    rangeProcDSPObj          *rangeProcObj,
    DPU_RangeProcDSP_HW_Resources  *hwRes
);

static int32_t rangeProcDSP_ConfigDataOutEDMA
(
    rangeProcDSPObj          *rangeProcObj,
    DPU_RangeProcDSP_HW_Resources  *hwRes
);

static int32_t rangeProcDSP_dcRangeSignatureCompensation_init
(
    rangeProcDSPObj     *rangeProcObj,
    DPU_RangeProc_CalibDcRangeSigCfg *calibDcRangeSigCfg,
    uint8_t             resetBuffer
);

static void rangeProcDSP_dcRangeSignatureCompensation
(
    rangeProcDSPObj         *rangeProcObj,
    DPU_RangeProc_CalibDcRangeSigCfg  *calibDcCfg,
    uint8_t                 chirpPingPongId,
    uint8_t                 txIdx
);

/**
 *  @b Description
 *  @n
 *      Polling function to wait for EDMA transper complete.
 *
 *  @param[in]  edmaHandle              EDMA Handle
 *  @param[in]  chId                    EDMA channel id
 *
 *  \ingroup    DPU_RANGEPROC_INTERNAL_FUNCTION
 *
 *  @retval     N/A
 * 
 */
static void rangeProcDSP_WaitEDMAComplete
(
    EDMA_Handle         edmaHandle,
    uint8_t             chId
)
{
    volatile bool isTransferDone = false;
    do {
        if (EDMA_isTransferComplete(edmaHandle,
                                    (uint8_t) chId,
                                    (bool *)&isTransferDone) != EDMA_NO_ERROR)
        {
        }
    } while (isTransferDone == false);
}

/**
 *  @b Description
 *  @n
 *      Helper function to configuration data in EDMA
 *
 *  @param[in]  rangeProcObj             Pointer to rangeProc object
 *  @param[in]  hwRes                    Pointer to hard resource configuration
 *
 *  \ingroup    DPU_RANGEPROC_INTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
static int32_t rangeProcDSP_ConfigDataInEDMA
(
    rangeProcDSPObj          *rangeProcObj,
    DPU_RangeProcDSP_HW_Resources  *hwRes
)
{
    int32_t retVal;
    rangeProc_dpParams   *dpParams;
    DPEDMA_syncACfg         syncACfg;

    dpParams = &rangeProcObj->DPParams;

    /* Copy data from ADCbuffer to internal adcbufIn scratch buffer 
      Assumes the input data is in non-interleaved mode
     */
    syncACfg.aCount = dpParams->numAdcSamples * BYTES_PER_SAMP_1D;
    syncACfg.bCount =MAX(dpParams->numRxAntennas / 2U, 1U) * dpParams->numChirpsPerChirpEvent;
    syncACfg.srcBIdx = rangeProcObj->rxChanOffset * 2U ; 
    syncACfg.dstBIdx = 0U;

    /* PING configuration */
    syncACfg.srcAddress = (uint32_t)rangeProcObj->ADCdataBuf;
    syncACfg.destAddress = (uint32_t)rangeProcObj->adcDataIn;
    
    retVal = DPEDMA_configSyncA_singleFrame(hwRes->edmaCfg.edmaHandle,
                         &hwRes->edmaCfg.dataInPing,
                         NULL,  /* no Chaining */
                         &syncACfg,
                         false,
                         true,
                         true,
                         NULL,
                         NULL);
    if (retVal < 0)
    {
        goto exit;
    }

    /* PONG src/dest address */
    syncACfg.srcAddress = (uint32_t)rangeProcObj->ADCdataBuf + rangeProcObj->rxChanOffset;
    syncACfg.destAddress = (uint32_t)&rangeProcObj->adcDataIn[dpParams->numRangeBins];

    retVal = DPEDMA_configSyncA_singleFrame(hwRes->edmaCfg.edmaHandle,
                         &hwRes->edmaCfg.dataInPong,
                         NULL,  /* no Chaining */
                         &syncACfg,
                         false,
                         true,
                         true,
                         NULL,
                         NULL);
    if (retVal < 0)
    {
        goto exit;
    }
exit:
    return(retVal);
}

/**
 *  @b Description
 *  @n
 *      Helper function to configuration data out EDMA
 *
 *  @param[in]  rangeProcObj             Pointer to rangeProc object
 *  @param[in]  hwRes                    Pointer to hard resource configuration
 *
 *  \ingroup    DPU_RANGEPROC_INTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
static int32_t rangeProcDSP_ConfigDataOutEDMA
(
    rangeProcDSPObj          *rangeProcObj,
    DPU_RangeProcDSP_HW_Resources  *hwRes
)
{
    int32_t     retVal;
    uint16_t    samplesPerChirp, aCount;
    uint16_t     oneD_destinationCindex;
    uint8_t     *oneD_destinationPongAddress;
    DPEDMA_syncABCfg         syncABCfg;
    rangeProc_dpParams      *dpParams;
    DPU_RangeProcDSP_EDMAConfig *edmaCfg;

    edmaCfg = &hwRes->edmaCfg;
    dpParams = &rangeProcObj->DPParams;

    /*****************************************************
     * EDMA configuration for storing 1d fft output to L3.
     * It copies all Rx antennas of the chirp per trigger event.
     *****************************************************/
    samplesPerChirp = dpParams->numRangeBins * dpParams->numRxAntennas;
    aCount = samplesPerChirp * sizeof(cmplx16ImRe_t);

    if ((dpParams->numTxAntennas == 2U) || (dpParams->numTxAntennas == 3U))
    {
        oneD_destinationCindex = (int16_t)aCount;

        /* Calculation address for 2 TxAnt. 3 txAnt , the address is calculated at runtime */
        oneD_destinationPongAddress = (uint8_t *)(&rangeProcObj->radarCubebuf[samplesPerChirp * dpParams->numDopplerChirps]);
    }
    else if (dpParams->numTxAntennas == 1U)
    {
        oneD_destinationCindex = (int16_t)(aCount * 2U);
        oneD_destinationPongAddress = (uint8_t *)(&rangeProcObj->radarCubebuf[samplesPerChirp]);
    }
    else
    {
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    /* Ping/Pong common configuration */
    syncABCfg.aCount = dpParams->numRangeBins * sizeof(cmplx16ImRe_t);
    syncABCfg.bCount = dpParams->numRxAntennas;
    syncABCfg.cCount = dpParams->numChirpsPerFrame / 2U; /*bCount */
    syncABCfg.srcBIdx = dpParams->numRangeBins * sizeof(cmplx16ImRe_t);
    syncABCfg.srcCIdx = 0;
    syncABCfg.dstBIdx = dpParams->numRangeBins * sizeof(cmplx16ImRe_t);
    syncABCfg.dstCIdx = oneD_destinationCindex;

    syncABCfg.srcAddress = (uint32_t)rangeProcObj->fftOut1D;
    syncABCfg.destAddress= (uint32_t)rangeProcObj->radarCubebuf;

    /* Ping - Copies from ping FFT output (even chirp indices)  to L3 */
    retVal = DPEDMA_configSyncAB (edmaCfg->edmaHandle,
                                 &edmaCfg->dataOutPing,
                                 NULL,
                                 &syncABCfg,
                                 false,
                                 true,
                                 true,
                                 NULL,
                                 NULL
                                 );
    if (retVal < 0)
    {
        goto exit;
    }

    /* Pong - copies from pong FFT output (odd chirp indices)  to L3 */
    syncABCfg.srcAddress = (uint32_t)&rangeProcObj->fftOut1D[samplesPerChirp];
    syncABCfg.destAddress= (uint32_t)oneD_destinationPongAddress;

    retVal = DPEDMA_configSyncAB (edmaCfg->edmaHandle,
                                 &edmaCfg->dataOutPong,
                                 NULL,
                                 &syncABCfg,
                                 false,
                                 true,
                                 true,
                                 NULL,
                                 NULL
                                 );

exit:
    return(retVal);
}

/**
 *  @b Description
 *  @n
 *      Compensation of DC range antenna signature Init function
 *
 *  @param[in]  rangeProcObj                 Pointer to rangeProc object
 *  @param[in]  calibDcRangeSigCfg           Pointer DC range compensation configuration
 *  @param[in]  resetMeanBuffer              Flag to indicate if buffer need to be reset
 *
 *  \ingroup    DPU_RANGEPROC_INTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
static int32_t rangeProcDSP_dcRangeSignatureCompensation_init
(
    rangeProcDSPObj     *rangeProcObj,
    DPU_RangeProc_CalibDcRangeSigCfg *calibDcRangeSigCfg,
    uint8_t             resetMeanBuffer
)
{
    int32_t                 retVal = 0;
    uint32_t                meanbufSize;

    meanbufSize = DPU_RANGEPROC_SIGNATURE_COMP_MAX_BIN_SIZE * rangeProcObj->DPParams.numVirtualAntennas
                 * sizeof(cmplx32ImRe_t);

    /* Validate DC revomal configuraiton */
    if(calibDcRangeSigCfg->enabled)
    {
        if( (rangeProcObj->dcRangeSigMean == (cmplx32ImRe_t*)NULL) ||
            (rangeProcObj->dcRangeSigMeanBufSize < meanbufSize))
        {
            /* Check DC range average buffer pointer */
            retVal = DPU_RANGEPROCDSP_EINVAL;
            goto exit;
        }
        else
        {
            if(resetMeanBuffer == 1U)
            {
                /* Initialize memory */
                memset((void *)rangeProcObj->dcRangeSigMean, 0, meanbufSize);
                rangeProcObj->dcRangeSigCalibCntr = 0;
            }
            rangeProcObj->calibDcNumLog2AvgChirps = mathUtils_floorLog2(calibDcRangeSigCfg->numAvgChirps);
        }
    }
    else
    {
        /* Feature is disabled , nothing needs to done here */
    }

exit:
    return (retVal);
}

/**
 *  @b Description
 *  @n
 *    Compensation of DC range antenna signature
 *
 *
 *  @retval
 *      Not Applicable.
 */
/**
 *  @b Description
 *  @n
 *      Compensation of DC range antenna signature
 *
 *  @param[in]  rangeProcObj                 Pointer to rangeProc object
 *  @param[in]  calibDcCfg                   Pointer DC range compensation configuration
 *  @param[in]  chirpPingPongId              Ping/Pong id to calculate offset
 *  @param[in]  txIdx                        Tx index
 *
 *  \ingroup    DPU_RANGEPROC_INTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
static void rangeProcDSP_dcRangeSignatureCompensation
(
    rangeProcDSPObj         *rangeProcObj,
    DPU_RangeProc_CalibDcRangeSigCfg *calibDcCfg,
    uint8_t                 chirpPingPongId,
    uint8_t                 txIdx
)
{
    rangeProc_dpParams      *params;
    uint32_t rxAntIdx, binIdx;
    uint32_t ind;
    int32_t chirpPingPongOffs;
    int32_t chirpPingPongSize;

    params = &rangeProcObj->DPParams;
    chirpPingPongSize = params->numRxAntennas * (calibDcCfg->positiveBinIdx - calibDcCfg->negativeBinIdx + 1);

    chirpPingPongOffs = txIdx * chirpPingPongSize;

    /* Calibration */
    if (rangeProcObj->dcRangeSigCalibCntr < (calibDcCfg->numAvgChirps * params->numTxAntennas))
    {
        /* Accumulate */
        ind = 0;
        for (rxAntIdx = 0; rxAntIdx < params->numRxAntennas; rxAntIdx++)
        {
            uint32_t chirpInOffs = chirpPingPongId * (params->numRxAntennas * params->numRangeBins) +
                                  (params->numRangeBins * rxAntIdx);
            int64_t *meanPtr = (int64_t *) &rangeProcObj->dcRangeSigMean[chirpPingPongOffs];
            uint32_t *fftPtr = (uint32_t *) &rangeProcObj->fftOut1D[chirpInOffs];
            int64_t meanBin;
            uint32_t fftBin;
            int32_t Re, Im;
            for (binIdx = 0; binIdx <= calibDcCfg->positiveBinIdx; binIdx++)
            {
                meanBin = _amem8(&meanPtr[ind]);
                fftBin = _amem4(&fftPtr[binIdx]);
                Im = _loll(meanBin) + _ext(fftBin, 0, 16);
                Re = _hill(meanBin) + _ext(fftBin, 16, 16);
                _amem8(&meanPtr[ind]) = _itoll(Re, Im);
                ind++;
            }

            chirpInOffs = chirpPingPongId * (params->numRxAntennas * params->numRangeBins) +
                (params->numRangeBins * rxAntIdx) + params->numRangeBins + calibDcCfg->negativeBinIdx;
            fftPtr =  (uint32_t *) &rangeProcObj->fftOut1D[chirpInOffs];
            for (binIdx = 0; binIdx < -calibDcCfg->negativeBinIdx; binIdx++)
            {
                meanBin = _amem8(&meanPtr[ind]);
                fftBin = _amem4(&fftPtr[binIdx]);
                Im = _loll(meanBin) + _ext(fftBin, 0, 16);
                Re = _hill(meanBin) + _ext(fftBin, 16, 16);
                _amem8(&meanPtr[ind]) = _itoll(Re, Im);
                ind++;
            }
        }

        if (rangeProcObj->dcRangeSigCalibCntr == (calibDcCfg->numAvgChirps * params->numTxAntennas - 1))
        {
            /* Divide */
            int64_t *meanPtr = (int64_t *) rangeProcObj->dcRangeSigMean;
            int32_t Re, Im;
            int64_t meanBin;
            int32_t divShift = mathUtils_floorLog2(calibDcCfg->numAvgChirps);
            for (ind  = 0; ind < (params->numTxAntennas * chirpPingPongSize); ind++)
            {
                meanBin = _amem8(&meanPtr[ind]);
                Im = _sshvr(_loll(meanBin), divShift);
                Re = _sshvr(_hill(meanBin), divShift);
                _amem8(&meanPtr[ind]) = _itoll(Re, Im);
            }
        }
    }
    else
    {
       /* fftOut1D -= dcRangeSigMean */
        ind = 0;
        for (rxAntIdx  = 0; rxAntIdx < params->numRxAntennas; rxAntIdx++)
        {
            uint32_t chirpInOffs = chirpPingPongId * (params->numRxAntennas * params->numRangeBins) +
                                   (params->numRangeBins * rxAntIdx);
            int64_t *meanPtr = (int64_t *) &rangeProcObj->dcRangeSigMean[chirpPingPongOffs];
            uint32_t *fftPtr =  (uint32_t *) &rangeProcObj->fftOut1D[chirpInOffs];
            int64_t meanBin;
            uint32_t fftBin;
            int32_t Re, Im;
            for (binIdx = 0; binIdx <= calibDcCfg->positiveBinIdx; binIdx++)
            {
                meanBin = _amem8(&meanPtr[ind]);
                fftBin = _amem4(&fftPtr[binIdx]);
                Im = _ext(fftBin, 0, 16) - _loll(meanBin);
                Re = _ext(fftBin, 16, 16) - _hill(meanBin);
                _amem4(&fftPtr[binIdx]) = _pack2(Im, Re);
                ind++;
            }

            chirpInOffs = chirpPingPongId * (params->numRxAntennas * params->numRangeBins) +
                (params->numRangeBins * rxAntIdx) + params->numRangeBins + calibDcCfg->negativeBinIdx;
            fftPtr =  (uint32_t *) &rangeProcObj->fftOut1D[chirpInOffs];
            for (binIdx = 0; binIdx < -calibDcCfg->negativeBinIdx; binIdx++)
            {
                meanBin = _amem8(&meanPtr[ind]);
                fftBin = _amem4(&fftPtr[binIdx]);
                Im = _ext(fftBin, 0, 16) - _loll(meanBin);
                Re = _ext(fftBin, 16, 16) - _hill(meanBin);
                _amem4(&fftPtr[binIdx]) = _pack2(Im, Re);
                ind++;
            }
        }
    }
}

/**
 *  @b Description
 *  @n
 *      Internal function to parse rangeProc configuration and save in internal rangeProc object
 *
 *  @param[in]  rangeProcObj              Pointer to rangeProc object
 *  @param[in]  pConfigIn                 Pointer to rangeProcDSP configuration structure
 *
 *  \ingroup    DPU_RANGEPROC_INTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
static int32_t rangeProcDSP_ParseConfig
(
    rangeProcDSPObj          *rangeProcObj,
    DPU_RangeProcDSP_Config  *pConfigIn
)
{
    int32_t                 retVal = 0;
    rangeProc_dpParams  *params;
    DPU_RangeProcDSP_StaticConfig      *pStaticCfg;
    DPU_RangeProcDSP_HW_Resources      *pHwRes;

    /* Get configuration pointers */
    pStaticCfg = &pConfigIn->staticCfg;
    pHwRes = &pConfigIn->hwRes;
    params    = &rangeProcObj->DPParams;

    /* Save datapath parameters */
    params->numTxAntennas = pStaticCfg->numTxAntennas;
    params->numRxAntennas = pStaticCfg->ADCBufData.dataProperty.numRxAntennas;
    params->numVirtualAntennas = pStaticCfg->numVirtualAntennas;
    params->numChirpsPerChirpEvent = pStaticCfg->ADCBufData.dataProperty.numChirpsPerChirpEvent;
    params->numAdcSamples = pStaticCfg->ADCBufData.dataProperty.numAdcSamples;
    params->numRangeBins = pStaticCfg->numRangeBins;
    params->numChirpsPerFrame = pStaticCfg->numChirpsPerFrame;
    params->numDopplerChirps = pStaticCfg->numChirpsPerFrame/pStaticCfg->numTxAntennas;

    /* Debug parameters */
    System_printf("RangeProc Params: numRxAntennas=%d, numAdcSamples=%d, numChirpsPerChirpEvent=%d, numDopplerChirps=%d\n",
                 params->numRxAntennas, params->numAdcSamples, 
                 params->numChirpsPerChirpEvent, params->numDopplerChirps);
      

    /* Validate parameters */
    if (params->numRxAntennas == 0 || params->numAdcSamples == 0 || 
        params->numChirpsPerFrame == 0 || params->numTxAntennas == 0)
    {
        System_printf("ERROR: Invalid RangeProc parameters (zero values detected)\n");
          
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    /* Basic validation for range bin calculation */
    if (params->numRangeBins != params->numAdcSamples)
    {
        System_printf("WARNING: numRangeBins(%d) != numAdcSamples(%d)\n",
                    params->numRangeBins, params->numAdcSamples);
          
    }

    /* Basic buffer validation */
    if (pHwRes->fftOut1D == NULL || pHwRes->adcDataIn == NULL || 
        pHwRes->twiddle16x16 == NULL || pHwRes->window == NULL)
    {
        System_printf("ERROR: NULL buffer pointers detected\n");
          
        retVal = DPU_RANGEPROCDSP_EBUFFER_POINTER;
        goto exit;
    }

    /* Save EDMA Handle */
    rangeProcObj->edmaHandle = pHwRes->edmaCfg.edmaHandle;

    /* Save interface buffers */
    rangeProcObj->ADCdataBuf        = (cmplx16ImRe_t *)pStaticCfg->ADCBufData.data;
    rangeProcObj->radarCubebuf      = (cmplx16ImRe_t *)pHwRes->radarCube.data;

    /* Debug interface buffers */
    System_printf("RangeProc Interface Buffers: ADCdataBuf=%p, radarCubebuf=%p\n",
                 rangeProcObj->ADCdataBuf, rangeProcObj->radarCubebuf);
      

    /* Save Scratch buffers */
    rangeProcObj->fftOut1D          = pHwRes->fftOut1D;
    rangeProcObj->adcDataIn         = pHwRes->adcDataIn;
    rangeProcObj->dcRangeSigMean    = pHwRes->dcRangeSigMean;
    rangeProcObj->dcRangeSigMeanBufSize    = pHwRes->dcRangeSigMeanSize;

    /* Scratch windowing & twiddle buffers */
    rangeProcObj->twiddle16x16      = pHwRes->twiddle16x16;
    rangeProcObj->window            = pHwRes->window;

    if(params->numRxAntennas > 1)
    {
        /* For rangeProc DPU needs rx channel has same offset from one channel to the next channel
           Use first two channel offset to calculate the BIdx for EDMA
         */
        rangeProcObj->rxChanOffset = pStaticCfg->ADCBufData.dataProperty.rxChanOffset[1] -
                                     pStaticCfg->ADCBufData.dataProperty.rxChanOffset[0];

        /* Debug RX channel offset */
        System_printf("RangeProc rxChanOffset=%d, required=%d\n",
                     rangeProcObj->rxChanOffset, 
                     rangeProcObj->DPParams.numAdcSamples * sizeof(cmplx16ImRe_t));
          

        /* rxChanOffset should be 16 bytes aligned and should be big enough to hold numAdcSamples */
        if (rangeProcObj->rxChanOffset < rangeProcObj->DPParams.numAdcSamples * sizeof(cmplx16ImRe_t) ||
          (rangeProcObj->rxChanOffset & 0xF != 0))
        {
            System_printf("ERROR: Invalid rxChanOffset (too small or not 16-byte aligned): %d\n", 
                         rangeProcObj->rxChanOffset);
              
            retVal = DPU_RANGEPROCDSP_EINVAL;
            goto exit;
        }
    }

    /* Prepare Ping/Pong EDMA data in/out channels */
    rangeProcObj->dataInChan[0] = pHwRes->edmaCfg.dataInPing.channel;
    rangeProcObj->dataInChan[1] = pHwRes->edmaCfg.dataInPong.channel;
    rangeProcObj->dataOutChan[0] = pHwRes->edmaCfg.dataOutPing.channel;
    rangeProcObj->dataOutChan[1] = pHwRes->edmaCfg.dataOutPong.channel;

    /* Calculation used at runtime */
    rangeProcObj->numSamplePerChirp = params->numRangeBins * params->numRxAntennas;
    rangeProcObj->numSamplePerTx = params->numDopplerChirps * rangeProcObj->numSamplePerChirp;

    System_printf("RangeProc Runtime Calculations: numSamplePerChirp=%d, numSamplePerTx=%d\n",
                 rangeProcObj->numSamplePerChirp, rangeProcObj->numSamplePerTx);
      

exit:
    if (retVal != 0)
    {
        System_printf("ERROR: rangeProcDSP_ParseConfig failed with error code %d (%s)\n", 
                     retVal, ERROR_CODE_STRING(retVal));
          
    }
    return(retVal);
}



/**
 *  @b Description
 *  @n
 *      The function is rangeProc DPU init function. It allocates memory to store
 *  its internal data object and returns a handle if it executes successfully.
 *
 *  @param[in]  errCode                 Pointer to errCode generates from the API
 *
 *  \ingroup    DPU_RANGEPROC_EXTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - valid rangeProc handle
 *  @retval
 *      Error       - NULL
 */
DPU_RangeProcDSP_Handle DPU_RangeProcDSP_init
(
    int32_t*    errCode
)
{
    rangeProcDSPObj *rangeProcObj;

    rangeProcObj = MemoryP_ctrlAlloc(sizeof(rangeProcDSPObj), 0);
    if(rangeProcObj == NULL)
    {
        *errCode = DPU_RANGEPROCDSP_ENOMEM;
        System_printf("ERROR: DPU_RangeProcDSP_init failed to allocate memory\n");
          
        return NULL;
    }

    /* Initialize memory */
    memset((void *)rangeProcObj, 0, sizeof(rangeProcDSPObj));
    System_printf("DPU_RangeProcDSP_init successful\n");
      

    return ((DPU_RangeProcDSP_Handle)rangeProcObj);
}

/**
 *  @b Description
 *  @n
 *      The function is rangeProc DPU config function. It saves buffer pointer and configurations 
 *  including system resources and configures EDMA for runtime range processing.
 *  
 *  @pre    DPU_RangeProcDSP_init() has been called
 *
 *  @param[in]  handle                  rangeProc DPU handle
 *  @param[in]  pConfig                 Pointer to rangeProc configuration data structure
 *
 *  \ingroup    DPU_RANGEPROC_EXTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
int32_t DPU_RangeProcDSP_config
(
    DPU_RangeProcDSP_Handle     handle,
    DPU_RangeProcDSP_Config*    pConfig
)
{
    rangeProcDSPObj          *rangeProcObj;
    DPU_RangeProcDSP_StaticConfig *pStaticCfg;
    DPU_RangeProcDSP_HW_Resources      *pHwRes;
    int32_t                  retVal = 0;
    /* Move variable declarations to the beginning of the function */
    uint32_t requiredFftOutSize;
    uint32_t requiredTwiddleSize;
    uint32_t requiredWindowSize;

    System_printf("DPU_RangeProcDSP_config start\n");
    
    /* Debug radar cube info */
    if (pConfig != NULL && pConfig->hwRes.radarCube.data != NULL) {
        System_printf("Radar Cube: data=%p, dataSize=%d, datafmt=%d\n", 
                     pConfig->hwRes.radarCube.data, 
                     pConfig->hwRes.radarCube.dataSize, 
                     pConfig->hwRes.radarCube.datafmt);
        System_printf("Static cfg: numTxAntennas=%d, numVirtualAntennas=%d\n",
                     pConfig->staticCfg.numTxAntennas,
                     pConfig->staticCfg.numVirtualAntennas);
    } else {
        System_printf("WARNING: NULL radar cube data detected in config\n");
    }
    
      

    rangeProcObj = (rangeProcDSPObj *)handle;
    if(rangeProcObj == NULL)
    {
        System_printf("ERROR: DPU_RangeProcDSP_config - NULL handle\n");
          
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    pStaticCfg = &pConfig->staticCfg;
    pHwRes = &pConfig->hwRes;

    /* Debug configuration parameters */
    System_printf("RangeProc Config: numTxAntennas=%d, numVirtualAntennas=%d, numRangeBins=%d, numChirpsPerFrame=%d\n",
                 pStaticCfg->numTxAntennas, pStaticCfg->numVirtualAntennas, 
                 pStaticCfg->numRangeBins, pStaticCfg->numChirpsPerFrame);
      

    /* Debug buffer sizes */
    System_printf("RangeProc Buffers: dcRangeSigMeanSize=%d, twiddleSize=%d, windowSize=%d, adcDataInSize=%d, fftOut1DSize=%d\n",
                 pHwRes->dcRangeSigMeanSize, pHwRes->twiddleSize, 
                 pHwRes->windowSize, pHwRes->adcDataInSize, pHwRes->fftOut1DSize);
      
    
    /* Debug ADCBuf properties */
    System_printf("RangeProc ADCBuf: numRxAntennas=%d, numAdcSamples=%d, numChirpsPerChirpEvent=%d\n",
                 pStaticCfg->ADCBufData.dataProperty.numRxAntennas,
                 pStaticCfg->ADCBufData.dataProperty.numAdcSamples,
                 pStaticCfg->ADCBufData.dataProperty.numChirpsPerChirpEvent);
      

    /* Validate params */
    if(!pConfig->hwRes.edmaCfg.edmaHandle ||
       !pHwRes->radarCube.data ||
       !pStaticCfg->ADCBufData.data||
       !pHwRes->adcDataIn ||
       !pHwRes->fftOut1D ||
       !pHwRes->window ||
       !pHwRes->twiddle16x16 ||
       !pHwRes->dcRangeSigMean
       )
    {
        System_printf("ERROR: NULL buffer pointers detected in config\n");
        if (!pConfig->hwRes.edmaCfg.edmaHandle) System_printf("  - edmaCfg.edmaHandle is NULL\n");
        if (!pHwRes->radarCube.data) System_printf("  - radarCube.data is NULL\n");
        if (!pStaticCfg->ADCBufData.data) System_printf("  - ADCBufData.data is NULL\n");
        if (!pHwRes->adcDataIn) System_printf("  - adcDataIn is NULL\n");
        if (!pHwRes->fftOut1D) System_printf("  - fftOut1D is NULL\n");
        if (!pHwRes->window) System_printf("  - window is NULL\n");
        if (!pHwRes->twiddle16x16) System_printf("  - twiddle16x16 is NULL\n");
        if (!pHwRes->dcRangeSigMean) System_printf("  - dcRangeSigMean is NULL\n");
          
        retVal = DPU_RANGEPROCDSP_EBUFFER_POINTER;
        goto exit;
    }

    /* Calculate and validate all buffer sizes */    
    
    requiredFftOutSize = sizeof(cmplx16ImRe_t) * pStaticCfg->numRangeBins * 
                                pStaticCfg->ADCBufData.dataProperty.numRxAntennas * 2;
    
    requiredTwiddleSize = sizeof(cmplx16ImRe_t) * pStaticCfg->numRangeBins;
    
    requiredWindowSize = pStaticCfg->ADCBufData.dataProperty.numAdcSamples * sizeof(int16_t) / 2;
    

    System_printf("RangeProc Provided: radarCubeSize=%d, adcDataInSize=%d, fftOutSize=%d, twiddleSize=%d, windowSize=%d\n",
                 pHwRes->radarCube.dataSize, pHwRes->adcDataInSize, pHwRes->fftOut1DSize, 
                 pHwRes->twiddleSize, pHwRes->windowSize);
      

    
    if (pHwRes->fftOut1DSize < requiredFftOutSize)
    {
        System_printf("ERROR: FFT output buffer too small! Required: %d, Provided: %d\n", 
                     requiredFftOutSize, pHwRes->fftOut1DSize);
          
        retVal = DPU_RANGEPROCDSP_EBUFFER_SIZE;
        goto exit;
    }
    
    if (pHwRes->twiddleSize < requiredTwiddleSize)
    {
        System_printf("ERROR: Twiddle buffer too small! Required: %d, Provided: %d\n", 
                     requiredTwiddleSize, pHwRes->twiddleSize);
          
        retVal = DPU_RANGEPROCDSP_EBUFFER_SIZE;
        goto exit;
    }
    
    if (pHwRes->windowSize != requiredWindowSize)
    {
        System_printf("ERROR: Window buffer size mismatch! Required: %d, Provided: %d\n", 
                     requiredWindowSize, pHwRes->windowSize);
          
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    if(rangeProcObj->inProgress == true)
    {
        System_printf("ERROR: Range processing is already in progress\n");
          
        retVal = DPU_RANGEPROCDSP_EINPROGRESS;
        goto exit;
    }

    /* Parameter check: validate Adc data interface configuration
        Support:
            - Complex 16bit ADC data in IMRE format
            - Non-interleaved mode
     */
    if( (pStaticCfg->ADCBufData.dataProperty.dataFmt != DPIF_DATAFORMAT_COMPLEX16_IMRE) ||
       (pStaticCfg->ADCBufData.dataProperty.interleave != DPIF_RXCHAN_NON_INTERLEAVE_MODE) )
    {
        System_printf("ERROR: Unsupported ADC data format or interleave mode\n");
        System_printf("  - dataFmt=%d (expected %d)\n", 
                     pStaticCfg->ADCBufData.dataProperty.dataFmt, DPIF_DATAFORMAT_COMPLEX16_IMRE);
        System_printf("  - interleave=%d (expected %d)\n", 
                     pStaticCfg->ADCBufData.dataProperty.interleave, DPIF_RXCHAN_NON_INTERLEAVE_MODE);
          
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    /* Validate dp radarCube interface */
    if (pConfig->hwRes.radarCube.datafmt != DPIF_RADARCUBE_FORMAT_1)
    {
        /* Only one format is supported */
        System_printf("ERROR: Unsupported radar cube format, only DPIF_RADARCUBE_FORMAT_1 is supported\n");
        System_printf("  - Current format: %d\n", pConfig->hwRes.radarCube.datafmt);
          
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    /* Save hardware resources */
    memcpy((void *)&rangeProcObj->calibDcRangeSigCfg, (void *)pConfig->dynCfg.calibDcRangeSigCfg, sizeof(DPU_RangeProc_CalibDcRangeSigCfg));

    /* Continue with regular config function */
    System_printf("Calling rangeProcDSP_ParseConfig\n");
      
    retVal = rangeProcDSP_ParseConfig(rangeProcObj, pConfig);
    if(retVal < 0)
    {
        System_printf("ERROR: rangeProcDSP_ParseConfig failed with error %d (%s)\n", retVal, ERROR_CODE_STRING(retVal));
          
        goto exit;
    }

    /* Generate twiddle factors for 1D FFT. This is one time */
    System_printf("Generating twiddle factors for 1D FFT\n");
      
    mmwavelib_gen_twiddle_fft16x16_imre_sa((short *)rangeProcObj->twiddle16x16, pStaticCfg->numRangeBins);

    /* Configure EDMA */
    System_printf("Configuring EDMA for data in\n");
      
    retVal = rangeProcDSP_ConfigDataInEDMA(rangeProcObj, &pConfig->hwRes);
    if(retVal < 0)
    {
        System_printf("ERROR: rangeProcDSP_ConfigDataInEDMA failed with error %d (%s)\n", retVal, ERROR_CODE_STRING(retVal));
          
        goto exit;
    }
    
    System_printf("Configuring EDMA for data out\n");
      
    retVal = rangeProcDSP_ConfigDataOutEDMA(rangeProcObj, &pConfig->hwRes);
    if(retVal < 0)
    {
        System_printf("ERROR: rangeProcDSP_ConfigDataOutEDMA failed with error %d (%s)\n", retVal, ERROR_CODE_STRING(retVal));
          
        goto exit;
    }

    /* DC calibration and compensation init */
    System_printf("Initializing DC range signature compensation\n");
      
    retVal = rangeProcDSP_dcRangeSignatureCompensation_init(rangeProcObj,
                                                             pConfig->dynCfg.calibDcRangeSigCfg,
                                                             pStaticCfg->resetDcRangeSigMeanBuffer);
    if (retVal < 0)
    {
        System_printf("ERROR: rangeProcDSP_dcRangeSignatureCompensation_init failed with error %d (%s)\n", 
                     retVal, ERROR_CODE_STRING(retVal));
          
        goto exit;
    }

    rangeProcObj->chirpCount = 0;
    rangeProcObj->inProgress = false;
    
    System_printf("DPU_RangeProcDSP_config completed successfully\n");
      
exit:
    if (retVal != 0)
    {
        System_printf("ERROR: DPU_RangeProcDSP_config failed with error code %d (%s)\n", 
                     retVal, ERROR_CODE_STRING(retVal));
          
    }
    return retVal;
}

/**
 *  @b Description
 *  @n
 *      The function is rangeProc DPU process function. It executes FFT operation. 
 *  It can be called multiple times in a frame until all chirps are handled in the frame.
 *
 *  @pre    DPU_RangeProcDSP_init() has been called
 *
 *  @param[in]  handle                  rangeProc DPU handle
 *  @param[in]  outParams               DPU output parameters
 *
 *  \ingroup    DPU_RANGEPROC_EXTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
int32_t DPU_RangeProcDSP_process
(
    DPU_RangeProcDSP_Handle     handle,
    DPU_RangeProcDSP_OutParams     *outParams
)
{
    rangeProc_dpParams  *DPParams;
    rangeProcDSPObj     *rangeProcObj;
    EDMA_Handle         edmaHandle;
    uint32_t            rxChanId;
    volatile uint32_t   startTime;
    volatile uint32_t   startTime1;
    uint32_t            waitingTime;
    uint32_t            outChannel;
    uint16_t            chirpIndex;
    uint8_t             chirpPingPongId;
    int32_t             retVal = 0;
    uint32_t            totalChirpIndex;

    rangeProcObj = (rangeProcDSPObj *)handle;
    if(rangeProcObj == NULL)
    {
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    if(rangeProcObj->inProgress == true)
    {
        retVal = DPU_RANGEPROCDSP_EINPROGRESS;
        goto exit;
    }
    else
    {
        rangeProcObj->inProgress = true;
    }

    DPParams = &rangeProcObj->DPParams;
    edmaHandle = rangeProcObj->edmaHandle;

    startTime = Cycleprofiler_getTimeStamp();
    waitingTime = 0;

    outParams->endOfChirp = false;

    /* 
     Process chirp data per loop, loops for numChirpsPerChirpEvent
     */
    for (chirpIndex = 0; chirpIndex < DPParams->numChirpsPerChirpEvent; chirpIndex++)
    {
        uint32_t    dataInAddr[2];
        uint32_t    numAdcSampleAligned;
        cmplx16ImRe_t *adcBufPongOffset;

        /*********************************
         * Prepare for the FFT
         *********************************/
        numAdcSampleAligned = (DPParams->numAdcSamples + 3U)/4U * 4U;
        adcBufPongOffset = (cmplx16ImRe_t *)((uint32_t)rangeProcObj->ADCdataBuf + rangeProcObj->rxChanOffset);

        dataInAddr[0] = (uint32_t)&rangeProcObj->ADCdataBuf[chirpIndex * numAdcSampleAligned];
        dataInAddr[1] = (uint32_t)&adcBufPongOffset[chirpIndex * numAdcSampleAligned];

        /* Set Ping source Address */
        retVal = EDMA_setSourceAddress(edmaHandle,  rangeProcObj->dataInChan[0],
            (uint32_t) SOC_translateAddress(dataInAddr[0], SOC_TranslateAddr_Dir_TO_EDMA, NULL));
        if (retVal != 0)
        {
            goto exit;
        }

        /* Set Pong source Address */
        retVal = EDMA_setSourceAddress(edmaHandle,  rangeProcObj->dataInChan[1],
            (uint32_t) SOC_translateAddress(dataInAddr[1], SOC_TranslateAddr_Dir_TO_EDMA, NULL));
        if (retVal != 0)
        {
            goto exit;
        }

        /* Kick off DMA to fetch data from ADC buffer for first channel */
        EDMA_startDmaTransfer(edmaHandle, rangeProcObj->dataInChan[0]);

        chirpPingPongId = pingPongId(rangeProcObj->chirpCount);
        /* 1d fft for first antenna, followed by kicking off the DMA of fft output */
        for (rxChanId = 0; rxChanId < DPParams->numRxAntennas; rxChanId++)
        {
            int16_t     *fftSrcAddr;
            int16_t     *fftDestAddr;
            uint8_t     inChannel;

            /*********************************
             * Data Input
             *********************************/
            inChannel = rangeProcObj->dataInChan[pingPongId(rxChanId)];

             /* Get the src/dest Address for FFT operation */
            fftSrcAddr = (int16_t*)&rangeProcObj->adcDataIn[pingPongId(rxChanId) * DPParams->numRangeBins];
            fftDestAddr = (int16_t*)&rangeProcObj->fftOut1D[chirpPingPongId * rangeProcObj->numSamplePerChirp +
                                                       (DPParams->numRangeBins * rxChanId)];

            if(rxChanId<DPParams->numRxAntennas -1U)
            {
                /* Kick off DMA to fetch data from ADC buffer for the next channel */
                EDMA_startDmaTransfer(edmaHandle, rangeProcObj->dataInChan[pingPongId(rxChanId + 1)]);
            }

            /* Verify if DMA has completed for current antenna */
            startTime1 = Cycleprofiler_getTimeStamp();
            rangeProcDSP_WaitEDMAComplete (  edmaHandle, inChannel);
            waitingTime += (Cycleprofiler_getTimeStamp() - startTime1);

            /*********************************
             * Data Processing
             *********************************/

            /* Only support even length of windowing */
            mmwavelib_windowing16x16_evenlen(
                    (int16_t *) fftSrcAddr,
                    (int16_t *) rangeProcObj->window,
                    DPParams->numAdcSamples);

            /* Zero out padding region */
            memset((void *)&rangeProcObj->adcDataIn[pingPongId(rxChanId) * DPParams->numRangeBins + DPParams->numAdcSamples],
                0 , (DPParams->numRangeBins - DPParams->numAdcSamples) * sizeof(cmplx16ImRe_t));

            /* 16bit FFT in imre format */
            DSP_fft16x16_imre(
                    (int16_t *) rangeProcObj->twiddle16x16,
                    DPParams->numRangeBins,
                    (int16_t *)fftSrcAddr,
                    (int16_t *) fftDestAddr);

        }

        /*********************************
         * DC removal processing
         *********************************/

        if(rangeProcObj->calibDcRangeSigCfg.enabled)
        {
            int32_t chirpPingPongSize;

            chirpPingPongSize = DPParams->numRxAntennas * (rangeProcObj->calibDcRangeSigCfg.positiveBinIdx -
                                                      rangeProcObj->calibDcRangeSigCfg.negativeBinIdx + 1);

            if (rangeProcObj->dcRangeSigCalibCntr == 0)
            {
                memset(rangeProcObj->dcRangeSigMean, 0, DPParams->numTxAntennas * chirpPingPongSize * sizeof(cmplx32ImRe_t));
            }

            rangeProcDSP_dcRangeSignatureCompensation(rangeProcObj, 
                                                         &rangeProcObj->calibDcRangeSigCfg,
                                                         chirpPingPongId,
                                                         rangeProcObj->chirpCount%DPParams->numTxAntennas);

            rangeProcObj->dcRangeSigCalibCntr++;
        }

        /*********************************
         * Data Output
         *********************************/
        outChannel = rangeProcObj->dataOutChan[chirpPingPongId];

        /* If numSamplePerChirp > 8192, we would need to be extra careful with BIndex value for 
             destination address in this situation.
             e.g if numRangeBins = 1024, numRxAntennas = 4 then destinationBindex becomes -32768 */
        if ((DPParams->numTxAntennas == 1U) && (rangeProcObj->numSamplePerChirp * sizeof(cmplx16ImRe_t) >= (uint32_t)16384U))
        {
            uint32_t    radarCubeAddr;

            radarCubeAddr = (uint32_t)(rangeProcObj->radarCubebuf + rangeProcObj->chirpCount * rangeProcObj->numSamplePerChirp);
            EDMA_setDestinationAddress(edmaHandle, outChannel,
                (uint32_t)SOC_translateAddress((radarCubeAddr), SOC_TranslateAddr_Dir_TO_EDMA, NULL));
        }
        /* Limitation of EDMA index settings, manually set the destination Address */
        else if ((DPParams->numTxAntennas == 2U) && (rangeProcObj->numSamplePerChirp * sizeof(cmplx16ImRe_t) >= (uint32_t)32768) ||
               (DPParams->numTxAntennas == 3U))
        {
            uint32_t    radarCubeAddr;
            uint8_t     numTxAnt;

            totalChirpIndex = rangeProcObj->chirpCount;
            numTxAnt = DPParams->numTxAntennas;

            radarCubeAddr = (uint32_t)(rangeProcObj->radarCubebuf + rangeProcObj->numSamplePerTx *(totalChirpIndex % numTxAnt) +
                                    (totalChirpIndex/numTxAnt)*rangeProcObj->numSamplePerChirp);
            EDMA_setDestinationAddress(edmaHandle, outChannel,
                (uint32_t)SOC_translateAddress((radarCubeAddr), SOC_TranslateAddr_Dir_TO_EDMA, NULL));
        }
        else
        {
            /* Other cases does not need manually set the destination address */
        }

        if(rangeProcObj->chirpCount > 1U)
        {
            startTime1 = Cycleprofiler_getTimeStamp();
            rangeProcDSP_WaitEDMAComplete (  edmaHandle, outChannel);
            waitingTime += (Cycleprofiler_getTimeStamp() - startTime1);
        }

        EDMA_startDmaTransfer(edmaHandle, outChannel);

        /* Increment chirp count */
        rangeProcObj->chirpCount++;

        /* Last chirp , wait until EDMA is completed */
        if(rangeProcObj->chirpCount == DPParams->numChirpsPerFrame)
        {
            /* Wait until last tansfer is done */
            rangeProcDSP_WaitEDMAComplete (  edmaHandle, outChannel);
            rangeProcObj->chirpCount = 0;
            outParams->endOfChirp = true;
        }

        rangeProcObj->numProcess++;
    }

    /* Update outParams */
    outParams->stats.processingTime = Cycleprofiler_getTimeStamp() - startTime - waitingTime;
    outParams->stats.waitTime = waitingTime;

    rangeProcObj->inProgress = false;

exit:
    return retVal;
}

/**
 *  @b Description
 *  @n
 *      The function is rangeProc DPU control function. 
 *
 *  @pre    DPU_RangeProcDSP_init() has been called
 *
 *  @param[in]  handle           rangeProc DPU handle
 *  @param[in]  cmd              rangeProc DPU control command
 *  @param[in]  arg              rangeProc DPU control argument pointer
 *  @param[in]  argSize          rangeProc DPU control argument size
 *
 *  \ingroup    DPU_RANGEPROC_EXTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
int32_t DPU_RangeProcDSP_control
(
    DPU_RangeProcDSP_Handle     handle,
    DPU_RangeProcDSP_Cmd        cmd,
    void*                       arg,
    uint32_t                    argSize
)
{
    int32_t             retVal = 0;
    rangeProcDSPObj     *rangeProcObj;

    /* Get rangeProc data object */
    rangeProcObj = (rangeProcDSPObj *)handle;

    /* Sanity check */
    if (rangeProcObj == NULL)
    {
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }

    /* Check if control() is called during processing time */
    if(rangeProcObj->inProgress == true)
    {
        retVal = DPU_RANGEPROCDSP_EINPROGRESS;
        goto exit;
    }

    /* Control command handling */
    switch(cmd)
    {
        case DPU_RangeProcDSP_Cmd_dcRangeCfg:
        {
            DPU_RangeProc_CalibDcRangeSigCfg   *calibDc;
            
            if((argSize != sizeof(DPU_RangeProc_CalibDcRangeSigCfg)) ||
               (arg == NULL))
            {
                retVal = DPU_RANGEPROCDSP_EINVAL;
                goto exit;
            }
            else
            {
                calibDc = (DPU_RangeProc_CalibDcRangeSigCfg *)arg;

                if(memcmp((void *)&rangeProcObj->calibDcRangeSigCfg,
                           (void *)calibDc, sizeof(DPU_RangeProc_CalibDcRangeSigCfg)) == 0) 
                {
                    /* NO configuration change, nothing needs to be done here */
                }
                else
                {
                    retVal = rangeProcDSP_dcRangeSignatureCompensation_init(rangeProcObj, calibDc, 1U);
                    if(retVal < 0)
                    {
                        goto exit;
                    }
                    else
                    {
                        /* Save configuration */
                        memcpy((void *)&rangeProcObj->calibDcRangeSigCfg, (void *)arg, argSize);
                    }
                }
            }
        }
        break;

        default:
            retVal = DPU_RANGEPROCDSP_ECMD;
            break;
    }
exit:
    return (retVal);
}

/**
 *  @b Description
 *  @n
 *      The function is rangeProc DPU deinitl function. It frees up the 
 *   resources allocated during init.
 *
 *  @pre    DPU_RangeProcDSP_init() has been called
 *
 *  @param[in]  handle           rangeProc DPU handle
 *
 *  \ingroup    DPU_RANGEPROC_EXTERNAL_FUNCTION
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
int32_t DPU_RangeProcDSP_deinit(DPU_RangeProcDSP_Handle handle)
{
    rangeProcDSPObj     *rangeProcObj;
    int32_t             retVal = 0;

    /* Sanity Check */
    rangeProcObj = (rangeProcDSPObj *)handle;
    if(rangeProcObj == NULL)
    {
        retVal = DPU_RANGEPROCDSP_EINVAL;
        goto exit;
    }
    else
    {
        /* Free memory */
        MemoryP_ctrlFree(handle, sizeof(rangeProcDSPObj));
    }
exit:
    return (retVal);
}