#include <SPI.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_GFX.h>
#include <Keypad.h>
#include <Servo.h>

// Pins
#define TFT_DC 2
#define TFT_CS 3
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
Servo myServo;

// Keypad
const byte ROWS = 4, COLS = 4;
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','.','D'}
};
byte rowPins[ROWS] = {A0, A1, A2, A3};
byte colPins[COLS] = {A4, A5, PD0, PD2};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// State
int traverse_mrad = 0;
int elevation_mrad = 0;
char inputValue[6] = "";
byte inputLen = 0;
bool isTraverse = true;

void setup() {
  tft.begin();
  tft.setRotation(1);
  tft.setTextSize(2);
  tft.fillScreen(ILI9341_BLACK);
  myServo.attach(D4);
  drawUI();
  updateUI();
}

void loop() {
  char key = keypad.getKey();
  if (key) {
    if ((key >= '0' && key <= '9') && inputLen < 5) {
      inputValue[inputLen++] = key;
      inputValue[inputLen] = '\0';
    } else if (key == '*') {
      int val = atoi(inputValue);
      if (isTraverse) {
        traverse_mrad = constrain(val, 0, 3141);  // ~180 deg
        myServo.write((traverse_mrad * 180L) / 3141);
      } else {
        elevation_mrad = constrain(val, 0, 1570); // ~90 deg
        myServo.write((elevation_mrad * 180L) / 3141);
      }
      inputLen = 0;
      inputValue[0] = '\0';
    } else if (key == 'A') {
      isTraverse = true;
    } else if (key == 'B') {
      isTraverse = false;
    } else if (key == 'C') {
      traverse_mrad = 0;
      elevation_mrad = 0;
      inputLen = 0;
      inputValue[0] = '\0';
      myServo.write(0);  // reset servo
    }  // Return to normal display
    


    updateUI();
  }
}

void drawUI() {
  for (int i = 0; i < 4; i++) {
    tft.drawRect(10, 10 + i * 50, 300, 40, ILI9341_WHITE);
  }
}

void updateUI() {
  // Clear UI sections
  tft.fillRect(10, 10, 300, 200, ILI9341_BLACK);
  
  // Traverse
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(15, 20);
  tft.print("Traverse (mrad): ");
  tft.setTextColor(ILI9341_CYAN);
  tft.print(traverse_mrad);

  // Elevation
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(15, 70);
  tft.print("Elevation (mrad): ");
  tft.setTextColor(ILI9341_CYAN);
  tft.print(elevation_mrad);

  // Mode
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(15, 120);
  tft.print("Mode: ");
  tft.setTextColor(ILI9341_YELLOW);
  tft.print(isTraverse ? "Traverse" : "Elevation");

  // Input
  tft.setTextColor(ILI9341_WHITE);
  tft.setCursor(15, 170);
  tft.print("Input: ");
  tft.setTextColor(ILI9341_GREEN);
  tft.print(inputValue);

  clearAlert();  // Clear any previous alert
}

void showAlert(const char* msg) {
  tft.setTextColor(ILI9341_RED, ILI9341_BLACK);
  tft.setCursor(15, 220);
  tft.print("ALERT: ");
  tft.print(msg);
}

void clearAlert() {
  tft.fillRect(10, 210, 300, 20, ILI9341_BLACK);
}