#include <Wire.h>
#include <MPU6050.h>
#include <SoftwareSerial.h>

// 블루투스 핀 설정 (RX, TX)
const int btRx = 10;
const int btTx = 11;
SoftwareSerial mySerial(btRx, btTx); // RX, TX

// MPU6050 인스턴스 생성
MPU6050 mpu;

// 스텝모터 드라이버 핀 설정
const int stepPin1 = 2;
const int dirPin1 = 3;
const int stepPin2 = 4;
const int dirPin2 = 5;

// PID 제어 변수
float Kp = 1.5;
float Ki = 0.5;
float Kd = 0.05;

float targetAngle = 0.0;
float currentAngle = 0.0;
float error = 0.0;
float previousError = 0.0;
float integral = 0.0;
float derivative = 0.0;
float control = 0.0;
float previousControl = 0.0; // 이전 제어값 저장 변수

// 센서 데이터 변수
int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  // 기본 시리얼 통신 시작
  Serial.begin(9600);
  Serial.println("시리얼 통신 시작");

  // 블루투스 통신 시작
  mySerial.begin(9600);
  mySerial.println("블루투스 통신 시작");

  // MPU6050 초기화
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 연결 실패");
    mySerial.println("MPU6050 연결 실패");
    while (1); // 무한 루프에 빠져서 더 이상 진행하지 않음
  } else {
    Serial.println("MPU6050 연결 성공");
    mySerial.println("MPU6050 연결 성공");
  }

  // 스텝모터 드라이버 핀 모드 설정
  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  Serial.println("스텝모터 드라이버 핀 설정 완료");
}

void loop() {
  // 자이로스코프 데이터를 읽어 현재 각도 계산
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  currentAngle = atan2((double)ay, (double)az) * 180 / PI;

  // PID 제어 계산
  error = targetAngle - currentAngle;
  integral += error;
  derivative = error - previousError;
  control = Kp * error + Ki * integral + Kd * derivative;
  previousError = error;

  // 제어 값 변동 여부 확인 및 출력
  if (control != previousControl) {
    Serial.print("현재 각도: ");
    Serial.println(currentAngle);
    mySerial.print("현재 각도: ");
    mySerial.println(currentAngle);

    Serial.print("제어 값: ");
    Serial.println(control);
    mySerial.print("제어 값: ");
    mySerial.println(control);

    // 제어 값에 따라 스텝모터 제어
    if (control > 0) {
      digitalWrite(dirPin1, HIGH);
      digitalWrite(dirPin2, HIGH);
    } else {
      digitalWrite(dirPin1, LOW);
      digitalWrite(dirPin2, LOW);
    }

    Serial.print("스텝모터 방향: ");
    Serial.println(control > 0 ? "앞" : "뒤");
    mySerial.print("스텝모터 방향: ");
    mySerial.println(control > 0 ? "앞" : "뒤");

    // 스텝모터 속도 설정 및 동작
    int steps = abs(control);
    for (int i = 0; i < steps; i++) {
      digitalWrite(stepPin1, HIGH);
      digitalWrite(stepPin2, HIGH);
      delayMicroseconds(500);
      digitalWrite(stepPin1, LOW);
      digitalWrite(stepPin2, LOW);
      delayMicroseconds(500);
    }

    // 블루투스 통신을 통해 제어 값을 전송
    mySerial.print("Angle: ");
    mySerial.print(currentAngle);
    mySerial.print(" Control: ");
    mySerial.println(control);

    // 이전 제어값 업데이트
    previousControl = control;
  }

  // 짧은 지연 시간
  delay(10);
}