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

#include <xdc/runtime/System.h>

#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>

#include <ti/common/sys_common.h>
#include <ti/drivers/soc/soc.h>
#include <ti/drivers/esm/esm.h>
#include <ti/drivers/mailbox/mailbox.h>
#include <ti/drivers/hwa/hwa.h>
#include <ti/drivers/spi/SPI.h>
#include <ti/drivers/qspiflash/qspiflash.h>

#include <ti/control/mmwave/mmwave.h>
#include <ti/control/mmwavelink/mmwavelink.h>
#include <ti/control/mmwave/include/mmwave_internal.h>
#include "rl_datatypes.h"
#include "rl_sensor.h"
#include "rl_driver.h"

/* Demo Includes */
#include "demo.h"
#include "iwr_gpio.h"

/* Local compile-time options */

//CZ_BOARD should be defined when running on the CZ board, especially with
//waveguide, since the Tx/Rx power requirements are different. This should
//not be defined for normal use.
//#define CZ_BOARD

//When RUN_ONESHOT_CAL is defined, the demo will command the BSS to run
//one-shot runtime cal prior to the chirps. This is the default, to update
//the full cal data that is stored in flash. If running full cal, this does
//have to run.
#define RUN_ONESHOT_CAL

//When POWER_TEST is defined, the GPIO_0 toggles will not match what the
//MSP432 expects for measurement power ups. This is disabled by default,
//and will require INCLUDE_CYCLE_PROFILER to be defined as well.
//#define POWER_TEST

//When RUN_FULL_CAL is defined, the configuration process will command
//the BSS to run all boot-time cal tasks, instead of reading data from flash.
//This is disabled by default.  When enabled, it disables flash access.
//#define RUN_FULL_CAL

//When INCLUDE_CYCLE_PROFILER is defined, the Cycle Profiler will be included
//so that you may add calls to capture the number of 200Mhz clocks a certain
//task takes. To get the time in milliseconds, simply subtract two times,
//then divide by 200000.
//#define INCLUDE_CYCLE_PROFILER

#ifdef INCLUDE_CYCLE_PROFILER
#include <ti/utils/cycleprofiler/cycle_profiler.h>
extern unsigned int cptime[12];
#endif

extern SOC_Handle socHandle;
extern SPI_Handle spiHandle;

#ifndef RUN_FULL_CAL
extern QSPI_Handle qspiHandle;
extern QSPIFlash_Handle qspiflashHandle;
#endif

extern UART_Handle uartHandle;
extern RF_Param_t *RF_Param;
extern const RF_Param_t RF_Param_10m;

extern ParamNeedSave_t *ParamNeedSave;
extern FlagAboutMCU_t FlagAboutMcu;

extern void communications_protocol(void);
void Save_Data2Flash(uint8_t *const srcDataAddr, uint16_t DataLen, uint32_t flashAddrOffset);
/* Globals */
LSDemo_MCB gMmwMCB;

rlCalibrationData_t init_cal_data[RL_MAX_CALIB_DATA_CHUNK];

/*! L3 RAM buffer */
uint8_t gMmwL3[SOC_AR14XX_MSS_L3RAM_SIZE];
#pragma DATA_SECTION(gMmwL3, ".l3ram");

/*! L3 heap for convenicence of partitioning L3 RAM */
DemoMemPool_t gMmwL3heap =
    {
        &gMmwL3[0],
        SOC_AR14XX_MSS_L3RAM_SIZE,
        0};

#pragma DATA_SECTION(gMmwHwaMemBuf, ".hwaBufs");
mmwHwaBuf_t gMmwHwaMemBuf[MMW_HWA_NUM_MEM_BUFS];
#define MMW_HWA_1D_ADCBUF_INP (&gMmwHwaMemBuf[0])

uint32_t datapath_err;
LSDemo_ChirpData chirp_data;
LSDemo_CalData cal_data;
SendMessage2MCU_t FirstFrame;

uint32_t log2Approx(uint32_t x)
{
    uint32_t idx, detectFlag = 0;

    idx = 32U;
    while ((detectFlag == 0U) || (idx == 0U))
    {
        if (x & 0x80000000U)
        {
            detectFlag = 1;
        }
        x <<= 1U;
        idx--;
    }

    if (x != 0)
    {
        idx = idx + 1;
    }

    return (idx);
}

/**
 *  @b Description
 *  @n
 *      Function to generate a single FFT window sample.
 *
 *  @param[out] win Pointer to output calculated window sample.
 *  @param[in]  winIndx Index of window to generate sample at.
 *  @param[in]  phi Pre-calculated constant by caller as (2*pi/(window length - 1)).
 *  @param[in]  winType Type of window, one of @ref MMW_WIN_BLACKMAN, @ref MMW_WIN_HANNING,
 *              or @ref MMW_WIN_RECT.
 *  @retval none.
 */
static inline demo_genWindow(uint32_t *win, uint32_t winIndx, float phi, uint32_t winType)
{
    if (winType == MMW_WIN_BLACKMAN)
    {
        //Blackman window
        float a0 = 0.42;
        float a1 = 0.5;
        float a2 = 0.08;
        *win = (uint32_t)((ONE_Q17 * (a0 - a1 * cos(phi * winIndx) +
                                      a2 * cos(2 * phi * winIndx))) +
                          0.5); //in Q17
        if (*win >= ONE_Q17)
        {
            *win = ONE_Q17 - 1;
        }
    }
    else if (winType == MMW_WIN_HANNING)
    {
        //Hanning window
        *win = (uint32_t)((ONE_Q17 * 0.5 * (1 - cos(phi * winIndx))) + 0.5); //in Q17
        if (*win >= ONE_Q17)
        {
            *win = ONE_Q17 - 1;
        }
    }
    else if (winType == MMW_WIN_RECT)
    {
        //Rectangular window
        *win = (uint32_t)(ONE_Q17 / 16); //in Q17
    }
}

uint32_t demo_pow2roundup(uint32_t x)
{
    uint32_t result = 1;
    while (x > result)
    {
        result <<= 1;
    }
    return result;
}

/* */
void demo_memPoolReset(DemoMemPool_t *pool)
{
    pool->indx = 0;
}

uint8_t *demo_memPoolAlloc(DemoMemPool_t *pool, uint32_t size)
{
    uint8_t *ptr = NULL;

    if ((size % 4) != 0)
    {
        return (ptr);
    }

    if ((pool->indx + size) <= pool->size)
    {
        ptr = pool->base + pool->indx;
        pool->indx += size;
    }

    return (ptr);
}

/**
 *  @b Description
 *  @n
 *      HWA processing completion call back function as per HWA API.
 *      Depending on the programmed transfer completion codes,
 *      posts HWA done semaphore.
 */
void demo_dataPathHwaDoneIsrCallback(void *arg)
{
    Semaphore_Handle semHandle;

    gMmwMCB.dataPathObj.hwaDoneIsrCounter++;

    if (arg != NULL)
    {
        semHandle = (Semaphore_Handle)arg;
        Semaphore_post(semHandle);
    }
}
void HWAutil_configRangeFFT(HWA_Handle handle,
                            uint32_t paramSetStartIdx,
                            uint32_t numAdcSamples,
                            uint32_t numRangeBins,
                            uint8_t numRxAnt,
                            uint32_t windowOffsetBytes,

                            uint8_t dmaTriggerSourcePing,
                            uint8_t dmaTriggerSourcePong,

                            uint8_t dmaDestChannelPing,
                            uint8_t dmaDestChannelPong,

                            uint16_t hwaMemAdcBufOffset,

                            // uint16_t hwaMemDestPingOffset,
                            //  uint16_t hwaMemDestPongOffset,
                            uint8_t hwaTriggerMode,
                            uint8_t windowEn)
{
    HWA_InterruptConfig paramISRConfig;
    int32_t errCode = 0;
    uint32_t paramsetIdx = paramSetStartIdx;
    uint32_t pingParamSetIdx1 = 0;
    uint32_t pingParamSetIdx2 = 0;
    HWA_ParamConfig hwaParamCfg[HWAUTIL_NUM_PARAM_SETS_1D];

    memset(hwaParamCfg, 0, sizeof(hwaParamCfg));

    /***********************/
    /* PING DUMMY PARAMSET */
    /***********************/
    hwaParamCfg[paramsetIdx].triggerMode = HWA_TRIG_MODE_DMA;      //Software triggered  - in demo this will be HWA_TRIG_MODE_DMA
    hwaParamCfg[paramsetIdx].dmaTriggerSrc = dmaTriggerSourcePing; //in demo this will be first EDMA Src channel id
    hwaParamCfg[paramsetIdx].accelMode = HWA_ACCELMODE_NONE;       //dummy
    errCode = HWA_configParamSet(handle, paramsetIdx, &hwaParamCfg[paramsetIdx], NULL);
    /* if (errCode != 0)
    {
        //System_printf("Error: HWA_configParamSet(%d) returned %d\n", paramsetIdx, errCode);

        return;
    }*/

    /***********************/
    /* PING PROCESS PARAMSET */
    /***********************/
    paramsetIdx++;
    pingParamSetIdx1 = paramsetIdx;
    hwaParamCfg[paramsetIdx].triggerMode = hwaTriggerMode;
    hwaParamCfg[paramsetIdx].accelMode = HWA_ACCELMODE_FFT; //do FFT

    hwaParamCfg[paramsetIdx].source.srcAddr = hwaMemAdcBufOffset;                // address is relative to start of MEM0
    hwaParamCfg[paramsetIdx].source.srcAcnt = numAdcSamples / 4 - 1;             //this is samples - 1
    hwaParamCfg[paramsetIdx].source.srcAIdx = 4 * numRxAnt * sizeof(uint32_t);   // 4Kstitching 16 bytes
    hwaParamCfg[paramsetIdx].source.srcBcnt = 3;                                 //4Kstitching 4 back-to-back 1024 point fft
    hwaParamCfg[paramsetIdx].source.srcBIdx = sizeof(uint32_t);                  //should be dont care
    hwaParamCfg[paramsetIdx].source.srcShift = 0;                                //no shift
    hwaParamCfg[paramsetIdx].source.srcCircShiftWrap = 0;                        //no shift
    hwaParamCfg[paramsetIdx].source.srcRealComplex = HWA_SAMPLES_FORMAT_COMPLEX; //complex data
    hwaParamCfg[paramsetIdx].source.srcWidth = HWA_SAMPLES_WIDTH_16BIT;          //16-bit
    hwaParamCfg[paramsetIdx].source.srcSign = HWA_SAMPLES_SIGNED;                //signed
    hwaParamCfg[paramsetIdx].source.srcConjugate = 0;                            //no conjugate
    hwaParamCfg[paramsetIdx].source.srcScale = 8;
    hwaParamCfg[paramsetIdx].source.bpmEnable = 0; //bpm removal not enabled
    hwaParamCfg[paramsetIdx].source.bpmPhase = 0;  //dont care

    hwaParamCfg[paramsetIdx].dest.dstAddr = ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PING_M0_M2); //hwaMemDestPingOffset; // address is relative to start of MEM0
    hwaParamCfg[paramsetIdx].dest.dstAcnt = numRangeBins / 4 - 1;                                 //this is samples - 1
    hwaParamCfg[paramsetIdx].dest.dstAIdx = 4 * numRxAnt * sizeof(uint32_t);                      //
    hwaParamCfg[paramsetIdx].dest.dstBIdx = sizeof(uint32_t);                                     //should be dont care
    hwaParamCfg[paramsetIdx].dest.dstRealComplex = HWA_SAMPLES_FORMAT_COMPLEX;                    //same as input - complex
    hwaParamCfg[paramsetIdx].dest.dstWidth = HWA_SAMPLES_WIDTH_16BIT;                             //same as input - 16 bit
    hwaParamCfg[paramsetIdx].dest.dstSign = HWA_SAMPLES_SIGNED;                                   //same as input - signed
    hwaParamCfg[paramsetIdx].dest.dstConjugate = 0;                                               //no conjugate
    hwaParamCfg[paramsetIdx].dest.dstScale = 0;
    hwaParamCfg[paramsetIdx].dest.dstSkipInit = 0; // no skipping

    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.fftEn = 1;
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.fftSize = log2Approx(numRangeBins / 4);
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.butterflyScaling = 0x3; //LSB fftSize bits are relevant - revisit this for all FFT size and data size
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.interfZeroOutEn = 1;    //disabled
#ifdef I_DO_NOT_WANT_USE_WIN
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.windowEn = 0; //enabled
#else
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.windowEn = windowEn;
#endif
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.windowStart = windowOffsetBytes;                 //start of window RAM
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.winSymm = 0;                                     //non-symmetric - in demo do we make this symmetric
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.winInterpolateMode = 1;                          //4Kstitching, enabling window interpolation
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.magLogEn = HWA_FFT_MODE_MAGNITUDE_LOG2_DISABLED; //disabled
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.fftOutMode = HWA_FFT_MODE_OUTPUT_DEFAULT;        // output FFT samples

    hwaParamCfg[paramsetIdx].complexMultiply.mode = HWA_COMPLEX_MULTIPLY_MODE_DISABLE;
    errCode = HWA_configParamSet(handle, paramsetIdx, &hwaParamCfg[paramsetIdx], NULL);
    /*   if (errCode != 0)
    {
        //System_printf("Error: HWA_configParamSet(%d) returned %d\n",errCode,paramsetIdx);
        return;
    }*/
    /***********************/
    /* PING PROCESS PARAMSET 4K stitching Step 2*/
    /***********************/
    paramsetIdx++;
    pingParamSetIdx2 = paramsetIdx;
    hwaParamCfg[paramsetIdx].triggerMode = HWA_TRIG_MODE_IMMEDIATE;
    hwaParamCfg[paramsetIdx].accelMode = HWA_ACCELMODE_FFT; //do FFT

    hwaParamCfg[paramsetIdx].source.srcAddr = ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PING_M0_M2); //hwaMemDestPingOffset; // MEM2 (32KB)
    hwaParamCfg[paramsetIdx].source.srcAcnt = 3;                                                    //4 point FFT
    hwaParamCfg[paramsetIdx].source.srcAIdx = 4;                                                    // adjacent samples are 4 bytes apart
    hwaParamCfg[paramsetIdx].source.srcBcnt = numRangeBins / 4 - 1;                                 //1024 4-point fft
    hwaParamCfg[paramsetIdx].source.srcBIdx = 16;                                                   //Each set of 4-point fft is spaced 16 bytes apart
    hwaParamCfg[paramsetIdx].source.srcShift = 0;                                                   //no shift
    hwaParamCfg[paramsetIdx].source.srcCircShiftWrap = 0;                                           //no shift
    hwaParamCfg[paramsetIdx].source.srcRealComplex = HWA_SAMPLES_FORMAT_COMPLEX;                    //complex data
    hwaParamCfg[paramsetIdx].source.srcWidth = HWA_SAMPLES_WIDTH_16BIT;                             //16-bit
    hwaParamCfg[paramsetIdx].source.srcSign = HWA_SAMPLES_SIGNED;                                   //signed
    hwaParamCfg[paramsetIdx].source.srcConjugate = 0;                                               //no conjugate
    hwaParamCfg[paramsetIdx].source.srcScale = 8;
    hwaParamCfg[paramsetIdx].source.bpmEnable = 0; //bpm removal not enabled
    hwaParamCfg[paramsetIdx].source.bpmPhase = 0;  //dont care

    hwaParamCfg[paramsetIdx].dest.dstAddr = ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PING_M2_M3); //hwaMemAdcBufOffset; // MEM0
    hwaParamCfg[paramsetIdx].dest.dstAcnt = 3;                                                    //this is samples - 1
    hwaParamCfg[paramsetIdx].dest.dstAIdx = numRangeBins;                                         //
    hwaParamCfg[paramsetIdx].dest.dstBIdx = 4;                                                    //should be dont care
    hwaParamCfg[paramsetIdx].dest.dstRealComplex = HWA_SAMPLES_FORMAT_COMPLEX;                    //same as input - complex
    hwaParamCfg[paramsetIdx].dest.dstWidth = HWA_SAMPLES_WIDTH_16BIT;                             //same as input - 16 bit
    hwaParamCfg[paramsetIdx].dest.dstSign = HWA_SAMPLES_SIGNED;                                   //same as input - signed
    hwaParamCfg[paramsetIdx].dest.dstConjugate = 0;                                               //no conjugate
    hwaParamCfg[paramsetIdx].dest.dstScale = 0;
    hwaParamCfg[paramsetIdx].dest.dstSkipInit = 0; // no skipping

    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.fftEn = 1;
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.fftSize = 2;
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.butterflyScaling = 0x3;          //LSB fftSize bits are relevant - revisit this for all FFT size and data size
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.interfZeroOutEn = 0;             //disabled
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.windowEn = 0;                    //enabled
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.windowStart = windowOffsetBytes; //start of window RAM
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.winSymm = 0;                     //non-symmetric - in demo do we make this symmetric
    //hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.winInterpolateMode = 0; //fftsize is less than 1K
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.winInterpolateMode = 0;                          //4Kstitching, enabling window interpolation
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.magLogEn = HWA_FFT_MODE_MAGNITUDE_LOG2_DISABLED; //disabled
    hwaParamCfg[paramsetIdx].accelModeArgs.fftMode.fftOutMode = HWA_FFT_MODE_OUTPUT_DEFAULT;        // output FFT samples

    hwaParamCfg[paramsetIdx].complexMultiply.mode = HWA_COMPLEX_MULTIPLY_MODE_FFT_STITCHING;
    hwaParamCfg[paramsetIdx].complexMultiply.cmpMulArgs.twidIncrement = HWA_FFT_STITCHING_TWID_PATTERN_4K;
    errCode = HWA_configParamSet(handle, paramsetIdx, &hwaParamCfg[paramsetIdx], NULL);
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        //System_printf("Error: HWA_configParamSet(%d) returned %d\n",errCode,paramsetIdx);
        return;
    }
    /**********************************************************************************************************************************/
    /***********************/
    /* PONG DUMMY PARAMSET */
    /***********************/
    paramsetIdx++;
    hwaParamCfg[paramsetIdx].triggerMode = HWA_TRIG_MODE_DMA;
    hwaParamCfg[paramsetIdx].dmaTriggerSrc = dmaTriggerSourcePong; //in demo this will be second EDMA Src channel id
    hwaParamCfg[paramsetIdx].accelMode = HWA_ACCELMODE_NONE;       //dummy
    errCode = HWA_configParamSet(handle, paramsetIdx, &hwaParamCfg[paramsetIdx], NULL);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_configParamSet(%d) returned %d\n",errCode,paramsetIdx);
        //MmwDemo_debugAssert(0);
        return;
    }

    /***********************/
    /* PONG PROCESS PARAMSET */
    /***********************/
    paramsetIdx++;
    hwaParamCfg[paramsetIdx] = hwaParamCfg[pingParamSetIdx1];
    //hwaParamCfg[paramsetIdx].source.srcAddr = ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PONG_M3_M1);
    hwaParamCfg[paramsetIdx].dest.dstAddr = ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PONG_M1_M3); //hwaMemDestPongOffset;

    errCode = HWA_configParamSet(handle, paramsetIdx, &hwaParamCfg[paramsetIdx], NULL);
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        System_printf("Error: HWA_configParamSet(%d) returned %d\n", errCode, paramsetIdx);
        return;
    }

    /***********************/
    /* PONG PROCESS PARAMSET Step 2*/
    /***********************/
    paramsetIdx++;
    hwaParamCfg[paramsetIdx] = hwaParamCfg[pingParamSetIdx2];
    hwaParamCfg[paramsetIdx].source.srcAddr = ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PONG_M1_M3);
    hwaParamCfg[paramsetIdx].dest.dstAddr = ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PONG_M3_M2); //hwaMemDestPongOffset;
    errCode = HWA_enableParamSetInterrupt(handle, paramsetIdx, &paramISRConfig);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_enableParamSetInterrupt(PING DMA) returned %d\n",errCode);
        //MmwDemo_debugAssert(0);
        return;
    }
    /**********************************************************************************************************************************/
    /* enable the DMA hookup to this paramset so that data gets copied out */
    paramISRConfig.interruptTypeFlag = HWA_PARAMDONE_INTERRUPT_TYPE_DMA;
    paramISRConfig.dma.dstChannel = dmaDestChannelPing; //TODO sync this define EDMA channel to trigger to copy the data out
    //paramISRConfig.cpu.callbackArg = paramSetSem;//TODO check if NULL is required
    errCode = HWA_enableParamSetInterrupt(handle, paramsetIdx, &paramISRConfig);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_enableParamSetInterrupt(PING DMA) returned %d\n",errCode);
        // MmwDemo_debugAssert (0);
        return;
    }
}

