#include <ESP32Servo.h>  // Library untuk kontrol ESC menggunakan PWM

const int hallSensorPin1 = 0;
const int hallSensorPin2 = 15;
const int hallSensorPin3 = 34;
const int escPin = 19; // Pin untuk ESC

float rpm;
float rps;
volatile unsigned long counter = 0;
float kecepatan;
unsigned long lalu = millis();
unsigned long sekarang;

Servo esc; // Objek Servo untuk ESC
int escSignal = 1504.9; // Nilai default untuk ESC

// Fungsi untuk menghitung lebar pulsa dari kecepatan (km/jam) menggunakan regresi polinomial
float regresiPolinomial(float x) {
  return  0.2383 * (x * x * x) - 5.2815 * (x * x) + 39.703 * x + 1506.7;
}

void IRAM_ATTR hallInterrupt() { counter++; }
void IRAM_ATTR hallInterrupt2() { counter++; }
void IRAM_ATTR hallInterrupt3() { counter++; }

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

  // Konfigurasi pin sensor sebagai input1
  pinMode(hallSensorPin1, INPUT_PULLUP);
  pinMode(hallSensorPin2, INPUT_PULLUP);
  pinMode(hallSensorPin3, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(hallSensorPin1), hallInterrupt, RISING);
  attachInterrupt(digitalPinToInterrupt(hallSensorPin2), hallInterrupt2, RISING);
  attachInterrupt(digitalPinToInterrupt(hallSensorPin3), hallInterrupt3, RISING);

  // Konfigurasi ESC menggunakan PWM
  esc.attach(escPin, 1000, 2000); // ESC menggunakan sinyal PWM 1ms-2ms (1000-2000µs)
  Serial.println("Masukkan nilai kecepatan (0-20 KM/Jam) via Serial Monitor:");
}

void loop() {
  // Membaca input dari Serial Monitor
  if (Serial.available() > 0) {
    int inputValue = Serial.parseInt();
    Serial.flush();  // Membersihkan buffer input Serial
    
    // Validasi input agar tetap dalam rentang 0 - 20 KM/Jam
    if (inputValue >= 0 && inputValue <= 20) {
      //escSignal = (float)regresiPolinomial(inputValue); // Ubah km/jam ke PWM
      Serial.print("ESC Signal diatur ke: ");
      Serial.println(escSignal);
    } else {
      Serial.println("Masukkan nilai 0 - 20 KM/Jam.");
    }
  }
  // Mengatur ESC berdasarkan nilai input
  //esc.writeMicroseconds(escSignal);
  sekarang = millis();
  if (sekarang - lalu >= 1000) {
    rps = (float)counter / 21;
    rpm = (float)counter * 60 / 21;
   
    kecepatan = (float)counter / 21 * 3600 / 100000 * PI * 5 / 3;

    float Error = inputValue - kecepatan;
    iError += Error;                      // Intergral Error, deklarasi di Luar
    dError = Error - ErrorLalu;           // Differensial Error, deklarasi di Luar
    ErrorLalu = Error;

    float PID = KP * Error + KI * iError * KD * dError;
    if(PID<0) PID=0;                      // tidak boleh kurang dari 0 km/jam
    if(PID>20) PID=20;                    // tidak boleh lebih dari 20km/jam

    escSignal = (float)regresiPolinomial(PID);  // Koversi ke Lebar Pulsa
    // Mengatur ESC berdasarkan nilai input
    esc.writeMicroseconds(escSignal);

    // Menampilkan hasil ke Serial Monitor
    Serial.print("Kecepatan : ");
    Serial.print(kecepatan);
    Serial.print(" km/jam | RPM : ");
    Serial.print(rpm, 1);
    Serial.print(" | RPS : ");
    Serial.print(rps, 1);
    Serial.print(" | ESC Signal: ");
    Serial.println(escSignal);

    counter = 0;
    lalu = sekarang;
  }

  delay(1); // Stabilisasi loop
}