#include <SD.h>
#include <ESP32Servo.h>

//slave slect using SPI for SD card
int slaveSelect = 5;
//Servo pins
int servo1 = 15, servo2 = 2, servo3 = 4;
//Stepper motor pins
int y1_step = 22, y1_dir = 21, x1_step = 17, x1_dir = 16;
int y2_step = 26, y2_dir = 27, x2_step = 12, x2_dir = 14;
int y3_step = 32, y3_dir = 13, x3_step = 25, x3_dir = 33;
//Servo objects
Servo s1, s2, s3;

#include <WiFi.h>
#include "PubSubClient.h"
#include <ArduinoJson.h>

const char* ssid = "Wokwi-GUEST";                 //wifi's ssid
const char* password = "";                        //wifi's password
const char* mqttServer = "broker.hivemq.com";     //node-REDs mqtt server
int port = 1883;
String stMac;
char mac[50];
char clientId[50];
//objects for wifi client and pubsubclient
WiFiClient espClient;
PubSubClient client(espClient);

void setup() {
  //Serial monitor 
  Serial.begin(115200);
  //stepper motor
  pinMode(y1_step, OUTPUT); pinMode(y1_dir, OUTPUT);
  pinMode(x1_step, OUTPUT); pinMode(x1_dir, OUTPUT);
  pinMode(y2_step, OUTPUT); pinMode(y2_dir, OUTPUT);
  pinMode(x2_step, OUTPUT); pinMode(x2_dir, OUTPUT);
  pinMode(y3_step, OUTPUT); pinMode(y3_dir, OUTPUT);
  pinMode(x3_step, OUTPUT); pinMode(x3_dir, OUTPUT);
  //Servo
  s1.attach(servo1);
  s2.attach(servo2);
  s3.attach(servo3);

  //SD card 
  SD.begin(slaveSelect);

  File input = SD.open("/example.csv", FILE_WRITE);         //writing object
  input.print(105); input.print(","); input.print(104); input.print(","); input.print(103); input.print(","); input.print(102); input.print(","); input.print(101); input.print(","); input.println(100);
  input.print(111); input.print(","); input.print(110); input.print(","); input.print(109); input.print(","); input.print(108); input.print(","); input.print(107); input.print(","); input.println(106);
  input.print(117); input.print(","); input.print(116); input.print(","); input.print(115); input.print(","); input.print(114); input.print(","); input.print(113); input.print(","); input.println(112);
  input.print(123); input.print(","); input.print(122); input.print(","); input.print(121); input.print(","); input.print(120); input.print(","); input.print(119); input.print(","); input.println(118);
  input.print(129); input.print(","); input.print(128); input.print(","); input.print(127); input.print(","); input.print(126); input.print(","); input.print(125); input.print(","); input.println(124);
  input.close();

  File output = SD.open("/example.csv");                   //reading object
  Serial.println("THE DATA INSIDE SD CARD: ");
  while(output.available()){
    for(int i=0; i<5; i++){
      for(int j=0; j<6; j++){
        String x = "";
        for(int k=0; k<3; k++) {x += char(output.read());}
        Serial.print(x); Serial.print(" ");
        output.read();//for comma
      }
      output.read();//for new line
      Serial.println();
    }
  }
  output.close();

  //connecting to WIFI
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  wifiConnect();
  Serial.println("WiFi connected");
  
  //connecting with mqtt server of Node-RED
  client.setServer(mqttServer, port);
  //printing values based on input from mqtt
  client.setCallback(callback);
}


void wifiConnect() {      //Connecting to WI-FI
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {delay(500);}
}

int id, place;
void callback(char* topic, byte* payload, unsigned int length) {
  //initialising variable to store input
  StaticJsonDocument<200> doc;

  //display error if input is not of proper format
  DeserializationError error = deserializeJson(doc, payload, length);
  if (error) {
    Serial.println("ORDERS HAVE EXCEEDED STOCK AVAILABILITY");
    Serial.println("please start your order from beginning");
    return;
  }

  //reading and storing inputs
  int chips = doc["c1"];
  int biriyani = doc["c2"];
  int coke = doc["c3"];
  int puffs = doc["c4"];
  int sides = doc["c5"];
  id = doc["id"];
  int display = doc["display"];
  place = doc["a"];
  //display the values based on show button
  if (place == 1) {
    Serial.println("--CONFIRMED ORDER--");
    Serial.print("Customer ID: "); Serial.println(id);
    Serial.print("Chips     :"); Serial.println(chips);
    Serial.print("Biriyani  :"); Serial.println(biriyani);
    Serial.print("Coke      :"); Serial.println(coke);
    Serial.print("Egg puffs :"); Serial.println(puffs);
    Serial.print("Chicken 65:"); Serial.println(sides);
  }
  else if (display == 1) {
    Serial.println("--TEMPERORY ORDER--");
    Serial.print("Customer ID: "); Serial.println(id);
    Serial.print("Chips     :"); Serial.println(chips);
    Serial.print("Biriyani  :"); Serial.println(biriyani);
    Serial.print("Coke      :"); Serial.println(coke);
    Serial.print("Egg puffs :"); Serial.println(puffs);
    Serial.print("Chicken 65:"); Serial.println(sides);
  }
}

void mqttReconnect() {                    //connect with node-red using MQTT 
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    long r = random(1000);
    sprintf(clientId, "clientId-%ld", r);
    if (client.connect(clientId)) {
      Serial.print(clientId);
      Serial.println(" connected");
      client.subscribe("order");    //mqtt topic to be connected with
    } 
  }
}

