#include <Wire.h>                 // I2C  Library
#include <Adafruit_GFX.h>         // OLED Library
#include <Adafruit_SSD1306.h>     // OLED Library
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include <DallasTemperature.h>    //Temperature sensor Library
#include <Adafruit_MPU6050.h>


//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(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); //Declaring the display name (display)
Adafruit_SSD1306 display(OLED_RESET); //Declaring the display name (display)

//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;

//Interrupt function for pulse generator
void HeartRateInterrupt(){
  pulse++;
}

void setup() {
  //Setup serial monitor
  Serial.begin(115200);
  Serial.println("Started Temperature sensor...");

  //Setup OLED display
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  delay(2000); 
  display.clearDisplay();

  //Setup Temperature sensor
  temperatureSensors.begin();

  //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);
  Serial.println("Started Pulse sensor...");

}

void loop() {
  //codes for 
  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
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  
  display.setCursor(0,0);
  display.print("Temp: ");
  display.print(temperature);
  display.print(" C");

  //Output temperature to Serial monitor
  Serial.print("Body Temperature: ");
  Serial.print(temperature);
  Serial.println("C");

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

  display.setCursor(0,10);
  display.print("HR: ");
  display.print(String(heartRate, 2 ) + " BPM");
  Serial.println("Heart Rate: " + String(heartRate, 2) + " BPM");
  
  

  mpu.getEvent(&accelerometer, &gyroscope, &temp);
  /*Serial.print(" X: ");
  Serial.print(accelerometer.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(accelerometer.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(accelerometer.acceleration.z);
  Serial.println(" m/s^2");*/
  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));
  /*Serial.print("Msg: ");
  Serial.print(String(accMag) + " ");
  Serial.println(String(gyroMag));*/



  if (accMag > STEP_THRESHOLD || gyroMag > STEP_THRESHOLD) {
    stepCount++;
  }

  Serial.print("Step detected! Count: ");
  Serial.println(stepCount);
  display.setCursor(0,20);
  display.print("Steps: ");
  display.print(String(stepCount));

  display.display();
}