#define BAUD_RATE 9600

#define LEFT_RED_PIN         2
#define LEFT_YELLOW_PIN      3
#define LEFT_GREEN_PIN       4
#define LEFT_GREEN_ARROW_PIN 8

#define TOP_GREEN_PIN        5
#define TOP_YELLOW_PIN       6
#define TOP_RED_PIN          7
#define TOP_RED_HUMAN_PIN    9
#define TOP_GREEN_HUMAN_PIN 10

#define TRAIN_PIN_1 11
#define TRAIN_PIN_2 12

#define NORMAL     0
#define TRAIN      1
#define EMERGENCY  2

#define GO_TOP_STATE      0
#define WAIT_TOP_STATE    1 
#define GO_LEFT_R_STATE   2
#define WAIT_LEFT_R_STATE 3 
#define GO_LEFT_STATE     4
#define WAIT_LEFT_STATE   5
#define TRAIN_STATE       6
#define TRAIN_STATE_2     7
#define EMERGENCY_STATE   8
#define EMERGENCY_STATE_2 9

/* 
  Bits distributions in ascending order (11 bits):
    2 - train      (r, r)
    2 - pedestrian (r, g)
    4 - left       (r, y, left g, g)
    3 - top        (r, y, g)
*/

#define GO_TOP_OUT           0b00101000001
#define WAIT_TOP_OUT         0b00101000010
#define GO_LEFT_ARROW_OUT    0b00011001100
#define WAIT_LEFT_ARROW_OUT  0b00011100100
#define GO_LEFT_OUT          0b00010010100
#define WAIT_LEFT_OUT        0b00100100110
#define TRAIN_OUT_1          0b10101000100
#define TRAIN_OUT_2          0b01101000100
#define EMERGENCY_OUT        0b11010100010
#define EMERGENCY_OUT_2      0b00010000000

#define GO_TOP_TIME      2000
#define WAIT_TOP_TIME    2000
#define GO_LEFT_R_TIME   2000
#define WAIT_LEFT_R_TIME 2000
#define GO_LEFT_TIME     2000
#define WAIT_LEFT_TIME   2000
#define GO_TRAIN_TIME    500
#define EMERGENCY_TIME   500

struct State {
  unsigned long Out; 
  unsigned long Time; 
  unsigned long Next[3];
}; 

typedef const struct State FSM_type;

FSM_type FSM[20]={
  { GO_TOP_OUT, GO_TOP_TIME,             { WAIT_TOP_STATE,    TRAIN_STATE,   EMERGENCY_STATE }},
  { WAIT_TOP_OUT, WAIT_TOP_TIME,         { GO_LEFT_R_STATE,   TRAIN_STATE,   EMERGENCY_STATE }},
  { GO_LEFT_ARROW_OUT, GO_LEFT_R_TIME,   { WAIT_LEFT_R_STATE, TRAIN_STATE,   EMERGENCY_STATE }},
  { WAIT_LEFT_ARROW_OUT, WAIT_LEFT_TIME, { GO_LEFT_STATE,     TRAIN_STATE,   EMERGENCY_STATE }}, 
  { GO_LEFT_OUT, GO_LEFT_TIME,           { WAIT_LEFT_STATE,   TRAIN_STATE,   EMERGENCY_STATE }},
  { WAIT_LEFT_OUT, WAIT_LEFT_TIME,       { GO_TOP_STATE,      TRAIN_STATE,   EMERGENCY_STATE }}, 
  { TRAIN_OUT_1, GO_TRAIN_TIME,          { GO_TOP_STATE,      TRAIN_STATE_2, EMERGENCY_STATE }},
  { TRAIN_OUT_2, GO_TRAIN_TIME,          { GO_TOP_STATE,      TRAIN_STATE,   EMERGENCY_STATE }},
  { EMERGENCY_OUT, EMERGENCY_TIME,       { GO_TOP_STATE,      TRAIN_STATE,   EMERGENCY_STATE_2 }},
  { EMERGENCY_OUT_2, EMERGENCY_TIME,     { GO_TOP_STATE,      TRAIN_STATE,   EMERGENCY_STATE }},
};