void loop() {

  //reconnect to mqtt server if connection is lost
  if (!client.connected()) {
    mqttReconnect();
  }
  //loop the callback function for every click in node-RED dashboard
  client.loop();
  //convert id to string
  String ID = "";
  while (id>0){
    ID = char(id%10 + 48) + ID;
    id = id/10;
  }

  //start the robot if order is confirmed
  if (place == 1){
    Serial.println("ORDER PICKED UP FROM CANTEEN");
    int I=0, J=0;
    File check = SD.open("/example.csv");                   //read from SD
    while(check.available()){
      for(int m1=0; m1<5; m1++){
        for(int m2=0; m2<6; m2++){
          String x = "";
          for(int k=0; k<3; k++) {x += char(check.read());}
          if(x==ID) {
            I= 5 - m2 +2; 
            J=m1; 
          }
          check.read();//for comma
        }
        check.read();//for new line
      }
    }
    check.close();
    
    //move robot 1 for first two rows
    if(I<4){
      //servo picking up food
      for(int i=0; i<=180; i++ ){ s1.write(i); delay(10); } 
      Serial.println("BOT 1 ON THE WAY"); 
      //stepper motors move bot to table
      for(int x=0; x<I; x++){ 
        digitalWrite(x1_dir, HIGH); 
        digitalWrite(x1_step, HIGH); digitalWrite(x1_step, LOW); 
        delay(500);
      }
      for(int y=0; y<J; y++){ 
        digitalWrite(y1_dir, HIGH); 
        digitalWrite(y1_step, HIGH); digitalWrite(y1_step, LOW); 
        delay(500); 
      } 
      Serial.print("BOT's MOVEMENT =  x:"); Serial.print(I); Serial.print(" y:"); Serial.println(J); 
      delay(500); 
      Serial.println("ORDER REACHED TABLE");
      //servo dropping the food 
      for(int i=180; i>=0; i-- ){ s1.write(i); delay(10); } 
      Serial.println("ORDER DELIVERED"); 
      //stepper motors move bot back to canteen
      for(int x=0; x<I; x++){ 
        digitalWrite(x1_dir, LOW); 
        digitalWrite(x1_step, HIGH); digitalWrite(x1_step, LOW); 
        delay(500); 
      } 
      for(int y=0; y<J; y++){ 
        digitalWrite(y1_dir, LOW); 
        digitalWrite(y1_step, HIGH); digitalWrite(y1_step, LOW); 
        delay(500);
      } 
      Serial.println("BOT 1 REACHED BACK TO CANTEEN"); 
    }
    //move robot 2 for next two rows
    else if(I<6){
      for(int i=0; i<=180; i++ ){ s2.write(i); delay(10); } 
      Serial.println("BOT 2 ON THE WAY"); 
      for(int x=0; x<I; x++){ 
        digitalWrite(x2_dir, HIGH); 
        digitalWrite(x2_step, HIGH); digitalWrite(x2_step, LOW); 
        delay(500);
      }
      for(int y=0; y<J; y++){ 
        digitalWrite(y2_dir, HIGH); 
        digitalWrite(y2_step, HIGH); digitalWrite(y2_step, LOW); 
        delay(500); 
      } 
      Serial.print("BOT's MOVEMENT =  x:"); Serial.print(I); Serial.print(" y:"); Serial.println(J); 
      delay(500); 
      Serial.println("ORDER REACHED TABLE"); 
      for(int i=180; i>=0; i-- ){ s2.write(i); delay(10); } 
      Serial.println("ORDER DELIVERED"); 
      for(int x=0; x<I; x++){ 
        digitalWrite(x2_dir, LOW); 
        digitalWrite(x2_step, HIGH); digitalWrite(x2_step, LOW); 
        delay(500); 
      } 
      for(int y=0; y<J; y++){ 
        digitalWrite(y2_dir, LOW); 
        digitalWrite(y2_step, HIGH); digitalWrite(y2_step, LOW); 
        delay(500);
      } 
      Serial.println("BOT 2 REACHED BACK TO CANTEEN"); 
    } 
    //move robot 3 for last two rows
    else if(I<8){
      for(int i=0; i<=180; i++ ){ s3.write(i); delay(10); } 
      Serial.println("BOT 3 ON THE WAY"); 
      for(int x=0; x<I; x++){ 
        digitalWrite(x3_dir, HIGH); 
        digitalWrite(x3_step, HIGH); digitalWrite(x3_step, LOW); 
        delay(500);
      }
      for(int y=0; y<J; y++){ 
        digitalWrite(y3_dir, HIGH); 
        digitalWrite(y3_step, HIGH); digitalWrite(y3_step, LOW); 
        delay(500); 
      } 
      Serial.print("BOT's MOVEMENT =  x:"); Serial.print(I); Serial.print(" y:"); Serial.println(J); 
      delay(500); 
      Serial.println("ORDER REACHED TABLE"); 
      for(int i=180; i>=0; i-- ){ s3.write(i); delay(10); } 
      Serial.println("ORDER DELIVERED"); 
      for(int x=0; x<I; x++){ 
        digitalWrite(x3_dir, LOW); 
        digitalWrite(x3_step, HIGH); digitalWrite(x3_step, LOW); 
        delay(500); 
      } 
      for(int y=0; y<J; y++){ 
        digitalWrite(y3_dir, LOW); 
        digitalWrite(y3_step, HIGH); digitalWrite(y3_step, LOW); 
        delay(500);
      } 
      Serial.println("BOT 3 REACHED BACK TO CANTEEN"); 
    }

    delay(1000);
  }
  delay(10);;
}