/*
 Subroutines for IIR filter control: coefficient generation, transfer
 function calculation, etc.
 
 Copyright (c) 2009 Dimtel, Inc., All Rights Reserved

 $Id: subIIR.c,v 1.2 2012/11/11 05:48:57 dim Exp $
 $Date: 2012/11/11 05:48:57 $
 $Author: dim $
 $Revision: 1.2 $
 $Log: subIIR.c,v $
 Revision 1.2  2012/11/11 05:48:57  dim
 Updates in 2009-2012 range


*/

#include   <stdio.h>
#include   <string.h>
#include   <math.h>
#include   <complex.h>

#include   <dbDefs.h>
#include   <registryFunction.h>
#include   <subRecord.h>
#include <aSubRecord.h>
#include   <epicsExport.h>
#include   <dbAccess.h>

#include   <fftw3.h>

#include   "cmath.h"
#include   "map.h"

typedef long (*processMethod)(subRecord *precord);

static long subIIRinit(subRecord *psub, processMethod process)
{
  psub->val = 0;
  return 0;
}

#if 0
/* Generate IIR filter coefficients */
static long subIIRgen(aSubRecord *pgsub)
{
  /*
   * Inputs:  A = denominator radius
   *          B = denominator frequency
   *          C = gain
   * Outputs: A = Coefficient vector
   */
  char  comment[41];
  short int coeff[MAXFIRLEN];
  int k, taps;
  double gain, phase, freq, temp, cf[MAXFIRLEN];
  
  gain  = *((double *)pgsub->a);
  phase = *((double *)pgsub->b);
  freq  = *((double *)pgsub->c);
  taps  = *((long *)pgsub->d);
  
  /* Clip to MAXFIRLEN to prevent wild segfaults */
  if (taps > MAXFIRLEN)
    taps = MAXFIRLEN;

#if 0
  printf("subFilterGen called with gain=%0.2f;phase=%3.1f;freq=%0.2f;taps=%d\n",
         gain, phase, freq, taps);
#endif
  for (k = 0, temp = 0.0; k<taps; k++)
  {
    cf[k] = gain*sin(2.0*M_PI*freq*k + phase*M_PI/180.0);
    temp += cf[k];
  }
  temp /= taps;
  
  if (freq != 0.0)
  {
    for (k = 0; k < taps; k++)
      cf[k] -= temp;
    
    /* Find the largest coefficient */
    for (k = 0, temp = 0.0; k < taps; k++)
      if (fabs(cf[k]) > temp)
        temp = fabs(cf[k]);

    /* Normalize to gain */
    if (temp != 0.0)
    {
      for (k = 0; k < taps; k++)
        cf[k] *= gain/temp;
    }
  }
  for (k = 0; k < taps; k++)
    coeff[k] = (short int)(32767*cf[k]);
  
  for (; k < MAXFIRLEN; k++)
    coeff[k] = 0;

  snprintf(comment, 41, "Gain=%0.2f;Phase=%0.1f;Freq=%0.2f;Taps=%d",
          gain, phase, freq, taps);

  memcpy(pgsub->vala, coeff, MAXFIRLEN*sizeof(short));
  memcpy(pgsub->valb, comment, 40);
    
  return 0;
}
#endif

/* Double to integer conversion */

#define IIR_CLIP(a)	(a == full_scale) ? full_scale - 1 : \
                        (a == -full_scale) ? -full_scale + 1 : a;
static long subIIRconvert(aSubRecord *pgsub)
{
  /*
   * Inputs:  A = a1
   *          B = a2
   *          C = b0
   *          D = b1
   *          E = b2
   *          F = g0
   * Outputs: A = A1
   *          B = A2
   *          B = B0
   *          B = B1
   *          B = B2
   */
  double a1, a2, b0, b1, b2, g0;
  int    full_scale;
  
  a1 = *((double *)pgsub->a);
  a2 = *((double *)pgsub->b);
  b0 = *((double *)pgsub->c);
  b1 = *((double *)pgsub->d);
  b2 = *((double *)pgsub->e);
  g0 = *((double *)pgsub->f);

  full_scale = (1 << (IIR_COEFF_BITS-1));
  
  *(long int *)pgsub->vala = IIR_CLIP((long int)(-a1/2.0*full_scale + 0.5));
  *(long int *)pgsub->valb = IIR_CLIP((long int)(-a2*full_scale + 0.5));
  *(long int *)pgsub->valc = IIR_CLIP((long int)(g0*b0*full_scale + 0.5));
  *(long int *)pgsub->vald = IIR_CLIP((long int)(g0*b1*full_scale + 0.5));
  *(long int *)pgsub->vale = IIR_CLIP((long int)(g0*b2*full_scale + 0.5));
  
  return 0;
}

/* Compute FIR filter TF */
static long subIIRtf(aSubRecord *pgsub)
{
  /*
   * Inputs:  A = A1
   *          B = A2
   *          C = B0
   *          D = B1
   *          E = B2
   *          F = sampling frequency
   *          G = start frequency
   *          H = stop frequency
   * Outputs: A = Magnitude vector
   *          B = Phase vector
   *          C = Frequency vector
   */
  int points = pgsub->nova, k, full_scale = (1 << (IIR_COEFF_BITS - 1));
  long int A1, A2, B0, B1, B2;
  double fs, freq, f_start, f_stop;
  double complex z, H;
  
  A1      = *(long int *)pgsub->a;
  A2      = *(long int *)pgsub->b;
  B0      = *(long int *)pgsub->c;
  B1      = *(long int *)pgsub->d;
  B2      = *(long int *)pgsub->e;
  fs      = *(double *)  pgsub->f;
  f_start = *(double *)  pgsub->g;
  f_stop  = *(double *)  pgsub->h;
  
  for (k = 0; k < points; k++)
  {
    freq = f_start + k*(f_stop - f_start)/(points - 1);
    z = cexp_loc(-I*2*M_PI*freq/fs);
    H = (B0 + B1*z + B2*z*z)/(full_scale - 2*A1*z - A2*z*z);
    *((double *)pgsub->vala + k) = 20.0*log10(cabs(H)+1.0e-4);
    *((double *)pgsub->valb + k) = 180*carg_loc(H)/M_PI;
    *((double *)pgsub->valc + k) = freq;
  }

  return 0;
}

static registryFunctionRef subIIRRef[] = {
  {"subIIRinit", (REGISTRYFUNCTION)subIIRinit},
  {"subIIRconvert", (REGISTRYFUNCTION)subIIRconvert},
  {"subIIRtf", (REGISTRYFUNCTION)subIIRtf}
};

static void subIIRRegistrar(void)
{
  registryFunctionRefAdd(subIIRRef, NELEMENTS(subIIRRef));
}
epicsExportRegistrar(subIIRRegistrar);
