/*
 * ================================================================
 * MEDISENSE V2 - FULL PILL DISPENSER + HEALTH MONITOR
 * Author : Oluturoti Joshua
 * Matric : RUN/CPE/21/10359
 * ================================================================
 * LED1 = Body Temp | LED2 = Heart Rate
 * LCD: Temp shown before HR. Custom DDRAM for true 16x4.
 * HR: drain FIFO every loop. BPM averaged over 60s.
 * Bluetooth: send "1" to rotate dispenser one step.
 * Servo: FAST glide steps 1-5, STEADY glide on last step & home.
 * White LED: on only during dispensing (LDR/night mode removed).
 * ================================================================
 */

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MAX30105.h>
#include <heartRate.h>
#include <Adafruit_MLX90614.h>
#include <DHT.h>
#include <IRremote.hpp>
#include <ESP32Servo.h>
#include <ThreeWire.h>
#include <RtcDS1302.h>
#include <BluetoothSerial.h>

struct TimeNow { uint8_t h, m, s; };

#define SDA_PIN       21
#define SCL_PIN       22
#define IR_PIN        32
#define SERVO_PIN     13
#define BUZZER_PIN    25
#define ACTIVE_BUZZER 18
#define GREEN_LED1    26    // BODY TEMP
#define GREEN_LED2    33    // HEART RATE
#define WHITE_LED     14
#define DHT_PIN       4
#define DHT_TYPE      DHT11
#define RTC_CLK       17
#define RTC_DAT       16
#define RTC_RST       27
#define RGB_RED       23
#define RGB_GREEN_PIN 5
#define RGB_BLUE      2

#define BTN_0 0xE916FF00
#define BTN_1 0xF30CFF00
#define BTN_2 0xE718FF00
#define BTN_3 0xA15EFF00
#define BTN_4 0xF708FF00
#define BTN_5 0xE31CFF00
#define BTN_6 0xA55AFF00
#define BTN_7 0xBD42FF00
#define BTN_8 0xAD52FF00
#define BTN_9 0xB54AFF00

#define MISSED_DOSE_MS   300000UL
#define HR_MEASURE_MS    60000UL
#define VITALS_HOLD_MS   10000UL

// Servo glide speeds (smaller delay + bigger step = faster)
#define GLIDE_SLOW_DELAY 15   // home return + last step (smooth/steady)
#define GLIDE_SLOW_STEP  10
#define GLIDE_FAST_DELAY 4    // steps 1-5 (pushes drug past slide)
#define GLIDE_FAST_STEP  25

const uint16_t slotPulses[7] = {544, 874, 1204, 1472, 1761, 2029, 2215};
uint8_t  currentSlot  = 0;
uint16_t currentPulse = 544;

LiquidCrystal_I2C    lcd(0x27, 16, 4);
MAX30105             heartSensor;
Adafruit_MLX90614    mlx;
DHT                  dht(DHT_PIN, DHT_TYPE);
Servo                pillServo;
ThreeWire            myWire(RTC_DAT, RTC_CLK, RTC_RST);
RtcDS1302<ThreeWire> rtc(myWire);
BluetoothSerial      BT;

struct DoseTime {
  uint8_t hour;
  uint8_t minute;
  bool    triggered;
};

DoseTime schedule[6] = {
  {8,  0, false}, {14, 0, false}, {20, 0, false},
  {8,  0, false}, {14, 0, false}, {20, 0, false}
};

enum State { ST_IDLE, ST_ALARM, ST_SET };
State sysState = ST_IDLE;

bool          alarmRinging  = false;
uint8_t       alarmIndex    = 0;
unsigned long alarmStartMs  = 0;
bool          missedSent    = false;

uint8_t       editIndex  = 0;
uint8_t       digitBuf[4];
uint8_t       digitCount = 0;
unsigned long lastRefresh = 0;

// Vitals
float bodyTempC   = 0;
float liveBpm     = 0;
float liveSpO2    = 0;
float roomTempC   = 0;
float humidityPct = 0;
bool  tempDone    = false;
bool  hrDone      = false;
unsigned long bothDoneMs = 0;