void HWAutil_configCFAR(HWA_Handle handle,
                        uint32_t paramSetStartIdx,
                        uint32_t numRangeBins,
                        uint32_t numDopplerBins,
                        uint32_t winLen,
                        uint32_t guardLen,
                        uint32_t noiseDivRightShift,
                        uint8_t peakGrouping,
                        uint8_t cyclicMode,
                        uint8_t nAvgMode,
                        uint16_t detObjectListSize,
                        uint8_t dmaTriggerSource,
                        uint8_t dmaDestChannel,
                        uint16_t hwaSourceBufOffset,
                        uint16_t hwaDestBufOffset)
{
    HWA_ParamConfig hwaParamCfg;
    HWA_InterruptConfig paramISRConfig;
    int32_t errCode = 0;

    memset((void *)&hwaParamCfg, 0, sizeof(hwaParamCfg));
    hwaParamCfg.triggerMode = HWA_TRIG_MODE_DMA;
    hwaParamCfg.dmaTriggerSrc = dmaTriggerSource;

    hwaParamCfg.accelModeArgs.cfarMode.peakGroupEn = peakGrouping;
    hwaParamCfg.accelMode = HWA_ACCELMODE_CFAR;

    //cfarInpMode = 1, cfarLogMode = 1, cfarAbsMode = 00b
    hwaParamCfg.accelModeArgs.cfarMode.operMode = HWA_CFAR_OPER_MODE_MAG_INPUT_REAL;

    hwaParamCfg.source.srcAddr = hwaSourceBufOffset;
    // hwaParamCfg.source.srcAcnt = numRangeBins-1+winLen*2+guardLen*2;
    hwaParamCfg.source.srcAcnt = numRangeBins - 1;
    hwaParamCfg.source.srcRealComplex = HWA_SAMPLES_FORMAT_REAL;
    hwaParamCfg.source.srcAIdx = numDopplerBins * 2;
    hwaParamCfg.source.srcBIdx = sizeof(uint16_t);
    hwaParamCfg.source.srcBcnt = numDopplerBins - 1;
    // hwaParamCfg.source.srcShift = numRangeBins-winLen*2-guardLen;
    // hwaParamCfg.source.srcCircShiftWrap = 12;
    hwaParamCfg.source.srcScale = 8;

    hwaParamCfg.dest.dstAddr = hwaDestBufOffset;
    hwaParamCfg.dest.dstAcnt = detObjectListSize - 1;
    hwaParamCfg.dest.dstRealComplex = HWA_SAMPLES_FORMAT_REAL;
    hwaParamCfg.dest.dstWidth = HWA_SAMPLES_WIDTH_16BIT;
    hwaParamCfg.dest.dstAIdx = 2;
    hwaParamCfg.dest.dstBIdx = 4096;
    hwaParamCfg.dest.dstScale = 0;

    // hwaParamCfg.dest.dstSkipInit = 0;

    hwaParamCfg.accelModeArgs.cfarMode.numGuardCells = guardLen;
    hwaParamCfg.accelModeArgs.cfarMode.nAvgDivFactor = noiseDivRightShift;
    hwaParamCfg.accelModeArgs.cfarMode.cyclicModeEn = cyclicMode;
    hwaParamCfg.accelModeArgs.cfarMode.nAvgMode = nAvgMode;
    hwaParamCfg.accelModeArgs.cfarMode.numNoiseSamplesRight = winLen >> 1;
    hwaParamCfg.accelModeArgs.cfarMode.numNoiseSamplesLeft = winLen >> 1;
    hwaParamCfg.accelModeArgs.cfarMode.outputMode = HWA_CFAR_OUTPUT_MODE_I_nAVG_ALL_Q_CUT;

    errCode = HWA_configParamSet(handle, paramSetStartIdx, &hwaParamCfg, NULL);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_configParamSet(%d) returned %d\n", paramSetStartIdx, errCode);
        // MmwDemo_debugAssert (0);
        return;
    }

    /* Enable the DMA hookup to the last paramset */
    paramISRConfig.interruptTypeFlag = HWA_PARAMDONE_INTERRUPT_TYPE_DMA;
    paramISRConfig.dma.dstChannel = dmaDestChannel; //EDMA channel to trigger to copy the data out
    paramISRConfig.cpu.callbackArg = NULL;
    errCode = HWA_enableParamSetInterrupt(handle, paramSetStartIdx, &paramISRConfig);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_enableParamSetInterrupt(PONG DMA) returned %d\n",errCode);
        // MmwDemo_debugAssert (0);
        return;
    }
}

void HWAutil_configCFAR_near(HWA_Handle handle,
                             uint32_t paramSetStartIdx,
                             uint32_t numRangeBins,
                             uint32_t numDopplerBins,
                             uint32_t winLen,
                             uint32_t guardLen,
                             uint32_t noiseDivRightShift,
                             uint8_t peakGrouping,
                             uint8_t cyclicMode,
                             uint8_t nAvgMode,
                             uint16_t detObjectListSize,
                             uint8_t dmaTriggerSource,
                             uint8_t dmaDestChannel,
                             uint16_t hwaSourceBufOffset,
                             uint16_t hwaDestBufOffset)
{
    HWA_ParamConfig hwaParamCfg;
    HWA_InterruptConfig paramISRConfig;
    int32_t errCode = 0;

    memset((void *)&hwaParamCfg, 0, sizeof(hwaParamCfg));
    hwaParamCfg.triggerMode = HWA_TRIG_MODE_DMA;
    hwaParamCfg.dmaTriggerSrc = dmaTriggerSource;

    hwaParamCfg.accelModeArgs.cfarMode.peakGroupEn = peakGrouping;
    hwaParamCfg.accelMode = HWA_ACCELMODE_CFAR;

    hwaParamCfg.accelModeArgs.cfarMode.operMode = HWA_CFAR_OPER_MODE_LOG_INPUT_REAL;

    hwaParamCfg.source.srcAddr = hwaSourceBufOffset;
    hwaParamCfg.source.srcAcnt = numRangeBins - 1;
    hwaParamCfg.source.srcRealComplex = HWA_SAMPLES_FORMAT_REAL;
    hwaParamCfg.source.srcAIdx = 2;
    hwaParamCfg.source.srcBIdx = sizeof(int16_t);
    hwaParamCfg.source.srcBcnt = numDopplerBins - 1;
    hwaParamCfg.source.srcScale = 8;

    hwaParamCfg.dest.dstAddr = hwaDestBufOffset;
    hwaParamCfg.dest.dstAcnt = detObjectListSize - 1;
    hwaParamCfg.dest.dstRealComplex = HWA_SAMPLES_FORMAT_REAL;
    hwaParamCfg.dest.dstWidth = HWA_SAMPLES_WIDTH_32BIT;
    hwaParamCfg.dest.dstAIdx = 4;
    hwaParamCfg.dest.dstBIdx = 4096;
    hwaParamCfg.dest.dstScale = 8;
    // hwaParamCfg.dest.dstSkipInit = 0;

    hwaParamCfg.accelModeArgs.cfarMode.numGuardCells = guardLen;
    hwaParamCfg.accelModeArgs.cfarMode.nAvgDivFactor = noiseDivRightShift;
    hwaParamCfg.accelModeArgs.cfarMode.cyclicModeEn = cyclicMode;
    hwaParamCfg.accelModeArgs.cfarMode.nAvgMode = nAvgMode;
    hwaParamCfg.accelModeArgs.cfarMode.numNoiseSamplesRight = winLen >> 1;
    hwaParamCfg.accelModeArgs.cfarMode.numNoiseSamplesLeft = winLen >> 1;
    hwaParamCfg.accelModeArgs.cfarMode.outputMode = HWA_CFAR_OUTPUT_MODE_I_PEAK_IDX_Q_NEIGHBOR_NOISE_VAL;

    errCode = HWA_configParamSet(handle, paramSetStartIdx, &hwaParamCfg, NULL);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_configParamSet(%d) returned %d\n", paramSetStartIdx, errCode);
        // MmwDemo_debugAssert (0);
        return;
    }

    /* Enable the DMA hookup to the last paramset */
    paramISRConfig.interruptTypeFlag = HWA_PARAMDONE_INTERRUPT_TYPE_DMA;
    paramISRConfig.dma.dstChannel = dmaDestChannel; //EDMA channel to trigger to copy the data out
    paramISRConfig.cpu.callbackArg = NULL;
    errCode = HWA_enableParamSetInterrupt(handle, paramSetStartIdx, &paramISRConfig);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_enableParamSetInterrupt(PONG DMA) returned %d\n",errCode);
        // MmwDemo_debugAssert (0);
        return;
    }
}

/**
 *  @b Description
 *  @n
 *      EDMA transfer completion call back function as per EDMA API.
 *      Depending on the programmed transfer completion codes,
 *      posts the corresponding done/completion semaphore.
 *      Per current design, a single semaphore could have been used as the
 *      1D, 2D and CFAR stages are sequential, this code gives some flexibility
 *      if some design change in future.
 */
void demo_EDMA_transferCompletionCallbackFxn(uintptr_t arg,
                                             uint8_t transferCompletionCode)
{
    Demo_DataPathObj *obj = &gMmwMCB.dataPathObj;

    if (transferCompletionCode == DEMO_EDMA_TRANSFER_COMPLETION_CODE_1D_DONE)
        Semaphore_post(obj->EDMA_1Ddone_semHandle);
    else if (transferCompletionCode == MMWDEMO_EDMA_TRANSFER_COMPLETION_CODE_CFAR_DONE)
        Semaphore_post(obj->EDMA_CFARdone_semHandle);
    else
        demo_printf("Error: EDMA xfr complete returned invalid code: %d\n", transferCompletionCode);
}

