/**
 * Medibox with Light Intensity Monitoring and Servo Control
 * Features:
 * - Alarms, OLED display, temperature/humidity monitoring
 * - LDR (Photoresistor Sensor) for light intensity on GPIO36
 * - Servo for window control
 * - MQTT to Node-RED
 */

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <DHTesp.h>
#include <WiFi.h>
#include <time.h>
#include <PubSubClient.h>
#include <ESP32Servo.h> // Use ESP32Servo library

// ============= Hardware Configuration =============
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C

#define BUZZER_PIN 5
#define ALARM_LED_PIN 15
#define HUMI_LED_PIN 2
#define TEMP_LED_PIN 4
#define CANCEL_BTN_PIN 34
#define OK_BTN_PIN 32
#define UP_BTN_PIN 33
#define DOWN_BTN_PIN 35
#define DHT_PIN 12
#define LDR_PIN 36 // GPIO36 (VP pin)
#define SERVO_PIN 13

// ============= MQTT Configuration =============
#define MQTT_SERVER "test.mosquitto.org"
#define MQTT_PORT 1883
#define MQTT_CLIENT_ID "Medibox_ESP32"
#define TOPIC_LIGHT "medibox/light"
#define TOPIC_PARAMS "medibox/params"

// ============= Time Configuration =============
#define NTP_SERVER "pool.ntp.org"
#define UTC_OFFSET_DST 0

// ============= Global Objects =============
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
DHTesp dhtSensor;
WiFiClient espClient;
PubSubClient mqttClient(espClient);
Servo servoMotor; // ESP32Servo uses Servo class

// ============= Time Tracking =============
struct {
  int days = 0, hours = 0, minutes = 0, seconds = 0, months = 0, years = 0;
} currentTime;

// ============= Alarm System =============
const int ALARM_COUNT = 3;
struct {
  bool enabled = true;
  int hours[ALARM_COUNT] = {7, 13, 19};
  int minutes[ALARM_COUNT] = {0, 0, 0};
  bool triggered[ALARM_COUNT] = {false, false, false};
} alarmSystem;

// ============= Buzzer Configuration =============
const int NOTE_COUNT = 3;
const int BUZZER_TONES[NOTE_COUNT] = {262, 294, 330};

// ============= System Modes =============
const int MODE_COUNT = 5;
const String SYSTEM_MODES[MODE_COUNT] = {
  "1- Set Alarm 1", "2- Set Alarm 2", "3- Set Alarm 3", 
  "4- Toggle Alarm", "5- Set Time Zone"
};
int currentMode = 0;
const int SNOOZE_DURATION = 5;
bool alarmActive = false;
unsigned long alarmStartTime = 0;

// ============= Light Monitoring Variables =============
float lightIntensity = 0;
float lightSum = 0;
int lightReadingsCount = 0;
unsigned long lastSampleTime = 0;
unsigned long lastSendTime = 0;
float samplingInterval = 5; // seconds
float sendingInterval = 120; // seconds (2 minutes)
float thetaOffset = 30; // degrees
float gammaFactor = 0.75; // Renamed to avoid conflict
float Tmed = 30; // °C

// ============= Setup Function =============
void setup() {
  initializeHardware();
  setupDisplay();
  connectToWiFi();
  configureSystemTime();
  setupMQTT();
  displayWelcomeMessage();
  servoMotor.attach(SERVO_PIN); // Attach servo to GPIO13
}

// ============= Main Loop =============
void loop() {
  updateTimeAndCheckAlarms();
  monitorEnvironment();
  readLDR();
  controlServo();
  maintainMQTT();
  
  if (digitalRead(OK_BTN_PIN) == LOW) {
    delay(200);
    enterMenuMode();
  }
  
  delay(100);
}

