#include <WiFi.h>
#include <Streaming.h>
#include <HTTPClient.h>
#include <Preferences.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include <ArduinoJson.h>
#include <WebSocketsClient.h>
#include <SocketIOclient.h>
#include <driver/gptimer.h>
#include "Defines.hpp"

const uint8_t portToServo[4] = {15, 12, 13, 26};
const uint8_t portTo0Button[4] = {2, 36, 34, 14};
const uint8_t portTo1Button[4] = {4, 39, 35, 25};

volatile uint8_t calibCCW = 0;
volatile uint8_t calibCW = 0;
uint8_t targetPos[4] = {0, 0, 0, 0};
volatile uint8_t movingCCW = 0;
volatile uint8_t movingCW = 0;
volatile uint32_t cwTimes[4] = {0,0,0,0};
volatile uint32_t ccwTimes[4] = {0,0,0,0};
volatile uint32_t totalTimes[4] = {0,0,0,0};
volatile uint8_t calibDone = 0;
uint8_t blocked = 0;

gptimer_handle_t portTimers[4] = {NULL, NULL, NULL, NULL};
gptimer_config_t timer_config = {
    .clk_src = GPTIMER_CLK_SRC_DEFAULT,
    .direction = GPTIMER_COUNT_UP,
    .resolution_hz = 10000, // 10KHz, 1 tick = 1ms
};

portMUX_TYPE sharedData = portMUX_INITIALIZER_UNLOCKED;

void IRAM_ATTR portStop(uint8_t portNum) {
  gptimer_stop(portTimers[portNum]);
  portENTER_CRITICAL_ISR(&sharedData);
  ledcWrite(portToServo[portNum], offSpeed);
  clearMovingCW(portNum);
  clearMovingCCW(portNum);
  portEXIT_CRITICAL_ISR(&sharedData);
}

void IRAM_ATTR portStopButton(uint8_t portNum) {
  portENTER_CRITICAL_ISR(&sharedData);
  ledcWrite(portToServo[portNum], offSpeed);
  clearMovingCW(portNum);
  clearMovingCCW(portNum);
  portEXIT_CRITICAL_ISR(&sharedData);
}

static IRAM_ATTR bool port0Stop(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *user_data) {
  BaseType_t high_task_awoken = pdFALSE;
  portStop(0);
  return high_task_awoken == pdTRUE;
}
static IRAM_ATTR bool port1Stop(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *user_data) {
  BaseType_t high_task_awoken = pdFALSE;
  portStop(1);
  return high_task_awoken == pdTRUE;
}
static IRAM_ATTR bool port2Stop(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *user_data) {
  BaseType_t high_task_awoken = pdFALSE;
  portStop(2);
  return high_task_awoken == pdTRUE;
}
static IRAM_ATTR bool port3Stop(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *user_data) {
  BaseType_t high_task_awoken = pdFALSE;
  portStop(3);
  return high_task_awoken == pdTRUE;
}

void IRAM_ATTR port0StopButton() {portStopButton(0);}
void IRAM_ATTR port1StopButton() {portStopButton(1);}
void IRAM_ATTR port2StopButton() {portStopButton(2);}
void IRAM_ATTR port3StopButton() {portStopButton(3);}

typedef bool (* GptimerEventCallback_t)(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *user_data);

GptimerEventCallback_t portStops[] = {
  &port0Stop,
  &port1Stop,
  &port2Stop,
  &port3Stop
};

void (*portStopsButton[])(void) = {
  &port0StopButton, // Take the address of the function
  &port1StopButton,
  &port2StopButton,
  &port3StopButton
};

void IRAM_ATTR portSwitchCalib(uint8_t portNum) {
  portENTER_CRITICAL_ISR(&sharedData);
  if (!getCalibCW(portNum)) {
    ledcWrite(portToServo[portNum], cwSpeed);
    ESP_ERROR_CHECK(gptimer_stop(portTimers[portNum]));
    uint64_t count;
    ESP_ERROR_CHECK(gptimer_get_raw_count(portTimers[portNum], &count));
    ccwTimes[portNum] = count;
    setCalibCW(portNum);
  }
  portEXIT_CRITICAL_ISR(&sharedData);
}