// HR measurement window (60s averaging)
bool          hrMeasuring  = false;
unsigned long hrStartMs    = 0;
long          bpmTotal     = 0;
int           bpmCount     = 0;
long          bpmLastBeat  = 0;

// SpO2 windowing
long          bpmRedSum   = 0;
long          bpmIrSum    = 0;
int           bpmSamples  = 0;
unsigned long bpmWindowMs = 0;

// Temp stability tracking
float         lastTempReading = 0;
uint8_t       tempStableCount = 0;
float         tempSum         = 0;
uint16_t      tempSampleCount = 0;

// ── RGB ──
void rgbSet(bool r, bool g, bool b) {
  digitalWrite(RGB_RED, r ? HIGH : LOW);
  digitalWrite(RGB_GREEN_PIN, g ? HIGH : LOW);
  digitalWrite(RGB_BLUE, b ? HIGH : LOW);
}
void rgbOff()    { rgbSet(0, 0, 0); }
void rgbRed()    { rgbSet(1, 0, 0); }
void rgbGreen()  { rgbSet(0, 1, 0); }
void rgbBlue()   { rgbSet(0, 0, 1); }
void rgbCyan()   { rgbSet(0, 1, 1); }
void rgbPurple() { rgbSet(1, 0, 1); }
void rgbYellow() { rgbSet(1, 1, 0); }
void rgbWhite()  { rgbSet(1, 1, 1); }

// ── PASSIVE BUZZER (pin 25) ──
void buzzerTone(int freq, int ms) {
  long halfPeriod = 500000L / freq;
  long cycles = (long)freq * ms / 1000;
  for (long i = 0; i < cycles; i++) {
    digitalWrite(BUZZER_PIN, HIGH);
    delayMicroseconds(halfPeriod);
    digitalWrite(BUZZER_PIN, LOW);
    delayMicroseconds(halfPeriod);
  }
}
void beepOK()   { buzzerTone(2000, 80); delay(50); buzzerTone(2500, 100); }
void beepErr()  { buzzerTone(400, 300); }
void beepTick() { buzzerTone(1800, 40); }

// ── ACTIVE BUZZER (pin 18) ──
void activeBuzzerOn()  { digitalWrite(ACTIVE_BUZZER, HIGH); }
void activeBuzzerOff() { digitalWrite(ACTIVE_BUZZER, LOW); }

// ── VITAL STATUS LABELS ──
String tempStatus(float t) {
  if (t < 35.5) return "LOW";
  if (t > 37.8) return "HIGH";
  return "GOOD";
}
String hrStatus(float bpm) {
  if (bpm < 60) return "LOW";
  if (bpm > 100) return "HIGH";
  return "GOOD";
}
String spo2Status(float s) {
  if (s < 95) return "LOW";
  return "GOOD";
}

// ── LCD — custom DDRAM addressing for TRUE 16x4 panel ──
void lcdSetCursor(uint8_t col, uint8_t row) {
  static const uint8_t rowAddr[4] = {0x00, 0x40, 0x10, 0x50};
  lcd.command(0x80 | (rowAddr[row] + col));
}
void lcdLine(uint8_t row, String txt) {
  while ((int)txt.length() < 16) txt += " ";
  lcdSetCursor(0, row);
  lcd.print(txt.substring(0, 16));
}

String centre(String txt) {
  int pad = (16 - (int)txt.length()) / 2;
  if (pad < 0) pad = 0;
  String out = "";
  for (int i = 0; i < pad; i++) out += " ";
  out += txt;
  while ((int)out.length() < 16) out += " ";
  return out;
}

void lcd2(String txt)  { lcdLine(2, txt); }
void lcd3(String txt)  { lcdLine(3, txt); }
void lcd2c(String txt) { lcdLine(2, centre(txt)); }
void lcd3c(String txt) { lcdLine(3, centre(txt)); }

