#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// ตั้งค่าหน้าจอ OLED 128x64 (ต่อพิน SDA = GPIO 21, SCL = GPIO 22 ของ ESP32)
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// กำหนดพินอุปกรณ์
const int servoPin = 18;
const int joystickXPin = 33; // พินอนาล็อกที่รับค่าแกน X ของจอยสติ๊ก

Servo sx;

// ตัวแปรเก็บมุมปัจจุบันของเซอร์โว (เริ่มต้นที่กึ่งกลาง 90 องศา)
int currentAngle = 90; 

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

  // เริ่มต้นใช้งานหน้าจอ OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // หยุดทำงานหากหาหน้าจอไม่เจอ
  }
  
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.display();

  // ตั้งค่าพินและมุมเริ่มต้นของเซอร์โว
  sx.attach(servoPin, 500, 2400); 
  sx.write(currentAngle);          
}

void loop() {
  // อ่านค่าแกน X จากจอยสติ๊ก (ค่าที่ได้จากพิน Analog ของ ESP32 จะอยู่ระหว่าง 0 ถึง 4095)
  int xValue = analogRead(joystickXPin);

  // ตรวจสอบทิศทางขยับจอยสติ๊ก (ปกติค่าตรงกลางจะอยู่แถวๆ 2048)
  if (xValue > 2500) {
    // ขยับไปทางขวา -> ค่อยๆ เพิ่มมุมขึ้นครั้งละ 2 องศา (ไม่เกิน 180)
    currentAngle += 2; 
    if (currentAngle > 180) {
      currentAngle = 180;
    }
    delay(15); // หน่วงเวลาเล็กน้อยเพื่อให้เซอร์โวค่อยๆ หมุนตามอย่างนุ่มนวล
  } 
  else if (xValue < 1500) {
    // ขยับไปทางซ้าย -> ค่อยๆ ลดมุมลงครั้งละ 2 องศา (ไม่ต่ำกว่า 0)
    currentAngle -= 2;
    if (currentAngle < 0) {
      currentAngle = 0;
    }
    delay(15); // หน่วงเวลาเล็กน้อย
  }

  // สั่งให้มอเตอร์หมุนไปยังมุมปัจจุบัน
  sx.write(currentAngle);

  // พิมพ์ค่าออก Serial Monitor เพื่อดูสถานะ
  Serial.print("Joystick X: ");
  Serial.print(xValue);
  Serial.print(" | Servo Angle: ");
  Serial.println(currentAngle);
  

  // แสดงผลมุมปัจจุบันบนหน้าจอ OLED
  display.clearDisplay();
  
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("JOSTIK CONTROL");
  
  display.setTextSize(2);
  display.setCursor(0, 25);
  display.print("Angle: ");
  display.print(currentAngle);
  display.write(247); // แสดงสัญลักษณ์องศา (°)
  display.display();
}