#include <ESP32Servo.h> // Allow ESP32 to control servo

//* The following are codes to connect to MQTT*//
#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h>

// MQTT set up Code starts here
const char* ssid = "Wokwi-GUEST";
const char* password = "";

const char* hostname = "broker.hivemq.com";

WiFiClient espClient;
PubSubClient client(espClient);
const char* espClientName = "Group 3840032A"; // yourStudentID must be unique
int PORTNUM = 1883;

// Connect to Wifi
void setup_wifi()
{
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED )
  {
    delay(1000);
    Serial.print(".");
  }
  Serial.println();
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("Given IP by the router to ESP32 is ");
  Serial.println(WiFi.localIP());
}

// Code to see if it can connect, "while" code is to allow keep connecting. Note that "MQTTSubscribe" function is called here (To subscribe to topic)
void connectMQTT()
{
  while (!client.connected() )
  {
    Serial.println("Connecting to MQTT ...");
    if (client.connect(espClientName) ) //, mqttUser, mqttPassword) )
    {
      Serial.println("Connected");
    }
    else
    {
      Serial.print("Failed with state ");
      Serial.print(client.state() );
      delay(2000);
    }
  }
}

void setup_MQTT()
{
  client.setServer(hostname, PORTNUM);
}

//MQTT codes end here//

Servo myservo; // Create Servo Object to control a servo

int ServoPin = 23; // Setting pin 23 for servo

long ADC_previous_millis = 0;
int ADC_interval = 50;

int angle = 0; // Set the initial angle of Servo
int incrementRate = 3; // Increment rate
int direction = -1;// Direction Variable for the servo motor

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200); // Begin Serial Monitor
  Serial.println("Hello, ESP32!"); // "Hello" Print
  myservo.attach(ServoPin); // Set up ServoPin using Servo library

  //* Set up MQTT*//
  setup_wifi();
  setup_MQTT();
  //* Set up MQTT*//
}

void loop () {

  if ( !client.connected() )
  {
    connectMQTT();
  }
  client.loop();

  long current_millis = millis(); //Setting current millis variable as millis type

  if (current_millis - ADC_previous_millis >= ADC_interval) {

    ADC_previous_millis = current_millis; // Reset timer

    int ADC_value; // Variable to read the value from analog pin
    ADC_value = analogRead(A5);

    Serial.print("Voltage now is ");
    Serial.println(ADC_value);

    if (ADC_value < 3102.3 && ADC_value > 1861.4) 
    {
      myservo.write(angle); // Set the servo to current angle
      angle += incrementRate * direction ; // Increment the angle by increment rate and the direction is set by diection integer
      if (angle >= 180) { // Upon reaching 180 degree, it will go in anti-clockwise direction
        direction = -1; //
      } else if (angle <= 0) { // Upon reaching 0 degree, it will go in clockwise direction
        direction = 1; //
      }
      client.publish("IOT/GROUP/PP1", "ON");
    } else {
      client.publish("IOT/GROUP/PP1", "OFF");
    }
  }
}