// Робот-собака Pin на ESP32 с панелью GyverLibs/Settings.
//
// Те же рысь и прыжок, что в dog_walk.ino и dog_jump.ino, но управляются
// со страницы: кнопка «Веб-интерфейс» над схемой открывает панель. На ней
// переключатель «Идти», ползунки длины шага и времени полушага, выбор
// поворота и кнопка «Прыжок». Курс при ходьбе прямо держит гироскоп.
//
// Привод: PCA9685 (0x40) и MPU6050 (0x68) на шине I2C платы: SDA — D21,
// SCL — D22. Каналы: 0–2 передняя левая, 3–5 передняя правая, 6–8 задняя
// левая, 9–11 задняя правая — отведение, бедро, колено.
// Значение «прямо» для write(): отведение и бедро 90, колено 0.
//
// Приводы обновляются кадрами по 10 мс, все 12 каналов одной передачей I2C,
// а между кадрами плата спит: так эмулятору ESP32 почти нечего считать.

#include <WiFi.h>
#include <Wire.h>
#include <LittleFS.h>
#include <math.h>
#include <SettingsESP.h>

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

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

// --- собака ------------------------------------------------------------

const uint8_t PCA = 0x40, MPU = 0x68;

const float L1 = 0.21, L2 = 0.21;   // бедро, голень, м
const float H = 0.28;               // ось бедра над стопой при ходьбе, м
const float H0 = 0.4195;            // ноги почти прямые (как собака стоит при включении)
const float LIFT = 0.05;            // подъём лапы при шаге, м
const float T_STAND = 1.0;          // встать, с
const float K_HEAD = 0.002;         // выравнивание курса: м разницы шага на градус ошибки

// Прыжок (как в dog_jump.ino).
const float H_CROUCH = 0.20;        // присед
const float H_PUSH = 0.41;          // толчок: лапы почти прямые
const float H_LAND = 0.30;          // в воздухе и при приземлении
const float Z_FRONT = -0.04;        // передние лапы при толчке: назад от плеча, м
const float Z_REAR = -0.08;         // задние лапы при толчке: назад от бедра, м
const float T_PUSH = 0.12;          // сколько длится толчок, с
const float T_JUMP = 3.0;           // весь прыжок вместе с возвратом в стойку, с

const uint16_t FRAME_MS = 10;       // кадр: приводы и гироскоп 100 раз в секунду
const float ZERO[3] = {90, 90, 0};  // write() при прямом суставе: отведение, бедро, колено
const float R = 180.0 / M_PI;

// --- что меняется со страницы -----------------------------------------

bool go = false;                    // идти
uint8_t stepCm = 10;                // длина шага, см
uint8_t halfCs = 25;                // полшага, сотые секунды (25 — 0,25 с)
uint8_t turnMode = 0;               // 0 — прямо, 1 — влево, 2 — вправо
uint8_t turnCm = 5;                 // разница шага левых и правых лап, см
float jumpAt = -1;                  // когда начался прыжок, с; -1 — не прыгает
int jumps = 0;

float walk = 0;                     // 0 — стоит, 1 — идёт (плавно)
float phase = 0;                    // 0…2: полшага рыси
float heading = 0;                  // курс по гироскопу, градусы, + влево
unsigned long lastImu = 0, lastMs = 0;

SettingsESP sett("Pin");
enum : size_t { W_STATE = 1 };

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

void reg(uint8_t dev, uint8_t r, uint8_t v) {
  Wire.beginTransmission(dev); Wire.write(r); Wire.write(v); Wire.endTransmission();
}

// Кадр приводов: 12 значений PCA9685 копятся здесь и уходят на плату одной
// передачей I2C (sendFrame). На ESP32 каждая передача Wire - это драйвер с
// очередями и прерываниями, тысячи команд процессора; двенадцать передач на
// каждом проходе loop() съедали всё время ядра, и модель шла в 5-6 раз
// медленнее настоящего времени.
uint16_t frame[12];

void setJoint(uint8_t ch, uint8_t kind, float deg) {
  float w = ZERO[kind] + deg;
  if (w < 0) w = 0;
  if (w > 180) w = 180;
  uint16_t us = 500 + (uint16_t)(w * (2000.0 / 180.0));
  frame[ch] = (uint32_t)us * 4096UL / 19988UL;
}

// Все 12 каналов разом: PCA9685 с битом AI (MODE1 = 0xA0 в setup) сам
// переходит к следующему регистру, так что хватает одного адреса LED0_ON_L
// (0x06) и 48 байт подряд - буфер Wire на ESP32 вмещает 128.
void sendFrame() {
  Wire.beginTransmission(PCA);
  Wire.write(0x06);
  for (uint8_t ch = 0; ch < 12; ch++) {
    Wire.write(0); Wire.write(0); Wire.write(frame[ch] & 0xff); Wire.write(frame[ch] >> 8);
  }
  Wire.endTransmission();
}

// Лапа: стопа относительно оси бедра — fh вниз, fz вперёд (м).
void paw(uint8_t c0, float fh, float fz) {
  float d = sqrt(fh * fh + fz * fz);
  if (d > L1 + L2 - 0.0005) d = L1 + L2 - 0.0005;
  float k = M_PI - acos((L1 * L1 + L2 * L2 - d * d) / (2 * L1 * L2));
  float th = atan2(fz, fh) - acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d));
  setJoint(c0 + 0, 0, 0);
  setJoint(c0 + 1, 1, th * R);
  setJoint(c0 + 2, 2, k * R);
}

