// ____________________IMPORT LIBRARY____________________ //
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <Servo.h>
#include <Wire.h>


// ____________________PIN DEFINITION____________________ //
// define stepper motor pin
#define motorPin1_1  8     // IN1
#define motorPin1_2  9     // IN2
#define motorPin1_3  10    // IN3
#define motorPin1_4  11    // IN4
#define motorPin2_1  52    // IN1 
#define motorPin2_2  50    // IN2
#define motorPin2_3  48    // IN3
#define motorPin2_4  46    // IN4

// define servo motor pin
#define servoPin_1  13
#define servoPin_2  12

// define trig and echo pin:
#define trigPin_1 24
#define echoPin_1 25
#define trigPin_2 22
#define echoPin_2 23


// ____________________MOTORS SETUP____________________ //
// servo motor set up
Servo servo_1;
Servo servo_2;

// stepper motor set up
AccelStepper stepper_1(AccelStepper::FULL4WIRE, motorPin1_1, motorPin1_3, motorPin1_2, motorPin1_4);
AccelStepper stepper_2(AccelStepper::FULL4WIRE, motorPin2_1, motorPin2_3, motorPin2_2, motorPin2_4);
MultiStepper steppers;


// ____________________DEFINE VARIABLES____________________ //
// define accelerometer settings
const int MPU=0x68; 
int16_t AcX;
int tiltingValue = 1500;

// define ultrasonic variables
long duration;
int distance;
int avoidDistance = 30;
float temp_a;
float temp_b;
int temp_rnd;

// define stepper motor variables
long stepperPositions[2] = {0, 0}; // Array of desired stepper positions
int speed_1;
int speed_2;
int standardSpeed = 10;
int stepperDir = 1;
int extendDistance = 1ㄓ00;


// ____________________MAIN SETUP____________________ // *Run Once
void setup() {
  // accelerometer set up
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  // basic set up
  Serial.begin(9600);

  // Ultrasonic Set Up
  pinMode(trigPin_1, OUTPUT);
  pinMode(echoPin_1, INPUT);
  pinMode(trigPin_2, OUTPUT);
  pinMode(echoPin_2, INPUT);

  // servo pin set up
  servo_1.attach(servoPin_1);
  servo_2.attach(servoPin_2);

  // configure each stepper
  stepper_1.setMaxSpeed(300);
  stepper_2.setMaxSpeed(300);

  // then give them to multiStepper to manage
  steppers.addStepper(stepper_1);
  steppers.addStepper(stepper_2);
}


// ____________________MAIN SCRIPT____________________ // *Continuous Looping
void loop() {
  // set servo to initial position
  ServoMove(true, 90);
  ServoMove(false, 90);

  // if it tilt too far then change direction
  if (stepperDir == 1){
    if (AccelerometerRead() > tiltingValue){
      stepperDir = -1;}
     }
  else if (stepperDir == -1){
    if (AccelerometerRead() < -tiltingValue){
      stepperDir = 1;}
     }

  // if the obstacle is too close then turn it's head
  if (stepperDir == 1){ // if move forward check front sensor
    if (UltrasonicRun(trigPin_1, echoPin_1) < avoidDistance){
      UltrasonicChangeDirection(true, trigPin_1, echoPin_1, speed_1, speed_2, stepperDir, extendDistance);
      }
    }else if (stepperDir == -1){ // if move backward check back sensor
    if (UltrasonicRun(trigPin_2, echoPin_2) < avoidDistance){
      UltrasonicChangeDirection(false, trigPin_2, echoPin_2, speed_1, speed_2, stepperDir, extendDistance);
      }
    }

  // regular moving set up
  speed_1 += standardSpeed*stepperDir;
  speed_2 += standardSpeed*stepperDir;
  StepperMove(speed_1, speed_2);
 }


// ____________________FUNCTIONS____________________ //

float AccelerometerRead(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true);   
  AcX=Wire.read()<<8|Wire.read(); // AcX is the value coming from tilting angle
  Serial.println(AcX);
  return AcX;
  }

float UltrasonicRun(int trigPin, int echoPin) {
  // Clear the trigPin by setting it LOW:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  
  // Trigger the sensor by setting the trigPin high for 10 microseconds:
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
  duration = pulseIn(echoPin, HIGH);
  // Calculate the distance:
  distance = duration * 0.034 / 2; // distance in cm
  return distance;
  }

void UltrasonicChangeDirection(bool servoDir, int trigPin, int echoPin, int speed_1, int speed_2, int stepperDir, int stepperDistance){
    StepperMove(0, 0); // stop moving
    ServoMove(servoDir, 0);
    temp_a = UltrasonicRun(trigPin, echoPin);
    delay(500);
    ServoMove(servoDir, 180);
    temp_b = UltrasonicRun(trigPin, echoPin);
    delay(500);
    if (temp_a > temp_b){
      ServoMove(servoDir, 0);
    }else if(temp_b > temp_a){
      ServoMove(servoDir, 180);
    }else if(temp_a == temp_b){
      temp_rnd = random(0, 2);
      if (temp_rnd == 0){
        ServoMove(servoDir, 0);
      }else if (temp_rnd == 1){
        ServoMove(servoDir, 180);
      }
    }
    StepperMove((speed_1+stepperDistance)*stepperDir, (speed_2+stepperDistance)*stepperDir);
    delay(200);
    Serial.println(temp_a);
    Serial.println(temp_b);
    }

void ServoMove(bool servoDir, int degree){
  if (servoDir == true){
    servo_1.write(degree);}
  else{
    servo_2.write(degree);}
  }

void StepperMove(int speed_1, int speed_2){
  stepperPositions[0] += speed_1;
  stepperPositions[1] -= speed_2;
  steppers.moveTo(stepperPositions);
  steppers.runSpeedToPosition(); // Blocks until all are in position
  }