//! Joints between occurrences (`solidcraft_doc::joints`): Joint Origin, Joint, As-built Joint,
//! Rigid Group, Drive Joints, Motion Link, limits, and Interference. Every change re-solves the
//! joint graph and moves the occurrences it places; the session re-solves after any edit too,
//! so joints follow the geometry their origins snap to.

use std::collections::{BTreeMap, BTreeSet};

use serde_json::{Value, json};
use solidcraft_doc::expr::Kind;
use solidcraft_doc::joints::{self, Coupling, Joint, JointKind, JointOrigin, MotionLink, NamedOrigin, Size, Snap};
use solidcraft_geom::{Aabb3, Vec3};

use super::CommandSpec;
use crate::params::{bad, bool_, num, str_, string_list, vec3};
use crate::{EngineError, Result, Session};

pub static COMMANDS: &[CommandSpec] = &[
    CommandSpec::new("joint.origin", "Joint Origin", joint_origin).at("SOLID", "CREATE").icon("joint_origin").params(
        "name?, occurrence?: id|name (default: from the pick), face: [x,y,z] (centre of a planar face) | circle: [x,y,z] (centre of a round end) | point: [x,y,z], z?: [x,y,z]",
    ),
    CommandSpec::new("joint.create", "Joint", joint).at("SOLID", "ASSEMBLE").icon("joint").key("J").params(
        "type: rigid|revolute|slider|cylindrical|pin_slot|planar|ball; a, b: joint origins ({occurrence, face|circle|point: [x,y,z] in world} | origin name); \
         values?: [angles in deg or expressions, distances]; limits?: [[min, max] | null…]; flip?: bool (default false: the origins are mated, \
         b's z against a's as two faces meet; true: b's z along a's, so two origins given at one point with the same z stay as they are); \
         offset?; angle?; name?; quiet?: bool (leave out dof and fixed)",
    ),
    CommandSpec::new("joint.as_built", "As-Built Joint", as_built).at("SOLID", "ASSEMBLE").icon("as_built").params(
        "type, a: occurrence, b: occurrence (stay where they are); at?: joint origin (default: b's origin) — the joint's frame; \
         name?; limits?: [[min, max] | null…]; quiet?: bool",
    ),
    CommandSpec::new("joint.rigid_group", "Rigid Group", rigid_group).at("SOLID", "ASSEMBLE").icon("rigid_group").params("occurrences: [ids or names] (move together, as they are)"),
    CommandSpec::new("joint.drive", "Drive Joints", drive).at("SOLID", "ASSEMBLE").icon("drive").params(
        "joint: id|name; value | values: [angles in deg or expressions, distances] (values not given keep theirs; clamped to the limits; the other joints of a \
         closed loop follow, and the motion stops where the loop can no longer close → stopped_by_loop); quiet?: bool (leave out dof \
         and fixed) → values: [{name, value, unit: deg|mm, limits}] as joint.list gives them; moved: whether any occurrence moved; \
         reason: why the joint can move nothing (suppressed, its origins lost, or its chain joined to nothing grounded → unconnected: \
         the occurrence named)",
    ),
    CommandSpec::new("joint.motion_link", "Motion Link", motion_link)
        .at("SOLID", "ASSEMBLE")
        .icon("motion_link")
        .params("a, b: joints; ratio (b = ratio · a + offset): b's units per a's (mm per deg for an angle driving a distance); offset?: in b's units (deg for an angle, mm for a distance, or an expression); ia?, ib?: value indices (default 0)"),
    CommandSpec::new("joint.gear", "Gear", gear).at("SOLID", "ASSEMBLE").icon("gear").params(
        "a, b: joints that turn (revolute), b driven by a; ra, rb: the gears' pitch radii (or tooth counts): mm, an expression, or \
         {circle: [x,y,z], occurrence?}: the round face there, its radius (read again whenever the design changes, so the ratio follows \
         edits); internal?: bool (one is a ring gear: both turn the same way; by default b turns the other way); reverse?: bool; \
         link?: index (replace that motion link) → link, ratio (b's degrees per a's); b keeps where it is",
    ),
    CommandSpec::new("joint.rack_pinion", "Rack and Pinion", rack_pinion).at("SOLID", "ASSEMBLE").icon("rack_pinion").params(
        "a, b: a joint that turns (revolute: the pinion) and one that slides (slider: the rack), in either order, b driven by a; \
         radius: the pinion's pitch radius (mm, an expression, or {circle: [x,y,z], occurrence?}): the rack travels radius × the turn in \
         radians; reverse?: bool; link?: index (replace that motion link) → link, ratio (mm per degree, or degrees per mm); b keeps \
         where it is",
    ),
    CommandSpec::new("joint.screw", "Screw", screw).at("SOLID", "ASSEMBLE").icon("screw").params(
        "joint: a cylindrical joint (its turn drives its travel) | a, b: a joint that turns and one that slides, b driven by a; pitch: \
         travel per turn (mm or an expression); reverse?: bool (a left-hand thread); link?: index (replace that motion link) → link, \
         ratio (mm per degree, or degrees per mm); b keeps where it is",
    ),
    CommandSpec::new("joint.limits", "Joint Limits", limits).params("joint; index?: value index (default 0); min?, max? (omit both to clear)"),
    CommandSpec::new("joint.edit", "Edit Joint", edit).params("joint; type?, flip?, offset?, angle?, suppressed?, name?"),
    CommandSpec::new("joint.delete", "Delete Joint", delete).params("joint: id|name (or origin: name, link: index)"),
    CommandSpec::new("joint.list", "List Joints", list).noundo().params(
        "→ joints (type, occurrences, values, limits), origins, links, degrees of freedom, conflicts, floating: [{occurrences, joints}] \
         (joint chains joined to nothing grounded: the solver places none of them, so driving them moves nothing)",
    ),
    CommandSpec::new("joint.solve", "Solve Joints", solve_cmd).params("re-place the occurrences from the joints → conflicts, errors, dof"),
    CommandSpec::new("inspect.interference", "Interference", interference).at("SOLID", "INSPECT").icon("interference").noundo().params(
        "bodies?: [names] (default: all, in world placement) → interferences: pairs that overlap with the overlap volume, area, bounding box and centre; unchecked: pairs that could not be checked, with the reason. Bodies are pruned by bounding box first, so any number of bodies is fine; at most 20000 pairs whose boxes overlap are intersected per call (truncated: true and skipped_pairs: n say when more were left out)",
    ),
];