void IRAM_ATTR port0SwitchCalib() {portSwitchCalib(0);}
void IRAM_ATTR port1SwitchCalib() {portSwitchCalib(1);}
void IRAM_ATTR port2SwitchCalib() {portSwitchCalib(2);}
void IRAM_ATTR port3SwitchCalib() {portSwitchCalib(3);}

void (*portSwitchCalibs[])(void) = {
  &port0SwitchCalib, // Take the address of the function
  &port1SwitchCalib,
  &port2SwitchCalib,
  &port3SwitchCalib
};

void IRAM_ATTR portEndCalib(uint8_t portNum) {
  portENTER_CRITICAL_ISR(&sharedData);
  if (!getCalibDone(portNum)) {
    ledcWrite(portToServo[portNum], offSpeed);
    ESP_ERROR_CHECK(gptimer_stop(portTimers[portNum]));
    uint64_t count;
    ESP_ERROR_CHECK(gptimer_get_raw_count(portTimers[portNum], &count));
    cwTimes[portNum] = count;
    setCalibDone(portNum);
  }
  portEXIT_CRITICAL_ISR(&sharedData);
}

void IRAM_ATTR port0EndCalib() {portEndCalib(0);}
void IRAM_ATTR port1EndCalib() {portEndCalib(1);}
void IRAM_ATTR port2EndCalib() {portEndCalib(2);}
void IRAM_ATTR port3EndCalib() {portEndCalib(3);}

void (*portEndCalibs[])(void) = {
  &port0EndCalib, // Take the address of the function
  &port1EndCalib,
  &port2EndCalib,
  &port3EndCalib
};

void IRAM_ATTR hitPos0(uint8_t portNum) {
  portENTER_CRITICAL_ISR(&sharedData);
  if (!getPos0(portNum)) {
    ledcWrite(portToServo[portNum], offSpeed);
    setPos0(portNum);
  }
  portEXIT_CRITICAL_ISR(&sharedData);
}

void IRAM_ATTR port0hit0() {hitPos0(0);}
void IRAM_ATTR port1hit0() {hitPos0(1);}
void IRAM_ATTR port2hit0() {hitPos0(2);}
void IRAM_ATTR port3hit0() {hitPos0(3);}

void (*portsHit0[])(void) = {
  &port0hit0, // Take the address of the function
  &port1hit0,
  &port2hit0,
  &port3hit0
};

void IRAM_ATTR hitPos10(uint8_t portNum) {
  portENTER_CRITICAL_ISR(&sharedData);
  if (!getPos10(portNum)) {
    ledcWrite(portToServo[portNum], offSpeed);
    setPos10(portNum);
  }
  portEXIT_CRITICAL_ISR(&sharedData);
}

void IRAM_ATTR port0hit10() {hitPos10(0);}
void IRAM_ATTR port1hit10() {hitPos10(1);}
void IRAM_ATTR port2hit10() {hitPos10(2);}
void IRAM_ATTR port3hit10() {hitPos10(3);}

void (*portsHit10[])(void) = {
  &port0hit10, // Take the address of the function
  &port1hit10,
  &port2hit10,
  &port3hit10
};

Preferences prefs;
SocketIOclient socketIO;

String ssid = "";
String password = "";
String token = "";
const String socketUrl = "/socket.io/?EIO=4";
const char* ntpServer = "pool.ntp.org";
bool ssidgiven = false;
bool passgiven = false;
bool tokengiven = false;
bool connecting = false;
bool socketIoHandshakeDone = false;
BLECharacteristic *ssidChar;
BLECharacteristic *passChar;
BLECharacteristic *ssidListChar;
BLECharacteristic *tokenChar;
BLECharacteristic *ssidRefreshChar;
BLECharacteristic *connectConfirmChar;
HTTPClient http;
bool restart = false;


const String address = "192.168.1.190";
const uint16_t port = 3000;
const String path = "/socket.io/?EIO=4&transport=websocket";
const String httpSrv = "http://192.168.1.190:3000/";

void socketIOEvent(socketIOmessageType_t type, uint8_t * payload, size_t length);
String getEncryption(wifi_auth_mode_t type) {
  switch (type) {
    case WIFI_AUTH_OPEN:
      return "OPEN";
    case WIFI_AUTH_WEP:
    case WIFI_AUTH_WPA_PSK:
    case WIFI_AUTH_WPA2_PSK:
    case WIFI_AUTH_WPA_WPA2_PSK:
    case WIFI_AUTH_WPA3_PSK:
      return "SECURED";
    default:
      // These include WPA2/WPA3-Enterprise and others requiring user/password or certs
      return "UNSUPPORTED";
  }
}

