// Эталонный скетч: робот-человек идёт к стене и останавливается перед ней
// (площадка «Стена в 2 м»). Походка — та же, что в walk.ino; дальномер в голове
// (TRIG — D7, ECHO — D8) меряет расстояние раз в 0,2 с. Ближе STOP_CM —
// следующий шаг приставной (стопа встаёт рядом с опорной), и робот замирает.
// В порт: «до стены 150 см» и «стоп: 58 см».
//
// Привод: PCA9685 0x40 — ноги (0–11), гироскоп MPU6050 0x68 — равновесие и курс.

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

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

// --- размеры и параметры походки -------------------------------------------
const float L1 = 0.42, L2 = 0.42;   // бедро, голень, м
const float HIP = 0.06;             // полуширина таза, м
const float H = 0.87;               // высота таза над полом, м
const float ANKLE = 0.08;           // высота голеностопа над полом, м
const float STEP = 0.12;            // длина шага, м
const float T_STEP = 0.5;           // длительность шага, с
const float SWAY = 0.02;            // качание таза вбок, м
const float LIFT = 0.05;            // подъём стопы, м
const float DOUBLE = 0.2;           // доля шага на двух ногах
const float T_START = 0.5;          // постоять перед ходом, с
const float RAMP = 2;               // шагов на разгон
const float STOP_CM = 60;          // остановиться ближе, см
const uint8_t TRIG = 7, ECHO = 8;    // дальномер в голове
const float TURN = 0;               // поворот за шаг, градусы (+ влево); 0 — прямо
const float K_HEAD = 0.3;           // выравнивание курса: доля ошибки курса за шаг

// Обратная связь: поправки в градусах на градус крена.
const float K_ANKLE = -0.3;         // голеностопы (крен)
const float K_HIP = 0.3;            // бёдра (крен)
const float K_FOOT = 0.3;           // шагающая стопа вбок, м на радиан крена
const float K_STEP = 0.6;           // шаг длиннее при наклоне вперёд, м на радиан
const float STEP_ADJ = 0.08;        // предел подстройки шага, м

const float ZERO[6] = {90, 40, 0, 90, 90, 90};   // 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();
}

// Угол сустава (градусы от «прямо») → импульс → канал PCA9685.
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();
}

// Нога по положению стопы относительно таза (м): dx вбок, fh вниз, fz вперёд.
// corrA — поправка голеностопа (крен), corrH — бедра (крен), градусы.
// yaw — поворот бедра (градусы, + носок влево) на канале yawCh: стопа
// задана в осях таза, нога под приводом поворота повёрнута — переводим.
void leg(uint8_t c0, uint8_t yawCh, float yaw, float dx, float fh, float fz, float corrA, float corrH) {
  float a = -yaw / R, ca = cos(a), sa = sin(a);
  float dx2 = dx * ca + fz * sa;
  fz = -dx * sa + fz * ca;
  dx = dx2;
  setJoint(yawCh, 5, yaw);
  float phi = atan2(dx, fh);                       // крен бедра
  float lv = sqrt(fh * fh + dx * dx);
  float d = sqrt(lv * lv + 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, lv) + acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d));  // бедро
  setJoint(c0 + 0, 0, phi * R + corrH);
  setJoint(c0 + 1, 1, th * R);
  setJoint(c0 + 2, 2, k * R);
  setJoint(c0 + 3, 3, (k - th) * R);               // стопа параллельно полу
  setJoint(c0 + 4, 4, -phi * R + corrA);
}

// --- датчик наклона ---------------------------------------------------------
float roll = 0;                     // крен корпуса, градусы (+ — вправо)
float pitch = 0;                    // тангаж, градусы (+ — вперёд)
float heading = 0;                  // курс от старта, градусы (+ — влево)
unsigned long lastImu = 0;

int16_t rd16() { int16_t h = Wire.read(); return (h << 8) | Wire.read(); }

void readImu() {
  Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14);
  int16_t ax = rd16(), ay = rd16(), az = rd16(); rd16();
  int16_t gx = rd16(), gy = rd16(), gz = rd16();
  unsigned long now = micros();
  float dt = lastImu ? (now - lastImu) * 1e-6 : 0;
  lastImu = now;
  // Ось X датчика смотрит вперёд: крен — поворот вокруг неё. Акселерометр
  // на шагающем роботе шумит (удары стоп), поэтому он лишь медленно
  // подтягивает угол, а быстрые изменения даёт гироскоп.
  heading += gz / 131.0 * dt;                  // курс: только гироскоп (ось Z датчика — вверх)
  if (az < 4000) return;                       // удар или свободное падение — не верим
  float accRoll = atan2((float)ay, (float)az) * R;
  float accPitch = atan2(-(float)ax, (float)az) * R;
  float w = millis() < 400 ? 0.1 : 0.005;      // пока робот садится на ноги — быстрее
  roll = (1 - w) * (roll + gx / 131.0 * dt) + w * accRoll;
  pitch = (1 - w) * (pitch + gy / 131.0 * dt) + w * accPitch;
}

