//PROGRAM BY: RANDY N. CAÑADA
/*SUBSCRIBE: FB PAGES:
Playlist.ph: 
https://www.facebook.com/profile.php?id=61573177858722

Arduino:
https://www.facebook.com/rhacan2?__tn__=R

YOUTUBE:
PlaylistPH
https://www.youtube.com/@PlaylistPh-h2v

ElectroCrafters:
https://www.youtube.com/channel/UCu1aqzUdTqSnTN5BHNlbVgA */


#include <WiFi.h>
#include <ESP32Servo.h>

const char* ssid = "YOUR WIFI";
const char* password = "YOUR PASSWORD";
WiFiServer server(1234);

// Servo Pins
const int pinJaw = 14;
const int pinA   = 15;   //FWD_REVERSE ARM
const int pinB   = 2;    // UP_DOWN ARM
const int pinD   = 13;   //BOTTOM BASE 

// Servo Objects
Servo servoJaw, servoA, servoB, servoD;

// Angles
int jawAngle = 80, targetJaw = 80;
int angleA = 45, targetA = 45;
int angleB = 90, targetB = 90;
int angleD = 90, targetD = 90;

// Attachment Flags + Inactivity Timing
bool servoA_attached = false;
bool servoB_attached = false;
bool servoD_attached = false;

unsigned long lastAActivity = 0;
unsigned long lastBActivity = 0;
unsigned long lastDActivity = 0;

const unsigned long detachDelay = 3000;

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

  Serial.println("📡 Connecting to Wi-Fi...");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500); Serial.print(".");
  }

  Serial.println("\n✅ Connected! IP: " + WiFi.localIP());
  server.begin();

  // Jaw always attached
  servoJaw.setPeriodHertz(50);
  servoJaw.attach(pinJaw, 500, 2400);
  servoJaw.write(jawAngle);
  Serial.println("🦷 Jaw attached (pin 14)");

  // Servo D is safe to initialize at center (90°)
  servoD.setPeriodHertz(50);
  servoD.attach(pinD, 500, 2400);
  servoD.write(angleD);
  servoD_attached = true;
  Serial.println("🌀 Servo D attached (pin 13) → 90°");
}

void loop() {
  WiFiClient client = server.available();

  if (client) {
    Serial.println("🎮 Controller connected");

    while (client.connected()) {
      if (client.available()) {
        String input = client.readStringUntil('\n');
        input.trim();

        // Jaw servo (ABS_RX)
        if (input.startsWith("AXIS:ABS_RX:")) {
          int absRx = input.substring(input.lastIndexOf(':') + 1).toInt();
          targetJaw = map(absRx, 0, 32767, 70, 90);
        }

        // Servo A (ABS_X)
        else if (input.startsWith("AXIS:ABS_X:")) {
          int absX = input.substring(input.lastIndexOf(':') + 1).toInt();
          targetA = map(absX, -32768, 32767, 0, 90);
          lastAActivity = millis();
          if (!servoA_attached) {
            servoA.setPeriodHertz(50);
            servoA.attach(pinA, 500, 2400);
            servoA.write(angleA);
            servoA_attached = true;
            Serial.println("🦿 Servo A attached (pin 15)");
          }
        }

        // Servo B (ABS_Y)
        else if (input.startsWith("AXIS:ABS_Y:")) {
          int absY = input.substring(input.lastIndexOf(':') + 1).toInt();
          targetB = map(absY, -32768, 32767, 180, 0);
          lastBActivity = millis();
          if (!servoB_attached) {
            servoB.setPeriodHertz(50);
            servoB.attach(pinB, 500, 2400);
            servoB.write(angleB);
            servoB_attached = true;
            Serial.println("🦿 Servo B attached (pin 2)");
          }
        }

        // Servo D (ABS_RY)
        else if (input.startsWith("AXIS:ABS_RY:")) {
          int absRy = input.substring(input.lastIndexOf(':') + 1).toInt();
          targetD = map(absRy, -32768, 32767, 0, 180);
          lastDActivity = millis();
          if (!servoD_attached) {
            servoD.setPeriodHertz(50);
            servoD.attach(pinD, 500, 2400);
            servoD.write(angleD);
            servoD_attached = true;
            Serial.println("🌀 Servo D attached (pin 13)");
          }
        }
      }

      // Smooth jaw motion
      if (jawAngle != targetJaw) {
        jawAngle += (jawAngle < targetJaw) ? 1 : -1;
        servoJaw.write(jawAngle);
        Serial.printf("🦷 Jaw angle → %d°\n", jawAngle);
      }

      // Servo A motion + detach
      if (servoA_attached) {
        if (angleA != targetA) {
          angleA += (angleA < targetA) ? 1 : -1;
          servoA.write(angleA);
          Serial.printf("🦿 Servo A angle → %d°\n", angleA);
        }
        if (millis() - lastAActivity > detachDelay) {
          servoA.detach();
          servoA_attached = false;
          Serial.println("⛓️ Servo A detached (timeout)");
        }
      }

      // Servo B motion + detach
      if (servoB_attached) {
        if (angleB != targetB) {
          angleB += (angleB < targetB) ? 1 : -1;
          servoB.write(angleB);
          Serial.printf("🦿 Servo B angle → %d°\n", angleB);
        }
        if (millis() - lastBActivity > detachDelay) {
          servoB.detach();
          servoB_attached = false;
          Serial.println("⛓️ Servo B detached (timeout)");
        }
      }

      // Servo D motion + detach
      if (servoD_attached) {
        if (angleD != targetD) {
          angleD += (angleD < targetD) ? 1 : -1;
          servoD.write(angleD);
          Serial.printf("🌀 Servo D angle → %d°\n", angleD);
        }
        if (millis() - lastDActivity > detachDelay) {
          servoD.detach();
          servoD_attached = false;
          Serial.println("⛓️ Servo D detached (timeout)");
        }
      }

      delay(10);
    }

    client.stop();
    Serial.println("⚡ Controller disconnected");
  }
}