#include <Wire.h>                 // I2C  Library
#include <Adafruit_GFX.h>         // OLED Library
#include <Adafruit_SSD1306.h>     // OLED Library
#include <DallasTemperature.h>    // Temperature sensor Library
#include <Adafruit_MPU6050.h>     // Accelerometer library

//OLED Display Settings
#define SCREEN_WIDTH 128     // OLED display width, in pixels
#define SCREEN_HEIGHT 64     // OLED display height, in pixels
#define OLED_RESET -1        // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(OLED_RESET); //Declaring the display name (display)
//Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


//Accelerometer Setting
Adafruit_MPU6050 mpu;
sensors_event_t accelerometer, gyroscope, temp;
const float STEP_THRESHOLD = 1.5; // Adjust this value to fine-tune step detection
float accMag, gyroMag;
int stepCount = 0;

//Temperature Sensor Setting
#define ONE_WIRE_BUS 3          //define input pin
OneWire oneWire(ONE_WIRE_BUS);  // 
DallasTemperature temperatureSensors(&oneWire);

//Pulse Generator Setting
#define PULSE_PER_BEAT 1
#define PULSE_PIN     2
#define SAMPLING_INTERVAL 1000
volatile uint16_t pulse;
uint16_t count;
float heartRate;

//Blood oximeter & Blood Pressure Setting
#define SYS_PIN A0
#define DIA_PIN A1
#define SPO2_PIN A2
float sysReading, diaReading, spo2Reading;

//Interrupt function for pulse generator
void HeartRateInterrupt(){
  pulse++; //when interrupt occurs, treat it as 1 beat pulse
}

bool checkHeartRateRange(float pulseRate){
    if(pulseRate < 40 || pulseRate > 200){
      return true;
    }
    return false;
}

void setup() {
  //Setup serial monitor
  Serial.begin(115200);

  //Setup OLED display
  display.begin(SSD1306_SWITCHCAPVCC, 0x3D);
  delay(2000); 
  display.clearDisplay();
  display.setTextSize(1.5);
  display.setTextColor(WHITE);


  //Setup Temperature sensor
  temperatureSensors.begin();
  Serial.println("Temperature sensor is ready.");

  //Setup Accelerometer
  while(!mpu.begin()){
    Serial.println("Accelerometer not working.");
    delay(1000);
  }
  Serial.println("Accelerometer is ready.");

  //Setup pulse generator
  pinMode(PULSE_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(PULSE_PIN), HeartRateInterrupt, RISING); //trigger interrupt when the sine wave goes up
  Serial.println("Pulse sensor is ready.");

  //Setup blood pressure sensor & blood oximeter
  pinMode(SYS_PIN, INPUT);
  pinMode(DIA_PIN, INPUT);
  pinMode(SPO2_PIN, INPUT);
}

void loop() {
  //codes for adjusting the reading interval
  static unsigned long startTime;
  if (millis() - startTime < SAMPLING_INTERVAL){
    return;
  }
  startTime = millis();
  
  //Get temperature readings
  temperatureSensors.requestTemperatures();
  float temperature = temperatureSensors.getTempCByIndex(0);

  //Output temperature to OLED display and serial monitor
  display.clearDisplay();
  display.setCursor(2,2);
  display.print("BT: " + String(temperature));
  Serial.println("Body Temperature: " + String(temperature) +"C");

  //calculate and output heart rate
  count = pulse;
  pulse = 0;
  heartRate = map(count, 0, 220, 0, 220);

  //Output heart Rate to Oled display and Serial monitor
  display.print(" HR: " + String(heartRate, 0 ));
  Serial.println("Heart Rate: " + String(heartRate, 0) + " BPM");

  //Check pulse rate range
  if(checkHeartRateRange(heartRate)==true){
    Serial.println("Warning! Pulse rate outside normal range!!");
  }


  //Read blood pressure
  sysReading = analogRead(SYS_PIN);
  sysReading = map(sysReading, 0, 1023, 0, 250);
  diaReading = analogRead(DIA_PIN);
  diaReading = map(diaReading, 0, 1023, 0, 250);

  //Output disolved oxygen level to Oled display and Serial monitor
  display.setCursor(2,12);
  display.print("SYS:" +  String(sysReading, 0) + " DIA:" + String(diaReading,0));
  Serial.println("SYS: " + String(sysReading, 0) + "mmHg");
  Serial.println("DIA: " + String(diaReading, 0) + "mmHg");

  //Read disolved oxygen level in blood
  spo2Reading = analogRead(SPO2_PIN);
  spo2Reading = map(spo2Reading, 0, 1023, 0, 1000);
  
  //Output disolved oxygen level to Oled display and Serial monitor
  display.setCursor(2,22);
  display.print("SpO2:" +  String(spo2Reading/10, 1));
  Serial.println("SpO2: " + String(spo2Reading/10, 1) + "%");

  //Get Accelerometer and gyroscope data
  mpu.getEvent(&accelerometer, &gyroscope, &temp);
  
  //Calculate the changes in acceleration and orientation 
  accMag = sqrt(sq(accelerometer.acceleration.x) + sq(accelerometer.acceleration.y) + sq(accelerometer.acceleration.z));
  gyroMag = sqrt(sq(gyroscope.gyro.x) + sq(gyroscope.gyro.y) + sq(gyroscope.gyro.z));

  //Assume a step is done when either the values greater than the thershold
  if (accMag > STEP_THRESHOLD || gyroMag > STEP_THRESHOLD) {
    stepCount++;
  }

  //Output step count to Oled display and Serial monitor  
  display.print(" Stp:" + String(stepCount));
  Serial.println("Steps: " + String(stepCount));

  display.display();
}