#include <SpeedyStepper.h>
#include <Wire.h>

#define DIR_LEFT  1   //PA5
#define DIR_RIGHT 2   //PA6
#define DIR_FRONT 3   //PA7

#define STEP_LEFT  8  //PA1
#define STEP_RIGHT 9  //PA2
#define STEP_FRONT 10 //PA3

#define EN 0 //PA4

#define MICROSTEP 1     // microstepping modifier
#define FULL_CYCLE 200  // how many steps to fufil full rotation of stepper in full-step

#define CIRCUMFERENCE 188.4 // pi * d, diameter of wheel is 60mm

#define SPEED 1200 // target speed is 0.3m/s

#define THRESHOLD 5 // if less than 5cm, dangerous

float left = 0, right = 0, front = 0;
int distances[] = {60, 60, 60};

SpeedyStepper stepper_left, stepper_right, stepper_front;

void getNewLocation() {
  left = Serial.parseFloat();
  right = Serial.parseFloat();
  front = Serial.parseFloat();
  while (Serial.available()) {
    Serial.read(); // clear buffer
  }

  // set new position
  stepper_left.setupRelativeMoveInMillimeters(left);
  stepper_right.setupRelativeMoveInMillimeters(right);
  stepper_front.setupRelativeMoveInMillimeters(front);

  float absLeft = left > 0 ? left : -left;
  float absRight = right > 0 ? right : - right;
  float absFront = front > 0 ? front : -front;

  float expectedTime = max(absLeft, max(absRight, absFront))/SPEED;

  // set speed of steppers
  stepper_left.setSpeedInMillimetersPerSecond(absLeft / expectedTime);
  stepper_right.setSpeedInMillimetersPerSecond(absRight / expectedTime);
  stepper_front.setSpeedInMillimetersPerSecond(absFront / expectedTime);

  digitalWrite(EN, LOW);
}

//void checkUltrasound(pin) {
//  return;
//}

void moveToLocation() {
  digitalWrite(EN, LOW);
  while (!stepper_left.motionComplete() ||
         !stepper_right.motionComplete() ||
         !stepper_front.motionComplete()) {
    stepper_left.processMovement();
    stepper_right.processMovement();
    stepper_front.processMovement();
        if (2 == Wire.requestFrom(0x10, 2)) {
          while (Wire.available()) {
            int index = Wire.read();
            int distance = Wire.read();
            distances[index] = distance;
            if (distance < THRESHOLD) {
              digitalWrite(EN, HIGH);
            }
          }
        }
  }
}

void setup() {
  // initialize ATTiny Stuff
  //  Serial.swap(1);
  Serial.begin(115200);

  //set enable pin
  pinMode(EN, OUTPUT);
  digitalWrite(EN, HIGH); // active low

  // initialize Stepper
  stepper_left.connectToPins(STEP_LEFT, DIR_LEFT);
  stepper_right.connectToPins(STEP_RIGHT, DIR_RIGHT);
  stepper_front.connectToPins(STEP_FRONT, DIR_FRONT);

  // set steps per mm
  float steps = (FULL_CYCLE * MICROSTEP) * 1.0 / CIRCUMFERENCE;
  stepper_left.setStepsPerMillimeter(steps);
  stepper_right.setStepsPerMillimeter(steps);
  stepper_front.setStepsPerMillimeter(steps);

  // set speed of steppers
  stepper_left.setSpeedInMillimetersPerSecond(SPEED);
  stepper_right.setSpeedInMillimetersPerSecond(SPEED);
  stepper_front.setSpeedInMillimetersPerSecond(SPEED);

  // set acceleration of steppers, we set as the target speed so that robot can immediately
  // ramp up to that speed
  stepper_left.setAccelerationInMillimetersPerSecondPerSecond(SPEED);
  stepper_right.setAccelerationInMillimetersPerSecondPerSecond(SPEED);
  stepper_front.setAccelerationInMillimetersPerSecondPerSecond(SPEED);
 
  Wire.begin();
  digitalWrite(EN, LOW);
}

void loop() {
  // put your main code here, to run repeatedly:
  if (Serial.available()) {
    getNewLocation();
    moveToLocation();
  } else {
    delay(100);
    digitalWrite(EN, HIGH);
  }
}