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

// OLED Display settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_ADDRESS 0x3C // I2C Address ของ OLED

// Joystick Pins
#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN 13      // ปุ่มเลือกมอเตอร์
#define HOME 12         // ปุ่ม Reset มอเตอร์กลับสู่ตำแหน่งเริ่มต้น

const int MOTOR_COUNT = 3;

// Stepper Motor Pins
const int EN[MOTOR_COUNT] = {10, 7, 4};  // ขาเปิดใช้งานมอเตอร์
const int STEP[MOTOR_COUNT] = {9, 6, 3}; // ขาขั้นตอน
const int DIR[MOTOR_COUNT] = {8, 5, 2};  // ขาทิศทาง

const int stepsPerRev = 400; // จำนวนขั้นตอนต่อรอบของมอเตอร์
int currentPos[MOTOR_COUNT] = {0, 0, 0}; // ตำแหน่งปัจจุบันของมอเตอร์
int selectedMotor = 0;                   // มอเตอร์ที่ถูกเลือก

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  Serial.begin(115200);
  Serial.println("Starting...");

  // Set up stepper motor pins
  for (int i = 0; i < MOTOR_COUNT; i++) {
    pinMode(EN[i], OUTPUT);
    pinMode(STEP[i], OUTPUT);
    pinMode(DIR[i], OUTPUT);
    digitalWrite(EN[i], LOW); // Enable motors
  }

  // Set up Joystick and Reset button
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
  pinMode(HOME, INPUT_PULLUP);

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.display(); // แสดงผลหน้าจอ OLED ตอนเริ่มต้น
}

void loop() {
  checkJoystick();          // ตรวจสอบ Input จาก Joystick
  checkSerialCommand();     // ตรวจสอบคำสั่งจาก Serial Monitor
  if (digitalRead(HOME) == LOW) {
    resetToHome();
    delay(500);
  }
  updateOLED();             // อัปเดตหน้าจอ OLED
}

void resetToHome() {
  Serial.println("Resetting to Home Position");
  for (int i = 0; i < MOTOR_COUNT; i++) {
    moveToPosition(i, 0);
  }
}

void checkJoystick() {
  int vert = analogRead(VERT_PIN);
  int horz = analogRead(HORZ_PIN);

  // เลือกมอเตอร์ด้วย Joystick แนวนอน
  if (vert ==512 && horz > 700) {
    selectedMotor = (selectedMotor + MOTOR_COUNT - 1) % MOTOR_COUNT;
    delay(500); // ป้องกันการกดเปลี่ยนเร็วเกินไป
  } if (vert == 512 && horz < 300) {
    selectedMotor = (selectedMotor + 1) % MOTOR_COUNT;
    delay(500);
  }

  // ควบคุมการหมุนของมอเตอร์ด้วย Joystick แนวตั้ง
  if (vert > 700) {
    moveMotor(selectedMotor, 1);
  } else if (vert < 300) {
    moveMotor(selectedMotor, -1);
  }
}

void moveMotor(int motor, int direction) {
  digitalWrite(DIR[motor], direction == 1 ? HIGH : LOW);
  digitalWrite(STEP[motor], HIGH);
  delayMicroseconds(500);
  digitalWrite(STEP[motor], LOW);
  delayMicroseconds(500);
  currentPos[motor] += direction;
}

void checkSerialCommand() {
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();

    if (input[0] == 'M') {
      int x, y, z;
      if (sscanf(input.c_str(), "M %d %d %d", &x, &y, &z) == 3) {
        moveToPosition(0, x);
        moveToPosition(1, y);
        moveToPosition(2, z);
      } else {
        Serial.println("Invalid command. Use: M X Y Z");
      }
    }
  }
}

void moveToPosition(int motor, int targetAngle) {
  if (motor < 0 || motor >= MOTOR_COUNT) return;

  int targetSteps = round((targetAngle / 360.0) * stepsPerRev);
  int stepDiff = targetSteps - currentPos[motor];
  bool direction = stepDiff > 0 ? HIGH : LOW;

  Serial.print("Motor: "); Serial.print(motor);
  Serial.print(", Target Angle: "); Serial.print(targetAngle);
  Serial.print(", Target Steps: "); Serial.print(targetSteps);
  Serial.print(", Step Diff: "); Serial.println(stepDiff);

  digitalWrite(DIR[motor], direction);

  for (int i = 0; i < abs(stepDiff); i++) {
    digitalWrite(STEP[motor], HIGH);
    delayMicroseconds(300);
    digitalWrite(STEP[motor], LOW);
    delayMicroseconds(300);
  }

  currentPos[motor] = targetSteps;
}

void updateOLED() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Manual Control");

  for (int i = 0; i < MOTOR_COUNT; i++) {
    display.print((char)('X' + i));
    display.print(": ");
    display.println((float)currentPos[i] / stepsPerRev * 360.0, 2);
  }

  display.print("Selected: ");
  display.println((char)('X' + selectedMotor));

  display.display();
}