#include <WiFi.h>
#include <PubSubClient.h>
#include <Stepper.h>
#include <ESP32Servo.h>
#include <SD.h>
#include <ArduinoJson.h>

// WiFi and MQTT details
const char* ssid = "Wokwi-GUEST"; 
const char* password = ""; // Update with your WiFi password
const char *mqtt_server = "broker.emqx.io";
const char *topic = "SnackOrder";
const char *mqtt_username = "emqx";
const char *mqtt_password = "public";
const int mqtt_port = 1883;

WiFiClient espClient;
PubSubClient client(espClient);

// Constants for stepper motors
const int stepsPerRevolution = 200; // Update this based on your stepper motor
const int stepPin1 = 13; // Step pin for stepper motor 1
const int dirPin1 = 12;  // Direction pin for stepper motor 1
const int stepPin2 = 26; // Step pin for stepper motor 2
const int dirPin2 = 27;  // Direction pin for stepper motor 2
const int stepPin3 = 32; // Step pin for stepper motor 3
const int dirPin3 = 33;  // Direction pin for stepper motor 3

// Constants for servo motors
const int servoPin1 = 34; // Pin for servo motor 1
const int servoPin2 = 25; // Pin for servo motor 2
const int servoPin3 = 16; // Pin for servo motor 3

// SD card chip select pin
const int chipSelect = 5;

// Define stepper motors
Stepper stepper1(stepsPerRevolution, stepPin1, dirPin1);
Stepper stepper2(stepsPerRevolution, stepPin2, dirPin2);
Stepper stepper3(stepsPerRevolution, stepPin3, dirPin3);

// Define servo motors
Servo servo1;
Servo servo2;
Servo servo3;

// Order structure
struct Order 
{
  int officeID;
  String snackOption;
  int quantity;
};

// Define order arrays for robots
Order currentOrder1 = {0, "", 0};
Order currentOrder2 = {0, "", 0};
Order currentOrder3 = {0, "", 0};

// Distance matrix (example values)
const int distances[30] = {
  2, 3, 4, 5, 6, 7, 8, 9, 10, 11,
  12, 13, 14, 15, 16, 17, 18, 19, 20, 21,
  22, 23, 24, 25, 26, 27, 28, 29, 30, 31
};

// Define the maximum JSON capacity you expect to use
const size_t JSON_CAPACITY = JSON_OBJECT_SIZE(3) + 60; 
void setup() {
  Serial.begin(115200);

  // Initialize SD card
  if (!SD.begin(chipSelect)) {
    Serial.println("SD card initialization failed!");
    while (true);
  }
  Serial.println("SD card initialization done.");

  // Initialize servos
  servo1.attach(servoPin1, 100, 500);
  servo2.attach(servoPin2, 100, 500);
  servo3.attach(servoPin3, 100, 500);

  // Connect to WiFi
  Serial.println("Connecting to WiFi...");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nConnected to WiFi");
    
  readCSVFromFile("/office database.csv");


  // Set up MQTT
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
  reconnect();

  // Set stepper motor speed
  stepper1.setSpeed(60);
  stepper2.setSpeed(60);
  stepper3.setSpeed(60);
}

void loop() 
{

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

  processOrder(currentOrder1, stepper1, servo1);
  processOrder(currentOrder2, stepper2, servo2);
  processOrder(currentOrder3, stepper3, servo3);

  delay(1000);
}
void readCSVFromFile(const char* filename) 
{
  File file = SD.open(filename);
  if (!file) 
  {
    Serial.println("Failed to open file for reading");
    return;
  }

  while (file.available()) {
    String line = file.readStringUntil('\n');
    Serial.println(line);
    // Process the line as needed
  }

  file.close();
}
void callback(char* topic, byte* payload, unsigned int length) 
{
  Serial.print("Message arrived on topic: ");
  Serial.print(topic);
  Serial.print(". Message: ");
  String messageTemp;

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

  // Parse JSON
  DynamicJsonDocument doc(JSON_CAPACITY);
  DeserializationError error = deserializeJson(doc, messageTemp);
  if (error) 
  {
    Serial.print("deserializeJson() failed: ");
    Serial.println(error.c_str());
    return;
  }

  int officeID = doc["officeID"];
  String snackOption = doc["snackOption"];
  int quantity = doc["quantity"];

  Serial.print("Office ID: ");
  Serial.println(officeID);
  Serial.print("Snack Option: ");
  Serial.println(snackOption);
  Serial.print("Quantity: ");
  Serial.println(quantity);

  // Process the order based on the office ID
  if (isRobot1(officeID) && currentOrder1.quantity == 0) 
  {
    currentOrder1 = {officeID, snackOption, quantity};
  } 
  else if (isRobot2(officeID) && currentOrder2.quantity == 0) 
  {
    currentOrder2 = {officeID, snackOption, quantity};
  } 
  else if (isRobot3(officeID) && currentOrder3.quantity == 0) 
  {
    currentOrder3 = {officeID, snackOption, quantity};
  } 
  else 
  {
    Serial.println("All robots are currently busy or invalid robot assignment");
  }
}

void reconnect() 
{
  while (!client.connected()) 
  {
    Serial.print("Attempting MQTT connection...");
    String clientId = "ESP32Client-" + String(random(0xffff), HEX);
    if (client.connect(clientId.c_str())) 
    {
      Serial.println("connected");
      client.subscribe("SnackOrder");
    } else 
    {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void processOrder(Order &order, Stepper &stepper, Servo &servo) 
{
  if (order.quantity > 0) 
  {
    Serial.print("Processing order for office ID: ");
    Serial.println(order.officeID);

    int stepsToMove = calculateSteps(order.officeID);
    Serial.print("Steps to move: ");
    Serial.println(stepsToMove);

    if (stepsToMove > 0) {
      // Move the stepper motor
      stepper.step(stepsToMove);

      // Simulate servo operation (e.g., picking up the snack)
      servo.write(90);
      delay(1000); // Adjust the delay as necessary
      servo.write(0);

      // Reset the order
      order = {0, "", 0};
    } else 
    {
      Serial.println("Invalid office ID or steps to move");
    }
  }
}

int calculateSteps(int officeID) 
{
    if (officeID >= 100 && officeID <= 105) {
        return distances[officeID - 100];
    } else if (officeID >= 106 && officeID <= 111) {
        return distances[officeID - 100];
    } else if (officeID >= 112 && officeID <= 117) {
        return distances[officeID - 100];
    } else if (officeID >= 118 && officeID <= 123) {
        return distances[officeID - 100];
    } else if (officeID >= 124 && officeID <= 129) {
        return distances[officeID - 100];
    } else {
        return -1; // Return an error value if officeID is out of range
    }
}

bool isRobot1(int officeID) 
{
  return officeID == 100 || officeID == 101 || officeID == 106 || officeID == 107 ||
         officeID == 112 || officeID == 113 || officeID == 118 || officeID == 119 ||
         officeID == 124 || officeID == 125;
}

bool isRobot2(int officeID) 
{
  return officeID == 102 || officeID == 103 || officeID == 108 || officeID == 109 ||
         officeID == 114 || officeID == 115 || officeID == 120 || officeID == 121 ||
         officeID == 126 || officeID == 127;
}

bool isRobot3(int officeID) 
{
  return officeID == 104 || officeID == 105 || officeID == 110 || officeID == 111 ||
         officeID == 116 || officeID == 117 || officeID == 122 || officeID == 123 ||
         officeID == 128 || officeID == 129;
}