// MuTech Robotics • DOG-12 robot dog (concept build) — ESP32 + 16-channel servo driver
// Moves: walk, turn, side step, sit, give paw, play bow, dance, pounce hop, lie down.
// NOT yet tested on hardware: calibrate every servo first (see CAL[] and DIR[]), test with the dog on a stand.
//
// Wiring (see video / description):
//   Driver VCC→3V3, GND→GND, SDA→D21, SCL→D22. Tilt sensor + I2C display on the same bus.
//   Ultrasonic TRIG→D27, ECHO→1k→D26, 2k from D26 to GND (5 V echo → 3.3 V).
//   Servos: signal → driver CH0..CH13, + → 6 V servo bus, − → GND bus. All GNDs common.
//   Channels: FL side/hip/knee 0/1/2 • FR 3/4/5 • RL 6/7/8 • RR 9/10/11 • head 12 • tail 13
//
// Libraries: "Adafruit PWM Servo Driver Library" (Library Manager).
// Serial commands (115200): w walk, t turn, x side step, s sit, p paw, b bow, d dance, h hop, l lie, u stand

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

Adafruit_PWMServoDriver pwm(0x40);

// ---------- geometry (mm) — matches the printed parts ----------
const float L1 = 110, L2 = 120, OFF = 53;      // upper leg, lower leg, hip side offset
const float STAND_H = 170;                      // hip height when standing

// ---------- servo calibration ----------
// CAL: degrees added so that 90° = neutral stand pose. DIR: +1 / -1 to flip a servo.
int   CAL[14] = {0,0,0, 0,0,0, 0,0,0, 0,0,0, 0,0};
int   DIR[14] = {1,1,1, -1,-1,-1, 1,1,1, -1,-1,-1, 1,1};
const int US_MIN = 500, US_MAX = 2500;          // servo pulse range (check your servo's datasheet)
const float NEU_HIP = -0.84, NEU_KNEE = 1.62;    // joint angles (rad) of the neutral stand

const int TRIG = 27, ECHO = 26;

void writeServo(int ch, float deg) {
  deg = constrain(deg + CAL[ch], 0, 180);
  int us = US_MIN + (US_MAX - US_MIN) * deg / 180.0;
  pwm.writeMicroseconds(ch, us);
}

// joint angles (rad) → servo degrees for one leg
void setLeg(int leg, float roll, float th1, float bend) {
  int c = leg * 3;
  writeServo(c + 0, 90 + DIR[c + 0] * roll * RAD_TO_DEG);
  writeServo(c + 1, 90 + DIR[c + 1] * (th1 - NEU_HIP) * RAD_TO_DEG);
  writeServo(c + 2, 90 + DIR[c + 2] * (bend - NEU_KNEE) * RAD_TO_DEG);
}

// inverse kinematics: foot target relative to the hip (x forward, y down, z outward), mm
bool legIK(int leg, float x, float y, float z) {
  float h2 = y * y + z * z - OFF * OFF;
  if (h2 < 1) return false;
  float h = sqrtf(h2);
  float roll = atan2f(z, y) - atan2f(OFF, h);
  float D = sqrtf(x * x + h * h);
  D = constrain(D, fabsf(L1 - L2) + 5, L1 + L2 - 1);
  float a1 = acosf(constrain((L1 * L1 + D * D - L2 * L2) / (2 * L1 * D), -1, 1));
  float th1 = atan2f(x, h) - a1;                                   // knee points backward
  float bend = PI - acosf(constrain((L1 * L1 + L2 * L2 - D * D) / (2 * L1 * L2), -1, 1));
  setLeg(leg, roll, th1, bend);
  return true;
}

// feet relative to each hip; body height/pitch handled by changing y per leg
const float FX[4] = {20, 20, 12, 12};   // neutral foot x offset (front legs slightly forward)
void pose(float hF, float hR, float dxF = 0, float dxR = 0, float dz = 0) {
  legIK(0, FX[0] + dxF, hF, OFF + dz); legIK(1, FX[1] + dxF, hF, OFF - dz);
  legIK(2, FX[2] + dxR, hR, OFF + dz); legIK(3, FX[3] + dxR, hR, OFF - dz);
}
void head(float deg) { writeServo(12, 90 + deg); }
void tail(float deg) { writeServo(13, 90 + deg); }

void stand(int ms = 600) {
  for (int i = 0; i <= 20; i++) { pose(STAND_H, STAND_H); delay(ms / 20); }
}

