#include <WiFi.h>
#include <MQTT.h>
#include <ESP32Servo.h>
#include <NusabotSimpleTimer.h>

// Object Initialization
WiFiClient net;
MQTTClient client;
Servo srv;
NusabotSimpleTimer timer;

// Const WiFi SSID and Password
const char ssid[] = "Wokwi-GUEST";
const char pass[] = "";

// Pin GPIO for RGB LED
int pinRed = 2;
int pinGrn = 4;
int pinBlu = 16;
int pinLed = 13;
int pinSrv = 12;

// FUnction of RGB Light
void rgb(bool red, bool grn, bool blu){
  digitalWrite(pinRed, red);
  digitalWrite(pinGrn, grn);
  digitalWrite(pinBlu, blu);
}

// Function of Blinking Light
void blink(){
  rgb(1, 1, 0);
  delay(500);
  rgb(0, 0, 0);
  delay(500);
}

void conn(){
  // Connecting to WiFi Indicator
  while(WiFi.status() != WL_CONNECTED){
    blink();
  }

  // Connected WiFi Indicator
  rgb(0, 1, 0);

  // Connecting to MQTT Broker Indicator
  while(!client.connect("client.robotics")){
    delay(500);
  }

  // Connected MQTT Broker
  rgb(0, 0, 1);

  // Subscribe to Broker
  client.subscribe("robotics.upnjatim/#", 1); // Subs to Topic, QoS Value
}

void subs(String &topic, String &data){
  Serial.print("Topic  = ");
  Serial.println(topic);
  Serial.print("Data   = ");
  Serial.println(data);

  if(topic == "robotics.upnjatim/led"){
    if(data == "merah1"){
      digitalWrite(pinLed, 1);
    }
    else if(data == "merah0"){
      digitalWrite(pinLed, 0);
    }
  }

  if(topic == "robotics.upnjatim/srv"){
    int posSrv = data.toInt();
    srv.write(posSrv);
  }
}

void publish(){
  client.publish("robotics.upnjatim/data", "halo", false, 1);
}

void setup() {
  // WiFi and Client Setup
  WiFi.begin(ssid, pass);
  client.begin("broker.emqx.io", net);
  client.onMessage(subs);
  timer.setInterval(1000, publish);

  // Servo Setup
  srv.attach(pinSrv, 500, 2400);

  // RGB LED Setup
  pinMode(pinRed, OUTPUT);
  pinMode(pinGrn, OUTPUT);
  pinMode(pinBlu, OUTPUT);
  pinMode(pinLed, OUTPUT);
  
  Serial.begin(9600);

  // Startup Indicator
  rgb(1, 0, 0);

  // Setup Connect
  conn();
}

void loop() {
  client.loop();
  timer.run();
  // Reconnecting Function when Disconnected
  if(!client.connected()){
    conn();
  }

  delay(100); // this speeds up the simulation
}