/// Re-solve the joints and move the occurrences they place (after any edit). Gears, racks and
/// screws first take their ratios from the radii and pitches they are defined by.
pub fn resolve(s: &mut Session) -> joints::Solution {
    recouple(s);
    resolve_holding(s, &BTreeMap::new())
}

/// Work out the ratios of the coupled motion links again (a radius or a parameter may have
/// changed). A coupling that can't be worked out (its face gone) keeps its last ratio;
/// `joint.list` says why.
fn recouple(s: &mut Session) {
    if s.doc.assembly.links.iter().all(|l| l.coupling.is_none()) {
        return;
    }
    let st = s.model.state();
    let changed: Vec<(usize, f64)> = s
        .doc
        .assembly
        .links
        .iter()
        .enumerate()
        .filter_map(|(i, l)| joints::coupling_ratio(&s.doc, &st, l)?.ok().filter(|r| *r != l.ratio).map(|r| (i, r)))
        .collect();
    if changed.is_empty() {
        return;
    }
    let doc = s.doc_mut();
    for (i, r) in changed {
        if let Some(l) = doc.assembly.links.get_mut(i) {
            l.ratio = r;
        }
    }
}

/// Re-solve the joints, keeping the `held` values (the ones being driven, mapped to the values
/// they move from) where loops are closed.
pub fn resolve_holding(s: &mut Session, held: &joints::Held) -> joints::Solution {
    let st = s.model.state();
    let sol = joints::solve_holding(&s.doc, &st, held);
    if !sol.world.is_empty() {
        let mut next = (*s.doc).clone();
        if next.apply_joint_solution(&sol) {
            *s.doc_mut() = next;
            s.revision += 1;
        }
    }
    sol
}

/// A solve's answer; `quiet: true` in the command's params leaves out the per-occurrence lists.
fn sol_json(sol: &joints::Solution, p: &Value) -> Value {
    let mut out = json!({
        "conflicts": sol.conflicts.iter().map(|(id, n, w)| json!({"joint": id, "name": n, "why": w})).collect::<Vec<_>>(),
        "errors": sol.errors.iter().map(|(id, n, w)| json!({"joint": id, "name": n, "why": w})).collect::<Vec<_>>(),
    });
    if !bool_(p, "quiet").unwrap_or(false) {
        out["dof"] = sol.dof.iter().map(|(o, d)| json!({"occurrence": o, "dof": d})).collect();
        out["fixed"] = json!(sol.fixed);
    }
    out
}

/// A joint's values as `joint.list` gives them: degrees for angles, mm for distances, each with
/// its name and limits.
pub(crate) fn named_values(j: &Joint, q: &[f64]) -> Vec<Value> {
    j.kind
        .dofs()
        .iter()
        .enumerate()
        .map(|(i, n)| {
            let (v, angle) = (q.get(i).copied().unwrap_or(0.0), j.kind.is_angle(i));
            json!({"name": n, "value": if angle { v.to_degrees() } else { v }, "unit": if angle { "deg" } else { "mm" },
                "limits": j.limits.get(i).copied().flatten().map(|(a, b)| if angle { [a.to_degrees(), b.to_degrees()] } else { [a, b] })})
        })
        .collect()
}

/// A joint's values as the solver places it (after motion links, so a screw's travel follows its
/// turn), named as `joint.list` gives them.
pub(crate) fn effective_values(s: &Session, id: u64) -> Option<Vec<Value>> {
    let j = s.doc.assembly.joints.iter().find(|j| j.id == id)?;
    let vals = joints::joint_values(&s.doc.assembly);
    Some(named_values(j, vals.get(&id).map(Vec::as_slice).unwrap_or(&j.values)))
}

fn occurrence_name(s: &Session, id: u64) -> String {
    s.doc.occurrences.iter().find(|o| o.id == id).map(|o| o.name.clone()).unwrap_or_else(|| id.to_string())
}

/// Why driving joint `id` moves nothing whatever its values (`None`: it places what it joins),
/// with the occurrence that is joined to nothing grounded when that is the reason.
pub(crate) fn cannot_move(s: &Session, sol: &joints::Solution, id: u64) -> Option<(String, Option<String>)> {
    let j = s.doc.assembly.joints.iter().find(|j| j.id == id)?;
    // Its value carries through motion links to joints that may move (a suppressed dial too).
    if s.doc.assembly.links.iter().any(|l| l.a == id || l.b == id) {
        return None;
    }
    if j.suppressed {
        return Some((format!("{} is suppressed", j.name), None));
    }
    if let Some((_, _, why)) = sol.errors.iter().find(|e| e.0 == id) {
        return Some((why.clone(), None));
    }
    // The solver places what it reaches from the root and grounded occurrences.
    let placed = |o: u64| o == 0 || sol.world.contains_key(&o);
    if placed(j.a.occurrence) || placed(j.b.occurrence) {
        return None;
    }
    let name = occurrence_name(s, j.a.occurrence);
    Some((format!("{name} is not connected to anything grounded (ground it, or join it to a grounded part)"), Some(name)))
}

