#include "AiEsp32RotaryEncoder.h"
#include "Arduino.h"
#include "OneButton.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>


#define SCREEN_WIDTH 128 // Lebar layar OLED, dalam piksel
#define SCREEN_HEIGHT 64 // Tinggi layar OLED, dalam piksel

#define OLED_RESET    -1 // Pin reset # (atau -1 jika menggunakan pin reset Arduino)

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int icon_dir = 1;

void printTwoDigit(int num) {
  if (num < 10) {
    display.print("0");
  } else if (num > 99) {
    num /= 10;
  }
  display.print(num);
}



#define LED_STATE_BLUE 2 // Indique si le bluetooth est connecté

// Pin Definitions
#if defined(ESP8266)
#define ROTARY_ENCODER_A_PIN_1 D6
#define ROTARY_ENCODER_B_PIN_1 D5
#define ROTARY_ENCODER_BUTTON_PIN_1 D7
#define ROTARY_ENCODER_A_PIN_2 D3
#define ROTARY_ENCODER_B_PIN_2 D4
#define ROTARY_ENCODER_BUTTON_PIN_2 D8
#else
#define ROTARY_ENCODER_A_PIN_1 6
#define ROTARY_ENCODER_B_PIN_1 7
#define ROTARY_ENCODER_BUTTON_PIN_1 25
#define ROTARY_ENCODER_A_PIN_2 18
#define ROTARY_ENCODER_B_PIN_2 19
#define ROTARY_ENCODER_BUTTON_PIN_2 26
#endif

#define PIN_INPUT1 2
#define PIN_INPUT2 3

#define ROTARY_ENCODER_VCC_PIN -1 // Set to -1 if VCC is directly connected to 3.3V

#define ROTARY_ENCODER_STEPS 4

// Setup a new OneButton on pin PIN_INPUT2.
OneButton button1(PIN_INPUT1, true);
// Setup a new OneButton on pin PIN_INPUT2.
OneButton button2(PIN_INPUT2, true);

bool stat_ENC = true;
bool stat_PLAY = false;
bool stat_DIR = true;
bool encRng = false;

int lastrot1pos;
int lastrot1enc;


///////////////////////////////////////////////////////////////////////////////////////

// Konstanta untuk mengubah derajat menjadi radian
#define DEG_TO_RAD(x) (x * 0.01745329252)  // π / 180

// Fungsi untuk meremap nilai dari satu rentang ke rentang lainnya
float remap(float value, float fromLow, float fromHigh, float toLow, float toHigh) {
  return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
}

// Fungsi untuk menghitung jarak lurus
float hitungJarakLurus(float radiusRoda, float sudutPutaran) {
  // Konversi sudut ke radian
  float sudutRadian = DEG_TO_RAD(sudutPutaran);

  // Menggunakan rumus jarak lurus
  float jarakLurus = 2 * radiusRoda * sin(sudutRadian / 2);

  return jarakLurus;
}

float radiusRoda = 130;
////////////////////////////////////////////////////////////////////////////////////////

// Rotary Encoder Objects
AiEsp32RotaryEncoder rotaryEncoder1 = AiEsp32RotaryEncoder(ROTARY_ENCODER_A_PIN_1, ROTARY_ENCODER_B_PIN_1, ROTARY_ENCODER_BUTTON_PIN_1, ROTARY_ENCODER_VCC_PIN, ROTARY_ENCODER_STEPS);
AiEsp32RotaryEncoder rotaryEncoder2 = AiEsp32RotaryEncoder(ROTARY_ENCODER_A_PIN_2, ROTARY_ENCODER_B_PIN_2, ROTARY_ENCODER_BUTTON_PIN_2, ROTARY_ENCODER_VCC_PIN, ROTARY_ENCODER_STEPS);


volatile int rotary1Pos = 0; // Nilai posisi Rotary Encoder 1
volatile int rotary2Pos = 0; // Nilai posisi Rotary Encoder 2

