// Libraries for Servo and HC-SR04
#include <ESP32Servo.h>
#include <NewPing.h>

// Libraries for WiFi and ThingsBoard
#if defined(ESP8266)
#include <ESP8266WiFi.h>
#elif defined(ESP32)
#include <WiFi.h>
#endif
#include <Arduino_MQTT_Client.h>
#include <ThingsBoard.h>

// Servo Configuration
const int servoPin = 18;
Servo servo;

// HC-SR04 Configuration
#define TRIGGER_PIN  20
#define ECHO_PIN     19
#define MAX_DISTANCE 415
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// Alarms Configuration
const int levelAlarm = 42;
const int tempAlarm = 41;
const int levelTempAlarm = 40;

// WiFi Configuration
#define WIFI_AP "Wokwi-GUEST"
#define WIFI_PASS ""

// ThingsBoard COnfiguration
#define TB_SERVER "demo.thingsboard.io"
#define TOKEN "XZ3GcLdxw1xgIWtyLJjz"
constexpr uint16_t THINGSBOARD_PORT = 1883U;
constexpr uint32_t MAX_MESSAGE_SIZE = 1024U;

// Initialize underlying client, used to establish a connection
WiFiClient wifiClient;
// Initalize the Mqtt client instance
Arduino_MQTT_Client mqttClient(wifiClient);
// Initialize ThingsBoard instance with the maximum needed buffer size
ThingsBoard tb(mqttClient, MAX_MESSAGE_SIZE);

/// @brief Processes function for RPC call "setLedMode"
/// RPC_Data is a JSON variant, that can be queried using operator[]
/// See https://arduinojson.org/v5/api/jsonvariant/subscript/ for more details
/// @param data Data containing the rpc data that was called and its current value
/// @return Response that should be sent to the cloud. Useful for getMethods
RPC_Response processSetLedMode(const RPC_Data &data) {
  Serial.println("Received the set led state RPC method");

  // Process data
  int new_mode = data;

  Serial.print("Mode to change: ");
  Serial.println(new_mode);

  if (new_mode != 0 && new_mode != 1) {
    return RPC_Response("error", "Unknown mode!");
  }

  ledMode = new_mode;

  attributesChanged = true;

  // Returning current mode
  return RPC_Response("newMode", (int)ledMode);
}


// Optional, keep subscribed shared attributes empty instead,
// and the callback will be called for every shared attribute changed on the device,
// instead of only the one that were entered instead
const std::array<RPC_Callback, 1U> callbacks = {
  RPC_Callback{ "setLedMode", processSetLedMode }
};

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  delay(500);

  pinMode(levelAlarm, OUTPUT);
  pinMode(tempAlarm, OUTPUT);
  pinMode(levelTempAlarm, OUTPUT);
  servo.attach(servoPin, 500, 2400);
 
  initWiFi();
  delay(500);

  initThingsBoard();
}

void loop() {
  for (int i = 0; i <= 180; i++) {
    servo.write(i);
    delay(15);
  }
  digitalWrite(levelAlarm, LOW);
  digitalWrite(tempAlarm, LOW);
  digitalWrite(levelTempAlarm, LOW);

  for (int i = 180; i >= 0; i--) {
    servo.write(i);
    delay(15);
  }
  digitalWrite(levelAlarm, HIGH);
  digitalWrite(tempAlarm, HIGH);
  digitalWrite(levelTempAlarm, HIGH);

  Serial.print("Distancia: ");
  Serial.print(sonar.ping_cm());
  Serial.println("cm");

  tb.sendTelemetryData("liquidLevel", sonar.ping_cm());

  Serial.println("Subscribing for RPC...");
  // Perform a subscription. All consequent data processing will happen in
  // processSetLedState() and processSetLedMode() functions,
  // as denoted by callbacks array.
  if (!tb.RPC_Subscribe(callbacks.cbegin(), callbacks.cend())) {
    Serial.println("Failed to subscribe for RPC");
    return;
  }
}

void initWiFi() {
  Serial.println("Conecting ....");
  WiFi.begin(WIFI_AP,WIFI_PASS, 6);
  while(WiFi.status() != WL_CONNECTED){
    Serial.print(".");
    delay(100);
   }
  Serial.println("Connected!");
}

void initThingsBoard() {
  while(!tb.connected()){
      Serial.println("Not Connected to Thingsboard");
      if(!tb.connect(TB_SERVER, TOKEN)){
        Serial.println("Still not connected");
        return;
      }else{
      Serial.println("Connected to Thingsboard");;
      }
  }
}