/// Joint chains joined to nothing grounded, which the solver places none of: each chain's
/// occurrences and joints, by name.
fn floating_chains(s: &Session, sol: &joints::Solution) -> Vec<Value> {
    let placed = |o: u64| o == 0 || sol.world.contains_key(&o);
    let errors: BTreeSet<u64> = sol.errors.iter().map(|e| e.0).collect();
    let mut next: BTreeMap<u64, Vec<(u64, &Joint)>> = BTreeMap::new();
    for j in s.doc.assembly.joints.iter().filter(|j| !j.suppressed && !errors.contains(&j.id)) {
        if !placed(j.a.occurrence) && !placed(j.b.occurrence) {
            next.entry(j.a.occurrence).or_default().push((j.b.occurrence, j));
            next.entry(j.b.occurrence).or_default().push((j.a.occurrence, j));
        }
    }
    let mut seen = BTreeSet::new();
    let mut out = Vec::new();
    for &start in next.keys() {
        if !seen.insert(start) {
            continue;
        }
        let (mut occs, mut js, mut todo) = (vec![start], BTreeSet::new(), vec![start]);
        while let Some(o) = todo.pop() {
            for (other, j) in next.get(&o).into_iter().flatten() {
                js.insert((j.id, j.name.as_str()));
                if seen.insert(*other) {
                    occs.push(*other);
                    todo.push(*other);
                }
            }
        }
        out.push(json!({"occurrences": occs.iter().map(|o| occurrence_name(s, *o)).collect::<Vec<_>>(),
            "joints": js.iter().map(|(_, n)| *n).collect::<Vec<_>>()}));
    }
    out
}

pub(crate) fn occurrence_id(s: &Session, v: Option<&Value>, cmd: &str) -> Result<u64> {
    let key = match v {
        Some(Value::Number(n)) => n.to_string(),
        Some(Value::String(x)) => x.clone(),
        _ => return Err(bad(cmd, "an occurrence must be an id or a name")),
    };
    if key == "0" || key.eq_ignore_ascii_case("root") {
        return Ok(0);
    }
    if let Some(o) = s.doc.occurrences.iter().find(|o| o.id.to_string() == key || o.name == key) {
        return Ok(o.id);
    }
    // A component name: its first occurrence.
    s.doc.find_component(&key).and_then(|c| s.doc.occurrence_of(c)).map(|o| o.id).ok_or_else(|| bad(cmd, format!("no occurrence `{key}`")))
}

/// The occurrence whose bodies are nearest a world point (0 = root bodies).
pub(crate) fn occurrence_at(s: &Session, p: Vec3) -> Option<u64> {
    let st = s.model.state();
    let mut best: Option<(f64, u64)> = None;
    for b in &st.bodies {
        let comp = s.doc.model_body_component(b);
        let occs: Vec<u64> = if comp == 0 { vec![0] } else { s.doc.occurrences.iter().filter(|o| o.component == comp).map(|o| o.id).collect() };
        for occ in occs {
            let t = if occ == 0 { solidcraft_doc::IDENTITY } else { joints::occurrence_world(&s.doc, occ) };
            let inv = solidcraft_doc::mat_inverse(&t)?;
            let q = solidcraft_doc::apply_point(&inv, p);
            let bb = b.mesh().bounds();
            let c = Vec3::new(q.x.clamp(bb.min.x, bb.max.x), q.y.clamp(bb.min.y, bb.max.y), q.z.clamp(bb.min.z, bb.max.z));
            let d = c.dist(q);
            if best.is_none_or(|(bd, _)| d < bd) {
                best = Some((d, occ));
            }
        }
    }
    best.map(|(_, o)| o)
}

/// A joint origin from parameters: a named origin, or a snap given in world coordinates.
fn origin_param(s: &Session, v: Option<&Value>, cmd: &str) -> Result<JointOrigin> {
    let v = v.ok_or_else(|| bad(cmd, "joint origins `a` and `b` are required"))?;
    if let Value::String(name) = v {
        return s
            .doc
            .assembly
            .origins
            .iter()
            .find(|o| &o.name == name)
            .map(|o| o.origin.clone())
            .ok_or_else(|| bad(cmd, format!("no joint origin `{name}`")));
    }
    let pick = ["face", "circle", "point"].iter().find_map(|k| v.get(*k).and_then(vec3).map(|p| (*k, p)));
    let (what, world) = match (pick, v.get("frame")) {
        (Some(x), _) => x,
        (None, Some(f)) => ("frame", f.get("origin").and_then(vec3).ok_or_else(|| bad(cmd, "`frame` needs origin and z"))?),
        _ => return Err(bad(cmd, "a joint origin needs face, circle, point or frame [x, y, z]")),
    };
    let occurrence = match v.get("occurrence") {
        Some(o) => occurrence_id(s, Some(o), cmd)?,
        None => occurrence_at(s, world).ok_or_else(|| bad(cmd, "no body there"))?,
    };
    // World → the component's own coordinates.
    let t = if occurrence == 0 { solidcraft_doc::IDENTITY } else { joints::occurrence_world(&s.doc, occurrence) };
    let inv = solidcraft_doc::mat_inverse(&t).ok_or_else(|| bad(cmd, "occurrence transform"))?;
    let local = |p: Vec3| solidcraft_doc::apply_point(&inv, p);
    let local_dir = |d: Vec3| solidcraft_doc::apply_vector(&inv, d);
    let snap = match what {
        "face" | "circle" => {
            let pick = local(world);
            // A circle's name is its cylindrical wall's (a rim pick also touches the end face).
            let named_at = if what == "circle" {
                s.model.state().bodies.iter().filter_map(|b| cylinder_wall_point(&b.body, pick)).next().unwrap_or(pick)
            } else {
                pick
            };
            let face = solidcraft_doc::naming::face_names_at(&s.model.state(), &[named_at]).into_iter().next().filter(|n| !n.is_empty());
            if what == "face" { Snap::FaceCenter { pick, face } } else { Snap::CircleCenter { pick, face } }
        }
        "point" => Snap::Point { point: local(world), z: v.get("z").and_then(vec3).map(local_dir) },
        _ => {
            let f = v.get("frame").cloned().unwrap_or_default();
            let z = f.get("z").and_then(vec3).ok_or_else(|| bad(cmd, "`frame` needs a z axis"))?;
            Snap::Frame { origin: local(world), z: local_dir(z), x: f.get("x").and_then(vec3).map(local_dir) }
        }
    };
    let o = JointOrigin { occurrence, snap };
    joints::resolve_origin(&s.doc, &s.model.state(), &o).map_err(|e| bad(cmd, e.to_string()))?;
    Ok(o)
}

