#include <WiFi.h>
#include <PubSubClient.h>
#include <DHT.h>
#include <math.h>

// WiFi credentials
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// MQTT broker
const char* mqtt_server = "test.mosquitto.org";
const int mqtt_port = 1883;
const int mqtt_keep_alive = 60; // Keep-alive interval in seconds

// MQTT topics
const char* ldr_topic = "medibox/ldr";
const char* config_topic = "medibox/config";
const char* servo_topic = "medibox/servo";

// Pin definitions
#define LDR_PIN 34  // Analog pin for LDR
#define DHT_PIN 4   // Digital pin for DHT22
#define SERVO_PIN 5 // PWM pin for servo

// DHT22 setup
#define DHT_TYPE DHT22
DHT dht(DHT_PIN, DHT_TYPE);

// MQTT client
WiFiClient espClient;
PubSubClient client(espClient);

// Configuration defaults
float ts = 5.0;          // Sampling interval (seconds)
float tu = 120.0;        // Sending interval (seconds)
float theta_offset = 30.0; // Minimum servo angle (degrees)
float gamma_factor = 0.75; // Controlling factor
float T_med = 30.0;      // Ideal temperature (°C)

// LDR averaging
const int max_samples = 24; // 120s / 5s = 24 samples
float ldr_readings[max_samples];
int sample_count = 0;
float ldr_sum = 0;

// Timers
unsigned long last_sample = 0;
unsigned long last_send = 0;
unsigned long last_dht_read = 0;
unsigned long last_servo_update = 0;

// MQTT connection flag
bool mqttConnected = false;

// Servo PWM settings
float servo_theta = 0; // Current servo angle
const long servo_period = 20000; // 20ms (50Hz) in microseconds
const int servo_cycles = 50; // Number of PWM cycles per update

// Function to update servo PWM
void updateServo(float theta) {
  // Map theta (0–180) to pulse width (500us–2500us)
  long pulse_width = map(theta, 0, 180, 500, 2500);
  // Perform a few PWM cycles to maintain 50Hz, then exit to avoid blocking
  for (int i = 0; i < servo_cycles; i++) {
    digitalWrite(SERVO_PIN, HIGH);
    delayMicroseconds(pulse_width);
    digitalWrite(SERVO_PIN, LOW);
    delayMicroseconds(servo_period - pulse_width);
    // Check MQTT connection briefly
    if (!client.connected()) {
      mqttConnected = false;
      reconnect();
    }
    client.loop();
  }
}

void setup() {
  Serial.begin(115200);
  dht.begin();
  
  // Initialize servo pin
  pinMode(SERVO_PIN, OUTPUT);
  
  // Connect to WiFi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  Serial.println("Connected to WiFi");

  // Set up MQTT
  client.setServer(mqtt_server, mqtt_port);
  client.setKeepAlive(mqtt_keep_alive);
  client.setCallback(callback);
  reconnect();
}

