#include <Arduino.h>
#include <STM32FreeRTOS.h>

// --- Pines ---
#define TRG   D2     // Trigger (D2)
#define ECHO  D3      // Echo    (D3)

// Periodos de tareas (en ms)
#define PERIOD_DISPARO 500  
#define PERIOD_CHECK   1     // Cada 1 ms revisar flancos

// Variables globales
volatile unsigned long t_inicio = 0;
volatile unsigned long distancia_cm = 0;

// Declaración de tareas
void tareaDisparo(void *pvParameters);
void tareaFlancoSubida(void *pvParameters);
void tareaFlancoBajada(void *pvParameters);

void setup() {
  Serial.begin(115200);   // PA2/PA3
  pinMode(TRG, OUTPUT);
  pinMode(ECHO, INPUT);
  digitalWrite(TRG, LOW);

  Serial.println("STM32Duino + FreeRTOS listo");

  // Crear tareas
  xTaskCreate(tareaDisparo, "Disparo", 256, NULL, 2, NULL);
  xTaskCreate(tareaFlancoSubida, "FlancoUp", 256, NULL, 1, NULL);
  xTaskCreate(tareaFlancoBajada, "FlancoDown", 256, NULL, 1, NULL);

  // Iniciar FreeRTOS
  vTaskStartScheduler();
}

void loop() {
  // FreeRTOS no usa loop()
}

// =============================================================
//               TAREA: Genera pulso de Trigger
// =============================================================
void tareaDisparo(void *pvParameters)
{
  while (1)
  {
    digitalWrite(TRG, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRG, LOW);

    vTaskDelay(pdMS_TO_TICKS(PERIOD_DISPARO));
  }
}

// =============================================================
//         TAREA: Detecta flanco de SUBIDA (ECHO=0→1)
// =============================================================
void tareaFlancoSubida(void *pvParameters)
{
  bool ant = LOW;

  while (1)
  {
    bool act = digitalRead(ECHO);

    if (ant == LOW && act == HIGH)
    {
      t_inicio = micros();   // Registrar tiempo de inicio
    }

    ant = act;
    vTaskDelay(pdMS_TO_TICKS(PERIOD_CHECK));
  }
}

// =============================================================
//         TAREA: Detecta flanco de BAJADA (ECHO=1→0)
// =============================================================
void tareaFlancoBajada(void *pvParameters)
{
  bool ant = LOW;

  while (1)
  {
    bool act = digitalRead(ECHO);

    if (ant == HIGH && act == LOW)
    {
      unsigned long t_final = micros();
      unsigned long duracion = t_final - t_inicio;

      distancia_cm = duracion / 58;

      Serial.print("Distancia: ");
      Serial.print(distancia_cm);
      Serial.println(" cm");
    }

    ant = act;
    vTaskDelay(pdMS_TO_TICKS(PERIOD_CHECK));
  }
}