#include <Arduino.h>
#include <math.h>

class LabServo {
 public:
  void attach(int pin) {
    pin_ = pin;
    ledcAttach(pin_, 50, 16);
    write(angle_);
  }
  void write(int angle) {
    angle_ = constrain(angle, 0, 180);
    const uint32_t pulseUs = 500 + lroundf(angle_ * (1900.0f / 180.0f));
    ledcWrite(pin_, lroundf(pulseUs * (65535.0f / 20000.0f)));
  }
  int read() const { return angle_; }
 private:
  int pin_ = -1;
  int angle_ = 90;
};

LabServo baseServo, armServo, gripperServo;

struct Position { int base, arm, gripper; };

void moveToPosition(Position target, int duration);
void grabSequence();
void releaseSequence();
void pickAndPlace();

void setup() {
  Serial.begin(115200);
  baseServo.attach(18);
  armServo.attach(19);
  gripperServo.attach(21);
  moveToPosition({90, 90, 90}, 1000);
  Serial.println("Commands: h=home, g=grab, r=release, s=sequence");
}

void loop() {
  if (Serial.available()) {
    char cmd = Serial.read();
    switch (cmd) {
      case 'h': moveToPosition({90, 90, 90}, 1500); break;
      case 'g': grabSequence(); break;
      case 'r': releaseSequence(); break;
      case 's': pickAndPlace(); break;
    }
  }
}

void moveToPosition(Position target, int duration) {
  int curB = baseServo.read(), curA = armServo.read(), curG = gripperServo.read();
  int steps = max(1, duration / 20);
  for (int i = 0; i <= steps; i++) {
    float p = (float)i / steps;
    baseServo.write(curB + (target.base - curB) * p);
    armServo.write(curA + (target.arm - curA) * p);
    gripperServo.write(curG + (target.gripper - curG) * p);
    delay(20);
  }
  Serial.printf("Position base=%d arm=%d gripper=%d\n",
                target.base, target.arm, target.gripper);
}

void grabSequence() {
  Serial.println("Sequence: grab");
  moveToPosition({90, 45, 90}, 1000); delay(500);
  moveToPosition({90, 45, 45}, 800);  delay(500);
  moveToPosition({90, 90, 45}, 1000);
}

void releaseSequence() {
  Serial.println("Sequence: release");
  moveToPosition({90, 45, 45}, 1000); delay(500);
  moveToPosition({90, 45, 90}, 800);  delay(500);
  moveToPosition({90, 90, 90}, 1000);
}

void pickAndPlace() {
  Serial.println("Sequence: pick-and-place");
  moveToPosition({45, 45, 90}, 1500);  delay(500);
  moveToPosition({45, 45, 45}, 800);   delay(500);
  moveToPosition({135, 45, 45}, 2000); delay(500);
  moveToPosition({135, 45, 90}, 800);  delay(500);
  moveToPosition({90, 90, 90}, 1500);
}