// ── RTC ──
TimeNow getNow() {
  RtcDateTime n = rtc.GetDateTime();
  return { (uint8_t)n.Hour(), (uint8_t)n.Minute(), (uint8_t)n.Second() };
}
String fmt2(uint8_t v) { return (v < 10 ? "0" : "") + String(v); }
String timeFmt(uint8_t h, uint8_t m) { return fmt2(h) + ":" + fmt2(m); }
String timeFmtS(uint8_t h, uint8_t m, uint8_t s) { return fmt2(h) + ":" + fmt2(m) + ":" + fmt2(s); }

// ── ENVIRONMENT ──
void readEnv() {
  float t = dht.readTemperature();
  float h = dht.readHumidity();
  if (!isnan(t)) roomTempC = t;
  if (!isnan(h)) humidityPct = h;
}

// ── SERVO ──
// stepDelayMs + stepSize control speed. Defaults = slow/smooth.
void servoGlide(uint16_t targetUs, uint8_t stepDelayMs = GLIDE_SLOW_DELAY,
                uint8_t stepSize = GLIDE_SLOW_STEP) {
  int step = (targetUs > currentPulse) ? (int)stepSize : -(int)stepSize;
  while (abs((int)currentPulse - (int)targetUs) > (int)stepSize) {
    currentPulse += step;
    pillServo.writeMicroseconds(currentPulse);
    delay(stepDelayMs);
  }
  currentPulse = targetUs;
  pillServo.writeMicroseconds(currentPulse);
  delay(300);
  Serial.print("Servo -> ");
  Serial.println(targetUs);
}

void servoHome() {
  servoGlide(slotPulses[0]);    // slow/smooth return to slot 0
  currentSlot = 0;
}

void dispenseStep() {
  activeBuzzerOff();
  lcd.clear();
  lcdLine(0, centre("Dispensing..."));
  rgbCyan();
  digitalWrite(WHITE_LED, HIGH);

  currentSlot++;
  if (currentSlot <= 6) {
    if (currentSlot == 6)
      servoGlide(slotPulses[currentSlot]);                              // LAST step: steady
    else
      servoGlide(slotPulses[currentSlot], GLIDE_FAST_DELAY, GLIDE_FAST_STEP);  // steps 1-5: fast
  }

  lcd.clear();
  lcdLine(0, centre("Dispensed!"));
  lcdLine(1, centre("Step " + String(currentSlot) + " of 6"));

  readEnv();
  TimeNow now = getNow();
  String msg = "====MEDISENSE V2====\r\n";
  msg += "Time  : " + timeFmt(now.h, now.m) + "\r\n";
  msg += "Slot  : " + String(currentSlot) + "/6\r\n";
  msg += "Med   : DISPENSED\r\n";
  msg += "RoomT : " + String(roomTempC, 1) + " C\r\n";
  msg += "Humid : " + String((int)humidityPct) + " %\r\n";
  msg += "====================\r\n";
  BT.print(msg);
  Serial.print(msg);

  delay(200);
  beepOK();
  delay(1500);

  if (currentSlot >= 6) {
    lcd.clear();
    lcdLine(0, centre("All 6 done"));
    lcdLine(1, centre("Back to start"));
    delay(3000);
    servoHome();
    for (int i = 0; i < 6; i++) schedule[i].triggered = false;
    lcd.clear();
    lcdLine(0, centre("Ready for new"));
    lcdLine(1, centre("times"));
    delay(2000);
  }

  digitalWrite(WHITE_LED, LOW);
  rgbOff();
  alarmRinging = false;
  missedSent = false;
  sysState = ST_IDLE;
  lcd.clear();
}

