/*! 
 *  \file   RADARDEMO_highAccuRangeProc_priv.c
 *
 *  \brief   Windowing functions for range processing.
 *
 * Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/ 
 * 
 * 
 *  Redistribution and use in source and binary forms, with or without 
 *  modification, are permitted provided that the following conditions 
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright 
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the 
 *    documentation and/or other materials provided with the   
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#include "RADARDEMO_highAccuRangeProc_priv.h"

#ifdef _TMS320C6X
#include "c6x.h"
#endif

#ifdef ARMVERSION
extern void dft(int size, float * input, float * output);
#endif

#ifndef ARMVERSION

#pragma DATA_SECTION(addzeros111,".l3data");
float addzeros111[4*512];
#pragma DATA_SECTION(addzerosoutput122,".l3data");
float addzerosoutput122[4*512];

/*!
   \fn     RADARDEMO_highAccuRangeProc_accumulateInput

   \brief   Accumulate signals from all chirps and convert to floating-point format with 1D windowing.

   \param[in]    nSamplesPerChirp
               Number of samples per chirp.

   \param[in]    fftSize1D
               1D FFT size.

   \param[in]    nChirpPerFrame
               Number of chirps per frame.

   \param[in]    fftWin1D
               Input pointer to 1D window function.
			   
   \param[in]    fftWin1DSize
               Number of ADC bits.

   \param[in]    inputPtr
               Pointer to input signal.

   \param[in]    chirpInd
               Flag to enable DC adjustment for ADC samples when set to 1. Otherwise, no preprocessing for ADC samples.

   \param[out]    outputPtr
               Pointer to the output signal.

   \pre       none

   \post      none


 */

void	RADARDEMO_highAccuRangeProc_accumulateInput(
                            IN  uint32_t nSamplesPerChirp,
                            IN  uint32_t fftSize1D,
                            IN  uint32_t nChirpPerFrame,
                            IN  float   * RESTRICT fftWin1D,
							IN 	int16_t  fftWin1DSize,
							IN  cplx16_t * RESTRICT inputPtr,
							IN  int32_t  chirpInd,
							OUT float * outputPtr)
{

    int32_t i;
    float * tempOutputPtr;

    if (chirpInd == 0)
    {
        for( i = (int32_t)nSamplesPerChirp - 1; i >= 0; i--)
        {
            *outputPtr++                    =   (float)inputPtr->real;
            *outputPtr++                    =   (float)inputPtr->imag;
            inputPtr++;
        }
    }
    else
    {
        tempOutputPtr       =   outputPtr;
        for( i = (int32_t)nSamplesPerChirp - 1; i >= 0; i--)
        {
            *tempOutputPtr++                    +=  (float)inputPtr->real;
            *tempOutputPtr++                    +=  (float)inputPtr->imag;
            inputPtr++;
        }

        if (chirpInd == ((int32_t)nChirpPerFrame - 1))
        {
            float fwin1D;
            float *tempOutputPtr;

            tempOutputPtr   =   &outputPtr[2*nSamplesPerChirp - 1];

            for( i = fftWin1DSize - 1; i >= 0; i--)
            {
                fwin1D                      =   *fftWin1D++;
                *outputPtr                  =   *outputPtr * fwin1D;
                outputPtr++;
                *outputPtr                  =   *outputPtr * fwin1D;
                outputPtr++;
                *tempOutputPtr              =   *tempOutputPtr * fwin1D;
                tempOutputPtr--;
                *tempOutputPtr              =   *tempOutputPtr * fwin1D;
                tempOutputPtr--;
            }
            if (((int32_t)fftSize1D - (int32_t)nSamplesPerChirp) > 0)
            {
                tempOutputPtr   =   &outputPtr[2*nSamplesPerChirp];
                for( i = 0; i < ((int32_t)fftSize1D - (int32_t)nSamplesPerChirp); i++);
                {
                    *tempOutputPtr++    =   0.f;
                    *tempOutputPtr++    =   0.f;
                }
            }
        }
    }
}