// ============= Hardware Initialization =============
void initializeHardware() {
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(ALARM_LED_PIN, OUTPUT);
  pinMode(HUMI_LED_PIN, OUTPUT);
  pinMode(TEMP_LED_PIN, OUTPUT);
  pinMode(CANCEL_BTN_PIN, INPUT_PULLUP);
  pinMode(OK_BTN_PIN, INPUT_PULLUP);
  pinMode(UP_BTN_PIN, INPUT_PULLUP);
  pinMode(DOWN_BTN_PIN, INPUT_PULLUP);
  pinMode(LDR_PIN, INPUT);
  dhtSensor.setup(DHT_PIN, DHTesp::DHT22);
  Serial.begin(115200);
}

// ============= MQTT Functions =============
void setupMQTT() {
  mqttClient.setServer(MQTT_SERVER, MQTT_PORT);
  mqttClient.setCallback(mqttCallback);
  connectMQTT();
}

void connectMQTT() {
  while (!mqttClient.connected()) {
    if (mqttClient.connect(MQTT_CLIENT_ID)) {
      Serial.println("Connected to MQTT");
      mqttClient.subscribe(TOPIC_PARAMS);
    } else {
      Serial.println("MQTT connection failed. Retrying...");
      delay(5000);
    }
  }
}

void maintainMQTT() {
  if (!mqttClient.connected()) {
    connectMQTT();
  }
  mqttClient.loop();
}

void mqttCallback(char* topic, byte* payload, unsigned int length) {
  String message;
  for (unsigned int i = 0; i < length; i++) {
    message += (char)payload[i];
  }
  Serial.println("Received MQTT: " + message);

  // Parse parameters: format "ts:5,tu:120,theta:30,gammaFactor:0.75,Tmed:30"
  if (message.startsWith("ts:")) {
    int tsIndex = message.indexOf("ts:") + 3;
    int tuIndex = message.indexOf(",tu:");
    int thetaIndex = message.indexOf(",theta:");
    int gammaIndex = message.indexOf(",gammaFactor:");
    int TmedIndex = message.indexOf(",Tmed:");
    
    samplingInterval = message.substring(tsIndex, tuIndex).toFloat();
    sendingInterval = message.substring(tuIndex + 4, thetaIndex).toFloat();
    thetaOffset = message.substring(thetaIndex + 7, gammaIndex).toFloat();
    gammaFactor = message.substring(gammaIndex + 13, TmedIndex).toFloat();
    Tmed = message.substring(TmedIndex + 6).toFloat();
  }
}

// ============= LDR Reading =============
void readLDR() {
  unsigned long currentTime = millis();
  if (currentTime - lastSampleTime >= samplingInterval * 1000) {
    int ldrRaw = analogRead(LDR_PIN); // 0-1023 for Wokwi Photoresistor Sensor
    lightIntensity = ldrRaw / 1023.0; // Normalize to 0-1
    lightSum += lightIntensity;
    lightReadingsCount++;
    lastSampleTime = currentTime;

    Serial.println("LDR Reading: " + String(lightIntensity));

    // Check if it's time to send averaged data
    if (currentTime - lastSendTime >= sendingInterval * 1000) {
      float avgLight = lightSum / lightReadingsCount;
      sendLightData(avgLight);
      lightSum = 0;
      lightReadingsCount = 0;
      lastSendTime = currentTime;
    }
  }
}

void sendLightData(float avgLight) {
  char buffer[10];
  dtostrf(avgLight, 4, 2, buffer);
  mqttClient.publish(TOPIC_LIGHT, buffer);
  Serial.println("Published Light: " + String(buffer));
}

// ============= Servo Control =============
void controlServo() {
  TempAndHumidity envData = dhtSensor.getTempAndHumidity();
  float T = envData.temperature;

  // Calculate servo angle
  float theta = thetaOffset + (180 - thetaOffset) * lightIntensity * gammaFactor * 
                log(samplingInterval / sendingInterval) * (T / Tmed);
  
  // Constrain angle to 0-180
  theta = constrain(theta, 0, 180);
  servoMotor.write(int(theta));
  Serial.println("Servo Angle: " + String(theta));
}

