// Attribution: https://wokwi.com/projects/439554170007153665

#include <Servo.h>

// ---------------- Pins
#define FL_PIN 11  // Front-Left
#define FR_PIN 10  // Front-Right
#define BL_PIN 9   // Back-Left
#define BR_PIN 6   // Back-Right

// ---------------- Stride + geometry
const int   HIP_CENTER_L = 90;   // neutral for left side
const int   HIP_CENTER_R = 90;   // neutral for right side (trim if needed)
const int   HIP_SWING     = 38;  // stride half-width (bigger = wider steps)  << wider
const float BASE_FREQ_HZ  = 0.9; // cycles/sec (0.9 = 1.1s per full step)
const float STANCE_FRAC   = 0.62;// % of cycle spent pushing (rest is swing)

// Mirror hips on the right side if servos are mounted opposite
const bool MIRROR_RIGHT   = true;

// ---------------- Phase layout (TROT)
const float PHASE_FL = 0.00;  // FL with BR
const float PHASE_FR = 0.50;  // FR with BL (180° apart)
const float PHASE_BL = 0.50;
const float PHASE_BR = 0.00;

// ---------------- Helpers
struct Leg {
  Servo s;
  int   pin;
  bool  rightSide;
  float phase;      // 0..1
  float phaseOff;   // 0..1
};

Leg FL{Servo(), FL_PIN, false, 0.0f, PHASE_FL};
Leg FR{Servo(), FR_PIN, true,  0.0f, PHASE_FR};
Leg BL{Servo(), BL_PIN, false, 0.0f, PHASE_BL};
Leg BR{Servo(), BR_PIN, true,  0.0f, PHASE_BR};

unsigned long tPrev;

// smooth step (ease-in-out)
static inline float smooth(float x) {
  // clamp
  if (x < 0) x = 0; else if (x > 1) x = 1;
  return x * x * (3 - 2 * x);
}

// March-like hip profile:
// - STANCE: slow linear sweep from +SWING to -SWING (push)
// - SWING:  fast, eased return from -SWING to +SWING (recovery)
int hipAngleForPhase(float p, bool rightSide) {
  const int center = rightSide ? HIP_CENTER_R : HIP_CENTER_L;
  float a;

  if (p < STANCE_FRAC) {
    float t = p / STANCE_FRAC;                  // 0..1
    a = +HIP_SWING + (-2.0f * HIP_SWING) * t;   // linear, steady push
  } else {
    float t = (p - STANCE_FRAC) / (1.0f - STANCE_FRAC); // 0..1
    t = smooth(t);                               // snappy but smooth return
    a = -HIP_SWING + (2.0f * HIP_SWING) * t;    // swing forward
  }

  if (rightSide && MIRROR_RIGHT) a = -a;        // mirror if hardware flipped

  int deg = (int)round(center + a);
  if (deg < 0)   deg = 0;
  if (deg > 180) deg = 180;
  return deg;
}

void attachLeg(Leg& L) {
  L.s.attach(L.pin);
  L.phase = L.phaseOff;
  L.s.write( hipAngleForPhase(L.phase, L.rightSide) );
}

void advanceLeg(Leg& L, float dt) {
  L.phase += BASE_FREQ_HZ * dt;
  L.phase -= floor(L.phase);   // wrap 0..1
  L.s.write( hipAngleForPhase(L.phase, L.rightSide) );
}

void setup() {
  attachLeg(FL);
  attachLeg(FR);
  attachLeg(BL);
  attachLeg(BR);
  tPrev = millis();
}

void loop() {
  unsigned long tNow = millis();
  float dt = (tNow - tPrev) / 1000.0f;
  tPrev = tNow;

  advanceLeg(FL, dt);
  advanceLeg(FR, dt);
  advanceLeg(BL, dt);
  advanceLeg(BR, dt);

  delay(15); // smooth updates
}