// ---------- trot gait ----------
// vx forward step (mm), vz side step (mm), turn (mm, +ve = turn left), cycles
void trot(float stepX, float stepZ, float turn, int cycles) {
  const int N = 24; const float lift = 34;
  for (int c = 0; c < cycles; c++) {
    for (int i = 0; i < N; i++) {
      float ph = (float)i / N;
      for (int leg = 0; leg < 4; leg++) {
        bool pairA = (leg == 0 || leg == 3);              // FL+RR / FR+RL
        float p = fmodf(ph + (pairA ? 0 : 0.5), 1.0);
        float u, yl;
        if (p < 0.5) { u = 0.5 - 2 * p; yl = 0; }           // stance: foot moves back
        else { u = -0.5 + 2 * (p - 0.5); yl = lift * sinf(PI * (p - 0.5) * 2); } // swing
        float side = (leg % 2 == 0) ? 1 : -1;
        float x = FX[leg] + u * stepX + u * turn * side;
        float z = OFF + u * stepZ * side;
        legIK(leg, x, STAND_H - yl, z);
      }
      tail(12 * sinf(2 * PI * ph));
      if (stepX > 0 && obstacle()) { stand(); return; }  // stop for obstacles
      delay(800 / N);                                     // 0.8 s per cycle
    }
  }
  stand();
}

float distanceCm() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10); digitalWrite(TRIG, LOW);
  long us = pulseIn(ECHO, HIGH, 25000);
  return us ? us / 58.0 : 999;
}
bool obstacle() { static uint32_t t = 0; static bool o = false; if (millis() - t > 120) { t = millis(); o = distanceCm() < 20; } return o; }

void ease(float a0F, float a0R, float a1F, float a1R, float dxF0, float dxR0, float dxF1, float dxR1, int ms) {
  for (int i = 0; i <= 30; i++) { float k = i / 30.0; k = k * k * (3 - 2 * k);
    pose(a0F + (a1F - a0F) * k, a0R + (a1R - a0R) * k, dxF0 + (dxF1 - dxF0) * k, dxR0 + (dxR1 - dxR0) * k); delay(ms / 30); }
}
void sit()  { ease(STAND_H, STAND_H, 160, 75, 0, 0, 0, 30, 1000); for (int i = 0; i < 20; i++) { tail(35 * sinf(i * 0.9)); delay(40); } }
void paw()  {
  sit();
  for (int k = 0; k < 3; k++) for (int i = 0; i <= 20; i++) {         // FR (leg 1) shakes
    float y = 160 - 70 - 20 * sinf(i * PI / 10); legIK(1, FX[1] + 60, y, OFF); delay(25); }
  sit();
}
void bow()  { ease(STAND_H, STAND_H, 95, STAND_H, 0, 0, 70, 0, 900); for (int i = 0; i < 30; i++) { tail(40 * sinf(i)); delay(40); } stand(800); }
void lie()  { ease(STAND_H, STAND_H, 70, 70, 0, 0, -35, -40, 1400); head(-10); }
void hop()  {
  ease(STAND_H, STAND_H, 115, 115, 0, 0, 0, 0, 700); delay(300);
  pose(214, 214); delay(250);               // all 12 leg servos push at once (small hop)
  pose(140, 140); delay(250); stand(700);
}
void dance() {
  for (int b = 0; b < 16; b++) {            // 100 bpm = 0.6 s per beat
    for (int i = 0; i < 12; i++) {
      float k = (b + i / 12.0), s = sinf(PI * k);
      float dz = 26 * s, bob = 14 * fabsf(sinf(PI * k * 2));
      pose(STAND_H - bob, STAND_H - bob, 0, 0, dz);
      if (b >= 4 && b < 12) legIK(b % 2, FX[b % 2] + 20, STAND_H - bob - 42 * fabsf(sinf(PI * i / 12.0)), OFF);
      head(17 * sinf(PI * k + 1.57)); tail(45 * sinf(2 * PI * k));
      delay(50);
    }
  }
  stand();
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  Wire.begin(21, 22);
  pwm.begin(); pwm.setOscillatorFrequency(25000000); pwm.setPWMFreq(50);
  delay(200);
  stand(1200); head(0); tail(0);
  Serial.println("DOG-12 ready: w t x s p b d h l u");
}

void loop() {
  if (!Serial.available()) return;
  char c = Serial.read();
  switch (c) {
    case 'w': trot(55, 0, 0, 10); break;
    case 't': trot(0, 0, 30, 6); break;
    case 'x': trot(0, 28, 0, 6); break;
    case 's': sit(); break;
    case 'p': paw(); break;
    case 'b': bow(); break;
    case 'd': dance(); break;
    case 'h': hop(); break;
    case 'l': lie(); break;
    case 'u': stand(); head(0); break;
  }
}