#include <Ticker.h>
#define printf(str) Serial.println(str);

#define RED   0
#define BLUE  1
#define GREEN 2

const int LED_RED = 32;
const int LED_GREEN = 33;
const int LED_BLUE = 25;

const int TRIG_A  = 23;
const int ECHO_A = 22;
const int TRIG_B = 19;
const int ECHO_B = 18;

int colores[10] = {LED_RED,LED_GREEN,LED_BLUE};
/*estructura general*/
struct systemFlags
{
  unsigned int movingCar      :1;
  unsigned int time_update    :1;
  unsigned int timeOut        :8;
  float time;
}bandera;

/*estructura para almacenar datos de distancia*/
struct measures
{
  float distanceR;
  float distanceL;
  float lastDistance;
  float average;
  float diffPercent;
  float diffPercetPass;
}medicion;
/*esta clase contiene los metodos para manejar los leds*/
class Semaforo
{
  private:
    unsigned char blueLigthPin;
    unsigned char greenLightPin;
    unsigned char redLightPin;
  
  public:
    Semaforo(unsigned char green, unsigned char blue, unsigned char red)
    : blueLigthPin(blue),greenLightPin(green),redLightPin(red){}
  
    void begin()
    {
      pinMode(blueLigthPin, OUTPUT);
      digitalWrite(blueLigthPin,HIGH);
      pinMode(greenLightPin, OUTPUT);
      digitalWrite(greenLightPin,HIGH);
      pinMode(redLightPin, OUTPUT);
      digitalWrite(redLightPin,HIGH);
    }
    void output_high(unsigned char led)
    {
      digitalWrite(led,LOW);
    }
    void output_low(unsigned char led)
    {
      digitalWrite(led, HIGH);
    }
    void toggle_set(unsigned char led)
    {
      digitalWrite(led,!digitalRead(led));
    }
};
/*estructura para manejar los sensores de distancia*/
class UltrasonicSensor 
{
  private:
    unsigned char trigPin;
    unsigned char echoPin;
    unsigned long timeout;

  public:
    UltrasonicSensor(unsigned char trig,unsigned char echo,unsigned long maxWaitMicros = 30000)
    : trigPin(trig), echoPin(echo),timeout(maxWaitMicros){}

    void begin()
    {
      pinMode(trigPin, OUTPUT);
      pinMode(echoPin, INPUT);
      digitalWrite(trigPin,LOW);
    }
    float readDistanceCM()
    {
      digitalWrite(trigPin, LOW);
      delayMicroseconds(2);
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin,LOW);

      unsigned long duration = pulseIn(echoPin,HIGH,timeout);

      float distance =  duration * 0.0343 / 2;

      if(distance < 2 || distance > 4000)
      {
        return - 1.0;
      }
      return distance;
    }
};

bool verifiedMoving(float actual, float last, float level)
{
  bool status = false;

  float diff = actual - last;

  if((diff > level) || (diff < -level))
  {
    status = true;
  }

  return status;
}

void colorLed(int led)
{

  for(int i = 0; i < 3; i++)
  {
    int color = colores[i];
    if(color == led)
    {
      digitalWrite(color,!digitalRead(led));
    }
    else
    {
      digitalWrite(color,HIGH);
    }
  }
}

bool timeOutRes(int t = 20)
{
  bool status = false;

  if(bandera.timeOut++ >= t)
  {
    bandera.timeOut = 0;
    status = true;
  }
  return status;
}

UltrasonicSensor sensor_A(TRIG_A, ECHO_A);
UltrasonicSensor sensor_B(TRIG_B, ECHO_B);

Semaforo ligth(LED_GREEN,LED_BLUE,LED_RED);

Ticker ticker1;

int state_machine = 0;

void setup() {
  bandera.time = 2;
  Serial.begin(9600);
  sensor_A.begin();
  sensor_B.begin();
  ligth.begin();

  ticker1.attach(bandera.time,updateValue);
  // put your setup code here, to run once:
}   

void loop() {
  if(bandera.time_update)
    bandera.time_update = statusSensor(state_machine);
}

/*
1-funcion que se ejecuta mediante interrupcion de ticker
*/
void updateValue(void)
{
  float distanceR = sensor_B.readDistanceCM();
  float distanceL = sensor_A.readDistanceCM();

  medicion.distanceR = distanceR;
  medicion.distanceL = distanceL;
  medicion.average = distanceL;//((distanceR + distanceL) / 2);

  bandera.time_update = 1;
}

unsigned int statusSensor(int state)
{
  medicion.diffPercent = (medicion.average * 100) / 400;

  switch(state)
  {
    case 0: //actualizacion de valores
    {
      medicion.diffPercetPass = medicion.diffPercent;
      state_machine = 1;
      ticker1.detach();
      ticker1.attach(2,updateValue);
      printf("case 0");
    }
    break;

    case 1: //accion despues de movimiento
    {
      if(verifiedMoving(medicion.diffPercent,medicion.diffPercetPass,2.0))
      {
        ticker1.detach();
        ticker1.attach(0.5,updateValue);
        state_machine = 2;
      }
      
      printf("case 1");
    }
    break;

    case 2: //verifica color de luz
    {
      printf("case 2" + String(medicion.average));

      if(medicion.average < 20) //rojo
      {
        colorLed(LED_RED);

        printf("peligro");
      }
      else if(medicion.average < 99) //naranja
      {
        colorLed(LED_BLUE);

        printf("advertencia");
      }
      else if(medicion.average > 100) //verde
      {
        colorLed(LED_GREEN);

        printf("libre");
      }

      if(!verifiedMoving(medicion.diffPercent,medicion.diffPercetPass,3.0))
      {
        printf("sin movimiento");
        if(timeOutRes())
        {
          state_machine = 0;
          digitalWrite(LED_GREEN, HIGH);
          digitalWrite(LED_BLUE, HIGH);
          digitalWrite(LED_RED, HIGH);
          break;
        }
      }
      else
      {
        bandera.timeOut = 0; //reset contador
      }
      medicion.diffPercetPass = medicion.diffPercent;

    }
    break;

    default:
    break;
  }
  return 0;
}