// LPF.c
// Runs on any microcontroller
// implements seven FIR low-pass filters

// Jonathan Valvano
// January 15, 2020

/* This example accompanies the books
   "Embedded Systems: Introduction to ARM Cortex M Microcontrollers",
      ISBN: 978-1469998749, Jonathan Valvano, copyright (c) 2020
   "Embedded Systems: Real Time Interfacing to ARM Cortex M Microcontrollers",
      ISBN: 978-1463590154, Jonathan Valvano, copyright (c) 2020
   "Embedded Systems: Real-Time Operating Systems for ARM Cortex-M Microcontrollers",
      ISBN: 978-1466468863, Jonathan Valvano, copyright (c) 2020

 Copyright 2020 by Jonathan W. Valvano, valvano@mail.utexas.edu
    You may use, edit, run or distribute this file
    as long as the above copyright notice remains
 THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 VALVANO SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL,
 OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 For more information about my classes, my research, and my books, see
 http://users.ece.utexas.edu/~valvano/
 */

#include <stdint.h>
#include "../RTOS_Labs_common/LPF.h"
// Newton's method
// s is an integer
// sqrt(s) is an integer
uint32_t sqrt2(uint32_t s){
uint32_t t;         // t*t will become s
int n;                   // loop counter to make sure it stops running
  t = s/10+1;            // initial guess 
  for(n = 16; n; --n){   // guaranteed to finish
    t = ((t*t+s)/t)/2;  
  }
  return t; 
}

#define FILTERMAX 16
//**************Low pass Digital filter**************
int32_t Size1;    // Size-point average, Size=2 to FILTERMAX
int32_t x1[FILTERMAX];   // one copy of data in MACQ
uint32_t I1;       // index to oldest
int32_t LPFSum1;  // sum of the last Size samples
int32_t sigma1;
int32_t snr1;      // signal to noise ratio
void LPF_Init(int32_t initial, int32_t size){ int i;
  if(size>FILTERMAX) size=FILTERMAX; // max
  Size1 = size;
  I1 = Size1-1;
  LPFSum1 = Size1*initial; // prime MACQ with initial data
  for(i=0; i<Size1; i++){
    x1[i] = initial;
  }
}
// calculate one filter output, called at sampling rate
// Input: new ADC data   Output: filter output
// y(n) = (x(n)+x(n-1)+...+x(n-Size-1)/Size
int32_t LPF_Calc(int32_t newdata){
  if(I1 == 0){
    I1 = Size1-1;              // wrap
  } else{
    I1--;                     // make room for data
  }
  LPFSum1 = LPFSum1+newdata-x1[I1];   // subtract oldest, add newest
  x1[I1] = newdata;     // save new data
  return LPFSum1/Size1;
}
int32_t Noise1(void){ int32_t sum,mean;
  if(Size1<2) return 0;
  sum = 0;
  for(int i=0;i<Size1;i++){
    sum = sum+x1[i];
  }
  mean = sum/Size1; // DC component
  sum = 0;
  for(int i=0;i<Size1;i++){
    sum = sum+(x1[i]-mean)*(x1[i]-mean); // total energy in AC part
  }
	sigma1 = sqrt2(sum/(Size1-1));
  snr1 = mean/sigma1;
  return sigma1;
}

int32_t Size2;         // Size-point average, Size=2 to FILTERMAX
int32_t x2[FILTERMAX]; // one copy of data in MACQ
uint32_t I2;            // index to oldest
int32_t LPFSum2;       // sum of the last Size samples
int32_t sigma2;
int32_t snr2;      // signal to noise ratio
void LPF_Init2(int32_t initial, int32_t size){ int i;
  if(size>FILTERMAX) size=FILTERMAX; // max
  Size2 = size;
  I2 = Size2-1;
  LPFSum2 = Size2*initial; // prime MACQ with initial data
  for(i=0; i<Size2; i++){
    x2[i] = initial;
  }
}
// calculate one filter output, called at sampling rate
// Input: new ADC data   Output: filter output, DAC data
// y(n) = (x(n)+x(n-1)+...+x(n-Size-1)/Size
int32_t LPF_Calc2(int32_t newdata){
  if(I2 == 0){
    I2 = Size2-1;              // wrap
  } else{
    I2--;                     // make room for data
  }
  LPFSum2 = LPFSum2+newdata-x2[I2];   // subtract oldest, add newest
  x2[I2] = newdata;     // save new data
  return LPFSum2/Size2;
}
int32_t Noise2(void){ int32_t sum,mean;
  if(Size2<2) return 0;
  sum = 0;
  for(int i=0;i<Size2;i++){
    sum = sum+x2[i];
  }
  mean = sum/Size2; // DC component
  sum = 0;
  for(int i=0;i<Size2;i++){
    sum = sum+(x2[i]-mean)*(x2[i]-mean); // total energy in AC part
  }
  sigma2 = sqrt2(sum/(Size2-1));
  snr2 = mean/sigma2;
  return sigma2;
}