/*!
   \fn     RADARDEMO_highAccuRangeProc_rangeEst

   \brief   Windowing function for 2D FFT and prepare for the input.

   \param[in]    highAccuRangeHandle
               Handle to the module.

   \param[out]    estRange
               Estimated range.

   \param[out]    deltaPhaseEst
               Estimated delta phase.

   \param[out]    estLinearSNR
               Estimated SNR.

   \pre       none

   \post      none


 */

void	RADARDEMO_highAccuRangeProc_rangeEst(
                            IN  RADARDEMO_highAccuRangeProc_handle *highAccuRangeHandle,
							OUT  float * estRange,
							OUT  float * deltaPhaseEst,
							OUT float * estLinearSNR)
{

          int32_t         i, j, rad1D, coarseRangeInd;
           unsigned char   * brev = NULL;
            float           totalPower, max, ftemp, sigPower, * RESTRICT powerPtr;
            float       finput, * RESTRICT inputPtr, * RESTRICT inputPtr1,* RESTRICT inputPtr3,* RESTRICT inputPtr4;


          int32_t         i1, j1, k1, coarseRangeInd1 = highAccuRangeHandle->coarseRangeInd;
                      float           max1, ftemp1;

                   int32_t         zoomStartInd2, zoomEndInd2, itemp2, tempIndFine1, tempIndFine2, tempIndCoarse, indMask, shift;
                      int32_t         fineRangeInd, tempFineSearchIdx, tempCoarseSearchIdx;
                     float       *RESTRICT inputPtr2, *RESTRICT wncPtr, *RESTRICT wnfPtr, sigAccReal, sigAccImag;
                     float           freqFineEst, fdelta, finputReal, finputImag, tempReal, tempImag;


            j  = highAccuRangeHandle->log2fft1DSize;
            if ((j & 1) == 0)
                rad1D = 4;
            else
                rad1D = 2;
#if(1)
            /* copy to scratch is needed because FFT function will corrupt the input. We need to preserve if for zoom-in FFT */
            inputPtr    =   (float *) highAccuRangeHandle->inputSig;
         inputPtr1   =   (float *) &addzeros111[0];
         inputPtr4=(float *) &addzerosoutput122[0];

            for (i = 0; i <((int32_t)highAccuRangeHandle->fft1DSize)*2; i++ )
            {
                if(i<(int32_t)highAccuRangeHandle->fft1DSize)
                {
                *inputPtr1++        =   *inputPtr++;
                *inputPtr1++        =   *inputPtr++;
                *inputPtr4++        =   0;
                *inputPtr4++        =   0;
                }
                else
                {
                    *inputPtr1++        =  0 ;
                    *inputPtr1++        =  0 ;
                    *inputPtr4++        =  0 ;
                    *inputPtr4++        =   0;
                }

            }

           inputPtr1   =   (float *) &addzeros111[0];

           inputPtr4=(float *) &addzerosoutput122[0];
           dft(2*highAccuRangeHandle->fft1DSize, (float*) inputPtr1, (float *)addzerosoutput122);

#if(0)
               int k,  index;
               const double PI = 3.141592654;
               float *p_x;
               float arg, fx_0, fx_1, fy_0, fy_1, co, si;

               for (k = 0; k <2*highAccuRangeHandle->fft1DSize ; k++)

               {
                   p_x = inputPtr1;

                   fy_0 = 0;
                   fy_1 = 0;
                   for (i = 0; i <2*highAccuRangeHandle->fft1DSize; i++)
                   {
                       fx_0 = p_x[0];
                       fx_1 = p_x[1];
                       p_x += 2;
                       index = (i * k) % N;
                       arg = 2 * PI * index / N;
                       co = cos(arg);
                       si = -sin(arg);
                       fy_0 += ((fx_0 * co) - (fx_1 * si));
                       fy_1 += ((fx_1 * co) + (fx_0 * si));
                   }
                   addzerosoutput122[2 * k] = fy_0;
                   addzerosoutput122[2 * k + 1] = fy_1;
               }
           }