void setup() {
  Serial.begin(9600);
  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);           // разбудить датчик
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
}

// Дальномер: см до препятствия или −1 (нет эха). Ждём эхо не дольше 25 мс —
// это 4 м; цикл походки на это время замирает, поэтому мерим нечасто.
long distanceCm() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  unsigned long us = pulseIn(ECHO, HIGH, 25000);
  return us ? (long)(us / 58) : -1;
}

unsigned long lastSonar = 0;
bool stopping = false, halted = false;  // увидел стену; встал
int stopK = -1;                         // шаг, на котором стопа встала рядом

// Шаги с памятью: стопа встаёт туда, куда её довели, а не по расписанию —
// иначе подстройка шага по наклону на следующем шаге «отыгралась» бы назад.
int k = -1;                     // номер текущего шага
float zl = 0, zr = 0;           // где стоят стопы (вперёд, м)
float zFrom = 0, zTo = 0;       // шагающая нога: откуда и куда по плану
float adj = 0;                  // подстройка цели по наклону, м
float course = 0;               // курс, который должен быть, градусы
float turnStep = 0;             // поворот в этом шаге, градусы

void loop() {
  readImu();
  float t = millis() / 1000.0;
  if (millis() - lastSonar > 200 && !stopping) {
    lastSonar = millis();
    long cm = distanceCm();
    if (cm > 0) {
      Serial.print("до стены "); Serial.print(cm); Serial.println(" см");
      if (cm < STOP_CM) { stopping = true; Serial.print("стоп: "); Serial.print(cm); Serial.println(" см"); }
    }
  }

  float px = 0, pz = 0;         // таз: вбок (+ влево), вперёд, м
  float ZL = zl, ZR = zr;       // стопы сейчас
  float yl = 0, yr = 0;         // подъём стоп, м
  float ql = 0, qr = 0;         // поворот бёдер, градусы

  float s = t - T_START;
  if (halted) {
    // Стоим: стопы рядом, таз над ними, без качания.
    pz = (zl + zr) / 2;
  } else if (s > 0) {
    int kk = (int)(s / T_STEP);                // номер шага
    float u = (s - kk * T_STEP) / T_STEP;      // 0…1 внутри шага
    if (kk != k) {                             // новый шаг
      if (k >= 0) { if (k % 2 == 0) zr = zTo + adj; else zl = zTo + adj; }
      if (stopK >= 0) { halted = true; k = kk; return; }   // приставной шаг сделан
      k = kk;
      float stance = (k % 2 == 0) ? zl : zr;
      zFrom = (k % 2 == 0) ? zr : zl;
      zTo = stance + STEP * min(1.0, k / RAMP);
      adj = 0;
      if (stopping) { zTo = stance; stopK = k; }   // приставить стопу
      // Курс: задуманный копится по TURN, ошибку по гироскопу выбираем
      // частью на каждом шаге — робот не уводит в сторону.
      float t1 = TURN * min(1.0, k / RAMP);
      course += t1;
      turnStep = constrain(t1 + K_HEAD * (course - heading), -15, 15);
    }
    float dir = (k % 2 == 0) ? 1 : -1;         // +1 опора на левой
    float stance = dir > 0 ? zl : zr;
    px = dir * SWAY * min(1.0, (k + u) / RAMP) * sin(M_PI * u);
    float v = (u - DOUBLE / 2) / (1 - DOUBLE);
    // Пока нога в воздухе, цель сдвигается: клонится вперёд — шаг длиннее.
    if (v > 0 && v < 0.8 && stopK < 0) adj = constrain(K_STEP * pitch / R, -STEP_ADJ, STEP_ADJ);
    float target = zTo + adj;
    float lift = (v > 0 && v < 1) ? LIFT * sin(M_PI * v) : 0;
    float zs = zFrom + (target - zFrom) * smooth(v);
    if (dir > 0) { ZR = zs; yr = lift; } else { ZL = zs; yl = lift; }
    // Поворот: пока одна нога в воздухе, опорное бедро доворачивается на
    // TURN (корпус поворачивается над стопой), шагающее — в обратную сторону,
    // чтобы стопа встала уже по новому курсу. На двух ногах углы стоят.
    float qs = turnStep / 2 - turnStep * smooth(v), qw = -qs;
    if (dir > 0) { ql = qs; qr = qw; } else { qr = qs; ql = qw; }
    pz = (zFrom + stance) / 2 + (target - zFrom) / 2 * u;
  }

  float fx = -K_FOOT * roll / R;              // крен вправо → стопа правее
  float cA = K_ANKLE * roll, cH = K_HIP * roll;
  leg(0, 10, ql, HIP - (px + HIP) + (yl > 0 ? fx : 0), H - (ANKLE + yl), ZL - pz, cA, cH);
  leg(5, 11, qr, -HIP - (px - HIP) + (yr > 0 ? fx : 0), H - (ANKLE + yr), ZR - pz, cA, cH);
}