int32_t Size3;         // Size-point average, Size=2 to FILTERMAX
int32_t x3[FILTERMAX]; // one copy of data in MACQ
uint32_t I3;            // index to oldest
int32_t LPFSum3;       // sum of the last Size samples
int32_t sigma3;
int32_t snr3;      // signal to noise ratio
void LPF_Init3(int32_t initial, int32_t size){ int i;
  if(size>FILTERMAX) size=FILTERMAX; // max
  Size3 = size;
  I3 = Size3-1;
  LPFSum3 = Size3*initial; // prime MACQ with initial data
  for(i=0; i<Size3; i++){
    x3[i] = initial;
  }
}
// calculate one filter output, called at sampling rate
// Input: new ADC data   Output: filter output, DAC data
// y(n) = (x(n)+x(n-1)+...+x(n-Size-1)/Size
int32_t LPF_Calc3(int32_t newdata){
  if(I3 == 0){
    I3 = Size3-1;              // wrap
  } else{
    I3--;                     // make room for data
  }
  LPFSum3 = LPFSum3+newdata-x3[I3];   // subtract oldest, add newest
  x3[I3] = newdata;     // save new data
  return LPFSum3/Size3;
}
int32_t Noise3(void){ int32_t sum,mean;
  if(Size3<2) return 0;
  sum = 0;
  for(int i=0;i<Size3;i++){
    sum = sum+x3[i];
  }
  mean = sum/Size3; // DC component
  sum = 0;
  for(int i=0;i<Size3;i++){
    sum = sum+(x3[i]-mean)*(x3[i]-mean); // total energy in AC part
  }
	sigma3 = sqrt2(sum/(Size3-1));
  snr3 = (mean)/sigma3;
  return sigma3;
}

int32_t Size4;    // Size-point average, Size=2 to FILTERMAX
int32_t x4[FILTERMAX];   // one copy of data in MACQ
uint32_t I4;       // index to oldest
int32_t LPFSum4;  // sum of the last Size samples
int32_t sigma4;
int32_t snr4;      // signal to noise ratio
void LPF_Init4(int32_t initial, int32_t size){ int i;
  if(size>FILTERMAX) size=FILTERMAX; // max
  Size4 = size;
  I4 = Size4-1;
  LPFSum4 = Size4*initial; // prime MACQ with initial data
  for(i=0; i<Size4; i++){
    x4[i] = initial;
  }
}
// calculate one filter output, called at sampling rate
// Input: new ADC data   Output: filter output, DAC data
// y(n) = (x(n)+x(n-1)+...+x(n-Size-1)/Size
int32_t LPF_Calc4(int32_t newdata){
  if(I4 == 0){
    I4 = Size4-1;              // wrap
  } else{
    I4--;                     // make room for data
  }
  LPFSum4 = LPFSum4+newdata-x4[I4];   // subtract oldest, add newest
  x4[I4] = newdata;     // save new data
  return LPFSum4/Size4;
}
int32_t Noise4(void){ int32_t sum,mean;
  if(Size4<2) return 0;
  sum = 0;
  for(int i=0;i<Size4;i++){
    sum = sum+x4[i];
  }
  mean = sum/Size4; // DC component
  sum = 0;
  for(int i=0;i<Size4;i++){
    sum = sum+(x4[i]-mean)*(x4[i]-mean); // total energy in AC part
  }
	sigma4 = sqrt2(sum/(Size4-1));
  snr4 = mean/sigma4;
  return sigma4;
}

