#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_FT6206.h>
#include <SPI.h>
#include <Wire.h>
#include <ESP32Servo.h>

#define TFT_CS   15
#define TFT_DC   2
#define TFT_RST  4
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

#define I2C_SDA 21
#define I2C_SCL 22
Adafruit_FT6206 ctp = Adafruit_FT6206();

#define VRX 34  // Horizontal
#define VRY 35  // Vertical

#define SERVO1_PIN 25
#define SERVO2_PIN 26
Servo servo1;
Servo servo2;

int startY = 80;   
int spacing = 40;  
bool inMenu = true;       
bool inTurretMode = false;
bool inAimMode = false;

int joystickX;
int joystickY;
int threshold = 400; 

// Screen center
int centerX = 160; // Adjust for 320x240 display
int centerY = 120;

// Function to draw the sight
void drawSight() {
  tft.fillScreen(ILI9341_BLACK);

  int lineSpacing = 8; // Distance between large numbers
  int smallLineSpacing = 4; // Distance between small hash marks

  // Draw horizontal main line with numbers
  for (int i = -4; i <= 4; i++) {
    int x = centerX + i * lineSpacing * 2; // Multiply by 2 for spacing
    tft.drawLine(x, centerY - 5, x, centerY + 5, ILI9341_WHITE);

    // Draw numbers
    if (i != 0) {
      tft.setCursor(x - 10, centerY + 10);
      tft.setTextColor(ILI9341_WHITE);
      tft.setTextSize(1);
      tft.print(abs(i * lineSpacing));
    }
  }

  // Draw vertical hash lines under center circle
  int hashCount = 15;
  for (int i = -hashCount/2; i <= hashCount/2; i++) {
    int y = centerY + i * smallLineSpacing;
    tft.drawLine(centerX - 10, y, centerX + 10, y, ILI9341_WHITE);
  }

  // Draw center circle
  tft.drawCircle(centerX, centerY, 10, ILI9341_WHITE);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(I2C_SDA, I2C_SCL);
  tft.begin();
  tft.setRotation(1);
  tft.fillScreen(ILI9341_BLACK);

  if (!ctp.begin(40)) {  
    Serial.println("ERROR: Couldn't start FT6206 touch controller!");
    while (1);
  }
  Serial.println("FT6206 touch controller ready.");
  drawMenu();
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servo1.write(90);
  servo2.write(90);
}

void loop() {
  if (ctp.touched()) {
    TS_Point p = ctp.getPoint();
    int x = map(p.y, 0, 240, 0, tft.width());
    int y = map(320 - p.x, 0, 320, 0, tft.height());

    if (inMenu) {
      int turretX1 = 80, turretX2 = 240;
      int turretY1 = startY + spacing - 10;
      int turretY2 = turretY1 + 30;

      if (x > turretX1 && x < turretX2 && y > turretY1 && y < turretY2) {
        showTurretScreen();
        inMenu = false;
        inTurretMode = true;
      }

      int aimX1 = 80, aimX2 = 240;
      int aimY1 = startY + 2 * spacing - 10;
      int aimY2 = aimY1 + 30;

      if (x > aimX1 && x < aimX2 && y > aimY1 && y < aimY2) {
        drawSight();
        inMenu = false;
        inAimMode = true;
      }
    } else if (inTurretMode || inAimMode) {
      int backX1 = 0, backX2 = 60;
      int backY1 = 0, backY2 = 40;

      if (x > backX1 && x < backX2 && y > backY1 && y < backY2) {
        drawMenu();
        inMenu = true;
        inTurretMode = false; 
        inAimMode = false; 
      }
    }
  }

  if (inTurretMode) {
    joystickX = analogRead(VRX);
    joystickY = analogRead(VRY);

    if (joystickX < threshold) {
      int angle = map(joystickX, 0, threshold, 180, 90);
      servo1.write(angle);
    }

    if (joystickX > 4095 - threshold) {
      int angle = map(joystickX, 4095 - threshold, 4095, 90, 0);
      servo2.write(angle);
    }

    if (joystickY < threshold) {
      int angle = map(joystickY, 0, threshold, 0, 90);
      servo1.write(angle);
    }

    if (joystickY > 4095 - threshold) {
      int angle = map(joystickY, 4095 - threshold, 4095, 90, 180);
      servo2.write(angle);
    }
  }
}

void drawMenu() {
  inMenu = true;
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(10, 10);
  tft.print("Back");
  tft.setCursor(100, startY);
  tft.print("Turret");
  tft.setCursor(100, startY + spacing);
  tft.print("Aim");
  tft.setCursor(100, startY + 2 * spacing);
  tft.print("Engine Power");
  tft.setCursor(100, startY + 3 * spacing);
  tft.print("Speed");
}

void showTurretScreen() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(10, 10);
  tft.print("Back");
  tft.setTextSize(3);
  tft.setTextColor(ILI9341_GREEN);
  int16_t x1, y1;
  uint16_t w, h;
  tft.getTextBounds("Turret Control", 0, 0, &x1, &y1, &w, &h);
  int centerX = (tft.width() - w) / 2;
  int centerY = (tft.height() - h) / 2;
  tft.setCursor(centerX, centerY);
  tft.print("Turret Control");
}