pub(crate) fn joint_id(s: &Session, v: Option<&Value>, cmd: &str) -> Result<u64> {
    let key = match v {
        Some(Value::Number(n)) => n.to_string(),
        Some(Value::String(x)) => x.clone(),
        _ => return Err(bad(cmd, "`joint` must be an id or a name")),
    };
    s.doc.assembly.joints.iter().find(|j| j.id.to_string() == key || j.name == key).map(|j| j.id).ok_or_else(|| bad(cmd, format!("no joint `{key}`")))
}

/// A joint value: numbers are degrees for angles and mm for distances; text is an expression.
pub(crate) fn value(s: &Session, v: &Value, angle: bool, cmd: &str) -> Result<f64> {
    let kind = if angle { Kind::Angle } else { Kind::Length };
    let e = match v {
        Value::Number(n) => n.as_f64().filter(|x| x.is_finite() && x.abs() < 1e9).map(|x| x.to_string()),
        Value::String(t) => Some(t.clone()),
        _ => None,
    }
    .ok_or_else(|| bad(cmd, "values must be numbers or expressions"))?;
    s.doc.eval(&e, kind).map_err(|e| bad(cmd, e.to_string()))
}

fn values_param(s: &Session, p: &Value, kind: JointKind, cmd: &str) -> Result<Vec<f64>> {
    let n = kind.dofs().len();
    let list: Vec<Value> = match (p.get("values"), p.get("value")) {
        (Some(Value::Array(a)), _) => a.clone(),
        (_, Some(v)) => vec![v.clone()],
        _ => Vec::new(),
    };
    if list.len() > n {
        return Err(bad(cmd, format!("a {kind:?} joint has {n} values ({})", kind.dofs().join(", "))));
    }
    let mut out = vec![0.0; n];
    for (i, v) in list.iter().enumerate() {
        if let Some(slot) = out.get_mut(i) {
            *slot = value(s, v, kind.is_angle(i), cmd)?;
        }
    }
    Ok(out)
}

fn limits_param(s: &Session, p: &Value, kind: JointKind, cmd: &str) -> Result<Vec<Option<(f64, f64)>>> {
    let n = kind.dofs().len();
    let mut out = vec![None; n];
    if let Some(Value::Array(a)) = p.get("limits") {
        if a.len() > n {
            return Err(bad(cmd, "more limits than values"));
        }
        for (i, l) in a.iter().enumerate() {
            if let (Some(lo), Some(hi)) = (l.get(0), l.get(1))
                && let Some(slot) = out.get_mut(i)
            {
                *slot = Some((value(s, lo, kind.is_angle(i), cmd)?, value(s, hi, kind.is_angle(i), cmd)?));
            }
        }
    }
    Ok(out)
}

fn next_joint_id(s: &Session) -> u64 {
    s.doc.assembly.joints.iter().map(|j| j.id).max().unwrap_or(0) + 1
}

fn add_joint(s: &mut Session, mut j: Joint, p: &Value) -> Result<Value> {
    if s.doc.assembly.joints.len() >= 10_000 {
        return Err(EngineError::Other("too many joints".into()));
    }
    // Several parts as bodies of one component (a multi-body STEP import, #1239): say how to
    // get two components.
    if j.a.occurrence == j.b.occurrence {
        return Err(bad(
            "joint.create",
            "a joint needs two different occurrences: both origins are on one component; give each part a component of its own first \
             (component.from_bodies)",
        ));
    }
    // It starts at its values inside its limits. A value moved to a limit moves the part as it
    // is joined, which the answer says.
    let mut q = j.values.clone();
    q.resize(j.kind.dofs().len(), 0.0);
    joints::clamp(&j, &mut q);
    let round = |v: &Value| v.as_f64().map(|x| (x * 1e6).round() / 1e6).unwrap_or(f64::NAN);
    let warnings: Vec<String> = named_values(&j, &q)
        .iter()
        .zip(named_values(&j, &j.values))
        .filter(|(now, was)| now["value"] != was["value"])
        .map(|(now, was)| {
            let unit = now["unit"].as_str().unwrap_or("");
            format!(
                "{} {} starts at {} {unit}, inside its limits [{}, {}], not at {} {unit}: the part moved there as it was joined",
                j.name,
                now["name"].as_str().unwrap_or(""),
                round(&now["value"]),
                round(&now["limits"][0]),
                round(&now["limits"][1]),
                round(&was["value"])
            )
        })
        .collect();
    j.values = q;
    let id = j.id;
    let name = j.name.clone();
    // A new joint can take a deleted one's id; it starts shown.
    s.visibility.hidden_joints.retain(|h| *h != id);
    s.doc_mut().assembly.joints.push(j);
    let sol = resolve(s);
    let mut out = sol_json(&sol, p);
    out["joint"] = json!(id);
    out["name"] = json!(name);
    if !warnings.is_empty() {
        out["warnings"] = json!(warnings);
    }
    Ok(out)
}

fn joint_origin(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.origin";
    let origin = origin_param(s, Some(p), cmd)?;
    let name = match str_(p, "name").map(str::trim).filter(|n| !n.is_empty()) {
        Some(n) => n.to_string(),
        None => (1..).map(|k| format!("Joint Origin{k}")).find(|n| !s.doc.assembly.origins.iter().any(|o| &o.name == n)).unwrap_or_default(),
    };
    if s.doc.assembly.origins.iter().any(|o| o.name == name) {
        return Err(bad(cmd, format!("there is already a joint origin `{name}`")));
    }
    s.doc_mut().assembly.origins.push(NamedOrigin { name: name.clone(), origin });
    Ok(json!({"origin": name}))
}

fn kind_param(p: &Value, cmd: &str) -> Result<JointKind> {
    str_(p, "type")
        .and_then(JointKind::parse)
        .ok_or_else(|| bad(cmd, "`type` must be rigid, revolute, slider, cylindrical, pin_slot, planar or ball"))
}

