// === 1번 모터 핀 정의 ===
#define DIR_1 8
#define ST_1 9
#define PWM_1 45
#define ENCA_1 2
#define ENCB_1 3

// === 2번 모터 핀 정의 ===
#define DIR_2 22
#define ST_2 24
#define PWM_2 44
#define ENCA_2 18
#define ENCB_2 19

// === 목표 RPM (부호 포함) ===
int setRPM1 = 0;
int setRPM2 = 0;

// === PID 계수 ===
double kp1 = 0.5, ki1 = 3, kd1 = 0.01;
double kp2 = 0.0, ki2 = 0.0, kd2 = 0.0;
double kb1 = 2; // ← Back Calculation 계수

// === 상태 변수 ===
// 1번 모터
volatile long encoderPos1 = 0;
double rpm1 = 0;
int calPWM1 = 0;
double error1 = 0, prevError1 = 0, integral1 = 0, derivative1 = 0;
double prevRPM1 = 0;
double alpha = 0.2;
double backError1 = 0;

// 2번 모터
volatile long encoderPos2 = 0;
double rpm2 = 0;
int calPWM2 = 0;
double error2 = 0, prevError2 = 0, integral2 = 0, derivative2 = 0;

// 제어 주기
const unsigned long interval = 30;
unsigned long prevTime = 0;
double dt = interval / 1000.0;

// === 인터럽트 핸들러 ===
void doEncoderA1() {
  if (digitalRead(ENCB_1) == LOW) encoderPos1++;
  else encoderPos1--;
}
void doEncoderB1() {
  if (digitalRead(ENCA_1) == HIGH) encoderPos1++;
  else encoderPos1--;
}
void doEncoderA2() {
  if (digitalRead(ENCB_2) == LOW) encoderPos2--;
  else encoderPos2++;
}
void doEncoderB2() {
  if (digitalRead(ENCA_2) == HIGH) encoderPos2--;
  else encoderPos2++;
}

// === RPM 계산 함수 ===
double calculateRPM(long counts) {
  return ((double)counts / (4096.0 * 2)) * (60.0 / dt);
}

// === PID 계산 함수 ===
int computePID(double currentRPM, double &error, double &prevError,
               double &integral, double &derivative, int targetRPM,
               double kp, double ki, double kd, bool isMotor1) {
  error = targetRPM - currentRPM;
  integral += error * dt;

  if (isMotor1) {
    double rawDerivative = -(currentRPM - prevRPM1) / dt;
    derivative = alpha * rawDerivative + (1 - alpha) * derivative1;
    prevRPM1 = currentRPM;

    double P = kp * error;
    double I = ki * integral;
    double D = kd * derivative;
    double output = P + I + D;

    // Back Calculation
    double outputSat = constrain(output, 0, 255);
    backError1 = outputSat - output;
    integral += kb1 * backError1 * dt;

    Serial.print(setRPM1); Serial.print('\t');
    Serial.print(currentRPM, 2); Serial.print('\t');
    Serial.print("P : "); Serial.print(P, 2); Serial.print('\t');
    Serial.print("I : "); Serial.print(I, 2); Serial.print('\t');
    Serial.print("D : "); Serial.print(D, 2); Serial.println('\t');

    return (int)output;
  } else {
    derivative = (error - prevError) / dt;
    prevError = error;
    return (int)((kp * error) + (ki * integral) + (kd * derivative));
  }
}

