/* Filename: cascadeFilter
 *
 * Description: Multiplierless LWD filter implemented as a cascade of
 *              four third order filters. The purpose of this
 *              program is to exemplify the implementation in cases
 *              where the general multiplier is too costly.
 *               
 * Author: Juha Yli-Kaakinen
 * References: 
 * [1] J. Yli-Kaakinen and T. Saramäki, "A Systematic Algorithm for
 * the Synthesis of Multiplierless Lattice Wave Digital Filters,"
 * invited book chapter in Digital filters, Fausto Pedro García
 * Márquez (Ed.), ISBN: 978-953-307-190-9, Intech, pp. 257-289,
 * Apr. 2011.
 * [2] Gazsi, L, "Explicit formulas for lattice wave digital filters,"
 * IEEE Trans. Circuits Syst., Vol. CAS-32, No. 1, pp. 68--88, 1985.
 * 
 * (c) Juha Yli-Kaakinen,  7.2.2012
 */

#include <stdio.h>

#define BUFFER_SIZE 128 // Choose your favorite buffer size

void filter(long *buffer) {
  unsigned int n, k;
  // Internal variables
  long inp, out, D;
  // Delay elements
  static long d1[4] = {0}; 
  static long d2[4] = {0};
  static long d3[4] = {0};

  /* 1st two stages (two 3rd-order filters) */
  for (k = 0; k < 2; k++) 
    for (n = 0; n < BUFFER_SIZE; n++) { 
      // ========== 1st brach ========== 
      inp = buffer[n]>>1; // Scaling by 1/2 to prevent overflow
      
      // 1st adaptor : Type 1 [a = 2^(-1)+2^(-3)] 
      D = inp - d1[k]; 
      d1[k] = d1[k] + (D>>2)+(D>>3); 
      buffer[n] = d1[k] - D; 
      
      // ========== 2nd brach ========== 
      // 2nd adaptor : Type 1 [a = 1-2^(-3)+2^(-5)] 
      D = d2[k] - d3[k]; 
      d3[k] = d3[k] + (D>>3)-(D>>5); 
      out = d3[k] - D; 
      
      // 1st adaptor : Type 4 [a = -1+2^(-2)-2^(-5)] 
      D = out - inp; 
      d2[k] = (D>>2)-(D>>5) - out; 
      buffer[n] += d2[k] - D; 
    } 
  
  /* 3rd stage (3rd-order filter) */
  for (k = 2, n = 0; n < BUFFER_SIZE; n++) {
    // ========== 1st brach ==========
    inp = buffer[n]>>1; // Scaling by 1/2 to prevent overflow
    
    // 1st adaptor : Type 1 [a = 2^(-1)+2^(-3)+2^(-5)]
    D = inp - d1[k];
    d1[k] = d1[k] + (D>>1)-(D>>3)-(D>>5);
    buffer[n] = d1[k] - D;
      
    // ========== 2nd brach ==========
    // 2nd adaptor : Type 1 [a = 1-2^(-3)+2^(-5)]
    D = d2[k] - d3[k];
    d3[k] = d3[k] + (D>>4)+(D>>5);
    out = d3[k] - D;
    
    // 1st adaptor : Type 4 [a = -1+2^(-2)]
    D = out - inp;
    d2[k] = (D>>2) - out;
    buffer[n] += d2[k] - D;
  }

  /* 4th stage (3rd-order filter) */
  for (k = 3, n = 0; n < BUFFER_SIZE; n++) {
    // ========== 1st brach ==========
    inp = buffer[n]>>1; // Scaling by 1/2 to prevent overflow
    
    // 1st adaptor : Type 1 [a = 1-2^(-2)+2^(-5)]
    D = inp - d1[k];
    d1[k] = d1[k] + (D>>2)-(D>>5);
    buffer[n] = d1[k] - D;
      
    // ========== 2nd brach ==========
    // 2nd adaptor : Type 1 [a = 1-2^(-4)]
    D = d2[k] - d3[k];
    d3[k] = d3[k] + (D>>4);
    out = d3[k] - D;
    
    // 1st adaptor : Type 4 [a = -1+2^(-2)-2^(-4)]
    D = out - inp;
    d2[k] = (D>>2)-(D>>4) - out;
    buffer[n] += d2[k] - D;
  }
}

int main() {
  unsigned int counter = 0;
  char *inputFileName = "data/sines_long.bin";
  char *outputFileName = "data/output.bin";
  FILE *inputFile = fopen(inputFileName, "rb");
  FILE *outputFile = fopen(outputFileName, "wb");;
  long buffer[BUFFER_SIZE];

  if (BUFFER_SIZE%2 == 1) {
    fprintf(stderr, "BUFFER_SIZE has to be an even integer\n");
    return 1;
  }

  if (!inputFile) {
    fprintf(stderr, "Unable to open file %s\n", inputFileName);
    return 1;
  }

  if (!outputFile) {
    fprintf(stderr, "Unable to open file %s\n", outputFileName);
    return 1;
  }

  while (fread(buffer, sizeof(long), BUFFER_SIZE, inputFile) > 0) {
    filter(buffer);
    fwrite(buffer, sizeof(long), BUFFER_SIZE, outputFile);
    counter++; 
  }
  printf("Processed %i samples\n", counter*BUFFER_SIZE);

  fclose(inputFile);
  fclose(outputFile);
  
  return 0;
}