// Эталонный скетч: Port (робот-человек) поднимается по лестнице (1.9.72).
// Площадка детали — «Лестница»: четыре ступени по 8 см высотой и 32 см
// глубиной, первая — в 80 см впереди, наверху — площадка.
//
// Как устроено. Места постановки стоп известны заранее: по полу — шаги по
// 12 см, последний — вплотную к первой ступени; по лестнице — приставным
// шагом (одна нога на следующую ступень, вторая к ней), стопа — на середину
// ступени; наверху — снова обычные шаги.
//
// Корпус ведёт расчёт по ZMP (точке нулевого момента) с предуправлением
// (Kajita, 2003): робот — «тележка на столе» высотой ZC, и чтобы точка
// опоры шла по стопам, корпус должен двигаться заранее. Контроллер смотрит
// на 1,6 с вперёд по плану стоп и считает, куда вести центр масс. Коэффициенты
// посчитаны заранее для шага 20 мс (KI, GX, GD ниже).
//
// Центр масс не совпадает с тазом: когда одна нога выше и позади, он
// смещается. Скетч оценивает его по положениям звеньев (масса каждого
// звена — из описания робота) и наклону корпуса и ставит таз так, чтобы
// туда, куда нужно, шёл именно центр масс.
//
// Таз на лестнице опускается на 4 см (нога должна доставать до нижней
// ступени), шагающая стопа поднимается, идёт вперёд и опускается — носок
// не цепляет край. Крен корпуса поправляют голеностопы, бёдра и место
// постановки стопы (как в walk.ino).
//
// Привод: PCA9685 по I2C (0x40), каналы 0–4 левая нога, 5–9 правая
// (крен бедра, бедро, колено, голеностоп вперёд-назад, голеностоп крен),
// 10 и 11 — поворот бёдер (здесь прямо). Датчик наклона MPU6050 (0x68).

#include <Wire.h>
#include <math.h>
#include <avr/pgmspace.h>

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

// --- робот ------------------------------------------------------------------
const float L1 = 0.42, L2 = 0.42;   // бедро, голень, м
const float HIP = 0.06;             // полуширина таза, м
const float ANKLE = 0.08;           // голеностоп над подошвой, м
const float HEEL = 0.09, TOE = 0.17;  // подошва от голеностопа назад и вперёд, м
const float FOOT_C = 0.04;          // середина стопы впереди голеностопа, м

// --- лестница -----------------------------------------------------------------
const float STAIR_Z = 0.80, RISE = 0.08, TREAD = 0.32;
const int   STAIRS = 4;             // последняя — площадка наверху
const float GAP = 0.02;             // носок перед первой ступенью, м

// --- походка ------------------------------------------------------------------
const float H = 0.86, H_STAIR = 0.82;  // высота таза над опорой: по полу и на лестнице
const float STEP = 0.12;            // шаг по ровному, м
const float T_DS = 0.1, T_SS = 0.4;     // по ровному: на двух ногах, на одной, с
const float T_DS_S = 0.2, T_SS_S = 0.5; // на лестнице
const float LIFT = 0.04, LIFT_S = 0.06; // подъём стопы, м
const float UP = 0.4, ZB = 0.1;     // доли переноса: подъём/спуск стопы; до начала хода вперёд
const int   STEPS = 30;             // шагов всего, потом стоять
const float T0 = 1.0;               // постоять перед ходом, с

// Обратная связь по крену (как в walk.ino).
const float K_ANKLE = -0.3, K_HIP = 0.3, K_FOOT = 0.3;