fn joint(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.create";
    let kind = kind_param(p, cmd)?;
    let a = origin_param(s, p.get("a"), cmd)?;
    let b = origin_param(s, p.get("b"), cmd)?;
    let id = next_joint_id(s);
    let name = str_(p, "name").map(str::to_string).unwrap_or_else(|| format!("Joint{id}"));
    let offset = match p.get("offset") {
        Some(v) => value(s, v, false, cmd)?,
        None => 0.0,
    };
    let angle = match p.get("angle") {
        Some(v) => value(s, v, true, cmd)?,
        None => 0.0,
    };
    let j = Joint {
        id,
        name,
        kind,
        a,
        b,
        values: values_param(s, p, kind, cmd)?,
        limits: limits_param(s, p, kind, cmd)?,
        flip: bool_(p, "flip").unwrap_or(false),
        offset,
        angle,
        suppressed: false,
    };
    add_joint(s, j, p)
}

fn as_built(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.as_built";
    let kind = kind_param(p, cmd)?;
    let (oa, ob) = (occurrence_id(s, p.get("a"), cmd)?, occurrence_id(s, p.get("b"), cmd)?);
    // The joint's frame in the world: the given origin, or B's component origin.
    let w = match p.get("at") {
        Some(v) => {
            let o = origin_param(s, Some(v), cmd)?;
            s.doc.origin_world(&s.model.state(), &o).map_err(|e| bad(cmd, e.to_string()))?
        }
        None => joints::occurrence_world(&s.doc, ob),
    };
    let fa = s.doc.frame_in_occurrence(oa, &w).ok_or_else(|| bad(cmd, "occurrence transform"))?;
    let fb = s.doc.frame_in_occurrence(ob, &w).ok_or_else(|| bad(cmd, "occurrence transform"))?;
    let id = next_joint_id(s);
    let name = str_(p, "name").map(str::to_string).unwrap_or_else(|| format!("Joint{id}"));
    let n = kind.dofs().len();
    let j = Joint {
        id,
        name,
        kind,
        a: JointOrigin { occurrence: oa, snap: fa },
        b: JointOrigin { occurrence: ob, snap: fb },
        values: vec![0.0; n],
        limits: limits_param(s, p, kind, cmd)?,
        flip: true, // as built: the origins coincide as they are, not mated
        offset: 0.0,
        angle: 0.0,
        suppressed: false,
    };
    add_joint(s, j, p)
}

fn rigid_group(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.rigid_group";
    let list = p.get("occurrences").and_then(Value::as_array).cloned().unwrap_or_default();
    if list.len() < 2 || list.len() > 1000 {
        return Err(bad(cmd, "`occurrences` must list 2…1000 occurrences"));
    }
    let ids: Vec<u64> = list.iter().map(|v| occurrence_id(s, Some(v), cmd)).collect::<Result<_>>()?;
    let first = ids.first().copied().unwrap_or(0);
    let mut made = Vec::new();
    for other in ids.iter().skip(1) {
        let r = as_built(s, &json!({"type": "rigid", "a": first, "b": other, "name": format!("Rigid Group {first}:{other}")}))?;
        made.push(r["joint"].clone());
    }
    Ok(json!({"joints": made}))
}

fn drive(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.drive";
    let id = joint_id(s, p.get("joint"), cmd)?;
    let kind = s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.kind).ok_or_else(|| bad(cmd, "joint"))?;
    if kind == JointKind::Rigid {
        return Err(bad(cmd, "a rigid joint does not move"));
    }
    let mut q = values_param(s, p, kind, cmd)?;
    if p.get("values").is_none() && p.get("value").is_none() {
        return Err(bad(cmd, "give `value` or `values`"));
    }
    let doc = s.doc_mut();
    let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == id) else { return Err(bad(cmd, "joint")) };
    // Values not given (after `value`'s first, or past the end of `values`) keep theirs.
    let given = match p.get("values") {
        Some(Value::Array(a)) => a.len(),
        _ => 1,
    };
    for (i, v) in q.iter_mut().enumerate().skip(given) {
        *v = j.values.get(i).copied().unwrap_or(*v);
    }
    joints::clamp(j, &mut q);
    // Moved as far toward `q` as contact sets and closed loops allow. Values left out of
    // `values` are not driven: in a closed loop they follow.
    let before = super::motion::positions(s);
    let r = super::motion::drive_joint(s, id, q, given)?;
    let sol = joints::solve(&s.doc, &s.model.state());
    let mut out = sol_json(&sol, p);
    out["joint"] = json!(id);
    out["values"] = json!(effective_values(s, id));
    out["stopped_by_contact"] = r["stopped_by_contact"].clone();
    out["stopped_by_loop"] = r["stopped_by_loop"].clone();
    out["moved"] = json!(super::motion::moved(&before, &super::motion::positions(s)).next().is_some());
    if let Some((why, unconnected)) = cannot_move(s, &sol, id) {
        out["reason"] = json!(why);
        if let Some(o) = unconnected {
            out["unconnected"] = json!(o);
        }
    }
    Ok(out)
}

fn motion_link(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.motion_link";
    let (a, b) = (joint_id(s, p.get("a"), cmd)?, joint_id(s, p.get("b"), cmd)?);
    if a == b {
        return Err(bad(cmd, "link two different joints"));
    }
    let ratio = num(p, "ratio").ok_or_else(|| bad(cmd, "`ratio` must be a number"))?;
    let (ia, ib) = (num(p, "ia").unwrap_or(0.0).max(0.0) as usize, num(p, "ib").unwrap_or(0.0).max(0.0) as usize);
    let dofs = |id: u64| s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.kind.dofs().len()).unwrap_or(0);
    if ia >= dofs(a) || ib >= dofs(b) {
        return Err(bad(cmd, "the linked joints need moving values at those indices"));
    }
    // In b's units, like its values and limits (stored in radians for an angle).
    let offset = match p.get("offset") {
        Some(v) => value(s, v, is_angle(s, b, ib), cmd)?,
        None => 0.0,
    };
    let ratio = ratio * ratio_scale(s, a, ia, b, ib);
    s.doc_mut().assembly.links.push(MotionLink { a, ia, b, ib, ratio, offset, coupling: None });
    let sol = resolve(s);
    Ok(sol_json(&sol, p))
}

