// BenchSafe (HT10) - merged sketch for the ESP32-S3 DevKitC-1
//   Sensors (Riya): ultrasonic x2, current sensor, thermal camera (simulated)
//   Safety (Student A): status LEDs, buzzer, ACK/RESET button, two MOSFET cutoffs
//
// How the pieces connect:
//   thermal peak  -> tempLevel    -> alarm cuts the IRON  (GPIO 13)
//   current (A)   -> currentLevel -> alarm cuts the BULB  (GPIO 14)
//   Level 0 = normal, 1 = warning (amber LED), 2 = alarm (red LED + buzzer + cutoff)
//   The cutoff is decided here, on the ESP32-S3, with no camera/LLM needed (spec 2).
//
// Button: short press (<3 s) = ACK (silences buzzer, alarm stays latched)
//         long press (>=3 s) = RESET (only if both levels are below alarm)

#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

// --- TFT Display Pin Definitions (ILI9341) ---
#define TFT_DC 9
#define TFT_CS 10
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

// --- Ultrasonic Pins & Constants ---
#define L_TRIG 4
#define L_ECHO 5
#define R_TRIG 2
#define R_ECHO 42
#define BENCH_WIDTH_CM 45
#define EDGE_WARN_CM 42

// --- Current Sensor Pin ---
// Riya's code used GPIO 19, which is a USB pin on the ESP32-S3. GPIO 1 matches the schematic.
#define CURRENT_SENSOR_PIN 1

// --- Thermal Camera Simulation (MLX90640 - 32x24 matrix) ---
#define SIMULATION_MODE true
float thermalFrame[768];

#define MINTEMP 22
#define MAXTEMP 55

// --- Student A: outputs ---
const int BUTTON_PIN      = 15;  // to GND, internal pull-up
const int GREEN_LED_PIN   = 16;  // 220 ohm
const int AMBER_LED_PIN   = 17;  // 220 ohm
const int RED_LED_PIN     = 18;  // 220 ohm
const int BUZZER_PIN      = 21;
const int BULB_CUTOFF_PIN = 14;  // MOSFET gate Q2: HIGH = bulb ON, LOW/unpowered = OFF
const int IRON_CUTOFF_PIN = 13;  // MOSFET gate Q3: HIGH = iron ON, LOW/unpowered = OFF

// --- Thresholds (MCR II table; the current values assume the 0.83 A bulb) ---
const float TEMP_WARN_C      = 40.0;
const float TEMP_ALARM_C     = 50.0;
const float CURRENT_WARN_A   = 12.0;
const float CURRENT_ALARM_A  = 15.0;
const unsigned long INRUSH_IGNORE_MS = 500;  // ignore the bulb's switch-on surge
const int CURRENT_CONFIRM_SAMPLES    = 3;    // alarm must persist for 3 samples (150 ms)

// --- Timing ---
const unsigned long DEBOUNCE_MS       = 50;
const unsigned long LONG_PRESS_MS     = 3000;
const unsigned long BLINK_PERIOD      = 1000;
const unsigned long BLINK_ON          = 150;
const unsigned long CURRENT_PERIOD_MS = 50;
const unsigned long THERMAL_PERIOD_MS = 250;
const unsigned long LOG_PERIOD_MS     = 1500;

