#define MODE_OFF 0
#define DEBUG_WITH_BUILTIN_LED false
#define PIN_OUT_BUILTIN_LED 10

// Foreground Modes
#define FG_MODE_BLINK_L 1
#define FG_MODE_BLINK_R 2

// Background modes
#define BG_MODE_HAZARDS 1
#define BG_MODE_RUNNING_LIGHTS 2
#define BG_MODE_RUNNING_W_BLINK 3
#define BG_MODE_AUXILARY_LIGHTS 4

// STEM KEY
// Brown          - 5V Ground
// Brown striped  - 12V Hot
// Orange         - 12V Left Blinker ground
// Orange striped - 12V Right Blinker ground
// Blue           - 5V Left index switch
// Blue striped   - 5V Left aux switch
// Green          - 5V Right index switch
// Green striped  - 5V Right aux switch

// BOARD KEY
// Brown          - Gnd
// Brown striped  - 3 / PIN_IN_RIGHT_MIDDLE_FINGER
// Orange         - X
// Orange striped - 2 / PIN_IN_RIGHT_INDEX_FINGER
// Blue           - 0 / PIN_IN_LEFT_INDEX_FINGER
// Blue striped   - 1 / PIN_IN_LEFT_MIDDLE_FINGER
// Green          - X
// Green striped  - X

// 12V
// ===
// 
// 1 - Hot 12V              Red
// 2.- Negative Blinker L   Green
// 3 - Negative Blinker R   Yellow
// 4 - Negative Horn        Brown
// 5 - Negative ____        White

// Old config notes:
// Solid blue - Left temp wires
// Solid Green - Left util
// Striped green - Right util

// Inputs
#define PIN_IN_LEFT_INDEX_FINGER_ANALOG 7 // I somehow killed pin D0....
#define PIN_IN_LEFT_INDEX_FINGER A0 // Bal index
#define PIN_IN_LEFT_MIDDLE_FINGER 8 //
#define PIN_IN_RIGHT_INDEX_FINGER A4 // Jobb index
#define PIN_IN_RIGHT_MIDDLE_FINGER 9  //

// Outputs
#define PIN_OUT_BLINKER_L 3
#define PIN_OUT_BLINKER_R 2
#define PIN_OUT_HORN 4
#define PIN_OUT_HEADLIGHT 5

// Config
#define BLINK_RATE 250
#define RUNNING_LIGHT_BLINK_RATE 750
#define MINIMUM_PRESS_TIME 50
#define HOLD_DOWN_TIME 500
#define DEBOUNCE_COOLDOWN_TIME 200

#define SPEAKER_TICK_DURATION 20
#define SPEAKER_TICK_SPACING 60
#define SPEAKER_LONG_TICK_DURATION 300

// State
int elozoErtek0 = 0;
int fg_mode = MODE_OFF;
int bg_mode = MODE_OFF;
int last_fg_mode = MODE_OFF;
int last_bg_mode = MODE_OFF;
bool hold_waiting = false;

int speaker_tick_frame = 0;
bool speaker_solid_sound;

int blinker_light_state;
bool headlight_on = false;
bool actively_honking = false;

// FW Mode
bool firmware_mode = false;
bool light_test_mode = false;
int firmware_mode_frame;
bool firmware_bg_blink_flipper;

enum scooter_ui {
  btn_grip_left,
  btn_grip_right,
  btn_grip_both,

  btn_horn,
  btn_horn_quiet,

  num_ui_actions
};

// Flags calculated by the program for a single frame of loop as either single push & release
// or a hold. The hold bit is flipped ONCE at the frame exactly at the end of HOLD_DOWN_TIME.
// The user can continue to hold, which increments the input in hold_time, but won't flip
// any bit either "hit" array here.
bool input_single_hit[num_ui_actions];
bool input_hold_hit[num_ui_actions];
int hold_time[num_ui_actions];

// Time accumulator variables
int cycle_time = 0;
int new_input_blocked = 0;

#if PARTICLE
SYSTEM_MODE(MANUAL);
#endif