// Function to handle button click for the first rotary encoder
void rotary_onButtonClick1()
{
    // Your button click handling code for encoder 1
    // Example:
    // Serial.println("Button 1 clicked!");
}

// Function to handle button click for the second rotary encoder
void rotary_onButtonClick2()
{
    // Your button click handling code for encoder 2
    // Example:
    // Serial.println("Button 2 clicked!");
}

// Main loop for rotary encoder handling
void rotary_loop()
{
    // Handling encoder 1
    if (rotaryEncoder1.encoderChanged())
    {
        // Your code when encoder 1 value changes
        // Example:
         Serial.print("Encoder 1 value: ");
         Serial.println(rotaryEncoder1.readEncoder());     
          if (stat_ENC){
              float sudutPutaran = remap(rotaryEncoder1.readEncoder() * 8, 0, 4096, 0, 360);  
              // Hitung jarak lurus
              float jarak = hitungJarakLurus(radiusRoda, sudutPutaran);
              rotary1Pos = jarak;
              if (encRng){
                indikatorP();
                dir_status();
                paused_status();
              } else {
                indikatorR();
                dir_status();
                paused_status();
              }
              if (stat_PLAY){
              play_status();
              } else {
              paused_status();}
            }
          else {
              float sudutPutaran = remap(rotaryEncoder1.readEncoder() * 15, 0, 4096, 0, 360);  
              // Hitung jarak lurus
              float jarak = hitungJarakLurus(radiusRoda, sudutPutaran);
              rotary1Pos = jarak;
              position();
              dir_statPos();
            }
          
    }

    if (rotaryEncoder1.isEncoderButtonClicked())
    {
        rotary_onButtonClick1();
    }

    // Handling encoder 2
    if (rotaryEncoder2.encoderChanged())
    {
        // Your code when encoder 2 value changes
        // Example:
         Serial.print("Encoder 2 value: ");
         Serial.println(rotaryEncoder2.readEncoder());
         rotary2Pos = map(rotaryEncoder2.readEncoder(), 0, 51, 0, 100);
         if (stat_ENC){
            if (encRng){
                indikatorP();
                dir_status();
                paused_status();
              } else {
                indikatorR();
                dir_status();
                paused_status();
              }
                if (stat_PLAY){
                play_status();
              } else {
                paused_status();}
            if (stat_PLAY){
              play_status();
            } else {
              paused_status();}
          }
         
    }

    if (rotaryEncoder2.isEncoderButtonClicked())
    {
        rotary_onButtonClick2();
    }
}

// Interrupt Service Routine for reading encoder values
void IRAM_ATTR readEncoderISR()
{
    rotaryEncoder1.readEncoder_ISR();
    rotaryEncoder2.readEncoder_ISR();
}

uint8_t mac_address[] = {0x00,0x21,0x09,0x01,0x31,0x06};

///////////////////////////////////////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////////////////////////////////////


unsigned long loadingStartTime;
bool loadingComplete = false;
bool clientConnected = false;

int led_mrh = 13;
int led_bru = 12;

// Waktu untuk kedipan LED
const unsigned long intervalMerah = 100; // 200ms
const unsigned long intervalBiru = 1000; // 1000ms

// Variabel untuk menyimpan waktu terakhir
unsigned long previousMillisMerah = 0;
unsigned long previousMillisBiru = 0;

// Status LED
boolean ledMerahStatus = false;
boolean ledBiruStatus = false;

