#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 21
#define Ledkeepalive 23
#define Esperando2 24

// Switches
int Tracker;
int Todo;
int Modificacion;


// Pines
#define LdrIzq A0
#define LdrDer A1
#define MotDer 8
#define MotIzq 9
#define Viento 6
#define LED_KEEP_ALIVE 7
#define Bot 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(Bot, INPUT_PULLUP);
  Ms = 0;
  LKA = 0;
  Timer1.initialize(1000);  // Interrumpe cada 1 mSeg
  Timer1.attachInterrupt(ISR_Timer);
  Tracker = ARRIBA;
  Modificacion = 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 (Modificacion) {
    case FUNCANDO:
      Trabajo_Andando();
      if (digitalRead(Bot) == LOW) {
        Ms = 0;
        Modificacion = Esperando;
      }
      break;
    case Esperando:
      if (digitalRead(Bot) == LOW && Ms >= 3000) {
        Modificacion = Ledkeepalive;
      }
      if (digitalRead(Bot) == HIGH && Ms < 3000) {
        Modificacion = FUNCANDO;
      }
      break;
    case Ledkeepalive:
      if (LKA >= 500) {
        digitalWrite(LED_KEEP_ALIVE, !digitalRead(LED_KEEP_ALIVE));
        LKA = LKA - 500;
       
      }
      if (digitalRead(Bot) == HIGH) {
        Ms = 0;
        Modificacion = Esperando2;
     
      }
      
      break;
    case Esperando2:
    if (LKA >= 500) {
        digitalWrite(LED_KEEP_ALIVE, !digitalRead(LED_KEEP_ALIVE));
        LKA = LKA - 500;
       
      }
      if (digitalRead(Bot) == HIGH && Ms >= 5000) {
        Modificacion = FUNCANDO;
      }
      if (digitalRead(Bot) == LOW) {
        Modificacion = Ledkeepalive;
      }
        break;
      
  }
}



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



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