#include <PubSubClient.h>
#include <WiFi.h>
#include <Servo.h>

#define LDR_RIGHT 32
#define LDR_LEFT 33
#define SERVO_PIN 27

#define D_RIGHT 0.5
#define D_LEFT 1.5

WiFiClient espClient;
PubSubClient mqttClient(espClient);

Servo servo;

float left_intensity;
float right_intensity;

bool isScheduledON = false;
unsigned long scheduledOnTime;

float angle_offset;
float controlling_factor;

/**
 *  Initialize the initial angle for the servo motor and set up the WiFi and MQTT connections.
 */
void setup() {
  angle_offset = 30; // Initial angle offset for the servo motor
  controlling_factor = 0.75; // Initial controlling factor for servo motor movement
  Serial.begin(9600);
  setupWifi(); // Initialize the WiFi connection
  setupMqtt(); // Initialize the MQTT connection

  servo.attach(SERVO_PIN); // Attach the servo to the specified pin
  servo.write(angle_offset); // Set the initial angle for the servo motor
}

/**
 *  Main loop function that handles MQTT connection, measures light intensity, and controls the servo motor.
 */
void loop() {
  if(! mqttClient.connected()){
    connectToBroker(); // Reconnect to the MQTT broker if not connected
  }

  mqttClient.loop(); // Maintain the MQTT connection
  
  measure_light_intensity(); // Measure the light intensity using LDR sensors

  servo_controller(); // Control the servo motor based on the measured light intensity
  delay(1000);
}

/**
 *  Function to set up the WiFi connection.
 *  Connects to the WiFi network and prints the local IP address.
 */
void setupWifi(){
  WiFi.begin("Wokwi-GUEST", ""); // Connect to the WiFi network with the specified SSID and password

  // Wait for the WiFi connection to establish
  while (WiFi.status() != WL_CONNECTED){
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address : ");
  Serial.println(WiFi.localIP()); // Print the local IP address
}

/**
 *  Function to set up the MQTT connection.
 *  Set the MQTT server address and port, and define the callback function for MQTT message reception.
 */
void setupMqtt(){
  mqttClient.setServer("test.mosquitto.org", 1883); // Set the MQTT server address and port
  mqttClient.setCallback(receiveCallBack); // Set the callback function for MQTT message reception
}

/**
 *  Function to connect to the MQTT broker.
 *  Attempt to connect to the broker, subscribe to relevant topics, and publish initial parameters.
 */
void connectToBroker(){
  while (!mqttClient.connected()){
    Serial.print("Attempting MQTT connection...");
    if (mqttClient.connect("ESP32-4654564645645")){
      Serial.println("connected");
      mqttClient.subscribe("System-Medibox-gamma"); // Subscribe to the MQTT topic for controlling factor
      mqttClient.subscribe("System-Medibox-offset-angle"); // Subscribe to the MQTT topic for offset angle

      // Publish the initial control factor and offset angle values
      char ctrl_factor[6];
      char min_angle[6];
      String(angle_offset).toCharArray(min_angle, 6);
      String(controlling_factor).toCharArray(ctrl_factor, 6);
      mqttClient.publish("System-Medibox-gamma", ctrl_factor);
      mqttClient.publish("System-Medibox-offset-angle", min_angle);
    }else{
      Serial.println("failed");
      Serial.println(mqttClient.state());
      delay(500);
    }
  }
}

/**
 *  Callback function for receiving MQTT messages.
 *  Handle received messages and update relevant parameters.
 *  topic - The topic of the received message.
 *  payload - The payload of the received message.
 *  length - The length of the payload.
 */
void receiveCallBack(char* topic, byte* payload, unsigned int length){
  Serial.print("Message has Arrived");
  Serial.print(topic);
  Serial.print("]");

  char payloadCharAr[length];

  for (int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
    payloadCharAr[i] = (char)payload[i];
  }

  Serial.println();

  if(strcmp(topic, "System-Medibox-gamma") == 0){
    controlling_factor = atof(payloadCharAr); // Update the controlling factor from the MQTT message
    Serial.println(controlling_factor);
    servo_controller(); // Control the servo motor based on updated parameters
  }else if(strcmp(topic, "System-Medibox-offset-angle") == 0){
    angle_offset = atof(payloadCharAr); // Update the offset angle from the MQTT message
    Serial.println(angle_offset);
    servo_controller(); // Control the servo motor based on updated parameters
  }
}

/**
 *  Function to measure the light intensity using LDR sensors.
 *  Read analog values from the sensors, calculate light intensity, and publish to the MQTT broker.
 */
void measure_light_intensity(){
  analogReadResolution(10); // Set the resolution to 10 bits (0-1023)
  right_intensity = analogRead(LDR_RIGHT);
  left_intensity = analogRead(LDR_LEFT);

  right_intensity = 1 - (right_intensity / 1023); // Convert analog reading to light intensity
  left_intensity = 1 - (left_intensity / 1023); 

  Serial.println("Left light intensity : " + String(left_intensity));
  Serial.println("Right light intensity : " + String(right_intensity));
  
  char light_intensity[6];
  String highest_intensity_ldr;

  if(left_intensity >= right_intensity){
    highest_intensity_ldr = "Left";
    String(left_intensity).toCharArray(light_intensity, 6);
  }else{
    highest_intensity_ldr = "Right";
    String(right_intensity).toCharArray(light_intensity, 6);
  }
  mqttClient.publish("System-Medibox-light-intensity", light_intensity); // Publish light intensity to MQTT
  mqttClient.publish("System-Medibox-highest-intensity", highest_intensity_ldr.c_str()); // Publish highest intensity LDR to MQTT
}

/**
 *  Function to control the servo motor based on the light intensity.
 *  Calculate the desired angle based on parameters and set the servo angle accordingly.
 */
void servo_controller(){
  float D = 0;
  if(left_intensity > 0.95){
    D = 1.5;
  }else if(right_intensity > 0.95 ) {
    D = 0.5;
  }
  int angle = min(int((angle_offset * D) + ((180 - angle_offset) * max(left_intensity, right_intensity) * controlling_factor)), 180); // Calculate servo angle
  servo.write(angle); // Set servo angle
}