#define ANALOG_PIN A15

const int LED_CW = 2;
const int LED_CCW = 23;
const int LED_STOP = 22;

//connections to ultrasonic sensor
const int TRIG_PIN_M1 = 6;
const int ECHO_PIN_M1 = 5;
const int TRIG_PIN_M2 = 4;
const int ECHO_PIN_M2 = 3;

// Connections to motordriver
const int STEP_M1 = 13; // Step
const int DIR_M1 = 12;  // Direction
const int STEP_M2 = 11; // Step
const int DIR_M2 = 10;

//buttons
const int BUTTON_CW = 9;
const int BUTTON_CCW = 8;
const int BUTTON_STOP = 7;

// Motor steps per rotation
const int STEPS_PER_REV = 200;
const int MAX_SPEED_M1 = 1000;
 
//motor state
bool motorRunning;
bool motorCW;
bool motorCCW;

//motor run time
unsigned long previousMotor1Time = millis();
unsigned long previousMotor2Time = millis();

//motor delay
long Motor1Interval;
long Motor2Interval;

//ultrasonic sensor 
long duration1;
double currentDistance1; 
double distanceSet1;
long duration2;
double currentDistance2; 
double distanceSet2;

//sensor run time
unsigned long previousSensor1Time1 = micros();
unsigned long previousSensor2Time1 = micros();

unsigned long previousSensor1Time2 = micros();
unsigned long previousSensor2Time2 = micros();

//speed-distance values
double Diameter1;
double Diameter2;

double speed;
double speedValue;

double RPM1;
double RPM2;

double RotationTime1;
double RotationTime2;

double StepTime1;
double StepTime2;

void setup() {

  Serial.begin(9600);

  // Setup the pinsnand buttons
  pinMode(STEP_M1,OUTPUT); 
  pinMode(DIR_M1,OUTPUT);
  pinMode(STEP_M2,OUTPUT);
  pinMode(DIR_M2,OUTPUT);
  

  pinMode(BUTTON_STOP,INPUT); 
  pinMode(BUTTON_CCW,INPUT); 
  pinMode(BUTTON_CW,INPUT);

  pinMode(TRIG_PIN_M1, OUTPUT);
  pinMode(ECHO_PIN_M1, INPUT);
  pinMode(TRIG_PIN_M2, OUTPUT);
  pinMode(ECHO_PIN_M2, INPUT);

  pinMode(LED_CW, OUTPUT);
  pinMode(LED_CCW, OUTPUT);
  pinMode(LED_STOP, OUTPUT);

  //set motor state to false
  motorRunning = false;
  motorCW = false;
  motorCCW = false;

  digitalWrite(LED_STOP, HIGH);
  digitalWrite(LED_CW, LOW);
  digitalWrite(LED_CCW, LOW);

}
void loop() {
  
  //ULTRASONIC SENSOR

  //time in micros while running
  unsigned long currentSensor1Time1 = micros();
  unsigned long currentSensor2Time1 = micros();

  unsigned long currentSensor1Time2 = micros();
  unsigned long currentSensor2Time2 = micros();

  //sensor motor 1
  digitalWrite(TRIG_PIN_M1, LOW);

  if (currentSensor1Time1 - previousSensor1Time1 > 2) {
  digitalWrite(TRIG_PIN_M1, HIGH);
  
    if (currentSensor1Time2 - previousSensor1Time2 > 10) {
    digitalWrite(TRIG_PIN_M1, LOW);

   }
  }
  
  duration1 = pulseIn(ECHO_PIN_M1, HIGH);
  currentDistance1 = duration1*0.034/2;

  //sensor motor 2
  digitalWrite(TRIG_PIN_M2, LOW);

  if (currentSensor2Time1 - previousSensor1Time1 > 2) {
  digitalWrite(TRIG_PIN_M2, HIGH);
    
    if (currentSensor2Time2 - previousSensor1Time2 > 10) {
    digitalWrite(TRIG_PIN_M2, LOW);

    }
  }
  
  duration2 = pulseIn(ECHO_PIN_M2, HIGH);
  currentDistance2 = duration2*0.034/2;

  //print distances 
  Serial.print(" Distance: ");
  Serial.println(currentDistance1);

  Diameter1 = 23.8 - currentDistance1*2; //sensor is placed 3cm from tags
  Diameter2 = 23.8 - currentDistance2*2; //sensor is placed 3cm from tags

  Serial.print(" D1: ");
  Serial.print(Diameter1);

  //SPEED DETERMINATION

  //read potmeter value
  double potValue = analogRead(A15);
  speedValue = map(potValue, 0, 1023, 1, 10);

  speed = speedValue/100;
  
  Serial.print(" speed: ");
  Serial.print(speed);
  Serial.print(" m/s ");

  //calculate the RPM
  RPM1 = (60*speed)/(PI*(Diameter1/100));
  RPM2 = (60*speed)/(PI*(Diameter2/100));

  Serial.print(" RPM: ");
  Serial.print(RPM1);

  //calculate time per revolution
  RotationTime1 = 60*pow(10, 3)/RPM1;
  RotationTime2 = 60*pow(10, 3)/RPM2;

  StepTime1 = RotationTime1/STEPS_PER_REV;
  StepTime2 = RotationTime2/STEPS_PER_REV;

  Motor1Interval = long(StepTime1);
  Motor2Interval = long(StepTime2);

  Serial.print(" M1 Int: ");
  Serial.print(Motor1Interval);

  if (motorRunning == false) {
    
    distanceSet1 = currentDistance1;
  }

  if (digitalRead(BUTTON_CW) == HIGH && motorRunning == false) {

    //motor is running
    motorRunning = true;
    
    // Set motor direction clockwise
    digitalWrite(DIR_M1, HIGH);
    digitalWrite(DIR_M2, HIGH);

    digitalWrite(LED_CW, HIGH);
    digitalWrite(LED_STOP, LOW);
    digitalWrite(LED_CCW, LOW);

  }

  else if (digitalRead(BUTTON_CCW) == HIGH && motorRunning == false) {
   
    //motor is running
    motorRunning = true;

    // Set motor direction clockwise
    digitalWrite(DIR_M1, LOW);
    digitalWrite(DIR_M2, LOW);

    digitalWrite(LED_CCW, HIGH);
    digitalWrite(LED_STOP, LOW);
    digitalWrite(LED_CW, LOW);

  }

  else if (digitalRead(BUTTON_STOP) == HIGH || Diameter1 < 9.6 ) {

    digitalWrite(LED_STOP, HIGH);
    digitalWrite(LED_CW, LOW);
    digitalWrite(LED_CCW, LOW);
    motorRunning = false;
  }
  
  if (motorRunning == true) {

    unsigned long currentMotor1Time = millis();
    unsigned long currentMotor2Time = millis();

    digitalWrite(STEP_M1,LOW);
    digitalWrite(STEP_M2,LOW);

    if (currentMotor1Time - previousMotor1Time > Motor1Interval) {
      digitalWrite(STEP_M1, HIGH);

      previousMotor1Time = currentMotor1Time;

    }

    if (currentMotor2Time - previousMotor2Time > Motor2Interval) {
      digitalWrite(STEP_M2, HIGH);
      
      previousMotor2Time = currentMotor2Time;

    }
    

  }
}