// Эталонный скетч: робот-человек берёт кубик с полки (площадка «Полка с кубиком»).
//
// Ноги держат стойку. Правая рука: согнуть локоть (кисть к груди), поднять
// плечо, опустить кисть на кубик сверху, сжать пальцы — кубик схвачен, —
// и поднять руку. Кубик весит 200 г, полка — под правой рукой.
//
// Привод: PCA9685 0x40 — ноги (0–9) и руки (12–15: плечо и локоть левой,
// плечо и локоть правой); PCA9685 0x41 — пальцы (канал 1 — правая кисть).
// «Прямо»: плечо write(60), локоть write(0), пальцы write(0) — ладонь.
//
// Хват в модели: пальцы сжаты сильнее 60° и кубик у ладони — он держится в
// кисти; разжать (меньше 30°) — отпустить.

#include <Wire.h>
#include <math.h>

const uint8_t PCA = 0x40, PCA2 = 0x41;
const float L1 = 0.42, L2 = 0.42, H = 0.87, ANKLE = 0.08;
const float R = 180.0 / M_PI;

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

// Угол от «прямо» → write() → импульс → канал.
void pulse(uint8_t dev, uint8_t ch, float zero, float deg) {
  float w = zero + deg;
  if (w < 0) w = 0;
  if (w > 180) w = 180;
  uint16_t us = 500 + (uint16_t)(w * (2000.0 / 180.0));
  uint16_t off = (uint32_t)us * 4096UL / 19988UL;
  Wire.beginTransmission(dev);
  Wire.write(0x06 + 4 * ch);
  Wire.write(0); Wire.write(0); Wire.write(off & 0xff); Wire.write(off >> 8);
  Wire.endTransmission();
}

void joint(uint8_t ch, float zero, float deg) { pulse(PCA, ch, zero, deg); }

// Нога прямо под тазом на высоте H: бедро и колено по обратной кинематике.
void leg(uint8_t c0) {
  float d = H - ANKLE;
  float k = M_PI - acos((L1 * L1 + L2 * L2 - d * d) / (2 * L1 * L2));
  float th = acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d));
  joint(c0 + 0, 90, 0);
  joint(c0 + 1, 40, th * R);
  joint(c0 + 2, 0, k * R);
  joint(c0 + 3, 90, (k - th) * R);
  joint(c0 + 4, 90, 0);
}

// Ключевые позы правой руки: время, плечо, локоть, пальцы (градусы от «прямо»).
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 smooth(float x) { x = x < 0 ? 0 : (x > 1 ? 1 : x); return x * x * (3 - 2 * x); }

void setup() {
  Serial.begin(9600);
  Wire.begin();
  const uint8_t devs[2] = {PCA, PCA2};
  for (uint8_t i = 0; i < 2; i++) {
    reg(devs[i], 0x00, 0x10); reg(devs[i], 0xFE, 121); reg(devs[i], 0x00, 0x20);
  }
  delay(5);
  reg(PCA, 0x00, 0xA0); reg(PCA2, 0x00, 0xA0);
}

void loop() {
  float t = millis() / 1000.0;
  leg(0); leg(5);
  float p[3] = {KEYS[NKEYS - 1][1], KEYS[NKEYS - 1][2], KEYS[NKEYS - 1][3]};
  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]));
      for (uint8_t j = 0; j < 3; j++) p[j] = KEYS[i][j + 1] + (KEYS[i + 1][j + 1] - KEYS[i][j + 1]) * u;
      break;
    }
  }
  joint(14, 60, p[0]);                 // правое плечо
  joint(15, 0, p[1]);                  // правый локоть
  pulse(PCA2, 1, 0, p[2]);             // правая кисть
  joint(12, 60, 10); joint(13, 0, 10); // левая рука чуть вперёд
  delay(20);
}