/**
 *  @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 demo_EDMA_errorCallbackFxn(EDMA_Handle handle, EDMA_errorInfo_t *errorInfo)
{
    gMmwMCB.dataPathObj.EDMA_errorInfo = *errorInfo;
    demo_printf("Error: EDMA error callback code: %d\n", *errorInfo);
}

/**
 *  @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 demo_EDMA_transferControllerErrorCallbackFxn(EDMA_Handle handle,
                                                  EDMA_transferControllerErrorInfo_t *errorInfo)
{
    gMmwMCB.dataPathObj.EDMA_transferControllerErrorInfo = *errorInfo;
    demo_printf("Error: EDMA xfr error callback code: %d\n", *errorInfo);
}

static int32_t EDMA_setup_shadow_link(EDMA_Handle handle, uint32_t chId, uint32_t linkChId,
                                      EDMA_paramSetConfig_t *config, EDMA_transferCompletionCallbackFxn_t transferCompletionCallbackFxn,
                                      uintptr_t transferCompletionCallbackFxnArg)
{
    EDMA_paramConfig_t paramConfig;
    int32_t errorCode = EDMA_NO_ERROR;

    paramConfig.paramSetConfig = *config; //this will copy the entire param set config
    paramConfig.transferCompletionCallbackFxn = transferCompletionCallbackFxn;
    paramConfig.transferCompletionCallbackFxnArg = (uintptr_t)transferCompletionCallbackFxnArg;
    if ((errorCode = EDMA_configParamSet(handle, linkChId, &paramConfig)) != EDMA_NO_ERROR)
    {
        demo_printf("Error: EDMA_configParamSet() failed with error code = %d\n", errorCode);
        goto exit;
    }

    if ((errorCode = EDMA_linkParamSets(handle, chId, linkChId)) != EDMA_NO_ERROR)
    {
        demo_printf("Error: EDMA_linkParamSets() failed with error code = %d\n", errorCode);
        goto exit;
    }

    if ((errorCode = EDMA_linkParamSets(handle, linkChId, linkChId)) != EDMA_NO_ERROR)
    {
        demo_printf("Error: EDMA_linkParamSets() failed with error code = %d\n", errorCode);
        goto exit;
    }

exit:
    return (errorCode);
}

int32_t EDMAutil_configHwaTranspose(EDMA_Handle handle,
                                    uint8_t chId, uint16_t linkChId, uint8_t chainChId,
                                    uint32_t *pSrcAddress, uint32_t *pDestAddress,
                                    uint8_t numAnt, uint16_t numRangeBins, uint16_t numChirpsPerFrame,
                                    bool isIntermediateChainingEnabled,
                                    bool isFinalChainingEnabled,
                                    bool isTransferCompletionEnabled,
                                    EDMA_transferCompletionCallbackFxn_t transferCompletionCallbackFxn,
                                    uintptr_t transferCompletionCallbackFxnArg)
{
    EDMA_channelConfig_t config;
    int32_t errorCode = EDMA_NO_ERROR;

    config.channelId = chId;
    config.channelType = (uint8_t)EDMA3_CHANNEL_TYPE_DMA;
    config.paramId = chId;
    config.eventQueueId = 0;

    config.paramSetConfig.sourceAddress = (uint32_t)pSrcAddress;
    config.paramSetConfig.destinationAddress = (uint32_t)pDestAddress;

    config.paramSetConfig.aCount = numAnt * 4;
    config.paramSetConfig.bCount = numRangeBins;
    config.paramSetConfig.cCount = numChirpsPerFrame / 2;
    //config.paramSetConfig.cCount = 1;       //改
    config.paramSetConfig.bCountReload = 0; //config.paramSetConfig.bCount;

    config.paramSetConfig.sourceBindex = numAnt * 4;
    config.paramSetConfig.destinationBindex = numChirpsPerFrame * numAnt * 4;
    //config.paramSetConfig.destinationBindex = numAnt * 4; //改

    config.paramSetConfig.sourceCindex = 0;
    config.paramSetConfig.destinationCindex = numAnt * 4 * 2;
    //config.paramSetConfig.destinationCindex = numAnt * 4; //改

    config.paramSetConfig.linkAddress = EDMA_NULL_LINK_ADDRESS;
    config.paramSetConfig.transferType = (uint8_t)EDMA3_SYNC_AB;
    config.paramSetConfig.transferCompletionCode = chainChId;
    config.paramSetConfig.sourceAddressingMode = (uint8_t)EDMA3_ADDRESSING_MODE_LINEAR;
    config.paramSetConfig.destinationAddressingMode = (uint8_t)EDMA3_ADDRESSING_MODE_LINEAR;

    /* don't care because of linear addressing modes above */
    config.paramSetConfig.fifoWidth = (uint8_t)EDMA3_FIFO_WIDTH_8BIT;
    config.paramSetConfig.isStaticSet = false;
    config.paramSetConfig.isEarlyCompletion = false;
    config.paramSetConfig.isFinalTransferInterruptEnabled =
        isTransferCompletionEnabled;
    config.paramSetConfig.isIntermediateTransferInterruptEnabled = false;
    config.paramSetConfig.isFinalChainingEnabled =
        isFinalChainingEnabled;
    config.paramSetConfig.isIntermediateChainingEnabled =
        isIntermediateChainingEnabled;
    config.transferCompletionCallbackFxn = transferCompletionCallbackFxn;

    if (transferCompletionCallbackFxn != NULL)
    {
        config.transferCompletionCallbackFxnArg = (uintptr_t)transferCompletionCallbackFxnArg;
    }

    if ((errorCode = EDMA_configChannel(handle, &config, true)) != EDMA_NO_ERROR)
    {
        demo_printf("Error: EDMA_configChannel() failed with error code = %d\n", errorCode);
        goto exit;
    }

    errorCode = EDMA_setup_shadow_link(handle, chId, linkChId,
                                       &config.paramSetConfig, config.transferCompletionCallbackFxn, transferCompletionCallbackFxnArg);

exit:
    return (errorCode);
}

int32_t EDMAutil_configHwaOneHotSignature(EDMA_Handle handle,
                                          uint8_t chId, bool isEventTriggered,
                                          uint32_t *pSrcAddress, uint32_t *pDestAddress,
                                          uint16_t aCount, uint16_t bCount, uint16_t cCount,
                                          uint16_t linkChId)
{
    EDMA_channelConfig_t config;
    int32_t errorCode = EDMA_NO_ERROR;

    config.channelId = chId;
    config.channelType = (uint8_t)EDMA3_CHANNEL_TYPE_DMA;
    config.paramId = chId;
    config.eventQueueId = 0;

    config.paramSetConfig.sourceAddress = (uint32_t)pSrcAddress;
    config.paramSetConfig.destinationAddress = (uint32_t)pDestAddress;

    config.paramSetConfig.aCount = aCount;
    config.paramSetConfig.bCount = bCount;
    config.paramSetConfig.cCount = cCount;
    config.paramSetConfig.bCountReload = config.paramSetConfig.bCount;

    config.paramSetConfig.sourceBindex = 0;
    config.paramSetConfig.destinationBindex = 0;

    config.paramSetConfig.sourceCindex = 0;
    config.paramSetConfig.destinationCindex = 0;

    config.paramSetConfig.linkAddress = EDMA_NULL_LINK_ADDRESS;
    config.paramSetConfig.transferType = (uint8_t)EDMA3_SYNC_A;
    config.paramSetConfig.transferCompletionCode = 0;
    config.paramSetConfig.sourceAddressingMode = (uint8_t)EDMA3_ADDRESSING_MODE_LINEAR;
    config.paramSetConfig.destinationAddressingMode = (uint8_t)EDMA3_ADDRESSING_MODE_LINEAR;

    /* don't care because of linear addressing modes above */
    config.paramSetConfig.fifoWidth = (uint8_t)EDMA3_FIFO_WIDTH_8BIT;

    config.paramSetConfig.isStaticSet = false;
    config.paramSetConfig.isEarlyCompletion = false;
    config.paramSetConfig.isFinalTransferInterruptEnabled = false;
    config.paramSetConfig.isIntermediateTransferInterruptEnabled = false;
    config.paramSetConfig.isFinalChainingEnabled = false;
    config.paramSetConfig.isIntermediateChainingEnabled = false;
    config.transferCompletionCallbackFxn = NULL;

    if ((errorCode = EDMA_configChannel(handle, &config, isEventTriggered)) != EDMA_NO_ERROR)
    {
        demo_printf("Error: EDMA_configChannel() failed with error code = %d\n", errorCode);
        goto exit;
    }

    errorCode = EDMA_setup_shadow_link(handle, chId, linkChId,
                                       &config.paramSetConfig, config.transferCompletionCallbackFxn, NULL);

exit:
    return (errorCode);
}

int32_t EDMAutil_configHwaContiguous(EDMA_Handle handle,
                                     uint8_t chId, bool isEventTriggered,
                                     uint8_t linkChId, uint8_t chainChId,
                                     uint32_t *pSrcAddress, uint32_t *pDestAddress,
                                     uint16_t numBytes, uint16_t numBlocks,
                                     uint16_t srcIncrBytes, uint16_t dstIncrBytes,
                                     bool isIntermediateChainingEnabled,
                                     bool isFinalChainingEnabled,
                                     bool isTransferCompletionEnabled,
                                     EDMA_transferCompletionCallbackFxn_t transferCompletionCallbackFxn,
                                     uintptr_t transferCompletionCallbackFxnArg)
{
    EDMA_channelConfig_t config;
    int32_t errorCode = EDMA_NO_ERROR;

    config.channelId = chId;
    config.channelType = (uint8_t)EDMA3_CHANNEL_TYPE_DMA;
    config.paramId = chId;
    config.eventQueueId = 0;

    config.paramSetConfig.sourceAddress = (uint32_t)pSrcAddress;
    config.paramSetConfig.destinationAddress = (uint32_t)pDestAddress;

    config.paramSetConfig.aCount = numBytes;
    config.paramSetConfig.bCount = numBlocks;
    config.paramSetConfig.cCount = 1;
    config.paramSetConfig.bCountReload = config.paramSetConfig.bCount;

    config.paramSetConfig.sourceBindex = srcIncrBytes;
    config.paramSetConfig.destinationBindex = dstIncrBytes;

    config.paramSetConfig.sourceCindex = 0;
    config.paramSetConfig.destinationCindex = 0;

    config.paramSetConfig.linkAddress = EDMA_NULL_LINK_ADDRESS;
    config.paramSetConfig.transferType = (uint8_t)EDMA3_SYNC_A;
    config.paramSetConfig.transferCompletionCode = chainChId;
    config.paramSetConfig.sourceAddressingMode = (uint8_t)EDMA3_ADDRESSING_MODE_LINEAR;
    config.paramSetConfig.destinationAddressingMode = (uint8_t)EDMA3_ADDRESSING_MODE_LINEAR;

    /* don't care because of linear addressing modes above */
    config.paramSetConfig.fifoWidth = (uint8_t)EDMA3_FIFO_WIDTH_8BIT;

    config.paramSetConfig.isStaticSet = false;
    config.paramSetConfig.isEarlyCompletion = false;
    config.paramSetConfig.isFinalTransferInterruptEnabled =
        isTransferCompletionEnabled;
    config.paramSetConfig.isIntermediateTransferInterruptEnabled = false;
    config.paramSetConfig.isFinalChainingEnabled =
        isFinalChainingEnabled;
    config.paramSetConfig.isIntermediateChainingEnabled =
        isIntermediateChainingEnabled;
    config.transferCompletionCallbackFxn = transferCompletionCallbackFxn;

    if (transferCompletionCallbackFxn != NULL)
    {
        config.transferCompletionCallbackFxnArg = transferCompletionCallbackFxnArg;
    }

    if ((errorCode = EDMA_configChannel(handle, &config, isEventTriggered)) != EDMA_NO_ERROR)
    {
        //System_printf("Error: EDMA_configChannel() failed with error code = %d\n", errorCode);
        // MmwDemo_debugAssert (0);
        goto exit;
    }

    errorCode = EDMA_setup_shadow_link(handle, chId, linkChId,
                                       &config.paramSetConfig, config.transferCompletionCallbackFxn, transferCompletionCallbackFxnArg);

exit:
    return (errorCode);
}

/**
 *  @b Description
 *  @n
 *      Configures all 1D processing related EDMA configuration.
 *
 *  @param[in] obj Pointer to data path object
 *  @retval EDMA error code, see EDMA API.
 */
int32_t demo_config1D_EDMA(Demo_DataPathObj *obj)
{
    int32_t errorCode = EDMA_NO_ERROR;
    EDMA_Handle handle = obj->edmaHandle;
    HWA_SrcDMAConfig dmaConfig;

    /* Ping configuration to transfer 1D FFT output from HWA to L3 RAM transposed */
    errorCode = EDMAutil_configHwaTranspose(handle,
                                            MMW_EDMA_1D_PING_CH_ID,                                                                               //chId,
                                            MMW_EDMA_1D_PING_SHADOW_LINK_CH_ID,                                                                   //linkChId,
                                            MMW_EDMA_1D_PING_CHAIN_CH_ID,                                                                         //chainChId,
                                            (uint32_t *)SOC_translateAddress((uint32_t)MMW_HWA_1D_OUT_PING, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pSrcAddress,
                                            (uint32_t *)SOC_translateAddress((uint32_t)obj->radarCube, SOC_TranslateAddr_Dir_TO_EDMA, NULL),      //*pDestAddress,
                                            obj->numRxAntennas,                                                                                   //numAnt,
                                            obj->numRangeBins,                                                                                    //numRangeBins,
                                            obj->numChirpsPerFrame,                                                                               //numChirpsPerFrame,
                                            true,                                                                                                 //isIntermediateChainingEnabled,
                                            true,                                                                                                 //isFinalChainingEnabled,
                                            false,                                                                                                //isTransferCompletionEnabled
                                            NULL,                                                                                                 //transferCompletionCallbackFxn
                                            NULL);

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

    /* Pong configuration to transfer 1D FFT output from HWA to L3 RAM transposed */
    errorCode = EDMAutil_configHwaTranspose(handle,
                                            MMW_EDMA_1D_PONG_CH_ID,                                                                                                 //chId,
                                            MMW_EDMA_1D_PONG_SHADOW_LINK_CH_ID,                                                                                     //linkChId,
                                            MMW_EDMA_1D_PONG_CHAIN_CH_ID,                                                                                           //SR_DBT_xxx //chainChId,
                                            (uint32_t *)SOC_translateAddress((uint32_t)MMW_HWA_1D_OUT_PONG, SOC_TranslateAddr_Dir_TO_EDMA, NULL),                   //*pSrcAddress,
                                            (uint32_t *)SOC_translateAddress((uint32_t)(obj->radarCube + obj->numRxAntennas), SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pDestAddress,
                                            obj->numRxAntennas,                                                                                                     //numAnt,
                                            obj->numRangeBins,                                                                                                      //numRangeBins,
                                            obj->numChirpsPerFrame,                                                                                                 //numChirpsPerFrame,
                                            true,                                                                                                                   //isIntermediateChainingEnabled,
                                            true,                                                                                                                   //isFinalChainingEnabled,
                                            true,                                                                                                                   //isTransferCompletionEnabled
                                            demo_EDMA_transferCompletionCallbackFxn,                                                                                //transferCompletionCallbackFxn
                                            (uintptr_t)obj);

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

    /**************************************************************************
      *  PROGRAM EDMA2_ACC_CHAN0 (resp. EDMA2_ACC_CHAN1) to communicate completion
         of DMA CHAN EDMA_TPCC0_REQ_HWACC_0 (resp. EDMA_TPCC0_REQ_HWACC_1)
      *************************************************************************/

    HWA_getDMAconfig(obj->hwaHandle, MMW_HWA_DMA_TRIGGER_SOURCE_1D_PING, &dmaConfig);

    errorCode = EDMAutil_configHwaOneHotSignature(handle,
                                                  MMW_EDMA_1D_PING_CHAIN_CH_ID,                                                              //chId,
                                                  false,                                                                                     //isEventTriggered
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.srcAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL),  //pSrcAddress
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.destAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //pDestAddress
                                                  dmaConfig.aCnt,
                                                  dmaConfig.bCnt,
                                                  dmaConfig.cCnt,
                                                  MMW_EDMA_1D_PING_ONE_HOT_SHADOW_LINK_CH_ID); //linkChId

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

    HWA_getDMAconfig(obj->hwaHandle, MMW_HWA_DMA_TRIGGER_SOURCE_1D_PONG, &dmaConfig);

    errorCode = EDMAutil_configHwaOneHotSignature(handle,
                                                  MMW_EDMA_1D_PONG_CHAIN_CH_ID,                                                              //chId,
                                                  false,                                                                                     //isEventTriggered
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.srcAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL),  //pSrcAddress
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.destAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //pDestAddress
                                                  dmaConfig.aCnt,
                                                  dmaConfig.bCnt,
                                                  dmaConfig.cCnt,
                                                  MMW_EDMA_1D_PONG_ONE_HOT_SHADOW_LINK_CH_ID); //linkChId

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

exit:
    return (errorCode);
}

