boolean toggle1 = 0;                                                // a value to toggle on and off to drive the stepper
const int interval = 200;                                           // the interval over which to get each new speed value
unsigned previousMillis = 0;                                        // will store the last time the speed value was updated
const int numReadings = 30;                                         // the number of readings to be averaged out
int RPM = 0;                                                        // a single RPM reading
int RPMArrayIndex = 0;                                              // position within the array
int RPMArray[numReadings];                                          // an array of RPM readings
int RPMTotal = 0;                                                   // running total of the last numReadings RPM values
int averageRPM = 0;                                                 // the average of all numReadings values in the array
int toggleFrequency = 1;                                            // default frequency at which to toggle the output

#include "src/LiquidCrystal_I2C/LiquidCrystal_I2C.h"

LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {

  Serial.begin(9600);

  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);

  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    RPMArray[thisReading] = 0;                                      // set every reading in the array to 0
  }

  lcd.init();                                                       // initialize the LCD panel
  lcd.backlight();                                                  // turn the LCD backlight on

  cli();                                                            // stop interrupts

  TCCR1A = 0;                                                       // set entire TCCR1A register to 0
  TCCR1B = 0;                                                       // same for TCCR1B
  TCNT1  = 0;                                                       // initialize counter value to 0
  OCR1A = 233;                                                      // set compare match register (CMR) to 66.6 Hz interrupts
  TCCR1B |= (1 << WGM12);                                           // turn on CTC mode
  TCCR1B |= (1 << CS12) | (1 << CS10);                              // Set CS10 and CS12 bits for 1024 prescaler
  TIMSK1 |= (1 << OCIE1A);                                          // enable timer compare interrupt

  sei();                                                            // allow interrupts

  lcd.setCursor(2, 0);
  lcd.print("Set:");
  lcd.setCursor(10, 0);
  lcd.print(" RPM");
  lcd.setCursor(10, 1);
  lcd.print(" RPM");
  lcd.setCursor(0, 1);
  lcd.print("Actual:");

}

ISR(TIMER1_COMPA_vect)
{
  if (toggle1) {                                                    // toggle the pulse for the stepper on and off again
    digitalWrite(LED_BUILTIN, HIGH);
    toggle1 = 0;
  }
  else {
    digitalWrite(LED_BUILTIN, LOW);
    toggle1 = 1;
  }
}

void loop() {

  int currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {                 // if the interval has elapsed, then execute everything below
    previousMillis = currentMillis;

    RPMTotal = RPMTotal - RPMArray[RPMArrayIndex];                  // remove old value from running total

    if (digitalRead(2) == LOW) {                                    // either speed up, slow down, or
      RPM = RPM + 10;                                               // neither depending on input
    }

    else if (digitalRead(3) == LOW) {
      RPM = RPM - 10;
    }

    else {
      RPM = RPM;
    }

    if (RPM > 380) {
      RPM = 380;
    }

    else if (RPM < 10) {
      RPM = 10;
    }

    else {
      RPM = RPM;
    }

    RPMArray[RPMArrayIndex] = RPM;                                 // add new speed value to array

    RPMTotal = RPMTotal + RPMArray[RPMArrayIndex];                 // add new speed value to running total

    RPMArrayIndex = RPMArrayIndex + 1;                             // advance to next position in array

    if (RPMArrayIndex >= numReadings) {                            // if at end of array, return to beginning
      RPMArrayIndex = 0;
    }

    averageRPM = RPMTotal / numReadings;

    toggleFrequency = ((averageRPM * 200 / 60) * 2);
    long cmr = (1.6 * 10e6) / (1024 * toggleFrequency) - 1;
    OCR1A = cmr;

    Serial.print("rpm: ");
    Serial.println(averageRPM);
    Serial.print("freq: ");
    Serial.println(toggleFrequency);
    Serial.print("cmr: ");
    Serial.println(cmr);
    Serial.println();

    lcd.setCursor(7, 0);
    lcd.print(RPM);

    lcd.setCursor(7, 1);
    lcd.print(averageRPM);

  }

}