#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <XPT2046_Touchscreen.h>
#include <math.h>

// === PIN-konfiguration ===
#define TFT_CS   15
#define TFT_DC   2
#define TFT_RST  4
#define TOUCH_CS 33
#define TOUCH_IRQ 0

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
Adafruit_MPU6050 mpu;
XPT2046_Touchscreen ts(TOUCH_CS, TOUCH_IRQ);

float heading = 0;
unsigned long lastUpdate = 0;
float previousHeading = -999;
const int cx = 120;  // tft.width() / 2
const int cy = 160;  // tft.height() / 2
const int compassRadius = 70;

void drawFixedRedTriangle() {
  int height = 40;
  int width = 16;
  int x0 = cx;
  int y0 = cy - height;
  int x1 = cx - width / 2;
  int y1 = cy;
  int x2 = cx + width / 2;
  int y2 = cy;

  tft.fillTriangle(x0, y0, x1, y1, x2, y2, ILI9341_RED);
}

void setup() {
  Serial.begin(115200);

  tft.begin();
  tft.setRotation(0); // Stående
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);

  ts.begin();
  Serial.println(ts.touched() ? "Touch initierad!" : "Touch ej aktiv");

  Wire.begin(21, 22);
  if (!mpu.begin()) {
    Serial.println("MPU6050 kunde inte initieras!");
    while (1);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  drawFixedRedTriangle();
  lastUpdate = millis();
}

void loop() {
  unsigned long now = millis();
  if (now - lastUpdate < 100) return; // max 10 Hz uppdatering
  lastUpdate = now;

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float rotationSpeed = g.gyro.z * (180.0 / PI);
  heading += rotationSpeed * 0.1;
  heading = fmod(heading, 360.0);
  if (heading < 0) heading += 360.0;

  if (abs(heading - previousHeading) >= 1.0) {
    eraseCompassRing();
    drawCompassRing(heading);
    previousHeading = heading;
  }
}

void eraseCompassRing() {
  tft.fillCircle(cx, cy, compassRadius + 10, ILI9341_BLACK);
  drawFixedRedTriangle(); // behöver ritas om då vi fyllt över
}

void drawCompassRing(float heading) {
  tft.drawCircle(cx, cy, compassRadius, ILI9341_WHITE);

  for (int a = 0; a < 360; a += 30) {
    float rad = radians(a - heading);
    int x1 = cx + (compassRadius - 5) * sin(rad);
    int y1 = cy - (compassRadius - 5) * cos(rad);
    int x2 = cx + compassRadius * sin(rad);
    int y2 = cy - compassRadius * cos(rad);

    uint16_t color = (a % 90 == 0) ? ILI9341_WHITE : ILI9341_DARKGREY;
    tft.drawLine(x1, y1, x2, y2, color);
  }

  drawRotatedLabel("N", heading, 0);
  drawRotatedLabel("E", heading, 90);
  drawRotatedLabel("S", heading, 180);
  drawRotatedLabel("W", heading, 270);
}

void drawRotatedLabel(const char* label, float heading, float degOffset) {
  float angle = radians(degOffset - heading);
  int radius = compassRadius + 10;
  int x = cx + radius * sin(angle) - 6;
  int y = cy - radius * cos(angle) - 6;
  tft.setCursor(x, y);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print(label);
}