int32_t MmwDemo_configCFAR_EDMA(Demo_DataPathObj *obj)
{
    int32_t errorCode = EDMA_NO_ERROR;
    EDMA_Handle handle = obj->edmaHandle;
    HWA_SrcDMAConfig dmaConfig;
    uint16_t *const ptrFFT_Abs_16b = MY_OWN_BASE_ADDRESS(FFT_PROCESS_16b_OFFSET);      //长距离情况下的配置
    uint16_t *const ptrCfar_RefLevel = MY_OWN_BASE_ADDRESS(CFAR_REF_LEVEL_FAR_OFFSET); //CFAR输出结果

    //program DMA to MOVE DATA FROM L3_DETECTION TO ACCEL, THIS SHOULD BE LINKED TO DMA2ACC
    errorCode = EDMAutil_configHwaContiguous(handle,
                                             MMW_EDMA_CFAR_INP_CH_ID,                                                                           //chId,
                                             false,                                                                                             //isEventTriggered
                                             MMW_EDMA_CFAR_INP_SHADOW_LINK_CH_ID1,                                                              //linkChId,
                                             MMW_EDMA_CFAR_INP_CHAIN_CH_ID,                                                                     //chainChId,
                                             (uint32_t *)SOC_translateAddress((uint32_t)ptrFFT_Abs_16b, SOC_TranslateAddr_Dir_TO_EDMA, NULL),   //*pSrcAddress,
                                             (uint32_t *)SOC_translateAddress((uint32_t)MMW_HWA_CFAR_INP, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pDestAddress,
                                             4096 * sizeof(uint16_t),                                                                           //numBytes,
                                             1,                                                                                                 //numBlocks,
                                             0,                                                                                                 //srcIncrBytes,
                                             0,                                                                                                 //dstIncrBytes,
                                             false,                                                                                             //isIntermediateChainingEnabled,
                                             true,                                                                                              //isFinalChainingEnabled,
                                             false,                                                                                             //isTransferCompletionEnabled;
                                             NULL,                                                                                              //transferCompletionCallbackFxn
                                             NULL);

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

    HWA_getDMAconfig(obj->hwaHandle, MMW_HWA_DMA_TRIGGER_SOURCE_CFAR, &dmaConfig);

    errorCode = EDMAutil_configHwaOneHotSignature(handle,
                                                  MMW_EDMA_CFAR_INP_CHAIN_CH_ID,                                                             //chId,
                                                  false,                                                                                     //isEventTriggered
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.srcAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL),  //pSrcAddress
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.destAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //pDestAddress
                                                  dmaConfig.aCnt,
                                                  dmaConfig.bCnt,
                                                  dmaConfig.cCnt,
                                                  MMW_EDMA_CFAR_INP_SHADOW_LINK_CH_ID2); //linkChId

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

    //another DMA to move data from ACC to L3_OBJECT LIN
    errorCode = EDMAutil_configHwaContiguous(handle,
                                             MMW_EDMA_CFAR_OUT_CH_ID,                                                                           //chId,
                                             true,                                                                                              //isEventTriggered
                                             MMW_EDMA_CFAR_OUT_SHADOW_LINK_CH_ID1,                                                              //linkChId,
                                             MMW_EDMA_CFAR_OUT_CHAIN_CH_ID,                                                                     //chainChId,
                                             (uint32_t *)SOC_translateAddress((uint32_t)MMW_HWA_CFAR_OUT, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pSrcAddress,
                                             (uint32_t *)SOC_translateAddress((uint32_t)ptrCfar_RefLevel, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pDestAddress,
                                             4096 * sizeof(uint16_t),                                                                           //numBytes
                                             // 4096 * sizeof(cfarDetOutput_t), //numBytes
                                             1,                                       //numBlocks,
                                             0,                                       //srcIncrBytes,
                                             0,                                       //dstIncrBytes,
                                             false,                                   //isIntermediateChainingEnabled,
                                             false,                                   //isFinalChainingEnabled,
                                             true,                                    //isTransferCompletionEnabled;
                                             demo_EDMA_transferCompletionCallbackFxn, //transferCompletionCallbackFxn)
                                             (uintptr_t)obj);

exit:
    return (errorCode);
}

int32_t MmwDemo_configCFAR_EDMA_near(Demo_DataPathObj *obj)
{
    int32_t errorCode = EDMA_NO_ERROR;
    EDMA_Handle handle = obj->edmaHandle;
    HWA_SrcDMAConfig dmaConfig;
    int16_t *const ptrFFT_Abs_16b = MY_OWN_BASE_ADDRESS(FFT_ABS_START_OFFSET);       //长距离情况下的配置
    uint32_t *const ptrCfar_RefLevel = MY_OWN_BASE_ADDRESS(CFAR_NEAR_RESULT_OFFSET); //CFAR输出结果

    //program DMA to MOVE DATA FROM L3_DETECTION TO ACCEL, THIS SHOULD BE LINKED TO DMA2ACC
    errorCode = EDMAutil_configHwaContiguous(handle,
                                             MMW_EDMA_CFAR_INP_CH_ID,                                                                           //chId,
                                             false,                                                                                             //isEventTriggered
                                             MMW_EDMA_CFAR_INP_SHADOW_LINK_CH_ID1,                                                              //linkChId,
                                             MMW_EDMA_CFAR_INP_CHAIN_CH_ID,                                                                     //chainChId,
                                             (uint32_t *)SOC_translateAddress((uint32_t)ptrFFT_Abs_16b, SOC_TranslateAddr_Dir_TO_EDMA, NULL),   //*pSrcAddress,
                                             (uint32_t *)SOC_translateAddress((uint32_t)MMW_HWA_CFAR_INP, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pDestAddress,
                                             4096 * sizeof(int16_t),                                                                            //numBytes,
                                             1,                                                                                                 //numBlocks,
                                             0,                                                                                                 //srcIncrBytes,
                                             0,                                                                                                 //dstIncrBytes,
                                             false,                                                                                             //isIntermediateChainingEnabled,
                                             true,                                                                                              //isFinalChainingEnabled,
                                             false,                                                                                             //isTransferCompletionEnabled;
                                             NULL,                                                                                              //transferCompletionCallbackFxn
                                             NULL);

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

    HWA_getDMAconfig(obj->hwaHandle, MMW_HWA_DMA_TRIGGER_SOURCE_CFAR, &dmaConfig);

    errorCode = EDMAutil_configHwaOneHotSignature(handle,
                                                  MMW_EDMA_CFAR_INP_CHAIN_CH_ID,                                                             //chId,
                                                  false,                                                                                     //isEventTriggered
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.srcAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL),  //pSrcAddress
                                                  (uint32_t *)SOC_translateAddress(dmaConfig.destAddr, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //pDestAddress
                                                  dmaConfig.aCnt,
                                                  dmaConfig.bCnt,
                                                  dmaConfig.cCnt,
                                                  MMW_EDMA_CFAR_INP_SHADOW_LINK_CH_ID2); //linkChId

    if (errorCode != EDMA_NO_ERROR)
    {
        goto exit;
    }

    //another DMA to move data from ACC to L3_OBJECT LIN
    errorCode = EDMAutil_configHwaContiguous(handle,
                                             MMW_EDMA_CFAR_OUT_CH_ID,                                                                           //chId,
                                             true,                                                                                              //isEventTriggered
                                             MMW_EDMA_CFAR_OUT_SHADOW_LINK_CH_ID1,                                                              //linkChId,
                                             MMW_EDMA_CFAR_OUT_CHAIN_CH_ID,                                                                     //chainChId,
                                             (uint32_t *)SOC_translateAddress((uint32_t)MMW_HWA_CFAR_OUT, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pSrcAddress,
                                             (uint32_t *)SOC_translateAddress((uint32_t)ptrCfar_RefLevel, SOC_TranslateAddr_Dir_TO_EDMA, NULL), //*pDestAddress,
                                             MMW_MAX_OBJ_OUT * sizeof(uint32_t),                                                                //numBytes
                                             // 4096 * sizeof(cfarDetOutput_t), //numBytes
                                             1,                                       //numBlocks,
                                             0,                                       //srcIncrBytes,
                                             0,                                       //dstIncrBytes,
                                             false,                                   //isIntermediateChainingEnabled,
                                             false,                                   //isFinalChainingEnabled,
                                             true,                                    //isTransferCompletionEnabled;
                                             demo_EDMA_transferCompletionCallbackFxn, //transferCompletionCallbackFxn)
                                             (uintptr_t)obj);

exit:
    return (errorCode);
}

/**
 *  @b Description
 *  @n
 *      Trigger HWA for 1D processing.
 *  @param[in]  obj Pointer to data path obj.
 *  @retval none.
 */
void demo_dataPathTrigger1D(Demo_DataPathObj *obj)
{
    int32_t errCode;

    /* Enable the HWA */
    errCode = HWA_enable(obj->hwaHandle, 1); // set 1 to enable
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        demo_printf("Error: HWA_enable(1) returned %d\n", errCode);
        return;
    }

    /* trigger the HWA since triggerMode is set to DMA */
    errCode = HWA_setDMA2ACCManualTrig(obj->hwaHandle, MMW_HWA_DMA_TRIGGER_SOURCE_1D_PING);
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        demo_printf("Error: HWA_setDMA2ACCManualTrig(0) returned %d\n", errCode);
        return;
    }
    errCode = HWA_setDMA2ACCManualTrig(obj->hwaHandle, MMW_HWA_DMA_TRIGGER_SOURCE_1D_PONG);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_setDMA2ACCManualTrig(1) returned %d\n",errCode);
        //MmwDemo_debugAssert(0);
        demo_printf("Error: HWA_setDMA2ACCManualTrig(0) returned %d\n", errCode);
        return;
    }
}

/**
 *  @b Description
 *  @n
 *      Waits for 1D processing to finish. This is a blocking function.
 *
 *  @param[in] obj  Pointer to data path object
 *
 *  @retval
 *      NONE
 */
void demo_dataPathWait1D(Demo_DataPathObj *obj)
{
    Bool status;

    /**********************************************/
    /* WAIT FOR HWA NUMLOOPS INTERRUPT            */
    /**********************************************/
    /* wait for the all paramSets done interrupt */
    status = Semaphore_pend(obj->HWA_done_semHandle, BIOS_WAIT_FOREVER);
    if (status != TRUE)
    {
        datapath_err |= 0x01;

        demo_printf("Error: Semaphore_pend returned %d\n", status);
    }

    status = Semaphore_pend(obj->EDMA_1Ddone_semHandle, BIOS_WAIT_FOREVER);
    if (status != TRUE)
    {
        datapath_err |= 0x01;
    }
}

/**
 *  @b Description
 *  @n
 *      Configure HWA for 1D processing.
 *  @param[in]  obj Pointer to data path obj.
 *  @retval none.
 */
void demo_config1D_HWA(Demo_DataPathObj *obj, uint8_t windowEn)
{
    int32_t errCode;
    HWA_CommonConfig hwaCommonConfig;
    uint8_t hwaTriggerMode;

    /* Disable the HWA */
    errCode = HWA_enable(obj->hwaHandle, 0); // set 1 to enable
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        demo_printf("Error: HWA_enable(1) returned %d\n", errCode);
        return;
    }
    if (obj->dataPathMode == DATA_PATH_STANDALONE)
    {
        /* trigger manually and immediately */
        hwaTriggerMode = HWA_TRIG_MODE_SOFTWARE;
    }
    else
    {
        /* trigger done by ADC buffer */
        hwaTriggerMode = HWA_TRIG_MODE_DFE;
    }

    HWAutil_configRangeFFT(obj->hwaHandle,
                           MMW_HWA_START_POS_PARAMSETS_1D,
                           obj->numAdcSamples,
                           4096, //obj->numRangeBins,
                           obj->numRxAntennas,
                           MMW_HWA_WINDOWRAM_1D_OFFSET,
                           MMW_HWA_DMA_TRIGGER_SOURCE_1D_PING,
                           MMW_HWA_DMA_TRIGGER_SOURCE_1D_PONG,
                           MMW_HWA_DMA_DEST_CHANNEL_1D_PING,
                           MMW_HWA_DMA_DEST_CHANNEL_1D_PONG,
                           ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_ADCBUF_INP),
                           //  ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PING),
                           // ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_1D_OUT_PONG),
                           hwaTriggerMode,
                           windowEn);

    /***********************/
    /* HWA COMMON CONFIG   */
    /***********************/
    /* Config Common Registers */
    hwaCommonConfig.configMask = HWA_COMMONCONFIG_MASK_NUMLOOPS |
                                 HWA_COMMONCONFIG_MASK_PARAMSTARTIDX |
                                 HWA_COMMONCONFIG_MASK_PARAMSTOPIDX |
                                 HWA_COMMONCONFIG_MASK_FFT1DENABLE |
                                 HWA_COMMONCONFIG_MASK_INTERFERENCETHRESHOLD;
    hwaCommonConfig.numLoops = obj->numChirpsPerFrame / 2;
    hwaCommonConfig.paramStartIdx = MMW_HWA_START_POS_PARAMSETS_1D;
    hwaCommonConfig.paramStopIdx = MMW_HWA_START_POS_PARAMSETS_1D + HWAUTIL_NUM_PARAM_SETS_1D - 1;
    if (obj->dataPathMode == DATA_PATH_STANDALONE)
    {
        /* HWA will input data from M0 memory*/
        hwaCommonConfig.fftConfig.fft1DEnable = HWA_FEATURE_BIT_DISABLE;
    }
    else
    {
        /* HWA will input data from ADC buffer memory*/
        hwaCommonConfig.fftConfig.fft1DEnable = HWA_FEATURE_BIT_ENABLE;
    }
    hwaCommonConfig.fftConfig.interferenceThreshold = 0xFFFFFF;
    errCode = HWA_configCommon(obj->hwaHandle, &hwaCommonConfig);
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        demo_printf("Error: HWA_configCommon returned %d\n", errCode);
        return;
    }
}

/**
 *  @b Description
 *  @n
 *      Configures HWA for CFAR processing.
 *
 *  @param[in] obj  Pointer to data path object
 *
 *  @retval
 *      NONE
 */
void MmwDemo_configCFAR_HWA(Demo_DataPathObj *obj, CFAR_Config_t CFAR_Config)
{
    int32_t errCode;
    HWA_CommonConfig hwaCommonConfig;

    /* Disable the HWA */
    errCode = HWA_enable(obj->hwaHandle, 0); // set 1 to enable
    if (errCode != 0)
    {
        //System_printf("Error: HWA_enable(1) returned %d\n",errCode);
        // MmwDemo_debugAssert (0);
        return;
    }

    /**************************************/
    /* CFAR PROCESSING                    */
    /**************************************/
    if (CFAR_Config.outputMode == 0)
    {
        HWAutil_configCFAR(obj->hwaHandle,
                           HWAUTIL_NUM_PARAM_SETS_1D + HWAUTIL_NUM_PARAM_SETS_2D,
                           obj->numRangeBins,
                           1,                        //obj->numDopplerBins,
                           CFAR_Config.refLen,       //obj->cliCfg->cfarCfg.winLen, //检测窗长
                           CFAR_Config.guardLen,     //obj->cliCfg->cfarCfg.guardLen,//保护窗长
                           CFAR_Config.noiseDiv,     //obj->cliCfg->cfarCfg.noiseDivShift,
                           CFAR_Config.peakGroup,    //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
                           CFAR_Config.cycleMode,    //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
                           CFAR_Config.averangeMode, //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
                           4096,
                           MMW_HWA_DMA_TRIGGER_SOURCE_CFAR,
                           MMW_HWA_DMA_DEST_CHANNEL_CFAR,
                           ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_CFAR_INP), //hwaMemSourceOffset
                           ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_CFAR_OUT)  //hwaMemDestOffset
        );
    }
    else
    {
        HWAutil_configCFAR_near(obj->hwaHandle,
                                HWAUTIL_NUM_PARAM_SETS_1D + HWAUTIL_NUM_PARAM_SETS_2D,
                                obj->numRangeBins,
                                1,                        //obj->numDopplerBins,
                                CFAR_Config.refLen,       //obj->cliCfg->cfarCfg.winLen, //检测窗长
                                CFAR_Config.guardLen,     //obj->cliCfg->cfarCfg.guardLen,//保护窗长
                                CFAR_Config.noiseDiv,     //obj->cliCfg->cfarCfg.noiseDivShift,
                                CFAR_Config.peakGroup,    //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
                                CFAR_Config.cycleMode,    //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
                                CFAR_Config.averangeMode, //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
                                4096,
                                MMW_HWA_DMA_TRIGGER_SOURCE_CFAR,
                                MMW_HWA_DMA_DEST_CHANNEL_CFAR,
                                ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_CFAR_INP), //hwaMemSourceOffset
                                ADDR_TRANSLATE_CPU_TO_HWA(MMW_HWA_CFAR_OUT)  //hwaMemDestOffset
        );
    }

    /***********************/
    /* HWA COMMON CONFIG   */
    /***********************/
    /* Config Common Registers */
    hwaCommonConfig.configMask = HWA_COMMONCONFIG_MASK_NUMLOOPS |
                                 HWA_COMMONCONFIG_MASK_PARAMSTARTIDX |
                                 HWA_COMMONCONFIG_MASK_PARAMSTOPIDX |
                                 HWA_COMMONCONFIG_MASK_FFT1DENABLE |
                                 HWA_COMMONCONFIG_MASK_INTERFERENCETHRESHOLD |
                                 HWA_COMMONCONFIG_MASK_CFARTHRESHOLDSCALE;
    hwaCommonConfig.numLoops = 1;
    hwaCommonConfig.paramStartIdx = HWAUTIL_NUM_PARAM_SETS_1D + HWAUTIL_NUM_PARAM_SETS_2D;
    hwaCommonConfig.paramStopIdx = HWAUTIL_NUM_PARAM_SETS_1D + HWAUTIL_NUM_PARAM_SETS_2D;
    hwaCommonConfig.fftConfig.fft1DEnable = HWA_FEATURE_BIT_DISABLE;
    hwaCommonConfig.fftConfig.interferenceThreshold = 0xFFFFFF;
    hwaCommonConfig.cfarConfig.cfarThresholdScale = CFAR_Config.thresholdScale; //标称因子T

    errCode = HWA_configCommon(obj->hwaHandle, &hwaCommonConfig);
    if (errCode != 0)
    {
        //System_printf("Error: HWA_configCommon returned %d\n",errCode);
        // MmwDemo_debugAssert (0);
        return;
    }
}