int32_t Size5;    // Size-point average, Size=2 to FILTERMAX
int32_t x5[FILTERMAX];   // one copy of data in MACQ
uint32_t I5;       // index to oldest
int32_t LPFSum5;  // sum of the last Size samples
int32_t sigma5;
int32_t snr5;      // signal to noise ratio
void LPF_Init5(int32_t initial, int32_t size){ int i;
  if(size>FILTERMAX) size=FILTERMAX; // max
  Size5 = size;
  I5 = Size5-1;
  LPFSum5 = Size5*initial; // prime MACQ with initial data
  for(i=0; i<Size5; i++){
    x5[i] = initial;
  }
}
// calculate one filter output, called at sampling rate
// Input: new ADC data   Output: filter output, DAC data
// y(n) = (x(n)+x(n-1)+...+x(n-Size-1)/Size
int32_t LPF_Calc5(int32_t newdata){
  if(I5 == 0){
    I5 = Size5-1;              // wrap
  } else{
    I5--;                     // make room for data
  }
  LPFSum5 = LPFSum5+newdata-x5[I5];   // subtract oldest, add newest
  x5[I5] = newdata;     // save new data
  return LPFSum5/Size5;
}
int32_t Noise5(void){ int32_t sum,mean;
  if(Size5<2) return 0;
  sum = 0;
  for(int i=0;i<Size5;i++){
    sum = sum+x5[i];
  }
  mean = sum/Size5; // DC component
  sum = 0;
  for(int i=0;i<Size5;i++){
    sum = sum+(x5[i]-mean)*(x5[i]-mean); // total energy in AC part
  }
	sigma5 = sqrt2(sum/(Size5-1));
  snr5 = mean/sigma5;
  return sigma5;
}
int32_t Size6;    // Size-point average, Size=2 to FILTERMAX
int32_t x6[FILTERMAX];   // one copy of data in MACQ
uint32_t I6;       // index to oldest
int32_t LPFSum6;  // sum of the last Size samples
int32_t sigma6;
int32_t snr6;      // signal to noise ratio
void LPF_Init6(int32_t initial, int32_t size){ int i;
  if(size>FILTERMAX) size=FILTERMAX; // max
  Size6 = size;
  I6 = Size6-1;
  LPFSum6 = Size6*initial; // prime MACQ with initial data
  for(i=0; i<Size6; i++){
    x6[i] = initial;
  }
}
// calculate one filter output, called at sampling rate
// Input: new ADC data   Output: filter output, DAC data
// y(n) = (x(n)+x(n-1)+...+x(n-Size-1)/Size
int32_t LPF_Calc6(int32_t newdata){
  if(I6 == 0){
    I6 = Size6-1;              // wrap
  } else{
    I6--;                     // make room for data
  }
  LPFSum6 = LPFSum6+newdata-x6[I6];   // subtract oldest, add newest
  x6[I6] = newdata;     // save new data
  return LPFSum6/Size6;
}
int32_t Noise6(void){ int32_t sum,mean;
  if(Size6<2) return 0;
  sum = 0;
  for(int i=0;i<Size6;i++){
    sum = sum+x6[i];
  }
  mean = sum/Size6; // DC component
  sum = 0;
  for(int i=0;i<Size6;i++){
    sum = sum+(x6[i]-mean)*(x6[i]-mean); // total energy in AC part
  }
	sigma6 = sqrt2(sum/(Size6-1));
  snr6 = mean/sigma6;
  return sigma6;
}
int32_t Size7;    // Size-point average, Size=2 to FILTERMAX
int32_t x7[FILTERMAX];   // one copy of data in MACQ
uint32_t I7;       // index to oldest
int32_t LPFSum7;  // sum of the last Size samples
int32_t sigma7;
int32_t snr7;      // signal to noise ratio
void LPF_Init7(int32_t initial, int32_t size){ int i;
  if(size>FILTERMAX) size=FILTERMAX; // max
  Size7 = size;
  I7 = Size7-1;
  LPFSum7 = Size7*initial; // prime MACQ with initial data
  for(i=0; i<Size7; i++){
    x7[i] = initial;
  }
}
// calculate one filter output, called at sampling rate
// Input: new ADC data   Output: filter output, DAC data
// y(n) = (x(n)+x(n-1)+...+x(n-Size-1)/Size
int32_t LPF_Calc7(int32_t newdata){
  if(I7 == 0){
    I7 = Size7-1;              // wrap
  } else{
    I7--;                     // make room for data
  }
  LPFSum7 = LPFSum7+newdata-x7[I7];   // subtract oldest, add newest
  x7[I7] = newdata;     // save new data
  return LPFSum7/Size7;
}
int32_t Noise7(void){ int32_t sum,mean;
  if(Size7<2) return 0;
  sum = 0;
  for(int i=0;i<Size7;i++){
    sum = sum+x7[i];
  }
  mean = sum/Size7; // DC component
  sum = 0;
  for(int i=0;i<Size7;i++){
    sum = sum+(x7[i]-mean)*(x7[i]-mean); // total energy in AC part
  }
	sigma7 = sqrt2(sum/(Size7-1));
  snr7 = mean/sigma7;
  return sigma7;
}

int32_t u1,u2,u3;   // last three points
int32_t Median(int32_t newdata){
  int32_t result;
  u3 = u2;
  u2 = u1;
  u1 = newdata;
  if(u1>u2){
    if(u2>u3){
      result = u2;   // u1>u2,u2>u3       u1>u2>u3
    }else{
      if(u1>u3){
        result = u3;   // u1>u2,u3>u2,u1>u3 u1>u3>u2
      }else{ 
        result = u1;   // u1>u2,u3>u2,u3>u1 u3>u1>u2
      }
    }
  }else{
    if(u3>u2){
      result = u2;   // u2>u1,u3>u2       u3>u2>u1
    }else{
      if(u1>u3){
        result = u1;   // u2>u1,u2>u3,u1>u3 u2>u1>u3
      }
      else{ 
        result = u3;   // u2>u1,u2>u3,u3>u1 u2>u3>u1
      }
    }
  }
  return(result);
}