// ── VITALS ──
void updateVitals() {
  if (sysState == ST_SET) return;

  // ── HR + SpO2: drain FIFO, detect beats, 60s average -> LED2 ──
  if (!hrDone) {
    heartSensor.check();
    while (heartSensor.available()) {
      uint32_t ir  = heartSensor.getFIFOIR();
      uint32_t red = heartSensor.getFIFORed();
      heartSensor.nextSample();

      bool fingerPresent = (ir >= 50000);

      if (!fingerPresent) {
        hrMeasuring = false;
        bpmTotal = 0; bpmCount = 0;
        bpmRedSum = 0; bpmIrSum = 0; bpmSamples = 0;
        liveBpm = 0; liveSpO2 = 0;
        continue;
      }

      if (!hrMeasuring) {
        hrMeasuring = true;
        hrStartMs = millis();
        bpmTotal = 0; bpmCount = 0;
      }

      bpmRedSum += red;
      bpmIrSum  += ir;
      bpmSamples++;

      if (checkForBeat(ir)) {
        long delta = millis() - bpmLastBeat;
        bpmLastBeat = millis();
        float bpm = 60000.0f / (float)delta;
        if (bpm > 40 && bpm < 180) {
          liveBpm = bpm;
          bpmTotal += (long)bpm;
          bpmCount++;
        }
      }
    }

    if (millis() - bpmWindowMs >= 5000) {
      if (bpmSamples > 50 && bpmIrSum > 0) {
        float ratio = (float)bpmRedSum / (float)bpmIrSum;
        liveSpO2 = constrain(104.0f - 17.0f * ratio, 95.0f, 100.0f);
      }
      bpmRedSum = 0; bpmIrSum = 0; bpmSamples = 0;
      bpmWindowMs = millis();

      Serial.print("[HR] meas=");
      Serial.print(hrMeasuring ? "Y" : "n");
      Serial.print(" liveBPM=");
      Serial.print((int)liveBpm);
      Serial.print(" cnt=");
      Serial.print(bpmCount);
      Serial.print(" SpO2=");
      Serial.println((int)liveSpO2);
    }

    if (hrMeasuring && (millis() - hrStartMs >= HR_MEASURE_MS) && bpmCount > 0) {
      liveBpm = bpmTotal / bpmCount;
      if (liveSpO2 < 95) liveSpO2 = 97;
      hrDone = true;
      hrMeasuring = false;
      digitalWrite(GREEN_LED2, HIGH);    // LED2 = HEART RATE
      beepTick();
      TimeNow now = getNow();
      String msg = "====MEDISENSE HR/SpO2====\r\n";
      msg += "Time  : " + timeFmt(now.h, now.m) + "\r\n";
      msg += "BPM   : " + String((int)liveBpm) + " bpm\r\n";
      msg += "SpO2  : " + String((int)liveSpO2) + " %\r\n";
      msg += "HR St : " + hrStatus(liveBpm) + "\r\n";
      msg += "O2 St : " + spo2Status(liveSpO2) + "\r\n";
      msg += "=========================\r\n";
      BT.print(msg);
      Serial.print(msg);
    }
  }

  // ── BODY TEMP: 34C+ floor, 15 stable -> LED1 ──
  if (!tempDone) {
    float t = mlx.readObjectTempC();

    if (!isnan(t) && t >= 34.0f && t <= 42.0f) {
      if (abs(t - lastTempReading) < 0.3f) {
        if (tempStableCount == 0) { tempSum = 0; tempSampleCount = 0; }
        tempStableCount++;
        tempSum += t;
        tempSampleCount++;
      } else {
        tempStableCount = 0;
        tempSum = 0;
        tempSampleCount = 0;
      }
      lastTempReading = t;
      bodyTempC = t;

      if (tempStableCount >= 15 && tempSampleCount > 0) {
        bodyTempC = tempSum / tempSampleCount;
        tempDone = true;
        digitalWrite(GREEN_LED1, HIGH);    // LED1 = BODY TEMP
        beepTick();
        TimeNow now = getNow();
        String msg = "====MEDISENSE TEMP====\r\n";
        msg += "Time  : " + timeFmt(now.h, now.m) + "\r\n";
        msg += "BTemp : " + String(bodyTempC, 1) + " C\r\n";
        msg += "Status: " + tempStatus(bodyTempC) + "\r\n";
        msg += "======================\r\n";
        BT.print(msg);
        Serial.print(msg);
      }
    } else {
      tempStableCount = 0;
      tempSum = 0;
      tempSampleCount = 0;
    }
  }

  // ── BOTH DONE: hold 10s, turn off LEDs, reset ──
  if (tempDone && hrDone) {
    if (bothDoneMs == 0) bothDoneMs = millis();
    if (millis() - bothDoneMs >= VITALS_HOLD_MS) {
      digitalWrite(GREEN_LED1, LOW);
      digitalWrite(GREEN_LED2, LOW);
      tempDone = false;
      hrDone   = false;
      bothDoneMs = 0;
      tempStableCount = 0; tempSum = 0; tempSampleCount = 0;
      lastTempReading = 0;
      liveBpm = 0; liveSpO2 = 0;
      hrMeasuring = false; bpmTotal = 0; bpmCount = 0;
      Serial.println("Vitals reset - ready for next reading");
    }
  }
}

