#include <AccelStepper.h>

#define STEP_PIN 22
#define DIR_PIN 19
#define EN_PIN 23
#define BTN_PIN 18

AccelStepper st(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// Distance you want the vehicle to travel
float dist_m = 8.0f;

// Wheel + motor setup
float wheel_d_mm = 63.91f;
int full_steps_per_rev = 200;
int microsteps = 16;

// Target time for the move
float T_target_s = 10.0f;

// Acceleration in steps/sec^2
float accel_stepsps2 = 8000.0f;

/*
Solves for the max speed needed to travel a certain number
of steps in exactly T seconds using a trapezoidal profile.

Motion equation used:
  T = D/v + v/a

Rearranged into:
  (1/a)v^2 - T*v + D = 0

Then quadratic formula is used to solve for v.
*/
static bool solve_vmax(long d_steps, float T, float a, float &vmax_out) {
  if (T <= 0.0f || a <= 0.0f) return false;

  // Make distance positive for the calculation
  double d = (double)((d_steps >= 0) ? d_steps : -d_steps);

  double A = 1.0 / (double)a;
  double B = -(double)T;
  double C = d;

  // Quadratic discriminant
  double disc = B * B - 4.0 * A * C;

  // If negative, the requested time is physically impossible
  // with the acceleration that was chosen.
  if (disc < 0.0) return false;

  double sqrt_disc = sqrt(disc);

  // Two possible quadratic solutions
  double v1 = (-B - sqrt_disc) / (2.0 * A);
  double v2 = (-B + sqrt_disc) / (2.0 * A);

  // Normally the smaller positive root is what we want
  double v = v1;

  if (v <= 0.0) v = v2;
  if (v <= 0.0) return false;

  vmax_out = (float)v;
  return true;
}

/*
Travels a specified distance in METERS.

The function:
1. Converts wheel diameter to meters
2. Calculates wheel circumference
3. Calculates microsteps per wheel revolution
4. Calculates steps per meter
5. Converts requested distance into steps
6. Calculates the required AccelStepper max speed
7. Runs the motor using AccelStepper
*/
void runMoveTimedMeters(float meters) {
  // Convert wheel diameter mm -> meters
  float wheel_d_m = wheel_d_mm / 1000.0f;

  // Wheel circumference
  float circ_m = 3.14159265358979323846f * wheel_d_m;

  // Total microsteps per wheel revolution
  float microsteps_per_rev = (float)(full_steps_per_rev * microsteps);

  // Calculate how many step pulses equal one meter
  float steps_per_meter = microsteps_per_rev / circ_m;

  // Convert requested travel distance into steps
  long steps = lroundf(meters * steps_per_meter);

  // This will contain the calculated peak speed
  float vmax = 0.0f;

  /*
  Calculate the exact max speed required so that:
    distance = steps
    acceleration = accel_stepsps2
    total time = T_target_s
  */
  bool ok = solve_vmax(steps, T_target_s, accel_stepsps2, vmax);

  /*
  If the requested time is impossible with the selected
  acceleration, use average speed as a fallback.
  */
  if (!ok) {
    float d = (float)((steps >= 0) ? steps : -steps);
    vmax = d / T_target_s;

    // Prevent extremely slow speeds
    if (vmax < 50.0f)
      vmax = 50.0f;
  }

  // Give AccelStepper the calculated profile
  st.setMaxSpeed(vmax);
  st.setAcceleration(accel_stepsps2);

  // Reset position so this move starts at 0
  st.setCurrentPosition(0);

  // Tell AccelStepper how many steps to move
  st.move(steps);

  // Continue calling run() until movement is complete
  while (st.distanceToGo() != 0) {
    st.run();
  }
}

void setup() {
  // Enable stepper driver
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);

  // Start button
  pinMode(BTN_PIN, INPUT_PULLUP);

  // Reset position
  st.setCurrentPosition(0);
}

void loop() {
  static int lastBtn = HIGH;
  int btn = digitalRead(BTN_PIN);

  /*
  Detect button press edge:
  HIGH -> LOW
  */
  if (lastBtn == HIGH && btn == LOW) {
    // Run the vehicle for the specified distance
    runMoveTimedMeters(dist_m);

    // Simple debounce
    delay(300);
  }

  lastBtn = btn;
}