/*
Copyright (c) 2017, Texas Instruments Incorporated - http://www.ti.com/
All rights reserved.

* 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 <string.h> // for memcpy
#include <xdc/std.h>
#include <xdc/runtime/Log.h>

#include <ti/sysbios/hal/Cache.h>

#include "common.h"

#ifndef _TMS320C6X
#include "c67x_cintrins.h"
#endif

#include "inpbuf.h"
#include "dbgDib.h"

// sinusoid generator parameters
/* Performs floating-point to 24-bit fixed point conversion.
Resulting fixed-point value is left-justified in 32-bit word. */
#define F2INT_SCALE     (float)0x7FFFFF
#define F2INT_ROUND(x)  _spint(x)
#define F2INT(x)        (((Int32)F2INT_ROUND(F2INT_SCALE * x) << 0x8) & 0xFFFFFF00)

#define TWO_PI          (6.283185307179586476925286766559L)
#define FS_48KHZ        (48000.0)
#define TWOPIOVERSRATE  ( TWO_PI / FS_48KHZ )

#define SINP_MAX_CHS    ( 8 )       // sin probe maximum number of channels
Int8 gSinPNumChs = SINP_MAX_CHS;    // sin probe number of channels 
Int8 gSinPChIdx = 0;                // sin probe channel index
// sinusoid data generated on these DIB channels
Int8 gSinPCh[SINP_MAX_CHS] = {0,1,2,3,4,5,6,7};

#define SINP_MAX_GEN ( 2 )      // sin probe maximum number of generators
// Configurable from CCS
Int8 gSinPNumGen = SINP_MAX_GEN;
float gSineProbeAmp[SINP_MAX_GEN]   = {0.0625, 0.125};  // sinusoid amplitudes
float gSineProbeFreq[SINP_MAX_GEN]  = {440.0, 1004.0};  // sinusoid frequencies (Hz)

static double gSineProbeArg[SINP_MAX_GEN] = {0.0, 0.0};  // sinusoid function arguments

#ifdef CAP_IB_PCM
// IB capture (PCM) buffer
#ifdef _TMS320C6X
#pragma DATA_SECTION(gCapIbPcmBuf, ".gCapIbPcmBuf");
Int32 gCapIbPcmBuf[CAP_IB_PCM_MAX_NUM_CH][CAP_IB_PCM_MAX_NUM_SAMP];
#else
Int32 gCapIbPcmBuf[CAP_IB_PCM_MAX_NUM_CH][CAP_IB_PCM_MAX_NUM_SAMP] __attribute__ ((section(".gCapIbPcmBuf")));
#endif
Int32 gCapIbPcmBufIdx=0;
Int32 gCapIbPcmBufWrapCnt=0;
static UInt32 capIbPcmStopCnt=5000;
#endif // CAP_IB_PCM

#ifdef CAP_IP
// IB capture buffer
#ifdef _TMS320C6X
#pragma DATA_SECTION(gCapIbBuf, ".gCapIbBuf");
Int8 gCapIbBuf[2][CAP_IB_BUF_SZ];
#else
Int8 gCapIbBuf[2][CAP_IB_BUF_SZ] __attribute__ ((section(".gCapIbBuf")));
//Int32 gCapIbBuf[CAP_IB_BUF_SZ] __attribute__ ((section(".noinit")));
#endif
Int32 gCapIbBufIdx[2]={0,0};
Int32 gCapIbBufWrapCnt[2]={0,0};
Int8 gCapIbBufPingPongSel=1;
Int32 gCapIbAccBytes=0;
Int32 gNumDiffFrame[2]={0,0};

#endif // CAP_IP

// Generate sinusoids in IB buffer
Void genSinIb(
    PAF_InpBufConfig *pInpBufConfig
)
{
    Int8 numCh;
    Int16 numSamp;
    Int8 genIdx;
    double phaseInc, arg, amp;
    Int32 *pCh;
    Int16 i;
    
    numCh = pInpBufConfig->stride; // get number of channels
    numSamp = pInpBufConfig->frameLength / numCh; // get number of samples to generate

    for (genIdx=0; genIdx<gSinPNumGen; genIdx++)
    {
        // compute generator phase increment
        phaseInc = (double)gSineProbeFreq[genIdx] * TWOPIOVERSRATE;
        
        arg = gSineProbeArg[genIdx]; // get generator arg
        amp = gSineProbeAmp[genIdx]; // get generator amplitude
        
        // generate sinusoid on selected channel
        pCh = &pInpBufConfig->pntr.pLgInt[gSinPCh[gSinPChIdx]];
        for (i=0; i<numSamp; i++)
        {
            *pCh = F2INT(amp * sin(arg));
            arg += phaseInc;
            pCh += numCh; // skipped interleaved channels
        }
        
        gSineProbeArg[genIdx] = arg; // save generator arg
        
        // update sin probe channel index
        gSinPChIdx++;
        if (gSinPChIdx >= gSinPNumChs)
        {
            gSinPChIdx = 0;
        }
    }
}

