// Projeto: Experimento 2 - Sistema de Controle com FreeRTOS
// Arquivo: sketch.ino
// Autor: Nilton Mangueira

#include <Arduino.h>

// === Definição de Pinos ===
#define TRIG_PIN_FRENTE    2
#define ECHO_PIN_FRENTE    4
#define TRIG_PIN_DIREITA   5
#define ECHO_PIN_DIREITA   18
#define TRIG_PIN_TRAS      19
#define ECHO_PIN_TRAS      21
#define TRIG_PIN_ESQUERDA  22
#define ECHO_PIN_ESQUERDA  23

#define LED_PIN_FRENTE_DIR 13
#define LED_PIN_FRENTE_ESQ 25
#define LED_PIN_TRAS_DIR   14
#define LED_PIN_TRAS_ESQ   26

volatile float distFrente = 500, distDireita = 500, distTras = 500, distEsquerda = 500;

void TaskSensorFrente(void *pvParameters);
void TaskSensorDireita(void *pvParameters);
void TaskSensorTras(void *pvParameters);
void TaskSensorEsquerda(void *pvParameters);
void TaskControleMotores(void *pvParameters);

float lerDistancia(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duracao = pulseIn(echoPin, HIGH, 30000);
  return duracao ? duracao * 0.0343 / 2.0 : 500.0;
}

int calcularPWM(float distancia) {
  const float distMin = 10.0, distMax = 60.0;
  float fator = constrain((distancia - distMin) / (distMax - distMin), 0.0, 1.0);
  return (int)(pow(fator, 2) * 255);
}

void setup() {
  Serial.begin(115200);
  Serial.println("Sistema FreeRTOS com Testes de Tempo");

  pinMode(TRIG_PIN_FRENTE, OUTPUT); pinMode(ECHO_PIN_FRENTE, INPUT);
  pinMode(TRIG_PIN_DIREITA, OUTPUT); pinMode(ECHO_PIN_DIREITA, INPUT);
  pinMode(TRIG_PIN_TRAS, OUTPUT); pinMode(ECHO_PIN_TRAS, INPUT);
  pinMode(TRIG_PIN_ESQUERDA, OUTPUT); pinMode(ECHO_PIN_ESQUERDA, INPUT);

  pinMode(LED_PIN_FRENTE_DIR, OUTPUT); pinMode(LED_PIN_FRENTE_ESQ, OUTPUT);
  pinMode(LED_PIN_TRAS_DIR, OUTPUT);  pinMode(LED_PIN_TRAS_ESQ, OUTPUT);

  xTaskCreatePinnedToCore(TaskSensorFrente,   "SensorFrente", 1024, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(TaskSensorDireita,  "SensorDireita", 1024, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(TaskSensorTras,     "SensorTras", 1024, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(TaskSensorEsquerda, "SensorEsquerda", 1024, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(TaskControleMotores,"ControleMotores", 2048, NULL, 2, NULL, 1);
}

void loop() { vTaskDelay(1000); }

void TaskSensorFrente(void *pv) {
  for(;;) {
    TickType_t start = xTaskGetTickCount();
    distFrente = lerDistancia(TRIG_PIN_FRENTE, ECHO_PIN_FRENTE);
    TickType_t end = xTaskGetTickCount();
    Serial.printf("[Tempo] SensorFrente: %lu ticks\n", end - start);
    vTaskDelay(pdMS_TO_TICKS(250));
  }
}
void TaskSensorDireita(void *pv) {
  for(;;) {
    TickType_t start = xTaskGetTickCount();
    distDireita = lerDistancia(TRIG_PIN_DIREITA, ECHO_PIN_DIREITA);
    TickType_t end = xTaskGetTickCount();
    Serial.printf("[Tempo] SensorDireita: %lu ticks\n", end - start);
    vTaskDelay(pdMS_TO_TICKS(250));
  }
}
void TaskSensorTras(void *pv) {
  for(;;) {
    TickType_t start = xTaskGetTickCount();
    distTras = lerDistancia(TRIG_PIN_TRAS, ECHO_PIN_TRAS);
    TickType_t end = xTaskGetTickCount();
    Serial.printf("[Tempo] SensorTras: %lu ticks\n", end - start);
    vTaskDelay(pdMS_TO_TICKS(250));
  }
}
void TaskSensorEsquerda(void *pv) {
  for(;;) {
    TickType_t start = xTaskGetTickCount();
    distEsquerda = lerDistancia(TRIG_PIN_ESQUERDA, ECHO_PIN_ESQUERDA);
    TickType_t end = xTaskGetTickCount();
    Serial.printf("[Tempo] SensorEsquerda: %lu ticks\n", end - start);
    vTaskDelay(pdMS_TO_TICKS(250));
  }
}

void TaskControleMotores(void *pv) {
  for(;;) {
    int pwmFrente   = calcularPWM(distFrente);
    int pwmDireita  = calcularPWM(distDireita);
    int pwmTras     = calcularPWM(distTras);
    int pwmEsquerda = calcularPWM(distEsquerda);

    analogWrite(LED_PIN_FRENTE_DIR, pwmFrente);
    analogWrite(LED_PIN_FRENTE_ESQ, pwmFrente);
    analogWrite(LED_PIN_TRAS_DIR,   pwmTras);
    analogWrite(LED_PIN_TRAS_ESQ,   pwmTras);

    Serial.printf("[PWM] Frente: %d | Direita: %d | Tras: %d | Esquerda: %d\n", pwmFrente, pwmDireita, pwmTras, pwmEsquerda);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}