#include <TaskScheduler.h>

const int RED_TIME    = 5000;
const int YELLOW_TIME = 3000;
const int GREEN_TIME  = 3000;
const int RIGHT_TIME  = 3000;

bool redOE();
void redOD();

bool yellowOE();
void yellowOD();

bool greenOE();
void greenOD();

bool greenBlinkOE();
void greenBlinkCB();
void greenBlinkOD();

bool rightOE();
void rightOD();

bool rightBlinkOE();
void rightBlinkCB();
void rightBlinkOD();

Scheduler ts;

Task red(RED_TIME,       TASK_ONCE, NULL,         &ts, false,   redOE,    redOD);
Task yellow(YELLOW_TIME, TASK_ONCE, NULL,         &ts, false,   yellowOE, yellowOD);
Task yellow_2(YELLOW_TIME/3, TASK_ONCE, NULL,         &ts, false,   yellowOE, yellowOD);
Task green(GREEN_TIME,   TASK_ONCE, NULL,         &ts, false,   greenOE,  greenOD);
Task greenBlink(500,     6,        greenBlinkCB, &ts, false,   greenBlinkOE, greenBlinkOD);
Task right(RIGHT_TIME,   TASK_ONCE, NULL,         &ts, false,  rightOE,  rightOD);
Task rightBlink(500,     6,        rightBlinkCB, &ts, false,   rightBlinkOE, rightBlinkOD);

bool redToGreen;

void setup() {
  pinMode(8, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(13, OUTPUT);


  redToGreen = false;
  red.restartDelayed();
}

void loop() {
  ts.execute();
}

bool redOE() {
  digitalWrite(8, HIGH);
  redToGreen = false;
  return true;
}

void redOD() {
  digitalWrite(8, LOW);
  green.restartDelayed();
}

bool greenOE() {
  digitalWrite(9, HIGH);
  return true;
}

void greenOD() {
  digitalWrite(9, LOW);
  greenBlink.restartDelayed();
  yellow.restartDelayed();
  right.restartDelayed();
}

bool greenState;
bool greenBlinkOE() {
  digitalWrite(9, HIGH);
  greenState = false;
  return true;
}

void greenBlinkCB()  {
  greenState = greenState ^ 1;
  digitalWrite(9, greenState ? HIGH : LOW);
}

void greenBlinkOD() {
  digitalWrite(9, LOW);
}

bool yellowOE() {
  digitalWrite(11, HIGH);
  return true;
}

void yellowOD() {
  digitalWrite(11, LOW);
  if(redToGreen == true)
  {
      red.restartDelayed();
  }
}

bool rightOE() {
  digitalWrite(13, HIGH);
  return true;
}

void rightOD() {
  digitalWrite(13, LOW);
  rightBlink.restartDelayed();
}

bool rightState;
bool rightBlinkOE() {
  digitalWrite(13, HIGH);
  rightState = true;
  return true;
}

void rightBlinkCB()  {
  rightState = rightState ^ 1;
  digitalWrite(13, rightState ? HIGH : LOW);
}

void rightBlinkOD() {
  digitalWrite(13, LOW);
  yellow_2.restartDelayed();
  redToGreen = true;
}