// ********** MPU6050 Definitions *******************
#include <Wire.h> // Comm with the gyro
#include <MPU6050_light.h> // Accelerometer / gyroscope

// *** Stepper motor/driver dependencies ************
#include <AccelStepper.h> // Stepper lib to drive motors
#include <MultiStepper.h> // Manage multiple motors

// *** Setup each stepper motor *********************
AccelStepper stepper1(AccelStepper::FULL4WIRE, 30, 32, 31, 33);
AccelStepper stepper2(AccelStepper::FULL4WIRE, 34, 36, 35, 37);
AccelStepper stepper3(AccelStepper::FULL4WIRE, 38, 40, 39, 41);
AccelStepper stepper4(AccelStepper::FULL4WIRE, 42, 44, 43, 45);
AccelStepper stepper5(AccelStepper::FULL4WIRE, 46, 48, 47, 49);
AccelStepper stepper6(AccelStepper::FULL4WIRE, 50, 52, 51, 53);

// Up to 10 steppers can be handled as a group by MultiStepper
MPU6050 mpu(Wire);   //Create object mpu
#define zeroAdj 7
unsigned long timer = 0;

MultiStepper steppers;
int iterations = 0;
long delta = 10;
long adelta = 50;
long ddelta = 100;
long positions[6];  // Array of desired stepper positions
float lastXAxisAngle = 0.0;

// **************************************************

void setup() {
  initializeGyro();
  initializeMotors();
}

void loop() {
  printGyroAxes();
  moveMotors();
}

void initializeGyro(){
  pinMode(zeroAdj, INPUT_PULLUP);
  Serial.begin(9600);

  Wire.begin();     
  byte status = mpu.begin();     
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  Serial.println(F("Calculating offsets, do not move MPU6050"));   
  mpu.calcOffsets();     //Calibrate gyroscope
  delay(1000);
  Serial.println("Done!\n");
}

void initializeMotors(){

  // Configure each stepper
  stepper1.setMaxSpeed(300);
  stepper2.setMaxSpeed(300);
  stepper3.setMaxSpeed(300);
  stepper4.setMaxSpeed(300);
  stepper5.setMaxSpeed(300);
  stepper6.setMaxSpeed(300);
  
  stepper1.setSpeed(100);
  stepper2.setSpeed(100);
  stepper3.setSpeed(100);
  stepper4.setSpeed(100);
  stepper5.setSpeed(100);
  stepper6.setSpeed(100);

  // Then give them to MultiStepper to manage
  steppers.addStepper(stepper1);
  steppers.addStepper(stepper2);
  steppers.addStepper(stepper3);
  steppers.addStepper(stepper4);
  steppers.addStepper(stepper5);
  steppers.addStepper(stepper6);

  positions[0] = delta;
  positions[1] = delta;
  positions[2] = delta;
  positions[3] = delta;
  positions[4] = delta;
  positions[5] = delta;  
}

void printGyroAxes(){
  mpu.update();    //Get values from MPU

  //if ((millis() - timer) > 100) { // print data every 100ms
    timer = millis();
    Serial.print("X : ");
    lastXAxisAngle = mpu.getAngleX();
    Serial.print(lastXAxisAngle);
    Serial.print("\tY : ");
    Serial.print(mpu.getAngleY());
    Serial.print("\tZ : ");
    Serial.println(mpu.getAngleZ());
    //delay(10);
  //}

  if(digitalRead(zeroAdj) == 0){
    Serial.println("Calibrating MPU6050");
    delay(1000);
    mpu.calcOffsets();     //Calibrate gyroscope;
  }
}

void moveMotors(){
  
  if (lastXAxisAngle < -45){
    Serial.print("lastXAxisAngle: ");
    Serial.println(lastXAxisAngle);
    return;
  } else if (lastXAxisAngle > 45){
    delta = delta > 0 ? adelta : -adelta;
  } else {
    delta = delta > 0 ? ddelta : -ddelta;
  }
  Serial.print("delta: ");
  Serial.println(delta);

  iterations += 1;

  Serial.print("iterations: ");
  Serial.println(iterations);

  if (iterations % 2 == 0) {
    steppers.moveTo(positions);
    steppers.runSpeedToPosition();  // Blocks until all are in position
    delta = -delta;
  } else {
    // Move to a different coordinate
    positions[0] = delta;
    positions[1] = delta;
    positions[2] = delta;
    positions[3] = delta;
    positions[4] = delta;
    positions[5] = delta;

    steppers.moveTo(positions);
    steppers.runSpeedToPosition();  // Blocks until all are in position
  }

  //Serial.print("position[0]");
  //Serial.println(positions[0]);
  delay(200);
}