/// A coupling length: a number (mm), an expression (kept as typed, so it follows the
/// parameters), or `{circle: [x, y, z], occurrence?}`: the round face there, by its radius.
fn size_param(s: &Session, p: &Value, key: &str, cmd: &str) -> Result<Size> {
    match p.get(key) {
        Some(Value::Number(n)) => n.as_f64().map(Size::Length).ok_or_else(|| bad(cmd, format!("`{key}` must be a finite length"))),
        Some(Value::String(t)) if t.len() <= 1000 => Ok(Size::Expr(t.trim().to_string())),
        Some(o @ Value::Object(m)) if m.contains_key("circle") => Ok(Size::Circle(origin_param(s, Some(o), cmd)?)),
        _ => Err(bad(cmd, format!("`{key}` must be a length (mm or an expression) or a round face ({{circle: [x, y, z]}})"))),
    }
}

/// A joint's first value that turns (an angle), or that travels (a distance).
fn value_index(s: &Session, id: u64, turn: bool) -> Option<usize> {
    let j = s.doc.assembly.joints.iter().find(|j| j.id == id)?;
    (0..j.kind.dofs().len()).find(|i| j.kind.is_angle(*i) == turn)
}

/// The values a rack or a screw couples: a's turn and b's travel, or a's travel and b's turn.
fn turn_and_travel(s: &Session, a: u64, b: u64, cmd: &str) -> Result<(usize, usize)> {
    match (value_index(s, a, true), value_index(s, b, false), value_index(s, a, false), value_index(s, b, true)) {
        (Some(ia), Some(ib), ..) | (_, _, Some(ia), Some(ib)) => Ok((ia, ib)),
        _ => Err(bad(cmd, "couple a joint that turns (revolute) with one that slides (slider)")),
    }
}

/// Two different joints `a` and `b`.
fn joint_pair(s: &Session, p: &Value, cmd: &str) -> Result<(u64, u64)> {
    let (a, b) = (joint_id(s, p.get("a"), cmd)?, joint_id(s, p.get("b"), cmd)?);
    if a == b {
        return Err(bad(cmd, "couple two different joints"));
    }
    Ok((a, b))
}

fn reverse(p: &Value) -> bool {
    bool_(p, "reverse").unwrap_or(false)
}

fn gear(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.gear";
    let (a, b) = joint_pair(s, p, cmd)?;
    let turn = |id: u64| value_index(s, id, true).ok_or_else(|| bad(cmd, "gears need two joints that turn (revolute)"));
    let (ia, ib) = (turn(a)?, turn(b)?);
    let c = Coupling::Gear {
        ra: size_param(s, p, "ra", cmd)?,
        rb: size_param(s, p, "rb", cmd)?,
        internal: bool_(p, "internal").unwrap_or(false),
        reverse: reverse(p),
    };
    couple(s, p, cmd, MotionLink { a, ia, b, ib, ratio: 1.0, offset: 0.0, coupling: Some(c) })
}

fn rack_pinion(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.rack_pinion";
    let (a, b) = joint_pair(s, p, cmd)?;
    let (ia, ib) = turn_and_travel(s, a, b, cmd)?;
    let c = Coupling::RackPinion { radius: size_param(s, p, "radius", cmd)?, reverse: reverse(p) };
    couple(s, p, cmd, MotionLink { a, ia, b, ib, ratio: 1.0, offset: 0.0, coupling: Some(c) })
}

fn screw(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.screw";
    let (a, ia, b, ib) = match p.get("joint") {
        Some(v) => {
            let id = joint_id(s, Some(v), cmd)?;
            match (value_index(s, id, true), value_index(s, id, false)) {
                (Some(ia), Some(ib)) => (id, ia, id, ib),
                _ => return Err(bad(cmd, "a screw on one joint needs a joint that turns and slides (cylindrical); or give a and b")),
            }
        }
        None => {
            let (a, b) = joint_pair(s, p, cmd)?;
            let (ia, ib) = turn_and_travel(s, a, b, cmd)?;
            (a, ia, b, ib)
        }
    };
    let c = Coupling::Screw { pitch: size_param(s, p, "pitch", cmd)?, reverse: reverse(p) };
    couple(s, p, cmd, MotionLink { a, ia, b, ib, ratio: 1.0, offset: 0.0, coupling: Some(c) })
}

/// Add a coupling's motion link, or put it in place of link `link`: its ratio worked out from its
/// sizes, a new link's offset leaving b where it is (the teeth mesh as they are modelled).
fn couple(s: &mut Session, p: &Value, cmd: &str, mut l: MotionLink) -> Result<Value> {
    let links = &s.doc.assembly.links;
    let replace = match p.get("link") {
        None => None,
        Some(v) => {
            Some(v.as_u64().and_then(|i| usize::try_from(i).ok()).filter(|i| *i < links.len()).ok_or_else(|| bad(cmd, "no such motion link"))?)
        }
    };
    if replace.is_none() && links.len() >= joints::MAX_JOINTS {
        return Err(bad(cmd, "too many motion links"));
    }
    let others: Vec<&MotionLink> = links.iter().enumerate().filter(|(i, _)| Some(*i) != replace).map(|(_, k)| k).collect();
    let name = |id: u64| s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.name.clone()).unwrap_or_default();
    if others.iter().any(|k| (k.b, k.ib) == (l.b, l.ib)) {
        return Err(bad(cmd, format!("{} already follows a motion link: couple it the other way round (as a)", name(l.b))));
    }
    // The values that set a's, back through the links: b among them would make a cycle.
    let mut at = (l.a, l.ia);
    for _ in 0..=others.len() {
        if at == (l.b, l.ib) {
            return Err(bad(cmd, format!("{} already drives {} through motion links", name(l.b), name(l.a))));
        }
        match others.iter().find(|k| (k.b, k.ib) == at) {
            Some(k) => at = (k.a, k.ia),
            None => break,
        }
    }
    l.ratio = joints::coupling_ratio(&s.doc, &s.model.state(), &l).unwrap_or(Ok(l.ratio)).map_err(|e| bad(cmd, e.to_string()))?;
    let vals = joints::joint_values(&s.doc.assembly);
    let value = |id: u64, i: usize| vals.get(&id).and_then(|q| q.get(i)).copied().unwrap_or(0.0);
    // A link put in place of another keeps its offset, as a radius edited in the design does.
    l.offset = match replace.and_then(|i| links.get(i)) {
        Some(old) => old.offset,
        None => value(l.b, l.ib) - l.ratio * value(l.a, l.ia),
    };
    let shown = l.ratio / ratio_scale(s, l.a, l.ia, l.b, l.ib);
    let doc = s.doc_mut();
    let index = match replace {
        Some(i) => {
            if let Some(slot) = doc.assembly.links.get_mut(i) {
                *slot = l;
            }
            i
        }
        None => {
            doc.assembly.links.push(l);
            doc.assembly.links.len() - 1
        }
    };
    let sol = resolve(s);
    let mut out = sol_json(&sol, p);
    out["link"] = json!(index);
    out["ratio"] = json!(shown);
    Ok(out)
}

