#include <IntervalTimer.h>
#include <SD.h>
#include <SPI.h>
#define LED_red 0
#define LED_IR 1

int SpO2_OpenMV = 0;
int SpO2_Raw_Zacurate = 0;
int SpO2_Raw_Fab = 0;

int Arr_SpO2_OpenMV[25000];
int Arr_SpO2_Raw_Zacurate[25000];
int Arr_SpO2_Raw_Fab[25000];
int Arr_micros[25000];

int counter = 0;
int led_counter = 0;

IntervalTimer sampleTimer;
IntervalTimer ledTimer;

void writelog() {
  Arr_SpO2_OpenMV[counter] = SpO2_OpenMV;
  Arr_SpO2_Raw_Zacurate[counter] = SpO2_Raw_Zacurate;
  Arr_SpO2_Raw_Fab[counter] = SpO2_Raw_Fab;
  Arr_micros[counter] = micros();
  counter++;
}

void updateLEDs() {
  if (led_counter < 6) {
    digitalWrite(0, HIGH);
  }
  else if (led_counter < 12) {
    digitalWrite(0, LOW);
  }
  else if (led_counter < 18) {
    digitalWrite(1, HIGH);
  }
  else {
    digitalWrite(1, LOW);
  }
  led_counter++;
  if (led_counter > 72) {
    led_counter = 0;
  }
}

void setup() {
  Serial.begin(115200); //USB serial port
  Serial5.begin(19200); //OpenMV UART
  analogReadResolution(16);
  pinMode(0, OUTPUT);
  pinMode(1, OUTPUT);
  pinMode(13, OUTPUT);
  SD.begin(BUILTIN_SDCARD);
  delay(5000); //mostly to let the OpenMV board boot up
  sampleTimer.begin(writelog, 200); // 200 us period
  ledTimer.begin(updateLEDs, 100); // 100 us period
  
}

void loop() {
  int i;
  digitalWrite(13, HIGH);
  if (Serial5.available() > 0) {
    SpO2_OpenMV = Serial5.read();
  }
  SpO2_Raw_Zacurate = analogRead(4);
  SpO2_Raw_Fab = analogRead(9);
  
  if (counter == 25000) {
    noInterrupts();
    File dataFile = SD.open("datalog.txt", FILE_WRITE);
    if (dataFile) {
      for (i = 0; i < 25000; i++) {
        dataFile.print(Arr_micros[i]);
        dataFile.print(",");
        dataFile.print(Arr_SpO2_OpenMV[i]);
        dataFile.print(",");
        dataFile.print(Arr_SpO2_Raw_Zacurate[i]);
        dataFile.print(",");
        dataFile.println(Arr_SpO2_Raw_Fab[i]);  
      }
      dataFile.close();
    }
    counter = 0;
    interrupts();
  }

}