#include "Limits.h"
#include <Arduino.h>
#include <math.h>

FootTarget Limits::clampWorkspace(int i, const FootTarget& target) {
  FootTarget neutral = Kinematics::neutralStance(i);
  float dx = target.x - neutral.x;
  float dy = target.y - neutral.y;
  float dz = target.z - neutral.z;

  constexpr float MAX_HORIZ_MM = 70.0f;   // tune to your real leg reach, with margin
  constexpr float MAX_VERT_MM  = 60.0f;

  float horiz = sqrt(dx * dx + dy * dy);
  if (horiz > MAX_HORIZ_MM) {
    float scale = MAX_HORIZ_MM / horiz;
    dx *= scale;
    dy *= scale;
  }
  dz = constrain(dz, -MAX_VERT_MM, MAX_VERT_MM);

  return { neutral.x + dx, neutral.y + dy, neutral.z + dz };
}

float Limits::clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt) {
  float max_step = max_deg_per_sec * dt;
  float delta = constrain(target_deg - current_deg, -max_step, max_step);
  return current_deg + delta;
}

bool Limits::stancePolygonOk(const bool in_stance[6]) {
  int count = 0;
  for (int i = 0; i < 6; i++) if (in_stance[i]) count++;
  return count >= 3;
}