#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.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);

#define ROTARY1_A_PIN 23 // Pin A dari Rotary Encoder 1
#define ROTARY1_B_PIN 35 // Pin B dari Rotary Encoder 1

#define ROTARY2_A_PIN 18 // Pin A dari Rotary Encoder 2
#define ROTARY2_B_PIN 19 // Pin B dari Rotary Encoder 2

int stat_dir;


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

void IRAM_ATTR rotary1Encoder() {
  if (digitalRead(ROTARY1_A_PIN) == digitalRead(ROTARY1_B_PIN)) {
    rotary1Pos++;
  } else {
    rotary1Pos--;
  }
}

void IRAM_ATTR rotary2Encoder() {
  if (digitalRead(ROTARY2_A_PIN) == digitalRead(ROTARY2_B_PIN)) {
    rotary2Pos++;
  } else {
    rotary2Pos--;
  }
}

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



void setup() {
  pinMode(ROTARY1_A_PIN, INPUT_PULLUP);
  pinMode(ROTARY1_B_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ROTARY1_A_PIN), rotary1Encoder, CHANGE);
  
  pinMode(ROTARY2_A_PIN, INPUT_PULLUP);
  pinMode(ROTARY2_B_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ROTARY2_A_PIN), rotary2Encoder, CHANGE);

  // Inisialisasi layar OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Alamat 0x3C untuk 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  //display.display();
  //idle();
  //delay(2000); // Delay 2 detik untuk menampilkan pesan selamat datang
  display.clearDisplay();
}

void loop() {

  //idle();
  indikator();
  //position();
  //paused_status();
  //dir_status();
 
}

void idle(){
  display.setTextSize(1); // Ukuran font untuk nama
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(15, 10);
  display.println("Not Connected..");
  display.display();
}

void dir_status(){
  if (stat_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, 7);
    display.println("POSISI");
    display.setCursor(45, 32);
    display.setTextSize(4); // Ukuran font untuk angka
       // Menampilkan nilai rotary1Pos dengan menangani kasus nilai negatif
    if (rotary1Pos < 0) {
      display.setCursor(21, 32);
      display.print("-");
      printTwoDigit(abs(rotary1Pos)); // Menggunakan fungsi abs() untuk mendapatkan nilai absolut
    } else if (rotary1Pos > 0){
      display.setCursor(21, 32);
      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, 46);
      display.setTextSize(2);
      display.println("cm");
    }
    display.display();
  }



void indikator(){

    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();
  }