#include <Wire.h>

#include <AccelStepper.h>

#define PULSE_PIN  12 // Input signal connected to Pin 12 of Arduino

// Stepper Motor Pins
#define MP1  8 // IN1 on the ULN2003
#define MP2  9 // IN2 on the ULN2003
#define MP3  10 // IN3 on the ULN2003
#define MP4  11 // IN4 on the ULN2003

#define POSITION_0_MPH  -70
#define POSITION_120_MPH  70

#define REVERSE_LOCKOUT_MAX_MPH 2

AccelStepper stepper = AccelStepper(AccelStepper::FULL4WIRE, MP1, MP2, MP3, MP4);//Define the pin sequence (IN1-IN3-IN2-IN4)

const float STEPS_PER_MPH = ((float) POSITION_120_MPH - POSITION_0_MPH)/120;

#include <LiquidCrystal_I2C.h>

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);



const int SPEEDO_UPDATE_MS = 300; // Update speedo every x ms
const int MIN_DELAY_MS = 50; // If reading speed takes >(delay ms - minDelay ms), default to a delay of minDelayMs

const int PULSES_PER_MILE = 30000;

bool lockReverse = false;

long ticks = 0;

int currentPin = 6;

int pins[] = {2, 3, 6};

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  lcd.print("start");




  updateReverseLockout();
  stepper.setMaxSpeed(1000);//Set the maximum motor speed in steps per second
  stepper.setAcceleration(333);//Set the maximum acceleration in steps per second^2///

  // DEBUG generate pulse signal
  analogWrite(currentPin , 127);
  pinMode(13, OUTPUT);

  Serial.println("Starting shizz.");
}

// Reads instantaneous frequency - too variable/inaccurate?
float readFrequencyInstant() {
  int pulseHigh = pulseIn(PULSE_PIN, HIGH);

  if(!pulseHigh) {
    return 0;
  }

  int pulseLow = pulseIn(PULSE_PIN, LOW);

  float pulseTotal = pulseHigh + pulseLow; // Time period of the pulse in microseconds

  return (1000000 / pulseTotal); // Frequency in Hertz (Hz) = times per second
}

float frequencyToMph(float frequency) {
  return 3600 * frequency/PULSES_PER_MILE; // 3600 * (pulses/second) / (pulses/mile) -> miles/second * 3600 -> miles/hour
}

void maybeUpdateReverseLockout(bool shouldLock) {
  if (shouldLock != lockReverse) {
    lockReverse = shouldLock;
    updateReverseLockout();
  }
}

void updateReverseLockout() {
  if (lockReverse) {
    digitalWrite(13, HIGH);
  } else {
    digitalWrite(13, LOW);
  }
}

int mphToStepperPosition(float mph) {
  int stepsFromZero = mph * STEPS_PER_MPH;
  return POSITION_0_MPH + stepsFromZero;
}

void setMph(float mph) {
  lcd.clear();
  lcd.print(mph);
  maybeUpdateReverseLockout((mph <= REVERSE_LOCKOUT_MAX_MPH));
  stepper.moveTo(mphToStepperPosition(mph));
  stepper.runToPosition();
}

void loop() {
  ticks++;
  long int start = millis();
  float frequency = readFrequencyInstant();
  float mph = frequencyToMph(frequency);
  setMph(mph);
  long int end = millis();
  if(!(ticks % 10)) {
    Serial.print("Total time to get a signal from pin #");
    Serial.print(currentPin);
    Serial.print(" : ");
    Serial.println(((float)(end-start)/1000), 3);
    analogWrite(currentPin, 0);
    currentPin = pins[rand() % (3)];
    Serial.print("Choosing new pin: ");
    Serial.println(currentPin);
    analogWrite(currentPin, 127);
  }
  delay(max(MIN_DELAY_MS, SPEEDO_UPDATE_MS - (end - start))); // Probably want to see how long end-start actually takes
}