// --- предуправление по ZMP: шаг 20 мс, высота ЦМ 0,78 м, взгляд вперёд 1,6 с --
const float DT = 0.02;
const int   NP = 80;
const float KI = 364.9678, GX[3] = {10789.72, 3297.714, 75.59776};
const float GD[80] PROGMEM = {
  -364.9678, -576.6090, -658.9314, -642.1584, -594.5369, -547.5335, -507.2573, -472.0079,
  -439.8386, -409.8777, -381.8693, -355.7254, -331.3588, -308.6620, -287.5223, -267.8316,
  -249.4896, -232.4037, -216.4878, -201.6618, -187.8512, -174.9864, -163.0027, -151.8396,
  -141.4410, -131.7546, -122.7315, -114.3263, -106.4968, -99.20346, -92.40960, -86.08101,
  -80.18583, -74.69438, -69.57900, -64.81394, -60.37521, -56.24047, -52.38889, -48.80108,
  -45.45897, -42.34575, -39.44574, -36.74432, -34.22792, -31.88384, -29.70030, -27.66630,
  -25.77159, -24.00664, -22.36256, -20.83108, -19.40448, -18.07557, -16.83768, -15.68456,
  -14.61041, -13.60983, -12.67777, -11.80954, -11.00077, -10.24739, -9.545600, -8.891875,
  -8.282920, -7.715668, -7.187264, -6.695048, -6.236541, -5.809434, -5.411578, -5.040968,
  -4.695740, -4.374154, -4.074592, -3.795545, -3.535608, -3.293474, -3.067921, -2.857816
};
const float ZC_G = 0.78 / 9.81;

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

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 вперёд.
// Возвращает угол бедра (рад) — нужен оценке центра масс.
float leg(uint8_t c0, float dx, float fh, float fz, float corrA, float corrH) {
  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);
  return th;
}

// --- лестница -------------------------------------------------------------------
int treadOf(float z) {
  if (z < STAIR_Z) return -1;
  int i = (int)((z - STAIR_Z) / TREAD);
  return i < STAIRS - 1 ? i : STAIRS - 1;
}
float ground(float z) { return (treadOf(z + 0.001) + 1) * RISE; }
float treadAnkle(int i) { return STAIR_Z + i * TREAD + HEEL + (TREAD - HEEL - TOE) / 2; }
const float LIM = STAIR_Z - GAP - TOE;  // ближе к лестнице носок по полу не ставим

float nextFoot(float zs, float len, float zw) {
  int ts = treadOf(zs + 0.001), tw = treadOf(zw + 0.001);
  if (ts >= STAIRS - 1 && tw >= STAIRS - 1) return zs + len;        // наверху
  if (tw < ts || (ts < 0 && zs >= LIM - 0.001 && zw < zs - 0.001)) return zs;  // приставить
  if (ts >= 0 || zs >= LIM - 0.001) return treadAnkle(ts + 1);      // на следующую ступень
  return min(zs + len, LIM);                                         // к лестнице
}

// --- план шагов: кольцо на несколько шагов вперёд ----------------------------------
struct Step { float z0, z1, zs, t0; uint8_t foot, stair; };
const uint8_t RING = 8;
Step ring[RING];
int planned = 0;                    // сколько шагов уже в плане
int first = 0;                      // первый шаг, который ещё не кончился
float planF[2] = {0, 0}, planT = T0;

void planMore() {
  // Два последних законченных шага кольцо держит: по ним видно, где стоят стопы.
  while (planned < STEPS && planned < first + RING - 2) {
    int k = planned;
    uint8_t foot = k % 2 == 0 ? 1 : 0, st = 1 - foot;   // первой шагает правая
    float len = STEP * min(1.0, (k + 1) / 2.0);
    Step &s = ring[k % RING];
    s.foot = foot; s.z0 = planF[foot]; s.zs = planF[st];
    s.z1 = nextFoot(planF[st], len, planF[foot]);
    float y0 = ground(s.z0), y1 = ground(s.z1), ys = ground(s.zs);
    s.stair = y0 != y1 || ys != y1;
    s.t0 = planT;
    planT += s.stair ? T_DS_S + T_SS_S : T_DS + T_SS;
    planF[foot] = s.z1;
    planned++;
  }
}
float tds(const Step &s) { return s.stair ? T_DS_S : T_DS; }
float tss(const Step &s) { return s.stair ? T_SS_S : T_SS; }
float tEnd(const Step &s) { return s.t0 + tds(s) + tss(s); }

// Цель ZMP в момент t: [x, z]. Шаги просматриваются с номера *from (растёт).
void zmpRef(float t, int &from, float &rx, float &rz) {
  while (from < planned && t >= tEnd(ring[from % RING])) from++;
  if (from >= planned) {                       // план кончился — стоять
    const Step &s = ring[(planned - 1) % RING];
    rx = 0; rz = (s.z1 + s.zs) / 2 + FOOT_C; return;
  }
  const Step &s = ring[from % RING];
  float sx = s.foot == 0 ? -HIP : HIP;         // опорная — другая нога
  if (t < s.t0) { rx = 0; rz = FOOT_C; return; }
  if (t < s.t0 + tds(s)) {
    float px = from == 0 ? 0 : -sx, pz = from == 0 ? FOOT_C : s.z0 + FOOT_C;
    float a = (t - s.t0) / tds(s);
    rx = px + (sx - px) * a; rz = pz + (s.zs + FOOT_C - pz) * a; return;
  }
  rx = sx; rz = s.zs + FOOT_C;
}