fn is_angle(s: &Session, joint: u64, i: usize) -> bool {
    s.doc.assembly.joints.iter().find(|j| j.id == joint).is_some_and(|j| j.kind.is_angle(i))
}

/// A link's `ratio` is in b's units per a's (mm per degree, degrees per mm) and stored per
/// radian: the given one times this is the stored one.
fn ratio_scale(s: &Session, a: u64, ia: usize, b: u64, ib: usize) -> f64 {
    let unit = |angle: bool| if angle { 1f64.to_radians() } else { 1.0 };
    unit(is_angle(s, b, ib)) / unit(is_angle(s, a, ia))
}

fn limits(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.limits";
    let id = joint_id(s, p.get("joint"), cmd)?;
    let i = num(p, "index").unwrap_or(0.0).max(0.0) as usize;
    let kind = s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.kind).ok_or_else(|| bad(cmd, "joint"))?;
    if i >= kind.dofs().len() {
        return Err(bad(cmd, "no such value"));
    }
    let lim = match (p.get("min"), p.get("max")) {
        (Some(lo), Some(hi)) => Some((value(s, lo, kind.is_angle(i), cmd)?, value(s, hi, kind.is_angle(i), cmd)?)),
        (None, None) => None,
        _ => return Err(bad(cmd, "give both `min` and `max`, or neither to clear")),
    };
    let doc = s.doc_mut();
    let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == id) else { return Err(bad(cmd, "joint")) };
    j.limits.resize(kind.dofs().len(), None);
    if let Some(slot) = j.limits.get_mut(i) {
        *slot = lim;
    }
    let mut q = j.values.clone();
    q.resize(kind.dofs().len(), 0.0);
    joints::clamp(j, &mut q);
    j.values = q;
    let sol = resolve(s);
    Ok(sol_json(&sol, p))
}

fn edit(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.edit";
    let id = joint_id(s, p.get("joint"), cmd)?;
    let kind = match str_(p, "type") {
        Some(t) => Some(JointKind::parse(t).ok_or_else(|| bad(cmd, "unknown joint type"))?),
        None => None,
    };
    let offset = match p.get("offset") {
        Some(v) => Some(value(s, v, false, cmd)?),
        None => None,
    };
    let angle = match p.get("angle") {
        Some(v) => Some(value(s, v, true, cmd)?),
        None => None,
    };
    let doc = s.doc_mut();
    let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == id) else { return Err(bad(cmd, "joint")) };
    if let Some(k) = kind {
        j.kind = k;
        j.values.resize(k.dofs().len(), 0.0);
        j.limits.resize(k.dofs().len(), None);
    }
    if let Some(o) = offset {
        j.offset = o;
    }
    if let Some(a) = angle {
        j.angle = a;
    }
    if let Some(f) = bool_(p, "flip") {
        j.flip = f;
    }
    if let Some(x) = bool_(p, "suppressed") {
        j.suppressed = x;
    }
    if let Some(n) = str_(p, "name").map(str::trim).filter(|n| !n.is_empty() && n.len() <= 128) {
        j.name = n.to_string();
    }
    let sol = resolve(s);
    Ok(sol_json(&sol, p))
}

fn delete(s: &mut Session, p: &Value) -> Result<Value> {
    let cmd = "joint.delete";
    if let Some(name) = str_(p, "origin") {
        let used = s.doc.assembly.joints.len();
        let doc = s.doc_mut();
        let before = doc.assembly.origins.len();
        doc.assembly.origins.retain(|o| o.name != name);
        if doc.assembly.origins.len() == before {
            return Err(bad(cmd, format!("no joint origin `{name}`")));
        }
        return Ok(json!({"deleted_origin": name, "joints": used}));
    }
    if let Some(i) = num(p, "link") {
        let i = i.max(0.0) as usize;
        let doc = s.doc_mut();
        if i >= doc.assembly.links.len() {
            return Err(bad(cmd, "no such motion link"));
        }
        doc.assembly.links.remove(i);
        return Ok(json!({"deleted_link": i}));
    }
    let id = joint_id(s, p.get("joint"), cmd)?;
    let doc = s.doc_mut();
    doc.assembly.joints.retain(|j| j.id != id);
    doc.assembly.links.retain(|l| l.a != id && l.b != id);
    let sol = resolve(s);
    let mut out = sol_json(&sol, p);
    out["deleted"] = json!(id);
    Ok(out)
}

