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

LiquidCrystal_I2C lcd(0x27, 16, 2);
MPU6050 mpu;

const int ENC_CLK = 2;
const int ENC_DT = 3;
const int ENC_SW = 4;
const int MOTOR_IN1 = 5;
const int MOTOR_IN2 = 6;
const int MOTOR_ENA = 9;
const int BTN_EMERGENCY = 7;
const int LED_GREEN = 10;
const int LED_RED = 11;
const int BUZZER = 12;

volatile int encoderPos = 15;  // Стартовое значение Target = 15°
int targetAngle = 15;
int lastEncCLK;
float currentAngle = 0.0;
bool emergencyStop = false;
bool angleReached = false;
unsigned long lastLCDUpdate = 0;

void setup() {
  Serial.begin(9600);
  
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Dump Truck Ctrl");
  lcd.setCursor(0, 1);
  lcd.print("Init...");
  
  Wire.begin();
  mpu.initialize();
  
  lcd.setCursor(0, 1);
  lcd.print("MPU6050 init OK");
  delay(500);
  
  pinMode(ENC_CLK, INPUT);
  pinMode(ENC_DT, INPUT);
  pinMode(ENC_SW, INPUT_PULLUP);
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  pinMode(MOTOR_ENA, OUTPUT);
  pinMode(BTN_EMERGENCY, INPUT_PULLUP);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  
  motorStop();
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_RED, LOW);
  digitalWrite(BUZZER, LOW);
  
  lastEncCLK = digitalRead(ENC_CLK);
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Target: 15");
  lcd.setCursor(0, 1);
  lcd.print("Angle: 00.0");
  
  // Выводим в Serial Monitor начальные значения
  Serial.print("Target: ");
  Serial.print(targetAngle);
  Serial.print(" | Current: ");
  Serial.println(currentAngle);
  
  delay(1000);
}

void loop() {
  if (digitalRead(BTN_EMERGENCY) == LOW) {
    emergencyStop = true;
    motorStop();
    digitalWrite(LED_RED, HIGH);
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(BUZZER, HIGH);
    lcd.setCursor(0, 0);
    lcd.print("EMERGENCY STOP!");
    lcd.setCursor(0, 1);
    lcd.print("PRESS RESET    ");
    while (digitalRead(BTN_EMERGENCY) == LOW) {
      delay(10);
    }
    emergencyStop = false;
    digitalWrite(LED_RED, LOW);
    digitalWrite(BUZZER, LOW);
    lcd.clear();
    return;
  }
  
  readEncoder();
  readMPU6050();
  
  if (!emergencyStop) {
    if (currentAngle < targetAngle - 0.5) {
      motorUp();
      angleReached = false;
      digitalWrite(LED_GREEN, LOW);
    } else if (currentAngle > targetAngle + 0.5) {
      motorDown();
      angleReached = false;
      digitalWrite(LED_GREEN, LOW);
    } else {
      motorStop();
      if (!angleReached) {
        angleReached = true;
        digitalWrite(LED_GREEN, HIGH);
        digitalWrite(BUZZER, HIGH);
        delay(200);
        digitalWrite(BUZZER, LOW);
      }
    }
  }
  
  if (millis() - lastLCDUpdate > 200) {
    lastLCDUpdate = millis();
    lcd.setCursor(0, 0);
    lcd.print("Target:");
    lcd.setCursor(7, 0);
    if (targetAngle < 10) lcd.print(" ");
    lcd.print(targetAngle);
    lcd.print("   ");
    lcd.setCursor(0, 1);
    lcd.print("Angle:");
    lcd.setCursor(6, 1);
    lcd.print(currentAngle, 1);
    lcd.print("   ");
  }
  
  if (millis() % 1000 < 10) {
    Serial.print("Target: ");
    Serial.print(targetAngle);
    Serial.print(" | Current: ");
    Serial.println(currentAngle);
  }
  
  delay(10);
}

void readEncoder() {
  int clkState = digitalRead(ENC_CLK);
  if (clkState != lastEncCLK) {
    if (digitalRead(ENC_DT) != clkState) {
      encoderPos++;
    } else {
      encoderPos--;
    }
    lastEncCLK = clkState;
  }
  if (digitalRead(ENC_SW) == LOW) {
    targetAngle = constrain(encoderPos, 0, 30);
    delay(200);
  }
  targetAngle = constrain(encoderPos, 0, 30);
}

void readMPU6050() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float angleY = atan2(ay, az) * 180.0 / PI;
  currentAngle = angleY;
  if (currentAngle < -90) currentAngle = -90;
  if (currentAngle > 90) currentAngle = 90;
}

void motorUp() {
  digitalWrite(MOTOR_IN1, HIGH);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(MOTOR_ENA, 200);
}

void motorDown() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, HIGH);
  analogWrite(MOTOR_ENA, 200);
}

void motorStop() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(MOTOR_ENA, 0);
}