#include <LiquidCrystal.h>

//initialize variables
int led = 13; //led pin output
int sensor = 3; //sensor pin
int flySensor = 5; //sensor pin 2
LiquidCrystal lcd(7, 8, 9, 10, 11, 12); //display pins
int wheel_val = 0;
int fly_val = 0;
int hallVal;
int flyRpm;
const float WHEEL_DIAMETER = 26.0;
const float WHEEL_CIRCUMFERENCE = 81.68;
const int MAX_MPH = 20;
float E;

const unsigned long sampleTime = 1000; //longest time for pin to be signaled


//set up board, pins, and display
void setup()
{
  Serial.begin(115200);
  lcd.begin(16, 2);
  pinMode(led, OUTPUT);
  pinMode(sensor, INPUT);
  //delay(1000);
  //lcd.clear();
}


void loop()
{
  //delay(100);

  //read from the sensor
  wheel_val = digitalRead(sensor);
  fly_val = digitalRead(flySensor);

  //get rpm and mph
  int rpm = getRPM(sensor);
  float mph = rpmToMph(rpm);
  int flyRpm = getRPM(flySensor);
  float rps = rpmToRps(flyRpm);
  E = getFlyEnergy(rps);

  //turns on led if sensor is triggered
  displayLed(sensor, wheel_val);
  displayLed(flySensor, fly_val);

  //display metrics
  //displayRPM(rpm);
  displayMPH(mph);

  displayFlywheelEnergy(E);
}

//calculates rpm
float getRPM(int sensor)
{
  int count = 0;
  boolean count_flag = LOW;
  unsigned long currentTime = 0;
  unsigned long startTime = millis();

  //while the sensor hasn't timed out loop through
  while (currentTime <= sampleTime)
  {
    //check if sensor has been triggered
    if (digitalRead(sensor) == HIGH)
    {
      count_flag = HIGH;
    }

    //if so update the count and flag
    if (digitalRead(sensor) == LOW && count_flag == HIGH)
    {
      count++;
      count_flag = LOW;
    }
    currentTime = millis() - startTime;
  }

  //calculates rpm
  float current_rpm = int(60000 / float(sampleTime)) * count;

  return current_rpm;
}

float getFlyEnergy(float rps)
{
  float k = 0.5; //inertial constant
  float M = 20.54; //mass
  float R = 0.0635; //flywheel radius  .0635 & .3302
  float I = k * M * (pow(R, 2)); //inertia
  float E = (.5) * I * (pow(rps, 2)); //energy equation
  return E;

}

//convert rpm to mph
float rpmToMph(int rpm)
{
  float inches_per_min = WHEEL_CIRCUMFERENCE * rpm;
  float inches_per_hour = inches_per_min * 60;
  float mph = inches_per_hour / 63360;
  return mph;
}

//convert rpm to rps (radians per second)
float rpmToRps(int flyRpm)
{
  float rps = flyRpm * 0.10472;
  return rps;
}

//displays RPM
void displayRPM(int rpm)
{
  //Serial.println(rpm,DEC);
  Serial.print("RPM = ");
  Serial.println(rpm);
}

//displays MPH
void displayMPH(float mph)
{
  //Serial.println(mph,DEC);
  //lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  Serial.print("MPH = ");
  Serial.println(mph);
  lcd.print("MPH: ");
  lcd.print(mph);

  //alerts user if they're approaching max speed
  if ( mph >= MAX_MPH)
  {
    lcd.begin(16, 2);
    lcd.setCursor(0, 0);
    Serial.println("Slow Down");
    lcd.print("Slow Down");
  }
}

//displays the energy stored in the flywheel
float displayFlywheelEnergy(float E)
{
  Serial.print("Energy Stored = ");
  Serial.println(E);
  //lcd.begin(16, 2);
  lcd.setCursor(0, 1);
  lcd.print("Joule Stored:");
  lcd.print(E);
}

//registers hall sensor and led
void displayLed(int sensor, int hallVal)
{
  //light LED when sensor detects magnet
  if (hallVal == LOW)
  {
    digitalWrite(led, HIGH);
  }
  else
  {
    digitalWrite(led, LOW);
  }
}