#include <Arduino.h>
#include <Bounce2.h>
#include <MsTimer2.h>

/*******************************************************/
#define LEFT_SIG_BUTTON_PIN         2    // Left signal button 
#define RIGHT_SIG_BUTTON_PIN        3    // Right signal button
#define BEAM_BUTTON_PIN             4    // Beam select button 
#define LEFT_SIG_PIN                5    // Left signal 
#define RIGHT_SIG_PIN               6    // Right signal
/*******************************************************/
#define HIGH_BEAM_PIN               7    // High beam signal
#define LOW_BEAM_PIN                8    // Low beam signal
#define HORN_BUTTON_PIN             9    // Horn button 
/*******************************************************/
#define HORN_PIN                   10
#define EMERGENCY_SIG_PIN          11
#define BUZZER_PIN                 12
//#define START_ENGINE_SIG_BUTTON_PIN 6
//#define STOP_ENGINE_SIG_BUTTON_PIN  7
/*******************************************************/
const unsigned long BLINK_INTERVAL = 500; // ms

typedef enum { BEAM_OFF=0, BEAM_LOW=1, BEAM_HIGH=2 } beam_state_t;
volatile beam_state_t beam_state;
typedef enum { SIG_OFF=0, LEFT_SIG=1, RIGHT_SIG=2, EMERGENCY_SIG=3 } sig_state_t;
//volatile byte beam_state; //OFF=0 LOW_BEAM=1 HIGH_BEAM=2
volatile byte sig_state;  //OFF=0 LEFT_SIG=1 RIGHT_SIG=2 EMERGENCY_SIG=3

Bounce2::Button beam_button = Bounce2::Button();
Bounce2::Button left_sig_button = Bounce2::Button();
Bounce2::Button right_sig_button = Bounce2::Button();
Bounce2::Button horn_button = Bounce2::Button();


unsigned long lastBlink;
bool onCycle;

void blinker() {
    lastBlink = millis ();
     onCycle = !onCycle;

  static boolean print_state = true;
  static boolean blink = HIGH;
  const int toneFreq = 523;
  if (print_state) {
    Serial.print("[blinker]: set sig_state=");
    Serial.println(sig_state);
    print_state=false;
  }
  if (sig_state != 0 && blink == HIGH) {
      tone(BUZZER_PIN, toneFreq);
    } else {
      noTone(BUZZER_PIN);
    }
  
  switch (sig_state) {
    case 0: //SIG_OFF:
      digitalWrite(LEFT_SIG_PIN, LOW);
      digitalWrite(RIGHT_SIG_PIN, LOW);
      digitalWrite(EMERGENCY_SIG_PIN, LOW);
      noTone(BUZZER_PIN);
      break;
    case 1: //SIG_LEFT:
      digitalWrite(LEFT_SIG_PIN, blink);
      digitalWrite(RIGHT_SIG_PIN, LOW);
      digitalWrite(EMERGENCY_SIG_PIN, LOW);
      break;
    case 2: //SIG_RIGHT:
      digitalWrite(RIGHT_SIG_PIN, blink);
      digitalWrite(LEFT_SIG_PIN, LOW);
      digitalWrite(EMERGENCY_SIG_PIN, LOW);
      break;
    case 3: //SIG_EMERGENCY:;
      digitalWrite(LEFT_SIG_PIN, blink);
      digitalWrite(RIGHT_SIG_PIN, blink);
      digitalWrite(EMERGENCY_SIG_PIN, blink);
      break;
    }
  blink = !blink;
  }


