// Testing interrupt-based analog reading
// ATMega328p

// Note, many macro values are defined in <avr/io.h> and
// <avr/interrupts.h>, which are included automatically by
// the Arduino interface
#include <LiquidCrystal.h>

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

#include <Ewma.h>
Ewma L_press_filter(0.05);
Ewma R_press_filter(0.05);
Ewma RPM_filter(0.005);
#define sensor1 A1           // left pressure sensor
#define sensor2 A2           // right pressure sensor

#define DEBUG // toogle debug: uncomment-> debug ON


//volatile int analogVal_ADC4;     // Value to store analog result from ADC4
//volatile int analogVal_ADC5;     // Value to store analog result from ADC5

unsigned long TimeStamp = 0;
unsigned long previousTimeStampMin = 0;
unsigned long TimeStampTemp = 0;
unsigned long TimeStampMin = 20000;
unsigned long TimeStampDiff = 0;
int update_rate =500;
int prev_update =0;
int RPM;
int RPM_f;

int PressureLeft = 1023;
int previousPressureLeft = 1023;
int PressureLeftTemp = 1023;
int PressureLeftMin = 1023;
float PressureLeftFiltered = 1023;

int PressureRight = 1023;
int previousPressureRight = 1023;
int PressureRightTemp = 1023;
int PressureRightMin = 1023;
float PressureRightFiltered = 1023;

int DifferenceLeft = 0;
int DifferenceRight = 0;

const int RANGE = 3;

void setup() {
  Serial.begin(115200);

  pinMode(sensor1, INPUT);
  pinMode(sensor2, INPUT);

  // determine difference between senor values
  // get some values and find the lowest value
  for (int i=0; i <= 1000; i++){

    PressureLeft = analogRead(sensor1);
    PressureRight = analogRead(sensor2);
    if (PressureLeft < previousPressureLeft) {
      previousPressureLeft = PressureLeft;  // store the lowest value
    }
    if (PressureRight < previousPressureRight) {
      previousPressureRight = PressureRight;  // store the lowest value
    }
  }

  // compare the lowest values of ADC4 and ADC5, then calculating the difference
  if (previousPressureLeft > previousPressureRight){
    DifferenceRight = previousPressureLeft - previousPressureRight;
  } else {
    DifferenceLeft = previousPressureRight - previousPressureLeft;
  }
}
void loop(){

  // calibrate pressure sensors to the same levels
  PressureLeft = analogRead(sensor1) + DifferenceLeft;
  PressureRight = analogRead(sensor2) + DifferenceRight;

  // filter out noise
  if (PressureLeft > (PressureLeftFiltered + RANGE) || PressureLeft < (PressureLeftFiltered - RANGE)) {
    PressureLeftFiltered = PressureLeft;
  }
  if (PressureRight > (PressureRightFiltered + RANGE) || PressureRight < (PressureRightFiltered - RANGE)) {
    PressureRightFiltered = PressureRight;
  }

  TimeStamp = millis();

  // check for min values
  // The pressure may flutter at the top dead center of the engine.
  // To exclude this range, the measurement only starts at a value below 780.
  if (PressureLeftFiltered < previousPressureLeft && PressureLeftFiltered < 780) {
    PressureLeftTemp = PressureLeftFiltered;
    previousTimeStampMin = TimeStampMin;
    TimeStampTemp = TimeStamp;
  } else {
    PressureLeftMin = PressureLeftTemp;
    TimeStampMin = TimeStampTemp;
    TimeStampDiff = TimeStampMin - previousTimeStampMin;
    // In a four-stroke engine that uses a camshaft, each valve is opened every second
    // rotation of the crankshaft. According to this the camshaft runs with half speed
    // of the crankshaft. Because the time difference is derived from the camshaft (Intake to
    // Intake), we have to multiply by 2 to get the speed of the engine.
    // RPM = (1 / TimeStampDiff) * MSecPerSec * SecPerMin * 2
    //     = (1 / TimeStampDiff) * 1000 * 60 * 2
    //     = (1/ TimeStampDiff) * 120000
    RPM = 120000 / TimeStampDiff;
    RPM_f = RPM_filter.filter(RPM);
  }

  if (PressureRightFiltered < previousPressureRight && PressureRightFiltered < 780) {
    PressureRightTemp = PressureRightFiltered;
  } else {
    PressureRightMin = PressureRightTemp;
  }

  #ifdef DEBUG
  Serial.println(PressureLeftFiltered);
//  Serial.print("  ");
  #endif
  Serial.println(PressureLeftMin);
//  Serial.print("  ");
  #ifdef DEBUG
  //Serial.print(PressureRightFiltered);
  //Serial.print("  ");
  #endif
  //Serial.println(PressureRightMin);
  //Serial.println(RPM_f);


  if (TimeStamp-prev_update > update_rate) {
    prev_update = TimeStamp;
  	lcd.clear();
  	lcd.print(RPM_f);lcd.print(" RPM");
  }

  previousPressureLeft = PressureLeftFiltered;
  previousPressureRight = PressureRightFiltered;
}