#include <ESP32Servo.h>

// ====== Servo pins (كما قلت: 4-5-6-7) ======
const int SERVO1_PIN = 4; // joystick X
const int SERVO2_PIN = 5; // Blue/Red up/down
const int SERVO3_PIN = 6; // Green/Yellow right/left
const int SERVO4_PIN = 7; // joystick Y

// ====== Joystick pins (عدّل إذا توصيلك مختلف) ======
const int JOY_VERT = 1; // VERT (Y)
const int JOY_HORZ = 2; // HORZ (X)

// ====== Button pins (عدّل إذا توصيلك مختلف) ======
const int BTN_GREEN  = 11; // يحرك Servo3 +
const int BTN_YELLOW = 12; // يحرك Servo3 -
const int BTN_BLUE   = 13; // يحرك Servo2 +
const int BTN_RED    = 14; // يحرك Servo2 -

Servo s1, s2, s3, s4;

// زوايا البداية
int a1 = 90, a2 = 90, a3 = 90, a4 = 90;

// إعدادات تحكم
const int STEP = 3;          // مقدار الحركة لكل كبسة
const int DEADZONE = 120;    // منطقة ميتة للجويستك لتخفيف الاهتزاز (تقريباً)

// helper
int clampAngle(int a) {
  if (a < 0) return 0;
  if (a > 180) return 180;
  return a;
}

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

  pinMode(BTN_GREEN,  INPUT_PULLUP);
  pinMode(BTN_YELLOW, INPUT_PULLUP);
  pinMode(BTN_BLUE,   INPUT_PULLUP);
  pinMode(BTN_RED,    INPUT_PULLUP);

  s1.attach(SERVO1_PIN);
  s2.attach(SERVO2_PIN);
  s3.attach(SERVO3_PIN);
  s4.attach(SERVO4_PIN);

  // روح على وضعية البداية
  s1.write(180);
  s2.write(a2);
  s3.write(a3);
  s4.write(a4);
}

void loop() {
  // ====== Joystick controls two servos (Servo1 & Servo4) ======
  int x = analogRead(JOY_HORZ); // 0..4095
  int y = analogRead(JOY_VERT); // 0..4095

  // Map to angles
  int target1 = map(x, 0, 4095, 0, 180);
  int target4 = map(y, 0, 4095, 0, 180);

  // Deadzone around center to reduce jitter
  if (abs(x - 2048) < DEADZONE) target1 = a1;
  if (abs(y - 2048) < DEADZONE) target4 = a4;

  // Smooth move (خفيف)
  a1 = (a1 * 7 + target1) / 8;
  a4 = (a4 * 7 + target4) / 8;

  s1.write(a1);
  s4.write(a4);

  // ====== Buttons control Servo3 (Green/Yellow) ======
  if (digitalRead(BTN_GREEN) == LOW) {
    a3 = clampAngle(a3 + STEP);
    s3.write(a3);
    delay(120); // debounce
  }
  if (digitalRead(BTN_YELLOW) == LOW) {
    a3 = clampAngle(a3 - STEP);
    s3.write(a3);
    delay(120);
  }

  // ====== Buttons control Servo2 (Blue/Red) ======
  if (digitalRead(BTN_BLUE) == LOW) {
    a2 = clampAngle(a2 + STEP);
    s2.write(a2);
    delay(120);
  }
  if (digitalRead(BTN_RED) == LOW) {
    a2 = clampAngle(a2 - STEP);
    s2.write(a2);
    delay(120);
  }

  delay(10);
}