#pragma region Include Library
#include <Arduino.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include <WiFi.h>
#include <WebSocketsServer.h>
#pragma endregion

#pragma region Objek Library
Adafruit_MPU6050 mpu;
WebSocketsServer webSocket(81);
TaskHandle_t TaskLED = NULL;
TaskHandle_t TaskSTB = NULL;
TaskHandle_t TaskBCN = NULL;
#pragma endregion

#pragma region Variabel Global
const char semicolon = ';';
const char* SSID = "ESP32WebSocket";
const char* Password = "123321abccba";

float pitch = 0;
float roll = 0;
float yaw = 0;

bool LEDStatus[3], lastLEDStatus[3];

String dataSD, dataWS, pldStr;

unsigned long currTime = 0;
unsigned long prevTime = 0;
const uint8_t timeStep = 100;
#pragma endregion

#pragma region Pin-pin
#define SD_CS 5
#define NAV_LT 4
#define STB_LT 16
#define BCN_LT 17
#define STS_LT 2
#pragma endregion

#pragma region Fungsi-fungsi
void sensorReading() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  pitch = (atan2(a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180) / M_PI;
  roll = (atan2(a.acceleration.y, a.acceleration.z) * 180) / M_PI;
  yaw -= g.gyro.heading * 6;

  if(yaw > 359) {
    yaw -= 360;
  }
  if(yaw < 0) {
    yaw += 360;
  }
}