// Temperature Color Palette (256-color gradient from blue to red)
const uint16_t camColors[] = {
0x480F,0x400F,0x400F,0x400F,0x4010,0x3810,0x3810,0x3810,0x3810,0x3010,0x3010,
0x3010,0x2810,0x2810,0x2810,0x2810,0x2010,0x2010,0x2010,0x1810,0x1810,
0x1811,0x1811,0x1011,0x1011,0x1011,0x0811,0x0811,0x0811,0x0011,0x0011,
0x0011,0x0011,0x0011,0x0031,0x0031,0x0051,0x0072,0x0072,0x0092,0x00B2,
0x00B2,0x00D2,0x00F2,0x00F2,0x0112,0x0132,0x0152,0x0152,0x0172,0x0192,
0x0192,0x01B2,0x01D2,0x01F3,0x01F3,0x0213,0x0233,0x0253,0x0253,0x0273,
0x0293,0x02B3,0x02D3,0x02D3,0x02F3,0x0313,0x0333,0x0333,0x0353,0x0373,
0x0394,0x03B4,0x03D4,0x03D4,0x03F4,0x0414,0x0434,0x0454,0x0474,0x0474,
0x0494,0x04B4,0x04D4,0x04F4,0x0514,0x0534,0x0534,0x0554,0x0554,0x0574,
0x0574,0x0573,0x0573,0x0573,0x0572,0x0572,0x0572,0x0571,0x0591,0x0591,
0x0590,0x0590,0x058F,0x058F,0x058F,0x058E,0x05AE,0x05AE,0x05AD,0x05AD,
0x05AD,0x05AC,0x05AC,0x05AB,0x05CB,0x05CB,0x05CA,0x05CA,0x05CA,0x05C9,
0x05C9,0x05C8,0x05E8,0x05E8,0x05E7,0x05E7,0x05E6,0x05E6,0x05E6,0x05E5,
0x05E5,0x0604,0x0604,0x0604,0x0603,0x0603,0x0602,0x0602,0x0601,0x0621,
0x0621,0x0620,0x0620,0x0620,0x0620,0x0E20,0x0E20,0x0E40,0x1640,0x1640,
0x1E40,0x1E40,0x2640,0x2640,0x2E40,0x2E60,0x3660,0x3660,0x3E60,0x3E60,
0x3E60,0x4660,0x4660,0x4E60,0x4E80,0x5680,0x5680,0x5E80,0x5E80,0x6680,
0x6680,0x6E80,0x6EA0,0x76A0,0x76A0,0x7EA0,0x7EA0,0x86A0,0x86A0,0x8EA0,
0x8EC0,0x96C0,0x96C0,0x9EC0,0x9EC0,0xA6C0,0xAEC0,0xAEC0,0xB6E0,0xB6E0,
0xBEE0,0xBEE0,0xC6E0,0xC6E0,0xCEE0,0xCEE0,0xD6E0,0xD700,0xDF00,0xDEE0,
0xDEC0,0xDEA0,0xDE80,0xDE80,0xE660,0xE640,0xE620,0xE600,0xE5E0,0xE5C0,
0xE5A0,0xE580,0xE560,0xE540,0xE520,0xE500,0xE4E0,0xE4C0,0xE4A0,0xE480,
0xE460,0xEC40,0xEC20,0xEC00,0xEBE0,0xEBC0,0xEBA0,0xEB80,0xEB60,0xEB40,
0xEB20,0xEB00,0xEAE0,0xEAC0,0xEAA0,0xEA80,0xEA60,0xEA40,0xF220,0xF200,
0xF1E0,0xF1C0,0xF1A0,0xF180,0xF160,0xF140,0xF100,0xF0E0,0xF0C0,0xF0A0,
0xF080,0xF060,0xF040,0xF020,0xF800
};

// ---------------- State ----------------
enum State { MONITORING, WARNING, ALARM };
State state = MONITORING;
bool alarmAcked = false;
bool bulbCut = false;
bool ironCut = false;

int tempLevel = 0;      // from the thermal camera: 0 / 1 / 2
int currentLevel = 0;   // from the current sensor: 0 / 1 / 2
String tempCause = "";
String currentCause = "";

float maxDetectedTemp = 0;
float currentAmps = 0;
unsigned long bulbOnSince = 0;
int currentHighCount = 0;

// Thermal simulation: toggles between a "Low Heat" and a "High Heat" picture every 4 seconds
bool highHeatState = false;
unsigned long lastToggleTime = 0;

// button
bool lastReading = HIGH, stableState = HIGH, longPressHandled = false;
unsigned long lastChangeTime = 0, pressStartTime = 0;

// schedulers
unsigned long lastCurrentMs = 0, lastThermalMs = 0, lastLogMs = 0;
int heatRow = 0;

// ---------------- Events for Jannis's backend ----------------
// JSON line (backend attaches wall-clock time; ms = time since boot)
void sendEvent(const char* event, const String& detail) {
  Serial.print("{\"src\":\"SENSOR\",\"event\":\"");
  Serial.print(event);
  Serial.print("\",\"detail\":\"");
  Serial.print(detail);
  Serial.print("\",\"ms\":");
  Serial.print(millis());
  Serial.println("}");
}

// ---------------- Riya's sensors ----------------
int readDistanceCm(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  long duration = pulseIn(echo, HIGH, 30000);
  int distanceCM = duration / 58;
  if (duration == 0 || distanceCM > 400) return -1;
  return distanceCM;
}

// Returns the current in amps (always positive).
//  SIMULATION_MODE: potentiometer, 0 V = 0 A and 3.3 V = 2 A
//                   (1.0 A warning is half-way, 1.25 A alarm is about 63%).
//  Real hardware:   ACS712-5A (185 mV/A around 2.5 V) behind the 10k/20k divider (x2/3).
float readCurrentSensor() {
  int rawADC = analogRead(CURRENT_SENSOR_PIN);
  float voltage = (rawADC / 4095.0) * 3.3;
#if SIMULATION_MODE
  return (voltage / 3.3) * 2.0;
#else
  float sensorVolts = voltage * 1.5;
  return fabsf((sensorVolts - 2.5) / 0.185);
#endif
}