// Кольцо целей ZMP: refHead — нынешний такт, дальше — следующие.
int16_t refX[NP + 1], refZ[NP + 1];
uint8_t refHead = 0, refTail = 0;
bool refReady = false;
int refFrom = 0;
void pushRef(float t) {
  float rx, rz;
  zmpRef(t, refFrom, rx, rz);
  refX[refTail] = (int16_t)lround(rx * 1000); refZ[refTail] = (int16_t)lround(rz * 1000);
  refTail = (refTail + 1) % (NP + 1);
}

// --- предуправление по одной оси ------------------------------------------------------
struct Axis { float c, v, a, e; };
Axis ax = {0, 0, 0, 0}, az = {FOOT_C, 0, 0, 0};

float previewStep(Axis &q, float p0, float sumGd) {
  float p = q.c - ZC_G * q.a;
  q.e += p - p0;
  float u = -KI * q.e - (GX[0] * q.c + GX[1] * q.v + GX[2] * q.a) - sumGd;
  q.c += DT * q.v + DT * DT / 2 * q.a + DT * DT * DT / 6 * u;
  q.v += DT * q.a + DT * DT / 2 * u;
  q.a += DT * u;
  return q.c;
}

// --- датчик наклона ------------------------------------------------------------------
float roll = 0, pitch = 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(); (void)gz;
  unsigned long now = micros();
  float dt = lastImu ? (now - lastImu) * 1e-6 : 0;
  lastImu = now;
  if (az_ < 4000) return;
  float accRoll = atan2((float)ay_, (float)az_) * R;
  float accPitch = atan2(-(float)ax_, (float)az_) * R;
  // Пока робот приседает и успокаивается — угол по акселерометру быстрее.
  float w = millis() < 900 ? 0.05 : 0.005;
  roll = (1 - w) * (roll + gx / 131.0 * dt) + w * accRoll;
  pitch = (1 - w) * (pitch + gy / 131.0 * dt) + w * accPitch;
}

// --- оценка центра масс: смещение от начала торса, м -----------------------------------
// Массы — из описания робота: торс с головой и руками 22 кг, у каждой ноги
// приводы бедра 2,5, бедро 4,5, голень 2,5, голеностоп 1,5, стопа 3.
float legDx[2], legFh[2] = {0.78, 0.78}, legFz[2], legTh[2];
void comOffset(float &ox, float &oz) {
  float mx = 0, my = 22 * 0.265, mz = 0, M = 22;
  for (uint8_t i = 0; i < 2; i++) {
    float hx = i == 0 ? HIP : -HIP, hy = -0.07;
    float lv = sqrt(legFh[i] * legFh[i] + legDx[i] * legDx[i]);
    float kd = L1 * cos(legTh[i]), kz = L1 * sin(legTh[i]);
    float kx = hx + legDx[i] * kd / lv, ky = hy - legFh[i] * kd / lv;
    float fx = hx + legDx[i], fy = hy - legFh[i], fz = legFz[i];
    mx += 2.5 * hx + 4.5 * (hx + kx) / 2 + 2.5 * (kx + fx) / 2 + 1.5 * fx + 3.0 * fx;
    my += 2.5 * hy + 4.5 * (hy + ky) / 2 + 2.5 * (ky + fy) / 2 + 1.5 * fy + 3.0 * (fy - 0.05);
    mz += 4.5 * kz / 2 + 2.5 * (kz + fz) / 2 + 1.5 * fz + 3.0 * (fz + 0.04);
    M += 14;
  }
  float x = mx / M, y = my / M, z = mz / M;
  ox = x - y * sin(roll / R);
  oz = z * cos(pitch / R) + y * sin(pitch / R);
}

// --- такт управления (20 мс) ------------------------------------------------------------
unsigned long nextMs = 0;
float tc = 0;                       // время по тактам, с
float offX = 0, offZ = 0;           // таз минус центр масс (сглажено)
float hBase = L1 + L2 + ANKLE - 0.002;      // стартует на прямых ногах и плавно приседает
int reported = -1;

