/*
  Program     : Kontrol KI & Safety System
  Dibuat Oleh : Abimanyu & Syahbella
  Tanggal     : 07 / 06 / 2025
  Keterangan  : Suksess sampai tujuan
*/

// Deklarasi semua Variable Bersama
#include <ESP32Servo.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include <nRF24L01.h>
#include <RF24.h>

// Tulis semua Variable bersama di sini
// NRF24L01 Setup
RF24 radio(2, 4);
const byte address[6] = "00001";
struct PayloadRX {
  int heartbeat;
  int joystickX;
  int buttonState;
};
PayloadRX dataRX;
unsigned long lastHeartbeatTime = 0;

// Converting Joystick
const int SMOOTHING_WINDOW = 10;
int throttleSamples[SMOOTHING_WINDOW];
int throttleIndex = 0;
int throttleSum = 0;
bool smoothingInitialized = false;

int joyConvert(int rawValue) {
  int joyNetral = 2755;
  int deadzone = 100;
  int joyMax = 4095;
  int joyMin = 1000;
  int rawConverted;

  if (rawValue > (joyNetral + deadzone)) {
    float mapped = (float)(rawValue - (joyNetral + deadzone)) / (joyMax - (joyNetral + deadzone)) * 15.0;
    rawConverted = constrain((int)mapped, 0, 15);
  } else if (rawValue < (joyNetral - deadzone)) {
    // Mundur 
    float mapped = (float)((joyNetral - deadzone) - rawValue) / ((joyNetral - deadzone) - joyMin) * -2.0;
    rawConverted = constrain((int)mapped, -2, 0);
  } else {
    rawConverted = 0;
  }
if (!smoothingInitialized) {
    for (int i = 0; i < SMOOTHING_WINDOW; i++) {
      throttleSamples[i] = rawConverted;
    }
    throttleSum = rawConverted * SMOOTHING_WINDOW;
    smoothingInitialized = true;
  }
  throttleSum -= throttleSamples[throttleIndex];
  throttleSamples[throttleIndex] = rawConverted;
  throttleSum += rawConverted;
  throttleIndex = (throttleIndex + 1) % SMOOTHING_WINDOW;
  
  return throttleSum / SMOOTHING_WINDOW;
}
  int throttleValue = 0;
  bool throttleActive = false;
  int buttonState = 0;

  //FSR
  bool loadDetected = false;
  bool isPressed = false;
  unsigned long fallStart = 0;
  const unsigned long releaseDelay = 1000;
  const int fsrPin = 13;
  const int numReadings = 25;
  int readings[numReadings];
  int readIndex =0;
  int total = 0;
  int fsrData = 0;
  float vout;

  //Battery
  const int battPin = 35;
  float persenBaterai = 100.0;
  const float voltageDividerGain = (21.5 / 1.5);
  const float realValueGain = 1.117;
  const float voltageMin = 18.7;
  const float voltageMax = 25.2;
  const int numBaca = 25;
  int membaca[numBaca];
  int bacaIndex =0;
  int jumlah = 0;
  int battData = 0;

  //Hall
  const int hallPin1 = 25;
  const int hallPin2 = 26;
  const int hallPin3 = 27;

  // ESC
  const int escPin1 = 17;
  const int escPin2 = 16;
  Servo esc1;
  Servo esc2;

  //Buzzer
  const int buzzPin = 14;
  bool lastRemoteConnected = false;

  //OLED
  #include <Wire.h>
  #include <Adafruit_GFX.h>
  #include <Adafruit_SH110X.h>
  #define SCREEN_WIDTH 128
  #define SCREEN_HEIGHT 64
  Adafruit_SH1106G display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
  int direction = 1;

  //Kontrol KI / Kecepatan
  float inputValue;  // target kecepatan
  float kecepatan;
  float KI = 0;
  float error;
  float integral = 0;
  volatile unsigned long counter = 0;
  unsigned long lalu = millis();

  // ISR Hall Effect (Global interrupt)
  void IRAM_ATTR hallInterrupt1() {
    counter++;
  }
  void IRAM_ATTR hallInterrupt2() {
    counter++;
  }
  void IRAM_ATTR hallInterrupt3() {
    counter++;
  }

// BLE UUID
#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

// BLE Global Object
BLECharacteristic* pCharacteristic = nullptr;
BLEAdvertising* pAdvertising = nullptr;
BLEServer* pServer = nullptr;
bool restartBLE = false;

// BLE Callback
class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) {
    Serial.println("Client connected");
  }
  void onDisconnect(BLEServer* pServer) {
    Serial.println("Client disconnected");
    restartBLE = true;
  }
};

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// PWM Regresi convertor
float konversiPWM(float x) {
  return 0.3805 * pow(x, 3) - 6.0601 * pow(x, 2) + 38.528 * x + 1530.7;
}