void setup()
{
  // Initialize a serial connection for reporting values to the host
  Serial.begin(9600);

  pinMode(PIN_IN_LEFT_INDEX_FINGER_ANALOG, INPUT_PULLUP);
  pinMode(PIN_IN_RIGHT_INDEX_FINGER, INPUT);
  pinMode(PIN_IN_LEFT_MIDDLE_FINGER, INPUT);
  pinMode(PIN_IN_RIGHT_MIDDLE_FINGER, INPUT);

  pinMode(PIN_OUT_BLINKER_L, OUTPUT);
  pinMode(PIN_OUT_BLINKER_R, OUTPUT);
  pinMode(PIN_OUT_HORN, OUTPUT);
  pinMode(PIN_OUT_HEADLIGHT, OUTPUT);

  digitalWrite(PIN_OUT_BLINKER_L, LOW);
  digitalWrite(PIN_OUT_BLINKER_R, LOW);
  digitalWrite(PIN_OUT_HORN, LOW);

   pinMode (A3, INPUT_PULLUP);
   pinMode (7, OUTPUT);
  if (digitalRead(PIN_IN_RIGHT_INDEX_FINGER) == LOW)
    firmware_mode = true;
  else if (left_index_pressed())
    light_test_mode = true;
}

void debounce_subsequent_input()
{
  new_input_blocked = DEBOUNCE_COOLDOWN_TIME;
}

void blink_frame(bool left, bool right)
{
  if (cycle_time > BLINK_RATE)
  {
    cycle_time = 0;
    blinker_light_state = !blinker_light_state;
  }

  // Actual light
  if (left)
    digitalWrite(PIN_OUT_BLINKER_L, blinker_light_state ? LOW : HIGH);

  if (right)
    digitalWrite(PIN_OUT_BLINKER_R, blinker_light_state ? LOW : HIGH);

  if (blinker_light_state)
    speaker_tick(1);

  #if DEBUG_WITH_BUILTIN_LED
  digitalWrite(PIN_OUT_BUILTIN_LED, blinker_light_state ? LOW : HIGH);
  #endif

  cycle_time++;
}

void running_light_frame(bool with_blink = false)
{
  if (cycle_time >= RUNNING_LIGHT_BLINK_RATE)
    cycle_time = 0;

  digitalWrite(PIN_OUT_BLINKER_L, cycle_time < 20 ? HIGH : LOW);
  digitalWrite(PIN_OUT_BLINKER_R, cycle_time < 20 ? HIGH : LOW);

  #if DEBUG_WITH_BUILTIN_LED
  digitalWrite(PIN_OUT_BUILTIN_LED, cycle_time < 20 ? HIGH : LOW);
  #endif

  cycle_time++;
}

void auxiliary_light_frame()
{
  digitalWrite(PIN_OUT_BLINKER_L, HIGH);
  digitalWrite(PIN_OUT_BLINKER_R, HIGH);

  #if DEBUG_WITH_BUILTIN_LED
  digitalWrite(PIN_OUT_BUILTIN_LED, HIGH);
  #endif
}

// This is a crutch because I screwed up pin D0 on the particle size :-(
bool left_index_pressed()
{
  #if PARTICLE
  return analogRead(PIN_IN_LEFT_INDEX_FINGER_ANALOG) < 100;
  #else
  return analogRead(PIN_IN_LEFT_INDEX_FINGER) == LOW;
  #endif
}

// Each frame, this increments L/R/B hold time variables while the user is holding a button (or both) down.
// It also checks for the release. In that case it flips on either input_single_* or input_hold_* variables
// to be consumed later in the frame.
void read_buttons()
{
  for (int i = 0; i < num_ui_actions; i++) {
    input_single_hit[i] = false;
    input_hold_hit[i] = false;
  }

  if (left_index_pressed() && digitalRead(PIN_IN_RIGHT_INDEX_FINGER) == LOW)
    hold_time[btn_grip_both]++;
  else if (left_index_pressed())
    hold_time[btn_grip_left]++;
  else if (digitalRead(PIN_IN_RIGHT_INDEX_FINGER) == LOW)
    hold_time[btn_grip_right]++;
  else if (digitalRead(PIN_IN_RIGHT_MIDDLE_FINGER) == LOW)
    hold_time[btn_horn]++;
  else if (digitalRead(PIN_IN_LEFT_MIDDLE_FINGER) == LOW)
    hold_time[btn_horn_quiet]++;
  else
  {
    for (int i = 0; i < num_ui_actions; i++)
    {
      if (hold_time[i] > MINIMUM_PRESS_TIME && !new_input_blocked && !hold_waiting)
      {
        input_single_hit[i] = true;
        debounce_subsequent_input();
      }
    }

    for (int i = 0; i < num_ui_actions; i++)
      hold_time[i] = 0;

    if (hold_waiting)
      debounce_subsequent_input();

    hold_waiting = false;
  }

  for (int i = 0; i < num_ui_actions; i++)
  {
    if (hold_time[i] == HOLD_DOWN_TIME && !new_input_blocked)
    {
      input_hold_hit[i] = true;
      hold_waiting = true;
      debounce_subsequent_input();
    }
  }
}

