/*
Inputs:

DRONE_IN : el drone se guarda y vuelve a la posicion de home (drone centrado y
pad retraido)

DRONE_OUT : el pad sale y el drone se libera

STOP : detiene todo. despues con cualquieras de los otros mensajes reanuda. Si
haces DRONE_IN, se guarda, si haces DRONE_OUT, se guarda para hacer el auto home
y desp vuelve a salir solo.

OPERATION: operacion con motores habilitados

SIMULATION: deshabilita motores para operar en simulacion

DRONE_TURN_ON / DRONE_TURN_OFF : mueve servo para encender/apagar drone
(funciona solo cuando el pad está retraido y el drone centrado)

RC_TURN_ON / RC_TURN_OFF : mueve servo para encender/apagar radiocontrol
(funciona solo cuando el pad está retraido y el drone centrado)

ODROID_TURN_ON / ODROID_TURN_OFF : activa/desactiva relé para encender/apagar
odroid (funciona solo cuando el pad está retraido y el drone centrado)

-------------------------------
Outputs:
DRONE_CENTERED_OK
DRONE_RELEASED_OK
DRONE_CENTERING
DRONE_RELEASING
DRONE_UNKNOWN_POSITION
PAD_EXTENDED_OK
PAD_RETRACTED_OK
PAD_EXTENDING
PAD_RETRACTING
PAD_UNKNOWN_POSITION


*/

#include <Adafruit_Sensor.h>
#include <DHT.h>
#include <ESP32Servo.h>
#include "funciones.h"

void setup()
{
  Serial.begin(9600);
  while (!Serial)
  {
    ; // wait for serial port to connect. Needed for native USB
  }

  pinMode(EN, OUTPUT);

  // FOR CNC SHIELD
  pinMode(X_STP, OUTPUT);
  pinMode(X_DIR, OUTPUT);

  pinMode(DC_MOTOR_LEN, OUTPUT);
  pinMode(DC_MOTOR_REN, OUTPUT);
  pinMode(DC_MOTOR_LPWM, OUTPUT);
  pinMode(DC_MOTOR_RPWM, OUTPUT);

  digitalWrite(DC_MOTOR_LEN, HIGH);
  digitalWrite(DC_MOTOR_REN, HIGH);

  pinMode(X_LIM, INPUT_PULLUP);
  pinMode(Y_LIM, INPUT_PULLUP);
  pinMode(Z_LIM, INPUT_PULLUP);

  SERVO_DRONE.attach(18); // 
  SERVO_RC.attach(19);    // 
  // SET SERVO ANGLES??? -------------------------------------------------------?

  pinMode(RELAY_ODROID, OUTPUT);
  pinMode(RELAY_CHARGER, OUTPUT);
  pinMode(RELAY_FAN, OUTPUT);

  pinMode(DHTPIN, INPUT); // temp & humidity
  dht.begin();

  pinMode(RC_LDRPIN, INPUT);

  Serial.println("SETUP_OK");

  // drone_in();
  // AUTO HOME
  pad_retract();
  center_drone();
  digitalWrite(RELAY_CHARGER, HIGH);
  digitalWrite(RELAY_ODROID, HIGH);
  fan_on();
}