/**
 *  @b Description
 *  @n
 *      Trigger CFAR processing.
 *
 *  @param[in] obj  Pointer to data path object
 *
 *  @retval
 *      NONE
 */
void MmwDemo_dataPathTriggerCFAR(Demo_DataPathObj *obj)
{
    int32_t errCode;

    /* Enable the HWA */
    errCode = HWA_enable(obj->hwaHandle, 1); // set 1 to enable
    if (errCode != 0)
    {
        //System_printf("Error: HWA_enable(1) returned %d\n",errCode);
        // MmwDemo_debugAssert (0);
        return;
    }

    EDMA_startTransfer(obj->edmaHandle, MMW_EDMA_CFAR_INP_CH_ID, EDMA3_CHANNEL_TYPE_DMA);
}

/**
 *  @b Description
 *  @n
 *      Waits for CFAR processing to finish. This is a blocking function.
 *
 *  @param[in] obj  Pointer to data path object
 *
 *  @retval
 *      NONE
 */
void MmwDemo_dataPathWaitCFAR(Demo_DataPathObj *obj)
{
    Bool status;

    /* then wait for the all paramSets done interrupt */
    status = Semaphore_pend(obj->HWA_done_semHandle, BIOS_WAIT_FOREVER);
    if (status != TRUE)
    {
        //System_printf("Error: Semaphore_pend returned %d\n",status);
        // MmwDemo_debugAssert (0);
        return;
    }

    /* wait for EDMA done */
    status = Semaphore_pend(obj->EDMA_CFARdone_semHandle, BIOS_WAIT_FOREVER);
    if (status != TRUE)
    {
        //System_printf("Error: Semaphore_pend returned %d\n",status);
        // MmwDemo_debugAssert (0);
        return;
    }
}

/**
 *  @b Description
 *  @n
 *      CFAR process chain.
 */
void MmwDemo_processCfar(Demo_DataPathObj *obj, uint16_t *numDetectedObjects)
{
    CFAR_Config_t CFAR_Config;
    switch (ParamNeedSave->ParamFromMcu.Transmit_Waveform)
    {
    case 0:                               //10m
        CFAR_Config.refLen = 64;          //obj->cliCfg->cfarCfg.winLen, //检测窗长
        CFAR_Config.guardLen = 2;         //obj->cliCfg->cfarCfg.guardLen,//保护窗长
        CFAR_Config.noiseDiv = 6;         //obj->cliCfg->cfarCfg.noiseDivShift,
        CFAR_Config.peakGroup = 1;        //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
        CFAR_Config.cycleMode = 0;        //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
        CFAR_Config.averangeMode = 0;     //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
        CFAR_Config.thresholdScale = 2.0; //标称因子T
        break;
    case 1:                               //35m
        CFAR_Config.refLen = 64;          //obj->cliCfg->cfarCfg.winLen, //检测窗长
        CFAR_Config.guardLen = 2;         //obj->cliCfg->cfarCfg.guardLen,//保护窗长
        CFAR_Config.noiseDiv = 6;         //obj->cliCfg->cfarCfg.noiseDivShift,
        CFAR_Config.peakGroup = 0;        //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
        CFAR_Config.cycleMode = 0;        //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
        CFAR_Config.averangeMode = 0;     //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
        CFAR_Config.thresholdScale = 2.0; //标称因子T
        break;
    case 2:                               //75m
        CFAR_Config.refLen = 32;          //obj->cliCfg->cfarCfg.winLen, //检测窗长
        CFAR_Config.guardLen = 3;         //obj->cliCfg->cfarCfg.guardLen,//保护窗长
        CFAR_Config.noiseDiv = 5;         //obj->cliCfg->cfarCfg.noiseDivShift,
        CFAR_Config.peakGroup = 0;        //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
        CFAR_Config.cycleMode = 0;        //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
        CFAR_Config.averangeMode = 0;     //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
        CFAR_Config.thresholdScale = 2.0; //标称因子T
        break;
    case 3:                               //150m
        CFAR_Config.refLen = 32;          //obj->cliCfg->cfarCfg.winLen, //检测窗长
        CFAR_Config.guardLen = 4;         //obj->cliCfg->cfarCfg.guardLen,//保护窗长
        CFAR_Config.noiseDiv = 5;         //obj->cliCfg->cfarCfg.noiseDivShift,
        CFAR_Config.peakGroup = 0;        //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
        CFAR_Config.cycleMode = 0;        //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
        CFAR_Config.averangeMode = 0;     //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
        CFAR_Config.thresholdScale = 2.6; //标称因子T
        break;
    default:                              //35m
        CFAR_Config.refLen = 64;          //obj->cliCfg->cfarCfg.winLen, //检测窗长
        CFAR_Config.guardLen = 2;         //obj->cliCfg->cfarCfg.guardLen,//保护窗长
        CFAR_Config.noiseDiv = 6;         //obj->cliCfg->cfarCfg.noiseDivShift,
        CFAR_Config.peakGroup = 0;        //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
        CFAR_Config.cycleMode = 0;        //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
        CFAR_Config.averangeMode = 0;     //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
        CFAR_Config.thresholdScale = 2.0; //标称因子T
        break;
    }

    CFAR_Config.outputMode = 0;
    MmwDemo_configCFAR_HWA(obj, CFAR_Config);
    MmwDemo_dataPathTriggerCFAR(obj);
    MmwDemo_dataPathWaitCFAR(obj);

    HWA_readCFARPeakCountReg(obj->hwaHandle,
                             (uint8_t *)numDetectedObjects, sizeof(uint16_t));
    obj->numHwaCfarDetections = *numDetectedObjects;
}

/**
 *  @b Description
 *  @n
 *      CFAR process chain.
 */
void MmwDemo_processCfar_near(Demo_DataPathObj *obj, uint16_t *numDetectedObjects)
{
    CFAR_Config_t CFAR_Config;

    CFAR_Config.refLen = 64;          //obj->cliCfg->cfarCfg.winLen, //检测窗长
    CFAR_Config.guardLen = 2;         //obj->cliCfg->cfarCfg.guardLen,//保护窗长
    CFAR_Config.noiseDiv = 6;         //obj->cliCfg->cfarCfg.noiseDivShift,
    CFAR_Config.peakGroup = 1;        //obj->cliCfg->peakGroupingCfg.inRangeDirectionEn,
    CFAR_Config.cycleMode = 0;        //obj->cliCfg->cfarCfg.cyclicMode, //1 环形检测
    CFAR_Config.averangeMode = 0;     //obj->cliCfg->cfarCfg.averageMode,//0 CA-CFAR
    CFAR_Config.thresholdScale = 2.0; //标称因子T

    CFAR_Config.outputMode = 1;
    MmwDemo_configCFAR_HWA(obj, CFAR_Config);
    MmwDemo_dataPathTriggerCFAR(obj);
    MmwDemo_dataPathWaitCFAR(obj);

    HWA_readCFARPeakCountReg(obj->hwaHandle,
                             (uint8_t *)numDetectedObjects, sizeof(uint16_t));
    obj->numHwaCfarDetections = *numDetectedObjects;
}

/**
 *  @b Description
 *  @n
 *  This function is called at the init time from @ref demo_initTask.
 *  It initializes drivers: ADCBUF, HWA, EDMA, and semaphores used
 *  by  @ref demo_dataPathTask
 *  @retval
 *      Not Applicable.
 */
void demo_dataPathInit(Demo_DataPathObj *obj)
{
    uint8_t edmaNumInstances, inst;
    Semaphore_Params semParams;
    int32_t errorCode;

    /* Initialize the ADCBUF */
    ADCBuf_init();

    /* Initialize the HWA */
    HWA_init();

    edmaNumInstances = EDMA_getNumInstances();
    for (inst = 0; inst < edmaNumInstances; inst++)
    {
        errorCode = EDMA_init(inst);
        if (errorCode != EDMA_NO_ERROR)
        {
            demo_printf("Debug: EDMA instance %d initialization returned error %d\n", errorCode);
            return;
        }
        demo_printf("Debug: EDMA instance %d has been initialized\n", inst);
    }

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

    Semaphore_Params_init(&semParams);
    semParams.mode = Semaphore_Mode_BINARY;
    obj->EDMA_1Ddone_semHandle = Semaphore_create(0, &semParams, NULL);

    /* Enable Done Interrupt */
    Semaphore_Params_init(&semParams);
    semParams.mode = Semaphore_Mode_BINARY;
    obj->HWA_done_semHandle = Semaphore_create(0, &semParams, NULL);

    Semaphore_Params_init(&semParams);
    semParams.mode = Semaphore_Mode_BINARY;
    obj->EDMA_CFARdone_semHandle = Semaphore_create(0, &semParams, NULL);
}

void demo_dataPathOpen(Demo_DataPathObj *obj)
{
    int32_t errCode;
    EDMA_instanceInfo_t edmaInstanceInfo;
    EDMA_errorConfig_t errorConfig;
    ADCBuf_Params ADCBufparams;

    /* Open the HWA Instance */
    obj->hwaHandle = HWA_open(0, gMmwMCB.socHandle, &errCode);
    if (obj->hwaHandle == NULL)
    {
        demo_printf("Error: Unable to open the HWA Instance err:%d\n", errCode);
        return;
    }

    demo_printf("Debug: HWA Instance %p has been opened successfully\n", obj->hwaHandle);

    obj->edmaHandle = EDMA_open(0, &errCode, &edmaInstanceInfo);

    if (obj->edmaHandle == NULL)
    {
        demo_printf("Error: Unable to open the EDMA Instance err:%d\n", errCode);
        return;
    }
    demo_printf("Debug: EDMA Instance %p has been opened successfully\n", obj->edmaHandle);

    errorConfig.isConfigAllEventQueues = true;
    errorConfig.isConfigAllTransferControllers = true;
    errorConfig.isEventQueueThresholdingEnabled = true;
    errorConfig.eventQueueThreshold = EDMA_EVENT_QUEUE_THRESHOLD_MAX;
    errorConfig.isEnableAllTransferControllerErrors = true;
    errorConfig.callbackFxn = demo_EDMA_errorCallbackFxn;
    errorConfig.transferControllerCallbackFxn = demo_EDMA_transferControllerErrorCallbackFxn;
    if ((errCode = EDMA_configErrorMonitoring(obj->edmaHandle, &errorConfig)) != EDMA_NO_ERROR)
    {
        demo_printf("Debug: EDMA_configErrorMonitoring() failed with errorCode = %d\n", errCode);
        return;
    }

    /*****************************************************************************
     * Start ADCBUF driver:
     *****************************************************************************/
    /* ADCBUF Params initialize */
    ADCBuf_Params_init(&ADCBufparams);
    ADCBufparams.chirpThreshold = 1;
    ADCBufparams.continousMode = 0;

    /* Open ADCBUF driver */
    obj->adcbufHandle = ADCBuf_open(0, &ADCBufparams);
    if (obj->adcbufHandle == NULL)
    {
        demo_printf("Error: Unable to open the ADCBUF driver\n");
        return;
    }
    demo_printf("Debug: ADCBUF Instance(0) %p has been opened successfully\n", obj->adcbufHandle);
}

/**
 *  @b Description
 *  @n
 *      Common configuration of data path processing, required to be done only
 *      once. Configures all of EDMA and window RAM of HWA.
 *  @param[in]  obj Pointer to data path obj.
 *  @retval none.
 */
void demo_dataPathConfigCommon(Demo_DataPathObj *obj)
{
    int32_t errCode;
    uint32_t fftWindow;
    uint32_t winLen, winIndx, hwaRamOffset;
    float phi;
    HWA_CommonConfig hwaCommonConfig;

    demo_config1D_EDMA(obj);
    MmwDemo_configCFAR_EDMA(obj);

    /**********************************************/
    /* Disable HWA and reset to known state       */
    /**********************************************/

    /* Disable the HWA */
    errCode = HWA_enable(obj->hwaHandle, 0);
    if (errCode != 0)
    {
        demo_printf("Error: demo_dataPathConfigCommon:HWA_enable(0) returned %d\n", errCode);
        return;
    }

    /* Reset the internal state of the HWA */
    errCode = HWA_reset(obj->hwaHandle);
    if (errCode != 0)
    {
        demo_printf("Error: demo_dataPathConfigCommon:HWA_reset returned %d\n", errCode);
        return;
    }

    /***********************/
    /* CONFIG WINDOW RAM   */
    /***********************/
    /* if windowing is enabled, load the window coefficients in RAM */
    //1D-FFT window

    hwaRamOffset = 0;
    winLen = obj->numAdcSamples / 4;
    phi = 2 * PI_ / ((float)winLen - 1);
    for (winIndx = 0; winIndx < winLen; winIndx++)
    {
        demo_genWindow(&fftWindow, winIndx, phi, I_WANT_USE_THIS_WIN);
        errCode = HWA_configRam(obj->hwaHandle,
                                HWA_RAM_TYPE_WINDOW_RAM,
                                (uint8_t *)&fftWindow,
                                sizeof(uint32_t), //size in bytes
                                hwaRamOffset);    //offset in bytes
        if (errCode != 0)
        {
            //System_printf("Error: HWA_configRam returned %d\n",errCode);
            return;
        }
        hwaRamOffset += sizeof(uint32_t);
    }

    /**********************************************/
    /* ENABLE NUMLOOPS DONE INTERRUPT FROM HWA */
    /**********************************************/
    errCode = HWA_enableDoneInterrupt(obj->hwaHandle,
                                      demo_dataPathHwaDoneIsrCallback,
                                      obj->HWA_done_semHandle);
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        demo_printf("Error: HWA_enableDoneInterrupt returned %d\n", errCode);
        return;
    }

    /***********************************************/
    /* ENABLE FFT Twiddle coefficient dithering    */
    /***********************************************/
    /* Config Common Registers */
    hwaCommonConfig.configMask = HWA_COMMONCONFIG_MASK_TWIDDITHERENABLE |
                                 HWA_COMMONCONFIG_MASK_LFSRSEED;
    hwaCommonConfig.fftConfig.twidDitherEnable = HWA_FEATURE_BIT_ENABLE;
    hwaCommonConfig.fftConfig.lfsrSeed = 0x1234567; /*Some non-zero value*/

    errCode = HWA_configCommon(obj->hwaHandle, &hwaCommonConfig);
    if (errCode != 0)
    {
        //retCode = HWA_TEST_ERROR;
        demo_printf("Error: HWA_configCommon returned %d\n", errCode);
        return;
    }
}

/**
 *  @b Description
 *  @n
 *      The function is used to configure the data path based on the chirp profile.
 *      After this function is executed, the data path processing will ready to go
 *      when the ADC buffer starts receiving samples corresponding to the chirps.
 *
 *  @retval
 *      Not Applicable.
 */
