// Эталонный скетч: робот-собака идёт рысью.
//
// Рысь: диагональные лапы (передняя левая + задняя правая, передняя правая +
// задняя левая) шагают парами. Пока одна пара переносится вперёд по дуге,
// другая стоит и толкает корпус. Положение лапы задаётся точкой под осью
// бедра, углы считает обратная кинематика (бедро и голень по 21 см, колено
// назад — голень уходит вперёд).
//
// Привод: PCA9685 по I2C (0x40). Каналы: 0–2 передняя левая, 3–5 передняя
// правая, 6–8 задняя левая, 9–11 задняя правая — отведение, бедро, колено.
// Значение «прямо» для write(): отведение и бедро 90, колено 0.
// Курс держит гироскоп MPU6050 (0x68): левые и правые лапы шагают чуть
// по-разному, пока курс не выровняется. TURN — поворот (разница шага).

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

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 STEP = 0.10;            // длина шага, м
const float LIFT = 0.05;            // подъём лапы, м
const float T_STEP = 0.25;          // полшага рыси (перенос одной пары), с
const float T_STAND = 1.0;          // встать, с
const float T_START = 1.5;          // начать идти, с
const float TURN = 0;               // поворот: разница шага левых и правых лап, м (+ влево)
const float K_HEAD = 0.002;         // выравнивание курса: м разницы шага на градус ошибки

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

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();
}

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));
  uint16_t off = (uint32_t)us * 4096UL / 19988UL;
  Wire.beginTransmission(PCA);
  Wire.write(0x06 + 4 * ch);
  Wire.write(0); Wire.write(0); Wire.write(off & 0xff); Wire.write(off >> 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);
}

// --- курс по гироскопу -------------------------------------------------------
float heading = 0;                  // градусы, + влево
unsigned long lastImu = 0;

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;
}

void setup() {
  Wire.begin();
  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);
}

void loop() {
  readImu();
  float t = millis() / 1000.0;
  float h = H0 + (H - H0) * smooth(t / T_STAND);
  float s = t - T_START;
  // Диагональные пары: 0 — передняя левая и задняя правая, 1 — две другие.
  const uint8_t pair[4] = {0, 1, 1, 0};
  float ramp = s > 0 ? min(1.0, s / 1.5) : 0;
  // Разница шага левых и правых лап: + — левые короче, поворот влево.
  // Прямо (TURN = 0) — курс держит гироскоп: увело влево — шагаем вправо.
  float diff = TURN != 0 ? TURN : constrain(-K_HEAD * heading, -0.03, 0.03);
  for (uint8_t l = 0; l < 4; l++) {
    float fz = 0, lift = 0;
    if (s > 0) {
      float ph = fmod(s / T_STEP, 2.0);
      float mine = pair[l] == 0 ? ph : fmod(ph + 1, 2.0);
      float L = (STEP - (l % 2 == 0 ? diff : -diff)) * ramp;
      if (mine < 1) {                      // перенос: сзади вперёд по дуге
        fz = -L / 2 + L * smooth(mine);
        lift = LIFT * ramp * sin(M_PI * mine);
      } else {                             // опора: спереди назад — толкает корпус
        fz = L / 2 - L * (mine - 1);
      }
    }
    paw(l * 3, h - lift, fz);
  }
}