#include <Arduino.h>
#define L1_pin 6
#define L2_pin 7
#define L6_pin 11
#define L7_pin 12
#define S1_pin 2
#define S2_pin 3
#define t_ciclo 2000 
#define t_blink 300
#define t_double_click 500


typedef struct{ 
  int state , new_state;
  unsigned long tis , tes , current_time;
} fsm_t;

typedef struct{
  int S1 , S2;
} inputs_t;

typedef struct{
  int N_leds , blink, L7;
} outputs_t;

fsm_t fsm_main , fsm_blinking;
inputs_t input, prev_input;
outputs_t out;
unsigned long now , last , interval;


void init_pins(){
  for ( int i = L1_pin ; i <= L7_pin ; i++){
    pinMode( i , OUTPUT);
  }
  pinMode( S1_pin , INPUT_PULLUP);
  pinMode( S2_pin , INPUT_PULLUP);
}

void setup() {
  // put your setup code here, to run once:
  Serial1.begin(115200);
  init_pins();

  interval = 10;
}

void print_(){
  Serial1.print("S1: "); Serial1.print(input.S1);
  Serial1.print(" S2: ") ; Serial1.print(input.S2);
  Serial1.print(" LED1: ");
  if (out.L7 == 1){ Serial1.print("on");}
  else if (out.L7 == 0){ Serial1.print("off");}
  Serial1.print(" LED2: ");
  if (out.L7 == 1){ Serial1.println("on");}
  else if (out.L7 == 0){ Serial1.println("off");}

}

void read_inputs(){
  input.S1 = ! digitalRead(S1_pin);
  input.S2 = ! digitalRead(S2_pin);
}

void reset_tis(fsm_t& fsm){
  fsm.tes = millis();
  fsm.tis = millis() - fsm.tes;
}

void pause_time(fsm_t& fsm){
  fsm.current_time = fsm.tis * 1;
}

void resume_time(fsm_t& fsm){
  fsm.tes = millis() - fsm.current_time;
  fsm.tis = millis() - fsm.tes;
}

void main_calc_next_state(){
  if (fsm_main.state == 0 && input.S1){
    fsm_main.new_state = 1;
    out.N_leds = 6;
  }

  if (fsm_main.state == 1 && input.S1){
    reset_tis(fsm_main);
    out.N_leds = 6;
  }

  else if ( fsm_main.state == 1 && fsm_main.tis >= t_ciclo / 2){
    reset_tis(fsm_blinking);
    fsm_blinking.new_state = 0;
    fsm_main.new_state = 2;
  }

  else if ( fsm_main.state == 1 && out.N_leds == 0 ){
    fsm_main.new_state = 3;
  }

  else if ( fsm_main.state == 1 && !prev_input.S2 && input.S2 ){
    pause_time(fsm_main);
    fsm_main.new_state = 4;
  }

  if ( fsm_main.state == 2 && fsm_main.tis >= t_ciclo / 2){
    reset_tis(fsm_blinking);
    out.N_leds -= 1;
    fsm_main.new_state = 1;
  }

  else if ( fsm_main.state == 2 && input.S1){
    reset_tis(fsm_main);
    out.N_leds = 6;
    fsm_main.new_state = 1;
  }

  else if ( fsm_main.state == 2 && !prev_input.S2 && input.S2){
    pause_time(fsm_main);
    fsm_main.new_state = 6;
  }

  if ( fsm_main.state == 3 && input.S1){
    out.N_leds = 6;
    fsm_main.new_state = 1;
  }

  if ( fsm_main.state == 4 && fsm_main.tis >= t_double_click ){
    fsm_main.new_state = 5;
  }

  else if ( fsm_main.state == 4 && !prev_input.S2 && input.S2 ){
    resume_time(fsm_main);
    out.N_leds += 1;
    fsm_main.new_state = 1;
  }

  if ( fsm_main.state == 5 && !prev_input.S2 && input.S2 ){
    resume_time(fsm_main);
    fsm_main.new_state = 1;
  }

  if ( fsm_main.state == 6 && !prev_input.S2 && input.S2 ){
    resume_time( fsm_main );
    out.N_leds += 1;
    fsm_main.new_state = 2;
  }

  else if ( fsm_main.state == 6 && fsm_main.tis >= t_double_click){
    fsm_main.new_state = 7;
  }

  if ( fsm_main.state == 7 && !prev_input.S2 && input.S2 ){
    resume_time( fsm_main );
    fsm_main.new_state = 2;
  }
}

void blinking_calc_next_state(){
  if (fsm_blinking.state == 0 && fsm_blinking.tis >= t_blink){
    fsm_blinking.new_state = 1;
  }

  else if (fsm_blinking.state == 1 && fsm_blinking.tis >= t_blink){
    fsm_blinking.new_state = 0;
  }
}


void update_state(fsm_t& fsm){

  if ( fsm.state != fsm.new_state ){
    fsm.state = fsm.new_state * 1;
    fsm.tes = millis();
    fsm.tis = millis() - fsm.tes;
    
  }
}

void main_calc_outputs(){
  if (fsm_main.state == 0){
    out.N_leds = 0;
    out.L7 = 0;
  }

  else if ( fsm_main.state == 3){
    out.L7 = 1;
  }

  else{
    out.L7 = 0;
  }
}

void blinking_calc_outputs(){
  if (fsm_blinking.state == 0){
    out.blink = 0;
  }

  else if ( fsm_blinking.state == 1){
    out.blink = 1;
  }
}

void update_outputs(){
  if (fsm_main.state == 5 || fsm_main.state == 7){
    for (int i = L6_pin ; i > L6_pin - out.N_leds ; --i){
    digitalWrite( i , out.blink );
    }

    for (int i = L6_pin - out.N_leds ; i >= L1_pin - 1 ; --i){
    digitalWrite( i , 0 );
    }
  }

  else if ( fsm_main.state == 2){
    digitalWrite( L6_pin - out.N_leds + 1 , out.blink);
    for (int i = L6_pin ; i > L6_pin - out.N_leds + 1 ; --i){
    digitalWrite( i , 1 );
    }

    for (int i = L6_pin - out.N_leds ; i >= L1_pin - 1 ; --i){
    digitalWrite( i , 0 );
    }
  }

  else{
    for (int i = L6_pin ; i > L6_pin - out.N_leds ; --i){
    digitalWrite( i , 1 );
    }

    for (int i = L6_pin - out.N_leds ; i >= L1_pin - 1 ; --i){
      digitalWrite( i , 0 );
    }
  }
  
  digitalWrite( L7_pin , out.L7);
}



void loop() {
  // put your LED1 code here, to run repeatedly:
  delay(1); // this speeds up the simulation

  now = millis();
  if ( now - last >= interval ){
    last = millis();

    prev_input = input;
    fsm_main.tis = millis() - fsm_main.tes;
    fsm_blinking.tis = millis() - fsm_blinking.tes;

    read_inputs();

    main_calc_next_state();
    update_state(fsm_main);

    if (fsm_main.state == 2 || fsm_main.state == 5 || fsm_main.state == 7){
      blinking_calc_next_state();
      update_state(fsm_blinking);
    }

    main_calc_outputs();
    blinking_calc_outputs();

    update_outputs();

  }
}