void control() {
  planMore();
  while (first < planned && tc >= tEnd(ring[first % RING])) first++;
  planMore();
  // Цели ZMP на 1,6 с вперёд → предуправление. План стоп заранее известен и
  // не меняется, поэтому цели копятся в кольце (в миллиметрах): за такт
  // считается одна новая — на 1,6 с вперёд.
  if (!refReady) {
    for (int j = 0; j <= NP; j++) pushRef(tc + j * DT);
    refReady = true;
  } else pushRef(tc + NP * DT);
  float r0x = refX[refHead] * 0.001, r0z = refZ[refHead] * 0.001, sx = 0, sz = 0;
  for (int j = 0; j < NP; j++) {
    uint8_t i = (refHead + 1 + j) % (NP + 1);
    float g = pgm_read_float(&GD[j]);
    sx += g * refX[i]; sz += g * refZ[i];
  }
  sx *= 0.001; sz *= 0.001;
  refHead = (refHead + 1) % (NP + 1);
  float cx = previewStep(ax, r0x, sx), cz = previewStep(az, r0z, sz);

  // Стопы: где стоят, где шагающая; высота таза.
  float Z[2] = {0, 0}, Y[2];
  for (int i = max(0, first - 2); i < first; i++) Z[ring[i % RING].foot] = ring[i % RING].z1;
  Y[0] = ground(Z[0]); Y[1] = ground(Z[1]);
  int swing = -1;
  float yA = max(Y[0], Y[1]);
  bool nearStair = false;
  if (first < planned) {
    const Step &s = ring[first % RING];
    nearStair = s.stair || (first + 1 < planned && ring[(first + 1) % RING].stair);
    float y0 = ground(s.z0), y1 = ground(s.z1), ys = ground(s.zs);
    Z[s.foot] = s.z0; Z[1 - s.foot] = s.zs; Y[s.foot] = y0; Y[1 - s.foot] = ys;
    if (tc < s.t0) yA = max(y0, ys);
    else if (tc < s.t0 + tds(s)) yA = (y0 + ys) / 2;
    else {
      float v = (tc - s.t0 - tds(s)) / tss(s);
      swing = s.foot;
      float top = max(y0, y1) + (s.stair ? LIFT_S : LIFT);
      Z[s.foot] = s.z0 + (s.z1 - s.z0) * smooth((v - ZB) / (1 - 2 * ZB));
      Y[s.foot] = v < 0.5 ? y0 + (top - y0) * smooth(v / UP) : y1 + (top - y1) * smooth((1 - v) / UP);
      yA = (y0 + ys) / 2 + ((y1 + ys) / 2 - (y0 + ys) / 2) * smooth(v);
    }
    int tr = treadOf(min(s.zs, s.z0) + 0.001);
    if (tr != reported) { reported = tr; if (tr >= 0) { Serial.print("ступень "); Serial.println(tr + 1); } }
  }
  hBase += ((nearStair ? H_STAIR : H) - hBase) * (DT / (tc < T0 ? 0.15 : 0.3));
  float py = hBase + yA;

  // Таз: туда, где центр масс должен быть, плюс смещение таза от центра масс.
  float ox, oz;
  comOffset(ox, oz);
  offX += (-ox - offX) * (DT / 0.5);
  offZ += (-oz - offZ) * (DT / 0.5);
  float pelX = cx + offX, pelZ = cz + offZ;

  float fxs = -K_FOOT * roll / R;
  float cA = K_ANKLE * roll, cH = K_HIP * roll;
  for (uint8_t i = 0; i < 2; i++) {
    legDx[i] = -pelX + (i == swing ? fxs : 0);
    legFh[i] = py - (ANKLE + Y[i]);
    legFz[i] = Z[i] - pelZ;
  }
  legTh[0] = leg(0, legDx[0], legFh[0], legFz[0], cA, cH);
  legTh[1] = leg(5, legDx[1], legFh[1], legFz[1], cA, cH);
  setJoint(10, 5, 0); setJoint(11, 5, 0);
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.setClock(400000);
  reg(PCA, 0x00, 0x10);
  reg(PCA, 0xFE, 121);
  reg(PCA, 0x00, 0x20);
  delay(5);
  reg(PCA, 0x00, 0xA0);
  reg(MPU, 0x6B, 0);
  nextMs = millis();
}

void loop() {
  readImu();
  // Такты строго по 20 мс модельного времени: предуправление рассчитано на этот шаг.
  if ((long)(millis() - nextMs) >= 0) {
    control();
    tc += DT;
    nextMs += 20;
  }
}