#include <freertos/FreeRTOS.h>
#include <NewPing.h>
#include <math.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

// === Configurações gerais ===
#define MAX_DISTANCE   200
const int MINOR_CYCLE_MS = 20;

// === Instâncias dos sensores ===
NewPing sonar_front(TRIG_FRONT, ECHO_FRONT, MAX_DISTANCE);
NewPing sonar_back(TRIG_BACK, ECHO_BACK, MAX_DISTANCE);
NewPing sonar_left(TRIG_LEFT, ECHO_LEFT, MAX_DISTANCE);
NewPing sonar_right(TRIG_RIGHT, ECHO_RIGHT, MAX_DISTANCE);

// === Variáveis globais ===
volatile float dist_front_cm = MAX_DISTANCE;
volatile float dist_back_cm  = MAX_DISTANCE;
volatile float dist_left_cm  = MAX_DISTANCE;
volatile float dist_right_cm = MAX_DISTANCE;

// === Protótipos ===
void task_read_sensor_frontal();
void task_read_sensor_traseiro();
void task_read_sensor_esquerdo();
void task_read_sensor_direito();
void task_control_motors();
void cyclicExecutiveTask(void *pvParameters);

void setup() {
  Serial.begin(115200);
  Serial.println("Sistema Iniciado - Controle com Executivo Cíclico");

  // Configuração dos pinos PWM para os motores
  ledcSetup(MOTOR1_CH, 5000, 8);
  ledcAttachPin(MOTOR1_PIN, MOTOR1_CH);

  ledcSetup(MOTOR2_CH, 5000, 8);
  ledcAttachPin(MOTOR2_PIN, MOTOR2_CH);

  ledcSetup(MOTOR3_CH, 5000, 8);
  ledcAttachPin(MOTOR3_PIN, MOTOR3_CH);

  ledcSetup(MOTOR4_CH, 5000, 8);
  ledcAttachPin(MOTOR4_PIN, MOTOR4_CH);

  // Criação da tarefa principal
  xTaskCreate(
    cyclicExecutiveTask,
    "Executivo Ciclico",
    4096,
    NULL,
    1,
    NULL
  );

  Serial.println("Setup concluído.");
}

void loop() {
  vTaskDelete(NULL); // loop não utilizado
}

// === Executivo Cíclico ===
void cyclicExecutiveTask(void *pvParameters) {
  int slot = 0;
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    Serial.printf("\n[Slot %d]\n", slot);

    switch (slot) {
      case 0: task_read_sensor_frontal(); break;
      case 1: task_read_sensor_traseiro(); break;
      case 2: task_control_motors(); break;
      case 3: task_read_sensor_esquerdo(); break;
      case 4: task_read_sensor_frontal(); break;
      case 5: task_read_sensor_direito(); break;
      case 6: task_control_motors(); break;
      case 7: Serial.println("Slot 7: Slack Time"); break;
    }

    slot = (slot + 1) % 8;
    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(MINOR_CYCLE_MS));
  }
}

// === Funções de leitura dos sensores ===
void task_read_sensor_frontal() {
  unsigned long t0 = micros();
  dist_front_cm = sonar_front.ping_cm();
  if (dist_front_cm == 0) dist_front_cm = MAX_DISTANCE;
  Serial.printf("T1 (Frontal): %.2f cm | %lu us\n", dist_front_cm, micros() - t0);
}

void task_read_sensor_traseiro() {
  unsigned long t0 = micros();
  dist_back_cm = sonar_back.ping_cm();
  if (dist_back_cm == 0) dist_back_cm = MAX_DISTANCE;
  Serial.printf("T2 (Traseiro): %.2f cm | %lu us\n", dist_back_cm, micros() - t0);
}

void task_read_sensor_esquerdo() {
  unsigned long t0 = micros();
  dist_left_cm = sonar_left.ping_cm();
  if (dist_left_cm == 0) dist_left_cm = MAX_DISTANCE;
  Serial.printf("T3 (Esquerdo): %.2f cm | %lu us\n", dist_left_cm, micros() - t0);
}

void task_read_sensor_direito() {
  unsigned long t0 = micros();
  dist_right_cm = sonar_right.ping_cm();
  if (dist_right_cm == 0) dist_right_cm = MAX_DISTANCE;
  Serial.printf("T4 (Direito): %.2f cm | %lu us\n", dist_right_cm, micros() - t0);
}

// === Controle dos motores ===
void task_control_motors() {
  unsigned long t0 = micros();

  // Lei de controle exponencial com base no sensor frontal
  float k = 0.05;
  int duty = 255 * (1 - exp(-k * dist_front_cm));
  duty = constrain(duty, 0, 255);

  ledcWrite(MOTOR1_CH, duty);
  ledcWrite(MOTOR2_CH, duty);
  ledcWrite(MOTOR3_CH, duty);
  ledcWrite(MOTOR4_CH, duty);

  unsigned long execTime = micros() - t0;
  Serial.printf("T5 (Motores): Duty = %d | %lu us\n", duty, execTime);

  if (execTime > 80000) {
    Serial.println("!!! DEADLINE PERDIDO NA TAREFA DE MOTORES !!!");
  }
}