void readImu() {
  Wire.beginTransmission(MPU); Wire.write(0x47); Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)2);
  int16_t gz = (Wire.read() << 8) | Wire.read();
  unsigned long now = micros();
  if (lastImu) heading += gz / 131.0 * (now - lastImu) * 1e-6;
  lastImu = now;
}

// --- страница ----------------------------------------------------------

String stateText() {
  String s = jumpAt >= 0 ? "прыжок" : (walk > 0.05 ? "рысь" : "стоит");
  return s + ", курс " + String(heading, 1) + "°, прыжков " + String(jumps);
}

// build() зовётся и чтобы нарисовать страницу, и чтобы применить действие с
// неё: тронутый виджет возвращает true и уже записал значение по указателю.
void build(sets::Builder &b) {
  if (b.beginGroup("Движение")) {
    b.Switch("Идти", &go);
    b.Slider("Длина шага", 0, 20, 1, " см", &stepCm);
    b.Slider("Полшага", 15, 40, 1, " сотых с", &halfCs);
    b.Select("Поворот", "Прямо;Влево;Вправо", &turnMode);
    b.Slider("Сила поворота", 1, 10, 1, " см", &turnCm);
    b.endGroup();
  }
  if (b.beginGroup("Прыжок")) {
    if (b.beginButtons()) {
      // Прыгает только стоящая собака.
      if (b.Button("Прыжок", sets::Colors::Aqua) && jumpAt < 0 && !go && walk < 0.01) {
        jumpAt = millis() / 1000.0;
        jumps++;
        Serial.printf("прыжок %d\n", jumps);
      }
      b.endButtons();
    }
    b.endGroup();
  }
  if (b.beginGroup("Состояние")) {
    b.Label(W_STATE, "Pin", 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("Pin: http://%s/\n", ip.c_str());
}

// --- setup и loop -----------------------------------------------------

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.setClock(400000);
  reg(PCA, 0x00, 0x10);
  reg(PCA, 0xFE, 121);         // 50 Гц
  reg(PCA, 0x00, 0x20);
  delay(5);
  reg(PCA, 0x00, 0xA0);
  reg(MPU, 0x6B, 0);
  startNetwork();
  lastMs = millis();
}

void loop() {
  sett.tick();

  // Кадр раз в 10 мс. Между кадрами ядро спит в delay() - эмулятор
  // проскакивает сон, не считая его по такту. Кадр 20 мс (период импульсов
  // сервопривода) на быстрой рыси даёт рывки: в модели наклон до 30° вместо 15°.
  static unsigned long nextFrame = 0;
  unsigned long now = millis();
  if ((long)(now - nextFrame) < 0) { delay(nextFrame - now); return; }
  nextFrame = now + FRAME_MS;

  readImu();
  unsigned long ms = millis();
  float t = ms / 1000.0;
  float dt = (ms - lastMs) / 1000.0;
  lastMs = ms;
  if (dt > 0.1) dt = 0.1;

  // Встать с прямых лап, потом идти или прыгать.
  float h = H0 + (H - H0) * smooth(t / T_STAND);

  // --- рысь: плавно разгоняемся и тормозим ---
  float target = (go && jumpAt < 0 && t > T_STAND) ? 1 : 0;
  walk += constrain(target - walk, -dt / 1.5, dt / 1.5);
  if (walk > 0) phase = fmod(phase + dt / (halfCs / 100.0), 2.0);

  // Разница шага левых и правых лап: + — левые короче, поворот влево.
  // Прямо — курс держит гироскоп: увело влево — шагаем вправо.
  float diff;
  if (turnMode == 1) { diff = turnCm / 100.0; heading = 0; }
  else if (turnMode == 2) { diff = -turnCm / 100.0; heading = 0; }
  else diff = constrain(-K_HEAD * heading, -0.03, 0.03);

  // Диагональные пары: 0 — передняя левая и задняя правая, 1 — две другие.
  const uint8_t pair[4] = {0, 1, 1, 0};
  float hh[4], fz[4], lift[4];
  for (uint8_t l = 0; l < 4; l++) {
    hh[l] = h; fz[l] = 0; lift[l] = 0;
    float mine = pair[l] == 0 ? phase : fmod(phase + 1, 2.0);
    float len = (stepCm / 100.0 - (l % 2 == 0 ? diff : -diff)) * walk;
    if (mine < 1) {                    // перенос: сзади вперёд по дуге
      fz[l] = -len / 2 + len * smooth(mine);
      lift[l] = LIFT * walk * sin(M_PI * mine);
    } else {                           // опора: спереди назад — толкает корпус
      fz[l] = len / 2 - len * (mine - 1);
    }
  }

  // --- прыжок: присесть, лапы назад, толчок, поджать, приземлиться ---
  if (jumpAt >= 0) {
    float c = t - jumpAt;
    float zf = 0, zr = 0, jh;
    if (c < 1.0) jh = H + (H_CROUCH - H) * smooth(c);
    else if (c < 1.5) { jh = H_CROUCH; zf = Z_FRONT * (c - 1.0) / 0.5; zr = Z_REAR * (c - 1.0) / 0.5; }
    else if (c < 1.5 + T_PUSH) { jh = H_PUSH; zf = Z_FRONT; zr = Z_REAR; }
    else if (c < 2.3) jh = H_LAND;
    else jh = H_LAND + (H - H_LAND) * smooth((c - 2.3) / 0.5);
    for (uint8_t l = 0; l < 4; l++) { hh[l] = jh; fz[l] = l < 2 ? zf : zr; lift[l] = 0; }
    if (c > T_JUMP) jumpAt = -1;
  }

  for (uint8_t l = 0; l < 4; l++) paw(l * 3, hh[l] - lift[l], fz[l]);
  sendFrame();
}