#ifdef CAP_IB_PCM
// Capture data in IB buffer to memory
Void capIbPcm(
    PAF_InpBufConfig *pInpBufConfig
)
{
    Int8 numCh;
    Int16 numSamp;
    Int8 sampSz;
    Int32 samp;
    Int8 *pCh;
    Int16 i, j, k;
    Int32 *pCapBuf;

    if (--capIbPcmStopCnt == 0)
    {
        SW_BREAKPOINT;
    }

    numCh = pInpBufConfig->stride; // get number of channels
    numSamp = pInpBufConfig->frameLength / numCh; // get number of samples to capture
    sampSz = pInpBufConfig->sizeofElement; // get sample size (bytes)
    
    if ((CAP_IB_PCM_MAX_NUM_SAMP - gCapIbPcmBufIdx) < numSamp)
    {
        //return;
        gCapIbPcmBufIdx = 0;
        gCapIbPcmBufWrapCnt++;
    }
        
    for (i=0; i<numCh; i++)
    {
        pCapBuf = &gCapIbPcmBuf[i][gCapIbPcmBufIdx];
        pCh = &pInpBufConfig->pntr.pSmInt[i*sampSz];
        for (j=0; j<numSamp; j++)
        {
            samp = (Int32)(*(pCh+sampSz-1));
            for (k=sampSz-2; k>=0; k--)
            {
                samp <<= 8;
                samp |= (UInt8)(*(pCh+k));
            }
            samp <<= 32-8*sampSz;
            
            *pCapBuf = samp;
            pCapBuf++;
            pCh += numCh * sampSz;
        }
    }
    gCapIbPcmBufIdx += numSamp;
}
#endif // CAP_IB_PCM

#ifdef CAP_IP
// Reset IB capture buffer
Int capIbReset(Void)
{
    gCapIbBufPingPongSel ^= 0x1;
    gCapIbBufIdx[gCapIbBufPingPongSel] = 0;
    gCapIbBufWrapCnt[gCapIbBufPingPongSel] = 0;
    gNumDiffFrame[gCapIbBufPingPongSel] = 0;
    gCapIbAccBytes=0;

    return 0;
}

// Capture data in IB buffer to memory
Void capIb(
    PAF_InpBufConfig *pInpBufConfig
)
{
    UInt32 nBytes, bufEnd, currentBufSize, chunkSize, wrapSize;
    
    nBytes = pInpBufConfig->frameLength * pInpBufConfig->sizeofElement;
    
#if 0
    // FL: DDP debug
    if (nBytes != 24576)
    {
        Log_info1("capIb(): nBytes=%d", nBytes);
        gNumDiffFrame[gCapIbBufPingPongSel]++;
    }
#endif    
    
    bufEnd = (Int) pInpBufConfig->base.pVoid + pInpBufConfig->sizeofBuffer;
    currentBufSize = (bufEnd - (Int)pInpBufConfig->pntr.pSmInt);

    if (currentBufSize >= nBytes)
        chunkSize = nBytes;
    else
        chunkSize = currentBufSize;

    wrapSize = nBytes - chunkSize;

    if ((CAP_IB_BUF_SZ - gCapIbBufIdx[gCapIbBufPingPongSel]) < nBytes)
    {
            //return; // fixed buffer
            gCapIbBufIdx[gCapIbBufPingPongSel] = 0;
            gCapIbBufWrapCnt[gCapIbBufPingPongSel]++;
    }

    // invalidate input data
    Cache_inv((Ptr)pInpBufConfig->pntr.pSmInt, chunkSize, Cache_Type_ALLD, 0);
    Cache_wait();

    memcpy(&gCapIbBuf[gCapIbBufPingPongSel][gCapIbBufIdx[gCapIbBufPingPongSel]], pInpBufConfig->pntr.pSmInt, chunkSize);

    gCapIbBufIdx[gCapIbBufPingPongSel] += chunkSize;
    gCapIbAccBytes += chunkSize;

    if(wrapSize > 0)
	{
		// invalidate input data
		Cache_inv((Ptr)pInpBufConfig->base.pSmInt, wrapSize, Cache_Type_ALLD, 0);
		Cache_wait();
		memcpy(&gCapIbBuf[gCapIbBufPingPongSel][gCapIbBufIdx[gCapIbBufPingPongSel]], pInpBufConfig->base.pSmInt, wrapSize);
		gCapIbBufIdx[gCapIbBufPingPongSel] += wrapSize;
		gCapIbAccBytes += wrapSize;
	}
}

#endif // CAP_IP