#include <ESP32Servo.h>
// #include <tone.h>
// #include <ESP32Tone.h>
// #include <ESP32PWM.h>
// #include <WiFiManager.h> 
#include "WiFi.h"
#include "PubSubClient.h"



// #define WiFi_pin 18
// #define rem_pin 19
#define SERVO_PIN_1 2
#define SERVO_PIN_2 4
#define SERVO_PIN_3 5
#define SERVO_PIN_4 12
#define SERVO_PIN_5 13
#define SERVO_PIN_6 14

int statusArray[7] = {0, 0, 0, 0, 0, 0, 0};


Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

void openFinger1();
void closeFinger1();
void openFinger2();
void closeFinger2();
void openFinger3();
void closeFinger3();
void openFinger4();
void closeFinger4();
void openFinger5();
void closeFinger5();
void openFinger6();
void closeFinger6();
void all_fingers_open();
void all_fingers_close();

int timeout = 120;

const char* broker = "test.mosquitto.org";
const char* clientid = "ESP32Client-";
// const int port = 1883;  

const char* ssid = "Wokwi-GUEST";
const char* pass = "";
const char* brokerUser = "";
const char* brokerPass = "";

// const char* outTopic = "/hand/status";
const char* intopic="/hand/com";

WiFiClient espClient;
PubSubClient client(espClient);

void reconnect() {
  // if(!digitalRead(WiFi_pin) == HIGH){
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    if (client.connect(clientid, brokerUser, brokerPass)) {
      // Serial.println("\nConnected");
      // client.subscribe(outTopic);
      Serial.print("\nConnected to ");
      Serial.print(broker);
      client.subscribe("/hand/status/all");
      client.subscribe("/hand/status/index");
      client.subscribe("/hand/status/middle");
      client.subscribe("/hand/status/ring");
      client.subscribe("/hand/status/little");
      client.subscribe("/hand/status/thumb");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  // }
  }
}

void setup() {
  // WiFi.mode(WIFI_STA); 
  Serial.begin(115200);
  // WiFi.begin(ssid,pass);
  // Serial.println("\n Starting");
  // // WiFi.begin("Wokwi-GUEST", "");
  // while ((!(WiFi.status() == WL_CONNECTED))) {
  //   delay(300);
  //   Serial.print("..");
  // }
  Serial.print("\nConnecting to");
  Serial.println(ssid);

  WiFi.begin(ssid, pass);

  while(WiFi.status() != WL_CONNECTED){
    delay(100);
    Serial.print("-");
  }
  Serial.println("Connected");
  Serial.println("Your IP is");
  Serial.println((WiFi.localIP()));
  client.setServer(broker, 1883);
  client.setCallback(callback);
  // pinMode(WiFi_pin, INPUT);
  // pinMode(rem_pin, INPUT);
  servo1.attach(SERVO_PIN_1);
  servo2.attach(SERVO_PIN_2);
  servo3.attach(SERVO_PIN_3);
  servo4.attach(SERVO_PIN_4);
  servo5.attach(SERVO_PIN_5);
  servo6.attach(SERVO_PIN_6);
}



void loop() {

  // if ( digitalRead(WiFi_pin) == HIGH) {
  //   WiFiManager wm;    
  //   //wm.resetSettings();
  //   wm.setConfigPortalTimeout(timeout);
  //   if (!wm.startConfigPortal("Kara")) {
  //     Serial.println("failed to connect and hit timeout");
  //     delay(3000);
  //     ESP.restart();
  //     delay(5000);
  //   }
  //   Serial.println("connected...yeey :)");
  // }
  // if(digitalRead(rem_pin) == HIGH){
    if (!client.connected()) {
      reconnect();
    }
    client.loop();


  // }  
}