float updateThermalCamera() {
  #if SIMULATION_MODE
    if (millis() - lastToggleTime > 4000) {
      highHeatState = !highHeatState;
      lastToggleTime = millis();
    }

    float centerX = 16.0;
    float centerY = 12.0;
    float peakTemp = 0;

    for (int y = 0; y < 24; y++) {
      for (int x = 0; x < 32; x++) {
        float dist = sqrt(pow(x - centerX, 2) + pow(y - centerY, 2));
        float temp;

        if (!highHeatState) {
          // PICTURE 1: Low heat signature (mild warm object, peak ~28 C)
          temp = 21.0 + (7.0 * exp(-dist / 3.0));
        } else {
          // PICTURE 2: High heat signature (very hot object, peak ~52 C)
          temp = 22.0 + (30.0 * exp(-dist / 2.5));
        }

        thermalFrame[y * 32 + x] = temp;
        if (temp > peakTemp) peakTemp = temp;
      }
    }
    return peakTemp;
  #else
    return 0.0;
  #endif
}

// Draws ONE row of the heatmap per call, so the safety loop never blocks on the screen.
void drawHeatmapRow(int y) {
  int pixelSize = 7;
  int offsetX = 8;
  int offsetY = 20;

  for (int x = 0; x < 32; x++) {
    float temp = thermalFrame[y * 32 + x];
    uint8_t colorIndex = map(temp, MINTEMP, MAXTEMP, 0, 255);
    colorIndex = constrain(colorIndex, 0, 255);
    tft.fillRect(offsetX + (x * pixelSize), offsetY + (y * pixelSize),
                 pixelSize, pixelSize, camColors[colorIndex]);
  }
}

void checkSensor(const char* name, int trig, int echo) {
  int cm = readDistanceCm(trig, echo);
  Serial.print(name);
  if (cm < 0) {
    Serial.println(": out of range / no object detected");
    return;
  }
  Serial.print(": ");
  Serial.print(cm);
  Serial.println(" cm");

  if (cm >= BENCH_WIDTH_CM) {
    Serial.println("   >>> WARNING: object off the bench! <<<");
  } else if (cm >= EDGE_WARN_CM) {
    Serial.println("   >>> WARNING: object close to the edge! <<<");
  }
}

// ---------------- Readings -> levels ----------------
void evaluateTemp(float t) {
  if (t >= TEMP_ALARM_C) {
    tempLevel = 2;
    tempCause = "Temp >= " + String(TEMP_ALARM_C, 0) + " C (peak " + String(t, 1) + " C)";
  } else if (t >= TEMP_WARN_C) {
    tempLevel = 1;
    tempCause = "Temp >= " + String(TEMP_WARN_C, 0) + " C (peak " + String(t, 1) + " C)";
  } else {
    tempLevel = 0;
    tempCause = "";
  }
}

void evaluateCurrent(float amps, unsigned long now) {
  int level = 0;
  if (amps >= CURRENT_ALARM_A) level = 2;
  else if (amps >= CURRENT_WARN_A) level = 1;

  // Ignore the bulb's switch-on surge, and require the alarm to persist for a few samples.
  if (!bulbCut && now - bulbOnSince < INRUSH_IGNORE_MS) level = 0;
  if (level == 2) {
    currentHighCount++;
    if (currentHighCount < CURRENT_CONFIRM_SAMPLES) level = 1;
  } else {
    currentHighCount = 0;
  }

  currentLevel = level;
  if (level > 0) currentCause = "Current " + String(amps, 2) + " A (limit " + String(CURRENT_ALARM_A, 2) + " A)";
  else currentCause = "";
}

// ---------------- Safety state machine ----------------
void enterAlarm() {
  state = ALARM;
  alarmAcked = false;   // a new cutoff always sounds the buzzer again
}

void updateState() {
  // Cutoffs are independent of each other and of the camera/LLM (spec #2)
  if (tempLevel == 2 && !ironCut) {
    ironCut = true;
    digitalWrite(IRON_CUTOFF_PIN, LOW);
    enterAlarm();
    sendEvent("ALARM_CUTOFF_IRON", tempCause);
  }
  if (currentLevel == 2 && !bulbCut) {
    bulbCut = true;
    digitalWrite(BULB_CUTOFF_PIN, LOW);
    enterAlarm();
    sendEvent("ALARM_CUTOFF_BULB", currentCause);
  }

  if (state == ALARM) return;                 // latched until manual reset

  int level = max(tempLevel, currentLevel);
  if (level == 1 && state != WARNING) {
    state = WARNING;
    sendEvent("WARNING", tempLevel == 1 ? tempCause : currentCause);
  } else if (level == 0 && state == WARNING) {
    state = MONITORING;
    sendEvent("WARNING_CLEARED", "Readings back to normal");
  }
}

void onShortPress() {
  if (state == ALARM && !alarmAcked) {
    alarmAcked = true;
    sendEvent("ACK", "Alarm acknowledged, buzzer silenced");
  }
}