void loop()
{
  unsigned long microsCurrent = micros();

  if (Serial.available() > 0)
  {
    // read the incoming byte:
    incoming_msj = Serial.readString();

    Serial.print("Received: ");
    Serial.println(incoming_msj);
  }

  // READ TEMPERATURE
  if (microsCurrent - microsDHT > 10000000) // aumentar valor
  {
    microsDHT = microsCurrent;

    float h = dht.readHumidity();    // Leemos la Humedad
    float t = dht.readTemperature(); // Leemos la temperatura en grados Celsius

    // Serial.print("Humedad: ");
    // Serial.println(h);
    Serial.print("TEMP");
    Serial.print(t);
    Serial.println("C");
  }

  // READ RC_LIGHT
  if (microsCurrent - microsRC_LDR > 200000)
  {
    microsRC_LDR = microsCurrent;

    // RC_LIGHT = digitalRead(RC_LDRPIN); //Esto solo se usa por el Serial
    // Serial.println (RC_LIGHT);

    if (digitalRead(RC_LDRPIN)) // En caso de usar RC_LIGHT, reemplazar
    {
      microsRC_ON = microsCurrent;
      RC_IS_ON = true;
      if (microsCurrent - microsRC_Log > 4500000){
        microsRC_Log = microsCurrent;
        Serial.println("RC_STATE_ON");
      }
    }    

    if (microsCurrent - microsRC_ON > 5000000)
    {
      RC_IS_ON = false;
      if (microsCurrent - microsRC_Log > 4500000){
        microsRC_Log = microsCurrent;
        Serial.println("RC_STATE_OFF");
      }
    }
  }

  // DIGITAL END_SWITCHES ACTIVATION - NO INTERNAL PULL_UP - NORMALY CLOSED
  if (x_state == DRONE_CENTERING && simulation_running == false)
  {
    if (digitalRead(X_LIM))
    {
      x_end = true;
      x_steps = 0;
    }
  }
  else
  {
    x_end = false;
  }

  if (y_state == PAD_RETRACTING && digitalRead(Y_LIM))
  {
    pad_ended();
    y_state = PAD_RETRACTED;
  }

  if (y_state == PAD_EXTENDING && digitalRead(Z_LIM))
  {
    pad_ended();
    y_state = PAD_EXTENDED;
  }

  // SERVOS
  if (drone_servo_activated)
  {
    if (drone_servo_move == 1)
    {
      SERVO_DRONE.write(DRONE_SERVO_PRESS_ANGLE);
      drone_servo_move = 2;
      drone_servo_micros = microsCurrent;
    }

    if ((drone_servo_move == 2) && (microsCurrent - drone_servo_micros > SERVO_SHORT_PRESS))
    {
      SERVO_DRONE.write(DRONE_SERVO_RELEASE_ANGLE);
      drone_servo_move = 3;
      drone_servo_micros = microsCurrent;
    }

    if ((drone_servo_move == 3) && (microsCurrent - drone_servo_micros > SERVO_WAIT))
    {
      SERVO_DRONE.write(DRONE_SERVO_PRESS_ANGLE);
      drone_servo_move = 4;
      drone_servo_micros = microsCurrent;
    }

    if ((drone_servo_move == 4) && (microsCurrent - drone_servo_micros > SERVO_LONG_PRESS))
    {
      SERVO_DRONE.write(DRONE_SERVO_RELEASE_ANGLE);
      drone_servo_move = 0;
      drone_servo_micros = 0;
      drone_servo_activated = false;
    }
  }

  if (rc_servo_activated)
  {
    if (rc_servo_move == 1)
    {
      SERVO_RC.write(RC_SERVO_PRESS_ANGLE);
      rc_servo_move = 2;
      rc_servo_micros = microsCurrent;
    }

    if ((rc_servo_move == 2) && (microsCurrent - rc_servo_micros > SERVO_SHORT_PRESS))
    {
      SERVO_RC.write(RC_SERVO_RELEASE_ANGLE);
      rc_servo_move = 3;
      rc_servo_micros = microsCurrent;
    }

    if ((rc_servo_move == 3) && (microsCurrent - rc_servo_micros > SERVO_WAIT))
    {
      SERVO_RC.write(RC_SERVO_PRESS_ANGLE);
      rc_servo_move = 4;
      rc_servo_micros = microsCurrent;
    }

    if ((rc_servo_move == 4) && (microsCurrent - rc_servo_micros > SERVO_LONG_PRESS))
    {
      SERVO_RC.write(RC_SERVO_RELEASE_ANGLE);
      rc_servo_move = 0;
      rc_servo_micros = 0;
      rc_servo_activated = false;
    }
  }

  // TIMEOUT CONTROL

  if ((x_state == DRONE_CENTERING) && (microsCurrent - microsxTimer > X_AXIS_TIMEOUT)) // timeout centering
  {
    if (simulation_running)
    {
      x_state = DRONE_CENTERED;
      x_steps = SWITCH_CLEARANCE;
      Serial.println("simulation_timeout_centered");
    }
    else
    {
      mec_cent_stop();
      Serial.println("TIMEOUT_X_CENTERING");
    }
  }

  if ((x_state == DRONE_RELEASING) && (microsCurrent - microsxTimer > X_AXIS_TIMEOUT)) // timeout releasing
  {
    if (simulation_running)
    {
      x_state = DRONE_RELEASED;
      x_steps = X_AXIS_STEPS;
      Serial.println("simulation_timeout_released");
    }
    else if (simulation_running == false)
    {
      mec_cent_stop();
      Serial.println("TIMEOUT_X_RELEASING");
    }
  }

  if ((y_state == PAD_RETRACTING) && (microsCurrent - microsyTimer > Y_AXIS_TIMEOUT)) // timeout retracting
  {
    if (simulation_running)
    {
      y_state = PAD_RETRACTED;
      Serial.println("simulation_timeout_retracted");
    }
    else if (simulation_running == false)
    {
      pad_stop();
      Serial.println("TIMEOUT_Y_RETRACTING");
    }
  }

  if ((y_state == PAD_EXTENDING) && (microsCurrent - microsyTimer > Y_AXIS_TIMEOUT)) // timeout releasing
  {
    if (simulation_running)
    {
      y_state = PAD_EXTENDED;
      Serial.println("simulation_timeout_extended");
    }
    else if (simulation_running == false)
    {
      pad_stop();
      Serial.println("TIMEOUT_Y_EXTENDING");
    }
  }

  // PRINT DRONE STATES
    /*Imprime una bitácora periódica del dron.*/
    /*No tengo en claro por qué se actualiza el temporizador del log de pad con el de centrado. Dada la configuración actual creo que se puede simplemente usar un solo microsxyLog para ambas porque de todas formas los logs van a ser siempre una de x con cada 2 de y.*/

  if (microsCurrent - microsxyLog >= 2000000)
  { // Cada 2 segundos
    microsxyLog = microsCurrent;
    switch (x_state)
    { // Comunuica el estado de centrado
    case DRONE_CENTERED:
      Serial.println("DRONE_CENTERED_OK");
      break;
    case DRONE_RELEASED:
      Serial.println("DRONE_RELEASED_OK");
      break;
    case DRONE_CENTERING:
      Serial.println("DRONE_CENTERING");
      break;
    case DRONE_RELEASING:
      Serial.println("DRONE_RELEASING");
      break;
    case UNKNOWN_POSITION:
      Serial.println("DRONE_UNKNOWN_POSITION");
      break;
    default:
      break;
    }
  }

  // PRINT PAD STATES
  if (microsCurrent - microsxyLog > 1000000)
  { // Cada segundo
    microsxyLog = microsCurrent;
    switch (y_state)
    {
    case PAD_EXTENDED:
      Serial.println("PAD_EXTENDED_OK");
      break;
    case PAD_RETRACTED:
      Serial.println("PAD_RETRACTED_OK");
      break;
    case PAD_EXTENDING:
      Serial.println("PAD_EXTENDING");
      break;
    case PAD_RETRACTING:
      Serial.println("PAD_RETRACTING");
      break;
    case UNKNOWN_POSITION:
      Serial.println("PAD_UNKNOWN_POSITION");
      break;
    default:
      break;
    }
  }

  // SERVOS ON/OFF & RELAY ODROID/CHARGER

  if (incoming_msj == "DRONE_TURN_ON" || incoming_msj == "DRONE_TURN_OFF")
  {
    incoming_msj = "";
    drone_onoff();
  }

  if ((incoming_msj == "RC_TURN_ON") && (RC_IS_ON == false))
  {
    incoming_msj = "";
    rc_onoff();
  }

  if ((incoming_msj == "RC_TURN_OFF") && (RC_IS_ON == true))
  {
    incoming_msj = "";
    rc_onoff();
  }

  if (incoming_msj == "ODROID_TURN_ON")
  {
    incoming_msj = "";
    odroid_on();
  }

  if (incoming_msj == "ODROID_TURN_OFF")
  {
    incoming_msj = "";
    odroid_off();
  }

  if (incoming_msj == "CHARGER_TURN_ON")
  {
    incoming_msj = "";
    charger_on();
  }

  if (incoming_msj == "CHARGER_TURN_OFF")
  {
    incoming_msj = "";
    charger_off();
  }

  if (incoming_msj == "DRONE_OUT" && (x_state != UNKNOWN_POSITION))
  {
    incoming_msj = "";
    pad_extend();
    release_drone();
  }

  if (incoming_msj == "DRONE_IN")
  {
    incoming_msj = "";
    pad_retract();
    center_drone();
  }

  if (incoming_msj == "SIMULATION")
  {
    incoming_msj = "";
    simulation_running = true;
    Serial.println("SIMULATION STARTED");
  }

  if (incoming_msj == "OPERATION")
  {
    incoming_msj = "";
    simulation_running = false;
    Serial.println("OPERATION MODE ENABLED");
  }

  if (incoming_msj == "STOP")
  {
    incoming_msj = "";
    stop_all_now();
    Serial.println("STOPED_OK");
  }

  if (incoming_msj == "CENTER" && (x_state != DRONE_RELEASING && x_state != DRONE_CENTERING))
  {
    incoming_msj = "";
    center_drone();
  }

  if (incoming_msj == "RELEASE" && (x_state != DRONE_RELEASING && x_state != DRONE_CENTERING))
  {
    incoming_msj = "";
    release_drone();
  }

  if (incoming_msj == "EXTEND" && y_state != PAD_EXTENDING)
  {
    incoming_msj = "";
    pad_extend();
  }

  if (incoming_msj == "RETRACT" && y_state != PAD_RETRACTING)
  {
    incoming_msj = "";
    pad_retract();
  }

  // DRONE POSITIONING CONTROL ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // X STEPPER MOTOR DRONE_CENTERING
    /*Esto centra el dron en un periodo de dos mitades:
    (1) Espera y (2) Avanza. El movimiento es en la dirección de encierro X_DIR LOW.*/
  if (x_state == DRONE_CENTERING && (x_end == false)){
    // Segundo semiciclo: Avance
    if (microsCurrent - microsxStepper >= STEP_DURATION * 2)
    {
      microsxStepper = micros();
      digitalWrite(X_DIR, LOW);
      digitalWrite(X_STP, HIGH);
    }
    // Primer semiciclo: Espera
    else if (microsCurrent - microsxStepper >= STEP_DURATION)
    {
      digitalWrite(X_STP, LOW);
      x_steps--;
    }
  }
  // X STEPPER SWITCH_CLEARANCE
    /*Si tocó el final de carrera X y la secuencia de centrado no está terminada, que retroceda un poco así deja libre el switch.*/
  else if (x_end && x_state != DRONE_CENTERED)
  { // Replegar 100 micropasos
    if (x_steps <= SWITCH_CLEARANCE) // SI NO TERMINÓ
    { // (1) Wait, (2) Step 
      if (microsCurrent - microsxStepper >= STEP_DURATION * 2) // Segundo semiciclo: Retroceder un paso
      {
        microsxStepper = micros();
        digitalWrite(X_DIR, HIGH);
        digitalWrite(X_STP, HIGH);
      }
      else if (microsCurrent - microsxStepper >= STEP_DURATION) // Primer semiciclo: Esperar
      {
        digitalWrite(X_STP, LOW);
        x_steps++;
      }
    }

    if (x_steps >= SWITCH_CLEARANCE) // SI YA ESTÁ
    { // Dron centrado, apagar stepper y comunicar
      x_state = DRONE_CENTERED;
      x_end = false;
      digitalWrite(EN, HIGH); // MOTORS DISABLED
      Serial.println("DRONE_CENTERED_OK");
    }
  }

  // X STEPPER MOTOR DRONE_RELEASING
    /*Esto lo que hace es liberar el dron del centrado en el eje X moviendo el stepper una cantidad de veces igual a X_AXIS_STEPS en el sentido de X_DIR = HIGH.
    El movimiento se hace mandando un HIGH a X_STP durante un tiempo STEP_DURATION y luego soltando el X_STP en LOW durante otro tiempo STEP_DURATION. Así dejando un 50% de tasa en estado alto por periodo.
    Esta condición deja pasar un periodo entero sin movimiento antes de arrancar, no sé si es intencional o no, se evitaría haciendo que las condiciones sean de "Menor o igual" pero puedo imaginarme que convenga esperar un periodo por alguna razón de hardware.*/

  if (x_state == DRONE_RELEASING)
  { // Ciclo de dos mitades: (1) Espera (2) Avanza
    
    if (x_steps <= X_AXIS_STEPS) // ANTES DE TERMINAR
    {
      if (microsCurrent - microsxStepper >= STEP_DURATION * 2) // Segunda mitad del periodo: STEP
      {
        microsxStepper = micros();  // Registra el tiempo
        x_steps++;                  // Registra los pasos
        digitalWrite(X_DIR, HIGH);  // Dirección de soltar
        digitalWrite(X_STP, HIGH);  // Activa el paso
      }
      else if (microsCurrent - microsxStepper >= STEP_DURATION) // Primer mitad del periodo: WAIT
      {
        digitalWrite(X_STP, LOW);   // Desactiva el paso
      }
    }

    if (x_steps >= X_AXIS_STEPS) // CUANDO TERMINA
    {
      x_state = DRONE_RELEASED; // Actualiza el estado
      digitalWrite(EN, OFF);    // Apaga el motor
      Serial.println("DRONE_RELEASED_OK"); // Comunica
    }
  }

  // FAN ACTIVATION
  if (microsCurrent - fan_timer > 300000000)
  {
    fan_timer = microsCurrent;
    int t_now = dht.readTemperature();

    if (t_now >= 25)
    {
      fan_on();
    }
    else
    {
      fan_off();
    }
  }

  if (incoming_msj == "FAN_ON")
  {
    incoming_msj = "";
    fan_on();
  }

  if (incoming_msj == "FAN_OFF")
  {
    incoming_msj = "";
    fan_off();
  }

} // END VOID LOOP