void scanAndUpdateSSIDList() {
  int n = WiFi.scanNetworks();
  String ssidList = "";

  if (n == 0) ssidList = ";";

  else for (int i = 0; i < n; ++i) {
    ssidList += WiFi.SSID(i) + "," + getEncryption(WiFi.encryptionType(i));
    if (i < n - 1) ssidList += ";";  // comma-delimited
  }

  ssidListChar->setValue(ssidList.c_str());
  Serial.println("Updated SSID list: " + ssidList);
}

class MyCallbacks;
class MyServerCallbacks;

void saveWiFiCreds(const String& ssid, const String& password) {
  prefs.begin("wifi"); // false = read/write
  prefs.putString("ssid", ssid);
  if (password != "") prefs.putString("password", password);
  prefs.end();
}

void saveToken(const String& token) {
  prefs.begin("auth", false);
  prefs.putString("token", token);
  prefs.end();
}

void saveCalibTimes(const uint8_t& portNum) {
  String prefName = prefNameCalibs + String(portNum);
  prefs.begin(prefName.c_str());
  portENTER_CRITICAL(&sharedData);
  uint32_t cwTime = cwTimes[portNum];
  uint32_t ccwTime = ccwTimes[portNum];
  portEXIT_CRITICAL(&sharedData);
  Serial.printf("CwTime: %d", cwTime);
  Serial.printf("CcwTime: %d", ccwTime);
  prefs.putULong("cwTime", cwTime);
  prefs.putULong("ccwTime", ccwTime);
  if (!prefs.getBool("calibrated")) prefs.putBool("calibrated", true);
  if (prefs.getUChar("pos") != 0) prefs.putUChar("pos", 0);
  prefs.end();
  detachInterrupt(portTo1Button[portNum]);
  portENTER_CRITICAL(&sharedData);
  clearCalibDone(portNum);
  portEXIT_CRITICAL(&sharedData);
}

void beginCalib(const uint8_t& portNum) {
  prefs.begin((prefNameCalibs + String(portNum)).c_str());
  if (prefs.getBool("calibrated")) prefs.putBool("calibrated", false);
  prefs.end();
  Serial.printf("beginCalib%d\n", portNum);
  ledcWrite(portToServo[portNum], ccwSpeed);
  ESP_ERROR_CHECK(gptimer_disable(portTimers[portNum]));
  ESP_ERROR_CHECK(gptimer_del_timer(portTimers[portNum]));
  ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &portTimers[portNum]));
  gptimer_event_callbacks_t cbs = {
    .on_alarm = portStops[portNum],
  };
  ESP_ERROR_CHECK(gptimer_register_event_callbacks(portTimers[portNum], &cbs, NULL));
  ESP_ERROR_CHECK(gptimer_enable(portTimers[portNum]));
  ESP_ERROR_CHECK(gptimer_set_raw_count(portTimers[portNum], 0));
  ESP_ERROR_CHECK(gptimer_start(portTimers[portNum]));
  attachInterrupt(portTo0Button[portNum], portSwitchCalibs[portNum], RISING);
  portENTER_CRITICAL(&sharedData);
  clearCalibCCW(portNum);
  portEXIT_CRITICAL(&sharedData);
}

void switchCalib(const uint8_t& portNum) {
  detachInterrupt(portTo0Button[portNum]);
  ledcWrite(portToServo[portNum], cwSpeed);
  ESP_ERROR_CHECK(gptimer_set_raw_count(portTimers[portNum], 0));
  ESP_ERROR_CHECK(gptimer_start(portTimers[portNum]));
  attachInterrupt(portTo1Button[portNum], portEndCalibs[portNum], RISING);
  portENTER_CRITICAL(&sharedData);
  clearCalibCW(portNum);
  portEXIT_CRITICAL(&sharedData);
}