void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  String message = "";
  for (int i = 0; i < length; i++) {
    message += ((char)payload[i]);
  }
  Serial.println(message);

  // Use strcmp function to compare strings
  if (strcmp(topic, "/hand/status/index") == 0) {
    if (strcmp(message.c_str(), "S1o") == 0) {
      openFinger1();
    } else if (strcmp(message.c_str(), "S1c") == 0) {
      closeFinger1();
    }
    else {
      Serial.println("Unknown command for Index Fingers");
    }
  } else if (strcmp(topic, "/hand/status/middle") == 0) {
    if (strcmp(message.c_str(), "S2o") == 0) {
      openFinger2();
    } else if (strcmp(message.c_str(), "S2c") == 0) {
      closeFinger2();
    }
    else {
      Serial.println("Unknown command for Middle Fingers");
    }
  } else if (strcmp(topic, "/hand/status/ring") == 0) {
    if (strcmp(message.c_str(), "S3o") == 0) {
      openFinger3();
    } else if (strcmp(message.c_str(), "S3c") == 0) {
      closeFinger3();
    }
    else {
      Serial.println("Unknown command for Ring Fingers");
    }
  } else if (strcmp(topic, "/hand/status/little") == 0) {
    if (strcmp(message.c_str(), "S4o") == 0) {
      openFinger4();
    } else if (strcmp(message.c_str(), "S4c") == 0) {
      closeFinger4();
    }
    else {
      Serial.println("Unknown command for Little Fingers");
    }
  } else if (strcmp(topic, "/hand/status/thumb") == 0) {
    if (strcmp(message.c_str(), "S5o") == 0) {
      openFinger5();
    } else if (strcmp(message.c_str(), "S5c") == 0) {
      closeFinger5();
    }
    else {
      Serial.println("Unknown command for Thumb");
    }
  } else if (strcmp(topic, "/hand/status/all") == 0) {
    if (strcmp(message.c_str(), "AFo") == 0) {
      all_fingers_open();
    } else if (strcmp(message.c_str(), "AFc") == 0) {
      all_fingers_close();
    } else {
      Serial.println("Unknown command for All Fingers");
    }
  } else {
    // Default case for unexpected messages
    Serial.println("Unknown command. Please check message format");
  }
}

void openFinger1() {
  servo1.write(180);
  Serial.print("Index Finger Open");
}
void closeFinger1() {
  servo1.write(0); 
  Serial.print("Index Finger Close");
}

void openFinger2() {
  servo2.write(180); 
  Serial.print("Middle Finger Open");
}
void closeFinger2() {
  servo2.write(0); 
  Serial.print("Middle Finger Close");
}

void openFinger3() {
  servo3.write(180); 
  Serial.print("Ring Finger Open");
}
void closeFinger3() {
  servo3.write(0); 
  Serial.print("Ring Finger Close");
}

void openFinger4() {
  servo4.write(180); 
  Serial.print("Little Finger Open");
}
void closeFinger4() {
  servo4.write(0); 
  Serial.print("Little Finger Close");
}

void openFinger5() {
  servo5.write(90); 
  Serial.print("Thumb Open");
}
void closeFinger5() {
  servo5.write(0); 
  Serial.print("Thumb closed");
}

void all_fingers_open() {
    openFinger1();
    Serial.println("");
    openFinger2();
    Serial.println("");
    openFinger3();
    Serial.println("");
    openFinger4();
    Serial.println("");
    openFinger5();
    Serial.println("");
    Serial.print("All Finger Open");
}
void all_fingers_close() {
    closeFinger1();
    Serial.println("");
    closeFinger2();
    Serial.println("");
    closeFinger3();
    Serial.println("");
    closeFinger4();
    Serial.println("");
    closeFinger5(); 
    Serial.println("");
    Serial.print("All Finger Close");
}

// void openFinger1() {
//   servo1.write(180);
//   Serial.print("Finger1 Open");
// }
// void closeFinger1() {
//   servo1.write(0); 
//   Serial.print("Finger1 Close");
// }

// void openFinger2() {
//   servo2.write(180); 
//   Serial.print("Finger2 Open");
// }
// void closeFinger2() {
//   servo2.write(0); 
//   Serial.print("Finger2 Close");
// }

// void openFinger3() {
//   servo3.write(180); 
//   Serial.print("Finger3 Open");
// }
// void closeFinger3() {
//   servo3.write(0); 
//   Serial.print("Finger3 Close");
// }

// void openFinger4() {
//   servo4.write(180); 
//   Serial.print("Finger4 Open");
// }
// void closeFinger4() {
//   servo4.write(0); 
//   Serial.print("Finger4 Close");
// }

// void openFinger5() {
//   servo5.write(90); 
// Serial.print("Finger1 Open");
// }
// void closeFinger5() {
//   servo5.write(0); 
// }

// void openFinger6() {
//   servo6.write(90); 
// Serial.print("Finger1 Open");
// }
// void closeFinger6() {
//   servo6.write(0); 
// }

// void all_fingers_open() {
//     openFinger1();
//     openFinger2();
//     openFinger3();
//     openFinger4();
//     openFinger5();
//     Serial.print("All Finger Open");
// }
// void all_fingers_close() {
//     closeFinger1();
//     closeFinger2();
//     closeFinger3();
//     closeFinger4();
//     closeFinger5(); 
//     Serial.print("All Finger Close");
// }