#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <U8g2_for_Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <TimeLib.h>
#include <DS1307RTC.h>
//#include "MAX30105.h"
//#include "spo2_algorithm.h"

Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);
U8G2_FOR_ADAFRUIT_GFX u8g2;
//MAX30105 particleSensor;

int32_t bufferLength; //data length
int32_t spo2; //SPO2 value
int8_t validSPO2; //indicator to show if the SPO2 calculation is valid
int32_t heartRate;
int32_t oldHeartRate; //heart rate value
int8_t validHeartRate; //indicator to show if the heart ra

// Heart Sensor
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)
uint16_t irBuffer[100]; //infrared LED sensor data
uint16_t redBuffer[100];  //red LED sensor data
#else
uint32_t irBuffer[100]; //infrared LED sensor data
uint32_t redBuffer[100];  //red LED sensor data
#endif
// Heart Sensor

tmElements_t tm;
const int numPins = 4;
const int analogPins[numPins] = {A0, A2, A1, A3};
const char* labels[] = {"P1:", "P2:", "P3:", "P4:"};

uint16_t rawValues[numPins];
uint8_t EMA_values[numPins];

uint8_t wTime;
bool allFiveUp = false;
bool doStuff = false;

void setup() 
{
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  u8g2.begin(display);  

  //if (!particleSensor.begin(Wire, I2C_SPEED_FAST)) //Use default I2C port, 400kHz speed
  //{
    //Serial.println(F("MAX30105 was not found. Please check wiring/power."));
    //while (1);
  //}

  /*particleSensor.setup( 35, //Options: 0=Off to 255=50mA,
                        2,  //Options: 1, 2, 4, 8, 16, 32
                        2,  //Options: 1 = Red only, 2 = Red + IR, 3 = Red + IR + Green
                        100, //Options: 50, 100, 200, 400, 800, 1000, 1600, 3200
                        411, //Options: 69, 118, 215, 411
                        4096 //Options: 2048, 4096, 8192, 16384
  ); */

  display.clearDisplay();
  u8g2.setFontMode(1);

  u8g2.setFont(u8g2_font_luRS19_tr);
  //u8g2.setForegroundColor(WHITE);
  u8g2.setCursor(27,40);
  u8g2.println(F("Hello!"));
  display.display();
  delay(2000);
}

void loop(){
  //pots
  display.clearDisplay();
  readAndFilterPots();
  inputHandler();
  u8g2.setFont(u8g2_font_5x7_mf);
  u8g2.setCursor(0,7);
  u8g2.print(labels[0]);
  u8g2.print(EMA_values[0]);

  u8g2.setCursor(33,7);
  u8g2.print(labels[1]);
  u8g2.print(EMA_values[1]);

  u8g2.setCursor(66,7);
  u8g2.print(labels[2]);
  u8g2.print(EMA_values[2]);

  u8g2.setCursor(99,7);
  u8g2.print(labels[3]);
  u8g2.print(EMA_values[3]);

  u8g2.setCursor(60,55);
  u8g2.print(doStuff);

  u8g2.setCursor(60,62);
  u8g2.print(wTime);

  if(allFiveUp == true){
    wTime++;

  if(wTime == 5){
    allFiveUp = false;
  }
}

  if (RTC.read(tm)) {
    showTimeOnDisplay();
    digitalWrite(LED_BUILTIN, LOW);
  } else {
    if (RTC.chipPresent()) {
      Serial.println("The DS1307 is stopped.  Please run the SetTime");
      Serial.println();
      u8g2.setFont(u8g2_font_fub17_tf);
      u8g2.setCursor(3,40);
      u8g2.print("S-Time");
    } else {
      Serial.println("DS1307 read error!  Please check the circuitry.");
      Serial.println();
      u8g2.setFont(u8g2_font_fub17_tf);
      u8g2.setCursor(3,40);
      u8g2.print("C-err!");
    }

  }
  //heartSensor();
  display.display();
}

uint8_t readAndFilterPots(){
  const float EMA_a = 0.15;
  for (uint8_t j = 0; j < 15; j++ ){
    for (uint8_t i = 0; i < numPins; i++) {
      rawValues[i] = analogRead(analogPins[i]);
      EMA_values[i] = (EMA_a * (uint8_t)rawValues[i]) + ((1 - EMA_a) * (uint8_t)EMA_values[i]);
    }
      delay(60);
}

}
void print2digits(int number) {
  if (number >= 0 && number < 10) {
    Serial.write('0');
  }
  Serial.print(number);
}

void showTimeOnDisplay(){
  u8g2.setFont(u8g2_font_inb19_mn);
  u8g2.setCursor(3,40);
  print2Digit(tm.Hour);
  u8g2.print(":");
  print2Digit(tm.Minute);
  u8g2.print(":");
  print2Digit(tm.Second);
}

void print2Digit(int number){
  if(number >= 0 && number <10){
    u8g2.print("0");
  }
  u8g2.print(number);
}

void inputHandler(){
  if (EMA_values[0] > 180 && EMA_values[1] > 180 && EMA_values[2] > 180 && EMA_values[3] > 180) {
    wTime = 0;
    allFiveUp = true;
  }

  if (EMA_values[0] < 30 && EMA_values[1] < 30 && EMA_values[2] < 30 && EMA_values[3] < 30 && allFiveUp == true) {
    doStuff = true;
    allFiveUp = false;
    //heartSensor();
  }
}
/*
void heartSensor(){
  bufferLength = 100; //buffer length of 100 stores 4 seconds of samples running at 25sps
  //read the first 100 samples, and determine the signal range
  for (byte i = 0 ; i < bufferLength ; i++)
  {
    while (particleSensor.available() == false) //do we have new data?
      particleSensor.check(); //Check the sensor for new data

    redBuffer[i] = particleSensor.getRed();
    irBuffer[i] = particleSensor.getIR();
    particleSensor.nextSample(); //We're finished with this sample so move to next sample

  }

  //calculate heart rate and SpO2 after first 100 samples (first 4 seconds of samples)
  maxim_heart_rate_and_oxygen_saturation(irBuffer, bufferLength, redBuffer, &spo2, &validSPO2, &heartRate, &validHeartRate);
  
  //Continuously taking samples from MAX30102.  Heart rate and SpO2 are calculated every 1 second
  for( uint8_t k = 0; k < 300; k++){
    {
      //dumping the first 25 sets of samples in the memory and shift the last 75 sets of samples to the top
      for (byte i = 25; i < 100; i++)
      {
        redBuffer[i - 25] = redBuffer[i];
        irBuffer[i - 25] = irBuffer[i];
      }

      //take 25 sets of samples before calculating the heart rate.
      for (byte i = 75; i < 100; i++)

      {
        while (particleSensor.available() == false) //do we have new data?
          particleSensor.check(); //Check the sensor for new data

        redBuffer[i] = particleSensor.getRed();
        irBuffer[i] = particleSensor.getIR();
        particleSensor.nextSample(); //We're finished with this sample so move to next sample

      }
      
      Serial.print(F(", HR="));
      Serial.print(heartRate, DEC);
      Serial.print(F(", SPO2="));
      Serial.print(spo2, DEC);
      //After gathering 25 new samples recalculate HR and SP02
      maxim_heart_rate_and_oxygen_saturation(irBuffer, bufferLength, redBuffer, &spo2, &validSPO2, &heartRate, &validHeartRate);

    }
    break;
  }
  
}*/