// === 실시간 입력 ===
void serialSetRPM() {
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();

    int rIndex = input.indexOf('R');
    int lIndex = input.indexOf('L');

    if (rIndex == -1 && lIndex == -1) {
      Serial.println("입력 오류: 'R' 또는 'L'이 포함되어야 합니다. 예: R100 L-80");
      return;
    }

    bool validInput = false;

    if (rIndex != -1) {
      int end = (lIndex != -1 && lIndex > rIndex) ? lIndex : input.length();
      String rValStr = input.substring(rIndex + 1, end);
      rValStr.trim();

      if (rValStr.length() == 0) {
        Serial.println("오류: R 뒤에 숫자가 없습니다. 예: R100");
      } else {
        int rpmVal = rValStr.toInt();
        if (String(rpmVal) != rValStr || abs(rpmVal) > 400) {
          Serial.println("오류: R 값 형식 또는 범위 초과");
        } else {
          setRPM1 = rpmVal;
          Serial.print("모터1 (R) 목표 RPM → "); Serial.println(setRPM1);
          validInput = true;
        }
      }
    }

    if (lIndex != -1) {
      int end = (rIndex != -1 && rIndex > lIndex) ? rIndex : input.length();
      String lValStr = input.substring(lIndex + 1, end);
      lValStr.trim();

      if (lValStr.length() == 0) {
        Serial.println("오류: L 뒤에 숫자가 없습니다. 예: L-80");
      } else {
        int rpmVal = lValStr.toInt();
        if (String(rpmVal) != lValStr || abs(rpmVal) > 400) {
          Serial.println("오류: L 값 형식 또는 범위 초과");
        } else {
          setRPM2 = rpmVal;
          Serial.print("모터2 (L) 목표 RPM → "); Serial.println(setRPM2);
          validInput = true;
        }
      }
    }

    if (!validInput) {
      Serial.println("R숫자 L숫자 형식으로 입력해주세요. 예: R100 L-80");
    }
  }
}

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

  pinMode(DIR_1, OUTPUT); pinMode(PWM_1, OUTPUT); pinMode(ST_1, OUTPUT);
  pinMode(ENCA_1, INPUT_PULLUP); pinMode(ENCB_1, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCA_1), doEncoderA1, RISING);
  attachInterrupt(digitalPinToInterrupt(ENCB_1), doEncoderB1, RISING);

  pinMode(DIR_2, OUTPUT); pinMode(PWM_2, OUTPUT); pinMode(ST_2, OUTPUT);
  pinMode(ENCA_2, INPUT_PULLUP); pinMode(ENCB_2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCA_2), doEncoderA2, RISING);
  attachInterrupt(digitalPinToInterrupt(ENCB_2), doEncoderB2, RISING);

  digitalWrite(ST_1, LOW);
  digitalWrite(ST_2, LOW);
  prevTime = millis();
}

void loop() {
  unsigned long now = millis();

  if (now - prevTime >= interval) {
    prevTime += interval;

    // 모터 1
    noInterrupts(); long currentPos1 = encoderPos1; encoderPos1 = 0; interrupts();
    rpm1 = calculateRPM(currentPos1);
    calPWM1 = computePID(rpm1, error1, prevError1, integral1, derivative1, abs(setRPM1), kp1, ki1, kd1, true);
    digitalWrite(DIR_1, setRPM1 >= 0 ? HIGH : LOW);
    calPWM1 = constrain(abs(calPWM1), 0, 255);
    analogWrite(PWM_1, calPWM1);
    
    // 모터 2
    noInterrupts(); long currentPos2 = encoderPos2; encoderPos2 = 0; interrupts();
    rpm2 = calculateRPM(currentPos2);
    calPWM2 = computePID(rpm2, error2, prevError2, integral2, derivative2, abs(setRPM2), kp2, ki2, kd2, false);
    digitalWrite(DIR_2, setRPM2 >= 0 ? HIGH : LOW);
    calPWM2 = constrain(abs(calPWM2), 0, 255);
    analogWrite(PWM_2, calPWM2);

    // 시리얼 출력
    // Serial.print("R:"); Serial.println(setRPM1);
    // Serial.print("RPM:"); Serial.println(rpm1, 2);
    // Serial.print("PWM:"); Serial.println(calPWM1);
    // Serial.println(" ");
    // Serial.print("L:"); Serial.println(setRPM2);
    // Serial.print("RPM:"); Serial.println(rpm2, 2);
    // Serial.print("PWM:"); Serial.println(calPWM2);
    // Serial.println("--------------------");

    // Serial Plotter용 출력
    Serial.print(setRPM1); Serial.print('\t');
    Serial.println(rpm1, 2);
    // Serial.print(setRPM2); Serial.print('\t');
    // Serial.println(rpm2, 2);
  }
  

  serialSetRPM();
}