#include "feetech.h"
#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <WebSocketsServer.h>


#include <SPIFFS.h>

// Replace with your network credentials
const char* ssid = "Police Surveillance Van";
const char* password = "ourpassword";

// Create an instance of the server
AsyncWebServer server(80);
WebSocketsServer webSocket = WebSocketsServer(81);

void webSocketTask(void* parameter) {
  while (true) {
    webSocket.loop();  // Handle WebSocket messages
    delay(10);         // Small delay to prevent busy-waiting
  }
}

void webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length) {

  switch (type) {
    case WStype_DISCONNECTED:
      Serial.printf("[%u] Disconnected!\n", num);
      break;
    case WStype_CONNECTED:
      {
        IPAddress ip = webSocket.remoteIP(num);
        Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);

        // send message to client
        //webSocket.sendTXT(num, "Connected");
      }
      break;
    case WStype_TEXT:
      Serial.printf("[%u] get Text: %s\n", num, payload);

      // send message to client
      // webSocket.sendTXT(num, "message here");

      // send data to all connected clients
      // webSocket.broadcastTXT("message here");
      break;
    case WStype_BIN:
      Serial.printf("[%u] get binary length: %u\n", num, length);
      //hexdump(payload, length);

      // send message to client
      // webSocket.sendBIN(num, payload, length);
      break;
    case WStype_ERROR:
    case WStype_FRAGMENT_TEXT_START:
    case WStype_FRAGMENT_BIN_START:
    case WStype_FRAGMENT:
    case WStype_FRAGMENT_FIN:
      break;
  }
}

// Function to send messages over WebSocket
void wsPrint(const String& message) {
  String mutableMessage = message;  // Create a mutable copy
  webSocket.broadcastTXT(mutableMessage);
}

#define RX_PIN 17  // DI
#define TX_PIN 18  // RO
#define DE_PIN 33  // Driver Enable
#define RE_PIN 3   // Receiver Enable


Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);

uint16_t flipBytes(uint16_t value) {
  return (value >> 8) | (value << 8);
}

uint8_t ids[] = { 1, 10, 11, 15 };
uint16_t pos1[] = { 0, 0, 0, 0 };
uint16_t pos2[] = { 1023, 1023, 1023, 4095 };


void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 10; i++) {
    Serial.println(i);
    delay(500);
  }

  pos2[3] = flipBytes(pos2[3]);
  // WiFi.begin(ssid, password);
  // while (WiFi.status() != WL_CONNECTED) {
  //   delay(1000);
  //   Serial.println("Connecting to WiFi...");
  // }
  // Serial.println("Connected to WiFi");
  // Serial.println("\nConnected!");
  // Serial.print("IP address: ");
  // Serial.println(WiFi.localIP());


  // if (!SPIFFS.begin(false)) {
  //   Serial.println("SPIFFS mount failed");
  //   return;
  // } else {
  //   Serial.println("SPIFFS mount initialized");
  // }

  servos.begin();

  //SetID(34, 15);

  // Serve the file upload form
  // server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
  //   String html = "<form action='/upload' method='POST' enctype='multipart/form-data'>";
  //   html += "<input type='file' name='file'><br>";
  //   html += "<input type='submit' value='Upload'>";
  //   html += "</form>";
  //   request->send(200, "text/html", html);
  // });

  // // Handle file uploads
  // server.on(
  //   "/upload", HTTP_POST, [](AsyncWebServerRequest* request) {
  //     request->send(200, "text/plain", "File uploaded successfully!");
  //   },
  //   handleFileUpload);

  // server.begin();

  // writeFile("/hello.txt", "Hello ");
  // appendFile("/hello.txt", "World!\r\n");
  // readFile("/hello.txt");
}

void readFile(const char* path) {
  Serial.printf("Reading file: %s\r\n", path);

  File file = SPIFFS.open(path);
  if (!file || file.isDirectory()) {
    Serial.println("- failed to open file for reading");
    return;
  }

  Serial.println("- read from file:");
  while (file.available()) {
    Serial.write(file.read());
  }
  file.close();
}

void appendFile(const char* path, const char* message) {
  Serial.printf("Appending to file: %s\r\n", path);

  File file = SPIFFS.open(path, FILE_APPEND);
  if (!file) {
    Serial.println("- failed to open file for appending");
    return;
  }
  if (file.print(message)) {
    Serial.println("- message appended");
  } else {
    Serial.println("- append failed");
  }
  file.close();
}

void writeFile(const char* path, const char* message) {
  Serial.printf("Writing file: %s\r\n", path);

  File file = SPIFFS.open(path, FILE_WRITE);
  if (!file) {
    Serial.println("- failed to open file for writing");
    return;
  }
  if (file.print(message)) {
    Serial.println("- file written");
  } else {
    Serial.println("- write failed");
  }
  file.close();
}

void listFiles() {
  Serial.println("Files in SPIFFS:");
  File root = SPIFFS.open("/");
  if (!root || !root.isDirectory()) {
    Serial.println("Failed to open root directory");
    return;
  }

  File file = root.openNextFile();
  bool found = false;

  while (file) {
    Serial.print("File: ");
    Serial.print(file.name());
    Serial.print(" | Size: ");
    Serial.println(file.size());
    file = root.openNextFile();
    found = true;
  }

  if (!found) {
    Serial.println("No files found in SPIFFS");
  }
}





void printFile(const String& filename) {
  File file = SPIFFS.open(filename, "r");
  if (!file) {
    Serial.println("Failed to open file for reading");
    return;
  }

  Serial.print("Content of ");
  Serial.println(filename);
  while (file.available()) {
    Serial.write(file.read());
  }
  file.close();
  Serial.println();  // New line after file content
}
void handleFileUpload(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final) {
  filename = "/" + filename.substring(filename.lastIndexOf('/') + 1);

  if (index == 0) {
    Serial.printf("UploadStart: %s\n", filename.c_str());
  }

  size_t previewLen = len > 256 ? 256 : len;  // limit preview
  Serial.print("Chunk preview: ");
  for (size_t i = 0; i < previewLen; i++) {
    Serial.print((char)data[i]);
  }
  Serial.println();


  if (final) {
    Serial.printf("UploadEnd: %s (%u)\n", filename.c_str(), index + len);
  }
}

void SetID(uint8_t oldID, uint8_t newID) {
  Serial.println("Setting Lock to 0");
  Serial.println(servos.setLock(oldID, 0));
  delay(1000);
  Serial.print("Changing ID ");
  Serial.print(oldID);
  Serial.print(" to ");
  Serial.println(newID);
  Serial.println(servos.setID(oldID, newID));
  delay(1000);
  Serial.println("Setting Lock to 1");
  Serial.println(servos.setLock(newID, 1));
  delay(1000);
}

void loop() {
  // put your main code here, to run repeatedly:
  //PingAll();


  servos.syncWritePos(ids, pos1, 4);
  delay(2000);
  servos.syncWritePos(ids, pos2, 4);
  delay(2000);
}

void PingAll() {
  std::vector<uint8_t> successfulAddresses;
  servos.pingAll(successfulAddresses);

  // Now successfulAddresses contains all successful pings
  Serial.println("Successful Addresses:");
  for (uint8_t address : successfulAddresses) {
    Serial.print(address);
    Serial.print(" ");
  }
  Serial.println();
}