#endif
#if(0)
            inputPtr1   =   (float *) &highAccuRangeHandle->fft1DOutSig[0];
            for( i = 0; i < (int32_t)highAccuRangeHandle->skipLeft;  i++ )
            {
                *inputPtr1++    =   0.f;
                *inputPtr1++    =   0.f;
            }
            inputPtr1   =   (float *) &highAccuRangeHandle->fft1DOutSig[2*((int32_t)highAccuRangeHandle->fft1DSize- (int32_t)highAccuRangeHandle->skipRight)];
            for( i = (int32_t)highAccuRangeHandle->fft1DSize- (int32_t)highAccuRangeHandle->skipRight; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
            {
                *inputPtr1++    =   0.f;
                *inputPtr1++    =   0.f;
            }
            max         =   0.f;
            totalPower  =   0.f;
            coarseRangeInd  =   0;
            inputPtr    =   (float *)highAccuRangeHandle->fft1DOutSig;
            powerPtr    =   (float *)highAccuRangeHandle->scratchPad;
            for( i = 0; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
            {
                finput      =   (*inputPtr) * (*inputPtr);
                inputPtr++;
                finput      +=  (*inputPtr) * (*inputPtr);
                inputPtr++;
                ftemp       =   finput;
                powerPtr[i] =   ftemp;
                totalPower  +=  ftemp;
                if( max < ftemp )
                {
                    max         =   ftemp;
                    coarseRangeInd  =   i;
                }
            }
#endif
#endif
            highAccuRangeHandle->coarseRangeInd = 1;
            i           =   coarseRangeInd;
            sigPower    =   1;

            *estLinearSNR   =   ((float)((int32_t)highAccuRangeHandle->fft1DSize  - (int32_t)highAccuRangeHandle->skipLeft - (int32_t)highAccuRangeHandle->skipRight- 5) * sigPower) / (totalPower - sigPower);

    //  else if (procStep == 2)

#if(0)
            /* zoom in FFT: assuming always size of fft1DSize x fft1DSize */
            zoomStartInd2   =   coarseRangeInd1 - highAccuRangeHandle->numRangeBinZoomIn;
            zoomEndInd2     =   coarseRangeInd1 + highAccuRangeHandle->numRangeBinZoomIn;
            indMask         =   highAccuRangeHandle->fft1DSize - 1;
            shift           =   highAccuRangeHandle->log2fft1DSize;
            inputPtr2       =   (float *)highAccuRangeHandle->inputSig;
            wncPtr          =   (float *)highAccuRangeHandle->wnCoarse;
            wnfPtr          =   (float *)highAccuRangeHandle->wnFine;
            max1                =   0.0;
            itemp2          =   0;
            for( i1 = zoomStartInd2; i1 < zoomEndInd2; i1++)
            {
                for( j1 = 0; j1 < (int32_t)highAccuRangeHandle->fft1DSize; j1 ++)
                {
                    tempFineSearchIdx   =   j1;
                    sigAccReal  =   0.f;
                    sigAccImag  =   0.f;
                    tempCoarseSearchIdx =   0;
                    for( k1 = 0; k1 < (int32_t)highAccuRangeHandle->nSamplesPerChirp; k1 ++)
                    {
                        tempIndFine1    =   tempFineSearchIdx & indMask;
                        tempIndFine2    =   tempFineSearchIdx >> shift;
                        tempFineSearchIdx       +=  j1;
                        tempIndCoarse   =   (tempCoarseSearchIdx + tempIndFine2) & indMask;
                        tempCoarseSearchIdx     +=  i1;

                        finputReal      =   inputPtr2[2 * k1];
                        finputImag      =   inputPtr2[2 * k1 + 1];
                        tempReal        =   wncPtr[2 * tempIndCoarse + 1] * finputReal - wncPtr[2 * tempIndCoarse] * finputImag;
                        tempImag        =   wncPtr[2 * tempIndCoarse] * finputReal + wncPtr[2 * tempIndCoarse + 1] * finputImag;

                        sigAccReal      +=  wnfPtr[2 * tempIndFine1 + 1] * tempReal - wnfPtr[2 * tempIndFine1] * tempImag;
                        sigAccImag      +=  wnfPtr[2 * tempIndFine1] * tempReal + wnfPtr[2 * tempIndFine1 + 1] * tempImag;
                    }
                    ftemp1              =   sigAccReal * sigAccReal + sigAccImag * sigAccImag;
                    if( max1 < ftemp1)
                    {
                        max1            =   ftemp1;
                        fineRangeInd    =   itemp2;
                    }
                    itemp2++;
                }
            }
#endif
            fdelta          =   (highAccuRangeHandle->maxBeatFreq) / ((float)highAccuRangeHandle->fft1DSize * (float)highAccuRangeHandle->fft1DSize);
            freqFineEst     =   1;

            *estRange       =   1;


    #if 0
            if (highAccuRangeHandle->enablePhaseEst)
            {
                float phaseCoarseEst1, phaseCoarseEst2, phaseInitial;
                double PI = 3.14159265358979323846f;
                double real, imag, denom, initReal, initImag, dtemp1, dtemp2, dtemp;
                float phaseEst, totalPhase = 0.f, phaseCorrection, rangePhaseCorrection;
                __float2_t demodSig, corrSig;

                inputPtr            =   (__float2_t *)highAccuRangeHandle->inputSig;
                phaseCoarseEst1     =   2 * (float)PI * highAccuRangeHandle->fc * (divsp(2 * (*estRange), (float)3e8) + highAccuRangeHandle->adcStartTimeConst);
                phaseCoarseEst2     =   (float)PI * highAccuRangeHandle->chirpSlope * (divsp(2 * (*estRange), (float)3e8)  + highAccuRangeHandle->adcStartTimeConst) * (divsp(2 * (*estRange), (float)3e8)  + highAccuRangeHandle->adcStartTimeConst);
                phaseInitial        =   phaseCoarseEst1 - phaseCoarseEst2;

                denom               =   divdp(1.0, (double)highAccuRangeHandle->fft1DSize);
        #if 0
                dtemp1              =   cos((double)phaseInitial);
                dtemp2              =   sin(-(double)phaseInitial);
                initReal            =   cos(2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom) ;
                initImag            =   sin(-2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom);
        #else
                dtemp1              =   cosdp_i((double)phaseInitial);
                dtemp2              =   sindp_i(-(double)phaseInitial);
                initReal            =   cosdp_i(2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom) ;
                initImag            =   sindp_i(-2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom);
        #endif

                //sample @ t = 0;
                corrSig             =   _ftof2((float)dtemp1, (float)dtemp2);
                demodSig            =   _complex_mpysp(_amem8_f2(inputPtr++), corrSig);
                RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
                totalPhase          +=  phaseEst;

                if ((highAccuRangeHandle->enableFilter == 0) && (highAccuRangeHandle->enableLinearFit == 0))
                {
                    //sample @ t = 1;
                    dtemp           =   dtemp1 * initReal - dtemp2 * initImag;
                    imag            =   dtemp1 * initImag + dtemp2 * initReal;
                    real            =   dtemp;
                    corrSig             =   _ftof2((float)real, (float)imag);
                    demodSig            =   _complex_mpysp(_amem8_f2(inputPtr++), corrSig);
                    RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
                    totalPhase          +=  phaseEst;

                    for( j = 2; j < (int32_t)highAccuRangeHandle->fft1DSize; j ++)
                    {
                        dtemp           =   real * initReal - imag * initImag;
                        imag            =   real * initImag + imag * initReal;
                        real            =   dtemp;
                        corrSig         =   _ftof2((float)real, (float)imag);
                        demodSig        =   _complex_mpysp(_amem8_f2(inputPtr++), corrSig);
                        RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
                        totalPhase      +=  phaseEst;
                    }
                    phaseCorrection     =   totalPhase * (float)denom;
                    rangePhaseCorrection= divsp((phaseCorrection*(float)3e8), (4.f * (float) PI * highAccuRangeHandle->fc));
                    *estRange           +=  rangePhaseCorrection;
                }
                else
                {
                    //Not implemented yet
                }
            }
    #endif

        return;
}


#endif


/*!
   \fn     RADARDEMO_highAccuRangeProc_accumulateInputGeneric

   \brief   Accumulate signals from all chirps and convert to floating-point format with 1D windowing.

   \param[in]    nSamplesPerChirp
               Number of samples per chirp.

   \param[in]    fftSize1D
               1D FFT size.

   \param[in]    nChirpPerFrame
               Number of chirps per frame.

   \param[in]    fftWin1D
               Input pointer to 1D window function.
			   
   \param[in]    fftWin1DSize
               Number of ADC bits.

   \param[in]    inputPtr
               Pointer to input signal.

   \param[in]    chirpInd
               Flag to enable DC adjustment for ADC samples when set to 1. Otherwise, no preprocessing for ADC samples.

   \param[out]    outputPtr
               Pointer to the output signal.

   \pre       none

   \post      none


 */

void	RADARDEMO_highAccuRangeProc_accumulateInputGeneric(
                            IN  uint32_t nSamplesPerChirp,
                            IN  uint32_t fftSize1D,
                            IN  uint32_t nChirpPerFrame,
                            IN  float   * RESTRICT fftWin1D,
							IN 	int16_t  fftWin1DSize,
							IN  cplx16_t * RESTRICT inputPtr,
							IN  int32_t  chirpInd,
							OUT float * outputPtr)
{
	int32_t i;
	float * tempOutputPtr;

	if (chirpInd == 0)
	{
		for( i = (int32_t)nSamplesPerChirp - 1; i >= 0; i--)
		{
			*outputPtr++					=	(float)inputPtr->real;
			*outputPtr++					=	(float)inputPtr->imag;
			inputPtr++;
		}
	}
	else
	{
		tempOutputPtr		=	outputPtr;
		for( i = (int32_t)nSamplesPerChirp - 1; i >= 0; i--)
		{
			*tempOutputPtr++					+=	(float)inputPtr->real;
			*tempOutputPtr++					+=	(float)inputPtr->imag;
			inputPtr++;
		}

		if (chirpInd == ((int32_t)nChirpPerFrame - 1))
		{
			float fwin1D;
			float *tempOutputPtr;

			tempOutputPtr	=	&outputPtr[2*nSamplesPerChirp - 1];

			for( i = fftWin1DSize - 1; i >= 0; i--)
			{
				fwin1D						=	*fftWin1D++;
				*outputPtr					=	*outputPtr * fwin1D;
				outputPtr++;
				*outputPtr					=	*outputPtr * fwin1D;
				outputPtr++;
				*tempOutputPtr				=	*tempOutputPtr * fwin1D;
				tempOutputPtr--;
				*tempOutputPtr				=	*tempOutputPtr * fwin1D;
				tempOutputPtr--;
			}
			if (((int32_t)fftSize1D - (int32_t)nSamplesPerChirp) > 0)
			{
				tempOutputPtr	=	&outputPtr[2*nSamplesPerChirp];
				for( i = 0; i < ((int32_t)fftSize1D - (int32_t)nSamplesPerChirp); i++);
				{
					*tempOutputPtr++	=	0.f;
					*tempOutputPtr++	=	0.f;
				}
			}
		}
	}
}


/*!
   \fn     RADARDEMO_highAccuRangeProc_rangeEstGeneric

   \brief   Windowing function for 2D FFT and prepare for the input.

   \param[in]    procStep
               processing step, if 1, do preprocessing of the estimation, up to coarse range estimation, if 2, do the rest of estimation (fine freq and phase).

   \param[in]    highAccuRangeHandle
               Handle to the module.

   \param[out]    estRange
               Estimated range.

   \param[out]    deltaPhaseEst
               Estimated delta phase.

   \param[out]    estLinearSNR
               Estimated SNR.

   \pre       none

   \post      none


 */

void	RADARDEMO_highAccuRangeProc_rangeEstGeneric(
							IN int8_t procStep,
                            IN  RADARDEMO_highAccuRangeProc_handle *highAccuRangeHandle,
							OUT  float * estRange,
							OUT  float * deltaPhaseEst,
							OUT float * estLinearSNR)
{
	if (procStep == 1)
	{
		int32_t			i, j, rad1D, coarseRangeInd;
		unsigned char	* brev = NULL;
		float			totalPower, max, ftemp, sigPower, * RESTRICT powerPtr;
		float		finput, * RESTRICT inputPtr, * RESTRICT inputPtr1;

		j  = highAccuRangeHandle->log2fft1DSize;
		if ((j & 1) == 0)
			rad1D = 4;
		else
			rad1D = 2;

		/* copy to scratch is needed because FFT function will corrupt the input. We need to preserve if for zoom-in FFT */
		inputPtr	=	(float *) highAccuRangeHandle->inputSig;
		inputPtr1	=	(float *) &highAccuRangeHandle->scratchPad[2 * highAccuRangeHandle->fft1DSize];
		for (i = 0; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
		{
			*inputPtr1++		=	*inputPtr++;
			*inputPtr1++		=	*inputPtr++;
		}

		inputPtr1	=	(float *) &highAccuRangeHandle->scratchPad[2 * highAccuRangeHandle->fft1DSize];
#if 0
		DSPF_sp_fftSPxSP_cn (
				highAccuRangeHandle->fft1DSize,
				(float*) inputPtr1,
				(float *)highAccuRangeHandle->twiddle, 
				highAccuRangeHandle->fft1DOutSig,
				brev,
				rad1D,
				0,
				highAccuRangeHandle->fft1DSize);
#endif

		dft(highAccuRangeHandle->fft1DSize, (float*) inputPtr1, highAccuRangeHandle->fft1DOutSig);

		inputPtr1	=	(float *) &highAccuRangeHandle->fft1DOutSig[0];
		for( i = 0; i < (int32_t)highAccuRangeHandle->skipLeft;  i++ )
		{
			*inputPtr1++	=	0.f;
			*inputPtr1++	=	0.f;
		}
		inputPtr1	=	(float *) &highAccuRangeHandle->fft1DOutSig[2*((int32_t)highAccuRangeHandle->fft1DSize- (int32_t)highAccuRangeHandle->skipRight)];
		for( i = (int32_t)highAccuRangeHandle->fft1DSize- (int32_t)highAccuRangeHandle->skipRight; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
		{
			*inputPtr1++	=	0.f;
			*inputPtr1++	=	0.f;
		}
		max			=	0.f;
		totalPower	=	0.f;
		coarseRangeInd	=	0;
		inputPtr	=	(float *)highAccuRangeHandle->fft1DOutSig;
		powerPtr	=	(float *)highAccuRangeHandle->scratchPad;
		for( i = 0; i < (int32_t)highAccuRangeHandle->fft1DSize; i++ )
		{
			finput		=	(*inputPtr) * (*inputPtr);
			inputPtr++;
			finput		+=	(*inputPtr) * (*inputPtr);
			inputPtr++;
			ftemp		=	finput;
			powerPtr[i] =	ftemp;
			totalPower	+=	ftemp;
			if( max < ftemp )
			{
				max			=	ftemp;
				coarseRangeInd	=	i;
			}
		}
	
		highAccuRangeHandle->coarseRangeInd = coarseRangeInd;
		i			=	coarseRangeInd;
		sigPower	=	powerPtr[i-2] + powerPtr[i-1] + powerPtr[i] + powerPtr[i+1] + powerPtr[i+2];

		*estLinearSNR	=	((float)((int32_t)highAccuRangeHandle->fft1DSize  - (int32_t)highAccuRangeHandle->skipLeft - (int32_t)highAccuRangeHandle->skipRight- 5) * sigPower) / (totalPower - sigPower);
	}
//	else if (procStep == 2)

		int32_t		 	i1, j1, k1, coarseRangeInd1 = highAccuRangeHandle->coarseRangeInd;
		float			max1, ftemp1;

		int32_t			zoomStartInd2, zoomEndInd2, itemp2, tempIndFine1, tempIndFine2, tempIndCoarse, indMask, shift;
		int32_t			fineRangeInd, tempFineSearchIdx, tempCoarseSearchIdx;
		float		*RESTRICT inputPtr2, *RESTRICT wncPtr, *RESTRICT wnfPtr, sigAccReal, sigAccImag;
		float			freqFineEst, fdelta, finputReal, finputImag, tempReal, tempImag;

		/* zoom in FFT: assuming always size of fft1DSize x fft1DSize */
		zoomStartInd2	=	coarseRangeInd1 - highAccuRangeHandle->numRangeBinZoomIn;
		zoomEndInd2		=	coarseRangeInd1 + highAccuRangeHandle->numRangeBinZoomIn;
		indMask			=	highAccuRangeHandle->fft1DSize - 1;
		shift			=	highAccuRangeHandle->log2fft1DSize;
		inputPtr2		=	(float *)highAccuRangeHandle->inputSig;
		wncPtr			=	(float *)highAccuRangeHandle->wnCoarse;
		wnfPtr			=	(float *)highAccuRangeHandle->wnFine;
		max1				=	0.0;
		itemp2			=	0;
		for( i1 = zoomStartInd2; i1 < zoomEndInd2; i1++)
		{
			for( j1 = 0; j1 < (int32_t)highAccuRangeHandle->fft1DSize; j1 ++)
			{
				tempFineSearchIdx	=	j1;
				sigAccReal  =	0.f; 
				sigAccImag  =	0.f; 
				tempCoarseSearchIdx	=	0;
				for( k1 = 0; k1 < (int32_t)highAccuRangeHandle->nSamplesPerChirp; k1 ++)
				{
					tempIndFine1	=	tempFineSearchIdx & indMask;
					tempIndFine2	=	tempFineSearchIdx >> shift;
					tempFineSearchIdx		+=	j1;
					tempIndCoarse	=	(tempCoarseSearchIdx + tempIndFine2) & indMask;
					tempCoarseSearchIdx		+=	i1;

					finputReal		=	inputPtr2[2 * k1];
					finputImag		=	inputPtr2[2 * k1 + 1];
					tempReal		=	wncPtr[2 * tempIndCoarse + 1] * finputReal - wncPtr[2 * tempIndCoarse] * finputImag;
					tempImag		=	wncPtr[2 * tempIndCoarse] * finputReal + wncPtr[2 * tempIndCoarse + 1] * finputImag;

					sigAccReal		+=	wnfPtr[2 * tempIndFine1 + 1] * tempReal - wnfPtr[2 * tempIndFine1] * tempImag;
					sigAccImag		+=	wnfPtr[2 * tempIndFine1] * tempReal + wnfPtr[2 * tempIndFine1 + 1] * tempImag;
				}
				ftemp1				=	sigAccReal * sigAccReal + sigAccImag * sigAccImag;
				if( max1 < ftemp1)
				{
					max1			=	ftemp1;
					fineRangeInd	=	itemp2;
				}
				itemp2++;
			}
		}

		fdelta			=	(highAccuRangeHandle->maxBeatFreq) / ((float)highAccuRangeHandle->fft1DSize * (float)highAccuRangeHandle->fft1DSize);
		freqFineEst		=	fdelta * (float)(zoomStartInd2 * highAccuRangeHandle->fft1DSize + fineRangeInd);

		*estRange		=	(freqFineEst * (float)3e8 * highAccuRangeHandle->chirpRampTime) / (2.f * highAccuRangeHandle->chirpBandwidth);
#if 0
		if (highAccuRangeHandle->enablePhaseEst)
		{
			float phaseCoarseEst1, phaseCoarseEst2, phaseInitial;
			double PI = 3.14159265358979323846f;
			double real, imag, denom, initReal, initImag, dtemp1, dtemp2, dtemp;
			float phaseEst, totalPhase = 0.f, phaseCorrection, rangePhaseCorrection;
			__float2_t demodSig, corrSig;

			inputPtr			=	(__float2_t *)highAccuRangeHandle->inputSig;
			phaseCoarseEst1		=	2 * (float)PI * highAccuRangeHandle->fc * (divsp(2 * (*estRange), (float)3e8) + highAccuRangeHandle->adcStartTimeConst);
			phaseCoarseEst2		=	(float)PI * highAccuRangeHandle->chirpSlope * (divsp(2 * (*estRange), (float)3e8)  + highAccuRangeHandle->adcStartTimeConst) * (divsp(2 * (*estRange), (float)3e8)  + highAccuRangeHandle->adcStartTimeConst);
			phaseInitial		=	phaseCoarseEst1 - phaseCoarseEst2;

			denom				=	divdp(1.0, (double)highAccuRangeHandle->fft1DSize);
	#if 0
			dtemp1				=	cos((double)phaseInitial);
			dtemp2				=	sin(-(double)phaseInitial);
			initReal			=	cos(2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom) ;
			initImag			=	sin(-2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom);
	#else
			dtemp1				=	cosdp_i((double)phaseInitial);
			dtemp2				=	sindp_i(-(double)phaseInitial);
			initReal			=	cosdp_i(2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom) ;
			initImag			=	sindp_i(-2.0 * PI * highAccuRangeHandle->chirpRampTime * (double) freqFineEst * denom);
	#endif
		
			//sample @ t = 0;
			corrSig				=	_ftof2((float)dtemp1, (float)dtemp2);
			demodSig			=	_complex_mpysp(_amem8_f2(inputPtr++), corrSig);
			RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
			totalPhase			+=	phaseEst;

			if ((highAccuRangeHandle->enableFilter == 0) && (highAccuRangeHandle->enableLinearFit == 0))
			{
				//sample @ t = 1;
				dtemp			=	dtemp1 * initReal - dtemp2 * initImag;
				imag			=	dtemp1 * initImag + dtemp2 * initReal;
				real			=	dtemp;
				corrSig				=	_ftof2((float)real, (float)imag);
				demodSig			=	_complex_mpysp(_amem8_f2(inputPtr++), corrSig);
				RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
				totalPhase			+=	phaseEst;

				for( j = 2; j < (int32_t)highAccuRangeHandle->fft1DSize; j ++)
				{
					dtemp			=	real * initReal - imag * initImag;
					imag			=	real * initImag + imag * initReal;
					real			=	dtemp;
					corrSig			=	_ftof2((float)real, (float)imag);
					demodSig		=	_complex_mpysp(_amem8_f2(inputPtr++), corrSig);
					RADARDEMO_atan((cplxf_t *)&demodSig, &phaseEst);
					totalPhase		+=	phaseEst;
				}
				phaseCorrection		=	totalPhase * (float)denom;
				rangePhaseCorrection= divsp((phaseCorrection*(float)3e8), (4.f * (float) PI * highAccuRangeHandle->fc));
				*estRange			+=	rangePhaseCorrection;
			}
			else
			{
				//Not implemented yet
			}
		}
#endif

	return;
}