#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Wire.h>
#include <RF24.h>
#include <MPU6050.h>


#define TFT_CS 10
#define TFT_DC 9
Adafruit_ILI9341 tft(TFT_CS, TFT_DC);


RF24 radio(7, 8);


const byte address[6] = "00001";


struct DataPacket {
  int distances[4];
  float heading;
  float roll;
  float pitch;
} dataPacket;

void setup() {
  Serial.begin(9600);
  Wire.begin();


  tft.begin();
  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.setCursor(0, 0);
  tft.println("Demarrage...");


  radio.begin();
  radio.openReadingPipe(1, address);
  radio.setPALevel(RF24_PA_LOW);
  radio.startListening();

  delay(1000);
  tft.fillScreen(ILI9341_BLACK);
}

void loop() {

  if (radio.available()) {
    radio.read(&dataPacket, sizeof(dataPacket));


    tft.fillRect(0, 0, 160, 100, ILI9341_BLACK);
    tft.setCursor(0, 0);
    for (int i = 0; i < 4; i++) {
      tft.print("D");
      tft.print(i + 1);
      tft.print(": ");
      tft.print(dataPacket.distances[i]);
      tft.println(" cm");
    }


    tft.fillRect(160, 0, 160, 240, ILI9341_BLACK);
    drawSquareWithTriangle(dataPacket);


    Serial.print("Heading: ");
    Serial.print(dataPacket.heading);
    Serial.print("° | Roll: ");
    Serial.print(dataPacket.roll);
    Serial.print("° | Pitch: ");
    Serial.print(dataPacket.pitch);
    Serial.println("°");
  }

  delay(500);
}


void drawSquareWithTriangle(DataPacket data) {
  int centerX = 240;
  int centerY = 120;
  int size = 40;
  int squareSize = 100;


  tft.drawRect(centerX - squareSize / 2, centerY - squareSize / 2, squareSize, squareSize, ILI9341_WHITE);


  int thresholdObstacle = 15;
  int thresholdVoid = 20;

  uint16_t leftColor = (data.distances[0] <= thresholdObstacle) ? ILI9341_YELLOW : (data.distances[0] > thresholdVoid) ? ILI9341_RED : ILI9341_WHITE;
  uint16_t frontColor = (data.distances[1] <= thresholdObstacle) ? ILI9341_YELLOW : (data.distances[1] > thresholdVoid) ? ILI9341_RED : ILI9341_WHITE;
  uint16_t rightColor = (data.distances[2] <= thresholdObstacle) ? ILI9341_YELLOW : (data.distances[2] > thresholdVoid) ? ILI9341_RED : ILI9341_WHITE;
  uint16_t backColor = (data.distances[3] <= thresholdObstacle) ? ILI9341_YELLOW : (data.distances[3] > thresholdVoid) ? ILI9341_RED : ILI9341_WHITE;


  tft.drawFastHLine(centerX - squareSize / 2, centerY - squareSize / 2, squareSize, frontColor); // Côté avant
  tft.drawFastHLine(centerX - squareSize / 2, centerY + squareSize / 2, squareSize, backColor); // Côté arrière
  tft.drawFastVLine(centerX - squareSize / 2, centerY - squareSize / 2, squareSize, leftColor); // Côté gauche
  tft.drawFastVLine(centerX + squareSize / 2, centerY - squareSize / 2, squareSize, rightColor); // Côté droit


  uint16_t triangleColor = (abs(data.roll) > 65 || abs(data.pitch) > 65) ? ILI9341_RED : ILI9341_WHITE;


  float angleOffset = PI / 2;
  float angleRad = (data.heading * PI / 180) + angleOffset;

  int x1 = centerX + cos(angleRad) * size;
  int y1 = centerY - sin(angleRad) * size;
  int x2 = centerX + cos(angleRad + 2 * PI / 3) * size;
  int y2 = centerY - sin(angleRad + 2 * PI / 3) * size;
  int x3 = centerX + cos(angleRad + 4 * PI / 3) * size;
  int y3 = centerY - sin(angleRad + 4 * PI / 3) * size;


  tft.fillTriangle(x1, y1, x2, y2, x3, y3, triangleColor);


  int xIndicator = centerX + cos(angleRad) * (size + 10);
  int yIndicator = centerY - sin(angleRad) * (size + 10);
  tft.drawLine(centerX, centerY, xIndicator, yIndicator, ILI9341_RED);
}