// ============= Existing Functions (Unchanged) =============
void setupDisplay() {
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println("Display initialization failed");
    while (true);
  }
  display.display();
  delay(800);
}

void printDisplay(String text, int column, int row, int textSize) {
  display.setTextSize(textSize);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(column, row);
  display.println(text);
  display.display();
}

void displayCurrentTime() {
  display.clearDisplay();
  printDisplay(String(currentTime.hours), 0, 0, 2);
  printDisplay(":", 20, 0, 2);
  printDisplay(String(currentTime.minutes), 30, 0, 2);
  printDisplay(":", 50, 0, 2);
  printDisplay(String(currentTime.seconds), 60, 0, 2);
  printDisplay(String(currentTime.days), 0, 25, 1);
  printDisplay("/", 10, 25, 1);
  printDisplay(String(currentTime.months), 20, 25, 1);
  printDisplay("/", 30, 25, 1);
  printDisplay(String(currentTime.years), 40, 25, 1);
}

void displayWelcomeMessage() {
  display.clearDisplay();
  printDisplay("Medibox v2.0", 0, 0, 2);
  printDisplay("Smart System", 0, 30, 1);
  delay(2000);
  display.clearDisplay();
}

void updateCurrentTime() {
  struct tm timeinfo;
  if (!getLocalTime(&timeinfo)) {
    Serial.println("Failed to obtain time");
    return;
  }
  char timeStringBuff[20];
  strftime(timeStringBuff, sizeof(timeStringBuff), "%H", &timeinfo);
  currentTime.hours = atoi(timeStringBuff);
  strftime(timeStringBuff, sizeof(timeStringBuff), "%M", &timeinfo);
  currentTime.minutes = atoi(timeStringBuff);
  strftime(timeStringBuff, sizeof(timeStringBuff), "%S", &timeinfo);
  currentTime.seconds = atoi(timeStringBuff);
  strftime(timeStringBuff, sizeof(timeStringBuff), "%d", &timeinfo);
  currentTime.days = atoi(timeStringBuff);
  strftime(timeStringBuff, sizeof(timeStringBuff), "%m", &timeinfo);
  currentTime.months = atoi(timeStringBuff);
  strftime(timeStringBuff, sizeof(timeStringBuff), "%Y", &timeinfo);
  currentTime.years = atoi(timeStringBuff);
}

int configureTimeZone() {
  float timeZone = 0;
  bool configurationComplete = false;
  while (!configurationComplete) {
    display.clearDisplay();
    printDisplay("Set Time Zone", 0, 0, 2);
    printDisplay(String(timeZone), 0, 20, 2);
    if (digitalRead(UP_BTN_PIN) == LOW) {
      timeZone += 0.5;
      playButtonTone();
      delay(200);
    } else if (digitalRead(DOWN_BTN_PIN) == LOW) {
      timeZone -= 0.5;
      playButtonTone();
      delay(200);
    } else if (digitalRead(OK_BTN_PIN) == LOW) {
      configurationComplete = true;
      playButtonTone();
      delay(200);
    } else if (digitalRead(CANCEL_BTN_PIN) == LOW) {
      timeZone = 0;
      configurationComplete = true;
      playButtonTone();
      delay(200);
    }
  }
  display.clearDisplay();
  printDisplay("Time Zone Set", 0, 0, 2);
  delay(2000);
  return timeZone * 3600;
}

void configureSystemTime() {
  int utcOffset = configureTimeZone();
  configTime(utcOffset, UTC_OFFSET_DST, NTP_SERVER);
}