// SAFETY MODES
#define MODE_HOLD 0       // HOLD Mode
#define MODE_WALK 1       // WALK Mode
#define MODE_RIDE 2       // RIDE Mode
#define MODE_LOW_POWER 3  // LOW POWER Mode
#define MODE_STOP 4       // STOP Mode
#define MODE_LOCK 5       // LOCK Mode

//Safety
bool remoteConnected = false;
int currentMode = MODE_LOCK;
int lastButton;
float thresholdRide = 0.5;
float thresholdWalkToRide = 3.0;
unsigned long throttleStartWalkTime = 0;
bool throttleTimerWalkRunning = false;
unsigned long throttleStartTime = 0;
bool throttleTimerRunning = false;
unsigned long throttleLowPowerStart = 0;
bool throttleLowPowerTimerRunning = false;
unsigned long holdStartTime = 0;
bool holdStartRunning = false;
bool buzzerWalkPlayed = false;
int rideStatus = 0;

void SetupSafety();
void Safety();
void TestSafety();

void SetupControl();
void Control();
void TestControl();

void setup() {
  Serial.begin(115200);
  SetupSafety();   // Jalankan Inisialisasi bagian Safety
  SetupControl();  // Jalankan Inisialisasi bagian Control
}

void loop() {
  Safety();
  // Jalankan semua proses yang berkaitan dengan Safety
  Control();  // Jalankan semua proses yang berkaitan dengan Control
    Serial.print("User = ");
    Serial.print(vout);
    Serial.print(" | Button = ");
    Serial.print(buttonState);
    Serial.print(" | Mode = ");
    Serial.print(currentMode);
    Serial.print(" | BATT = ");
    Serial.print(persenBaterai);
    Serial.print("%");  
    Serial.print(" | Joystick = ");
    Serial.print(throttleValue);
    Serial.print(" | Ride Status = ");
    Serial.println(rideStatus);

    display.clearDisplay();

  // Ukuran dan posisi baterai
  int x = 34;
  int y = 10;
  int w = 60;
  int h = 30;
  int headWidth = 4;

  // Gambar outline baterai
  display.drawRect(x, y, w, h, SH110X_WHITE);
  display.fillRect(x + w, y + h / 3, headWidth, h / 3, SH110X_WHITE);

  // Hitung lebar isi baterai berdasarkan persentase
  int fillWidth = map(persenBaterai, 0, 100, 0, w - 2);

  // Geser isi baterai secara horizontal (animasi)
  display.fillRect(x + 1, y + 1, fillWidth, h - 2, SH110X_WHITE);

  // Tampilkan nilai persen di bawah baterai
  display.setTextSize(2);
  display.setCursor((SCREEN_WIDTH - 30) / 2, y + h + 4);
  display.printf("%2.0f%", persenBaterai);

  display.display();
  delay(50);

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  // Animasi: naik turun level baterai
  persenBaterai += direction;
  if (persenBaterai >= 100 || persenBaterai <= 0) {
    direction *= -1;
  }

  delay(1);
}

// =========== Bagian Safety ===================

void SetupSafety() {
  // NRF Setup
  radio.begin();
  radio.setPALevel(RF24_PA_LOW);
  radio.openReadingPipe(1, address);
  radio.startListening();

  display.begin(0x3C, true); // alamat default 0x3C
  display.clearDisplay();
  display.setTextColor(SH110X_WHITE);

  pinMode(buzzPin, OUTPUT);
  pinMode(hallPin1, INPUT_PULLUP);
  pinMode(hallPin2, INPUT_PULLUP);
  pinMode(hallPin3, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(hallPin1), hallInterrupt1, RISING);
  attachInterrupt(digitalPinToInterrupt(hallPin2), hallInterrupt2, RISING);
  attachInterrupt(digitalPinToInterrupt(hallPin3), hallInterrupt3, RISING);

  // FSR Setup
  pinMode(fsrPin, INPUT);
  for (int i = 0; i < numReadings; i++){
    readings[i] = 0;
  }

  // Baterai Setup
  pinMode (battPin, INPUT);
    for (int k = 0; k < numBaca; k++){
    membaca[k] = 0;
  }
}

