#include <avr/io.h>
#include <avr/interrupt.h>

#include "LiquidCrystal.h"

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

// define LED_INTERNAL_PIN
#define LED_INTERNAL 13
#define REGULAR_MODE_PIN 3
#define SAFE_MODE_PIN 2
#define MOTOR_ANALOG A0

#define REGULAR_MODE false
#define SAFE_MODE true

// define CONTROL strings using C++ style :)
const String CONTROL_START = "START";
const String CONTROL_STOP = "STOP";

String CONTROL_STRING; // string to read control sequence into
bool CONTROL_VAR = false; // shows if ENGINE is ON
bool CONTROL_MODE = REGULAR_MODE; // shows current mode

bool ENG_OVERDRIVE = false; // shows if ENGINE overdrive occured

float Signal_Current; // current ENG out
float Signal_Sum = 0; // ENG out SUM
float Signal_Seconds = 0; // ENG out Seconds

volatile int counter = 0;

void setup() 
{
  Serial.begin(9600);
  pinMode(LED_INTERNAL, OUTPUT);
  lcd.begin(16, 2);

  cli(); // disable global interrupts
  // zero Timer1 registers
  TCCR1A = 0;
  TCCR1B = 0;

  OCR1A = 1561; // set occurence register
  TCCR1B |= (1 << WGM12); // enable CTC mode

  // Set CS10 and CS12 bits to 1024 divider
  TCCR1B |= (1 << CS10);
  TCCR1B |= (1 << CS12);

  TIMSK1 |= (1 << OCIE1A); // enable occurence interrupts;
  sei(); // enable global interrupts
}

void READ_BUTTON_STATE()
{
  if (digitalRead(REGULAR_MODE_PIN))
  {
    CONTROL_MODE = REGULAR_MODE;
  }

  if (digitalRead(SAFE_MODE_PIN))
  {
    CONTROL_MODE = SAFE_MODE;
  }
}

void READ_ANALOG_STATE()
{
  Signal_Current = analogRead(MOTOR_ANALOG);
}

String FORM_LCD_SIGNAL()
{
  lcd.clear();
  if (CONTROL_VAR)
  {
    lcd.setCursor(0, 0);
    lcd.print("ENG");
    lcd.setCursor(0, 1);
    lcd.print(Signal_Current);
  }

  else if (ENG_OVERDRIVE)
  {
    lcd.setCursor(0, 0);
    lcd.print("ENG");
    lcd.setCursor(0, 1);
    lcd.print("OVERDRIVE");
  }

  else
  {
    lcd.setCursor(0, 0);
    lcd.print("WAITING FOR");
    lcd.setCursor(0, 1);
    lcd.print("START");
  }
}

ISR(TIMER1_COMPA_vect)
{
  // switch control variable AND
  // write value to internal led pin
  READ_BUTTON_STATE();
  READ_ANALOG_STATE();

  Signal_Sum += Signal_Current;
  
  if (counter == 10) {
    Serial.println(String(CONTROL_VAR) + " " + String(CONTROL_MODE) + " : " + 
                Signal_Sum / Signal_Seconds);
    counter = 0;

    FORM_LCD_SIGNAL();

    Signal_Seconds += 1;
  }

  counter++;
}

void loop() 
{
  if (Serial.available() > 0)
  {
    // Read control string from serial
    CONTROL_STRING = Serial.readString();
    CONTROL_STRING.trim(); // trim string to match pre-defined strings

    if (CONTROL_STRING == CONTROL_START)
    {
      CONTROL_VAR = true;
      ENG_OVERDRIVE = false;
      Signal_Sum = 0;
      Signal_Seconds = 0;
    }

    if (CONTROL_STRING == CONTROL_STOP)
    {
      CONTROL_VAR = false;
    }
  }

  // if ENG OVERDRIVE occurs
  if (CONTROL_MODE == SAFE_MODE && Signal_Current > 500)
  {
    CONTROL_VAR = false;
    ENG_OVERDRIVE = true;
  }
  
  if (ENG_OVERDRIVE)
  {
    Serial.println("ENGINE OVERDRIVE!");
    digitalWrite(LED_INTERNAL, LOW);
  }

  else
  {
    if (CONTROL_VAR)
      digitalWrite(LED_INTERNAL, HIGH);
    
    else
      digitalWrite(LED_INTERNAL, LOW);
  }
}