void cancelCalib(const uint8_t& portNum) {
  detachInterrupt(portTo0Button[portNum]);
  detachInterrupt(portTo1Button[portNum]);
  ledcWrite(portToServo[portNum], offSpeed);
  portENTER_CRITICAL(&sharedData);
  clearCalibCCW(portNum);
  clearCalibCW(portNum);
  clearCalibDone(portNum);
  cwTimes[portNum] = 0;
  ccwTimes[portNum] = 0;
  portEXIT_CRITICAL(&sharedData);
  ESP_ERROR_CHECK(gptimer_stop(portTimers[portNum]));
}

bool getCalibStatus(const uint8_t& portNum) {
  prefs.begin((prefNameCalibs + String(portNum)).c_str());
  bool retVal = prefs.getBool("calibrated");
  prefs.end();
  return retVal;
}

uint32_t getCWTime(const uint8_t& portNum) {
  prefs.begin((prefNameCalibs + String(portNum)).c_str());
  uint32_t retVal = prefs.getULong("cwTime");
  prefs.end();
  return retVal;
}

uint32_t getCCWTime(const uint8_t& portNum) {
  prefs.begin((prefNameCalibs + String(portNum)).c_str());
  uint32_t retVal = prefs.getULong("ccwTime");
  prefs.end();
  return retVal;
}

void clearNamespace(const char* nameSpace) {
  prefs.begin(nameSpace);
  prefs.clear();
  prefs.end();
}

void clearStorage() {
  clearNamespace("wifi");
  clearNamespace("auth");
  for (uint8_t i = 0; i < 4; i++) {
    String prefName = prefNameCalibs + String(i);
    clearNamespace(prefName.c_str());
  }
}

uint8_t getPos(const uint8_t& portNum) {
  prefs.begin((prefNameCalibs + String(portNum)).c_str());
  uint8_t prevPos = prefs.getUChar("pos");
  prefs.end();
  return prevPos;
}

void putPos(const uint8_t& portNum, const uint8_t& pos) {
  prefs.begin((prefNameCalibs + String(portNum)).c_str());
  prefs.putUChar("pos", pos);
  prefs.end();
}

uint32_t posUpdateAndGetTurnTime(const uint8_t& portNum, const uint8_t& newPos) {
  prefs.begin((prefNameCalibs + String(portNum)).c_str());
  uint8_t prevPos = prefs.getUChar("pos");
  if (newPos != prevPos) prefs.putUChar("pos", newPos);
  prefs.end();

  uint32_t timeToTurn = 0;
  uint32_t remainingTime;
  uint64_t readMillis;
  gptimer_get_raw_count(portTimers[portNum], &readMillis);
  portENTER_CRITICAL(&sharedData);
  remainingTime = (totalTimes[portNum] > readMillis) ? totalTimes[portNum] - (uint32_t)readMillis : 0;
  portEXIT_CRITICAL(&sharedData);
  uint32_t cwTime = getCWTime(portNum);
  uint32_t ccwTime = getCCWTime(portNum);

  if (newPos < prevPos) {
    timeToTurn = ((prevPos - newPos) * cwTime) / 10;
    Serial.printf("TimeToTurn: %d\n", timeToTurn);
    portENTER_CRITICAL(&sharedData);
    if (getMovingCW(portNum)) {
      portEXIT_CRITICAL(&sharedData);
      gptimer_stop(portTimers[portNum]);
      timeToTurn += remainingTime;
    } else if (getMovingCCW(portNum)) {
      portEXIT_CRITICAL(&sharedData);
      gptimer_stop(portTimers[portNum]);
      double result = ((double)cwTime / (double) ccwTime) * (double) remainingTime;
      uint32_t intRes = result;
      if (intRes > timeToTurn) {
        result = ((double)ccwTime / (double)cwTime) * (double) timeToTurn;
        timeToTurn = remainingTime - (uint32_t)result;
      } else {
        timeToTurn -= intRes;
        portENTER_CRITICAL(&sharedData);
        clearMovingCCW(portNum);
        setMovingCW(portNum);
        portEXIT_CRITICAL(&sharedData);
      }
    } else {
      setMovingCW(portNum);
      portEXIT_CRITICAL(&sharedData);
    }
  }
  else if (newPos > prevPos) {
    timeToTurn = ((newPos - prevPos) * ccwTime) / 10;
    portENTER_CRITICAL(&sharedData);
    if (getMovingCCW(portNum)) {
      portEXIT_CRITICAL(&sharedData);
      gptimer_stop(portTimers[portNum]);
      timeToTurn += remainingTime;
    } else if (getMovingCW(portNum)) {
      portEXIT_CRITICAL(&sharedData);
      gptimer_stop(portTimers[portNum]);
      double result = ((double)ccwTime / (double)cwTime) * (double)remainingTime;
      uint32_t intRes = result;
      if (intRes > timeToTurn) {
        result = ((double)cwTime / (double)ccwTime) * (double)timeToTurn;
        timeToTurn = remainingTime - (uint32_t)result;
      } else {
        timeToTurn -= intRes;
        portENTER_CRITICAL(&sharedData);
        clearMovingCW(portNum);
        setMovingCCW(portNum);
        portEXIT_CRITICAL(&sharedData);
      }
    } else {
      setMovingCCW(portNum);
      portEXIT_CRITICAL(&sharedData);
    }
  }
  else timeToTurn = 0;

  return timeToTurn;
}

