---

/*
 * Захват-манипулятор на 4 сервоприводах SG90
 * Платформа: Arduino Nano
 * Управление: Serial 9600
 *
 * Клешни (G1, G2)  — зеркально (открытие/закрытие).
 * Подъём  (L1, L2) — зеркально, т.к. стоят друг напротив друга.
 *
 * Угол наклона задаётся как delta от 90°:
 *   L1 = 90 - delta   (серво 1)
 *   L2 = 90 + delta   (серво 2)
 *   delta < 0 — вниз,  delta > 0 — вверх
 *
 * Питание серво — ОТДЕЛЬНОЕ 5В ≥ 3А! GND общий с Arduino.
 */

#include <Servo.h>

// ---------- ГРАНИЦЫ УГЛОВ КЛЕШНЕЙ ----------
const int G1_OPEN   = 20;
const int G1_CLOSED = 90;
const int G2_OPEN   = 160;
const int G2_CLOSED = 90;

// ---------- ГРАНИЦЫ НАКЛОНА ПОДЪЁМА ----------
// Задаём смещение относительно центра 90°, симметрично
const int LIFT_DELTA_DOWN = -60;   // насколько опущено (delta)
const int LIFT_DELTA_UP   =  60;   // насколько поднято  (delta)

// ---------- ПИНЫ ----------
const int G1_PIN = 9;
const int G2_PIN = 10;
const int L1_PIN = 11;
const int L2_PIN = 12;

// ---------- СКОРОСТЬ ----------
const int STEP_DELAY_MS = 15;

Servo servoG1, servoG2, servoL1, servoL2;

int curG1, curG2, curL1, curL2;
bool gripperOpen = true;
bool liftIsUp    = false;

// ========== Преобразование delta -> углы L1/L2 ==========
// delta: -70..+70 (0 = середина, + = вверх, − = вниз)
int lift1Angle(int delta) { return 90 - delta; }
int lift2Angle(int delta) { return 90 + delta; }

// ========== Плавное движение всех 4 серво ==========
// tL1delta — задаётся как delta для подъёма
void moveAllDelta(int tG1, int tG2, int liftDelta) {
  liftDelta = constrain(liftDelta,
                        min(LIFT_DELTA_DOWN, LIFT_DELTA_UP),
                        max(LIFT_DELTA_DOWN, LIFT_DELTA_UP));

  int tL1 = lift1Angle(liftDelta);
  int tL2 = lift2Angle(liftDelta);

  // Защита клешней
  tG1 = constrain(tG1, min(G1_OPEN, G1_CLOSED), max(G1_OPEN, G1_CLOSED));
  tG2 = constrain(tG2, min(G2_OPEN, G2_CLOSED), max(G2_OPEN, G2_CLOSED));

  while (curG1 != tG1 || curG2 != tG2 || curL1 != tL1 || curL2 != tL2) {
    if (curG1 < tG1) curG1++; else if (curG1 > tG1) curG1--;
    if (curG2 < tG2) curG2++; else if (curG2 > tG2) curG2--;
    if (curL1 < tL1) curL1++; else if (curL1 > tL1) curL1--;
    if (curL2 < tL2) curL2++; else if (curL2 > tL2) curL2--;

    servoG1.write(curG1);
    servoG2.write(curG2);
    servoL1.write(curL1);
    servoL2.write(curL2);
    delay(STEP_DELAY_MS);
  }
}

// ========== Простые действия ==========
void openGripper() {
  moveAllDelta(G1_OPEN, G2_OPEN, liftCurrentDelta());
  gripperOpen = true;
  Serial.println(F("-> GRIP: OPEN"));
}

void closeGripper() {
  moveAllDelta(G1_CLOSED, G2_CLOSED, liftCurrentDelta());
  gripperOpen = false;
  Serial.println(F("-> GRIP: CLOSE"));
}

// Текущий delta подъёма (по среднему положению L1 и L2)
int liftCurrentDelta() {
  // L1 = 90 - d, значит d = 90 - L1
  return 90 - curL1;
}

void liftArm() {
  moveAllDelta(curG1, curG2, LIFT_DELTA_UP);
  liftIsUp = true;
  Serial.println(F("-> LIFT: UP"));
}

void lowerArm() {
  moveAllDelta(curG1, curG2, LIFT_DELTA_DOWN);
  liftIsUp = false;
  Serial.println(F("-> LIFT: DOWN"));
}

// ========== Составные действия ==========
void pickUp() {
  Serial.println(F(">> PICK: open + lower"));
  moveAllDelta(G1_OPEN, G2_OPEN, LIFT_DELTA_DOWN);
  gripperOpen = true; liftIsUp = false;
  delay(300);

  Serial.println(F(">> PICK: close"));
  moveAllDelta(G1_CLOSED, G2_CLOSED, LIFT_DELTA_DOWN);
  gripperOpen = false;
  delay(300);

  Serial.println(F(">> PICK: lift up"));
  moveAllDelta(G1_CLOSED, G2_CLOSED, LIFT_DELTA_UP);
  liftIsUp = true;
  Serial.println(F("-> PICK done"));
}

