// include the library code:
#include <LiquidCrystal.h>
#include <Wire.h> 

#define samp_siz 4
#define rise_threshold 4
#define maxperiod_siz 80

// calibration constants
float K=1;      // these have to be determined from the other oximeter
float M=0;

// the variables we're looking to calculate
float HB,R,SpO2;

// set up LEDs and transistor
int sensorPin = A1; 
int REDLed = 9;
int IRLed = 8;

// timing constant for sampling
int T = 20;    // milliseconds to read a value from the sensor

// some global variables
float irZero, avg, beat, before;
int rise_count = 0;
bool rising = false;
bool beating = false;
float maxIR,minIR,maxRed,minRed;
float beatlen,last_beat;

// initialize the LCD library by associating any needed LCD interface pin
// with the arduino pin number it is connected to
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
char str[16] = "Quarantine4Life!";

// output to computer via serial connection
void graphPrint(float line1, float line2, float line3, float line4)
{
  Serial.print(line1);
  Serial.print(",");
  Serial.print(line2);
  Serial.print(",");
  Serial.print(line3);
  Serial.print(",");
  Serial.print(line4);
  Serial.println();
}

void setup() {
  // set the connection to computer and clear data:
  Serial.begin(9600);
  Serial.flush();

  // set up the arduino pins for I/O
  pinMode(sensorPin,INPUT);
  pinMode(REDLed,OUTPUT);
  pinMode(IRLed,OUTPUT);

  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
  // Print a message to the LCD.
  lcd.print("Blood Analyzer");
  for (int i = 0; i< 16; i++) {
    lcd.setCursor(i, 1);
    lcd.print(str[i]);
    delay(250);
    }
  
  // flash red LED to say hello
  for (int i=0;i<3;i++){
    digitalWrite(REDLed,HIGH);
    delay(1000);
    digitalWrite(REDLed,LOW);
    delay(1000);
    }

  for (int i = 0; i< 16; i++) {
    lcd.setCursor(i, 1);
    lcd.print(" ");
    delay(100);
    }

  // set the irZero - baseline for the signal traces
  digitalWrite(IRLed,HIGH);
  for (int i=0;i<maxperiod_siz;i++)
  {
    irZero += readSignal();
  }
  irZero/=maxperiod_siz;
}

// read the transistor for T ms. Filters external noise
float readSignal() {
  int n = 0;
  float start = millis();
  float aSignal = 0.;
  do
  {
    aSignal+=analogRead(sensorPin);
    n++;
  }
  while(millis() < start + T);
  aSignal/=n;
  return aSignal;
}

// set up the data arrays
int ptr = 0;
int bptr = 0;
float readsIR[maxperiod_siz],beatIR[maxperiod_siz];
float readsRed[maxperiod_siz],beatRed[maxperiod_siz];

// calculate a four point moving average of the signal
float avgPrev (float data[maxperiod_siz],int p)
{
  float avg=0;
  if (p>=4)
  {
    for (int i=0;i<4;i++) { avg += data[p-i]; }
    avg/=4;
  } else {
    for (int i=0; i<=p;i++) { avg += data[p-i]; }
    avg/=(p+1);
  }
  return avg; 
}

// main program
void loop() {
  
  // turn on LEDs, capture data, turn them off
  digitalWrite(REDLed,LOW);
  digitalWrite(IRLed,HIGH);
  readsIR[ptr] = readSignal()-irZero;
  digitalWrite(IRLed,LOW);
  digitalWrite(REDLed,HIGH);
  readsRed[ptr] = readSignal()-irZero;
  digitalWrite(REDLed,LOW);

  // calculate an average (smoothes out the noise and additional peaks)
  avg = avgPrev(readsIR,ptr);

  if (avg > before)
  {
    rise_count++;
    if (!rising && rise_count > rise_threshold)
    {
      // the beat has started
      rising = true;
      beating = !beating;
      beatlen = millis() - last_beat;
      last_beat = millis();
      HB = 60000./beatlen;

      // initialise beat array counter and min,max vars
      bptr = 0;
      maxIR = -1023;
      minIR = 1023;
      maxRed = -1023;
      minRed = 1023;
    }
  } else {
    // do nothing on a descending IR slope
    rise_count = 0;
    rising = false;
  }

  if (beating)
  {
    // find max and min of IR and Red traces during the beat
    beatIR[bptr] = readsIR[ptr];
    if(beatIR[bptr]>maxIR) {maxIR=beatIR[bptr];}
    if(beatIR[bptr]<minIR) {minIR=beatIR[bptr];}
    beatRed[bptr] = readsRed[ptr];
    if(beatRed[bptr]>maxRed) {maxRed=beatRed[bptr];}
    if(beatRed[bptr]<minRed) {minRed=beatRed[bptr];}
    
    // calculate R, SpO2
    R = ((maxRed-minRed)/minRed) / ((maxIR-minIR)/minIR);
    SpO2 = K * R + M;   

    // output to LCD
    lcd.setCursor(0, 0);
    lcd.print("HB: ");
    lcd.print(int(HB));
    lcd.print(" R: ");
    lcd.print(R);
    lcd.setCursor(0,1);
    lcd.print(" SpO2: ");
    lcd.print(SpO2);

    // output to serial monitor (beat cycle)
    graphPrint(beatIR[bptr],beatRed[bptr],0,0);

    // advance array pointer
    bptr++;
    
  } else {
    // output to serial monitor (non beat cycle)
    graphPrint(0,0,readsIR[ptr],readsRed[ptr]);
  }

  // prepare for next cycle
  before = avg;

  // advance array pointer, reset to zero at end
  ptr++;
  ptr%=maxperiod_siz;
}