#include <Ultrasonic.h>

#define PINO_TRG  3
#define PINO_ECHO 2

float distancia_minima = 5;
float distancia_maxima = 10;

Ultrasonic ultrasonic(PINO_TRG, PINO_ECHO);

//INPUT FSM0
#define FSM0_SFCF_pin 8
bool FSM0_SFCF;
//INPUT FSM1
#define FSM1_SFCA_pin 7
bool FSM1_SFCA;

//OUTPUT FSM0
int FSM0_SUBIR = A1;
int FSM0_DESCER = A2;
int FSM0_PARADO = A3;
//OUTPUT FSM1
int FSM1_APORTA = A4;
int FSM1_FPORTA = A5;

static int FSM0_state = 1; // initial state is 1, the "idle" state.
static int FSM1_state = 1; // initial state is 1, the "idle" state.
static unsigned long FSM1_ts; // To store the "current" time for delays.

void setup(){
  pinMode (FSM0_SFCF, INPUT);
  pinMode (FSM1_SFCA, INPUT);
  pinMode (FSM0_SUBIR, OUTPUT); pinMode (FSM0_DESCER, OUTPUT); pinMode(FSM0_PARADO, OUTPUT);
  pinMode (FSM1_APORTA, OUTPUT); pinMode (FSM1_FPORTA, OUTPUT);
  Serial.begin(9600);
}

void loop(){

  float cmMsec;

  long microsec = ultrasonic.timing();

  cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);

  
  int Pinloc0 = 12;
  int Pinloc1 = 11;
  int Pinloc2 = 10;
  int Pinloc3 = 9;

  int CA;
  int SA;

  bool E_MOV_DESC;
  bool E_MOV_SUB;
  bool E_MOV_PARA;

  bool FSM1_E_time;
  int TempoEspera=2000;

  CA = Binary2Decimal(Pinloc0, Pinloc1);
  SA = Binary2Decimal(Pinloc2, Pinloc3);
  Serial.print("CA: ");
  Serial.println(CA);
  Serial.print("SA: ");
  Serial.println(SA);
  delay(500); 

  FSM0_SFCF = digitalRead(8);
  FSM1_SFCA = digitalRead (7);
  
  if(CA < SA && FSM0_SFCF == true){E_MOV_DESC = true;}else{E_MOV_DESC = false;}
  if(CA > SA && FSM0_SFCF == true){E_MOV_SUB = true;}else{E_MOV_SUB = false;}
  if(CA == SA && FSM0_SFCF == ture){E_MOV_PARA = true;}else{E_MOV_PARA = false;}
  if(millis() - FSM1_ts < TempoEspera){FSM1_E_time = true;}else{FSM1_E_time = false;}
  
  bool PF;
  if(FSM1_state == 1){PF = true;}
  else{PF = false;}
    
  FSM0(E_MOV_DESC, E_MOV_SUB, E_MOV_PARA, PF);
  FSM1(FSM0_SFCF, FSM1_SFCA, E_MOV_PARA, PF, cmMsec);
}

int Binary2Decimal(int x0, int x1)
{
  int Sum = 0;
  
  if (digitalRead(x0) == HIGH)
    Sum += 1;
  if (digitalRead(x1) == HIGH)
    Sum += 2;
  return(Sum);
}

void FSM0(bool E_MOV_DESC, bool E_MOV_SUB, bool E_MOV_PARA, bool PF) 
{
  FSM0_next_state(E_MOV_DESC, E_MOV_SUB, E_MOV_PARA, PF); 
  FSM0_output();
}

void FSM0_next_state(bool E_MOV_DESC, bool E_MOV_SUB, bool E_MOV_PARA, bool PF) 
{
  switch(FSM0_state)
  {
    case 1:
      if(E_MOV_SUB == true){FSM0_state = 2;}
      if(E_MOV_DESC == true){FSM0_state = 3;}
      if(PF == false){FSM0_state = 1;}
      break;
    case 2:
      if(E_MOV_PARA == true){FSM0_state = 1;}
      break;
    case 3:
      if(E_MOV_PARA == true){FSM0_state = 1;}
      break;
  }

    Serial.print(FSM0_state);
    Serial.print("<-FSM0_state");
    Serial.println();
    delay(1000);
}

void FSM0_output()
{
  switch (FSM0_state)
  {
    case 1:
      digitalWrite(A3, HIGH); //STOP
      digitalWrite(A2, LOW); //DOWN
      digitalWrite(A1, LOW); //UP
      break;
    case 2:
      digitalWrite(A1, HIGH); //UP
      digitalWrite(A3, LOW); //STOP
      digitalWrite(A2, LOW); //DOWN
      digitalWrite(A4, LOW); //OPEN
      digitalWrite(A5, LOW); //CLOSE
      break;
    case 3:
      digitalWrite(A2, HIGH); //DOWN
      digitalWrite(A3, LOW); //STOP
      digitalWrite(A1, LOW); //UP
      digitalWrite(A4, LOW); //OPEN
      digitalWrite(A5, LOW); //CLOSE
      break;
  }
}

void FSM1(bool FSM0_SFCF,bool FSM1_SFCA,bool E_MOV_PARA, bool FSM1_E_time, float cmMsec)
{
  FSM1_next_state(FSM0_SFCF, FSM1_SFCA, E_MOV_PARA, FSM1_E_time, cmMsec);
  FSM1_output();
}

void FSM1_next_state(bool FSM0_SFCF, bool FSM1_SFCA, bool E_MOV_PARA, bool FSM1_E_time, float cmMsec)
{
  switch(FSM1_state)
  {
    case 1:
      if(E_MOV_PARA == true){FSM1_state = 2;}
      break;
    case 2:
      if(FSM1_SFCA == true){FSM1_state = 3;}
      break;
    case 3:
      if(FSM1_E_time == true){FSM1_state = 3;}else{FSM1_state = 4;}
      break;
    case 4:
      if(FSM0_SFCF == true){FSM1_state = 5;}
      if(cmMsec < distancia_minima){FSM1_state = 2;}
      break;
    case 5:
      if(E_MOV_PARA == false){FSM1_state = 1;}
      break;    
  }
    Serial.print(FSM1_state);
    Serial.print("<-FSM1_state");
    Serial.println();
    delay(1000);
}

void FSM1_output()
{
  switch (FSM1_state)
  {
    case 1:
      digitalWrite(A4, LOW); //OPEN
      digitalWrite(A5, LOW); //CLOSE
      break;
    case 2:
      digitalWrite(A4, HIGH); //OPEN
      digitalWrite(A5, LOW); //CLOSE
      break;
    case 3:
      digitalWrite(A4, LOW); //OPEN
      digitalWrite(A5, LOW); //CLOSE
      break;
    case 4:
      digitalWrite(A4, LOW); //OPEN
      digitalWrite(A5, HIGH); //CLOSE
      break;
    case 5:
      digitalWrite(A4, LOW); //OPEN
      digitalWrite(A5, LOW); //CLOSE
      break;
  }
}