void release() {
  Serial.println(F(">> RELEASE: lower"));
  moveAllDelta(curG1, curG2, LIFT_DELTA_DOWN);
  liftIsUp = false;
  delay(300);

  Serial.println(F(">> RELEASE: open"));
  moveAllDelta(G1_OPEN, G2_OPEN, LIFT_DELTA_DOWN);
  gripperOpen = true;
  delay(300);

  Serial.println(F(">> RELEASE: lift up"));
  moveAllDelta(G1_OPEN, G2_OPEN, LIFT_DELTA_UP);
  liftIsUp = true;
  Serial.println(F("-> RELEASE done"));
}

// ========== Ручные команды ==========
void setGrip(int delta) {
  delta = constrain(delta, -70, 70);
  int t1 = 90 - delta;
  int t2 = 90 + delta;
  moveAllDelta(t1, t2, liftCurrentDelta());
  gripperOpen = (delta > 0);
  Serial.print(F("-> GRIP POS "));
  Serial.print(t1); Serial.print(F(" / ")); Serial.println(t2);
}

// L<delta> : delta наклона (-70..+70)
void setLift(int delta) {
  delta = constrain(delta, -70, 70);
  moveAllDelta(curG1, curG2, delta);
  liftIsUp = (delta > 0);
  Serial.print(F("-> LIFT DELTA "));
  Serial.print(delta);
  Serial.print(F("  (L1=")); Serial.print(90 - delta);
  Serial.print(F(" L2=")); Serial.print(90 + delta);
  Serial.println(F(")"));
}

// ========== Инфо ==========
void printStatus() {
  Serial.print(F("STATUS: grip="));
  Serial.print(gripperOpen ? F("OPEN") : F("CLOSED"));
  Serial.print(F("  lift="));
  Serial.print(liftIsUp ? F("UP") : F("DOWN"));
  Serial.print(F("  [G1=")); Serial.print(curG1);
  Serial.print(F(" G2="));   Serial.print(curG2);
  Serial.print(F(" L1="));   Serial.print(curL1);
  Serial.print(F(" L2="));   Serial.print(curL2);
  Serial.println(F("]"));
}

void printHelp() {
  Serial.println(F("===== GRIPPER ARM 4x SG90 ====="));
  Serial.println(F(" O / OPEN    - open gripper"));
  Serial.println(F(" C / CLOSE   - close gripper"));
  Serial.println(F(" U / UP      - lift up"));
  Serial.println(F(" D / DOWN    - lower"));
  Serial.println(F(" P / PICK    - take object"));
  Serial.println(F(" R / RELEASE - put object down"));
  Serial.println(F(" G<deg>      - grip pos (offset), e.g. G45"));
  Serial.println(F(" L<delta>    - lift delta (-70..70), e.g. L40"));
  Serial.println(F(" S / STATUS  - show state"));
  Serial.println(F(" H / HELP    - this help"));
  Serial.println(F("==============================="));
}

// ========== SETUP ==========
void setup() {
  Serial.begin(9600);

  servoG1.attach(G1_PIN);
  servoG2.attach(G2_PIN);
  servoL1.attach(L1_PIN);
  servoL2.attach(L2_PIN);

  // Стартовое: клешни раскрыты, рука опущена
  curG1 = G1_OPEN;   curG2 = G2_OPEN;
  int startDelta = LIFT_DELTA_DOWN;
  curL1 = lift1Angle(startDelta);
  curL2 = lift2Angle(startDelta);

  servoG1.write(curG1);
  servoG2.write(curG2);
  servoL1.write(curL1);
  servoL2.write(curL2);

  delay(500);
  printHelp();
  printStatus();
}

// ========== Обработка команд ==========
void handleCommand(String cmd) {
  cmd.trim();
  if (cmd.length() == 0) return;

  char c = toupper(cmd.charAt(0));

  switch (c) {
    case 'O': openGripper(); break;
    case 'C': closeGripper(); break;
    case 'U': liftArm(); break;
    case 'D': lowerArm(); break;
    case 'P': pickUp(); break;
    case 'R': release(); break;
    case 'S': printStatus(); break;
    case 'H': printHelp(); break;

    case 'G': {
      String num = cmd.substring(1); num.trim();
      if (num.length() == 0) { Serial.println(F("Err: use G<deg>")); return; }
      setGrip(num.toInt());
      break;
    }

    case 'L': {
      String num = cmd.substring(1); num.trim();
      if (num.length() == 0) { Serial.println(F("Err: use L<delta>")); return; }
      setLift(num.toInt());
      break;
    }

    default:
      Serial.print(F("Unknown cmd: ")); Serial.println(cmd);
      Serial.println(F("Type H for help"));
  }
}

// ========== LOOP ==========
void loop() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    handleCommand(cmd);
  }
}