#pragma once
#include "Kinematics.h"

class Limits {
public:
  // Clamps a foot target into a conservative safe envelope around the leg's neutral stance.
  static FootTarget clampWorkspace(int leg_index, const FootTarget& target);
  // True if clampWorkspace() would leave the target unchanged.
  static bool inWorkspace(int leg_index, const FootTarget& target);

  // True if all six feet can be placed there at once: each target in its
  // workspace with a valid IK solution, and no two neighbouring legs'
  // segments closer than LEG_CLEARANCE_MM.
  static bool feetFeasible(const FootTarget feet[6]);

  // Smallest distance between neighbouring legs' segments for these feet
  // (diagnostics; a large negative number if some leg has no IK solution).
  static float legClearance(const FootTarget feet[6]);

  // Rate-limits a joint angle change to the servo's real max speed.
  static float clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt);

  // Coarse safety check: are enough legs grounded right now to be stable.
  // A real support-polygon/center-of-mass check is a good later upgrade.
  static bool stancePolygonOk(const bool in_stance[6]);
};