// Setup function
void setup()
{
    Serial.begin(115200);
    //SerialBT.setTimeout(8000);
    
    // Configuration des pins 
    pinMode(LED_STATE_BLUE, OUTPUT);

    // Initializing rotary encoders
    rotaryEncoder1.begin();
    rotaryEncoder2.begin();

    // Setting up ISR for encoder readings
    rotaryEncoder1.setup(readEncoderISR);
    rotaryEncoder2.setup(readEncoderISR);

    // Setting boundaries, acceleration, etc.
    bool circleValues = false;
    rotaryEncoder1.setBoundaries(0, 1000, circleValues);
    rotaryEncoder2.setBoundaries(0, 51, circleValues);

    rotaryEncoder1.disableAcceleration();
    //rotaryEncoder2.disableAcceleration();
      // link the button 1 functions.
  button1.attachClick(click1);
  button1.attachDoubleClick(doubleclick1);
  button1.attachLongPressStart(longPressStart1);
  button1.attachLongPressStop(longPressStop1);
  button1.attachDuringLongPress(longPress1);

  // link the button 2 functions.
  button2.attachClick(click2);
  button2.attachDoubleClick(doubleclick2);
  button2.attachLongPressStart(longPressStart2);
  button2.attachLongPressStop(longPressStop2);
  button2.attachDuringLongPress(longPress2);
   

    // Inisialisasi layar OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Alamat 0x3C untuk 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  
  digitalWrite(LED_STATE_BLUE, HIGH);
  loadingStartTime = millis(); // Catat waktu mulai animasi loading
  indikatorR();
  dir_status();
  paused_status();
  Serial.println("Starting Compelete..");
  

}

// Main loop
void loop()
{

    button1.tick();
    button2.tick();
    rotary_loop();
    delay(50);
}



void processInput(String input) {
  // Pisahkan input berdasarkan tanda pagar #
  int separatorIndex = input.indexOf('#');
  if (separatorIndex != -1) { // Pastikan ada tanda pagar dalam input
    String key = input.substring(0, separatorIndex);
    String valueStr = input.substring(separatorIndex + 1);
    
    // Konversi nilai menjadi integer
    int value = valueStr.toInt();
    
    // Lakukan sesuatu berdasarkan kunci yang diterima
    if (key == "e" && value >= -1000 && value <= 1000) {
      // Lakukan sesuatu dengan nilai enc1
        //Serial.println(as5600.getCumulativePosition());
    } else if (key == "sc1") {
      // Lakukan sesuatu dengan nilai esc1

    } else if (key == "dc1") {
      // Lakukan sesuatu dengan nilai edc1

    } else if (key == "elp1") {
      // Lakukan sesuatu dengan nilai elp1
    } else if (key == "n") {
      // Lakukan sesuatu dengan nilai enc2
    } else if (key == "sc2") {
      // Lakukan sesuatu dengan nilai esc2
        Serial.println("Button 2 click.");
        if (value == 1){
            Serial.println("start from BT");
            stat_PLAY = true;
            indikatorR();
            dir_status();
            play_status();
          }
          else {
             Serial.println("stop from BT");
             stat_PLAY = false;
             indikatorR();
             dir_status();
             paused_status();
          }
    } else if (key == "dc2") {
      // Lakukan sesuatu dengan nilai edc2
    } else if (key == "elp2") {
      // Lakukan sesuatu dengan nilai elp2
    } else if (key == "arah") {
      // Lakukan sesuatu dengan nilai elp2
      icon_dir = value;
      position();
      dir_statPos();
      
    }else {
      // Kunci tidak dikenali
      Serial.println("Kunci tidak dikenali.");
    } 
  }
  else {
    Serial.println("Format input tidak valid."); // Cetak pesan jika format input tidak sesuai
    }
}



// ----- button 1 callback functions

