#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 3000
#define TIMEOUT_NONE     0

#define BEEP_FREQ 1000

#define DEBOUNCE_DELAY_BUTTON 50L
#define DEBOUNCE_DELAY_SIGNAL 500L
#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 (sense && autoEnable)
    *timer = millis();

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

  switch (*state) {
    case OFF_AUTO:
      digitalWrite(outputPin, LOW);
      
      if (sense && 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 (sense && 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 (!sense && timer_expired) {
        *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_BUTTON) {
    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;
}

unsigned int debouncesig(
  unsigned int pin,
  bool* state,
  bool* lastState,
  unsigned long* lastDebounceTime
) {
  int reading = digitalRead(pin);
  if (reading != (*lastState)) {
    (*lastDebounceTime) = millis();
    (*lastState) = reading;
  }

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

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

  *lastState = reading;

  if ((*state) == HIGH)
      return 1;
  return 0;
}

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);
}

#define DEBOUNCE_SIGNAL(Name, Pin) \
  static bool Name##State = LOW; \
  static bool Name##LastState = LOW; \
  static unsigned long Name##LastDebounceTime = 0; \
  unsigned int Name##Enabled = debouncesig(Pin, &Name##State, &Name##LastState, &Name##LastDebounceTime);
#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);

void loop() {
  DEBOUNCE_SIGNAL(reverseSignal, PIN_INPUT_REVERSE)
  DEBOUNCE_SIGNAL(highBeamSignal, PIN_INPUT_HIGH_BEAM)

  FSM(psensor, PSENSOR, reverseSignalEnabled, TIMEOUT_AUTO_OFF)
  FSM(rlights, RLIGHTS, reverseSignalEnabled, TIMEOUT_AUTO_OFF)
  FSM(Flights, FLIGHTS, highBeamSignalEnabled, TIMEOUT_NONE)
}