#include <Servo.h>
const int servo1_pin = 3;
const int servo2_pin = 2;

const int dir1_pin = 4;
const int step1_pin = 5;
const int dir2_pin = 6;
const int step2_pin = 7;

const int led_L1 = 9;
const int led_L2 = 10;
const int led_L3 = 11;
const int led_L4 = 12;
Servo servo1, servo2;

const int sw1_pin = A5;
const int sw2_pin = A3;
const int sw3_pin = A1;
const int pot_pin = A0;

int motor_type = 0;
int motor_mode[2] = { 0, 0 };
int last_sw1_state = HIGH;
int last_sw2_state = HIGH;
int last_sw3_state = HIGH;
bool reset_mode = false;
int last_pot_value = 512;

long step1_position = 0;
long step2_position = 0;
const float steps_per_degree = 200.0 / 360.0;

unsigned long last_blink_time = 0;
bool blink_state = false;
const int blink_interval = 100;

void setServoLed(int pin, bool state) {
  digitalWrite(pin, state ? LOW : HIGH);
}
void setStepperLed(int pin, bool state) {
  digitalWrite(pin, state ? HIGH : LOW);
}
void setup() {
  pinMode(led_L1, OUTPUT);
  pinMode(led_L2, OUTPUT);
  pinMode(led_L3, OUTPUT);
  pinMode(led_L4, OUTPUT);

  pinMode(sw1_pin, INPUT_PULLUP);
  pinMode(sw2_pin, INPUT_PULLUP);
  pinMode(sw3_pin, INPUT_PULLUP);

  pinMode(step1_pin, OUTPUT);
  pinMode(dir1_pin, OUTPUT);
  pinMode(step2_pin, OUTPUT);
  pinMode(dir2_pin, OUTPUT);
  servo1.attach(servo1_pin);
  servo2.attach(servo2_pin);
  demoMode();
  setAllMotorsToVertical();
  updateLEDs();
}
void demoMode() {
  unsigned long startTime = millis();
  while (millis() - startTime < 5000) {
    blink_state = !blink_state;
    setServoLed(led_L1, blink_state);
    setServoLed(led_L2, blink_state);
    setStepperLed(led_L3, blink_state);
    setStepperLed(led_L4, blink_state);
    delay(200);
  }
  setServoLed(led_L1, false);
  setServoLed(led_L2, false);
  setStepperLed(led_L3, false);
  setStepperLed(led_L4, false);
  moveServoDemo();
  moveStepperDemo();
}
void moveServoDemo() {
  servo1.write(90);
  servo2.write(90);
  delay(1000);

  servo1.write(0);
  servo2.write(0);
  delay(1000);

  servo1.write(180);
  servo2.write(180);
  delay(1000);

  servo1.write(90);
  servo2.write(90);
  delay(1000);
}