// if the ADC is sampled at 1 kHz 
// 60-Hz notch high-Q, IIR filter, assuming fs=2000 Hz
// y(n) = (256x(n) -503x(n-1) + 256x(n-2) + 498y(n-1)-251y(n-2))/256 (2k sampling)
// y(n) = (256x(n) -476x(n-1) + 256x(n-2) + 471y(n-1)-251y(n-2))/256 (1k sampling)
int32_t Filter(int32_t data){
static int32_t x[6]; // this MACQ needs twice memory allocation
static int32_t y[6];
static uint32_t n=3;   // 3, 4, or 5
  n++;
  if(n==6) n=3;
  x[n] = x[n-3] = data;  // two copies of new data
  y[n] = (256*(x[n]+x[n-2])-476*x[n-1]+471*y[n-1]-251*y[n-2]+128)/256;
  y[n-3] = y[n];         // two copies of filter outputs too
  return y[n];
}

// Independent second instance of the 60-Hz notch IIR filter (e.g. for left IR sensor)
int32_t Filter2(int32_t data){
static int32_t x[6];
static int32_t y[6];
static uint32_t n=3;
  n++;
  if(n==6) n=3;
  x[n] = x[n-3] = data;
  y[n] = (256*(x[n]+x[n-2])-476*x[n-1]+471*y[n-1]-251*y[n-2]+128)/256;
  y[n-3] = y[n];
  return y[n];
}

int32_t mx7[7]; // last 7 inputs
int32_t f7[7]; // found flag
// 11 usec running at 80 MHz
int32_t Median5(int32_t x){ int i,j; int32_t max;
  for(i=3; i>=0; i--){
    mx7[i+1] = mx7[i]; // push into MACQ
    f7[i]=1; // 1 means look at it
  }
  f7[5] = 1;
  mx7[0] = x;
  max = mx7[0]; j=0;
  for(i=1; i<7; i++){
    if(mx7[i] > max){
      max = mx7[i];
      j = i;
    }
  }
  f7[j] = 0; // 0 means ignore the max
  max = INT32_MIN;
  for(i=0; i<7; i++){
    if((mx7[i] > max)&&f7[i]){
      max = mx7[i];
      j = i;
    }
  }
  f7[j] = 0; // 0 means ignore the second highest
  max = INT32_MIN;
  for(i=0; i<7; i++){
    if((mx7[i] > max)&&f7[i]){
      max = mx7[i];
    }
  }
  return max; // median is third highest
}
// Independent second instance of Median5 with separate MACQ (avoids sharing mx7/f7 with Median5)
int32_t mx7_2[7];
int32_t f7_2[7];
int32_t Median5_2(int32_t x){ int i,j; int32_t max;
  for(i=3; i>=0; i--){
    mx7_2[i+1] = mx7_2[i];
    f7_2[i]=1;
  }
  f7_2[5] = 1;
  mx7_2[0] = x;
  max = mx7_2[0]; j=0;
  for(i=1; i<7; i++){
    if(mx7_2[i] > max){
      max = mx7_2[i];
      j = i;
    }
  }
  f7_2[j] = 0;
  max = INT32_MIN;
  for(i=0; i<7; i++){
    if((mx7_2[i] > max)&&f7_2[i]){
      max = mx7_2[i];
      j = i;
    }
  }
  f7_2[j] = 0;
  max = INT32_MIN;
  for(i=0; i<7; i++){
    if((mx7_2[i] > max)&&f7_2[i]){
      max = mx7_2[i];
    }
  }
  return max;
}

// 16 usec running at 80 MHz
int32_t Median7(int32_t x){ int i,j; int32_t max;
  for(i=5; i>=0; i--){
    mx7[i+1] = mx7[i]; // push into MACQ
    f7[i]=1; // 1 means look at it
  }
  f7[6] = 1;
  mx7[0] = x;
  max = mx7[0]; j=0;
  for(i=1; i<7; i++){
    if(mx7[i] > max){
      max = mx7[i];
      j = i;
    }
  }
  f7[j] = 0; // 0 means ignore the max
  max = INT32_MIN;
  for(i=0; i<7; i++){
    if((mx7[i] > max)&&f7[i]){
      max = mx7[i];
      j = i;
    }
  }
  f7[j] = 0; // 0 means ignore the second highest
  max = INT32_MIN;
  for(i=0; i<7; i++){
    if((mx7[i] > max)&&f7[i]){
      max = mx7[i];
      j = i;
    }
  }
  f7[j] = 0;// 0 means ignore the third highest
  max = INT32_MIN;
  for(i=0; i<7; i++){
    if((mx7[i] > max)&&f7[i]){
      max = mx7[i];
    }
  }
  return max; // median is fourth highest
}