void set_up_mode()
{
  read_buttons();

  if (input_hold_hit[btn_grip_both])
  {
    // if (!bg_mode)
    //   bg_mode = BG_MODE_RUNNING_W_BLINK;
    // else if (bg_mode >= BG_MODE_AUXILARY_LIGHTS)
    //   bg_mode = MODE_OFF;
    // else
    //   bg_mode++;
    speaker_tick(bg_mode == BG_MODE_RUNNING_W_BLINK ? SPEAKER_LONG_TICK_DURATION : 3);
    bg_mode = bg_mode ? MODE_OFF : BG_MODE_RUNNING_W_BLINK;

    fg_mode = MODE_OFF;
  }
  else if (input_single_hit[btn_grip_left])
  {
    speaker_tick(fg_mode == FG_MODE_BLINK_L ? SPEAKER_LONG_TICK_DURATION : 1);
    fg_mode = fg_mode == FG_MODE_BLINK_L ? MODE_OFF : FG_MODE_BLINK_L;
  }
  else if (input_single_hit[btn_grip_right])
  {
    speaker_tick(fg_mode == FG_MODE_BLINK_R ? SPEAKER_LONG_TICK_DURATION : 1);
    fg_mode = fg_mode == FG_MODE_BLINK_R ? MODE_OFF : FG_MODE_BLINK_R;
  }
  else if (input_hold_hit[btn_grip_right])
  {
    speaker_tick(headlight_on ? SPEAKER_LONG_TICK_DURATION : 3);
    headlight_on = !headlight_on;
  }
}

void reset_all()
{
  cycle_time = 0;
  blinker_light_state = 0;

  #if DEBUG_WITH_BUILTIN_LED
  digitalWrite(PIN_OUT_BUILTIN_LED, LOW);
  #endif

  digitalWrite(PIN_OUT_BLINKER_L, LOW);
  digitalWrite(PIN_OUT_BLINKER_R, LOW);
  digitalWrite(PIN_OUT_HORN, LOW);
  digitalWrite(PIN_OUT_HEADLIGHT, LOW);
}

void speaker_tick(int count)
{
  if (actively_honking)
    return;

  if (count > 10)
  {
    speaker_solid_sound = true;
    speaker_tick_frame = count;
  }
  else
  {
    speaker_solid_sound = false;
    speaker_tick_frame = (SPEAKER_TICK_SPACING + SPEAKER_TICK_DURATION) * count;
  }
}

void speaker_tick_sound_frame()
{
  if (actively_honking)
  {
    speaker_tick_frame = 0;
    return;
  }
  else if (speaker_tick_frame > 0)
  {
    if (speaker_solid_sound)
    {
      digitalWrite(PIN_OUT_HORN, !(speaker_tick_frame % 2) ? HIGH : LOW);
    }
    else
    {
      const int frame_length = SPEAKER_TICK_SPACING + SPEAKER_TICK_DURATION;
      const int current_pattern_index = frame_length - (speaker_tick_frame % frame_length);

      digitalWrite(PIN_OUT_HORN, current_pattern_index <= SPEAKER_TICK_DURATION ? HIGH : LOW);
    }

    speaker_tick_frame--;
  }
  else
    digitalWrite(PIN_OUT_HORN, LOW);
}

bool try_light_test_mode()
{
  if (light_test_mode)
  {
    digitalWrite(PIN_OUT_BLINKER_L, HIGH);
    digitalWrite(PIN_OUT_BLINKER_R, HIGH);

    return true;
  }
  else
    return false;
}