void reconnect() {
  while (!client.connected()) {
    String clientId = "ESP32Client-";
    clientId += String(random(0xffff), HEX);
    if (client.connect(clientId.c_str())) {
      if (!mqttConnected) {
        Serial.println("Connected to MQTT");
        mqttConnected = true;
      }
      client.subscribe(config_topic);
    } else {
      Serial.println("MQTT connection failed, retrying...");
      delay(30000); // Wait 30s before retry
    }
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
  String message;
  for (int i = 0; i < length; i++) {
    message += (char)payload[i];
  }
  
  // Parse message (format: "ts:5,tu:120,theta_offset:30,gamma:0.75,T_med:30")
  int idx = 0;
  while (idx < message.length()) {
    if (message.startsWith("ts:", idx)) {
      ts = message.substring(idx + 3, message.indexOf(',', idx)).toFloat();
      idx = message.indexOf(',', idx) + 1;
    } else if (message.startsWith("tu:", idx)) {
      tu = message.substring(idx + 3, message.indexOf(',', idx)).toFloat();
      idx = message.indexOf(',', idx) + 1;
    } else if (message.startsWith("theta_offset:", idx)) {
      theta_offset = message.substring(idx + 13, message.indexOf(',', idx)).toFloat();
      idx = message.indexOf(',', idx) + 1;
    } else if (message.startsWith("gamma:", idx)) {
      gamma_factor = message.substring(idx + 6, message.indexOf(',', idx)).toFloat();
      idx = message.indexOf(',', idx) + 1;
    } else if (message.startsWith("T_med:", idx)) {
      T_med = message.substring(idx + 6, message.length()).toFloat();
      break;
    }
  }
  Serial.println("Config updated: ts=" + String(ts) + ", tu=" + String(tu) + 
                 ", theta_offset=" + String(theta_offset) + ", gamma=" + String(gamma_factor) + 
                 ", T_med=" + String(T_med));
}

void loop() {
  if (!client.connected()) {
    mqttConnected = false;
    reconnect();
  }
  client.loop();

  unsigned long now = millis();

  // Read and print DHT22 data every 2 seconds
  if (now - last_dht_read >= 2000) {
    float humidity = dht.readHumidity();
    float temperature = dht.readTemperature();
    if (!isnan(humidity) && !isnan(temperature)) {
      Serial.print("Humidity: ");
      Serial.print(humidity);
      Serial.print("%, Temperature: ");
      Serial.print(temperature);
      Serial.println("°C");
    } else {
      Serial.println("Failed to read from DHT22");
    }
    last_dht_read = now;
  }

  // Sample LDR every ts seconds
  if (now - last_sample >= ts * 1000) {
    int ldr_raw = analogRead(LDR_PIN);
    float ldr_normalized = ldr_raw / 4095.0; // ESP32 ADC is 12-bit (0–4095)
    ldr_sum += ldr_normalized;
    ldr_readings[sample_count] = ldr_normalized;
    sample_count++;
    last_sample = now;

    // Debug LDR value
    Serial.print("LDR Raw: ");
    Serial.print(ldr_raw);
    Serial.print(", Normalized: ");
    Serial.println(ldr_normalized);

    // Read temperature and control servo
    float T = dht.readTemperature();
    delay(100); // Stabilize DHT22 reading
    if (!isnan(T) && tu > 0) {
      float I = ldr_normalized;
      // Adjusted formula to increase theta range
      float ln_term = (ts > 0 && tu > 0) ? abs(log(ts / tu)) : 0;
      float theta = theta_offset + (180 - theta_offset) * I * gamma_factor * ln_term * (T / T_med);
      theta = constrain(theta, 0, 180); // Ensure angle is 0–180
      
      // Debug theta calculation
      Serial.print("Temperature: ");
      Serial.print(T);
      Serial.print("°C, I: ");
      Serial.print(I);
      Serial.print(", ln_term: ");
      Serial.print(ln_term);
      Serial.print(", Theta: ");
      Serial.print(theta);
      Serial.println(" degrees");

      // Update servo angle
      servo_theta = theta;
      
      // Publish servo angle
      String servo_msg = String(theta);
      client.publish(servo_topic, servo_msg.c_str());
      Serial.print("Published servo angle: ");
      Serial.println(servo_msg);
    } else {
      Serial.println("Invalid temperature reading, skipping servo update");
    }
  }

  // Send average LDR reading every tu seconds
  if (now - last_send >= tu * 1000 && sample_count > 0) {
    float ldr_avg = ldr_sum / sample_count;
    String ldr_msg = String(ldr_avg);
    client.publish(ldr_topic, ldr_msg.c_str());
    Serial.print("Published LDR average: ");
    Serial.println(ldr_msg);
    ldr_sum = 0;
    sample_count = 0;
    last_send = now;
  }

  // Update servo PWM periodically to reduce blocking
  if (now - last_servo_update >= servo_period / 1000 * servo_cycles) {
    updateServo(servo_theta);
    last_servo_update = now;
  }
}