/*************************************************************
SBC Example PLC ANSI-C Source Code
This is copy from HFP spec, just only for study and demo.
Please don't use in commercial product.
File: sbcplc.c
*************************************************************/
#include <math.h>
//#include "sbc.h"
#include "sbcplc.h"
#include <stdlib.h>
/* Local Function Prototypes */
float CrossCorrelation(short *x, short *y);
int PatternMatch(short *y);
float AmplitudeMatch(short *y, short bestmatch);
/* Raised COSine table for OLA */
float rcos[OLAL] = {0.99148655f,0.96623611f,0.92510857f,0.86950446f,
                    0.80131732f,0.72286918f,0.63683150f,0.54613418f,
                    0.45386582f,0.36316850f,0.27713082f,0.19868268f,
                    0.13049554f,0.07489143f,0.03376389f,0.00851345f};

/*****************************************************************************
* Function: InitPLC() *
* Purpose: Perform PLC initialization of memory vectors. *
* Inputs: *plc_state - pointer to PLC state memory *
* Outputs: *plc_state - initialized memory. *
* Date: 03-18-2009
*****************************************************************************/
void InitPLC(struct PLC_State *plc_state)
{
    int i;
    plc_state->nbf=0;
    plc_state->bestlag=0;
    for (i=0;i<LHIST+SBCRT;i++)
    plc_state->hist[i] = 0;
}

/***********************************************************
* Function: PLC_bad_frame()
*
* Purpose: Perform bad frame processing.
*
* Inputs: *plc_state - pointer to PLC state memory
* *ZIRbuf - pointer to the ZIR response of the SBC decoder
*
* Outputs: *out - pointer to the output samples
*
* Date: 03-18-2009
************************************************************/
void PLC_bad_frame(struct PLC_State *plc_state, short *ZIRbuf, short *out)
{
    int i;
    float val;
    float sf;
    plc_state->nbf++;
    sf=1.0f;
    i=0;
    if (plc_state->nbf==1)
    {
        /* Perform pattern matching to find where to replicate */
        plc_state->bestlag = PatternMatch(plc_state->hist);
        plc_state->bestlag += M; /* the replication begins after the template match*/

        /* Compute Scale Factor to Match Amplitude of Substitution Packet to that of
        Preceding Packet */
        sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);

        for (i = 0; i < OLAL; i++)
        {
            val = ZIRbuf[i]*rcos[i] + sf*plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-i-1];
            if (val > 32767.0) val= 32767.0;
            if (val < -32768.0) val=-32768.0;
            plc_state->hist[LHIST+i] = (short)val;
        }

        for (;i<FS;i++)
        {
            val = sf*plc_state->hist[plc_state->bestlag+i];
            if (val > 32767.0) val= 32767.0;
            if (val < -32768.0) val=-32768.0;
            plc_state->hist[LHIST+i] = (short)val;
        }

        for (;i<FS+OLAL;i++)
        {
            val = sf*plc_state->hist[plc_state->bestlag+i]*rcos[i-FS]+plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-1-i+FS];
            if (val > 32767.0) val= 32767.0;
            if (val < -32768.0) val=-32768.0;
            plc_state->hist[LHIST+i] = (short)val;
        }

        for (;i<FS+SBCRT+OLAL;i++)
            plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
    }
    else
    {
        for (;i<FS;i++)
            plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
        for (;i<FS+SBCRT+OLAL;i++)
            plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
    }

    for (i=0;i<FS;i++)
        out[i] = plc_state->hist[LHIST+i];

    /* shift the history buffer */
    for (i=0;i<LHIST+SBCRT+OLAL;i++)
        plc_state->hist[i] = plc_state->hist[i+FS];
}

