// 1. Joystick-Based Dual-Axis Servo Control
// The joystick controls two servos:

// X-axis (left/right) controls myservox on pin 3

// Y-axis (up/down) controls myservoy on pin 6

// It maps analog joystick input to servo angles (0–180°)

// 📺 2. OLED Display Feedback
// Real-time values shown on the OLED:

// Normalized joystick values (X and Y from -1 to 1)

// Calculated servo angles (AngleX, AngleY)

// Button press status

// Last keypad key pressed

// Current control mode

// 🔢 3. Keypad Interaction
// The 4x4 keypad can be used to change modes or trigger actions:

// Key	Action
// A	Switch to Joystick Mode (servos follow joystick live)
// B	Switch to Manual Mode (placeholder for future logic)
// C	Reset both servos to center (90°)
// D	Lock servos (stop moving even if joystick is active)

// ⚙️ 4. Mode Switching
// You have 3 control modes:

// JOYSTICK – live control with joystick

// MANUAL – reserved for keypad-based angle input or fixed steps

// LOCKED – disables servo movement for safety or calibration

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include <Keypad.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int VRx = A0;
int VRy = A1;
int SW = 2;

Servo myservox;
Servo myservoy;

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

uint8_t colPins[COLS] = { 9, 8, 5, 4 };
uint8_t rowPins[ROWS] = { 13, 12, 11, 10 };

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

enum Mode { JOYSTICK, MANUAL, LOCKED };
Mode currentMode = JOYSTICK;

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

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 initialization failed");
    while (1);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 20);
  display.println("Joystick \n Ready");

  display.fillCircle(97, 31, 5, WHITE);                   // Center
  display.fillRoundRect(104, 26, 15, 11, 2, WHITE);
  display.fillTriangle(118, 22, 118, 40, 127, 31, WHITE); // Right
  display.fillRoundRect(76, 26, 15, 11, 2, WHITE);
  display.fillTriangle(76, 22, 76, 40, 67, 31, WHITE);    // Left
  display.fillRoundRect(92, 10, 11, 15, 2, WHITE);
  display.fillTriangle(88, 10, 106, 10, 97, 1, WHITE);    // Up
  display.fillRoundRect(92, 38, 11, 15, 2, WHITE);
  display.fillTriangle(88, 52, 106, 52, 97, 61, WHITE);   // Down

  display.display();

  delay(2000);

  pinMode(SW, INPUT_PULLUP);
  myservox.attach(5);
  myservoy.attach(6);
}

void loop() {
  int rawX = analogRead(VRx);
  int rawY = analogRead(VRy);
  int button = digitalRead(SW);
  char key = keypad.getKey();
  bool pressed = (digitalRead(SW) == LOW);

  // Mode control via keypad
  if (key) {
    switch (key) {
      case 'A': currentMode = JOYSTICK; break;
      case 'B': currentMode = MANUAL; break;
      case 'C': myservox.write(90); myservoy.write(90); break;
      case 'D': currentMode = LOCKED; break;
    }
  }

  float x = -(rawX - 512.0) / 512.0;
  float y = (rawY - 512.0) / 512.0;
  x = constrain(x, -1.0, 1.0);
  y = constrain(y, -1.0, 1.0);
  int angleX = map(rawX, 0, 1023, 0, 180);
  int angleY = map(rawY, 0, 1023, 0, 180);

  // Servo control based on mode
  if (currentMode == JOYSTICK) {
    myservox.write(angleX);
    myservoy.write(angleY);
  } else if (currentMode == MANUAL) {
    // (optional future: control with keypad numbers)
  }

  // OLED Display
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  display.print(" X   : "); display.println(x, 2);
  display.print(" Y   : "); display.println(y, 2);
  display.print(" <X  : "); display.println(angleX);
  display.print(" <Y  : "); display.println(angleY);
  display.print(" Btn : "); display.println(button == LOW ? "P" : "R");
  display.print(" Key : "); display.println(key ? key : '-');

  display.print("Mode : ");
  switch (currentMode) {
    case JOYSTICK: display.println("J"); break;
    case MANUAL  : display.println("M"); break;
    case LOCKED  : display.println("L"); break;
  }

  display.drawRoundRect(92, 10, 11, 15, 2, WHITE);   // Up
  display.drawTriangle(88, 10, 106, 10, 97, 1, WHITE);
  display.drawRoundRect(76, 26, 15, 11, 2, WHITE);   // Left
  display.drawTriangle(76, 22, 76, 40, 67, 31, WHITE);
  display.drawRoundRect(92, 38, 11, 15, 2, WHITE);   // Down
  display.drawTriangle(88, 52, 106, 52, 97, 61, WHITE);
  display.drawRoundRect(104, 26, 15, 11, 2, WHITE);   // Right
  display.drawTriangle(118, 22, 118, 40, 127, 31, WHITE);
  display.drawCircle(97, 31, 5, WHITE);              // Center

  update(x, y, pressed);

  display.display();
  delay(100);
}

void update(float x, float y, bool pressed){
  if (pressed) {
    // Blink center and show message
    display.fillCircle(97, 31, 5, WHITE);
    display.setCursor(40, 54);
    display.setTextSize(1);
    display.print("Pressed!");
    return;
  }

  if (x == 0 && y == 0) {
    display.fillCircle(97, 31, 5, WHITE);  // Center
  } else if (x > 0) {
    display.fillRoundRect(104, 26, 15, 11, 2, WHITE);
    display.fillTriangle(118, 22, 118, 40, 127, 31, WHITE);  // Right
  } else if (x < 0) {
    display.fillRoundRect(76, 26, 15, 11, 2, WHITE);
    display.fillTriangle(76, 22, 76, 40, 67, 31, WHITE);  // Left
  }
  
  if (y > 0) {
    display.fillRoundRect(92, 10, 11, 15, 2, WHITE);
    display.fillTriangle(88, 10, 106, 10, 97, 1, WHITE);   // Up
  } else if (y < 0) {
    display.fillRoundRect(92, 38, 11, 15, 2, WHITE);
    display.fillTriangle(88, 52, 106, 52, 97, 61, WHITE);  // Down
  }
}