#pragma once
#include <Arduino.h>

struct JointAngles {
  float coxa_deg;
  float femur_deg;
  float tibia_deg;
  bool valid;   // false if the target was unreachable or outside joint limits
};

struct FootTarget {
  float x, y, z;   // body-frame mm, relative to body center
};

class Kinematics {
public:
  // Solves leg i's joint angles for a body-frame foot target.
  static JointAngles solve(int leg_index, const FootTarget& target);

  // The leg's resting/neutral foot target, in body frame, at the current
  // stance height.
  static FootTarget neutralStance(int leg_index);

  // Standing height: how far below the hips the neutral feet sit (starts at
  // STANCE_HEIGHT_MM). Everything built on neutralStance() - gait, sit/stand,
  // workspace limits - follows it.
  static void setStanceHeight(float mm);
  static float stanceHeight();

  // Forward kinematics: the leg's joint positions for the given angles, body
  // frame - [0] coxa pivot (hip), [1] femur pivot, [2] knee, [3] foot.
  static void jointPositions(int leg_index, const JointAngles& a, FootTarget out[4]);
};