void updateTimeAndCheckAlarms() {
  updateCurrentTime();
  displayCurrentTime();
  if (alarmSystem.enabled) {
    for (int i = 0; i < ALARM_COUNT; i++) {
      if (!alarmSystem.triggered[i] && 
          alarmSystem.hours[i] == currentTime.hours && 
          alarmSystem.minutes[i] == currentTime.minutes) {
        activateAlarm();
      }
      if (currentTime.hours == 0 && currentTime.minutes == 0) {
        alarmSystem.triggered[i] = false;
      }
    }
  }
}

void activateAlarm() {
  display.clearDisplay();
  printDisplay("ALARM ACTIVE!", 0, 0, 2);
  printDisplay("OK-Snooze", 0, 20, 1);
  printDisplay("Cancel-Stop", 0, 35, 1);
  digitalWrite(ALARM_LED_PIN, HIGH);
  alarmActive = true;
  alarmStartTime = millis();
  while (alarmActive) {
    if (millis() - alarmStartTime > 30000) {
      alarmActive = false;
      break;
    }
    for (int i = 0; i < NOTE_COUNT; i++) {
      if (digitalRead(OK_BTN_PIN) == LOW) {
        snoozeAlarm();
        alarmActive = false;
        break;
      } else if (digitalRead(CANCEL_BTN_PIN) == LOW) {
        stopAlarm();
        alarmActive = false;
        break;
      }
      tone(BUZZER_PIN, BUZZER_TONES[i]);
      delay(500);
      noTone(BUZZER_PIN);
      delay(20);
    }
  }
  digitalWrite(ALARM_LED_PIN, LOW);
  display.clearDisplay();
}

void snoozeAlarm() {
  for (int i = 0; i < ALARM_COUNT; i++) {
    if (alarmSystem.hours[i] == currentTime.hours && 
        alarmSystem.minutes[i] == currentTime.minutes) {
      int newMinutes = alarmSystem.minutes[i] + SNOOZE_DURATION;
      int carryOver = newMinutes / 60;
      alarmSystem.minutes[i] = newMinutes % 60;
      alarmSystem.hours[i] = (alarmSystem.hours[i] + carryOver) % 24;
      alarmSystem.triggered[i] = false;
      display.clearDisplay();
      printDisplay("Snoozed for", 0, 0, 2);
      printDisplay(String(SNOOZE_DURATION) + " minutes", 0, 25, 2);
      delay(2000);
      break;
    }
  }
}

void stopAlarm() {
  for (int i = 0; i < ALARM_COUNT; i++) {
    if (alarmSystem.hours[i] == currentTime.hours && 
        alarmSystem.minutes[i] == currentTime.minutes) {
      alarmSystem.triggered[i] = true;
      break;
    }
  }
  display.clearDisplay();
  printDisplay("Alarm Stopped", 0, 0, 2);
  delay(2000);
}

void setAlarm(int alarmIndex) {
  bool settingComplete = false;
  bool settingHours = true;
  int tempHours = alarmSystem.hours[alarmIndex];
  int tempMinutes = alarmSystem.minutes[alarmIndex];
  while (!settingComplete) {
    display.clearDisplay();
    if (settingHours) {
      printDisplay("Set Alarm " + String(alarmIndex + 1), 0, 0, 2);
      printDisplay("Hours: " + String(tempHours), 0, 20, 2);
    } else {
      printDisplay("Minutes: " + String(tempMinutes), 0, 20, 2);
    }
    if (digitalRead(UP_BTN_PIN) == LOW) {
      if (settingHours) {
        tempHours = (tempHours + 1) % 24;
      } else {
        tempMinutes = (tempMinutes + 1) % 60;
      }
      playButtonTone();
      delay(200);
    } else if (digitalRead(DOWN_BTN_PIN) == LOW) {
      if (settingHours) {
        tempHours = (tempHours - 1 + 24) % 24;
      } else {
        tempMinutes = (tempMinutes - 1 + 60) % 60;
      }
      playButtonTone();
      delay(200);
    } else if (digitalRead(OK_BTN_PIN) == LOW) {
      if (settingHours) {
        settingHours = false;
      } else {
        settingComplete = true;
      }
      playButtonTone();
      delay(200);
    } else if (digitalRead(CANCEL_BTN_PIN) == LOW) {
      settingComplete = true;
      playButtonTone();
      delay(200);
    }
  }
  if (settingHours == false) {
    alarmSystem.hours[alarmIndex] = tempHours;
    alarmSystem.minutes[alarmIndex] = tempMinutes;
    display.clearDisplay();
    printDisplay("Alarm " + String(alarmIndex + 1), 0, 0, 2);
    printDisplay("Set Successfully", 0, 20, 2);
    delay(2000);
  }
}

