#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);  // ใช้ที่อยู่ I2C 0x27, ถ้าไม่ขึ้นลองเปลี่ยนเป็น 0x3F

#define JOY_X_vert A0
#define JOY_X_horizon A1

#define JOY_Y_vert A2
#define JOY_Y_horizon A3

#define JOY_Z_vert A4
#define JOY_Z_horizon A5



#define EN_PIN 6
#define DIR_X 4
#define STEP_X 5

#define DIR_X2 2
#define STEP_X2 3

#define DIR_X3 7
#define STEP_X3 8

#define BUTTON_PIN  9

int step_rev = 3200; // (360/1.8)*32 = 200
int dir = 1;
int i = 0;

bool STATE = LOW;
bool STATE_DIR = HIGH;

int Home_step[3] = {0,0,0};

bool lastButtonState = HIGH; // เก็บสถานะของปุ่มกดครั้งก่อน
bool currentButtonState = HIGH; // เก็บสถานะของปุ่มกดในปัจจุบัน
bool buttonPressed = false; // เช็คสถานะการกดปุ่ม

unsigned long previousMillis_1;
const unsigned long interval_1 = 1; // Timer interval 1ms

void setup() {
   Serial.begin(115200);
    pinMode(JOY_X_vert, INPUT);
    pinMode(JOY_X_horizon, INPUT);
    pinMode(JOY_Y_horizon, INPUT);
    pinMode(JOY_Z_horizon, INPUT);
    pinMode(EN_PIN, OUTPUT);
    pinMode(DIR_X, OUTPUT);
    pinMode(STEP_X, OUTPUT);
    pinMode(DIR_X2, OUTPUT);
    pinMode(STEP_X2, OUTPUT);
    pinMode(DIR_X3, OUTPUT);
    pinMode(STEP_X3, OUTPUT);
    pinMode(BUTTON_PIN, INPUT_PULLUP); // ตั้งค่า pin ของสวิตช์ Home เป็น INPUT_PULLUP

    lcd.init();         // เริ่มต้นใช้งาน LCD
    lcd.backlight();    // เปิดไฟพื้นหลังของ LCD

    lcd.setCursor(0, 0);
    lcd.print("Hello, Chester!");
    lcd.setCursor(0, 1);
    lcd.print("LCD 20x4 Ready!");   

    Serial.println("กรอกค่าก้าวมอเตอร์ 3 ตัว (ตัวอย่าง: M 30 60 90)");

}

