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

// --- OLED Config ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// --- Pin Setup ---
#define JOY_X_PIN 34
#define JOY_Y_PIN 35
#define SERVO_X_PIN 13
#define SERVO_Y_PIN 12

// --- Servo Objects ---
Servo servoX;
Servo servoY;

// --- Calibration values (ปรับตามจอยจริง) ---
int joyX_min = 200;
int joyX_max = 3900;
int joyY_min = 200;
int joyY_max = 3900;

// --- ฟังก์ชันอ่านค่าแบบ Smoothing ---
int smoothAnalog(int pin) {
  long total = 0;
  for (int i = 0; i < 10; i++) { // อ่าน 10 ครั้งแล้วหาค่าเฉลี่ย
    total += analogRead(pin);
    delay(2);
  }
  return total / 10;
}

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

  // Attach servos
  servoX.attach(SERVO_X_PIN);
  servoY.attach(SERVO_Y_PIN);

  // Init OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED not found"));
    while (true);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
}

void loop() {
  // อ่านค่า analog พร้อม smoothing
  int joyX = smoothAnalog(JOY_X_PIN);
  int joyY = smoothAnalog(JOY_Y_PIN);

  // Map ค่าไปเป็นองศาเซอร์โว
  int angleX = map(joyX, joyX_min, joyX_max, 0, 180);
  int angleY = map(joyY, joyY_min, joyY_max, 0, 180);

  // จำกัดค่าให้อยู่ในช่วง 0-180
  angleX = constrain(angleX, 0, 180);
  angleY = constrain(angleY, 0, 180);

  // ควบคุมเซอร์โว
  servoX.write(angleX);
  servoY.write(angleY);

  // แสดงผลบน OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("2-Axis Joystick");
  display.print("X: "); display.print(angleX); display.println(" deg");
  display.print("Y: "); display.print(angleY); display.println(" deg");

  display.print("RawX: "); display.println(joyX);
  display.print("RawY: "); display.println(joyY);
  display.display();

  delay(50);
}