// ── ALARM CHECK ──
void checkAlarms() {
  if (sysState == ST_SET) return;
  TimeNow now = getNow();

  for (int i = 0; i < 6; i++) {
    if (schedule[i].triggered) continue;
    if (!alarmRinging &&
        now.h == schedule[i].hour &&
        now.m == schedule[i].minute &&
        now.s <= 3) {
      schedule[i].triggered = true;
      alarmRinging = true;
      alarmIndex   = i;
      alarmStartMs = millis();
      missedSent   = false;
      sysState     = ST_ALARM;
      lcd.clear();
      Serial.println("ALARM dose " + String(i + 1));
    }
  }

  if (alarmRinging) {
    rgbRed();
    static unsigned long lastBuzz = 0;
    if (millis() - lastBuzz > 2000) {
      activeBuzzerOn();
      delay(200);
      activeBuzzerOff();
      lastBuzz = millis();
    }

    if (!missedSent && millis() - alarmStartMs >= MISSED_DOSE_MS) {
      missedSent   = true;
      alarmRinging = false;
      activeBuzzerOff();
      TimeNow t = getNow();
      String miss = "====MEDISENSE V2====\r\n";
      miss += "Time : " + timeFmt(t.h, t.m) + "\r\n";
      miss += "Med  : MISSED\r\n";
      miss += "Dose : " + String(alarmIndex + 1) + "\r\n";
      miss += "====================\r\n";
      BT.print(miss);
      Serial.print(miss);
      rgbOff();
      sysState = ST_IDLE;
      lcd.clear();
    }
  }
}

// ── BLUETOOTH COMMAND — send "1" to rotate one step ──
void handleBT() {
  while (BT.available()) {
    char c = BT.read();
    if (c == '1') {
      if (sysState == ST_IDLE || sysState == ST_ALARM) {
        BT.println(">> Rotating one step...");
        Serial.println("BT: rotate one step");
        dispenseStep();
      } else {
        BT.println(">> Busy (in setup), try again");
      }
    }
  }
}

// ── IR ──
int irDigit(uint32_t code) {
  switch (code) {
    case BTN_0: return 0; case BTN_1: return 1;
    case BTN_2: return 2; case BTN_3: return 3;
    case BTN_4: return 4; case BTN_5: return 5;
    case BTN_6: return 6; case BTN_7: return 7;
    case BTN_8: return 8; case BTN_9: return 9;
    default: return -1;
  }
}

void showSetEntry() {
  String td = "";
  td += (digitCount > 0) ? String(digitBuf[0]) : "_";
  td += (digitCount > 1) ? String(digitBuf[1]) : "_";
  td += ":";
  td += (digitCount > 2) ? String(digitBuf[2]) : "_";
  td += (digitCount > 3) ? String(digitBuf[3]) : "_";
  lcd.clear();
  lcdLine(0, centre("Set Dose " + String(editIndex + 1) + "/6"));
  lcdLine(1, centre("Time: " + td));
  lcd2c("Type HHMM");
  lcd3c("9999 = skip");
}