void loop() {
    unsigned long currentMillis = millis();
    digitalWrite(EN_PIN, LOW);
    currentButtonState = digitalRead(BUTTON_PIN);
    int xValue = analogRead(JOY_X_horizon); 
    int yValue = analogRead(JOY_Y_horizon); 
    int zValue = analogRead(JOY_Z_horizon); 

    if (xValue > 550) {  // ถ้าเอียงไปทางขวา
        digitalWrite(DIR_X, LOW);  // ตั้งทิศทาง
        moveSteppe_manual(STEP_X, 16);     // เคลื่อนที่ 5 สเต็ป
        Home_step[0] = Home_step[0] - 16;
    } 
    else if (xValue < 450) {  // ถ้าเอียงไปทางซ้าย
        digitalWrite(DIR_X, HIGH);  // ตั้งทิศทาง
        moveSteppe_manual(STEP_X, 16);     // เคลื่อนที่ 5 สเต็ป (ทิศตรงข้าม)
        Home_step[0] = Home_step[0] + 16;
    }

    if (yValue > 550) {  // ถ้าเอียงไปทางขวา
        digitalWrite(DIR_X2, LOW);  // ตั้งทิศทาง
        moveSteppe_manual(STEP_X2, 16);     // เคลื่อนที่ 5 สเต็ป
        Home_step[1] = Home_step[1] - 16;
    } 
    else if (yValue < 450) {  // ถ้าเอียงไปทางซ้าย
        digitalWrite(DIR_X2, HIGH);  // ตั้งทิศทาง
        moveSteppe_manual(STEP_X2, 16);     // เคลื่อนที่ 5 สเต็ป (ทิศตรงข้าม)
        Home_step[1] = Home_step[1] + 16;
    }

    if (zValue > 550) {  // ถ้าเอียงไปทางขวา
        digitalWrite(DIR_X3, LOW);  // ตั้งทิศทาง
        moveSteppe_manual(STEP_X3, 16);     // เคลื่อนที่ 5 สเต็ป
        Home_step[2] = Home_step[2] - 16;
    } 
    else if (zValue < 450) {  // ถ้าเอียงไปทางซ้าย
        digitalWrite(DIR_X3, HIGH);  // ตั้งทิศทาง
        moveSteppe_manual(STEP_X3, 16);     // เคลื่อนที่ 5 สเต็ป (ทิศตรงข้าม)
        Home_step[2] = Home_step[2] + 16;
    }


    if (Serial.available()) {

      String input = Serial.readStringUntil('\n');  // อ่านบรรทัดข้อมูล
        input.trim();  // ตัดช่องว่างและตัวขึ้นบรรทัดใหม่ออก

        if (input.startsWith("M")) {  // ถ้าข้อความขึ้นต้นด้วย "M"
            int space1 = input.indexOf(' ');  
            int space2 = input.indexOf(' ', space1 + 1);
            int space3 = input.indexOf(' ', space2 + 1);

            if (space1 > 0 && space2 > space1 && space3 > space2) { 
                float X_degree = input.substring(space1 + 1, space2).toInt();
                float Y_degree = input.substring(space2 + 1, space3).toInt();
                float Z_degree = input.substring(space3 + 1).toInt();

                Serial.print("Setting motors: ");
                Serial.print(X_degree); Serial.print(" ");
                Serial.print(Y_degree); Serial.print(" ");
                Serial.println(Z_degree);

                float Pwm_X = (X_degree * 3200) / 360;
                float Pwm_Y = (Y_degree * 3200) / 360;
                float Pwm_Z = (Z_degree * 3200) / 360; 
                float DeltaX = (Pwm_X - Home_step[0]);
                float DeltaY = (Pwm_Y - Home_step[1]);
                float DeltaZ = (Pwm_Z - Home_step[2]);
                Home_step[0] = Home_step[0] + DeltaX;
                Home_step[1] = Home_step[1] + DeltaY;
                Home_step[2] = Home_step[2] + DeltaZ;
                Serial.print(Home_step[0]); Serial.print(" ");
                Serial.print(Home_step[1]); Serial.print(" ");
                Serial.println(Home_step[2]);

                moveStepper(DIR_X, STEP_X, DeltaX);
                moveStepper(DIR_X2, STEP_X2, DeltaY);
                moveStepper(DIR_X3, STEP_X3, DeltaZ);
            }
        }else{
          Serial.print("กรอกใหม่ เช่น M 30 60 90");
        }
    }
       
    if (lastButtonState == HIGH && currentButtonState == LOW) {
        buttonPressed = true; // เมื่อปุ่มถูกกด
    }

    lastButtonState = currentButtonState; // อัปเดตสถานะปุ่ม

    // ถ้ากดปุ่ม, ให้มอเตอร์เคลื่อนที่ไปที่จุดเริ่มต้น
    if (buttonPressed) {
        homeMovement(Home_step);
        buttonPressed = false; // รีเซ็ตสถานะปุ่มหลังจากกดแล้ว
    }
    
    int X = (Home_step[0]*360)/3200;
    int Y = (Home_step[1]*360)/3200;
    int Z = (Home_step[2]*360)/3200;

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Degree");
    lcd.setCursor(0, 1);
    lcd.print("X_degree : "); 
    lcd.println(((float(Home_step[0])*360)/3200)); 
    lcd.setCursor(0, 2);
    lcd.print("Y_degree : "); 
    lcd.println(((float(Home_step[1])*360)/3200)); 
    lcd.setCursor(0, 3);
    lcd.print("Z_degree : "); 
    lcd.println(((float(Home_step[2])*360)/3200)); 
    delay(100);  
    

}

void moveStepper(int dirPin, int stepPin, int steps) {
    if (steps>=0){
    digitalWrite(dirPin, HIGH); // กำหนดทิศทางการหมุน
    }else{
      digitalWrite(dirPin, LOW); 
      steps = -steps;
    }
    for (int i = 0; i < steps; i++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(1000);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(1000);
    }
}
void homeMovement(int Home_step[3]) {
    // หาค่ามากสุดที่ต้องหมุน
    int maxVal = findMax(Home_step, 3);

    // กำหนดทิศทางให้หมุนไปที่ 0 องศา
    digitalWrite(DIR_X, LOW);
    digitalWrite(DIR_X2, LOW);
    digitalWrite(DIR_X3, LOW);

    // ตัวแปรควบคุมอัตราการหมุน
    int stepX = 0, stepX2 = 0, stepX3 = 0;

    for (int i = 0; i < maxVal; i++) {
        if (stepX < Home_step[0]) {
            digitalWrite(STEP_X, HIGH);
        }
        if (stepX2 < Home_step[1]) {
            digitalWrite(STEP_X2, HIGH);
        }
        if (stepX3 < Home_step[2]) {
            digitalWrite(STEP_X3, HIGH);
        }
        delayMicroseconds(1000);

        digitalWrite(STEP_X, LOW);
        digitalWrite(STEP_X2, LOW);
        digitalWrite(STEP_X3, LOW);
        delayMicroseconds(1000);

        // เพิ่มค่าแต่ละมอเตอร์
        if (stepX < Home_step[0]) stepX++;
        if (stepX2 < Home_step[1]) stepX2++;
        if (stepX3 < Home_step[2]) stepX3++;
    }

    // อัปเดตค่าหลังจากโฮมเสร็จ
    Home_step[0] = 0;
    Home_step[1] = 0;
    Home_step[2] = 0;

    // เปลี่ยนทิศทางเมื่อถึง 0 องศา
    STATE_DIR = !STATE_DIR;
}

int findMax(int arr[], int n) {
    int maxVal = arr[0];  
    for (int i = 1; i < n; i++) {
        if (arr[i] > maxVal) {
            maxVal = arr[i];
        }
    }
    return maxVal;
}

void moveSteppe_manual(int stepPin, int steps) {
    for (int i = 0; i < steps; i++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(1000);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(1000);
    }
}