void Safety() {
  // NRF
  if (radio.available()) {
    radio.read(&dataRX, sizeof(dataRX));
    lastHeartbeatTime = millis();
  }
    remoteConnected = (millis() - lastHeartbeatTime <= 100);
    // Remot Terkoneksi
    if (remoteConnected != lastRemoteConnected) {
      if (remoteConnected) {
      } else {
      }
      lastRemoteConnected = remoteConnected;
    }
  
  // Perbarui nilai throttle dan button dari dataRX
  throttleValue = joyConvert(dataRX.joystickX);

  throttleActive = (throttleValue != 0);
  buttonState = (dataRX.buttonState);

  // FSR
  total -= readings[readIndex];
  readings[readIndex] = analogRead(fsrPin);
  total += readings[readIndex];
  readIndex = (readIndex + 1)% numReadings;
  fsrData = total / numReadings;
  vout = (fsrData * 3.3) / 4095;
  bool fsrTriggered = false;  // Flag buzzer nyala saat loadDetected
  bool fsrReleased = false;   // Flag buzzer nyala saat load dilepas

  if (vout >= 2.7) {
  if (!isPressed) {
    isPressed = true;
    fallStart = 0;

    if (!fsrTriggered) {
      // Buzzer aktif saat beban pertama kali terdeteksi
      digitalWrite(buzzPin, HIGH);
      delay(150);
      digitalWrite(buzzPin, LOW);
      delay(50);
      digitalWrite(buzzPin, HIGH);
      delay(150);
      digitalWrite(buzzPin, LOW);
      fsrTriggered = true;
      fsrReleased = false; // Reset flag release
    }
  }
  } else {
  if (isPressed && fallStart == 0) {
    fallStart = millis();  // Mulai timer delay pelepasan
  }

  if (isPressed && (millis() - fallStart >= releaseDelay)) {
    isPressed = false;
    fallStart = 0;

    if (!fsrReleased) {
      // Buzzer aktif saat beban dilepas
      digitalWrite(buzzPin, HIGH);
      delay(750);
      digitalWrite(buzzPin, LOW);
      fsrReleased = true;
      fsrTriggered = false; // Reset flag press
    }
  }
}

loadDetected = isPressed;

  // Battery
  jumlah -= membaca[bacaIndex];
  membaca[bacaIndex] = analogRead(battPin);
  jumlah += membaca[bacaIndex];
  bacaIndex = (bacaIndex + 1) % numBaca;
  battData = jumlah / numBaca;
  float voutDivider = (battData * 3.3) / 4095.0;
  float voutOffset = voutDivider * voltageDividerGain;
  float realValue = voutOffset * realValueGain;
  persenBaterai = (realValue - voltageMin) / (voltageMax - voltageMin) * 100.0;
  persenBaterai = constrain(persenBaterai, 0.0, 100.0);

  // SAFETY LOGIC
  if (!remoteConnected || !loadDetected) {
    currentMode = MODE_STOP;
    rideStatus = 0;
    throttleValue = 0;
    throttleTimerRunning = false;
    holdStartRunning = false;
    lastButton = buttonState;
    return;
  }
  
  if (currentMode == MODE_HOLD){
    if(throttleValue==0){
      if (!throttleTimerRunning){
      throttleStartTime = millis();
      throttleTimerRunning = true;
      holdStartRunning = false;
      buzzerWalkPlayed = false;
      }
    } else if (throttleValue >= 1) {
      currentMode = (persenBaterai <= 20.0) ? MODE_LOW_POWER : MODE_RIDE;
      throttleTimerRunning = false;
      holdStartRunning = false;
    }

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

if (millis() - throttleStartTime >= 5000) {
      currentMode = MODE_WALK;
      throttleTimerRunning = false;
      holdStartRunning = false;
    } 
      unsigned long elapsed = millis() - throttleStartTime;
      
      if(!buzzerWalkPlayed && elapsed >= 2000 && elapsed < 3000) {
        for (int i = 0; i < 3; i++) {
          digitalWrite(buzzPin, HIGH); delay(500);
          digitalWrite(buzzPin, LOW); delay(500);
        }
        buzzerWalkPlayed = true;
      }
    }
    
  if (currentMode==MODE_RIDE || currentMode==MODE_LOW_POWER || currentMode==MODE_WALK){
    rideStatus = 1;
  }

  if (currentMode==MODE_STOP && buttonState==1 && lastButton==0 && rideStatus==1){
      currentMode = MODE_LOCK;
      rideStatus = 0; 
  }

  if (throttleValue<0 && buttonState==1 && lastButton==0 && rideStatus==0){
    currentMode != MODE_LOCK;
    rideStatus = 0;
    }

  if (currentMode==MODE_HOLD && buttonState==1 && lastButton==0){
    currentMode != MODE_LOCK;
  }

  if (throttleValue < 0 && buttonState == 0) {
    currentMode = MODE_STOP;
    throttleTimerRunning = false;
    holdStartRunning = false;
  }

  if (currentMode==MODE_STOP && throttleValue==0){
    currentMode = MODE_HOLD;
  }

  if (currentMode == MODE_LOCK && buttonState == 0) { 
    currentMode = MODE_HOLD;
  }

  if (currentMode == MODE_WALK && throttleValue >= 3) {
    currentMode = (persenBaterai <= 20.0) ? MODE_LOW_POWER : MODE_RIDE;
    throttleTimerRunning = false;
    holdStartRunning = false;
  }

  if((currentMode==MODE_LOW_POWER || currentMode==MODE_RIDE ) && throttleValue==0){
    if(!holdStartRunning){
      holdStartTime = millis();
      holdStartRunning = true;
      buzzerWalkPlayed = false;
    }
    if (holdStartRunning && (millis() - holdStartTime >= 3000)) {
      currentMode = MODE_HOLD;
    }
  }else{
    holdStartRunning = false;
  }
  lastButton = buttonState;
}