void demo_dataPathConfig(void)
{
    ADCBuf_dataFormat dataFormat;
    ADCBuf_RxChanConf rxChanConf;
    uint32_t chirpThreshold;
    uint8_t channel;
    int32_t retVal = 0;
    uint8_t numRxAntennas = 0;
    uint8_t numBytePerSample = 0;
    Demo_ADCBufCfg *ptrAdcbufCfg;
    Demo_DataPathObj *dataPathObj = &gMmwMCB.dataPathObj;

    ptrAdcbufCfg = &gMmwMCB.adcbufCfg;

    /* Calculate the DMA transfer parameters */
    if (ptrAdcbufCfg->adcFmt == 0)
    {
        /* Complex dataFormat has 4 bytes */
        numBytePerSample = 4;
    }
    else
    {
        /* Real dataFormat has 2 bytes */
        numBytePerSample = 2;
    }

    /* Configure ADC buffer data format */
    dataFormat.adcOutFormat = ptrAdcbufCfg->adcFmt;
    dataFormat.sampleInterleave = ptrAdcbufCfg->iqSwapSel;
    dataFormat.channelInterleave = ptrAdcbufCfg->chInterleave;

    /* Debug Message: */
    demo_printf("Debug: Start ADCBuf driver dataFormat=%d, sampleSwap=%d, interleave=%d, chirpThreshold=%d\n",
                dataFormat.adcOutFormat, dataFormat.sampleInterleave, dataFormat.channelInterleave,
                ptrAdcbufCfg->chirpThreshold);

    retVal = ADCBuf_control(dataPathObj->adcbufHandle, ADCBufMMWave_CMD_CONF_DATA_FORMAT,
                            (void *)&dataFormat);
    if (retVal < 0)
    {
        demo_printf("Error: Unable to configure the data formats\n");
        return;
    }

    memset((void *)&rxChanConf, 0, sizeof(ADCBuf_RxChanConf));

    /* Enable Rx Channels. */
    for (channel = 0; channel < SYS_COMMON_NUM_RX_CHANNEL; channel++)
    {
        if (gMmwMCB.openCfg.chCfg.rxChannelEn & (0x1 << channel))
        {
            /* Populate the receive channel configuration: */
            rxChanConf.channel = channel;
            retVal = ADCBuf_control(dataPathObj->adcbufHandle, ADCBufMMWave_CMD_CHANNEL_ENABLE,
                                    (void *)&rxChanConf);
            if (retVal < 0)
            {
                demo_printf("Error: ADCBuf Control for Channel %d Failed\n");
                return;
            }

            rxChanConf.offset += dataPathObj->numAdcSamples * numBytePerSample;

            /* Track the number of receive channels: */
            numRxAntennas++;
        }
    }

    /* Save number of channels */
    dataPathObj->numRxAntennas = numRxAntennas;

    /* Set the chirp threshold: */
    chirpThreshold = ptrAdcbufCfg->chirpThreshold;
    retVal = ADCBuf_control(dataPathObj->adcbufHandle, ADCBufMMWave_CMD_SET_CHIRP_THRESHHOLD,
                            (void *)&chirpThreshold);
    if (retVal < 0)
    {
        demo_printf("Error: ADCbuf Chirp Threshold Failed with Error[%d]\n", retVal);
        return;
    }

    uint32_t numTxAntAzim = 0;
    uint32_t numTxAntElev = 0;
    dataPathObj->numTxAntennas = 0;
    if (gMmwMCB.openCfg.chCfg.txChannelEn & 0x1)
    {
        numTxAntAzim++;
    }
    if (gMmwMCB.openCfg.chCfg.txChannelEn & 0x4)
    {
        numTxAntAzim++;
    }
    if (gMmwMCB.openCfg.chCfg.txChannelEn & 0x2)
    {
        numTxAntElev++;
    }

    dataPathObj->numTxAntennas = numTxAntAzim + numTxAntElev;

    dataPathObj->numVirtualAntAzim = numTxAntAzim * dataPathObj->numRxAntennas;
    dataPathObj->numVirtualAntElev = numTxAntElev * dataPathObj->numRxAntennas;
    dataPathObj->numVirtualAntennas = dataPathObj->numVirtualAntAzim + dataPathObj->numVirtualAntElev;

    dataPathObj->numAdcSamples = gMmwMCB.profileCfg.numAdcSamples;
    dataPathObj->numRangeBins = 4096;
    dataPathObj->numChirpsPerFrame = (gMmwMCB.ctrlCfg.u.frameCfg.frameCfg.chirpEndIdx -
                                      gMmwMCB.ctrlCfg.u.frameCfg.frameCfg.chirpStartIdx + 1) *
                                     gMmwMCB.ctrlCfg.u.frameCfg.frameCfg.numLoops;
    dataPathObj->numDopplerBins = 1;

    dataPathObj->rangeResolution = DEMOCFG_SPEED_OF_LIGHT_IN_METERS * gMmwMCB.profileCfg.digOutSampleRate * 1e3 /
                                   (2 * gMmwMCB.profileCfg.freqSlopeConst * ((3.6 * 1e3 * 900) / (1U << 26)) * 1e12 * dataPathObj->numRangeBins);

    demo_memPoolReset(&gMmwL3heap);

    dataPathObj->radarCube = (uint32_t *)demo_memPoolAlloc(&gMmwL3heap, dataPathObj->numRangeBins *
                                                                            dataPathObj->numDopplerBins * dataPathObj->numVirtualAntennas * sizeof(uint32_t));

    // dataPathObj->cfarDetectionOut = (cfarDetOutput_t *) demo_memPoolAlloc(&gMmwL3heap, MMW_MAX_OBJ_OUT * sizeof(cfarDetOutput_t));
    // dataPathObj->cfarDetectionOut = MY_OWN_BASE_ADDRESS(CFAR_REF_LEVEL_FAR_OFFSET);

#ifdef RANGE_DC_CALIBRATION
    if (obj->dcRangeSigMean == NULL)
    {
        obj->dcRangeSigMean = (cmplx32ImRe_t *)Memory_alloc(NULL,
                                                            DC_RANGE_SIGNATURE_COMP_MAX_BIN_SIZE * SOC_MAX_NUM_TX_ANTENNAS * SOC_MAX_NUM_RX_ANTENNAS * sizeof(cmplx32ImRe_t),
                                                            (uint8_t)MMWDEMO_MEMORY_ALLOC_DOUBLE_WORD_ALIGN,
                                                            NULL);
    }
#endif

    demo_dataPathConfigCommon(dataPathObj);
    demo_config1D_HWA(dataPathObj, 1);
    demo_dataPathTrigger1D(dataPathObj);

    return;
}

#ifdef CZ_BOARD
//This is a capture of CZ board full cal.
uint32_t cz_chunk1[56] = {
    0x800017FE, 0x800017FE, 0x0000000F, 0x000017FE,
    0x001D001D, 0x00000002, 0x0000001F, 0x2C181010,
    0x00000000, 0x00000000, 0x00000000, 0xFFFE0114,
    0x00001611, 0x00003401, 0x0112A880, 0x00000000,
    0x00001C01, 0x01C9C380, 0x00000000, 0x00062C01,
    0x0146F95B, 0x00000192, 0x00120901, 0x017D7840,
    0x000001C3, 0x0157001D, 0x011A0136, 0x00ED0101,
    0x558E38E4, 0x58E38E38, 0x00000000, 0x00000000,
    0x00000000, 0x100A0476, 0x00000000, 0x00000000,
    0x00000000, 0xF0FA03F9, 0x00000000, 0x00000000,
    0x00000000, 0xFF3E0412, 0x00000000, 0x00000000,
    0x00000000, 0xFE3003BD, 0x1912001D, 0x3D302321,
    0x0003FFCC, 0x004D0043, 0x007E0071, 0x0008FFD6,
    0x004D0044, 0x007D0070, 0x00000000, 0x00000000};

uint32_t cz_chunk2[56] = {
    0x00000000, 0x00000000, 0x00000000, 0x00000000,
    0x00000000, 0x00000000, 0x00000000, 0x00000000,
    0x00000000, 0x00000000, 0x000F0001, 0xFFF30140,
    0x000B0142, 0x00020140, 0xFFFF0140, 0x00050142,
    0xFFF80140, 0x000F0102, 0x000B0140, 0x00190144,
    0x001D0140, 0xFFFF0142, 0x00180142, 0xFFEF0142,
    0x000C0140, 0x001A0102, 0xFFFF0144, 0xFFE90141,
    0x00000140, 0x0012014A, 0x00100102, 0xFFFD0106,
    0xFFF1010F, 0xFFE70104, 0x00100108, 0xFFF8010A,
    0xFFFE0148, 0xFFFC0140, 0xFFF5015C, 0x001D010A,
    0x00140104, 0x00010142, 0xFFFF0106, 0xFFF90103,
    0xFFE90108, 0xFFE6010A, 0x000C0154, 0x001B0108,
    0x00070150, 0x001D0140, 0xFFFF0142, 0xFFFF0102,
    0x001B0106, 0xFFFB014C, 0xFFFB0140, 0x001B010A};

uint32_t cz_chunk3[56] = {
    0x001D0140, 0xFFE50104, 0x00000154, 0x000D0151,
    0x00040102, 0xFFF50102, 0xFFF90144, 0x00160140,
    0xFFF9014E, 0xFFE30146, 0x0017014A, 0x001D0144,
    0x000006CF, 0x00000000, 0x00000000, 0x06D40000,
    0x00000000, 0x00000000, 0x00000000, 0x00000699,
    0x00000000, 0x00000000, 0x06D50000, 0x00000000,
    0x00000000, 0x00000000, 0x00000000, 0x06BC0000,
    0x06CC06C9, 0x069606DE, 0x06D706AB, 0x024A06E8,
    0x08040F06, 0x5C40480A, 0x0642040A, 0x540A0803,
    0x42405008, 0x404C0602, 0x5404400A, 0x44020251,
    0x4A464E40, 0x00000044, 0x00000000, 0x00000000,
    0x00000000, 0x00000000, 0x00000000, 0x00000000,
    0x00000000, 0x00000000, 0x00000000, 0x00000000,
    0x00000000, 0x00000000, 0x00000000, 0x00000000};
#endif

//This is a copy of MMWave_internalLinkSync().
static void demo_internalLinkSync(MMWave_MCB *ptrMMWaveMCB,
                                  uint32_t flag)
{
    int32_t completed = 0;

    /* Loop around till the condition is met: */
    while (completed == 0)
    {
        /* Is the flag set? */
        if (ptrMMWaveMCB->linkStatus & flag)
        {
            /* YES: Flag has been set. */
            completed = 1;
        }
    }
    return;
}

//This is a copy of MMWave_openLink. It is needed to control what calibration is
//performed, and whether to write stored calibration data back to the BSS.
#define MMWAVE_RF_OPERATIONAL 1

int32_t demo_openLink(MMWave_MCB *ptrMMWaveMCB,
                      MMWave_OpenCfg *openCfg,
                      uint32_t calMask,
                      rlCalibrationData_t *calData,
                      int32_t *errCode)
{
    int32_t retVal = -1;
    rlRfCalMonFreqLimitConf_t freqLimit;
    rlRfInitCalConf_t rfInitCalib;
    rlRfCalMonTimeUntConf_t timeCfg;
    rlRfLdoBypassCfg_t ldoCfg;

    /* Initialize the configurations: */
    memset((void *)&freqLimit, 0, sizeof(rlRfCalMonFreqLimitConf_t));
    memset((void *)&rfInitCalib, 0, sizeof(rlRfInitCalConf_t));
    memset((void *)&timeCfg, 0, sizeof(rlRfCalMonTimeUntConf_t));
    memset((void *)&ldoCfg, 0, sizeof(rlRfLdoBypassCfg_t));

    /* Link is not operational: */
    ptrMMWaveMCB->linkStatus = 0U;

    /* Setup the RF Calibration Time unit
     * - Our periodicity is set to 1 Frame  */
    timeCfg.calibMonTimeUnit = 1U;
    retVal = rlRfSetCalMonTimeUnitConfig(RL_DEVICE_MAP_INTERNAL_BSS, &timeCfg);
    if (retVal != RL_RET_CODE_OK)
    {
        /* Error: calibration init failed */
        *errCode = MMWAVE_ECALPERIOD;
        goto exit;
    }

    //Parameter for LDO Bypass. This is expressly linked to the
    //PMIC type and mode that it is functioning. If using the standard LP87524B PMIC keep
    //this parameter a 0. If using the installed LP87524J PMIC in PFM Mode (default configuration for high efficiency)
    //then you should also keep this a zero. If you are using in forced PWM mode then and only then
    //should this value be a 1. Setting this value to a 1 in any other circumstance will result in both device
    //and board damage.

    ldoCfg.ldoBypassEnable = 0; // 0 for No bypass (1.3 V, default), 1 for Yes Bypass (1.0 V).
#ifndef PMIC_MODE_PFM

#ifdef PMIC_TYPE_J
    ldoCfg.ldoBypassEnable = 1; // 0 for No bypass (1.3 V, default), 1 for Yes Bypass (1.0 V).
#endif
#endif

    retVal = rlRfSetLdoBypassConfig(RL_DEVICE_MAP_INTERNAL_BSS, &ldoCfg);

    /* Setup the RF Calibration Frequency limit as specified by the user configuration: */
    freqLimit.freqLimitLow = openCfg->freqLimitLow;
    freqLimit.freqLimitHigh = openCfg->freqLimitHigh;
    retVal = rlRfSetCalMonFreqLimitConfig(RL_DEVICE_MAP_INTERNAL_BSS, &freqLimit);
    if (retVal != RL_RET_CODE_OK)
    {
        /* Error: Frequency Limit Calibration  */
        *errCode = MMWAVE_ECALCFG;
        goto exit;
    }

    rfInitCalib.calibEnMask = calMask;

    retVal = rlRfInitCalibConfig(RL_DEVICE_MAP_INTERNAL_BSS, &rfInitCalib);
    if (retVal != RL_RET_CODE_OK)
    {
        /* Error: calibration init failed */
        *errCode = MMWAVE_ECALINIT;
        goto exit;
    }

    /* Set the channel configuration: */
    retVal = rlSetChannelConfig(RL_DEVICE_MAP_INTERNAL_BSS, &openCfg->chCfg);
    if (retVal != RL_RET_CODE_OK)
    {
        /* Error: Set the channel configuration failed */
        *errCode = MMWAVE_ECHCFG;
        goto exit;
    }

    /* Set the ADC Output configuration: */
    retVal = rlSetAdcOutConfig(RL_DEVICE_MAP_INTERNAL_BSS, &openCfg->adcOutCfg);
    if (retVal != RL_RET_CODE_OK)
    {
        /* Error: Set the ADC configuration failed */
        *errCode = MMWAVE_EADCCFG;
        goto exit;
    }

    /* Set the low power mode configuration */
    retVal = rlSetLowPowerModeConfig(RL_DEVICE_MAP_INTERNAL_BSS, &openCfg->lowPowerMode);
    if (retVal != RL_RET_CODE_OK)
    {
        /* Error: Set the Low power configuration failed */
        *errCode = MMWAVE_EPOWERCFG;
        goto exit;
    }

    /****************************************************************************************
     * Asynchronous event configuration:
     ****************************************************************************************/
    retVal = MMWave_deviceCfgAsyncEvent(ptrMMWaveMCB, errCode);
    if (retVal < 0)
    {
        /* Error: Asynchronous Event configuration failed; error code is already setup */
        goto exit;
    }

    //Write the calibration table to BSS if needed.
    if (calData != NULL)
    {
        retVal = rlRfCalibDataRestore(RL_DEVICE_MAP_INTERNAL_BSS, calData);
        if (retVal != RL_RET_CODE_OK)
        {
            /* Error: Calibration table restore failed */
            *errCode = MMWAVE_ERFINIT;
            goto exit;
        }

        //wait for the sync
        demo_internalLinkSync(ptrMMWaveMCB, MMWAVE_RF_OPERATIONAL);

        //clear the flag so it will wait again for rlRfInit
        ptrMMWaveMCB->linkStatus &= ~(MMWAVE_RF_OPERATIONAL);
    }

    /* Init RF: */
    retVal = rlRfInit(RL_DEVICE_MAP_INTERNAL_BSS);
    if (retVal != RL_RET_CODE_OK)
    {
        /* Error: RF Initialization failed */
        *errCode = MMWAVE_ERFINIT;
        goto exit;
    }

    /***************************************************************************
     * SYNCHRONIZATION: We need to loop around till the BSS has performed the
     * RF Initialization.
     ***************************************************************************/
    demo_internalLinkSync(ptrMMWaveMCB, MMWAVE_RF_OPERATIONAL);

    /* Setup the return value: */
    retVal = 0;

exit:
    return retVal;
}