void setup() {
  beam_state = BEAM_OFF;
  sig_state = 0;

Serial.begin(9600);
Serial.println("[setup]: high beam pin");
   pinMode(HIGH_BEAM_PIN, OUTPUT);
   digitalWrite(HIGH_BEAM_PIN, LOW);

Serial.println("[setup]: low beam pin");
   pinMode(LOW_BEAM_PIN, OUTPUT);
   digitalWrite(LOW_BEAM_PIN, LOW);
 
Serial.println("[setup]: left sig pin");
   pinMode(LEFT_SIG_PIN, OUTPUT);
   digitalWrite(LEFT_SIG_PIN, LOW);

Serial.println("[setup]: right sig pin");
   pinMode(RIGHT_SIG_PIN, OUTPUT);
   digitalWrite(RIGHT_SIG_PIN, LOW);

Serial.println("------------------------");
Serial.println("[setup]: buttons pins: attach");
  beam_button.attach(BEAM_BUTTON_PIN, INPUT_PULLUP);
  beam_button.interval(15);
  beam_button.setPressedState(LOW);

  left_sig_button.attach(LEFT_SIG_BUTTON_PIN, INPUT_PULLUP);
  left_sig_button.interval(15);
  left_sig_button.setPressedState(LOW);
  
  right_sig_button.attach(RIGHT_SIG_BUTTON_PIN, INPUT_PULLUP);
  right_sig_button.interval(15);
  right_sig_button.setPressedState(LOW);

  horn_button.attach(HORN_BUTTON_PIN, INPUT_PULLUP);
  horn_button.interval(15);
  horn_button.setPressedState(LOW);
  
  Serial.println("------------------------");
  Serial.println("[setup]: Start blinker timer");
  //MsTimer2::set(500, blinker); // 500ms period
  //MsTimer2::start();

//attachInterrupt(digitalPinToInterrupt(BEAM_BUTTON_PIN), beam_isr, RISING);
//attachInterrupt(digitalPinToInterrupt(HORN_BUTTON_PIN), horn_isr, RISING);

  pinMode(LED_BUILTIN, OUTPUT);   // Init builtin led and set it to 'LOW'
  digitalWrite(LED_BUILTIN, LOW); //  for indicate 'setup is starting'
Serial.println("------------------------------");

}

void loop() {
    

  beam_button.update();
  if ( beam_button.pressed() ) {
    Serial.print("[loop]: Beam button pressed. beam_state=");
    Serial.println(beam_state);
    if (beam_state == BEAM_OFF) {
      beam_state = BEAM_LOW;
      Serial.println("[loop]: Set from BEAM_OFF -> to BEAM_LOW");
        digitalWrite(LOW_BEAM_PIN, HIGH);
        digitalWrite(HIGH_BEAM_PIN, LOW);
    } else if (beam_state == BEAM_LOW) {
      beam_state = BEAM_HIGH;
      Serial.println("[loop]: Set from BEAM_LOW -> to BEAM_HiGH");
        digitalWrite(LOW_BEAM_PIN, LOW);
        digitalWrite(HIGH_BEAM_PIN, HIGH);
    } else if (beam_state == BEAM_HIGH) {
      beam_state = BEAM_OFF;
      Serial.println("[loop]: Set from BEAM_HIGH -> to BEAM_OFF"); 
        digitalWrite(LOW_BEAM_PIN, LOW);
        digitalWrite(HIGH_BEAM_PIN, LOW);
    }
    Serial.print("[loop]: Now beam_state is ");
    Serial.println(beam_state);
    }
  left_sig_button.update();
  right_sig_button.update();
  // if ( left_sig_button.pressed() && right_sig_button.pressed()) {
  //   if (sig_state != 3) {
  //     sig_state = 3;
  //   } else sig_state = 0;

  // } else 
  
  if (left_sig_button.pressed()) {
    if (sig_state != 1) {
      sig_state = 1;
    } else sig_state = 0;
     Serial.print("[loop]: Left sig button pressed. sig_state=");
     Serial.println(sig_state);
  } if (right_sig_button.pressed()) {
      if (sig_state != 2) {
        sig_state = 2;
      } else sig_state = 0;
        Serial.print("[loop]: Right sig button pressed. sig_state=");
        Serial.println(sig_state);
    }
if (millis () - lastBlink >= BLINK_INTERVAL)
blinker();

}