// This function will be called when the button1 was pressed 1 time (and no 2. button press followed).
void click1() {
  //int acc = 100;
  Serial.println("Button 1 click.");
  //rotaryEncoder1.setAcceleration(acc);
  

  if (stat_ENC) {
    if (encRng){
      rotaryEncoder1.setEncoderValue(lastrot1enc);
      rotaryEncoder1.setBoundaries(0, 1000, false);
      rotary1Pos = lastrot1pos;
      indikatorR();
      dir_status();
      paused_status();
      encRng = false;
    } else {
      lastrot1pos = rotary1Pos;
      lastrot1enc = rotaryEncoder1.readEncoder();
      rotaryEncoder1.setEncoderValue(0);
      rotaryEncoder1.setBoundaries(-1000, 1000, false);
      rotary1Pos = 0;
      indikatorP();
      dir_status();
      paused_status();
      encRng = true;
    }
  } else {
      encRng = false;
      rotaryEncoder1.setEncoderValue(20);
      rotaryEncoder1.setBoundaries(0, 1000, false);
      rotary1Pos=31;
      indikatorR();
      dir_status();
      paused_status();
  }
  
  stat_ENC = true;
}  // click1


// This function will be called when the button1 was pressed 2 times in a short timeframe.
void doubleclick1() {
  Serial.println("Button 1 doubleclick.");
  rotaryEncoder1.setEncoderValue(0);
  rotaryEncoder1.setBoundaries(-1000, 1000, false);
 // rotaryEncoder1.disableAcceleration();
  stat_ENC = false;
  //encRng = false;
  rotary1Pos=0;
  position();
  dir_statPos();

}  // doubleclick1


// This function will be called once, when the button1 is pressed for a long time.
void longPressStart1() {
  Serial.println("Button 1 longPress start");

}  // longPressStart1


// This function will be called often, while the button1 is pressed for a long time.
void longPress1() {
  Serial.println("Button 1 longPress...");
}  // longPress1


// This function will be called once, when the button1 is released after beeing pressed for a long time.
void longPressStop1() {
  Serial.println("Button 1 longPress stop");
}  // longPressStop1


// ... and the same for button 2:

void click2() {
  Serial.println("Button 2 click.");
      if (stat_ENC){
      Serial.println("play/stop");
	  } else {
      stat_DIR=!stat_DIR;
	  Serial.println("change direction..");
    }
  }

  // click2


void doubleclick2() {
  Serial.println("Button 2 doubleclick.");
  // if (!SerialBT.hasClient()) {
  //     SerialBT.connect(mac_address);
  //     loadingComplete = false;
  //     loadingStartTime = millis();
  // } else {
  //     SerialBT.print("dc2#");
  //     Serial.println("sudah tersambung");
  // }
  

}




void longPressStart2() {
  Serial.println("Button 2 longPress start");
}  // longPressStart2


void longPress2() {
  Serial.println("Button 2 longPress...");
}  // longPress2

void longPressStop2() {
  Serial.println("Button 2 longPress stop");
}  // longPressStop2



void idle(){
  display.clearDisplay();
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(18, 7);
  display.println("Not Connected!!");
  display.setCursor(48, 20);
  display.println("Click");
  display.setCursor(20,32);
  display.println("Encoder-Kanan");
  display.setCursor(12, 44);
  display.println("to Reconnecting..");
  display.display();
}

void dir_statPos(){
  if (icon_dir == 1) {
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(107, 5);
  display.println(">>");
  display.display();
  } else {
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(107, 5);
  display.println("<<");
  display.display();
  }
}

void dir_status(){
  if (icon_dir == 1) {
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(55, 3);
  display.println(">>");
  display.display();
  } else {
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(55, 3);
  display.println("<<");
  display.display();
  }
}

void paused_status(){
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(37, 54);
  display.println("Paused!!");
  display.display();
}

void play_status(){
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(37, 54);
  display.println("Playing..");
  display.display();
}

