/**************************************************************************
*
* ROUTINE
*               pgain
*
* FUNCTION
*               Find pitch gain and error
*
* SYNOPSIS
*               function pgain(ex, l, first, m, len, match)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ex[l]           float   i       excitation vector
*       l               int     i       size of ex
*       first           int     i       first call flag
*       m               int     i       pitch lag
*       len             int     i       length to truncate impulse response
*       match           float   o       negative partial squared error
*       pgain           float   fun     optimal gain for ex
*
*   external
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       e0[]            float   i
*       h[]             float   i
*
***************************************************************************/

#include "ccsub.h"
extern float e0[MAXLP], h[MAXLP];
extern int nseg;
float 
pgain(ex, l, first, m, len, match)
int l, first, m, len;
float ex[], *match;
{
  register float cor;
  static float eng, eng0;
  float y2[MAXLP]; 
  register float pgain;
  static float y[MAXLP];
  register int i;
  int j;

  register float temp;

  if (first)
  { 


    for (i = 0; i < len; i++)
    {
      y[i] = 0.0;
      for (j = 0; j <= i; j++)
	y[i] += h[j] * ex[i - j];
    }

#define FLIT(K)                  temp = h[0]*ex[K];     \
                                 temp += h[1]*ex[K-1];  \
                                 temp += h[2]*ex[K-2];  \
                                 temp += h[3]*ex[K-3];  \
                                 temp += h[4]*ex[K-4];  \
                                 y[K]  = temp;

        FLIT(5);
        FLIT(6);  FLIT(7);  FLIT(8);  FLIT(9);  FLIT(10); FLIT(11);
        FLIT(12); FLIT(13); FLIT(14); FLIT(15); FLIT(16); FLIT(17);
        FLIT(18); FLIT(19); FLIT(20); FLIT(21); FLIT(22); FLIT(23);
        FLIT(24); FLIT(25); FLIT(26); FLIT(27); FLIT(28); FLIT(29);
        FLIT(30); FLIT(31); FLIT(32); FLIT(33); FLIT(34); FLIT(35);
        FLIT(36); FLIT(37); FLIT(38); FLIT(39); FLIT(40); FLIT(41);
        FLIT(42); FLIT(43); FLIT(44); FLIT(45); FLIT(46); FLIT(47);
        FLIT(48); FLIT(49); FLIT(50); FLIT(51); FLIT(52); FLIT(53);
        FLIT(54); FLIT(55); FLIT(56); FLIT(57); FLIT(58); FLIT(59);

/*
    for (i = len; i < l; i++)
    {
      y[i] = 0.0;
      for (j = 0; j < len; j++)
	y[i] += h[j] * ex[i - j];
    }
*/
  }
  else
  {
  y[3] += ex[0]*h[4];
  y[2] += ex[0]*h[3];
  y[1] += ex[0]*h[2];
  y[0] += ex[0]*h[1];

/*     for (i = l - 1; i > 0; i--) y[i] = y[i - 1];      */
  memmove( (char *)(y+1), (char *)(y), (l-1)*sizeof(float) );

    y[0] = ex[0] * h[0];
  }

  /* *For lags (m) shorter than frame size (l), replicate the short
     *adaptive codeword to the full codeword length by
     *overlapping and adding the convolutions:			 	 */

/*  for (i = 0; i < l; i++) y2[i] = y[i];      */
  memcpy( (char *)(y2), (char *)(y), l*sizeof(float) );

  if (m < l)			
  {

    for (i = m; i < l; i++)
    {
      y2[i] = y[i] + y[i - m];
    }

    if (m < l / 2)		
    {
      j = 2*m;
      for (i = j; i < l; i++)
      {
         y2[i] = y2[i] + y[i - j];
      }
    }
  }

  /* *Calculate correlation and energy:
      e0 = r[n]   = spectrum prediction residual
      y2 = r[n-m] = error weighting filtered reconstructed
  	            pitch prediction signal (m = correlation lag)	 */

#define STEP(k)         cor += y2[k] * e0[k];
  cor = 0.0;
        STEP(0);  STEP(1);  STEP(2);  STEP(3);  STEP(4);  STEP(5);
        STEP(6);  STEP(7);  STEP(8);  STEP(9);  STEP(10); STEP(11);
        STEP(12); STEP(13); STEP(14); STEP(15); STEP(16); STEP(17);
        STEP(18); STEP(19); STEP(20); STEP(21); STEP(22); STEP(23);
        STEP(24); STEP(25); STEP(26); STEP(27); STEP(28); STEP(29);
        STEP(30); STEP(31); STEP(32); STEP(33); STEP(34); STEP(35);
        STEP(36); STEP(37); STEP(38); STEP(39); STEP(40); STEP(41);
        STEP(42); STEP(43); STEP(44); STEP(45); STEP(46); STEP(47);
        STEP(48); STEP(49); STEP(50); STEP(51); STEP(52); STEP(53);
        STEP(54); STEP(55); STEP(56); STEP(57); STEP(58); STEP(59);
#define ENG(k)   eng += y2[k] * y2[k];

     if(first || m<=l)
     {
  eng = 0.0;
       ENG(0);  ENG(1);  ENG(2);  
       eng0 = ENG(3);  ENG(4);  ENG(5);
        ENG(6);  ENG(7);  ENG(8);  ENG(9);  ENG(10); ENG(11);
        ENG(12); ENG(13); ENG(14); ENG(15); ENG(16); ENG(17);
        ENG(18); ENG(19); ENG(20); ENG(21); ENG(22); ENG(23);
        ENG(24); ENG(25); ENG(26); ENG(27); ENG(28); ENG(29);
        ENG(30); ENG(31); ENG(32); ENG(33); ENG(34); ENG(35);
        ENG(36); ENG(37); ENG(38); ENG(39); ENG(40); ENG(41);
        ENG(42); ENG(43); ENG(44); ENG(45); ENG(46); ENG(47);
        ENG(48); ENG(49); ENG(50); ENG(51); ENG(52); ENG(53);
        ENG(54); ENG(55); ENG(56); ENG(57); eng0-=ENG(58); 
       eng0+=ENG(59);
     }

     else
     {
        eng -= eng0;
        eng0 = y2[0] * y2[0];
        eng0 += y2[1] * y2[1];
        eng0 += y2[2] * y2[2];
        eng0 += y2[3] * y2[3];
        eng += y2[4] * y2[4];
        eng += eng0;
        eng0 += y2[59] * y2[59];
     }



  /* *Compute gain and error:
      NOTE: Actual MSPE = e0.e0 - pgain(2*cor-pgain*eng)
            since e0.e0 is independent of the code word,
	    minimizing MSPE is equivalent to maximizing:
	         match = pgain(2*cor-pgain*eng)   (1)
	    If unquantized pgain is used, this simplifies:
	         match = cor*pgain

      NOTE: When the delay is less than the frame length, "match"
	    is only an approximation to the actual error.		

      Independent (open-loop) quantization of gain and match (index):	 */

/*  if (eng <= 0.0) eng = 1.0;          */
  pgain = cor / eng;
  *match = cor * pgain;

  return (pgain);
}