#include <AccelStepper.h>
#include <Wire.h>
#include <MPU6050.h>
#include <math.h>

#define dirPin 2
#define stepPin 3
#define buttonCW 4        // Button for clockwise rotation
#define buttonCCW 5       // Button for counterclockwise rotation
#define buttonSweep 6     // Button for 180-degree continuous sweep
#define buttonMPU 7       // Button for MPU6050 control
#define enPin 10
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
MPU6050 mpu;

int mpuControl = 0;
int manualControl = 0;
bool sweeping = false;
int sweepSteps = 200;  // Steps for 180-degree rotation, adjust for your stepper motor

void disableStepper() {
  digitalWrite(enPin, HIGH);  // Disable stepper en pin
}
void enableStepper() {
  digitalWrite(enPin, LOW);  // Enable stepper en pin
}

void setup() {
  Serial.begin(9600);
  pinMode(enPin, OUTPUT);  // Set enable pin as low
  pinMode(buttonCW, INPUT_PULLUP);
  pinMode(buttonCCW, INPUT_PULLUP);
  pinMode(buttonSweep, INPUT_PULLUP);
  pinMode(buttonMPU, INPUT_PULLUP);

  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);

  Wire.begin();
  mpu.initialize();
}

void loop() {
  if (digitalRead(buttonCW) == LOW) {
    manualControl = 1;
    sweeping = false;  // Stop sweeping if another button is pressed
    rotateClockwise();
  } else if (digitalRead(buttonCCW) == LOW) {
    manualControl = 1;
    sweeping = false;  // Stop sweeping if another button is pressed
    rotateCounterclockwise();
  } else if (digitalRead(buttonSweep) == LOW) {
    manualControl = 1;
    mpuControl = 0;  // Turn off MPU control if the sweep button is pressed
    sweeping = true;
    enableStepper();  // Enable stepper motor
  } else if (digitalRead(buttonMPU) == LOW) {
    enableStepper();  // Enable stepper motor
    mpuControl = 1;
    manualControl = 0;
    sweeping = false;  // Stop sweeping when MPU control is enabled
  }

  if (sweeping) {
    continuousSweep();  // Call the sweep function repeatedly
  }

  if (mpuControl) {
    controlWithMPU();
  }
}

void rotateClockwise() {
  enableStepper();  // Enable Stepper Motor
  while (digitalRead(buttonCW) == LOW) {
    stepper.move(1);  // Rotate 1 step
    stepper.run();
    if (anyOtherButtonPressed()) {
      disableStepper();  // Disable Stepper Motor
      break;
    }
  }
  disableStepper();  // Disable Stepper Motor
}
void rotateCounterclockwise() {
  enableStepper();  // Enable Stepper Motor
  while (digitalRead(buttonCCW) == LOW) {
    stepper.move(-1);  // Rotate 1 step in the opposite direction
    stepper.run();
    if (anyOtherButtonPressed()) {
      disableStepper();  // Disable Stepper Motor
      break;
    }
  }
  disableStepper();  // Disable Stepper Motor
}

void continuousSweep() {
  static int direction = 1;  // 1 for forward, -1 for backward
  if (!sweeping) return;     // Only sweep if the sweep button is pressed

  // Move 180 degrees in the current direction
  stepper.move(direction * sweepSteps);
  while (stepper.distanceToGo() != 0) {
    stepper.run();

    // Check if any other button is pressed to stop sweeping
    if (anyOtherButtonPressed()) {
      stepper.stop();
      sweeping = false;
      disableStepper();
      return;
    }
  }

  delay(500);  // Small delay at the end of each sweep

  // Reverse direction for the next sweep
  direction = -direction;
}

bool anyOtherButtonPressed() {
  return (digitalRead(buttonCW) == LOW || digitalRead(buttonCCW) == LOW || digitalRead(buttonMPU) == LOW);
}

void controlWithMPU() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Calculate pitch angle in degrees (simplified calculation using accelerometer data)
  float pitch = atan2(ax, sqrt(ay * ay + az * az)) * 180 / PI;

  // Rotate the motor based on pitch angle
  if (pitch > 5) {    // Threshold to avoid minor vibrations
    stepper.move(1);  // Rotate clockwise for positive pitch
  } else if (pitch < -5) {
    stepper.move(-1);  // Rotate counterclockwise for negative pitch
  }

  stepper.run();

  // Stop MPU control if any button is pressed
  if (anyOtherButtonPressed()) {
    disableStepper();
    mpuControl = 0;
  }
}