#include <Arduino.h>

const int TRIG_PIN = 5;
const int ECHO_PIN = 18;

QueueHandle_t xSensorQueue
QueueHandle_t xRichtingQueue;

int lastDirection = 1;

enum Richting {
  LINKS,
  VOOR,
  RECHTS,
  STUCK
};

struct SensorData {
  long links;
  long voor;
  long rechts;
};

long meetAfstand() {
  // 30% kans op een obstakel dichtbij

  if (random(100) < 30) {
    return random(5, 20);   // obstakel dichtbij
  }
  return random(30, 150);  // vrije baan

}

void sensorTask(void *pvParameters) {
  while (true) {
    SensorData data;
    data.voor   = meetAfstand();  // of random
    // draai hoofd links
    data.links  = meetAfstand() ;
    // draai hoofd rechts
    data.rechts = meetAfstand();

    xQueueSend(xSensorQueue, &data, 0);
    vTaskDelay(pdMS_TO_TICKS(200));
  }
}

int findMax(SensorData data) {
  if (data.voor > data.links && data.voor > data.rechts) return 1;
  if (data.links > data.voor && data.links > data.rechts) return 0;
  if (data.rechts > data.voor && data.rechts > data.links) return 2;

  return -1;  // gelijkspel — breakTie nodig
}

int breakTie(SensorData data, int lastDirection) {
  if (data.voor == data.links) {
    if (lastDirection == 1) return 1;  // was voor, blijf voor
    if (lastDirection == 0) return 0;  // was links, blijf links
    return 1;  // standaard voor
  }

  if (data.voor == data.rechts) {
    if (lastDirection == 1) return 1;
    if (lastDirection == 2) return 2;
    return 1;
  }

  if (data.links == data.rechts) {
    if (lastDirection == 0) return 0;
    if (lastDirection == 2) return 2;
    return 0;  // standaard links
  }

  return lastDirection;
}

void beslisTask(void *pvParameters) {

  SensorData ontvangen;

  while (true) {

    if (xQueueReceive(xSensorQueue, &ontvangen, portMAX_DELAY) == pdPASS) {

      int max = -1;
      Richting winner = VOOR;  // standaard

      if (ontvangen.voor < 5 && ontvangen.links < 5 && ontvangen.rechts < 5) {
        winner = STUCK;
      } else {
        max = findMax(ontvangen);
        if (max == -1) max = breakTie(ontvangen, lastDirection);
        lastDirection = max;

        switch (max) {
          case 0: winner = LINKS; break;
          case 1: winner = VOOR; break;
          case 2: winner = RECHTS; break;
        }
      }

      xQueueSend(xRichtingQueue, &winner, 0);
    }
  }
}

void bestuurTask(void *pvParameters) {

  while (true) {
    Richting richting;
    if (xQueueReceive(xRichtingQueue, &richting, portMAX_DELAY) == pdPASS) {

      switch (richting) {
        case VOOR :
          Serial.println("ik ga rechtdoor");
          break;
        case LINKS:
          Serial.println("ik ga linksaf");
          break;
        case RECHTS:
          Serial.println("ik ga rechtsaf");
          break;
        case STUCK:
          Serial.println("ik ga achteruit");
          break;
      }
    }
  }
}

void setup() {

  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);  // trig stuurt signaal UIT
  pinMode(ECHO_PIN, INPUT);   // echo ontvangt signaal IN

  xSensorQueue = xQueueCreate(10, sizeof(SensorData));
  xQueue2 = xQueueCreate(10, sizeof(Richting));

  xTaskCreate(sensorTask, "sensorTask", 2048, NULL, 1, NULL);
  xTaskCreate(beslisTask, "beslisTask", 2048, NULL, 1, NULL);
  xTaskCreate(bestuurTask, "bestuurTask", 2048, NULL, 1, NULL);

}


void loop() {}