void writeFile(fs::FS &fs, const char *path, const char *message) {
  Serial.printf("Writing file: %s\n", path);
  File file = fs.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 appendFile(fs::FS &fs, const char *path, const char *message) {
  Serial.printf("Appending file: %s\n", path);
  File file = fs.open(path, FILE_APPEND);
  if(!file) {
    Serial.println("Failed to open file for appending");
    return;
  }
  if(file.print(message)) {
    Serial.println("File written");
  } else {
    Serial.println("Append failed");
  }
  file.close();
}

void logSDCard() {
  dataSD = String(pitch) + "\t\t" + String(roll) + "\t\t" + String(yaw) + "\t\t" + String(LEDStatus[2]) + String(LEDStatus[1]) + String(LEDStatus[0]) + "\r\n";
  Serial.print(dataSD);
  appendFile(SD, "/data.txt", dataSD.c_str());
}

void sendMessage() {
  dataWS = String(pitch, 0U) + semicolon + String(roll, 0U) + semicolon + String(yaw, 0U) + semicolon + String((LEDStatus[2] * 4) + (LEDStatus[1] * 2) + LEDStatus[0]);
  webSocket.broadcastTXT(dataWS);
}

void strobeLED(void *pvParameters) {
  while(1) {
    digitalWrite(STB_LT, HIGH);
    vTaskDelay(50 / portTICK_PERIOD_MS);
    digitalWrite(STB_LT, LOW);
    vTaskDelay(50 / portTICK_PERIOD_MS);
    digitalWrite(STB_LT, HIGH);
    vTaskDelay(50 / portTICK_PERIOD_MS);
    digitalWrite(STB_LT, LOW);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void beaconLED(void *pvParameters) {
  while(1) {
    digitalWrite(BCN_LT, HIGH);
    vTaskDelay(100 / portTICK_PERIOD_MS);
    digitalWrite(BCN_LT, LOW);
    vTaskDelay(1050 / portTICK_PERIOD_MS);
  }
}

void LEDProgram(void *pvParameters) {
  while(1) {
    if(LEDStatus[0] == 1 && lastLEDStatus[0] == 0) {
      digitalWrite(NAV_LT, HIGH);
      lastLEDStatus[0] = 1;
    } else if(LEDStatus[0] == 0 && lastLEDStatus[0] == 1) {
      digitalWrite(NAV_LT, LOW);
      lastLEDStatus[0] = 0;
    }

    if(LEDStatus[1] == 1 && lastLEDStatus[1] == 0) {
      xTaskCreatePinnedToCore(strobeLED, "TaskSTB", 1280, NULL, tskIDLE_PRIORITY, &TaskSTB, 0);
      lastLEDStatus[1] = 1;
    } else if(LEDStatus[1] == 0 && lastLEDStatus[1] == 1) {
      vTaskDelete(TaskSTB);
      lastLEDStatus[1] = 0;
    }

    if(LEDStatus[2] == 1 && lastLEDStatus[2] == 0) {
      xTaskCreatePinnedToCore(beaconLED, "TaskBCN", 1280, NULL, tskIDLE_PRIORITY, &TaskBCN, 0);
      lastLEDStatus[2] = 1;
    } else if(LEDStatus[2] == 0 && lastLEDStatus[2] == 1) {
      vTaskDelete(TaskBCN);
      lastLEDStatus[2] = 0;
    }
  }
}
#pragma endregion

#pragma region WebSocket Event
void onWebSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length) {
  switch(type) {
    case WStype_CONNECTED:
      Serial.println("Client Connected!");
      break;

    case WStype_DISCONNECTED:
      Serial.println("Client Disconnected!");
      break;

    case WStype_TEXT:
      pldStr = (char*)payload;
      if(pldStr.toInt() >= 0 && pldStr.toInt() < 8) {
        if(pldStr.toInt() & 1 << 0) {
          Serial.println("NAV Light On");
          LEDStatus[0] = 1;
        } else {
          Serial.println("NAV Light Off");
          LEDStatus[0] = 0;
        }

        if(pldStr.toInt() & 1 << 1) {
          Serial.println("Strobe Light On");
          LEDStatus[1] = 1;
        } else {
          Serial.println("Strobe Light Off");
          LEDStatus[1] = 0;
        }

        if(pldStr.toInt() & 1 << 2) {
          Serial.println("Beacon Light On");
          LEDStatus[2] = 1;
        } else {
          Serial.println("Beacon Light Off");
          LEDStatus[2] = 0;
        }
      } else {
        Serial.println("Wrong Command!");
      }

    default:
      break;
  }
}
#pragma endregion

#pragma region Program Utama
void setup() {
  Serial.begin(115200);
  pinMode(NAV_LT, OUTPUT);
  pinMode(STB_LT, OUTPUT);
  pinMode(BCN_LT, OUTPUT);
  pinMode(STS_LT, OUTPUT);
  digitalWrite(STS_LT, HIGH);

  Serial.println("Creating Tasks...");
  xTaskCreatePinnedToCore(LEDProgram, "TaskLED", 1280, NULL, tskIDLE_PRIORITY, &TaskLED, 0);
  delay(500);
  Serial.println("Tasks created successfully!");

  Serial.println("AP Starting...");
  WiFi.softAP(SSID, Password);
  IPAddress myIP = WiFi.softAPIP();
  Serial.print("AP Started! IP Address: ");
  Serial.println(myIP);

  webSocket.begin();
  webSocket.onEvent(onWebSocketEvent);

  if(!SD.begin(SD_CS)) {
    Serial.println("Card mount failed!");
    while(1) {
      digitalWrite(STS_LT, LOW);
      delay(200);
      digitalWrite(STS_LT, HIGH);
      delay(200);
    }
  }
  Serial.println("Card mount success!");

  uint8_t cardType = SD.cardType();
  if(cardType == CARD_NONE) {
    Serial.println("No SD card attached");
    while(1) {
      digitalWrite(STS_LT, LOW);
      delay(200);
      digitalWrite(STS_LT, HIGH);
      delay(50);
    }
  } else {
    Serial.println(cardType);
  }

  File file = SD.open("/data.txt");
  if(!file) {
    Serial.println("File doesn't exist! Creating file...");
    writeFile(SD, "/data.txt", "Pitch\t\tRoll\t\tYaw\t\tLED Status\r\n\r\n");
  } else {
    Serial.println("File already exist!");
  }
  file.close();

  Serial.println("Initializing MPU6050...");
  if(!mpu.begin()) {
    Serial.println("Failed to initialize MPU6050!");
    while(1) {
      digitalWrite(STS_LT, LOW);
      delay(50);
      digitalWrite(STS_LT, HIGH);
      delay(50);
    }
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_10_HZ);
  Serial.println("MPU6050 initialization success!");

  delay(1000);
  digitalWrite(STS_LT, LOW);
  Serial.printf("\nSystem Ready!\n");
  delay(1000);
}

void loop() {
  webSocket.loop();

  currTime = millis();
  if(currTime - prevTime >= timeStep) {
    sensorReading();
    logSDCard();
    sendMessage();
    prevTime = currTime;
  }
}
#pragma endregion