/****************************************************************************
*
* Function: PLC_good_frame()
*
* Purpose: Perform good frame processing. Most of the time, this function
* just updates history buffers and passes the input to the output,
* but in the first good frame after frame loss, it must conceal the
* received signal as it reconverges with the true output.
*
* Inputs: *plc_state - pointer to PLC state memory
* *in - pointer to the input vector
*
* Outputs: *out - pointer to the output samples
* Date: 03-18-2009
*****************************************************************************/
void PLC_good_frame(struct PLC_State *plc_state, short *in, short *out)
{
  int i;
  i=0;
  if (plc_state->nbf>0)
  {
    for (i=0;i<SBCRT;i++)
      out[i] = plc_state->hist[LHIST+i];
    for (;i<SBCRT+OLAL;i++)
      out[i] = (short)(plc_state->hist[LHIST+i]*rcos[i-SBCRT] + in[i]*rcos[OLAL-1-i+SBCRT]);
  }

  for (;i<FS;i++)
    out[i] = in[i];

  /*Copy the output to the history buffer */
  for (i=0;i<FS;i++)
    plc_state->hist[LHIST+i] = out[i];

  /* shift the history buffer */
  for (i=0;i<LHIST;i++)
    plc_state->hist[i] = plc_state->hist[i+FS];

  plc_state->nbf=0;
}

/****************************************************************************
*
* Function: CrossCorrelation()
*
* Purpose: Compute the cross correlation according to Eq. (4) of Goodman
* paper, except that the true correlation is used. His formula
* seems to be incorrect.
*
* Inputs: *x - pointer to x input vector
* *y - pointer to y input vector
*
* Outputs: Cn - return value containing the cross-correlation of x and y
*
* Date: 03-18-2009
*****************************************************************************/
float CrossCorrelation(short *x, short *y)
{
  int m;
  float num;
  float den;
  float Cn;
  float x2, y2;
  num=0;
  den=0;
  x2=0.0;
  y2=0.0;

  for (m=0;m<M;m++)
  {
    num+=((float)x[m])*y[m];
    x2+=((float)x[m])*x[m];
    y2+=((float)y[m])*y[m];
  }

  den = (float)sqrt(x2*y2);
  Cn = num/den;
  return(Cn);
}

/****************************************************************************
*
* Function: PatternMatch()
*
* Purpose: Perform pattern matching to find the match of template with the
* history buffer according to Section B of Goodman paper.
*
* Inputs: *y : pointer to history buffer
*
* Outputs: return(int): the lag corresponding to the best match. The lag is
* with respect to the beginning of the history buffer.
*
* Date: 03-18-2009
*****************************************************************************/
int PatternMatch(short *y)
{
  int n;
  float maxCn;
  float Cn;
  int bestmatch;
  maxCn=-999999.0; /* large negative number */
  bestmatch=0;

  for (n=0;n<N;n++)
  {
    Cn = CrossCorrelation(&y[LHIST-M] /* x */, &y[n]);
    if (Cn>maxCn)
    {
      bestmatch=n;
      maxCn = Cn;
    }
  }
  return(bestmatch);
}

/****************************************************************************
*
* Function: AmplitudeMatch()
*
* Purpose: Perform amplitude matching using mean-absolute-value according
* to Goodman paper.
*
* Inputs: *y : pointer to history buffer
* bestmatch : value of the lag to the best match
*
* Outputs: return(float): scale factor
*
* Date: 03-19-2009
*****************************************************************************/
float AmplitudeMatch(short *y, short bestmatch)
{
  int i;
  float sumx;
  float sumy;
  float sf;
  sumx = 0.0;
  sumy = 0.000001f;

  for (i=0;i<FS;i++)
  {
    sumx += abs(y[LHIST-FS+i]);
    sumy += abs(y[bestmatch+i]);
  }

  sf = sumx/sumy;

  /* This is not in the paper, but limit the scaling factor to something
  reasonable to avoid creating artifacts */
  if (sf<0.75f) sf=0.75f;

  if (sf>1.2f) sf=1.2f;

  return(sf);
}