#define BLYNK_TEMPLATE_ID "TMPL6iQqrdToD"
#define BLYNK_TEMPLATE_NAME "ANMAN WEVICE"
#define BLYNK_AUTH_TOKEN "F8JuGkQJqtBqEPga4FY7wabzi_ZtFh7c"
#include <Wire.h>
#include "MAX30105.h"
#include "heartRate.h"
#include "spo2_algorithm.h"
#include <Adafruit_MLX90614.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>
#include <BlynkSimpleEsp32.h>

// Sensor and OLED objects
MAX30105 particleSensor;
Adafruit_MLX90614 mlx = Adafruit_MLX90614();
Adafruit_SH1106G display = Adafruit_SH1106G(128, 64, &Wire, -1);  // 128x64 OLED Display

// GSR sensor pin and MPX5700 pin
#define GSR_PIN 33
#define MPX5700_PIN 12
#define IN1 26
#define IN2 27
#define ENA 25
#define LED1 2   // D2
#define LED2 4   // D4
#define LED3 15  // D15
#define LED4 16  // D16

// Push button pin
#define BUTTON_PIN 19

// Variables for MAX30102
const byte RATE_SIZE = 4;
byte rates[RATE_SIZE];
byte rateSpot = 0;
long lastBeat = 0;

int32_t beatsPerMinute;
int beatAvg;

// Variables for SpO2
#define BUFFER_SIZE 100
uint32_t irBuffer[BUFFER_SIZE];
uint32_t redBuffer[BUFFER_SIZE];
int32_t bufferLength;
int32_t spo2;
int8_t validSPO2;
int8_t validHeartRate;

unsigned const long delayTime = 1000;
unsigned long lastUpdateTime = 0;
int gsr_value;

// Variables for MPX5700 (Blood Pressure)
int rawValue;
int tensioffset = 810; // Adjusted for calibration
int fullScale = 6630;
float pressure;
float mmhg;

float sistole = 0;
float diastole = 0;
int stage = 0;
float baseline = 0;

// Variable to track button state
bool detectionStarted = false;

// Variable to track WiFi connection timeout
unsigned long wifiConnectStartTime;
bool wifiConnected = false;

// Blynk authentication
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

void setup() {
  Serial.begin(115200);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  pinMode(LED4, OUTPUT);

  pinMode(BUTTON_PIN, INPUT_PULLUP); // Configure button pin

  // Initialize OLED
  if (!display.begin(0x3C, true)) {
    Serial.println("SH110X not found");
    while (1);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SH110X_WHITE);

  // Display WiFi connection status
  display.setCursor(0, 0);
  display.println("Connecting to WiFi...");
  display.display();

  // Initialize Blynk
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);
  wifiConnectStartTime = millis(); // Start WiFi connection timer

  // Check if connected to WiFi
  while (WiFi.status() != WL_CONNECTED) {
    if (millis() - wifiConnectStartTime > 15000) { // 15 seconds timeout
      display.clearDisplay();
      display.setCursor(0, 0);
      display.println("WiFi Not Connected");
      display.display();
      break;
    }
    delay(1000);
    display.setCursor(0, 10);
    display.print("Connecting...");
    display.display();
  }

  if (WiFi.status() == WL_CONNECTED) {
    wifiConnected = true;
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("WiFi Connected!");
    display.print("IP: ");
    display.println(WiFi.localIP());
    display.display();
  }

  // Initialize MAX30102 sensor
  if (!particleSensor.begin(Wire, I2C_SPEED_STANDARD)) {
    Serial.println("MAX30102 was not found. Please check wiring/power.");
    while (1);
  }

  digitalWrite(LED1, LOW);
  delay(1000);
  particleSensor.setup();
  particleSensor.setPulseAmplitudeRed(0x0A);
  particleSensor.setPulseAmplitudeGreen(0);

  bufferLength = BUFFER_SIZE;

  // Initialize MLX90614 sensor
  if (!mlx.begin()) {
    Serial.println("Error connecting to MLX90614. Please check wiring/power.");
    while (1);
  }
  digitalWrite(LED2, HIGH);
  delay(500);
  digitalWrite(LED2, LOW);
  delay(500);
  digitalWrite(LED4, HIGH);
  delay(500);
  digitalWrite(LED4, LOW);
  Serial.println("MLX90614 initialized.");

  // Calibration for MPX5700
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Calibrating MPX5700...");
  display.display();

  Serial.println("Starting calibration. Ensure no pressure on the sensor.");
  digitalWrite(LED3, HIGH);
  delay(500);

  digitalWrite(LED3, LOW);
  delay(5000); // 5 seconds for calibration
  baseline = getAveragePressure();
  tensioffset = baseline; // Adjust tensioffset based on calibration
  Serial.print("Calibration complete. tensioffset: ");
  Serial.println(tensioffset, 2);

  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Calibration Complete");
  display.display();
  delay(2000);
}

