#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <FastAccelStepper.h>
#include <ezButton.h>

// Stepper pins
#define ENABLE_PIN 7
#define DIR_PIN 8
#define STEP_PIN 9
#define BUZZER_PIN 10 // Buzzer pin
#define TRIGGER_PIN 12 // HC-SR04 trigger pin
#define ECHO_PIN 13    // HC-SR04 echo pin

#define MAX_SPEED 30000 // in steps per second
#define ACCELERATION 5000
#define DEFAULT_SPEED 200 // default stepper speed in steps per second
#define SPEED_INC 100     // speed increment size in steps per second
#define DISTANCE_THRESHOLD 10 // Distance threshold for triggering the buzzer

// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);

ezButton incSpeedButton(5);
ezButton decSpeedButton(4);
ezButton startButton(3);
ezButton stopButton(6);

/******************************************
 * Motor
 */
long target_speed = DEFAULT_SPEED;
bool running = false;

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

/******************************************
 * Ultrasonic Sensor
 */
long distance = 0;

void setup()
{
  Serial.begin(115200);
  Serial.println("\nHello Step World!\n");

  // lcd
  lcd.init();
  lcd.backlight();

  lcd.setCursor(0, 0);
  lcd.print("Spindle C");
  lcd.setCursor(0, 1);
  lcd.print("Speed");

  engine.init();
  stepper = engine.stepperConnectToPin(STEP_PIN);
  if (stepper) {
    stepper->setDirectionPin(DIR_PIN);
    stepper->setEnablePin(ENABLE_PIN);
    stepper->setAutoEnable(true);

    stepper->setSpeedInHz(target_speed);
    stepper->setAcceleration(ACCELERATION);

    // ezButton for real buttons
    incSpeedButton.setDebounceTime(15);
    decSpeedButton.setDebounceTime(15);
    startButton.setDebounceTime(15);
    stopButton.setDebounceTime(15);

    // Buzzer initialization
    pinMode(BUZZER_PIN, OUTPUT);

    // HC-SR04 initialization
    pinMode(TRIGGER_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
  }
}

void loop()


{
  incSpeedButton.loop();
  decSpeedButton.loop();
  startButton.loop();
  stopButton.loop();

  // Read distance from HC-SR04
  distance = readDistance();
    delay(10);
    lcd.setCursor(0, 0);
    lcd.print("Distance = ");
    lcd.print(distance);
    lcd.setCursor(0, 1);
    lcd.print("Speed = ");
    lcd.print(target_speed);

  if (running) {
    // The motor is running, check if the user wants to stop it
    if (stopButton.isPressed() || distance < DISTANCE_THRESHOLD) {
      running = false;
      stepper->stopMove();
      notifyButtonPress();
    }
  } else {
    // The motor is not running, allow the user to set the target speed
    updateTargetSpeed();

    if (startButton.isPressed() && distance > DISTANCE_THRESHOLD) {
      running = true;
      lcd.clear();
      lcd.setCursor(0, 1);
      lcd.print("RPM = ");
      lcd.print(target_speed);
      stepper->runForward();
      notifyButtonPress();
    }
  }

  delay(50);
}

void updateTargetSpeed()
{
  static bool update; // need to update?

  if (incSpeedButton.isPressed() && target_speed < 1320)
  {
    target_speed += SPEED_INC;
    update = true;
  }
  else if (decSpeedButton.isPressed() && target_speed > 0)
  {
    target_speed -= SPEED_INC;
    update = true;
  }

  if (update)
  {
    notifyButtonPress();
    update = false;
  }
}

void notifyButtonPress()
{
  buzz(50); // Buzz for 50 milliseconds as a notification for button press
}

void buzz(int duration)
{
  digitalWrite(BUZZER_PIN, HIGH);
  delay(duration);
  digitalWrite(BUZZER_PIN, LOW);
}

long readDistance()
{
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  long cm = (duration / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343 for distance in inches

  return cm;
}