void enterMenuMode() {
  bool inMenu = true;
  while (inMenu) {
    display.clearDisplay();
    printDisplay(SYSTEM_MODES[currentMode], 0, 0, 2);
    int buttonPressed = waitForButtonPress();
    switch (buttonPressed) {
      case UP_BTN_PIN:
        currentMode = (currentMode + 1) % MODE_COUNT;
        break;
      case DOWN_BTN_PIN:
        currentMode = (currentMode - 1 + MODE_COUNT) % MODE_COUNT;
        break;
      case OK_BTN_PIN:
        executeSelectedMode();
        break;
      case CANCEL_BTN_PIN:
        inMenu = false;
        break;
    }
  }
}

int waitForButtonPress() {
  while (true) {
    if (digitalRead(UP_BTN_PIN) == LOW) return UP_BTN_PIN;
    if (digitalRead(DOWN_BTN_PIN) == LOW) return DOWN_BTN_PIN;
    if (digitalRead(OK_BTN_PIN) == LOW) return OK_BTN_PIN;
    if (digitalRead(CANCEL_BTN_PIN) == LOW) return CANCEL_BTN_PIN;
    delay(10);
  }
}

void executeSelectedMode() {
  switch (currentMode) {
    case 0: case 1: case 2:
      setAlarm(currentMode);
      break;
    case 3:
      alarmSystem.enabled = !alarmSystem.enabled;
      display.clearDisplay();
      printDisplay("Alarms " + String(alarmSystem.enabled ? "ON" : "OFF"), 0, 0, 2);
      delay(2000);
      break;
    case 4:
      configureSystemTime();
      break;
  }
}

void monitorEnvironment() {
  TempAndHumidity envData = dhtSensor.getTempAndHumidity();
  if (envData.temperature > 28) {
    digitalWrite(TEMP_LED_PIN, HIGH);
    displayAlert("High Temp:", String(envData.temperature) + "C", 40);
  } else if (envData.temperature < 22) {
    digitalWrite(TEMP_LED_PIN, HIGH);
    displayAlert("Low Temp:", String(envData.temperature) + "C", 40);
  } else {
    digitalWrite(TEMP_LED_PIN, LOW);
  }
  if (envData.humidity > 60) {
    digitalWrite(HUMI_LED_PIN, HIGH);
    displayAlert("High Humid:", String(envData.humidity) + "%", 50);
  } else if (envData.humidity < 40) {
    digitalWrite(HUMI_LED_PIN, HIGH);
    displayAlert("Low Humid:", String(envData.humidity) + "%", 50);
  } else {
    digitalWrite(HUMI_LED_PIN, LOW);
  }
}

void displayAlert(String title, String value, int yPos) {
  display.clearDisplay();
  printDisplay(title, 0, yPos - 10, 1);
  printDisplay(value, 0, yPos, 2);
}

void playButtonTone() {
  tone(BUZZER_PIN, 262, 100);
  delay(10);
  noTone(BUZZER_PIN);
}

void connectToWiFi() {
  display.clearDisplay();
  printDisplay("Connecting...", 0, 0, 2);
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(250);
    display.clearDisplay();
    printDisplay("Connecting...", 0, 0, 2);
  }
  display.clearDisplay();
  printDisplay("Connected!", 0, 0, 2);
  delay(1000);
}