//This function takes one of the pre-defined chirp configurations and loads
//it into the control structures with which to initialize the mmWave module.
int32_t demo_staticConfig(void)
{
    int32_t errCode;
    uint32_t flashAddr;
    MMWave_ProfileHandle profileHandle;
    MMWave_MCB *ptrMMWaveMCB;

#ifdef POWER_TEST
    //Power testing: set GPIO0 low:
    gpio_write(0, 0, 0);
    cptime[3] = Cycleprofiler_getTimeStamp();
#endif

    rlAdcOutCfg_t *adcOutCfg = &gMmwMCB.openCfg.adcOutCfg;
    Demo_ADCBufCfg *adcbufCfg = &gMmwMCB.adcbufCfg;
    rlProfileCfg_t *profileCfg = &gMmwMCB.profileCfg;
    rlLowPowerModeCfg_t *lowPwrMode = &gMmwMCB.openCfg.lowPowerMode;
    //  rlFrameCfg_t        *frameCfg   = &gMmwMCB.ctrlCfg.u.frameCfg.frameCfg;
    rlAdvFrameCfg_t *advFrameCfg = &gMmwMCB.ctrlCfg.u.advancedFrameCfg.frameCfg;
    //  rlSubFrameCfg_t     *subFrameCfg = &gMmwMCB.ctrlCfg.u.advancedFrameCfg.frameCfg.frameSeq.subFrameCfg[0];
    rlChanCfg_t *chCfg = &gMmwMCB.openCfg.chCfg;
    rlChirpCfg_t *chirpCfg = &gMmwMCB.chirpCfg;
    MMWave_CalibrationCfg *calCfg = &gMmwMCB.calCfg;
    Demo_DataPathObj *dataPathObj = &gMmwMCB.dataPathObj;
    rlSubFrameCfg_t subFrameCfg;
    /* Initialize the control configuration from the static config: */
    memset((void *)&gMmwMCB.ctrlCfg, 0, sizeof(MMWave_CtrlCfg));

    /* Populate the channel configuration: */
#ifndef CZ_BOARD
    chCfg->rxChannelEn = 0x01; //Rx enable mask
#else
    //FOR CAL TEMP TESTING (w/waveguide)
    chCfg->rxChannelEn = 0x08; //Rx enable mask
#endif
    chCfg->txChannelEn = 0x01; //Tx enable mask
    chCfg->cascading = 0;

    /* Populate the ADC output configuration: */
    adcOutCfg->fmt.b2AdcBits = 2;   //ADC_BITS_16
    adcOutCfg->fmt.b2AdcOutFmt = 1; //complex 1x

    /* Populate the low power configuration: */
    lowPwrMode->reserved = 0;
    lowPwrMode->lpAdcMode = 0;

    /* Setup the calibration frequency: */
    gMmwMCB.openCfg.freqLimitLow = DEMO_CAL_FREQ_LOW;
    gMmwMCB.openCfg.freqLimitHigh = DEMO_CAL_FREQ_HIGH;

    ptrMMWaveMCB = (MMWave_MCB *)gMmwMCB.ctrlHandle;

#ifdef POWER_TEST
    cptime[4] = Cycleprofiler_getTimeStamp();
#endif

#ifndef RUN_FULL_CAL
    //Read the init calibration table from flash.
    flashAddr = QSPIFlash_getExtFlashAddr(qspiflashHandle) +
                DEMO_CAL_FLASH_OFFSET;

    QSPIFlash_singleRead(qspiflashHandle, flashAddr, DEMO_CAL_FLASH_LEN,
                         (uint8_t *)init_cal_data);
#endif

#ifdef CZ_BOARD
    //For CZ board testing, load full cal data from C array:
    init_cal_data[0].numOfChunk = 3;
    init_cal_data[0].chunkId = 0;
    memcpy(&init_cal_data[0].calData, cz_chunk1, 56 * 4);
    init_cal_data[1].numOfChunk = 3;
    init_cal_data[1].chunkId = 1;
    memcpy(&init_cal_data[1].calData, cz_chunk2, 56 * 4);
    init_cal_data[2].numOfChunk = 3;
    init_cal_data[2].chunkId = 2;
    memcpy(&init_cal_data[2].calData, cz_chunk3, 56 * 4);
#endif

    /* Open the mmWave module: */

#ifdef RUN_FULL_CAL
    demo_openLink(ptrMMWaveMCB, &gMmwMCB.openCfg, 0x17F0, NULL, &errCode);
#else
    demo_openLink(ptrMMWaveMCB, &gMmwMCB.openCfg, 0x0010, init_cal_data, &errCode);
#endif
    if (errCode < 0)
    {
        demo_printf("Error: mmWave Open failed (code %d)\n", errCode);
        return -1;
    }

    /* The module has been opened successfully: */
    ptrMMWaveMCB->status = ptrMMWaveMCB->status | MMWAVE_STATUS_OPENED;

    /* Populate the profile configuration: */
    profileCfg->profileId = 0;
    profileCfg->pfVcoSelect = 0;
    profileCfg->pfCalLutUpdate = 0;
    profileCfg->startFreqConst = RF_Param->START_FREQCONST; //77 * 18641351; //(2^26) / 3.6;
    profileCfg->idleTimeConst = RF_Param->IDLE_TIME;
    profileCfg->adcStartTimeConst = RF_Param->ADC_START_TIME;
    profileCfg->rampEndTime = RF_Param->RAMP_END_TIME;
#ifndef CZ_BOARD
    profileCfg->txOutPowerBackoffCode = DEMOCFG_TxBackoff;
#else
    profileCfg->txOutPowerBackoffCode = 8;
#endif
    profileCfg->txPhaseShifter = 0;
    profileCfg->freqSlopeConst = RF_Param->FREQ_SLOPE;
    profileCfg->txStartTime = 100;
    profileCfg->numAdcSamples = RF_Param->NUM_ADC_SAMPLES;
    profileCfg->digOutSampleRate = RF_Param->SAMPLE_RATE;
    profileCfg->hpfCornerFreq1 = 0;
    profileCfg->hpfCornerFreq2 = 0;
#ifndef CZ_BOARD
    profileCfg->rxGain = RF_Param->RxLNAGAIN;
#else
    profileCfg->rxGain = 24;
#endif

    /* Populate the chirp configuration: */
    chirpCfg->chirpStartIdx = 0;
    chirpCfg->chirpEndIdx = 0;
    chirpCfg->profileId = 0;
    chirpCfg->startFreqVar = 0;
    chirpCfg->freqSlopeVar = 0;
    chirpCfg->idleTimeVar = 0;
    chirpCfg->adcStartTimeVar = 0;
    chirpCfg->txEnable = 1;

    //Create profile 0.
    profileHandle = MMWave_addProfile(gMmwMCB.ctrlHandle, profileCfg, &errCode);
    gMmwMCB.ctrlCfg.u.advancedFrameCfg.profileHandle[0] = profileHandle;

    //Add the chirp definition to the profile
    gMmwMCB.chirpHandle = MMWave_addChirp(gMmwMCB.ctrlCfg.u.advancedFrameCfg.profileHandle[0], chirpCfg, &errCode);

    /* Populate the profile 1 configuration: */
    profileCfg->profileId = 1;
    profileCfg->pfVcoSelect = 0;
    profileCfg->pfCalLutUpdate = 0;
    profileCfg->startFreqConst = RF_Param_10m.START_FREQCONST; //77 * 18641351; //(2^26) / 3.6;
    profileCfg->idleTimeConst = RF_Param_10m.IDLE_TIME;
    profileCfg->adcStartTimeConst = RF_Param_10m.ADC_START_TIME;
    profileCfg->rampEndTime = RF_Param_10m.RAMP_END_TIME;
#ifndef CZ_BOARD
    profileCfg->txOutPowerBackoffCode = DEMOCFG_TxBackoff;
#else
    profileCfg->txOutPowerBackoffCode = 8;
#endif
    profileCfg->txPhaseShifter = 0;
    profileCfg->freqSlopeConst = RF_Param_10m.FREQ_SLOPE;
    profileCfg->txStartTime = 100;
    profileCfg->numAdcSamples = RF_Param_10m.NUM_ADC_SAMPLES;
    profileCfg->digOutSampleRate = RF_Param_10m.SAMPLE_RATE;
    profileCfg->hpfCornerFreq1 = 0;
    profileCfg->hpfCornerFreq2 = 0;
#ifndef CZ_BOARD
    profileCfg->rxGain = RF_Param_10m.RxLNAGAIN;
#else
    profileCfg->rxGain = 24;
#endif

    /* Populate the chirp configuration: */
    chirpCfg->chirpStartIdx = 1;
    chirpCfg->chirpEndIdx = 1;
    chirpCfg->profileId = 1;
    chirpCfg->startFreqVar = 0;
    chirpCfg->freqSlopeVar = 0;
    chirpCfg->idleTimeVar = 0;
    chirpCfg->adcStartTimeVar = 0;
    chirpCfg->txEnable = 1;

    //Create profile 1.
    profileHandle = MMWave_addProfile(gMmwMCB.ctrlHandle, profileCfg, &errCode);
    gMmwMCB.ctrlCfg.u.advancedFrameCfg.profileHandle[1] = profileHandle;

    //Add the chirp definition to the profile
    gMmwMCB.chirpHandle = MMWave_addChirp(gMmwMCB.ctrlCfg.u.advancedFrameCfg.profileHandle[1], chirpCfg, &errCode);

    //advanced frame configuration command
    advFrameCfg->frameSeq.numOfSubFrames = 2;
    advFrameCfg->frameSeq.forceProfile = 0;
    advFrameCfg->frameSeq.numFrames = 1;
    advFrameCfg->frameSeq.triggerSelect = 1;
    advFrameCfg->frameSeq.frameTrigDelay = 0;

    memcpy((void *)&gMmwMCB.ctrlCfg.u.advancedFrameCfg.frameCfg,
           (void *)advFrameCfg, sizeof(rlAdvFrameCfg_t));
    //&ptrControlCfg->u.advancedFrameCfg.frameCfg

    memset((void *)&subFrameCfg, 0, sizeof(rlSubFrameCfg_t));
    //the subframe 0 configuration command.
    subFrameCfg.forceProfileIdx = 0;
    subFrameCfg.chirpStartIdx = 0;
    subFrameCfg.numOfChirps = 1;
    subFrameCfg.numLoops = 2;
    subFrameCfg.burstPeriodicity = (uint32_t)((float)20 * 1000000 / 5);
    subFrameCfg.chirpStartIdxOffset = 0;
    subFrameCfg.numOfBurst = 1;
    subFrameCfg.numOfBurstLoops = 1;
    subFrameCfg.subFramePeriodicity = (uint32_t)((float)20 * 1000000 / 5);

    /* Save Configuration to use later */
    memcpy((void *)&gMmwMCB.ctrlCfg.u.advancedFrameCfg.frameCfg.frameSeq.subFrameCfg[0],
           (void *)&subFrameCfg, sizeof(rlSubFrameCfg_t));

    memset((void *)&subFrameCfg, 0, sizeof(rlSubFrameCfg_t));
    //the subframe 1 configuration command.
    subFrameCfg.forceProfileIdx = 0;
    subFrameCfg.chirpStartIdx = 1;
    subFrameCfg.numOfChirps = 1;
    subFrameCfg.numLoops = 2;
    subFrameCfg.burstPeriodicity = (uint32_t)((float)20 * 1000000 / 5);
    subFrameCfg.chirpStartIdxOffset = 0;
    subFrameCfg.numOfBurst = 1;
    subFrameCfg.numOfBurstLoops = 1;
    subFrameCfg.subFramePeriodicity = (uint32_t)((float)20 * 1000000 / 5);

    /* Save Configuration to use later */
    memcpy((void *)&gMmwMCB.ctrlCfg.u.advancedFrameCfg.frameCfg.frameSeq.subFrameCfg[1],
           (void *)&subFrameCfg, sizeof(rlSubFrameCfg_t));

    /* Populate Adcbuf configuration: */
    adcbufCfg->adcFmt = 0;       //0=complex
    adcbufCfg->iqSwapSel = 1;    //1=Q in LSB, I in MSB
    adcbufCfg->chInterleave = 1; //not interleaved
    adcbufCfg->chirpThreshold = 1;

    dataPathObj->numRxAntennas = 1;                       //computed from Rx chan enables in _adcbufCfg
    dataPathObj->noiseEnergy = 29552;                     //not used
    dataPathObj->numTxAntennas = 1;                       //computed from Tx channel enables
    dataPathObj->numAdcSamples = DEMOCFG_NUM_ADC_SAMPLES; //copied from _profileCfg
    dataPathObj->numRangeBins = DEMOCFG_FFT_SIZE;         //recomputed
    dataPathObj->numChirpsPerFrame = 1;                   //computed from frameCfg chirp indexes and numLoops
    dataPathObj->rangeResolution = DEMOCFG_RANGE_RES;     //computed; dist res in meters
    dataPathObj->dataPathMode = DATA_PATH_WITH_ADCBUF;

/* Populate the runtime calibration configuration: */
#ifdef RUN_ONESHOT_CAL
    calCfg->u.chirpCalibrationCfg.enableCalibration = true;  //enable one-shot
    calCfg->u.chirpCalibrationCfg.enablePeriodicity = false; //disable periodic runtime
    calCfg->u.chirpCalibrationCfg.periodicTimeInFrames = 0U;
#else
    calCfg->u.chirpCalibrationCfg.enableCalibration = false;
    calCfg->u.chirpCalibrationCfg.enablePeriodicity = false;
    calCfg->u.chirpCalibrationCfg.periodicTimeInFrames = 0;
#endif
    calCfg->dfeDataOutputMode = MMWave_DFEDataOutputMode_ADVANCED_FRAME;

    gMmwMCB.ctrlCfg.dfeDataOutputMode = MMWave_DFEDataOutputMode_ADVANCED_FRAME;

    /* Configure the mmWave module: */
    if (MMWave_config(gMmwMCB.ctrlHandle, &gMmwMCB.ctrlCfg, &errCode) < 0)
    {
        demo_printf("Error: mmWave Config failed (code %d)\n", errCode);
        return -1;
    }

#ifdef POWER_TEST
    cptime[5] = Cycleprofiler_getTimeStamp();
#endif

    /* Setup the data path following MMWave_config: */
    demo_dataPathConfig();
    return 0;
}

int32_t demo_startSensor(void)
{
    int32_t errCode;

#ifdef POWER_TEST
    //Power testing: set GPIO0 high:
    gpio_write(0, 0, 1);
    cptime[6] = Cycleprofiler_getTimeStamp();
#endif

#ifdef REPORT_IR14_TIME_MEASUREMENTS
    //This will set GPIO 0 (header pin J6.15) low as a flag to
    //the MSP432 that mmwave and chirp configuration has completed.
#ifndef POWER_TEST
    gpio_write(0, 0, 0);
#endif
#endif

    /* Start the mmWave module: The configuration has been applied successfully. */
    if (MMWave_start(gMmwMCB.ctrlHandle, &gMmwMCB.calCfg, &errCode) < 0)
    {
        /* Error: Unable to start the mmWave control */
        demo_printf("Error: mmWave Control Start failed (code %d)\n", errCode);
        return -1;
    }

    return 0;
}

#ifdef RANGE_DC_CALIBRATION
/**
 *  @b Description
 *  @n
 *    Compensation of DC range antenna signature.  This function will examine
 *    the first and last several bins (configurable) over the chirps in the frame,
 *    and will remove the average power from these bins.
 *
 *  @retval
 *      Not Applicable.
 */