void loop() {
  Blynk.run();  // Run Blynk

  // Check if the button is pressed
  if (digitalRead(BUTTON_PIN) == LOW) {
    if (!detectionStarted) {
      detectionStarted = true;
      Serial.println("Button pressed. Starting detection...");
      delay(500); // Debounce delay
    }
  }

  // If detection has not started, display "PKM TEKNOLOGI"
  if (!detectionStarted) {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.setTextSize(1);
    display.println("PKM TEKNOLOGI");
    display.display();
    delay(500);
    return;
  }

  pressure = getAveragePressure();
  mmhg = pressureToMMHG(pressure);
  int pwmUp[] = { 0, 50, 100, 150, 200, 255 };
  int pwmDown[] = { 200, 150, 100, 50, 0 };

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  // Read GSR sensor value
  gsr_value = analogRead(GSR_PIN);
  int gsr_converted = map(gsr_value, 0, 1023, 0, 100);

  // Read data from MAX30102 sensor
  for (int i = 0; i < bufferLength; i++) {
    while (particleSensor.available() == false)
      particleSensor.check();
    redBuffer[i] = particleSensor.getRed();
    irBuffer[i] = particleSensor.getIR();
    particleSensor.nextSample();
  }

  // Calculate SpO2 and heart rate
  maxim_heart_rate_and_oxygen_saturation(irBuffer, bufferLength, redBuffer, &spo2, &validSPO2, &beatsPerMinute, &validHeartRate);

  // Read temperature from MLX90614
  float ambientTemp = mlx.readAmbientTempC();
  float objectTemp = mlx.readObjectTempC();

  // Measurement logic for blood pressure
  if (stage == 0 && mmhg > 90) {
    sistole = mmhg;
    stage = 1;
    Serial.println("Systole detected.");

    // Gradually stop the motor
    for (int i = 0; i < 5; i++) {
      analogWrite(ENA, pwmDown[i]);
      Serial.print("Decreasing PWM to: ");
      Serial.println(pwmDown[i]);
      delay(5000); // Delay for each stage
    }
  }

  if (stage == 1 && mmhg < 90 && mmhg > 50) {
    diastole = mmhg;
    stage = 2;
    Serial.println("Diastole detected.");

    // Stop the motor
    analogWrite(ENA, 0);
  }

  if (stage == 2 && mmhg < 50) {
    Serial.println("\n=== Measurement Results ===");
    Serial.print("Systole: ");
    Serial.print(sistole);
    Serial.println(" mmHg");
    Serial.print("Diastole: ");
    Serial.print(diastole);
    Serial.println(" mmHg");
    stage = 0; // Reset stage
  }

  // Update OLED display
  if (millis() - lastUpdateTime >= delayTime) {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("BPM: ");
    display.println(beatsPerMinute);
    display.print("SpO2: ");
    display.println(spo2);
    display.print("Temp: ");
    display.print(objectTemp);
    display.println(" C");
    display.print("GSR: ");
    display.println(gsr_converted);
    display.display();

    delay(2000); // Display data for 2 seconds

    // Display blood pressure data
    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Systole: ");
    display.print(sistole);
    display.println(" mmHg");
    display.print("Diastole: ");
    display.print(diastole);
    display.println(" mmHg");
    display.display();

    lastUpdateTime = millis(); // Update last update time
  }

  // Update Blynk values
  Blynk.virtualWrite(V0, beatsPerMinute);
  Blynk.virtualWrite(V1, spo2);
  Blynk.virtualWrite(V2, objectTemp);
  Blynk.virtualWrite(V3, gsr_converted);
  Blynk.virtualWrite(V4, sistole);
  Blynk.virtualWrite(V5, diastole);

  delay(1000); // Delay for stability
}

// Function to get average pressure from MPX5700 sensor
float getAveragePressure() {
  int totalADC = 0;
  rawValue = 0;
  for (int i = 0; i < 10; i++) {
    rawValue = analogRead(MPX5700_PIN);
    totalADC += rawValue;
    delay(10); // Short delay for stability
  }
  float avgADC = totalADC / 10.0;
  return (avgADC - tensioffset) * 700.0 / (fullScale - tensioffset);
}

// Convert pressure from kPa to mmHg
float pressureToMMHG(float pressureKPa) {
  return pressureKPa * 7.50062; // 1 kPa = 7.50062 mmHg
}