#include <LiquidCrystal.h>

#define ANALOG_PIN A15

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

// ultrasonic sensor variables
long duration1;
long duration2;

// sensor timing variables
unsigned long trigTime1 = 0;
unsigned long echoStartTime1 = 0;
unsigned long echoEndTime1 = 0;
bool isWaitingForEcho1 = false;

unsigned long trigTime2 = 0;
unsigned long echoStartTime2 = 0;
unsigned long echoEndTime2 = 0;
bool isWaitingForEcho2 = false;

double currentDistance1;
double currentDistance2;

// LCD
int Contrast = 60;
LiquidCrystal lcd(32, 31, 24, 25, 26, 27);

// LED pins
const int LED_CW = 2;
const int LED_CCW = 23;
const int LED_STOP = 22;

// button pins
const int BUTTON_CW = 9;
const int BUTTON_CCW = 8;
const int BUTTON_STOP = 7;

// motor driver pins
const int STEP_M1 = 13;
const int DIR_M1 = 12;
const int STEP_M2 = 11;
const int DIR_M2 = 10;

// motor state
bool motorRunning = false;
bool motorCW = false;
bool motorCCW = false;

// motor run time
unsigned long previousMotor1Time = 0;
unsigned long previousMotor2Time = 0;

// motor speed
double speedValue;

void setup() {
  // sensor setup
  pinMode(TRIG_PIN_M1, OUTPUT);
  pinMode(ECHO_PIN_M1, INPUT);
  pinMode(TRIG_PIN_M2, OUTPUT);
  pinMode(ECHO_PIN_M2, INPUT);

  // LCD setup
  analogWrite(36, Contrast);
  lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  lcd.print("Speed : ");

  // motor driver setup
  pinMode(STEP_M1, OUTPUT);
  pinMode(DIR_M1, OUTPUT);
  pinMode(STEP_M2, OUTPUT);
  pinMode(DIR_M2, OUTPUT);

  // button setup
  pinMode(BUTTON_STOP, INPUT);
  pinMode(BUTTON_CCW, INPUT);
  pinMode(BUTTON_CW, INPUT);

  // LED setup
  pinMode(LED_CW, OUTPUT);
  pinMode(LED_CCW, OUTPUT);
  pinMode(LED_STOP, OUTPUT);

  // set LED starting state
  digitalWrite(LED_STOP, HIGH);
  digitalWrite(LED_CW, LOW);
  digitalWrite(LED_CCW, LOW);
}

void loop() {
  // LCD print speed
  lcd.setCursor(8, 0);
  lcd.print(speedValue);

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

  // time in microseconds
  unsigned long currentTime = micros();

  // sensor motor 1
  if (!isWaitingForEcho1) {
    digitalWrite(TRIG_PIN_M1, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN_M1, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN_M1, LOW);
    trigTime1 = micros();
    isWaitingForEcho1 = true;
  } else {
    if (digitalRead(ECHO_PIN_M1) == HIGH && echoStartTime1 == 0) {
      echoStartTime1 = micros();
    }
    if (digitalRead(ECHO_PIN_M1) == LOW && echoStartTime1 != 0) {
      echoEndTime1 = micros();
      duration1 = echoEndTime1 - echoStartTime1;
      currentDistance1 = duration1 * 0.034 / 2;
      isWaitingForEcho1 = false;
      echoStartTime1 = 0;
    }
  }

  // sensor motor 2
  if (!isWaitingForEcho2) {
    digitalWrite(TRIG_PIN_M2, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN_M2, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN_M2, LOW);
    trigTime2 = micros();
    isWaitingForEcho2 = true;
  } else {
    if (digitalRead(ECHO_PIN_M2) == HIGH && echoStartTime2 == 0) {
      echoStartTime2 = micros();
    }
    if (digitalRead(ECHO_PIN_M2) == LOW && echoStartTime2 != 0) {
      echoEndTime2 = micros();
      duration2 = echoEndTime2 - echoStartTime2;
      currentDistance2 = duration2 * 0.034 / 2;
      isWaitingForEcho2 = false;
      echoStartTime2 = 0;
    }
  }

  // buttons
  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 counterclockwise
    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) {
    digitalWrite(LED_STOP, HIGH);
    digitalWrite(LED_CW, LOW);
    digitalWrite(LED_CCW, LOW);
    motorRunning = false;
  }

  if (motorRunning == true) {
    unsigned long currentMotor1Time = micros();
    unsigned long currentMotor2Time = micros();

    if (currentMotor1Time - previousMotor1Time > speedValue) {
      digitalWrite(STEP_M1, HIGH);
      delayMicroseconds(1); // Add a short delay to create a pulse
      digitalWrite(STEP_M1, LOW);
      previousMotor1Time = currentMotor1Time;
    }

    if (currentMotor2Time - previousMotor2Time > speedValue) {
      digitalWrite(STEP_M2, HIGH);
      delayMicroseconds(1); // Add a short delay to create a pulse
      digitalWrite(STEP_M2, LOW);
      previousMotor2Time = currentMotor2Time;
    }
  }
}