// Эталонный скетч: Port проходит лабиринт (площадка «Лабиринт», 1.9.73).
//
// Идёт по коридору той же походкой, что walk.ino; дальномер в голове (TRIG —
// D7, ECHO — D8) меряет путь раз в 0,2 с. Стена ближе STOP_CM:
//   1. повернуть направо (на месте, 90°, курс держит гироскоп) и посмотреть;
//   2. занято — повернуть налево (на 180° от прежнего), посмотреть;
//   3. и там занято — тупик: ещё 90°, назад, откуда пришли;
//   4. свободно — идти по новому коридору.
// Голова поворачивается только на ±60°, вбок под прямым углом не посмотреть —
// поэтому поворачивается весь робот. Это правило «сначала направо»: простой
// лабиринт без петель так проходится.
// В порт: «стена 68 см», «направо: 340 см — свободно», «поворот 2».
//
// Привод: PCA9685 0x40 — ноги (0–11); PCA9685 0x41 — голова (канал 2 —
// поворот, «прямо» write(90)); гироскоп 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 = 100;          // стена ближе — повернуть (полкоридора: встать посередине), см
const float FREE_CM = 120;          // сторона свободна, если дальше, см
const uint8_t PCA2 = 0x41;          // второй драйвер: голова
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);           // разбудить датчик
  reg(PCA2, 0x00, 0x10); reg(PCA2, 0xFE, 121); reg(PCA2, 0x00, 0x20); delay(5); reg(PCA2, 0x00, 0xA0);
  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;
}

// Голова: угол поворота от «прямо», градусы (+ — влево).
void head(float deg) {
  uint16_t us = 500 + (uint16_t)((90 + deg) * (2000.0 / 180.0));
  uint16_t off = (uint32_t)us * 4096UL / 19988UL;
  Wire.beginTransmission(PCA2);
  Wire.write(0x06 + 4 * 2);
  Wire.write(0); Wire.write(0); Wire.write(off & 0xff); Wire.write(off >> 8);
  Wire.endTransmission();
}

enum { WALK, TURN_TO, CHECK } mode = WALK;
unsigned long lastSonar = 0, checkAt = 0;
float stepLen = STEP;               // текущая длина шага (0 — на месте)
int tryNo = 0;                      // 0 — направо, 1 — налево, 2 — назад
int turns = 0;
uint8_t look = 0;                   // фаза осмотра на ходу
long sideL = 500, sideR = 500;      // косые замеры влево и вправо, см
float bias = 0;                     // поправка курса к середине коридора, градусы
const float K_CENTER = 0.1;         // градусов на сантиметр разницы (+ — к дальней стене)
const float TRY_TURN[3] = {-90, 180, 90};   // на сколько повернуть для попытки, градусы
const char *TRY_NAME[3] = {"направо", "налево", "назад"};

long freeCm(long cm) { return cm < 0 ? 500 : cm; }   // нет эха — значит, свободно

// Шаги с памятью (как в walk.ino).
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;
  unsigned long now = millis();
  if (mode == WALK && now - lastSonar > 250 && t > T_START) {
    // Замеры по кругу: прямо, влево 45°, прямо, вправо 45°. Голова встаёт на
    // следующее направление сразу после замера — к следующему уже повернулась.
    lastSonar = now;
    long cm = freeCm(distanceCm());
    uint8_t ph = look % 4;
    if (ph == 0 || ph == 2) {
      if (cm < STOP_CM) {
        Serial.print("стена "); Serial.print(cm); Serial.println(" см");
        stepLen = 0; tryNo = 0; course += TRY_TURN[0]; bias = 0; mode = TURN_TO;
        head(0);
      }
    } else if (ph == 1) sideL = cm;
    else {
      sideR = cm;
      // Держаться середины: по косым замерам до стен слева и справа. Ближе к
      // правой — довернуть влево, и наоборот (если обе стены видны).
      if (sideL < 250 && sideR < 250) bias = constrain(K_CENTER * (sideL - sideR), -10, 10);
      else bias = 0;
    }
    look++;
    if (mode == WALK) head((look % 4) == 1 ? 45 : (look % 4) == 3 ? -45 : 0);
  }
  if (mode == TURN_TO && fabs(course - heading) < 6) { mode = CHECK; checkAt = now; }
  if (mode == CHECK && now - checkAt > 400) {
    long cm = freeCm(distanceCm());
    Serial.print(TRY_NAME[tryNo]); Serial.print(": "); Serial.print(cm);
    Serial.println(cm > FREE_CM ? " см — свободно" : " см — занято");
    if (cm > FREE_CM || tryNo == 2) {
      turns++;
      Serial.print("поворот "); Serial.println(turns);
      mode = WALK; stepLen = STEP; lastSonar = now;
    } else {
      tryNo++;
      course += TRY_TURN[tryNo];
      mode = TURN_TO;
    }
  }

  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 (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; }
      k = kk;
      float stance = (k % 2 == 0) ? zl : zr;
      zFrom = (k % 2 == 0) ? zr : zl;
      zTo = stance + stepLen * min(1.0, k / RAMP);
      adj = 0;
      // Курс: задуманный копится по TURN, ошибку по гироскопу выбираем
      // частью на каждом шаге — робот не уводит в сторону.
      float t1 = TURN * min(1.0, k / RAMP);
      course += t1;
      // На месте курс выбираем быстрее (поворот), на ходу — мягко.
      turnStep = constrain(t1 + (stepLen > 0 ? K_HEAD : 0.6) * (course + bias - 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 && stepLen > 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);
}