#include "GaitEngine.h"

const GaitPattern GAIT_TRIPOD = {
  0.5f,
  { 0.5f, 0.0f, 0.5f, 0.0f, 0.5f, 0.0f }
};

const GaitPattern GAIT_WAVE = {
  5.0f / 6.0f,
  { 0.0f, 1.0f / 6, 2.0f / 6, 3.0f / 6, 4.0f / 6, 5.0f / 6 }
};

void GaitEngine::begin() {
  for (int i = 0; i < 6; i++) {
    legs_pattern_[i] = requested_pattern_;
    leg_phase_[i] = legs_pattern_[i]->leg_phase_offset[i];
  }
}

static float smoothstep(float t) { return t * t * (3.0f - 2.0f * t); }

void GaitEngine::update(float dt, const Twist& twist, FootTarget out[6]) {
  for (int i = 0; i < 6; i++) {
    leg_phase_[i] += dt / cycle_sec_;

    // Touchdown: this leg's cycle wraps back to 0 - the one moment it's safe
    // to hand it a newly requested pattern, since it's grounded with zero
    // relative velocity right here regardless of which two patterns are involved.
    if (leg_phase_[i] >= 1.0f) {
      leg_phase_[i] -= 1.0f;
      legs_pattern_[i] = requested_pattern_;
    }

    const GaitPattern* p = legs_pattern_[i];
    bool in_stance = leg_phase_[i] < p->duty_factor;

    FootTarget neutral = Kinematics::neutralStance(i);
    float stride_time = cycle_sec_ * p->duty_factor;

    // Half-stride displacement from the commanded twist.
    float half_len_x = twist.vx * stride_time * 0.5f;
    float half_len_y = twist.vy * stride_time * 0.5f;
    // Rough yaw contribution using this leg's mount position as the moment arm.
    float yaw_dx = -twist.omega * LEG_MOUNT_Y_MM[i] * stride_time * 0.5f;
    float yaw_dy =  twist.omega * LEG_MOUNT_X_MM[i] * stride_time * 0.5f;

    FootTarget start = { neutral.x - half_len_x - yaw_dx, neutral.y - half_len_y - yaw_dy, neutral.z };
    FootTarget end    = { neutral.x + half_len_x + yaw_dx, neutral.y + half_len_y + yaw_dy, neutral.z };

    FootTarget foot;
    if (in_stance) {
      float t = leg_phase_[i] / p->duty_factor;
      foot.x = start.x + (end.x - start.x) * t;
      foot.y = start.y + (end.y - start.y) * t;
      foot.z = neutral.z;
    } else {
      float t = (leg_phase_[i] - p->duty_factor) / (1.0f - p->duty_factor);
      float st = smoothstep(t);
      foot.x = end.x + (start.x - end.x) * st;
      foot.y = end.y + (start.y - end.y) * st;
      foot.z = neutral.z + SWING_LIFT_MM * sin(PI * t);
    }
    out[i] = foot;
  }
}