void moveStepperDemo() {
  moveStepperToAngle(step1_pin, dir1_pin, step1_position, 90);
  moveStepperToAngle(step2_pin, dir2_pin, step2_position, 90);
  delay(1000);
  moveStepperToAngle(step1_pin, dir1_pin, step1_position, -90);
  moveStepperToAngle(step2_pin, dir2_pin, step2_position, -90);
  delay(1000);
  moveStepperToAngle(step1_pin, dir1_pin, step1_position, 0);
  moveStepperToAngle(step2_pin, dir2_pin, step2_position, 0);
  delay(1000);
}
void handleButtons() {
  int sw1_state = digitalRead(sw1_pin);
  int sw2_state = digitalRead(sw2_pin);
  int sw3_state = digitalRead(sw3_pin);
  if (sw1_state == LOW && last_sw1_state == HIGH) {
    motor_type = 0;
    motor_mode[0] = (motor_mode[0] + 1) % 4;
    updateLEDs();
    delay(200);
  }
  last_sw1_state = sw1_state;
  if (sw2_state == LOW && last_sw2_state == HIGH) {
    motor_type = 1;
    motor_mode[1] = (motor_mode[1] + 1) % 4;
    updateLEDs();
    delay(200);
  }
  last_sw2_state = sw2_state;
  if (sw3_state == LOW && last_sw3_state == HIGH) {
    reset_mode = true;
    setAllMotorsToVertical();
    delay(200);
  }
  last_sw3_state = sw3_state;
}
void handlePotentiometer() {
  int pot_value = analogRead(pot_pin);
  if (reset_mode) {
    if (abs(pot_value - 512) > 10) {
      reset_mode = false;
      last_pot_value = pot_value;
    }
    return;
  }
  if (abs(pot_value - last_pot_value) > 2) {
    last_pot_value = pot_value;
    if (motor_type == 0) {
      controlServoMotors(pot_value);
    } else {
      controlStepperMotors(pot_value);
    }
  }
}
void controlServoMotors(int pot_value) {
  int angle = map(pot_value, 0, 1023, -90, 90);
  int servo_angle = map(angle, -90, 90, 0, 180);
  switch (motor_mode[0]) {
    case 0:
      servo1.write(servo_angle);
      servo2.write(servo_angle);
      break;
    case 1:
      servo1.write(servo_angle);
      // Правый мотор не двигается
      break;
    case 2:
      servo2.write(servo_angle);
      // Левый мотор не двигается
      break;
    case 3:
      servo1.write(servo_angle);
        servo2.write(180 - servo_angle);
        break;
    }
  }
  void controlStepperMotors(int pot_value) {
    int target_angle = map(pot_value, 0, 1023, -90, 90);
    switch (motor_mode[1]) {
      case 0:
        moveStepperToAngle(step1_pin, dir1_pin, step1_position, target_angle);
        moveStepperToAngle(step2_pin, dir2_pin, step2_position, target_angle);
        break;
      case 1:
        moveStepperToAngle(step1_pin, dir1_pin, step1_position, target_angle);
        break;
      case 2:
        moveStepperToAngle(step2_pin, dir2_pin, step2_position, target_angle);
        break;
      case 3:
        moveStepperToAngle(step1_pin, dir1_pin, step1_position, target_angle);
        moveStepperToAngle(step2_pin, dir2_pin, step2_position, -target_angle);
        break;
    }
  }

  void moveStepperToAngle(int step_pin, int dir_pin, long &position, int target_angle) {
    long target_steps = round(target_angle * steps_per_degree);
    long steps_to_move = target_steps - position;
    if (steps_to_move == 0) return;
    digitalWrite(dir_pin, steps_to_move > 0 ? HIGH : LOW);
    int steps = abs(steps_to_move);
    for (int i = 0; i < steps; i++) {
      digitalWrite(step_pin, HIGH);
      delayMicroseconds(500);
      digitalWrite(step_pin, LOW);
      delayMicroseconds(500);
    }
    position = target_steps;
  }
  void setAllMotorsToVertical() {
    servo1.write(90);
    servo2.write(90);
    moveStepperToAngle(step1_pin, dir1_pin, step1_position, 0);
    moveStepperToAngle(step2_pin, dir2_pin, step2_position, 0);
  }
  void updateLEDs() {
    setServoLed(led_L1, false);
    setServoLed(led_L2, false);
    setStepperLed(led_L3, false);
    setStepperLed(led_L4, false);
    int mode = motor_mode[motor_type];
    if (motor_type == 0) {
      switch (mode) {
        case 0:
          setServoLed(led_L1, true);
          setServoLed(led_L2, true);
          break;
        case 1:
          setServoLed(led_L1, true);
          break;
        case 2:
          setServoLed(led_L2, true);
          break;
        case 3:
          break;
      }
    } else {
      switch (mode) {
        case 0:
          setStepperLed(led_L3, true);
          setStepperLed(led_L4, true);
          break;
        case 1:
          setStepperLed(led_L3, true);
          break;
        case 2:
          setStepperLed(led_L4, true);
          break;
        case 3:
          break;
      }
    }
  }
  void updateBlink() {
    unsigned long current_time = millis();
    if (current_time - last_blink_time >= blink_interval) {
      last_blink_time = current_time;
      blink_state = !blink_state;
      if (motor_type == 0 && motor_mode[0] == 3) {
        setServoLed(led_L1, blink_state);
        setServoLed(led_L2, blink_state);
      } else if (motor_type == 1 && motor_mode[1] == 3) {
        setStepperLed(led_L3, blink_state);
        setStepperLed(led_L4, blink_state);
      }
    }
  }
  void loop() {
    handleButtons();
    handlePotentiometer();
    updateBlink();
  }