#include <TimerOne.h>

int Ms;
float Lect;
int LKA;

// Máquinas de estado
#define IZQUIERDA 8
#define DERECHA 7
#define ARRIBA 0
#define INIT 0
#define Config 9
#define SistOff 10
#define No_Hay_Viento 11
#define Config_No_Hay_Viento 12
#define FUNCANDO 18
#define ESPERANDO 19
#define APAGANDO 20

// Switches
int Tracker;
int Todo;
int Boton_Off;

// Pines
#define LdrIzq A0
#define LdrDer A1
#define MotDer 8
#define MotIzq 9
#define Viento 6
#define LED_KEEP_ALIVE 7
#define BotiOff 5

void Sol() {
  switch (Tracker) {
    case ARRIBA:
      digitalWrite(MotDer, LOW);
      digitalWrite(MotIzq, LOW);
      if (Lect > 0.5) {
        Tracker = IZQUIERDA;
      } else if (Lect < 0.5) {
        Tracker = DERECHA;
      }
      break;

    case IZQUIERDA:
      digitalWrite(MotIzq, HIGH);
      digitalWrite(MotDer, LOW);
      if (Lect < 0.5) {
        Tracker = DERECHA;
      } else if (Lect == 0.5) {
        Tracker = ARRIBA;
      }
      break;

    case DERECHA:
      digitalWrite(MotDer, HIGH);
      digitalWrite(MotIzq, LOW);
      if (Lect == 0.5) {
        Tracker = ARRIBA;
      } else if (Lect > 0.5) {
        Tracker = IZQUIERDA;
      }
      break;
  }
}

void setup() {
  pinMode(LdrIzq, INPUT);
  pinMode(LdrDer, INPUT);
  pinMode(MotDer, OUTPUT);
  pinMode(MotIzq, OUTPUT);
  pinMode(Viento, INPUT_PULLUP);
  pinMode(LED_KEEP_ALIVE, OUTPUT);
pinMode(BotiOff, INPUT_PULLUP);
  Ms = 0;
  LKA = 0;
  Timer1.initialize(1000);  // Interrumpe cada 1 mSeg
  Timer1.attachInterrupt(ISR_Timer);
  Tracker = ARRIBA;
  Boton_Off = FUNCANDO;
  Serial.begin(9600);
}

void loop() {
  Mod();
  float Sensor = analogRead(LdrIzq) - analogRead(LdrDer);
  Lect = Sensor * 5.0 / 1023.0;
  Serial.println(Ms);
LKP();
 
}

void Trabajo_Andando(void) {
  switch (Todo) {
    case INIT:
      Sol();
      if (digitalRead(Viento) == LOW) {
        Ms = 0;
        Todo = Config;
      }
      break;

    case Config:
      Sol();
      if (digitalRead(Viento) == LOW && Ms >= 3000) {
        Todo = SistOff;
      } if (digitalRead(Viento) == HIGH) {
        Todo = INIT;
      }
      break;

    case SistOff:
      digitalWrite(MotDer, LOW);
      digitalWrite(MotIzq, LOW);
      if (digitalRead(Viento) == HIGH) {
        Ms = 0;
        Todo = No_Hay_Viento;
      }
      break;

    case No_Hay_Viento:
      Ms = 0;
      Todo = Config_No_Hay_Viento;
      break;

    case Config_No_Hay_Viento:
      if (digitalRead(Viento) == HIGH && Ms >= 5000) {
        Todo = Config;
      } if (digitalRead(Viento) == LOW) {
        Todo = No_Hay_Viento;
      }
      break;
  }
}




void Mod () 
{
switch (Boton_Off) {
case FUNCANDO:
 Trabajo_Andando();
if (digitalRead(BotiOff) == LOW)
{
Ms = 0;
Boton_Off = ESPERANDO;
}
break;

case ESPERANDO:
 Trabajo_Andando();
if (digitalRead(BotiOff) == LOW && Ms >= 3000)
{
Boton_Off = APAGANDO;
}

if (digitalRead(BotiOff) == HIGH)
{
Boton_Off = FUNCANDO;
}
break;

case APAGANDO:

 digitalWrite(MotDer, LOW);
 digitalWrite(MotIzq, LOW);
digitalWrite(LED_KEEP_ALIVE, LOW);


}
}


void LKP ()
{
  if (LKA >= 1000) {
    digitalWrite(LED_KEEP_ALIVE, !digitalRead(LED_KEEP_ALIVE));
    LKA = LKA - 1000;
  }
}



void ISR_Timer(void) {
  Ms++;
LKA++;
}