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

int Left_Light = 10;
int Right_Light = 9;
int Right_Flasher_Pin = 4;
int Left_Flasher_Pin = 6;
int Hazard_Pin = 5;

int Blinkers_State;
int R_Flasher_Switch_Timer;
int L_Flasher_Switch_Timer;
int Hazard_Switch_Timer;
int Counter;
int Blinking_Counter;
int Switch_Released;
int Right_Left;
volatile int VAR_Interruption;

void INIT(void);
void INPUT_SETTINGS(void);
void PRIORITY(void);
void OUTPUT_SETTINGS(void);
void LANE_CHANGE_MODE(void);
void LANE_CHANGE_MODE_OUTPUT(void);
void NORMAL_MODE(void);
void HAZARD_MODE(void);
void LIGHTS_OFF_OUTPUT(void);

void CONFIGURE_TIMER1()
{
  noInterrupts();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  OCR1A = 153;
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS12) | (1 << CS10);
  TIMSK1 |= (1 << OCIE1A);
  TIMSK1 |= (1 << TOIE1);
  interrupts();
}

ISR(TIMER1_COMPA_vect)
{
  VAR_Interruption = 1;
}

void INIT(void)
{
  Serial.begin(115200);
  CONFIGURE_TIMER1();
  pinMode(Right_Flasher_Pin, INPUT);
  pinMode(Left_Flasher_Pin, INPUT);
  pinMode(Hazard_Pin, INPUT);
  pinMode(Left_Light, OUTPUT);
  pinMode(Right_Light, OUTPUT);
  Blinkers_State = 99;
}

int main(void)
{
  INIT();
  while (true)
  {
    if (VAR_Interruption == 1)
    {
      VAR_Interruption = 0;
      INPUT_SETTINGS();
      PRIORITY();
      OUTPUT_SETTINGS();
    }
  }
  return 0;
}

void INPUT_SETTINGS(void)
{
  if (digitalRead(Right_Flasher_Pin) == HIGH)
  {
    R_Flasher_Switch_Timer++;
    Right_Left = 1;
  }
  else
  {
    R_Flasher_Switch_Timer = 0;
  }

  if (digitalRead(Left_Flasher_Pin) == HIGH)
  {
    L_Flasher_Switch_Timer++;
    Right_Left = 0;
  }
  else
  {
    L_Flasher_Switch_Timer = 0;
  }

  if (digitalRead(Hazard_Pin) == HIGH)
  {
    Hazard_Switch_Timer++;
  }
}

void PRIORITY(void)
{
  switch (Blinkers_State)
  {
    case 1:
      LANE_CHANGE_MODE();
      break;
    case 2:
      NORMAL_MODE();
      break;
    case 3:
      HAZARD_MODE();
      break;
    default:
      LIGHTS_OFF();
  }
}

void OUTPUT_SETTINGS(void)
{
  switch (Blinkers_State)
  {
    case 1:
      LANE_CHANGE_MODE_OUTPUT();
      break;
    case 2:
      NORMAL_MODE_OUTPUT();
      break;
    case 3:
      HAZARD_MODE_OUTPUT();
      break;
    default:
      LIGHTS_OFF_OUTPUT();
  }
}

void LIGHTS_OFF(void)
{
  if (R_Flasher_Switch_Timer != 0 && R_Flasher_Switch_Timer < 50)
  {
    Counter = 100;
    Right_Left = 1;
    Blinkers_State = 1; // LANE_CHANGE_MODE
  }
  else if (L_Flasher_Switch_Timer != 0 && L_Flasher_Switch_Timer < 50)
  {
    Counter = 100;
    Right_Left = 0;
    Blinkers_State = 1; // LANE_CHANGE_MODE
  }
  else if (Hazard_Switch_Timer >= 20 && digitalRead(Hazard_Pin) == LOW)
  {
    Counter = 200;
    Blinkers_State = 3; // HAZARD_MODE
    Hazard_Switch_Timer = 0;
  }
}

void LANE_CHANGE_MODE(void)
{
  if (Counter == 0)
  {
    Blinking_Counter++;
    if (Blinking_Counter < 3)
    {
      Counter = 100;
    }
    else
    {
      if (Right_Left == 1)
      {
        if (R_Flasher_Switch_Timer >= 50 && digitalRead(Right_Flasher_Pin) == HIGH)
        {
          Counter = 100;
          Blinkers_State = 2; // NORMAL_MODE
        }
        else
        {
          Blinking_Counter = 0;
          Blinkers_State = 99;
          R_Flasher_Switch_Timer = 0;
        }
      }
      else
      {
        if (L_Flasher_Switch_Timer >= 50 && digitalRead(Left_Flasher_Pin) == HIGH)
        {
          Counter = 100;
          Blinkers_State = 2; // NORMAL_MODE
        }
        else
        {
          Blinking_Counter = 0;
          Blinkers_State = 99;
          L_Flasher_Switch_Timer = 0;
        }
      }
    }
  }
}


void NORMAL_MODE(void)
{
  if (Counter == 0)
  {
    Counter = 100;
  }
  if (digitalRead(Right_Flasher_Pin) == LOW && digitalRead(Left_Flasher_Pin) == LOW)
  {
    Switch_Released++;
    if (Switch_Released >= 100)
    {
      Counter = 0;
      Switch_Released = 0;
      R_Flasher_Switch_Timer = 0;
      L_Flasher_Switch_Timer = 0;
      Blinkers_State = 99;
    }
  }
}

void HAZARD_MODE(void)
{
  if (Counter == 0)
  {
    Counter = 200;
  }
  if (Hazard_Switch_Timer >= 20 && digitalRead(Hazard_Pin) == LOW)
  {
    Counter = 0;
    Hazard_Switch_Timer = 0;
    Blinkers_State = 99;
  }
}

void LIGHTS_OFF_OUTPUT(void)
{
  digitalWrite(Left_Light, LOW);
  digitalWrite(Right_Light, LOW);
}


void LANE_CHANGE_MODE_OUTPUT(void)
{
  Counter--;
  if (Right_Left == 1)
  {
    if (Counter > 70)
    {
      digitalWrite(Right_Light, HIGH);
      digitalWrite(Left_Light, LOW);
    }
    else
    {
      digitalWrite(Right_Light, LOW);
      digitalWrite(Left_Light, LOW);
    }
  }
  else
  {
    if (Counter > 70)
    {
      digitalWrite(Left_Light, HIGH);
      digitalWrite(Right_Light, LOW);
    }
    else
    {
      digitalWrite(Left_Light, LOW);
      digitalWrite(Right_Light, LOW);
    }
  }
}



void NORMAL_MODE_OUTPUT(void)
{
  Counter--;
  if (Right_Left == 1)
  {
    if (Counter > 70)
    {
      digitalWrite(Right_Light, HIGH);
    }
    else
    {
      digitalWrite(Right_Light, LOW);
    }
  }
  else
  {
    if (Counter > 70)
    {
      digitalWrite(Left_Light, HIGH);
    }
    else
    {
      digitalWrite(Left_Light, LOW);
    }
  }
}

void HAZARD_MODE_OUTPUT(void)
{
  Counter--;
  if (Counter > 100)
  {
    digitalWrite(Right_Light, HIGH);
    digitalWrite(Left_Light, HIGH);
  }
  else
  {
    digitalWrite(Right_Light, LOW);
    digitalWrite(Left_Light, LOW);
  }
}