#include <WiFi.h>
#include <PubSubClient.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

char clientId[50];
int sp1=12;
Servo s1;
int sp2=4;
Servo s2;
int sp3=15;
Servo s3;

WiFiClient espClient;
PubSubClient client(espClient);

int led1=4;
int led2=12;
int led3=13;
int led4=5;

Adafruit_SSD1306 display(128,64,&Wire,1);


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("Hello, ESP32!");
  s1.attach(sp1,500,2400);
  s2.attach(sp2,500,2400);
  s3.attach(sp3,500,2400);
  if(!display.begin(SSD1306_SWITCHCAPVCC,0x3C)){
    Serial.println(F("SSD1306 allocation failed."));
    for(;;);
  }
  delay(2000);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  Serial.print("Connecting to..");
  WiFi.mode(WIFI_STA);
  WiFi.begin("Wokwi-GUEST","",6);
  while(WiFi.status()!=WL_CONNECTED){
    delay(100);
  }
  client.setServer("broker.emqx.io",1883);
  client.setCallback(callback);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
  pinMode(led4, OUTPUT);
}

void mqttReconnect(){
  while(!client.connected()){
    Serial.print("Attempting mqtt");
    if(client.connect(clientId)){
      Serial.println("Connected");
      client.subscribe("topicName/iot");
    }
    else{
      Serial.print("Failed,rc=");
      Serial.print(client.state());
      Serial.print("Try again in 5 seconds");
      delay(5000);
    }
  }
}

void callback(char* topic, byte* message, unsigned int length){
  Serial.print("Message arrived on topic");
  Serial.print(topic);
  Serial.print("Message");
  String stMessage;
  for(int i=0; i<length;i++){
    Serial.print((char)message[i]);
    stMessage+=(char)message[i];
  }
  if(stMessage=="1"){
    Serial.println("1");
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(10,0);
    display.println("Release orbital access arm");
    display.display();
    s1.write(180);
  }
  else if(stMessage=="2"){
    Serial.println("2");
    digitalWrite(led1, HIGH);
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(10,0);
    display.println("Main engine test and lift the cap");
    display.display();
    s2.write(0);
  }
  else if(stMessage=="3"){
    Serial.println("3");
    display.clearDisplay();
    digitalWrite(led2,HIGH);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(10,0);
    display.println("Activate hydrogen burn of system");
    display.display();
  }
    else if(stMessage=="4"){
    Serial.println("4");
    display.clearDisplay();
    digitalWrite(led3,HIGH);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(10,0);
    display.println("Main engine ignite");
    display.display();
  }
    else if(stMessage=="5"){
    Serial.println("5");
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(10,0);
    display.println("Hydrogen vent arm/ release tail surface mass");
    display.display();
    s3.write(360);
  }
    else if(stMessage=="6"){
    Serial.println("6");
    display.clearDisplay();
    digitalWrite(led4,HIGH);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(10,0);
    display.println("Ignite both SRBs");
    display.display();
  }
  }



void loop() {
  // put your main code here, to run repeatedly:
  delay(1000);
  if(!client.connected()){
    mqttReconnect();
  }
  client.loop();

}