//這一部分目標達成(PID控制、可自行定義輸入移動步數、兩軸控制分別隔開
//目前狀態
//單軸可以自動控制
//雙軸自動控制會被帶篇(以解決)
//條件設置：單一PID函數，但分別用兩組不同輸入輸出參數，移動位置目前透過監控視窗控制，控制條件X0，位置0
//目標將input數據改成AS5600所讀取到的數值
//目標新增各軸限位器，移動第一步驟馬達移動到原點
//0613 21:25 已完成單一限位器數值歸零
//0613 23:32 碰到第二個限位器可以回歸原點
//目前遇到問題，反轉功能無法呈現，無法透過增加負號更改，目前方式為起始值移動到極小值(為符合移動距離為正)，碰到第一個限位器設定原點同時將目標移動距離設為極大值
//碰到第二個限位器，紀錄移動距離並移至原點
//0614 注意限位器高低位態的控制
//**相對位置目前都會強制歸零

//6/14目標：在PID情況，一開始不移動，輸入代碼馬達碰撞兩端，紀錄最長距離，輸入移動位置移動到該點，AS5600顯示目前馬達位制以及結合PID控制
//***可能碰到問題，速度控制

#include <AccelStepper.h>
#include <PID_v1_bc.h>

#define STEP_PIN_X 2
#define DIR_PIN_X 5
#define STEP_PIN_Y 3
#define DIR_PIN_Y 6

#define LIMIT_SWITCH_X_A_PIN 14  // X軸限位器腳位
#define LIMIT_SWITCH_Y_A_PIN 16  // Y軸限位器腳位
#define LIMIT_SWITCH_X_B_PIN 15  // X軸限位器腳位
#define LIMIT_SWITCH_Y_B_PIN 17  // Y軸限位器腳位
#define Buzzer_pin 46

AccelStepper stepper_X(1, STEP_PIN_X, DIR_PIN_X); //X馬達對於driver控制
AccelStepper stepper_Y(1, STEP_PIN_Y, DIR_PIN_Y);

double Setpoint_X = 300; 
double Setpoint_XA = 0;   //相對移動位置的暫存數
double Setpoint_Y = 100;
double Setpoint_YA = 0;
double Input_X;       //馬達用於PID控制的輸入輸出__X軸   
double Output;
double Output_X;
double Input_Y;       //馬達用於PID控制的輸入輸出__Y軸   
double Output_Y;

//PID共同參數設定
double Kp = 2;
double Ki = 0.0001;
double Kd = 0.9;
PID myPID_X(&Input_X, &Output, &Setpoint_X, Kp, Ki, Kd, DIRECT);  //X馬達PID控制方式
PID myPID_Y(&Input_Y, &Output_Y, &Setpoint_Y, Kp, Ki, Kd, DIRECT);

// 儲存最大距離
long maxDistance_X = 0; //X軸最大值
long maxDistance_Y = 0;
long mid_X ; //X軸移動中間值
long mid_Y ; 


#define DIRECTION_CCW -1  //馬達反轉參數
#define DIRECTION_CW   1  //馬達正轉參數
int direction = DIRECTION_CCW;
bool previousFlip = true; //改變方向用

//狀態控制變數
int currentMode = 0; //進入模式選擇條件
bool motorEnabled = false; //馬達開始條件
bool motorEnabled_Y = false; //馬達開始條件
bool manualInputMode = false; // 手動輸入模式

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

  stepper_X.setMaxSpeed(10000);
  stepper_X.setAcceleration(4500);
  stepper_Y.setMaxSpeed(4000);
  stepper_Y.setAcceleration(2500);

  myPID_X.SetMode(AUTOMATIC);
  myPID_X.SetOutputLimits(-40000, 40000); 
  myPID_Y.SetMode(AUTOMATIC);
  myPID_Y.SetOutputLimits(-4000, 4000); 

  pinMode(LIMIT_SWITCH_X_A_PIN, INPUT_PULLUP);  // 設定限位器腳位
  pinMode(LIMIT_SWITCH_Y_A_PIN, INPUT_PULLUP);  // 設定限位器腳位
  pinMode(LIMIT_SWITCH_X_B_PIN, INPUT_PULLUP);  // 設定限位器腳位
  pinMode(LIMIT_SWITCH_Y_B_PIN, INPUT_PULLUP);  // 設定限位器腳位

  stepper_X.setSpeed(5000);
  stepper_X.moveTo(Setpoint_X);
  stepper_Y.setSpeed(100);
  stepper_Y.moveTo(Setpoint_Y);
}

void handleLimitSwitches() {
  if (digitalRead(LIMIT_SWITCH_X_A_PIN) == HIGH) {
    stepper_X.setCurrentPosition(0);  // 歸零X軸位置
    direction *= -1;
    Setpoint_X = direction * Setpoint_X;
    Serial.println("Limit Switch X A Triggered");
    stepper_X.moveTo(Setpoint_X);
  }
  if (digitalRead(LIMIT_SWITCH_X_B_PIN) == HIGH) {
    maxDistance_X = stepper_X.currentPosition();
    direction *= -1;
    Setpoint_X = maxDistance_X / 2;
    Serial.println("Limit Switch X B Triggered");
    Serial.print("max distance：");
    Serial.println(maxDistance_X);
    stepper_X.move(Setpoint_X);
  }
  if (digitalRead(LIMIT_SWITCH_Y_A_PIN) == HIGH) {
    Serial.println("Limit Switch Y A Triggered");
    stepper_Y.setCurrentPosition(0);  // 歸零Y軸位置
    stepper_Y.moveTo(11);
  }
  if (digitalRead(LIMIT_SWITCH_Y_B_PIN) == HIGH) {
    motorEnabled_Y = false;
    // maxDistance_Y = stepper_Y.currentPosition();
    Serial.println("Limit Switch Y B Triggered");
    beepBuzzer();
    // stepper_Y.moveTo(0);
    return;
  }else if(digitalRead(LIMIT_SWITCH_Y_B_PIN) == LOW){
   // motorEnabled_Y=true;
    return;
  }
}

