#define pi 3.14
#define order 41

float Sine[2000];
unsigned int len,a1,a2,a3;
unsigned int freq1,freq2,freq3;
unsigned int fsamp;
unsigned long t2,t1=0;
unsigned int cnt=0;
unsigned int newest,oldest;

//fc=20Hz, fs=2k, hamming
/*float coef[41] = {-0.00123135f,-0.00143461f,-0.00181368f,-0.00237257f,-0.00310944f,-0.00401643f,-0.00507971f,-0.00627981f,-0.00759209f,-0.00898739f,-0.01043294f,
-0.01189326f,-0.01333137f,-0.01470984f,-0.01599209f,-0.01714352f,-0.01813264f,-0.01893214f,-0.01951978f,-0.01987909f,0.98000000f,-0.01987909f,-0.01951978f,-0.01893214f,
-0.01813264f,-0.01714352f,-0.01599209f,-0.01470984f,-0.01333137f,-0.01189326f,-0.01043294f,-0.00898739f,-0.00759209f,-0.00627981f,-0.00507971f,-0.00401643f,
-0.00310944f,-0.00237257f,-0.00181368f,-0.00143461f,-0.00123135f};*/

//fc 20Hz, fs=2k, rectangle
float coef[41] = {-0.01513653f,-0.01557669f,-0.01600086f,-0.01640806f,-0.01679737f,-0.01716787f,-0.01751871f,-0.01784907f,-0.01815818f,-0.01844531f,-0.01870979f,
-0.01895100f,-0.01916837f,-0.01936139f,-0.01952961f,-0.01967263f,-0.01979011f,-0.01988177f,-0.01994740f,-0.01998684f,0.98000000f,-0.01998684f,-0.01994740f,
-0.01988177f,-0.01979011f,-0.01967263f,-0.01952961f,-0.01936139f,-0.01916837f,-0.01895100f,-0.01870979f,-0.01844531f,-0.01815818f,-0.01784907f,-0.01751871f,
-0.01716787f,-0.01679737f,-0.01640806f,-0.01600086f,-0.01557669f,-0.01513653f};
 
 float buffer[order];

void sineGen()
{   int freqMin;
    fsamp =  2000;
    freq1 = 5;
    freq2 = 10;
    freq3 = 100;
    freqMin = 5;
    a1 = 2;
    a2 = 1;
    a3 = 10;
    len = fsamp/freqMin;
    long m2,m1=micros();
    for (int i=0;i<len;i++){
      Sine[i]=a1*sin(2*pi*(float)freq1*(float)i/(float)fsamp)+a2*sin(2*pi*(float)freq2*(float)i/(float)fsamp)+a3*sin(2*pi*(float)freq3*(float)i/(float)fsamp);
    }
  
}

void initFilter(){
  for(int i=0;i<order;i++){
    buffer[i]=0.0;
  }
  newest=order-1;
  oldest=0;
}

float updateFilter(float xin){
  int ptr;
  float acc;
  acc=0;
  ptr=newest;
  buffer[ptr]=xin;
  for(int i=0;i<order;i++){
    acc=acc+buffer[ptr]*coef[i];
    if(ptr>0){
      ptr--;
    }else{
      ptr=order-1;
    }
  }
  if(newest>0){
    newest--;
  }else{
    newest=order-1;
  }
  return acc;
}

void setup() {
  //int Stat=0;
  Serial.begin(115200);
  
  sineGen();
}

void loop() {
  // put your main code here, to run repeatedly:
  unsigned long Ts=10;//Ts=1000000/fsamp;
  float mySignal;
  float filtSignal;
  t2=millis();//micros();
  if(t2-t1>=Ts){
    t1=t2;
    mySignal=Sine[cnt];
    filtSignal = updateFilter(mySignal);
    Serial.print(mySignal);Serial.print(',');Serial.println(filtSignal);
    cnt++;
    if(cnt>=len){
      cnt=0;
    }
  }

}