// Рука Port на ESP32 с панелью GyverLibs/Settings (площадка «Полка с кубиком»).
//
// Три привода прямо на выводах платы (библиотека ESP32Servo):
//   S0 плечо  ← D25  (write(60) — рука вниз, больше — вперёд-вверх),
//   S1 локоть ← D26  (write(0) — прямой, больше — согнут),
//   S2 пальцы ← D27  (write(0) — ладонь, 90 — кулак).
//
// Кнопка «Веб-интерфейс» над схемой открывает панель: три ползунка ведут руку
// вручную, кнопка «Взять кубик» проигрывает те же позы, что рука из примера
// arm_pick.ino, а «Разжать» отпускает кубик. Хват в модели: пальцы сжаты
// сильнее 60° и кубик у ладони — он держится в кисти; меньше 30° — отпускает.

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

// --- сеть -------------------------------------------------------------

// В модели сеть PinPort-GUEST есть всегда. Не подключилась за 15 секунд —
// плата поднимает свою точку доступа «Arm».
const char *SSID = "PinPort-GUEST", *PASSWORD = "";
const char *AP_SSID = "Arm", *AP_PASSWORD = "robotarm1";

// --- рука --------------------------------------------------------------

const byte PIN_SHOULDER = 25, PIN_ELBOW = 26, PIN_GRIP = 27;
Servo shoulder, elbow, grip;

// Углы привода (write), которые задаёт страница.
uint8_t angShoulder = 60, angElbow = 0, angGrip = 0;

// Время (с), плечо, локоть, пальцы - градусы от «прямо» (как в arm_pick.ino).
const float KEYS[][4] = {
  {0, 0, 0, 0}, {1, 0, 0, 0},
  {2, 0, 70, 0},      // локоть вверх - кисть к груди
  {3, 10, 70, 0},     // кисть над кубиком
  {4.5, 20, 30, 0},   // опустить на кубик
  {5.3, 20, 30, 90},  // сжать пальцы
  {7, 30, 70, 90},    // поднять
};
const uint8_t NKEYS = sizeof(KEYS) / sizeof(KEYS[0]);
float seqAt = -1;                   // когда началась последовательность, с; -1 - не идёт

SettingsESP sett("Рука Port");
enum : size_t { W_STATE = 1 };

float smooth(float x) { x = x < 0 ? 0 : (x > 1 ? 1 : x); return x * x * (3 - 2 * x); }

String stateText() {
  String s = seqAt >= 0 ? "берёт кубик" : "вручную";
  return s + ": плечо " + String((int)angShoulder) + "°, локоть " + String((int)angElbow) +
         "°, пальцы " + String((int)angGrip) + "°";
}

// build() зовётся и чтобы нарисовать страницу, и чтобы применить действие с
// неё: тронутый виджет возвращает true и уже записал значение по указателю.
void build(sets::Builder &b) {
  if (b.beginGroup("Рука")) {
    b.Slider("Плечо", 0, 180, 1, "°", &angShoulder);
    b.Slider("Локоть", 0, 180, 1, "°", &angElbow);
    b.Slider("Пальцы", 0, 90, 1, "°", &angGrip);
    b.endGroup();
  }
  if (b.beginGroup("Действия")) {
    if (b.beginButtons()) {
      if (b.Button("Взять кубик", sets::Colors::Aqua) && seqAt < 0) seqAt = millis() / 1000.0;
      if (b.Button("Разжать", sets::Colors::Red)) { seqAt = -1; angGrip = 0; }
      b.endButtons();
    }
    b.endGroup();
  }
  if (b.beginGroup("Состояние")) {
    b.Label(W_STATE, "Рука", stateText());
    b.endGroup();
  }
}

// Раз в секунду страница спрашивает, что поменялось.
void update(sets::Updater &u) {
  u.update(W_STATE, stateText());
}

void startNetwork() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(SSID, PASSWORD);
  unsigned long t0 = millis();
  while (WiFi.status() != WL_CONNECTED && millis() - t0 < 15000) delay(250);
  String ip;
  if (WiFi.status() == WL_CONNECTED) {
    ip = WiFi.localIP().toString();
  } else {
    WiFi.mode(WIFI_AP);
    WiFi.softAP(AP_SSID, AP_PASSWORD);
    ip = WiFi.softAPIP().toString();
  }
  if (!LittleFS.begin(true)) Serial.println("LittleFS не поднялась");
  sett.begin();
  sett.onBuild(build);
  sett.onUpdate(update);
  sett.setUpdatePeriod(1000);
  Serial.printf("Рука: http://%s/\n", ip.c_str());
}

void setup() {
  Serial.begin(115200);
  shoulder.attach(PIN_SHOULDER, 500, 2500);
  elbow.attach(PIN_ELBOW, 500, 2500);
  grip.attach(PIN_GRIP, 500, 2500);
  startNetwork();
}

void loop() {
  sett.tick();

  // Последовательность «Взять кубик»: между ключевыми позами углы меняются
  // плавно. Закончилась - ползунки остаются на последней позе.
  if (seqAt >= 0) {
    float t = millis() / 1000.0 - seqAt;
    if (t >= KEYS[NKEYS - 1][0]) {
      angShoulder = 60 + KEYS[NKEYS - 1][1];
      angElbow = KEYS[NKEYS - 1][2];
      angGrip = KEYS[NKEYS - 1][3];
      seqAt = -1;
      Serial.println("кубик взят");
    } else {
      for (uint8_t i = 0; i + 1 < NKEYS; i++) {
        if (t < KEYS[i + 1][0]) {
          float u = smooth((t - KEYS[i][0]) / (KEYS[i + 1][0] - KEYS[i][0]));
          angShoulder = 60 + KEYS[i][1] + (KEYS[i + 1][1] - KEYS[i][1]) * u;
          angElbow = KEYS[i][2] + (KEYS[i + 1][2] - KEYS[i][2]) * u;
          angGrip = KEYS[i][3] + (KEYS[i + 1][3] - KEYS[i][3]) * u;
          break;
        }
      }
    }
  }

  shoulder.write(angShoulder);
  elbow.write(angElbow);
  grip.write(angGrip);
  // Между кадрами ядро спит в delay(): эмулятор проскакивает сон, не считая
  // его по такту.
  delay(10);
}