//=========== Bagian Control ===============

void SetupControl() {
  // ESC Setup
  esc1.attach(escPin1, 1100, 1940);
  esc2.attach(escPin2, 1100, 1940);
  esc1.writeMicroseconds(1530.7);
  esc2.writeMicroseconds(1530.7);

  // BLE Setup
  BLEDevice::init("ESP32_Kecepatan");
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService* pService = pServer->createService(SERVICE_UUID);

  pCharacteristic = pService->createCharacteristic(
    CHARACTERISTIC_UUID,
    BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_READ);

  pCharacteristic->addDescriptor(new BLE2902());
  pService->start();

  pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->start();

  Serial.println("BLE advertising started");
  delay(1);
}

void Control() {
  // ===== Restart BLE jika disconnect =====
  if (restartBLE) {
    BLEDevice::deinit();
    delay(10);
    SetupControl();
    restartBLE = false;
  }
  // Variabel untuk incremental speed
  static unsigned long lastIncrementTime = 0;
  static int walkSpeedStep = 0;

  switch (currentMode) {
    case MODE_HOLD:
      inputValue = 0;
      //walkSpeedStep = 0;
      break;
    case MODE_WALK:
    inputValue = 3;
    throttleValue = 3;
      if (walkSpeedStep == 0) {
        inputValue = 1;
        walkSpeedStep = 1;
        lastIncrementTime = millis();
      }
      else if (walkSpeedStep < 3 && (millis() - lastIncrementTime >= 2000)) {
        walkSpeedStep++;
        inputValue = walkSpeedStep;
        lastIncrementTime = millis();
      }
      break;
    case MODE_RIDE:
      inputValue = int(throttleValue);
      walkSpeedStep = 0;
      break;
    case MODE_LOW_POWER:
      inputValue = int(throttleValue);
      if (throttleValue > 8) inputValue = 8;
      walkSpeedStep = 0;
      break;
    case MODE_STOP:
      inputValue = 0;
      walkSpeedStep = 0;
      break;
    case MODE_LOCK:
      inputValue = -15;
      walkSpeedStep = 0;
      break;
  }

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    if (input.startsWith("KI")) {
      KI = atof(input.substring(3).c_str());
      Serial.print("KI diubah ke: ");
      Serial.println(KI, 4);
    }
  }

  // Rumus putaran Hall effect
  unsigned long sekarang = millis();
  if (sekarang - lalu >= 250) {
    float rps = (float)counter / 21 * 4;
    kecepatan = rps * 3600 / 100000 * PI * 5 / 4;

    // Error dan integral
    error = inputValue - kecepatan;
    if (error < 1 && error > -1) error = 0;
    integral += error;
    float koreksi = KI * integral;

    // Konversi koreksi ke PWM
    float koreksiPWM = konversiPWM(inputValue + koreksi) - konversiPWM(inputValue);
    float openLoopPWM = konversiPWM(inputValue);
    float totalPWM = openLoopPWM + koreksiPWM;
    totalPWM = constrain(totalPWM, 1100, 1940);

    // Kirim ke ESC
    esc1.writeMicroseconds(totalPWM);
    esc2.writeMicroseconds(totalPWM);

    // Di dalam fungsi Control(), ganti bagian pengiriman BLE dengan:
    if (pCharacteristic) {
        char buffer[32];  // Buffer yang lebih besar untuk menampung kedua data
        snprintf(buffer, sizeof(buffer), "%.2f,%d", kecepatan, currentMode);
        pCharacteristic->setValue(buffer);
        pCharacteristic->notify();

    // Untuk debugging
        Serial.print("BLE Sent: ");
        Serial.println(buffer);
    }

    counter = 0;
    lalu = sekarang;
  }
}