void notifyServerCalibrated(const uint8_t& port) {
  DynamicJsonDocument doc(256);
  JsonArray array = doc.to<JsonArray>();

  array.add("calib_done");
  JsonObject param1 = array.createNestedObject();
  param1["port"] = port + 1;
  String toSend;
  serializeJson(doc, toSend);
  socketIO.sendEVENT(toSend);
}

void notifyPos(const uint8_t& port, const uint8_t& pos) {
  DynamicJsonDocument doc(256);
  JsonArray array = doc.to<JsonArray>();

  array.add("pos_hit");
  JsonObject param1 = array.createNestedObject();
  param1["port"] = port + 1;
  param1["pos"] = pos;
  String toSend;
  serializeJson(doc, toSend);
  socketIO.sendEVENT(toSend);
}

void initSetup();
void connect();
void initializePeriphs();

void setup() {
  Serial.begin(9600);
  // while(!Serial);
  pinMode(15, OUTPUT);
  digitalWrite(15, HIGH);

  connect();

  configTime(0, 0, ntpServer);

  Serial.println("Waiting for NTP time synchronization...");
  struct tm timeinfo;
  uint8_t retryCount = 0;
  // Wait for time to be set (it might take a few seconds)
  while (!getLocalTime(&timeinfo) && retryCount < 10) {
    Serial.print(".");
    delay(1000);
    retryCount++;
  }

  if (retryCount >= 10) {
    Serial.println("\nFailed to obtain time from NTP server after multiple retries!");
  } else {
    Serial.println("\nTime synchronized successfully!");
  }

  for (uint8_t i = 0; i < 4; i++) {
    ledcAttach(portToServo[i], 50, 16);
    ledcWrite(portToServo[i], offSpeed);
  }
  for (uint8_t i = 0; i < 4; i++) pinMode(portTo0Button[i], INPUT);
  for (uint8_t i = 0; i < 4; i++) pinMode(portTo1Button[i], INPUT);
  for (uint8_t i = 0; i < 4; i++) {
    ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &portTimers[i]));
    gptimer_event_callbacks_t cbs = {
      .on_alarm = portStops[i],
    };
    ESP_ERROR_CHECK(gptimer_register_event_callbacks(portTimers[i], &cbs, NULL));
    ESP_ERROR_CHECK(gptimer_enable(portTimers[i]));
  }

  String auth = String("Bearer ") + token;
  
  // server address, port and URL
  socketIO.begin(address, port, socketUrl, true, auth);

  // event handler
  socketIO.onEvent(socketIOEvent);

  initializePeriphs();
}