void demo_dcRangeSignatureCompensation(Demo_DataPathObj *obj)
{
    uint32_t antIdx, rngBinIdx;
    uint32_t ind;
    int32_t dcRangeSigMeanSize;
    uint32_t doppIdx;
    cmplx16ImRe_t *fftOut1D = (cmplx16ImRe_t *)obj->radarCube;

    dcRangeSigMeanSize = obj->numVirtualAntennas * (obj->calibDcRangeSigCfg.positiveBinIdx - obj->calibDcRangeSigCfg.negativeBinIdx + 1);
    if (obj->dcRangeSigCalibCntr == 0)
    {
        memset(obj->dcRangeSigMean, 0, dcRangeSigMeanSize * sizeof(cmplx32ImRe_t));
    }

    /* Calibration */
    if (obj->dcRangeSigCalibCntr < obj->calibDcRangeSigCfg.numAvgChirps)
    {
        for (doppIdx = 0; doppIdx < obj->numDopplerBins; doppIdx++)
        {
            /* Accumulate */
            ind = 0;
            for (rngBinIdx = 0; rngBinIdx <= obj->calibDcRangeSigCfg.positiveBinIdx; rngBinIdx++)
            {
                for (antIdx = 0; antIdx < obj->numVirtualAntennas; antIdx++)
                {
                    obj->dcRangeSigMean[ind].real +=
                        fftOut1D[rngBinIdx * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].real;
                    obj->dcRangeSigMean[ind].imag +=
                        fftOut1D[rngBinIdx * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].imag;
                    ind++;
                }
            }
            for (rngBinIdx = 0; rngBinIdx < -obj->calibDcRangeSigCfg.negativeBinIdx; rngBinIdx++)
            {
                for (antIdx = 0; antIdx < obj->numVirtualAntennas; antIdx++)
                {
                    obj->dcRangeSigMean[ind].real +=
                        fftOut1D[(obj->numRangeBins + obj->calibDcRangeSigCfg.negativeBinIdx + rngBinIdx) * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].real;
                    obj->dcRangeSigMean[ind].imag +=
                        fftOut1D[(obj->numRangeBins + obj->calibDcRangeSigCfg.negativeBinIdx + rngBinIdx) * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].imag;
                    ind++;
                }
            }
            obj->dcRangeSigCalibCntr++;
        }

        if (obj->dcRangeSigCalibCntr == obj->calibDcRangeSigCfg.numAvgChirps)
        {
            /* Divide */
            for (ind = 0; ind < (obj->numVirtualAntennas * dcRangeSigMeanSize); ind++)
            {
                obj->dcRangeSigMean[ind].real = obj->dcRangeSigMean[ind].real >> obj->log2NumAvgChirps;
                obj->dcRangeSigMean[ind].imag = obj->dcRangeSigMean[ind].imag >> obj->log2NumAvgChirps;
            }
        }
    }
    else
    {
        /* fftOut1D -= dcRangeSigMean */
        for (doppIdx = 0; doppIdx < obj->numDopplerBins; doppIdx++)
        {
            ind = 0;
            for (rngBinIdx = 0; rngBinIdx <= obj->calibDcRangeSigCfg.positiveBinIdx; rngBinIdx++)
            {
                for (antIdx = 0; antIdx < obj->numVirtualAntennas; antIdx++)
                {
                    fftOut1D[rngBinIdx * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].real -= obj->dcRangeSigMean[ind].real;
                    fftOut1D[rngBinIdx * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].imag -= obj->dcRangeSigMean[ind].imag;
                    ind++;
                }
            }
            for (rngBinIdx = 0; rngBinIdx < -obj->calibDcRangeSigCfg.negativeBinIdx; rngBinIdx++)
            {
                for (antIdx = 0; antIdx < obj->numVirtualAntennas; antIdx++)
                {
                    fftOut1D[(obj->numRangeBins + obj->calibDcRangeSigCfg.negativeBinIdx + rngBinIdx) * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].real -= obj->dcRangeSigMean[ind].real;
                    fftOut1D[(obj->numRangeBins + obj->calibDcRangeSigCfg.negativeBinIdx + rngBinIdx) * (obj->numVirtualAntennas * obj->numDopplerBins) + doppIdx * obj->numVirtualAntennas + antIdx].imag -= obj->dcRangeSigMean[ind].imag;
                    ind++;
                }
            }
        }
    }
}
#endif

/**
 *  @b Description
 *  @n
 *      Registered event function to mmwave which is invoked when an event from the
 *      BSS is received.
 *
 *  @param[in]  msgId
 *      Message Identifier
 *  @param[in]  sbId
 *      Subblock identifier
 *  @param[in]  sbLen
 *      Length of the subblock
 *  @param[in]  payload
 *      Pointer to the payload buffer
 *
 *  @retval
 *      Always return 0
 */
int32_t bss_debugAssert = 0;
int32_t bss_EsmAssert = 0;

int32_t demo_eventCallbackFxn(uint16_t msgId, uint16_t sbId, uint16_t sbLen, uint8_t *payload)
{
    uint16_t asyncSB = RL_GET_SBID_FROM_UNIQ_SBID(sbId);

    /* Process the received message: */
    switch (msgId)
    {
    case RL_RF_ASYNC_EVENT_MSG:
    {
        /* Received Asychronous Message: */
        switch (asyncSB)
        {
        case RL_RF_AE_CPUFAULT_SB:
        {
            bss_debugAssert = 1;
            break;
        }
        case RL_RF_AE_ESMFAULT_SB:
        {
            bss_EsmAssert = 1;
            break;
        }
        case RL_RF_AE_INITCALIBSTATUS_SB:
        {
            rlRfInitComplete_t *ptrRFInitCompleteMessage;

            /* Get the RF-Init completion message: */
            ptrRFInitCompleteMessage = (rlRfInitComplete_t *)payload;
            gMmwMCB.dataPathObj.calibrationStatus = ptrRFInitCompleteMessage->calibStatus & 0x1FFF;
            break;
        }
#if 0
                case RL_RF_AE_FRAME_TRIGGER_RDY_SB:
                {
                    gMmwMCB.stats.frameTriggerReady++;
                    break;
                }
                case RL_RF_AE_MON_TIMING_FAIL_REPORT_SB:
                {
                    gMmwMCB.stats.failedTimingReports++;
                    break;
                }
                case RL_RF_AE_RUN_TIME_CALIB_REPORT_SB:
                {
                    gMmwMCB.stats.calibrationReports++;
                    break;
                }
                case RL_RF_AE_FRAME_END_SB:
                {
                    /*Received Frame Stop async event from BSS.
                      No action required.*/
                    break;
                }
#endif
        default:
        {
            demo_printf("Error: Asynchronous Event SB Id %d not handled\n", asyncSB);
            break;
        }
        }
        break;
    }
    default:
    {
        demo_printf("Error: Asynchronous message %d is NOT handled\n", msgId);
        break;
    }
    }
    return 0;
}

/**
 *  @b Description
 *  @n
 *      The task is used to provide an execution context for the mmWave
 *      control task
 *
 *  @retval
 *      Not Applicable.
 */
void demo_mmWaveCtrlTask(UArg arg0, UArg arg1)
{
    int32_t errCode;

    while (1)
    {
        /* Execute the mmWave control module: */
        if (MMWave_execute(gMmwMCB.ctrlHandle, &errCode) < 0)
            demo_printf("Error: mmWave control execution failed [Error code %d]\n", errCode);
    }
}

//SPI发送函数
uint8_t SPI_Send_Data(void *ptrTxBuf, int __size)
{
    int i = 0;
    const uint8_t ramBufLen = SPIRAMBUFLEN * 2;
    int8_t errCode;
    SPI_Transaction transaction;
    transaction.rxBuf = NULL;
    transaction.slaveIndex = 0;

    while (__size > ramBufLen)
    {
        transaction.count = ramBufLen;
        transaction.txBuf = (uint8_t *)ptrTxBuf + i * ramBufLen;
        i++;
        __size -= ramBufLen;
        /* Start Data Transfer */
        errCode = SPI_transfer(spiHandle, &transaction);
    }

    transaction.count = __size;
    transaction.txBuf = (uint8_t *)ptrTxBuf + i * ramBufLen;

    /* Start Data Transfer */
    errCode = SPI_transfer(spiHandle, &transaction);
    return errCode;
}

/**
 *  @b Description
 *  @n
 *      This task is used to run full calibration and save the resulting
 *      calibration table to flash for subsequent runs.
 *
 *  @retval
 *      Not Applicable.
 */
void demo_calibrationTask(UArg arg0, UArg arg1)
{
    uint32_t flashAddr;
    int32_t errCode;
    uint32_t *calptr;
    MMWave_MCB *ptrMMWaveMCB;
    rlChanCfg_t *chCfg = &gMmwMCB.openCfg.chCfg;
    rlAdcOutCfg_t *adcOutCfg = &gMmwMCB.openCfg.adcOutCfg;
    rlLowPowerModeCfg_t *lowPwrMode = &gMmwMCB.openCfg.lowPowerMode;

    /* Get the pointer to the control module */
    ptrMMWaveMCB = (MMWave_MCB *)gMmwMCB.ctrlHandle;

    /* Populate the channel configuration: */
#ifndef CZ_BOARD
    chCfg->rxChannelEn = 0x01; //Rx enable mask
#else
    //FOR CALIBRATION TEMP TESTING (CZ board + waveguide)
    chCfg->rxChannelEn = 0x08; //Rx enable mask
#endif
    chCfg->txChannelEn = 0x01; //Tx enable mask
    chCfg->cascading = 0;

    /* Populate the ADC output configuration: */
    adcOutCfg->fmt.b2AdcBits = 2;   //ADC_BITS_16
    adcOutCfg->fmt.b2AdcOutFmt = 1; //complex 1x

    /* Populate the low power configuration: */
    lowPwrMode->reserved = 0;
    lowPwrMode->lpAdcMode = 0;

    gMmwMCB.openCfg.freqLimitLow = DEMO_CAL_FREQ_LOW;
    gMmwMCB.openCfg.freqLimitHigh = DEMO_CAL_FREQ_HIGH;

    /* Copy over the configuration: */
    memcpy((void *)&ptrMMWaveMCB->openCfg, &gMmwMCB.openCfg, sizeof(MMWave_OpenCfg));

    //Run full calibration
    errCode = 0;
    demo_openLink(ptrMMWaveMCB, &gMmwMCB.openCfg, 0x17F0, NULL, &errCode);

    if (errCode == 0)
    {
        //Grab the calibration table (chunks) from the BSS.
        errCode = rlRfCalibDataStore(RL_DEVICE_MAP_INTERNAL_BSS, (rlCalibrationData_t *)init_cal_data);

        if (errCode == 0)
        {
            //Do some sanity checks before writing it to flash
            calptr = (uint32_t *)init_cal_data;
            if ((calptr[0] != 0x00000003) ||
                (calptr[57] != 0x00010003) ||
                (calptr[114] != 0x00020003))
            {
                errCode = -1;
            }
            else if ((calptr[1] & 0x1fff) != DEMO_GOOD_CAL_RESULT)
            {
                errCode = -1;
            }
        }
        else
        {
        }

        if (errCode == 0) //so far, so good
        {
#ifndef RUN_FULL_CAL
            flashAddr = QSPIFlash_getExtFlashAddr(qspiflashHandle) +
                        DEMO_CAL_FLASH_OFFSET;

            /* Erase the 4KB sector to be written. */
            QSPIFlash_sectorErase(qspiflashHandle, flashAddr);

            QSPIFlash_singleWrite(qspiflashHandle, flashAddr, DEMO_CAL_FLASH_LEN,
                                  (uint8_t *)init_cal_data);
#endif
        }
    }
    else
    {
        errCode = -1;
    }

    /* Configure the SPI Data Transfer */

    while (1)
    {
    };
}

extern uint8_t receive_num;
extern uint8_t receive[256];

extern uint8_t data1;

void Wait_MCU_ProcessLastFrame(void)
{
    while (FlagAboutMcu.Send_Next_SpiFrame == 0)
    {
        UART_readPolling(uartHandle, &data1, 1);
        receive[receive_num++] = data1;
        communications_protocol();
    }
    FlagAboutMcu.Send_Next_SpiFrame = 0;
}

//通用存储程序
void Save_Data2Flash(uint8_t *const srcDataAddr, uint16_t DataLen, uint32_t flashAddrOffset)
{
    uint32_t flashAddr = 0U;
    flashAddr = QSPIFlash_getExtFlashAddr(qspiflashHandle);
    flashAddr = flashAddr + flashAddrOffset;

    QSPIFlash_sectorErase(qspiflashHandle, flashAddr); //The size of one sector is 4k byte
    QSPIFlash_singleWrite(qspiflashHandle, flashAddr, DataLen, (uint8_t *)srcDataAddr);

    // //System_printf ("MCU have saved last echo data. %d\n",DataLen);
}

void Get_DataFromFlash(uint8_t *dstDataAddr, uint16_t DataLen, uint32_t flashAddrOffset)
{
    uint32_t flashAddr = 0U;
    flashAddr = QSPIFlash_getExtFlashAddr(qspiflashHandle);
    flashAddr = flashAddr + flashAddrOffset;

    QSPIFlash_singleRead(qspiflashHandle, flashAddr, DataLen, (uint8_t *)dstDataAddr);

    // //System_printf ("MCU have gotten the last echo data. %d\n",DataLen);
}

inline uint32_t bigger(uint32_t a, uint32_t b)
{
    return a > b ? a : b;
}

/**
 *  @b Description
 *  @n
 *      The task is used for data path processing and to transmit the
 *      detected objects to the MSP432 via the SPI port.
 *
 *  @retval
 *      Not Applicable.
 */
uint16_t *const ptrTarget_Peak_num = MY_OWN_BASE_ADDRESS(CFAR_RESULT_INDEX_OFFSET);
uint16_t k = 0;
void demo_dataPathTask(UArg arg0, UArg arg1)
{
    uint32_t idx;
    int32_t err;

    Demo_DataPathObj *dataPathObj = &gMmwMCB.dataPathObj;
    int16_t numDetectedObjects;

    int16_t *const ptrFFT_Result = MY_OWN_BASE_ADDRESS(FFT_ABS_START_OFFSET);
    uint32_t *const ptrFFT_Abs_32b = MY_OWN_BASE_ADDRESS(FFT_PROCESS_OFFSET);
    uint16_t *const ptrFFT_Abs_16b = MY_OWN_BASE_ADDRESS(FFT_PROCESS_16b_OFFSET);
    uint32_t *const ptrFFT_Abs_original = MY_OWN_BASE_ADDRESS(FFT_ORIGINAL_OFFSET);

    uint16_t *const ptrCfar_RefLevel = MY_OWN_BASE_ADDRESS(CFAR_REF_LEVEL_FAR_OFFSET);
    uint64_t *const ptrCfar_Result_index = MY_OWN_BASE_ADDRESS(CFAR_RESULT_INDEX_OFFSET + sizeof(DataAndDistance_t));
    DataAndDistance_t *const ptrTarget_Peak = MY_OWN_BASE_ADDRESS(CFAR_RESULT_INDEX_OFFSET + sizeof(DataAndDistance_t));
    //uint16_t *const ptrTarget_Peak_num = MY_OWN_BASE_ADDRESS(CFAR_RESULT_INDEX_OFFSET);
    uint32_t *const ptrEnvironment_Echo = MY_OWN_BASE_ADDRESS(ENVIRONMENT_USE_OFFSET);

    datapath_err = 0;

    //Configure the static (built-in) chirp parameters and run calibration.
    err = demo_staticConfig();
    if (err != 0)
        err = DEMO_MEAS_ERR_CONFIG; //error in the configuration

    if (gMmwMCB.dataPathObj.calibrationStatus != DEMO_GOOD_CAL_RESULT)
        err = DEMO_MEAS_ERR_CAL; //not all calibration tasks passed

    //Communicate with the BSS to start the chirp.
    if (err == 0)
    {
        err = demo_startSensor();
        if (err != 0)
            err = DEMO_MEAS_ERR_START; //error in starting mmWave control
    }

#ifdef POWER_TEST
    cptime[7] = Cycleprofiler_getTimeStamp();
#endif

    if (err == 0)
    {

        while (1)
        {
        }
    }
}
