#include "stdafx.h"
#include "globals.h"
#include "CPI_Player.h"
#include "CPI_Equaliser.h"


typedef struct _CPs_EqualiserContext_Basic
{
    BOOL m_bEnabled;
    int m_aryLevels[10];
    BOOL m_bStreamIsCapable;
    unsigned int m_aryHistory[256];	
    unsigned int m_aryFuture[256];		
    unsigned int m_iCursor;

    int m_arySum_left[9];
    int m_arySum_right[9];

} CPs_EqualiserContext_Basic;


#define CIC_WRAPSAMPLE(expr)			(expr&0xFF)			
#define CIC_DECODESAMPLE_LEFT(expr)		(*(short*)&(expr))
#define CIC_DECODESAMPLE_RIGHT(expr)	(*((short*)&(expr) + 1))
#define CIC_FPMULTIPLY(expr1, expr2)	( ( (expr1) * (expr2) )>>8 )
#define CIC_TRUNCATESHORT(expr)    (((expr) > SHRT_MAX) ? SHRT_MAX : (((expr) < SHRT_MIN) ? SHRT_MIN : (expr)))

const int glb_iEQOffsets[] = { 1, 2, 4, 8, 16, 32, 64, 128, 256 };
const int glb_iEQOffsets_his[] = { -1, -2, -4, -8, -16, -32, -64, -128, -256 };


