/*
 * Experimento 2: Controle de Robô com FreeRTOS no ESP32
 * Sistema multitarefa com sensores ultrassônicos e controle PWM de motores.
 * A velocidade dos motores varia exponencialmente com a distância dos obstáculos.
 */

// --- Bibliotecas ---
#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"

// === Pinos dos Sensores Ultrassônicos ===
#define TRIG_FRONT     23
#define ECHO_FRONT     22

#define TRIG_RIGHT     18
#define ECHO_RIGHT     5

#define TRIG_BACK      32
#define ECHO_BACK      33

#define TRIG_LEFT      27
#define ECHO_LEFT      14

// === Motores controlados via canais de PWM ===
#define MOTOR1_PIN     25
#define MOTOR2_PIN     26
#define MOTOR3_PIN     12
#define MOTOR4_PIN     13

#define MOTOR1_CH      0
#define MOTOR2_CH      1
#define MOTOR3_CH      2
#define MOTOR4_CH      3

const int PWM_FREQ = 5000;
const int PWM_RESOLUTION = 8;
const int MAX_DUTY = 255;

// Variáveis compartilhadas
volatile float distFront, distRight, distBack, distLeft;
SemaphoreHandle_t mutexDist;

// Estrutura para sensor
struct SensorConfig {
  int trig;
  int echo;
  volatile float* distPtr;
};

// Protótipos
void TaskSensor(void* param);
void TaskMotorControl(void* param);
float lerDistancia(int trig, int echo);
int calcularDutyCycle(float distancia);

void setup() {
  Serial.begin(115200);
  Serial.println("Sistema Iniciado");

  // PWM: configuração dos canais
  ledcSetup(MOTOR1_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR1_PIN, MOTOR1_CH);
  ledcSetup(MOTOR2_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR2_PIN, MOTOR2_CH);
  ledcSetup(MOTOR3_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR3_PIN, MOTOR3_CH);
  ledcSetup(MOTOR4_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR4_PIN, MOTOR4_CH);

  // Cria mutex
  mutexDist = xSemaphoreCreateMutex();

  // Configuração dos sensores
  static SensorConfig cfgFront = {TRIG_FRONT, ECHO_FRONT, &distFront};
  static SensorConfig cfgRight = {TRIG_RIGHT, ECHO_RIGHT, &distRight};
  static SensorConfig cfgBack  = {TRIG_BACK,  ECHO_BACK,  &distBack};
  static SensorConfig cfgLeft  = {TRIG_LEFT,  ECHO_LEFT,  &distLeft};

  // Criação das tarefas
  xTaskCreatePinnedToCore(TaskSensor, "Sensor Front", 2048, &cfgFront, 1, NULL, 1);
  xTaskCreatePinnedToCore(TaskSensor, "Sensor Right", 2048, &cfgRight, 1, NULL, 1);
  xTaskCreatePinnedToCore(TaskSensor, "Sensor Back", 2048, &cfgBack,  1, NULL, 1);
  xTaskCreatePinnedToCore(TaskSensor, "Sensor Left", 2048, &cfgLeft,  1, NULL, 1);
  xTaskCreatePinnedToCore(TaskMotorControl, "Motor Control", 2048, NULL, 2, NULL, 1);
}

void loop() {
  vTaskDelete(NULL);  // Controle total entregue ao FreeRTOS
}

// Função de leitura ultrassônica
float lerDistancia(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long dur = pulseIn(echo, HIGH, 30000);
  return (dur == 0) ? 500.0 : (dur * 0.0343) / 2.0;
}

// Tarefa genérica de leitura de sensor
void TaskSensor(void* param) {
  SensorConfig* cfg = (SensorConfig*)param;
  pinMode(cfg->trig, OUTPUT);
  pinMode(cfg->echo, INPUT);

  while (true) {
    float d = lerDistancia(cfg->trig, cfg->echo);
    if (xSemaphoreTake(mutexDist, pdMS_TO_TICKS(10)) == pdTRUE) {
      *(cfg->distPtr) = d;
      xSemaphoreGive(mutexDist);
    }
    vTaskDelay(pdMS_TO_TICKS(75));
  }
}

// Calcula duty cycle com base na distância (controle exponencial)
int calcularDutyCycle(float distancia) {
  const float distMin = 10.0, distMax = 60.0;
  distancia = constrain(distancia, 0, distMax);
  if (distancia < distMin) return 0;
  float fator = (distancia - distMin) / (distMax - distMin);
  float base = 10.0;
  float resultado = (pow(base, fator) - 1) / (base - 1);
  return int(resultado * MAX_DUTY);
}

// Tarefa de controle dos motores
void TaskMotorControl(void* param) {
  float dF, dB;
  while (true) {
    if (xSemaphoreTake(mutexDist, pdMS_TO_TICKS(10)) == pdTRUE) {
      dF = distFront;
      dB = distBack;
      xSemaphoreGive(mutexDist);
    }

    int pwmF = calcularDutyCycle(dF);
    int pwmB = calcularDutyCycle(dB);

    ledcWrite(MOTOR1_CH, pwmF);
    ledcWrite(MOTOR2_CH, pwmF);
    ledcWrite(MOTOR3_CH, pwmB);
    ledcWrite(MOTOR4_CH, pwmB);

    Serial.printf("Frente: %.1f cm → %d | Trás: %.1f cm → %d\n", dF, pwmF, dB, pwmB);
    vTaskDelay(pdMS_TO_TICKS(50));
  }
}