/************************************************************************
 *                                                                      *
 * Copyright (c) 2007 Synfora, Inc.  All rights reserved.               *
 *                                                                      *
 * This file contains Confidential Information of Synfora, Inc.         *
 * In addition, certain inventions disclosed in this file may be        *
 * claimed within patents owned or patent applications filed by         *
 * Synfora or third parties.  Any use of Synfora's copyrighted works,   *
 * confidential information, patented inventions, or patent-pending     *
 * inventions is subject to the terms and conditions of your written    *
 * license agreement with Synfora, Inc.                                 *
 * All other use and disclosure is strictly prohibited.                 *
 *                                                                      *
 ************************************************************************/

#include "pico.h"
#include "fir.h"

short x[L][M];

short w[N];
short w2[N];

short y2[L][M2-N];
short y3[L][M2-N];

char task_num;

FIFO(y_to_filter2, short)
FIFO(y_to_filter3, short)
#pragma bitsize pico_stream_output_y_to_filter2 13
#pragma bitsize pico_stream_output_y_to_filter3 13

void pico_stream_output_y2(short val);
void pico_stream_output_y3(short val);
#pragma bitsize pico_stream_output_y2 13
#pragma bitsize pico_stream_output_y3 13

void fir() {
  int i,j;

  // Create a fixed, 31-point rectangular window with cutoff frequency 150 hz
  const short w3[N] = {0x0032, 0x000f, 0xffe3, 0xffb6, 0xff92, 0xff7f, 0xff87, 0xffad,
                       0xfff2, 0x0051, 0x00c2, 0x013a, 0x01ab, 0x0207, 0x0243, 0x0258,
                       0x0243, 0x0207, 0x01ab, 0x013a, 0x00c2, 0x0051, 0xfff2, 0xffad,
                       0xff87, 0xff7f, 0xff92, 0xffb6, 0xffe3, 0x000f, 0x0032};

  short y[N];
#pragma bitsize y 13
#pragma internal_fast y

  unsigned char index = 0;

  for (i=0; i < M-N; i++) {
    short sum = 0;
    unsigned char read_index = index;
    for (j=0; j < N; j++) {
      if (i >= N) {
        pico_stream_output_y_to_filter2(y[read_index]);
        pico_stream_output_y_to_filter3(y[read_index]);

        // Rotating index around N values
        read_index = (read_index == N-1) ? 0 : read_index + 1;
      }

      sum += ((w[j] * x[task_num][i+j]) >> (EXPY-EXPX-EXPW));
    }
    y[index] = sum;

    // Rotating index around N values
    index = (index == N-1) ? 0 : index + 1;
  }

  for (i=0; i < M2-N; i++) {
    short sum = y2[task_num][i];
    for (j=0; j < N; j++) {
      sum += ((w2[j] * pico_stream_input_y_to_filter2()) >> (EXPY-EXPX-EXPW));
    }
    pico_stream_output_y2(sum);
  }

  for (i=0; i < M2-N; i++) {
    short sum = y3[task_num][i];
    for (j=0; j < N; j++) {
      sum += ((w3[j] * pico_stream_input_y_to_filter3()) >> (EXPY-EXPX-EXPW));
    }
    pico_stream_output_y3(sum);
  }
}