fn list(s: &mut Session, p: &Value) -> Result<Value> {
    let st = s.model.state();
    let sol = joints::solve(&s.doc, &st);
    let vals = joints::joint_values(&s.doc.assembly);
    let js: Vec<Value> = s
        .doc
        .assembly
        .joints
        .iter()
        .map(|j| {
            let named = named_values(j, vals.get(&j.id).map(Vec::as_slice).unwrap_or_default());
            let world = |o: &JointOrigin| s.doc.origin_world(&st, o).ok().map(|m| json!({"origin": [m[3][0], m[3][1], m[3][2]], "z": [m[2][0], m[2][1], m[2][2]]}));
            json!({"id": j.id, "name": j.name, "type": j.kind, "a": j.a.occurrence, "b": j.b.occurrence, "values": named,
                "flip": j.flip, "offset": j.offset, "suppressed": j.suppressed, "visible": s.joint_visible(j.id), "frame_a": world(&j.a), "frame_b": world(&j.b)})
        })
        .collect();
    let links: Vec<Value> = s
        .doc
        .assembly
        .links
        .iter()
        .map(|l| {
            let angle = is_angle(s, l.b, l.ib);
            let offset = if angle { l.offset.to_degrees() } else { l.offset };
            let ratio = l.ratio / ratio_scale(s, l.a, l.ia, l.b, l.ib);
            let mut v =
                json!({"a": l.a, "ia": l.ia, "b": l.b, "ib": l.ib, "ratio": ratio, "offset": offset, "unit": if angle { "deg" } else { "mm" }});
            if let Some(c) = &l.coupling {
                v["coupling"] = json!(c);
                if let Some(Err(e)) = joints::coupling_ratio(&s.doc, &st, l) {
                    v["error"] = json!(e.to_string());
                }
            }
            v
        })
        .collect();
    let origins: Vec<Value> = s.doc.assembly.origins.iter().map(|o| json!({"name": o.name, "occurrence": o.origin.occurrence})).collect();
    let mut out = sol_json(&sol, p);
    out["joints"] = json!(js);
    out["links"] = json!(links);
    out["origins"] = json!(origins);
    out["floating"] = json!(floating_chains(s, &sol));
    Ok(out)
}

fn solve_cmd(s: &mut Session, p: &Value) -> Result<Value> {
    let sol = resolve(s);
    Ok(sol_json(&sol, p))
}

fn interference(s: &mut Session, p: &Value) -> Result<Value> {
    let want = string_list(p, "bodies");
    let st = s.world_state();
    let bodies: Vec<&solidcraft_doc::ModelBody> =
        st.bodies.iter().filter(|b| want.is_empty() || want.contains(&b.name)).filter(|b| !b.body.is_mesh()).collect();
    // Prune by bounding boxes first (sort and sweep), so the cost follows the pairs that can
    // touch rather than the square of the body count.
    let boxes: Vec<Aabb3> = bodies.iter().map(|b| b.mesh().bounds()).collect();
    let (pairs, skipped) = overlapping_pairs(&boxes, MAX_INTERFERENCE_PAIRS);
    let (mut hits, mut unchecked) = (Vec::new(), Vec::new());
    for (i, j) in pairs {
        let (Some(a), Some(b)) = (bodies.get(i), bodies.get(j)) else { continue };
        // A pair whose shared part can't be worked out is reported, not passed as clear.
        let (shared, report) =
            solidcraft_kernel::capture_boolean_diagnostics(|| solidcraft_kernel::boolean(&a.body, &b.body, solidcraft_kernel::BoolOp::Intersect));
        let mut skip = |reason: String| unchecked.push(json!({"a": a.name, "b": b.name, "reason": reason}));
        let shared = match shared {
            Ok(o) => o,
            Err(e) => {
                skip(e.to_string());
                continue;
            }
        };
        if let Some(reason) = report.unverified() {
            skip(reason);
        }
        let Some(o) = shared else { continue };
        let m = match solidcraft_kernel::measure(&o) {
            Ok(m) => m,
            Err(e) => {
                skip(e.to_string());
                continue;
            }
        };
        if m.volume > 1e-9 * a.body.size().max(1.0).powi(3) {
            hits.push(json!({"a": a.name, "b": b.name, "volume_mm3": m.volume, "area_mm2": m.area, "center": m.centroid, "bbox": {"min": m.bbox.min, "max": m.bbox.max}}));
        }
    }
    Ok(json!({"interferences": hits, "count": hits.len(), "checked": bodies.len(), "unchecked": unchecked,
        "truncated": skipped > 0, "skipped_pairs": skipped}))
}

/// Most pairs with overlapping bounding boxes `inspect.interference` intersects in one call.
const MAX_INTERFERENCE_PAIRS: usize = 20_000;

/// Index pairs `(i, j)`, `i < j`, whose boxes overlap or touch (sort and sweep along X), at most
/// `limit` of them, plus how many more there were.
fn overlapping_pairs(boxes: &[Aabb3], limit: usize) -> (Vec<(usize, usize)>, usize) {
    let mut order: Vec<usize> = (0..boxes.len()).collect();
    order.sort_by(|&a, &b| match (boxes.get(a), boxes.get(b)) {
        (Some(a), Some(b)) => a.min.x.total_cmp(&b.min.x),
        _ => std::cmp::Ordering::Equal,
    });
    let (mut pairs, mut skipped) = (Vec::new(), 0);
    for (k, &i) in order.iter().enumerate() {
        let Some(a) = boxes.get(i) else { continue };
        for &j in order.iter().skip(k + 1) {
            let Some(b) = boxes.get(j) else { continue };
            // Sorted by min.x: once one starts past `a`, all later ones do.
            if b.min.x > a.max.x {
                break;
            }
            if b.max.x < a.min.x || a.max.y < b.min.y || b.max.y < a.min.y || a.max.z < b.min.z || b.max.z < a.min.z {
                continue;
            }
            if pairs.len() < limit {
                pairs.push((i.min(j), i.max(j)));
            } else {
                skipped += 1;
            }
        }
    }
    pairs.sort_unstable();
    (pairs, skipped)
}

#[cfg(test)]
#[path = "joints_tests.rs"]
mod tests;

/// A point in the middle of the cylindrical face at `pick` (on its wall, halfway along).
fn cylinder_wall_point(b: &solidcraft_doc::kernel::Body, pick: Vec3) -> Option<Vec3> {
    let c = solidcraft_doc::kernel::cylinder_face_at(b, pick)?;
    let v = pick - c.axis_point;
    let along = v.dot(c.axis);
    let radial = (v - c.axis * along).normalized()?;
    Some(c.axis_point + c.axis * ((c.start + c.end) / 2.0) + radial * c.radius)
}