#include <Adafruit_GFX.h>    // Core graphics library
#include <Adafruit_ILI9341.h> // Hardware-specific library for ST7735
#include <SPI.h>

// Define Pins
#define VRX_PIN A0 // Arduino pin connected to VRX pin
#define VRY_PIN A1 // Arduino pin connected to VRY pin
#define SW_PIN  13  // Arduino pin connected to SW pin

// Variables to store values
int xValue = 0; 
int yValue = 0; 
int swState = 0;

// LCD
#define TFT_MISO 16
#define TFT_MOSI 19
#define TFT_SCLK 18
#define TFT_CS   17  // Chip Select

#define TFT_DC 15
#define TFT_RST 14

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

int menuIndex = 0;
const int totalItems = 3;
String items[] = {"Start Motor", "Settings", "Info"};

void setup() {
  // Initialize Serial Monitor
  Serial1.begin(115200);
  delay(1000);
  Serial1.println("JOYSTICK TEST");
  
  // Set button pin as input with internal pullup resistor
  pinMode(SW_PIN, INPUT_PULLUP);

  tft.begin();
  tft.setRotation(4);
  // tft.fillScreen(ILI9341_RED);
  drawMenu();
}

void loop() {
  // Read analog X and Y values (0 to 1023)
  xValue = analogRead(VRX_PIN);
  yValue = analogRead(VRY_PIN);
  
  // Read button state (0 = pressed, 1 = released)
  swState = digitalRead(SW_PIN);
  
  // Print values to Serial Monitor
/*
  Serial1.print("X: ");
  Serial1.print(xValue);
  Serial1.print(" | Y: ");
  Serial1.print(yValue);
  Serial1.print(" | Button: ");
  Serial1.println(swState == LOW ? "Pressed" : "Not Pressed");
*/
  if (yValue < 50) {
    Serial1.println("DOWN");
    menuIndex += 1;
    if (menuIndex >= totalItems)
      menuIndex = totalItems - 1;
    drawMenu();
  }
  else if (yValue > 900) {
    Serial1.println("UP");
    menuIndex -= 1;
    if (menuIndex <= 0)
      menuIndex = 0;
    drawMenu();
  }
  // TODO move menu item
  delay(1000); // Small delay for stability
}

void drawMenu() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  for (int i = 0; i < totalItems; i++) {
    if (i == menuIndex) {
      tft.setTextColor(ILI9341_YELLOW); // Highlight selected
    } else {
      tft.setTextColor(ILI9341_WHITE);
    }
    tft.setCursor(20, 30 + (i * 20));
    tft.print(items[i]);
  }
}