void loop() {
  socketIO.loop();
  for (uint8_t i = 0; i < 4; i++) {
    portENTER_CRITICAL(&sharedData);
    if (getCalibCCW(i)) {
      portEXIT_CRITICAL(&sharedData);
      beginCalib(i);
    } else if (getCalibCW(i)) {
      portEXIT_CRITICAL(&sharedData);
      switchCalib(i);
    } else if (getCalibDone(i)) {
      portEXIT_CRITICAL(&sharedData);
      saveCalibTimes(i);
      notifyServerCalibrated(i);
    } else if (getPos0(i)) {
      clearPos0(i);
      clearMovingCW(i);
      portEXIT_CRITICAL(&sharedData);
      gptimer_stop(portTimers[i]);
      if (!(getPos(i) == 0)) putPos(i, 0);
      notifyPos(i, 0);
      detachInterrupt(portTo1Button[i]);
    } else if (getPos10(i)) {
      clearPos10(i);
      clearMovingCCW(i);
      portEXIT_CRITICAL(&sharedData);
      gptimer_stop(portTimers[i]);
      if (!(getPos(i) == 10)) putPos(i, 10);
      notifyPos(i, 10);
      detachInterrupt(portTo0Button[i]);
    }
    else {
      portEXIT_CRITICAL(&sharedData);
      if (getCalibStatus(i)) {
        portENTER_CRITICAL(&sharedData);
        if (getBlocked(i) && !getMovingCW(i) && !getMovingCCW(i)) clearBlocked(i);
        portEXIT_CRITICAL(&sharedData);

        if (targetPos[i] != 0 && targetPos[i] != 10) {
          if (!getBlocked(i)) {
            uint32_t timeToTurn = posUpdateAndGetTurnTime(i, targetPos[i]);
            if (timeToTurn != 0) {
              portENTER_CRITICAL(&sharedData);
              totalTimes[i] = timeToTurn;
              bool movingcw = getMovingCW(i);
              bool movingccw = getMovingCCW(i);
              portEXIT_CRITICAL(&sharedData);
              if (movingcw) {
                ledcWrite(portToServo[i], cwSpeed);
                attachInterrupt(portTo1Button[i], portsHit0[i], RISING);
              }
              else if (movingccw) {
                ledcWrite(portToServo[i], ccwSpeed);
                attachInterrupt(portTo0Button[i], portsHit10[i], RISING);
              }
              gptimer_set_raw_count(portTimers[i], 0);
              gptimer_alarm_config_t alarm_config = {
                .alarm_count = timeToTurn,
                .flags = {
                  .auto_reload_on_alarm = false,
                }
              };
              ESP_ERROR_CHECK(gptimer_set_alarm_action(portTimers[i], &alarm_config));
              ESP_ERROR_CHECK(gptimer_start(portTimers[i]));
            }
          }
        } else if (!(targetPos[i] == getPos(i))){
          setBlocked(i);
          putPos(i, targetPos[i]);
          if (targetPos[i] == 0) {
            ledcWrite(portToServo[i], cwSpeed);
            gptimer_stop(portTimers[i]);
            portENTER_CRITICAL(&sharedData);
            setMovingCW(i);
            clearMovingCCW(i);
            portEXIT_CRITICAL(&sharedData);
            attachInterrupt(portTo1Button[i], portStopsButton[i], RISING);
          } 
          else {
            ledcWrite(portToServo[i], ccwSpeed);
            gptimer_stop(portTimers[i]);
            portENTER_CRITICAL(&sharedData);
            setMovingCCW(i);
            clearMovingCW(i);
            portEXIT_CRITICAL(&sharedData);
            attachInterrupt(portTo0Button[i], portStopsButton[i], RISING);
          }
        }
      }
    }
  }
}

void initializePeriphs() {
  if (http.begin(httpSrv + "position")) {
    http.addHeader("Authorization", "Bearer " + token);
    int httpCode = http.GET();
    if (httpCode > 0) {
      String payload = http.getString();
      StaticJsonDocument<256> doc;
      DeserializationError error = deserializeJson(doc, payload);

      if (error) {
        Serial.println("Error getting details");
      }

      JsonArray docArray = doc.as<JsonArray>();
      for (JsonObject periph : docArray) {
        uint8_t periphNum = periph["peripheral_number"].as<uint8_t>() - 1;
        if (periph["await_calib"].as<bool>()) {
          portENTER_CRITICAL(&sharedData);
          setCalibCCW(periphNum);
          clearCalibCW(periphNum);
          clearCalibDone(periphNum);
          portEXIT_CRITICAL(&sharedData);
        }
        targetPos[periphNum] = periph["last_pos"].as<uint8_t>();
      }
    }
  }
  http.end();
}

class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) {
    // Code to execute when a client connects
    Serial.println("Client connected");
  }

  void onDisconnect(BLEServer* pServer) {
    // Code to execute when a client disconnects
    Serial.println("Client disconnected");
    restart = true;
    // For example, restart advertising
    if (connecting) pServer->startAdvertising();
  }
};

class MyCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pChar) {
    if (pChar == ssidChar) {
      ssid = pChar->getValue().c_str();
      Serial.println("SSID received: " + ssid);
      ssidgiven = true;
    } else if (pChar == passChar) {
      password = pChar->getValue().c_str();
      Serial.println("Password received: " + password);
      passgiven = true;
    }
    else if (pChar == tokenChar) {
      token = pChar->getValue().c_str();
      Serial.println("Token received: " + token);
      tokengiven = true;
    }
    else if (pChar == ssidRefreshChar) {
      Serial.println("Wifi Refresh");
      restart = true;
      scanAndUpdateSSIDList();
      ssidListChar->notify();
    }
  }
};

void socketIOEvent(socketIOmessageType_t type, uint8_t * payload, size_t length) {
    switch(type) {
        case sIOtype_DISCONNECT:
            Serial.printf("[IOc] Disconnected!\n");
            break;
        case sIOtype_CONNECT:
            Serial.printf("[IOc] Connected to url: %s\n", payload);

            // join default namespace (no auto join in Socket.IO V3)
            socketIO.send(sIOtype_CONNECT, "/");
            break;
        case sIOtype_EVENT:
        {
            char * sptr = NULL;
            int id = strtol((char *)payload, &sptr, 10);
            Serial.printf("[IOc] get event: %s id: %d\n", payload, id);
            if(id) {
                payload = (uint8_t *)sptr;
            }
            DynamicJsonDocument doc(1024);
            DeserializationError error = deserializeJson(doc, payload, length);
            if(error) {
                Serial.print(F("deserializeJson() failed: "));
                Serial.printf(error.c_str());
                return;
            }

            String eventName = doc[0];
            Serial.printf("[IOc] event name: %s\n", eventName.c_str());

            if (eventName == "posUpdates") {
              JsonArray updatesList = doc[1].as<JsonArray>();
              for (JsonVariant update : updatesList) {
                if (update.is<JsonObject>()) {
                  JsonObject updateJSON = update.as<JsonObject>();
                  uint8_t periphNum = updateJSON["periphNum"].as<uint8_t>() - 1;
                  uint8_t pos = updateJSON["pos"].as<uint8_t>();
                  Serial.printf("Peripheral: %d, Position: %d\n", periphNum, pos);
                  targetPos[periphNum] = pos;
                }
              }
            } else if (eventName == "calib") {
              JsonObject toCalib = doc[1].as<JsonObject>();
              uint8_t periphNum = toCalib["periphNum"].as<uint8_t>() - 1;
              portENTER_CRITICAL(&sharedData);
              if (!(getCalibCW(periphNum) || getCalibDone(periphNum))) setCalibCCW(periphNum);
              portEXIT_CRITICAL(&sharedData);
            } else if (eventName == "cancel_calib") {
              JsonObject toCancel = doc[1].as<JsonObject>();
              cancelCalib(toCancel["periphNum"].as<uint8_t>() - 1);
            }
        }
            break;
        case sIOtype_ACK:
            Serial.printf("[IOc] get ack: %u\n", length);
            break;
        case sIOtype_ERROR:
            Serial.printf("[IOc] get error: %u\n", length);
            break;
        case sIOtype_BINARY_EVENT:
            Serial.printf("[IOc] get binary: %u\n", length);
            break;
        case sIOtype_BINARY_ACK:
            Serial.printf("[IOc] get binary ack: %u\n", length);
            break;
    }
}