void handleIR() {
  if (!IrReceiver.decode()) return;
  uint32_t code = IrReceiver.decodedIRData.decodedRawData;
  IrReceiver.resume();
  int d = irDigit(code);
  if (d < 0) return;

  if (sysState == ST_SET) {
    if (digitCount < 4) { digitBuf[digitCount++] = d; beepTick(); }
    showSetEntry();
    if (digitCount == 4) {
      uint8_t h = digitBuf[0] * 10 + digitBuf[1];
      uint8_t m = digitBuf[2] * 10 + digitBuf[3];
      if (h == 99 && m == 99) {
        lcd.clear(); lcdLine(0, centre("Skipped"));
        beepErr(); delay(1200); digitCount = 0;
        editIndex++;
        if (editIndex >= 6) {
          sysState = ST_IDLE; editIndex = 0;
          lcd.clear(); lcdLine(0, centre("All Set!"));
          beepOK(); delay(1500); lcd.clear(); return;
        }
        showSetEntry();
      }
      else if (h > 23 || m > 59) {
        lcd.clear(); lcdLine(0, centre("Invalid!"));
        beepErr(); delay(1500); digitCount = 0; showSetEntry();
      }
      else {
        schedule[editIndex].hour   = h;
        schedule[editIndex].minute = m;
        lcd.clear(); lcdLine(0, centre("Saved " + timeFmt(h, m)));
        beepOK(); delay(1000); digitCount = 0;
        editIndex++;
        if (editIndex >= 6) {
          sysState = ST_IDLE; editIndex = 0;
          lcd.clear(); lcdLine(0, centre("All Set!"));
          beepOK(); delay(1500); lcd.clear(); return;
        }
        showSetEntry();
      }
    }
    return;
  }

  if (sysState == ST_ALARM) {
    if (d == 1) dispenseStep();
    return;
  }

  if (sysState == ST_IDLE) {
    if (d == 0) {
      editIndex = 0; digitCount = 0;
      sysState = ST_SET;
      beepTick();
      showSetEntry();
    }
    else if (d == 1) {
      dispenseStep();
    }
  }
}

// ── SCREENS ──
void screenIdle() {
  TimeNow now = getNow();
  lcdLine(0, centre(timeFmtS(now.h, now.m, now.s)));

  String nextDose = "Done";
  for (int i = 0; i < 6; i++) {
    if (!schedule[i].triggered) {
      nextDose = timeFmt(schedule[i].hour, schedule[i].minute);
      break;
    }
  }
  lcdLine(1, "Nxt:" + nextDose + " " + String(roomTempC, 0) + "C " + String((int)humidityPct) + "%");

  // Row 2: Temp first, then HR
  String tStat = tempDone ? tempStatus(bodyTempC) : "....";
  String hStat = hrDone   ? hrStatus(liveBpm)     : "....";
  lcd2(centre("T:" + tStat + " HR:" + hStat));
  lcd3(centre("0=Set 1=Rotate"));
}

void screenAlarm() {
  TimeNow now = getNow();
  lcdLine(0, centre(timeFmtS(now.h, now.m, now.s)));
  lcdLine(1, centre("!! PILL TIME !!"));
  lcd2(centre("Step " + String(currentSlot + 1) + "/6"));
  lcd3(centre("Press 1 to disp"));
}