void beepBuzzer(){
  digitalWrite(Buzzer_pin,HIGH);
  delay(500);
  digitalWrite(Buzzer_pin, LOW);
}
void handleSerialInput() {
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    char command = input.charAt(0);
    int value = input.substring(1).toInt();

    if (input == "start" || input == "START") {
      motorEnabled = true;
      motorEnabled_Y = true;
      //handleLimitSwitches();//限位器控制區
      Serial.println("motor ON");
    } else if (input == "stop" || input == "STOP") {
      motorEnabled = false;
      motorEnabled_Y = false;
      Serial.println("motor OFF");
    }

    if (motorEnabled || motorEnabled_Y) {
      if (input == "manual" || input == "MANUAL") {
        manualInputMode = true;
        Serial.println("Manual input mode");
      } else if (input == "auto" || input == "AUTO") {
        manualInputMode = false;
        Serial.println("Auto input mode");
      }
      if (manualInputMode)
      {
        if (command == 'X' || command == 'x') {
          Setpoint_X = value;
          stepper_X.moveTo(Setpoint_X);
          Serial.print("Manual setpoint X: ");
          Serial.println(Setpoint_X);
        } else if (command == 'Y' || command == 'y') {
          Setpoint_Y = value;
          stepper_Y.moveTo(Setpoint_Y);
          Serial.print("Manual setpoint Y: ");
          Serial.println(Setpoint_Y);
        }else if (command == 'XA' || command == 'xa') {
          Setpoint_XA = value;
          Setpoint_X = Setpoint_X + Setpoint_XA;
          stepper_X.moveTo(Setpoint_X);
          Serial.print("Manual setpoint X: ");
          Serial.println(Setpoint_X);
        } else if (command == 'YA' || command == 'ya') {
          Setpoint_YA = value;
          Setpoint_Y = Setpoint_Y + Setpoint_YA;
          stepper_Y.moveTo(Setpoint_Y);
          Serial.print("Manual setpoint Y: ");
          Serial.println(Setpoint_Y);
        }else 
        {
          if (input == "code1") {
            Setpoint_X = 0;
            Setpoint_Y = 0;
            Serial.println("Auto setpoint: code1");
          } else if (input == "code2") {
            Setpoint_X = 1500;
            Setpoint_Y = 2500;
            Serial.println("Auto setpoint: code2");
          } else if (input == "code3") {
            Setpoint_X = 2000;
            Setpoint_Y = 3000;
            Serial.println("Auto setpoint: code3");
          }
          stepper_X.moveTo(Setpoint_X);
          stepper_Y.moveTo(Setpoint_Y);
        }  
      }
    }
  }
}
void updateMotors() {
  Input_X = stepper_X.currentPosition();
  Input_Y = stepper_Y.currentPosition();

  myPID_X.Compute();
  myPID_Y.Compute();

  Output_X = Output * 16;
  stepper_X.setSpeed(Output_X);
  stepper_Y.setSpeed(Output_Y);

  Serial.print("X馬達輸出：");
  Serial.print(Output);
  Serial.print("，Y馬達輸出：");
  Serial.println(Output_Y);
  Serial.print("，X馬達位置：");
  Serial.print(stepper_X.currentPosition());
  Serial.print("，Y馬達位置：");
  Serial.println(stepper_Y.currentPosition());
  Serial.print("X軸設定位置：");
  Serial.print(Setpoint_X);
  Serial.print("，Y軸設定位置：");
  Serial.println(Setpoint_Y);

  if (stepper_X.distanceToGo() == 0) {
    stepper_X.stop();
  }
  if (stepper_Y.distanceToGo() == 0) {
    stepper_Y.stop();
  }
  stepper_X.run();
  stepper_Y.run();
}

void updateMotors_X(){
  Input_X = stepper_X.currentPosition();

  myPID_X.Compute();

  Output_X = Output * 64;
  stepper_X.setSpeed(Output_X);

  Serial.print("X馬達位置：");
  Serial.print(stepper_X.currentPosition());

  Serial.print("，X軸設定位置：");
  Serial.println(Setpoint_X);

  if (stepper_X.distanceToGo() == 0) {
    stepper_X.stop();
  }
  stepper_X.run();
}

void updateMotors_Y(){
  Input_Y = stepper_Y.currentPosition();

  myPID_Y.Compute();

  Output_Y = Output_Y * 16;
  stepper_Y.setSpeed(Output_Y);

  Serial.print("Y馬達位置：");
  Serial.print(stepper_Y.currentPosition());

  Serial.print("，Y軸設定位置：");
  Serial.println(Setpoint_Y);

  if (stepper_Y.distanceToGo() == 0) {
    stepper_Y.stop();
  }
  stepper_Y.run();
}

void loop() {
  handleSerialInput();
  handleLimitSwitches();//限位器控制區
  
  
  if (motorEnabled ) {
    updateMotors_X();
  }
  if (motorEnabled_Y) {
    updateMotors_Y();
  }
  
}