void initSetup() {
  BLEDevice::init("BlindMaster Device");
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks);
  BLEService *pService = pServer->createService(BLEUUID((uint16_t)0x181C)); // custom service

  BLESecurity *pSecurity = new BLESecurity();
  pSecurity->setCapability(ESP_IO_CAP_NONE);
  pSecurity->setAuthenticationMode(ESP_LE_AUTH_REQ_SC_ONLY); // or ESP_LE_AUTH_BOND
  pSecurity->setInitEncryptionKey(ESP_BLE_ENC_KEY_MASK | ESP_BLE_ID_KEY_MASK);

  ssidListChar = pService->createCharacteristic("0000", BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
  ssidListChar->addDescriptor(new BLE2902());
  ssidRefreshChar = pService->createCharacteristic("0004", BLECharacteristic::PROPERTY_WRITE);
  ssidChar = pService->createCharacteristic("0001", BLECharacteristic::PROPERTY_WRITE);
  passChar = pService->createCharacteristic("0002", BLECharacteristic::PROPERTY_WRITE);
  connectConfirmChar = pService->createCharacteristic("0005", BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
  connectConfirmChar->addDescriptor(new BLE2902());
  tokenChar = pService->createCharacteristic("0003", BLECharacteristic::PROPERTY_WRITE);

  ssidChar->setCallbacks(new MyCallbacks());
  passChar->setCallbacks(new MyCallbacks());
  tokenChar->setCallbacks(new MyCallbacks());
  ssidRefreshChar->setCallbacks(new MyCallbacks());

  scanAndUpdateSSIDList();

  pService->start();
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID("181C");
  pAdvertising->start();
  Serial.println("BLE service started.");

  while (1) {
    while (!(ssidgiven && passgiven)) delay(100);
    if (password == "") WiFi.begin(ssid.c_str());
    else WiFi.begin(ssid.c_str(), password.c_str());

    uint32_t startAttemptTime = millis();
    const uint32_t timeout = 10000;
    Serial.print("Connecting to WiFi");

    while (WiFi.status() != WL_CONNECTED && millis() - startAttemptTime < timeout) {
      Serial.print(".");
      delay(500);
    }

    if (WiFi.status() == WL_CONNECTED) {
      Serial<<endl<<"Connected at "<<WiFi.localIP()<<endl;
      connectConfirmChar->setValue("Connected");
      connectConfirmChar->notify();
      connectConfirmChar->setValue("");  // Clear value after notify
      restart = false;
      saveWiFiCreds(ssid, password);
      while (!tokengiven) {
        delay(100);
        if (restart) {
          ssidgiven = false;
          passgiven = false;
          break;
        }
      }
      if (restart) continue;
      if (http.begin(httpSrv + "verify_device")) {
        http.addHeader("Authorization", "Bearer " + token);
        int httpCode = http.GET();
        if (httpCode > 0) {
          String payload = http.getString();
          StaticJsonDocument<256> doc;
          DeserializationError error = deserializeJson(doc, payload);
          if (error) {
            Serial.print("deserializeJson() failed: ");
            Serial.println(error.f_str());
            ssidgiven = false;
            passgiven = false;
            tokengiven = false;
          }
          token = doc["token"].as<String>();
          Serial.println(token);
          saveToken(token);
          break;
        }
        http.end();
      }
      
    } else {
      Serial<<endl<<"Timed out"<<endl;
      ssidgiven = false;
      passgiven = false;
      WiFi.disconnect(true);
    }
    WiFi.disconnect(true);
    connectConfirmChar->setValue("Error");
    connectConfirmChar->notify();
    connectConfirmChar->setValue("");  // Clear value after notify
  }
  pAdvertising->stop();
}

void connect() {
  connecting = true;
  prefs.begin("wifi", true); // true = read only
  String storedSSID = prefs.getString("ssid", "");
  String storedPass = prefs.getString("password", "");
  prefs.end();

  if (storedSSID.length() == 0 && storedPass.length() == 0) {
    initSetup();
  } else if (storedSSID.length() != 0 && storedPass.length() == 0) {
    WiFi.begin(storedSSID.c_str());
    uint32_t startAttemptTime = millis();
    const uint32_t timeout = 10000;
    while (WiFi.status() != WL_CONNECTED && millis() - startAttemptTime < timeout) {
      Serial.print(".");
      delay(500);
    }

    if (WiFi.status() != WL_CONNECTED) {
      initSetup();
      connect();
    }
  } else {
    WiFi.begin(storedSSID.c_str(), storedPass.c_str());
    uint32_t startAttemptTime = millis();
    const uint32_t timeout = 10000;
    while (WiFi.status() != WL_CONNECTED && millis() - startAttemptTime < timeout) {
      Serial.print(".");
      delay(500);
    }

    Serial.println(WiFi.status());

    if (WiFi.status() != WL_CONNECTED) {
      initSetup();
      connect();
    }
  } 

  connecting = false;
  prefs.begin("auth", true);
  token = prefs.getString("token", "");
  prefs.end();
}