// ── INTRO ──
void introSequence() {
  lcd.clear();
  lcdLine(0, centre("Welcome to"));
  lcdLine(1, centre("Medi-Sense V2"));

  rgbRed();   buzzerTone(523, 150); delay(30);
  rgbGreen(); buzzerTone(587, 150); delay(30);
  rgbBlue();  buzzerTone(659, 150); delay(30);
  rgbRed();   buzzerTone(698, 150); delay(30);
  rgbGreen(); buzzerTone(784, 200); delay(30);
  rgbBlue();  buzzerTone(880, 200); delay(30);
  rgbCyan();  buzzerTone(988, 200); delay(30);
  rgbWhite(); buzzerTone(1047, 300); delay(30);
  rgbPurple(); buzzerTone(880, 150); delay(30);
  rgbYellow(); buzzerTone(784, 150); delay(30);
  rgbRed();   buzzerTone(698, 100); delay(30);
  rgbGreen(); buzzerTone(659, 100); delay(30);
  rgbBlue();  buzzerTone(587, 150); delay(30);
  rgbCyan();  buzzerTone(523, 300); delay(30);
  rgbOff();
  delay(500);

  lcd.clear();
  lcdLine(0, centre("Created by:"));
  lcdLine(1, centre("Oluturoti Joshua"));
  lcd2c("Matric no:");
  lcd3c("RUN/CPE/21/10359");
  delay(3000);

  rgbBlue();
  lcd.clear();
  readEnv();
  TimeNow now = getNow();

  String envMsg = "====MEDISENSE V2 BOOT====\r\n";
  envMsg += "Time  : " + timeFmt(now.h, now.m) + "\r\n";
  envMsg += "RoomT : " + String(roomTempC, 1) + " C\r\n";
  envMsg += "Humid : " + String((int)humidityPct) + " %\r\n";
  envMsg += "Cmd   : Send '1' to rotate\r\n";
  envMsg += "=========================\r\n";
  BT.print(envMsg);
  Serial.print(envMsg);

  lcdLine(0, centre("MediSense V2"));
  lcdLine(1, centre("System Ready!"));
  lcd2c(timeFmt(now.h, now.m) + " " + String(roomTempC, 1) + "C");
  lcd3c("0 = Set Times");
  beepOK();
  delay(2500);
  rgbOff();
  lcd.clear();
}

// ── SETUP ──
void setup() {
  Serial.begin(115200);
  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(100000);

  pinMode(BUZZER_PIN,    OUTPUT);
  pinMode(ACTIVE_BUZZER, OUTPUT);
  pinMode(GREEN_LED1,    OUTPUT);
  pinMode(GREEN_LED2,    OUTPUT);
  pinMode(WHITE_LED,     OUTPUT);
  pinMode(RGB_RED,       OUTPUT);
  pinMode(RGB_GREEN_PIN, OUTPUT);
  pinMode(RGB_BLUE,      OUTPUT);

  digitalWrite(BUZZER_PIN,    LOW);
  digitalWrite(ACTIVE_BUZZER, LOW);
  digitalWrite(GREEN_LED1,    LOW);
  digitalWrite(GREEN_LED2,    LOW);
  digitalWrite(WHITE_LED,     LOW);
  rgbOff();

  BT.begin("MediSense_V2");
  lcd.init();
  lcd.backlight();

  if (!heartSensor.begin(Wire, I2C_SPEED_STANDARD)) {
    Serial.println("MAX30102 not found");
  } else {
    heartSensor.setup(0x3F, 4, 2, 100, 411, 4096);
    heartSensor.setPulseAmplitudeRed(0x3F);
    heartSensor.setPulseAmplitudeIR(0x3F);
    heartSensor.setPulseAmplitudeGreen(0);
  }
  if (!mlx.begin()) Serial.println("MLX90614 not found");
  dht.begin();

  rtc.Begin();
  rtc.SetIsWriteProtected(false);
  rtc.SetIsRunning(true);
  // rtc.SetDateTime(RtcDateTime(2026, 6, 7, 4, 23, 0));

  pillServo.attach(SERVO_PIN, 500, 2400);
  pillServo.writeMicroseconds(544);
  delay(1500);
  currentSlot = 0;
  currentPulse = 544;

  IrReceiver.begin(IR_PIN, DISABLE_LED_FEEDBACK);
  bpmWindowMs = millis();

  introSequence();
  Serial.println("Ready. 0=set, 1=rotate");
}

// ── LOOP ──
void loop() {
  updateVitals();
  handleIR();
  handleBT();
  checkAlarms();

  unsigned long now = millis();

  static unsigned long lastEnv = 0;
  if (now - lastEnv > 10000) { readEnv(); lastEnv = now; }

  if (sysState == ST_SET) return;

  if (sysState == ST_ALARM) {
    if (now - lastRefresh > 500) { screenAlarm(); lastRefresh = now; }
    return;
  }

  if (sysState == ST_IDLE) {
    if (tempDone && hrDone) rgbGreen();
    else rgbPurple();
    if (now - lastRefresh > 500) { screenIdle(); lastRefresh = now; }
  }
}