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

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

bool Limits::inWorkspace(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;
  return dx * dx + dy * dy <= MAX_HORIZ_MM * MAX_HORIZ_MM && fabs(dz) <= MAX_VERT_MM;
}

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;

  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 };
}

// Neighbouring legs: along each side, plus the front and rear pairs.
static const uint8_t NEIGHBOURS[][2] = { {0, 1}, {1, 2}, {3, 4}, {4, 5}, {0, 5}, {2, 3} };

static FootTarget sub(const FootTarget& a, const FootTarget& b) { return { a.x - b.x, a.y - b.y, a.z - b.z }; }
static float dot(const FootTarget& a, const FootTarget& b) { return a.x * b.x + a.y * b.y + a.z * b.z; }

// Shortest distance between segments p1-q1 and p2-q2 (Ericson, Real-Time
// Collision Detection, 5.1.9).
static float segmentDistance(const FootTarget& p1, const FootTarget& q1,
                             const FootTarget& p2, const FootTarget& q2) {
  FootTarget d1 = sub(q1, p1), d2 = sub(q2, p2), r = sub(p1, p2);
  float a = dot(d1, d1), e = dot(d2, d2), f = dot(d2, r);
  float s = 0, t = 0;
  constexpr float EPS = 1e-6f;
  if (a > EPS || e > EPS) {
    if (a <= EPS) {
      t = constrain(f / e, 0.0f, 1.0f);
    } else {
      float c = dot(d1, r);
      if (e <= EPS) {
        s = constrain(-c / a, 0.0f, 1.0f);
      } else {
        float b = dot(d1, d2);
        float denom = a * e - b * b;
        s = denom > EPS ? constrain((b * f - c * e) / denom, 0.0f, 1.0f) : 0.0f;
        t = (b * s + f) / e;
        if (t < 0)      { t = 0; s = constrain(-c / a, 0.0f, 1.0f); }
        else if (t > 1) { t = 1; s = constrain((b - c) / a, 0.0f, 1.0f); }
      }
    }
  }
  FootTarget c1 = { p1.x + d1.x * s, p1.y + d1.y * s, p1.z + d1.z * s };
  FootTarget c2 = { p2.x + d2.x * t, p2.y + d2.y * t, p2.z + d2.z * t };
  FootTarget d = sub(c1, c2);
  return sqrt(dot(d, d));
}

// Joint positions of all legs for these feet; false if any leg can't reach.
static bool solveAll(const FootTarget feet[6], FootTarget joints[6][4], bool check_workspace) {
  for (int i = 0; i < 6; i++) {
    if (check_workspace && !Limits::inWorkspace(i, feet[i])) return false;
    JointAngles a = Kinematics::solve(i, feet[i]);
    if (!a.valid) return false;
    Kinematics::jointPositions(i, a, joints[i]);
  }
  return true;
}

static float minClearance(const FootTarget joints[6][4]) {
  float best = 1e9f;
  for (const auto& n : NEIGHBOURS)
    for (int s1 = 0; s1 < 3; s1++)        // segments: coxa, femur, tibia
      for (int s2 = 0; s2 < 3; s2++)
        best = min(best, segmentDistance(joints[n[0]][s1], joints[n[0]][s1 + 1],
                                         joints[n[1]][s2], joints[n[1]][s2 + 1]));
  return best;
}

bool Limits::feetFeasible(const FootTarget feet[6]) {
  FootTarget joints[6][4];
  return solveAll(feet, joints, true) && minClearance(joints) >= LEG_CLEARANCE_MM;
}

float Limits::legClearance(const FootTarget feet[6]) {
  FootTarget joints[6][4];
  return solveAll(feet, joints, false) ? minClearance(joints) : -1e9f;
}

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;
}