void position(){


    display.clearDisplay();
    display.setTextSize(2); // Ukuran font untuk nama
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(30, 0);
    display.println("POSISI");
    display.setCursor(45, 27);
    display.setTextSize(4); // Ukuran font untuk angka
       // Menampilkan nilai rotary1Pos dengan menangani kasus nilai negatif
    if (rotary1Pos < 0) {
      display.setCursor(21, 27);
      display.print("-");
      printTwoDigit(abs(rotary1Pos)); // Menggunakan fungsi abs() untuk mendapatkan nilai absolut
    } else if (rotary1Pos > 0){
      display.setCursor(21, 27);
      display.print("+");
      printTwoDigit(rotary1Pos);
    }
      else{
      printTwoDigit(rotary1Pos);
    }

    if (rotary1Pos < 100 && rotary1Pos > -100) {
      display.setTextSize(2);
      display.println("mm");
    } else if (rotary1Pos < -99 || rotary1Pos > 99){
      display.setTextSize(2);
      int lastDigit = rotary1Pos % 10;
      lastDigit = abs(lastDigit);
      //display.setCursor(43, 28);
      display.setTextSize(2);
      display.print(".");
      display.println(lastDigit);
      display.setCursor(95, 41);
      display.setTextSize(2);
      display.println("cm");
    }
    display.display();
  }

void indikatorR(){

    display.clearDisplay();
    display.setTextSize(1); // Ukuran font untuk nama
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(12, 7);
    display.println("RANGE");
    display.setCursor(8, 24);
    display.setTextSize(3); // Ukuran font untuk angka
    printTwoDigit(rotary1Pos);

    if (rotary1Pos < 100) {
      display.setTextSize(1);
      display.println("mm");
    } else if (rotary1Pos > 99){
      int lastDigit = rotary1Pos % 10;
      display.setCursor(43, 28);
      display.setTextSize(1);
      display.print(".");
      display.println(lastDigit);
      display.setCursor(45, 38);
      display.setTextSize(1);
      display.println("cm");
    }

    display.setCursor(80, 7);
    display.setTextSize(1); // Kembali ke ukuran font untuk nama
    display.println("SPEED");
    display.setCursor(74, 24);
    display.setTextSize(3); // Ukuran font untuk angka
    if (rotary2Pos < 10) {
      display.print("0");
    } else if (rotary2Pos > 99){
        display.setTextSize(2);
        display.setCursor(74, 30);
    }
    display.print(rotary2Pos);
    display.setTextSize(2);
    display.setCursor(110, 30);
    display.println("%");
    display.display();
  }


void indikatorP(){

    display.clearDisplay();
    display.setTextSize(1); // Ukuran font untuk nama
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(12, 7);
    display.println("POSISI");
    display.setCursor(8, 24);
    display.setTextSize(3); // Ukuran font untuk angka
    printTwoDigit(abs(rotary1Pos));

    if (rotary1Pos < 100 && rotary1Pos > -100){
      display.setTextSize(1);
      display.println("mm");}
    if (rotary1Pos < 0) {
        display.setTextSize(1);
        display.setCursor(0, 32);
        display.print("-");}
    if (rotary1Pos < -99){
            int lastDigit = rotary1Pos % 10;
            display.setCursor(43, 28);
            display.setTextSize(1);
            display.print(".");
            display.print(abs(lastDigit));
            
            display.setCursor(45, 38);
            display.setTextSize(1);
            display.println("cm");
            }
     else if (rotary1Pos > 99){
      int lastDigit = rotary1Pos % 10;
      display.setCursor(43, 28);
      display.setTextSize(1);
      display.print(".");
      display.println(lastDigit);
      display.setCursor(45, 38);
      display.setTextSize(1);
      display.println("cm");
    }

    display.setCursor(80, 7);
    display.setTextSize(1); // Kembali ke ukuran font untuk nama
    display.println("SPEED");
    display.setCursor(74, 24);
    display.setTextSize(3); // Ukuran font untuk angka
    if (rotary2Pos < 10) {
      display.print("0");
    } else if (rotary2Pos > 99){
        display.setTextSize(2);
        display.setCursor(74, 30);
    }
    display.print(rotary2Pos);
    display.setTextSize(2);
    display.setCursor(110, 30);
    display.println("%");
    display.display();
  }