#include <WiFi.h>           
#include <ESP32Servo.h>
#include <Preferences.h>
#include "ThingsBoard.h"

#define COUNT_OF(x) ((sizeof(x)/sizeof(0[x])) / ((size_t)(!(sizeof(x) % sizeof(0[x])))))

#define WIFI_AP_NAME              "Wokwi-GUEST"
#define WIFI_PASSWORD             ""
#define THINGSBOARD_SERVER        "demo.thingsboard.io"
#define THINGSBOARD_ACCESSTOKEN   "RFxQva8l2dJtj4VUO5BS"

#define SERIAL_DEBUG_BAUD    115200

WiFiClient espClient;
ThingsBoard tb(espClient);
int status = WL_IDLE_STATUS;

uint8_t leds_control[] = { 32, 33 };
int ledPin = 25;

const int servoPin = 15; 
Servo servo;
int pos = 0;

bool subscribed = false;

Preferences preferences;

//rtos
int threshold=100;

void TaskAnalogRead(void *pvParameters );
void TaskAlert(void *pvParameters );
void TaskCheckConnect(void *pvParameters );

void TaskAnalogRead(void *pvParameters){
  for (;;){     
    tb.sendTelemetryFloat("position", pos);
    delay(1000); 
  }
}

void TaskAlert(void *pvParameters){
  for (;;){    
    int newpos = *((int*)pvParameters);
    if (newpos>threshold)
    {
      pinMode(ledPin, OUTPUT);
      analogWrite(ledPin, 255);
      delay(300);
      analogWrite(ledPin, 0);
      delay(300);  
    }
    delay(100);
  }
}

void TaskCheckConnect(void *pvParameters){
  for (;;){    
    // Reconnect to WiFi, if needed
    if (WiFi.status() != WL_CONNECTED) {
      reconnect();
      return;
    }
  }
}

void reconnect(){
  // Loop until we're reconnected
  status = WiFi.status();
  if ( status != WL_CONNECTED) {
    WiFi.begin(WIFI_AP_NAME, WIFI_PASSWORD);
    while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
    }
    Serial.println("Connected to AP");
  }
}

//.[ RPC ]...................................................................................

RPC_Response SetGpioState(const RPC_Data &data)
{
  Serial.println("Received the set GPIO RPC method");
  int pin = data["pin"];
  bool enabled = data["enabled"];
  digitalWrite(pin, enabled);
  if (pin==leds_control[0]) preferences.putUInt("led1", enabled);
  if (pin==leds_control[1]) preferences.putUInt("led2", enabled);
  return String("{\"" + String(pin) + "\": " + String(enabled?"true":"false") + "}");
}

RPC_Response GetGpioState(const RPC_Data &data)
{
  Serial.println("Received the get GPIO RPC method");
  String respStr = "{";
  
  for (size_t i = 0; i < COUNT_OF(leds_control); ++i) {
    int pin = leds_control[i];
    bool ledState = digitalRead(pin);
    Serial.print("Getting LED "+String(pin)+" state "+String(ledState));
    respStr += String("\"" + String(pin) + "\": " + String(ledState?"true":"false") + ", ");
  }
  respStr = respStr.substring(0, respStr.length() - 2);
  respStr += "}";
  return respStr;
}

RPC_Response SetServo(const RPC_Data &data)
{
  Serial.println("Received the set Servo RPC method");
  pos=data;
  if (pos<0) pos=0;
  servo.write(pos);
  delay(15);
  preferences.putUInt("servo", pos);
  return String(pos);
}

RPC_Response GetServo(const RPC_Data &data)
{
  if (pos<0) pos=0;
  Serial.println("Received the get Servo RPC method");
  Serial.println("Servo: "+String(pos));
  return RPC_Response(pos); 
}

// RPC handlers
RPC_Callback callbacks[] = {
  { "setGpioStatus",  SetGpioState },
  { "getGpioStatus",  GetGpioState },
  { "SetServo",       SetServo},
  { "GetServo",       GetServo},
};

//....................................................................................

void setup() {
  Serial.begin(SERIAL_DEBUG_BAUD);

  //WiFi
  WiFi.begin(WIFI_AP_NAME, WIFI_PASSWORD);
  Serial.println("Connecting to AP ...");
  WiFi.begin(WIFI_AP_NAME, WIFI_PASSWORD);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected to AP");

  for (size_t i = 0; i < COUNT_OF(leds_control); ++i) {
    pinMode(leds_control[i], OUTPUT);
  }

  pinMode(ledPin, OUTPUT);
  servo.attach(servoPin, 500, 2400);  

  //preferences - restore
  preferences.begin("xmt-tb-demo", false);

  pos = preferences.getUInt("servo", 0);
  servo.write(pos);
  delay(2000);

  digitalWrite(leds_control[0], preferences.getUInt("led1", 0));
  digitalWrite(leds_control[1], preferences.getUInt("led2", 0));

  digitalWrite(ledPin, preferences.getUInt("ledpin", 0));

  //preferences.end();

  //rtos - xTaskCreate( TaskFunction_t pvTaskCode, const char * const pcName,
  //                    configSTACK_DEPTH_TYPE usStackDepth, void *pvParameters, 
  //                    UBaseType_t uxPriority, TaskHandle_t *pxCreatedTask);
  xTaskCreate(TaskAnalogRead, "Analog Read", 2048, NULL, 1, NULL);
  xTaskCreate(TaskAlert, "Alert", 2048, (void*) &pos, 1, NULL);
  xTaskCreate(TaskCheckConnect, "Check Connect", 2048, NULL, 1, NULL);  
}

void loop() { 
  // Reconnect to ThingsBoard, if needed
  if (!tb.connected()) {
    subscribed = false;
    Serial.print("Connecting to: ");
    Serial.print(THINGSBOARD_SERVER);
    Serial.print(" with token ");
    Serial.print(THINGSBOARD_ACCESSTOKEN);
    if (!tb.connect(THINGSBOARD_SERVER, THINGSBOARD_ACCESSTOKEN)) {
      Serial.println("Failed to connect");
      return;
    }
  }

  if (!subscribed) {
    Serial.println("Subscribing for RPC... ");
    if (!tb.RPC_Subscribe(callbacks, COUNT_OF(callbacks))) {
      Serial.println("Failed to subscribe for RPC");
      return;
    }
    Serial.println("Subscribe done");
    subscribed = true;
  }    

  tb.loop();  
}