int FSM_State;
String user_input;

int get_FSM_input() {
  if (Serial.available() > 0) {
    user_input = Serial.readString();
    user_input.trim();

    if (user_input == "normal") {
      Serial.println("(Serial): Normal simulation");
      return NORMAL;
    } else if (user_input == "train") {
      Serial.println("(Serial): Train is coming.");
      return TRAIN;
    } else if (user_input == "emergency") {
      Serial.println("(Serial): Careful on the road.");
      return EMERGENCY;
    } else {
      Serial.println("(Serial): Unknown command.");
    }
  }
}

void get_FSM_output(int out) {
  int ledState;
  ledState = (out & (1 << 10)) ? HIGH : LOW;
  digitalWrite(TRAIN_PIN_2, ledState);

  ledState = (out & (1 << 9)) ? HIGH : LOW;
  digitalWrite(TRAIN_PIN_1, ledState);

  ledState = (out & (1 << 8)) ? HIGH : LOW;
  digitalWrite(TOP_RED_HUMAN_PIN, ledState);

  ledState = (out & (1 << 7)) ? HIGH : LOW;
  digitalWrite(TOP_GREEN_HUMAN_PIN, ledState);

  ledState = (out & (1 << 6)) ? HIGH : LOW;
  digitalWrite(LEFT_RED_PIN, ledState);

  ledState = (out & (1 << 5)) ? HIGH : LOW;
  digitalWrite(LEFT_YELLOW_PIN, ledState);

  ledState = (out & (1 << 4)) ? HIGH : LOW;
  digitalWrite(LEFT_GREEN_PIN, ledState);

  ledState = (out & (1 << 3)) ? HIGH : LOW;
  digitalWrite(LEFT_GREEN_ARROW_PIN, ledState);
  
  ledState = (out & (1 << 2)) ? HIGH : LOW;
  digitalWrite(TOP_RED_PIN, ledState);
  
  ledState = (out & (1 << 1)) ? HIGH : LOW;
  digitalWrite(TOP_YELLOW_PIN, ledState);
  
  ledState = (out & (1 << 0)) ? HIGH : LOW;
  digitalWrite(TOP_GREEN_PIN, ledState);
}

void setup() {
  Serial.begin(BAUD_RATE);
  pinMode(LEFT_GREEN_PIN,       OUTPUT);
  pinMode(LEFT_YELLOW_PIN,      OUTPUT);
  pinMode(LEFT_RED_PIN,         OUTPUT);
  pinMode(LEFT_GREEN_ARROW_PIN, OUTPUT);
  
  pinMode(TOP_GREEN_PIN,        OUTPUT);
  pinMode(TOP_YELLOW_PIN,       OUTPUT);
  pinMode(TOP_RED_PIN,          OUTPUT);
  pinMode(TOP_RED_HUMAN_PIN,    OUTPUT);
  pinMode(TOP_GREEN_HUMAN_PIN,  OUTPUT);

  pinMode(TRAIN_PIN_1,          OUTPUT);
  pinMode(TRAIN_PIN_2,          OUTPUT);

  Serial.println("===== COMMANDS =====");
  Serial.println("normal - default behaviour of traffic lights");
  Serial.println("train - turns all red traffic lights");
  Serial.println("emergency - turns all yellow traffic lights");

  FSM_State = GO_TOP_STATE;
}

int FSM_Output, FSM_Input;
void loop() {
  FSM_Output = FSM[FSM_State].Out;
  get_FSM_output(FSM_Output);
  delay(FSM[FSM_State].Time);

  FSM_Input = get_FSM_input();
  FSM_State = FSM[FSM_State].Next[FSM_Input];
}