#include <EEPROM.h>

#define PIN_OUTPUT_PSENSOR  17
#define PIN_OUTPUT_RLIGHTS  18
#define PIN_OUTPUT_FLIGHTS  7
#define PIN_OUTPUT_SPEAKER 10

#define PIN_INPUT_REVERSE      5
#define PIN_INPUT_HIGH_BEAM    6
#define PIN_INPUT_BTN_PSENSOR  2
#define PIN_INPUT_BTN_RLIGHTS  3
#define PIN_INPUT_BTN_FLIGHTS  4

#define TIMEOUT_AUTO_OFF 15000
#define TIMEOUT_NONE     0

#define BEEP_FREQ 1000

#define DEBOUNCE_DELAY 50L
#define LONG_PRESS_DURATION 1000L

#define PSENSOR_AUTO_ENABLE_ADDRESS 2
#define RLIGHTS_AUTO_ENABLE_ADDRESS 3
#define FLIGHTS_AUTO_ENABLE_ADDRESS 4

enum State {
  OFF_AUTO,
  ON_AUTO,
  ON_MANUAL,
  OFF_MANUAL,
};

enum State STATE_PSENSOR = OFF_AUTO;
enum State STATE_RLIGHTS = OFF_AUTO;
enum State STATE_FLIGHTS = OFF_AUTO;

unsigned long PSENSOR_TIMEOUT_TIMER = 0;
unsigned long RLIGHTS_TIMEOUT_TIMER = 0;
unsigned long FLIGHTS_TIMEOUT_TIMER = 0;

void fsm_tick_timer(
  unsigned int outputPin,
  enum State* state,
  unsigned int sense,
  unsigned int button,
  unsigned long* timer,
  unsigned long timeout,
  unsigned int autoEnableAddress
) {
  unsigned int autoEnable = EEPROM.read(autoEnableAddress);
  if ((digitalRead(sense) == HIGH) && autoEnable)
    *timer = millis();

  unsigned int timer_expired = 0;
  if ((millis() - (*timer)) > timeout)
    timer_expired = 1;

  switch (*state) {
    case OFF_AUTO:
      digitalWrite(outputPin, LOW);
      
      if ((digitalRead(sense) == HIGH) && autoEnable) {
        *state = ON_AUTO;
        break;
      }
      
      if (button) {
        *state = ON_MANUAL;
        break;
      }

      break;
      
    case ON_AUTO:
      digitalWrite(outputPin, HIGH);
      if (button) {
        *state = OFF_MANUAL;
        *timer = millis();
        break;
      }
      
      if ((digitalRead(sense) == HIGH) && autoEnable) {
        break;
      }
      
      if (timer_expired || (!autoEnable)) {
        *state = OFF_AUTO;
        break;
      }

      break;

    case ON_MANUAL:
      digitalWrite(outputPin, HIGH);
      if (button) {
        *state = OFF_AUTO;
        break;
      }
      break;

    case OFF_MANUAL:
      digitalWrite(outputPin, LOW);

      if (digitalRead(sense) == LOW) {
        // One improvement here is to wait until the timer expires before going to auto
        *state = OFF_AUTO;
        break;
      }

      if (button) {
        *state = ON_AUTO;
        break;
      }

      break;
  }
}

unsigned int debouncelp(
  unsigned int pin,
  bool* state,
  bool* lastState,
  unsigned long* lastDebounceTime,
  unsigned long* buttonPressTime,
  bool* longPressTriggered,
  unsigned int autoEnableAddress
) {
  int reading = digitalRead(pin);

  unsigned int btnPressed = 0;

  if (reading != (*lastState)) {
    (*lastDebounceTime) = millis();
    (*lastState) = reading;
  }

  if ((millis() - (*lastDebounceTime)) > DEBOUNCE_DELAY) {
    if (reading != (*state)) {
      (*state) = reading;

      if ((*state) == LOW) {
        (*buttonPressTime) = millis();
        (*longPressTriggered) = false;
      } else {
        if (!(*longPressTriggered)) {
          unsigned long pressDuration = millis() - (*buttonPressTime);
          if (pressDuration < LONG_PRESS_DURATION) {
            btnPressed = 1;
          }
        }
      }
    }

    if ((*state) == LOW && !(*longPressTriggered)) {
      if ((millis() - (*buttonPressTime)) >= LONG_PRESS_DURATION) {
        *longPressTriggered = true;
        unsigned int autoEnable = EEPROM.read(autoEnableAddress);
        if (autoEnable) {
          EEPROM.write(autoEnableAddress, 0);
          tone(PIN_OUTPUT_SPEAKER, BEEP_FREQ, 80); 
          delay(150);
          tone(PIN_OUTPUT_SPEAKER, BEEP_FREQ, 80); 
        } else {
          EEPROM.write(autoEnableAddress, 1);
          tone(PIN_OUTPUT_SPEAKER, BEEP_FREQ, 300); 
        }
      }
    }
  }
  return btnPressed;
}

void setup() {
  pinMode(PIN_INPUT_REVERSE, INPUT);
  pinMode(PIN_INPUT_HIGH_BEAM, INPUT);
  pinMode(PIN_INPUT_BTN_PSENSOR, INPUT_PULLUP);
  pinMode(PIN_INPUT_BTN_RLIGHTS, INPUT_PULLUP);
  pinMode(PIN_INPUT_BTN_FLIGHTS, INPUT_PULLUP);

  pinMode(PIN_OUTPUT_PSENSOR, OUTPUT);
  pinMode(PIN_OUTPUT_RLIGHTS, OUTPUT);
  pinMode(PIN_OUTPUT_FLIGHTS, OUTPUT);
  //Serial.begin(9600);
}

void loop() {
#define DEBOUNCE_BUTTON(Name, Pin, AutoEnable) \
  static bool Name##State = HIGH; \
  static bool Name##LastState = HIGH; \
  static unsigned long Name##LastDebounceTime = 0; \
  static unsigned long Name##ButtonPressTime = 0; \
  static bool Name##LongPressTriggered = false; \
  unsigned int Name##Pressed = debouncelp(Pin, &Name##State, &Name##LastState, &Name##LastDebounceTime, &Name##ButtonPressTime, &Name##LongPressTriggered, AutoEnable);
#define FSM(LName, UName, Signal, Timeout) \
  DEBOUNCE_BUTTON(LName, PIN_INPUT_BTN_##UName, UName##_AUTO_ENABLE_ADDRESS) \
  fsm_tick_timer(PIN_OUTPUT_##UName, &STATE_##UName, Signal, LName##Pressed, &UName##_TIMEOUT_TIMER, Timeout, UName##_AUTO_ENABLE_ADDRESS);

  FSM(psensor, PSENSOR, PIN_INPUT_REVERSE, TIMEOUT_AUTO_OFF)
  FSM(rlights, RLIGHTS, PIN_INPUT_REVERSE, TIMEOUT_AUTO_OFF)
  FSM(Flights, FLIGHTS, PIN_INPUT_HIGH_BEAM, TIMEOUT_NONE)
}