#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.
  static FootTarget neutralStance(int leg_index);
};