void CPP_EBSC_Initialise(CPs_EqualiserModule* pModule, const int iFrequency, const BOOL b16bit);
void CPP_EBSC_Uninitialise(CPs_EqualiserModule* pModule);
void CPP_EBSC_ApplySettings(CPs_EqualiserModule* pModule, const CPs_EQSettings* pSettings, BOOL* pbEnableStateChanged);
void CPP_EBSC_ApplyEQToBlock_Inplace(CPs_EqualiserModule* pModule, void* pPCMBlock, const DWORD dwBlockSize);
//
void CPI_Player_Equaliser_Initialise_Basic(CPs_EqualiserModule* pModule)
{
    CPs_EqualiserContext_Basic* pContext;

    pModule->Initialise = CPP_EBSC_Initialise;
    pModule->Uninitialise = CPP_EBSC_Uninitialise;
    pModule->ApplySettings = CPP_EBSC_ApplySettings;
    pModule->ApplyEQToBlock_Inplace = CPP_EBSC_ApplyEQToBlock_Inplace;

    pContext = (CPs_EqualiserContext_Basic*)malloc(sizeof(CPs_EqualiserContext_Basic));
    pModule->m_pModuleCookie = pContext;

    memset(pContext->m_aryLevels, 0, sizeof(pContext->m_aryLevels));
    pContext->m_bEnabled = FALSE;
}
//
void CPP_EBSC_Initialise(CPs_EqualiserModule* pModule, const int iFrequency, const BOOL b16bit)
{
    CPs_EqualiserContext_Basic* pContext = (CPs_EqualiserContext_Basic*)pModule->m_pModuleCookie;
    CP_TRACE0("Equaliser (re)initialising");

    memset(pContext->m_aryHistory, 0, sizeof(pContext->m_aryHistory));
    memset(pContext->m_aryFuture, 0, sizeof(pContext->m_aryFuture));
    memset(pContext->m_arySum_left, 0, sizeof(pContext->m_arySum_left));
    memset(pContext->m_arySum_right, 0, sizeof(pContext->m_arySum_right));
    pContext->m_iCursor = 0;

    if(iFrequency == 44100 && b16bit == TRUE)
        pContext->m_bStreamIsCapable = TRUE;
    else
        pContext->m_bStreamIsCapable = FALSE;
}
//
void CPP_EBSC_Uninitialise(CPs_EqualiserModule* pModule)
{
    CPs_EqualiserContext_Basic* pContext = (CPs_EqualiserContext_Basic*)pModule->m_pModuleCookie;
    CP_CHECKOBJECT(pContext);
    CP_TRACE0("Equaliser shutting down");

    free(pContext);
    pModule->m_pModuleCookie = NULL;
}
//
void CPP_EBSC_ApplySettings(CPs_EqualiserModule* pModule, const CPs_EQSettings* pSettings, BOOL* pbEnableStateChanged)
{
    CPs_EqualiserContext_Basic* pContext = (CPs_EqualiserContext_Basic*)pModule->m_pModuleCookie;
    int iBandIDX = 0;

    for(iBandIDX=0; iBandIDX < 8; iBandIDX++)
        pContext->m_aryLevels[9-iBandIDX] = (pSettings->m_aryBands[iBandIDX]<<1) + 256;

    pContext->m_aryLevels[0] = pContext->m_aryLevels[2];
    pContext->m_aryLevels[1] = pContext->m_aryLevels[2];

    if(pContext->m_bEnabled == pSettings->m_bEnabled)
        *pbEnableStateChanged = FALSE;
    else
        *pbEnableStateChanged = TRUE;
    pContext->m_bEnabled = pSettings->m_bEnabled;
}
//
void CPP_EBSC_ApplyEQToBlock_Inplace(CPs_EqualiserModule* pModule, void* _pPCMBlock, const DWORD dwBlockSize)
{
    CPs_EqualiserContext_Basic* pContext = (CPs_EqualiserContext_Basic*)pModule->m_pModuleCookie;
    const int iNumSamples = dwBlockSize >> 2;
    int* pSampleBase = (int*)_pPCMBlock;
    int iSampleIDX, iPointIDX;
    unsigned int iThisSample, iThisSample_EQ;
    int iThisSample_left, iThisSample_right;
    int aryCoefficient_left[9], aryCoefficient_right[9];
    int iClips = 0;

    if(pContext->m_bStreamIsCapable == FALSE || pContext->m_bEnabled == FALSE)
        return;

    for(iSampleIDX =0; iSampleIDX < iNumSamples; iSampleIDX++)
    {
        iThisSample = pContext->m_aryFuture[pContext->m_iCursor];
        pContext->m_aryFuture[pContext->m_iCursor] = pSampleBase[iSampleIDX];

        iThisSample_left = CIC_DECODESAMPLE_LEFT(iThisSample);
        iThisSample_right = CIC_DECODESAMPLE_RIGHT(iThisSample);


        for(iPointIDX =0; iPointIDX < 9; iPointIDX++)
        {
            pContext->m_arySum_left[iPointIDX] += CIC_DECODESAMPLE_LEFT(pContext->m_aryFuture[CIC_WRAPSAMPLE(pContext->m_iCursor+glb_iEQOffsets[iPointIDX])]);
            pContext->m_arySum_right[iPointIDX] += CIC_DECODESAMPLE_RIGHT(pContext->m_aryFuture[CIC_WRAPSAMPLE(pContext->m_iCursor+glb_iEQOffsets[iPointIDX])]);
        }

        for(iPointIDX =0; iPointIDX < 9; iPointIDX++)
        {
            aryCoefficient_left[iPointIDX] = pContext->m_arySum_left[iPointIDX] >> (iPointIDX+1);
            aryCoefficient_right[iPointIDX] = pContext->m_arySum_right[iPointIDX] >> (iPointIDX+1);
        }

        iThisSample_left = CIC_FPMULTIPLY(iThisSample_left - aryCoefficient_left[0], pContext->m_aryLevels[0]);
        iThisSample_right = CIC_FPMULTIPLY(iThisSample_right - aryCoefficient_right[0], pContext->m_aryLevels[0]);
        for(iPointIDX =0; iPointIDX < 8; iPointIDX++)
        {
            iThisSample_left += CIC_FPMULTIPLY(aryCoefficient_left[iPointIDX] - aryCoefficient_left[iPointIDX+1], pContext->m_aryLevels[iPointIDX+1]);
            iThisSample_right += CIC_FPMULTIPLY(aryCoefficient_right[iPointIDX] - aryCoefficient_right[iPointIDX+1], pContext->m_aryLevels[iPointIDX+1]);
        }
        iThisSample_left += CIC_FPMULTIPLY(aryCoefficient_left[8], pContext->m_aryLevels[9]);
        iThisSample_right += CIC_FPMULTIPLY(aryCoefficient_right[8], pContext->m_aryLevels[9]);

        for(iPointIDX =0; iPointIDX < 9; iPointIDX++)
        {
            pContext->m_arySum_left[iPointIDX] -= CIC_DECODESAMPLE_LEFT(pContext->m_aryHistory[CIC_WRAPSAMPLE(pContext->m_iCursor+glb_iEQOffsets_his[iPointIDX])]);
            pContext->m_arySum_right[iPointIDX] -= CIC_DECODESAMPLE_RIGHT(pContext->m_aryHistory[CIC_WRAPSAMPLE(pContext->m_iCursor+glb_iEQOffsets_his[iPointIDX])]);
        }

        *(short*)&iThisSample_EQ = CIC_TRUNCATESHORT(iThisSample_left);
        *((short*)&iThisSample_EQ + 1) = CIC_TRUNCATESHORT(iThisSample_right);

        pSampleBase[iSampleIDX] = iThisSample_EQ;
        pContext->m_aryHistory[pContext->m_iCursor] = iThisSample;

        pContext->m_iCursor = CIC_WRAPSAMPLE(pContext->m_iCursor+1);
    }
}
//