void horn_frame()
{
  // Horn is driven asynchronously from the rest of the blinking logic
  if (hold_time[btn_horn] > MINIMUM_PRESS_TIME ||
      (hold_time[btn_horn_quiet] > MINIMUM_PRESS_TIME &&
      !(hold_time[btn_horn_quiet] % 3)))
  {
    actively_honking = true;
    digitalWrite(PIN_OUT_HORN, HIGH); // TOOOOOOT

    #if DEBUG_WITH_BUILTIN_LED
    digitalWrite(PIN_OUT_BUILTIN_LED, HIGH);
    #endif
  }
  else
  {
    actively_honking = false;
    digitalWrite(PIN_OUT_HORN, LOW);
  }
}

void headlight_frame()
{
  // Make headlight flash rapidly when honking
  if (hold_time[btn_horn] > MINIMUM_PRESS_TIME) {
    digitalWrite(
      PIN_OUT_HEADLIGHT,
      headlight_on && (hold_time[btn_horn]) % 75 < 20 ? HIGH : LOW
    );
  }
  else
    digitalWrite(PIN_OUT_HEADLIGHT, headlight_on ? HIGH : LOW);
}

#if PARTICLE
void firmware_mode_indicator_frame()
{
  digitalWrite(PIN_OUT_BLINKER_L, firmware_mode_frame % 80 < 30 ? HIGH : LOW);
  digitalWrite(PIN_OUT_BLINKER_R, firmware_mode_frame % 80 < 30 ? HIGH : LOW);
}

void firmware_mode_wifi_frame()
{
  if (!Particle.connected())
    Particle.connect();

  Particle.process();
  delay(500);

  if (firmware_bg_blink_flipper = !firmware_bg_blink_flipper) {
    digitalWrite(PIN_OUT_BLINKER_L, HIGH);
    digitalWrite(PIN_OUT_BLINKER_R, HIGH);
    delay(5);
    digitalWrite(PIN_OUT_BLINKER_L, LOW);
    digitalWrite(PIN_OUT_BLINKER_R, LOW);
    delay(100);
    digitalWrite(PIN_OUT_BLINKER_L, HIGH);
    digitalWrite(PIN_OUT_BLINKER_R, HIGH);
    delay(5);
    digitalWrite(PIN_OUT_BLINKER_L, LOW);
    digitalWrite(PIN_OUT_BLINKER_R, LOW);
  }
}

bool try_firmware_mode()
{
  if (firmware_mode)
  {
    firmware_mode_indicator_frame();

    // Start with rapid blinks to indicate firmware mode, then after that's done
    // start connecting to wifi.
    if (firmware_mode_frame < 80 * 3 + 30)
      firmware_mode_frame++;
    else
      firmware_mode_wifi_frame(); // Blocking

    return true;
  }
  else
    return false;
}
#else
bool try_firmware_mode()
{
  return false;
}
#endif

void loop()
{
  delay(1);

  if (try_firmware_mode() || try_light_test_mode())
    return;

  speaker_tick_sound_frame();
  set_up_mode();

  // Reset all lights going directly from one direction to the next.
  if (fg_mode != last_fg_mode || bg_mode != last_bg_mode)
    reset_all();

  if (fg_mode == FG_MODE_BLINK_L)
    blink_frame(true, false);
  else if (fg_mode == FG_MODE_BLINK_R)
    blink_frame(false, true);
  else if (bg_mode == BG_MODE_HAZARDS)
    blink_frame(true, true);
  else if (bg_mode == BG_MODE_RUNNING_LIGHTS)
    running_light_frame();
  else if (bg_mode == BG_MODE_RUNNING_W_BLINK)
    running_light_frame(true);
  else if (bg_mode == BG_MODE_AUXILARY_LIGHTS)
    auxiliary_light_frame();
  else
    reset_all();

  horn_frame();
  headlight_frame();

  last_fg_mode = fg_mode;
  last_bg_mode = bg_mode;

  if (new_input_blocked > 0)
    new_input_blocked--;

    if(digitalRead(A3) == LOW && elozoErtek0 == 1)
  {
    digitalWrite(7, !digitalRead(7) );
    
  }
  elozoErtek0 = digitalRead(A3);
  delay(50);
}