void onLongPress() {
  if (state != ALARM) return;
  if (tempLevel < 2 && currentLevel < 2) {
    if (ironCut) { ironCut = false; digitalWrite(IRON_CUTOFF_PIN, HIGH); }
    if (bulbCut) { bulbCut = false; digitalWrite(BULB_CUTOFF_PIN, HIGH); bulbOnSince = millis(); currentHighCount = 0; }
    int level = max(tempLevel, currentLevel);
    state = (level == 1) ? WARNING : MONITORING;
    alarmAcked = false;
    sendEvent("RESET", "Alarm cleared, loads restored");
  } else {
    sendEvent("RESET_REFUSED", "Readings still above alarm threshold");
  }
}

void updateButton(unsigned long now) {
  bool reading = digitalRead(BUTTON_PIN);
  if (reading != lastReading) { lastChangeTime = now; lastReading = reading; }
  if (now - lastChangeTime > DEBOUNCE_MS && reading != stableState) {
    stableState = reading;
        Serial.println(stableState == LOW ? "button down" : "button up");
    if (stableState == LOW) { pressStartTime = now; longPressHandled = false; }
    else if (!longPressHandled) onShortPress();
  }
  if (stableState == LOW && !longPressHandled && now - pressStartTime >= LONG_PRESS_MS) {
    longPressHandled = true;
    onLongPress();
  }
}

void updateOutputs(unsigned long now) {
  digitalWrite(GREEN_LED_PIN, (now % BLINK_PERIOD) < BLINK_ON);   // heartbeat = monitoring active
  digitalWrite(AMBER_LED_PIN, state == WARNING);
  digitalWrite(RED_LED_PIN,   state == ALARM);
  bool beep = (state == ALARM && !alarmAcked) && ((now / 500) % 2 == 0);
  digitalWrite(BUZZER_PIN, beep);
}

// ---------------- Serial log (Riya's format) ----------------
void logReadings() {
  checkSensor("Left Sensor", L_TRIG, L_ECHO);
  delay(60);
  checkSensor("Right Sensor", R_TRIG, R_ECHO);

  Serial.print("Current Sensor: ");
  Serial.print(currentAmps, 2);
  Serial.println(" A");
  if (currentLevel == 2) {
    Serial.println(">>> ALARM: current at or above the alarm limit! <<<");
  } else if (currentLevel == 1) {
    Serial.println(">>> WARNING: current above the warning limit <<<");
  }

  Serial.print("Thermal Camera Mode: ");
  Serial.print(highHeatState ? "[HIGH HEAT SCENE] " : "[LOW HEAT SCENE] ");
  Serial.print("Max Temp Detected: ");
  Serial.print(maxDetectedTemp, 1);
  Serial.println(" C");
  Serial.println("----------------------------------------");
}

// ---------------- Arduino entry points ----------------
void setup() {
  Serial.begin(115200);

  // Keep the SPI bus off GPIO 13 (the default MISO pin), which is the iron cutoff gate.
  // SCK = 12, MISO = unused, MOSI = 11, SS = 10.
  SPI.begin(12, -1, 11, 10);
  tft.begin();
  tft.fillScreen(ILI9341_BLACK);

  pinMode(L_TRIG, OUTPUT);
  pinMode(L_ECHO, INPUT);
  pinMode(R_TRIG, OUTPUT);
  pinMode(R_ECHO, INPUT);

  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(AMBER_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(BULB_CUTOFF_PIN, OUTPUT);
  pinMode(IRON_CUTOFF_PIN, OUTPUT);
  digitalWrite(BULB_CUTOFF_PIN, HIGH);        // loads on once monitoring starts
  digitalWrite(IRON_CUTOFF_PIN, HIGH);
  bulbOnSince = millis();

  Serial.println("System Initialized...");
  sendEvent("MONITORING_START", "System active");
}

void loop() {
  unsigned long now = millis();

  // Fast, non-blocking safety checks
  if (now - lastCurrentMs >= CURRENT_PERIOD_MS) {
    lastCurrentMs = now;
    currentAmps = readCurrentSensor();
    evaluateCurrent(currentAmps, now);
  }
  if (now - lastThermalMs >= THERMAL_PERIOD_MS) {
    lastThermalMs = now;
    maxDetectedTemp = updateThermalCamera();
    evaluateTemp(maxDetectedTemp);
  }

  updateState();
  updateButton(now);
  updateOutputs(now);

  // Slow, human-readable log (ultrasonic reads can block for ~100 ms)
  if (now - lastLogMs >= LOG_PERIOD_MS) {
    lastLogMs = now;
    logReadings();
  }

  // Redraw the heatmap one row per loop
  drawHeatmapRow(heatRow);
  heatRow = (heatRow + 1) % 24;
}