//! `joint.reach`: inverse kinematics against closed-form answers (planar arms), the nearest pose
//! out of reach, limits, and a four-bar reached through its rocker.

use serde_json::{Value, json};
use solidcraft_geom::Vec3;

use super::tests::{four_bar, meet, occ, part, pin, place, run};
use crate::Session;

/// A planar arm about +Z on a grounded base: Shoulder at the origin, Upper 60 long, Elbow, Fore
/// 40 long, and with `hand` a Wrist and Hand 20 long, all along +X at 0°. Elbow starts at `elbow`°.
fn arm(hand: bool, elbow: f64) -> Session {
    let mut s = Session::default();
    part(&mut s, "Base", [-1.0, -1.0, -1.0]);
    part(&mut s, "Upper", [30.0, -1.0, -1.0]);
    part(&mut s, "Fore", [80.0, -1.0, -1.0]);
    let base = occ(&s, "Base");
    run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true}));
    pin(&mut s, "revolute", "Shoulder", "Base", "Upper", Vec3::ZERO, Vec3::Z);
    pin(&mut s, "revolute", "Elbow", "Upper", "Fore", Vec3::new(60.0, 0.0, 0.0), Vec3::Z);
    if hand {
        part(&mut s, "Hand", [110.0, -1.0, -1.0]);
        pin(&mut s, "revolute", "Wrist", "Fore", "Hand", Vec3::new(100.0, 0.0, 0.0), Vec3::Z);
    }
    run(&mut s, "joint.drive", json!({"joint": "Elbow", "value": elbow}));
    s
}

/// A joint's first value as `joint.list` gives it (degrees for an angle).
fn value(s: &mut Session, joint: &str) -> f64 {
    let l = run(s, "joint.list", json!({"quiet": true}));
    l["joints"]
        .as_array()
        .and_then(|js| js.iter().find(|j| j["name"] == joint))
        .map_or(f64::NAN, |j| j["values"][0]["value"].as_f64().unwrap_or(f64::NAN))
}

/// `joint.reach` of the occurrence's point `local` (its own coordinates).
fn reach(s: &mut Session, occurrence: &str, local: Vec3, mut p: Value) -> Value {
    p["occurrence"] = json!(occurrence);
    p["at"] = pt(place(s, occurrence, local));
    run(s, "joint.reach", p)
}

fn pt(v: Vec3) -> Value {
    json!([v.x, v.y, v.z])
}

fn got(r: &Value) -> Vec3 {
    Vec3::new(r["point"][0].as_f64().unwrap_or(f64::NAN), r["point"][1].as_f64().unwrap_or(f64::NAN), r["point"][2].as_f64().unwrap_or(f64::NAN))
}

/// The two-link arm's closed-form answer for a tip at (x, y), elbow bent the positive way: the
/// shoulder and elbow angles in degrees.
fn two_link(x: f64, y: f64, l1: f64, l2: f64) -> (f64, f64) {
    let c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);
    let t2 = c2.acos();
    let t1 = y.atan2(x) - (l2 * t2.sin()).atan2(l1 + l2 * t2.cos());
    (t1.to_degrees(), t2.to_degrees())
}

#[test]
fn a_two_link_arm_reaches_its_closed_form_pose() {
    let mut s = arm(false, 30.0);
    let tip = Vec3::new(100.0, 0.0, 0.0);
    let to = Vec3::new(50.0, 50.0, 0.0);
    let r = reach(&mut s, "Fore", tip, json!({"point": pt(to)}));
    assert_eq!(
        (&r["reached"], &r["moved"], &r["stopped_by_limit"], &r["stopped_by_loop"]),
        (&json!(true), &json!(true), &Value::Null, &Value::Null),
        "{r}"
    );
    assert!(r["error_mm"].as_f64().is_some_and(|e| e < 1e-6), "{r}");
    assert!(place(&s, "Fore", tip).dist(to) < 1e-6, "{:?}", place(&s, "Fore", tip));
    let (t1, t2) = two_link(50.0, 50.0, 60.0, 40.0);
    assert!((value(&mut s, "Shoulder") - t1).abs() < 1e-6 && (value(&mut s, "Elbow") - t2).abs() < 1e-6, "{r}");
    assert_eq!(r["values"].as_array().map(Vec::len), Some(2), "{r}");
    // Already there, a reach moves nothing and adds no undo step.
    let steps = s.undo.len();
    let r = reach(&mut s, "Fore", tip, json!({"point": pt(to)}));
    assert_eq!((&r["reached"], &r["moved"], s.undo.len()), (&json!(true), &json!(false), steps), "{r}");
    // One undo step puts it back.
    run(&mut s, "edit.undo", json!({}));
    assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-9 && value(&mut s, "Shoulder").abs() < 1e-9);
}

/// A three-link arm puts its tip on the point and its hand along the axis given: the wrist is
/// then the two-link answer, and the hand's turn makes up the rest.
#[test]
fn a_three_link_arm_reaches_a_point_and_an_axis() {
    let mut s = arm(true, 30.0);
    let tip = Vec3::new(120.0, 0.0, 0.0);
    // The hand's length, along its own X: at 30° in the world now (the elbow's turn).
    let along = place(&s, "Hand", tip) - place(&s, "Hand", Vec3::new(100.0, 0.0, 0.0));
    let r = reach(&mut s, "Hand", tip, json!({"point": [50, 50, 0], "z": [0, 1, 0], "axis": pt(along)}));
    assert_eq!(r["reached"], true, "{r}");
    assert!(r["error_deg"].as_f64().is_some_and(|e| e < 1e-6), "{r}");
    let (t1, t2) = two_link(50.0, 30.0, 60.0, 40.0);
    let t3 = 90.0 - t1 - t2;
    for (joint, want) in [("Shoulder", t1), ("Elbow", t2), ("Wrist", t3)] {
        assert!((value(&mut s, joint) - want).abs() < 1e-6, "{joint}: {} wants {want}: {r}", value(&mut s, joint));
    }
}

#[test]
fn out_of_reach_the_arm_points_at_the_target() {
    let tip = Vec3::new(100.0, 0.0, 0.0);
    // 150 mm away: the arm stretches toward it, 50 mm short.
    let mut s = arm(false, 30.0);
    let r = reach(&mut s, "Fore", tip, json!({"point": [-90, 120, 0]}));
    assert_eq!(
        (&r["reached"], &r["moved"], &r["stopped_by_limit"], &r["stopped_by_loop"]),
        (&json!(false), &json!(true), &Value::Null, &Value::Null),
        "{r}"
    );
    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - 50.0).abs() < 1e-3), "{r}");
    assert!(got(&r).dist(Vec3::new(-60.0, 80.0, 0.0)) < 1e-2, "{r}");
    assert!(place(&s, "Fore", tip).dist(got(&r)) < 1e-9, "{r}");
    // Out of its plane: the nearest is the point under the target.
    let mut s = arm(false, 30.0);
    let r = reach(&mut s, "Fore", tip, json!({"point": [50, 50, 30]}));
    assert_eq!(r["reached"], false, "{r}");
    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - 30.0).abs() < 1e-6), "{r}");
    assert!(got(&r).dist(Vec3::new(50.0, 50.0, 0.0)) < 1e-6, "{r}");
}

#[test]
fn a_limited_elbow_stops_the_reach_and_says_so() {
    let tip = Vec3::new(100.0, 0.0, 0.0);
    let mut s = arm(false, 10.0);
    run(&mut s, "joint.limits", json!({"joint": "Elbow", "min": -30, "max": 30}));
    let r = reach(&mut s, "Fore", tip, json!({"point": [50, 50, 0]}));
    // The target needs the elbow at 92°: it bends to its limit, and the arm points at the target.
    assert_eq!(r["reached"], false, "{r}");
    assert_eq!(r["stopped_by_limit"], json!([{"joint": "Elbow", "value": "angle", "limit": "max"}]), "{r}");
    assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-9, "{r}");
    let span = (60f64.powi(2) + 40f64.powi(2) + 2.0 * 60.0 * 40.0 * 30f64.to_radians().cos()).sqrt();
    let d = 50.0 * 2f64.sqrt();
    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - (span - d)).abs() < 1e-4), "{r}");
    // Only the shoulder moves when only it is given: the tip swings on its circle.
    let mut s = arm(false, 30.0);
    let r = reach(&mut s, "Fore", tip, json!({"point": [0, 100, 0], "joints": ["Shoulder"]}));
    let span = (60f64.powi(2) + 40f64.powi(2) + 2.0 * 60.0 * 40.0 * 30f64.to_radians().cos()).sqrt();
    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - (100.0 - span)).abs() < 1e-4), "{r}");
    assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-12, "{r}");
}

/// A linked elbow (elbow = −shoulder, so the forearm keeps pointing along +X) follows the
/// shoulder: the tip moves on a circle about (40, 0), and the reach turns the shoulder only.
#[test]
fn a_linked_joint_follows_the_reach() {
    let tip = Vec3::new(100.0, 0.0, 0.0);
    let mut s = arm(false, 0.0);
    run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Elbow", "ratio": -1}));
    let th = 40f64.to_radians();
    let to = json!([40.0 + 60.0 * th.cos(), 60.0 * th.sin(), 0.0]);
    let r = reach(&mut s, "Fore", tip, json!({"point": to}));
    assert_eq!(r["reached"], true, "{r}");
    assert!((value(&mut s, "Shoulder") - 40.0).abs() < 1e-6 && (value(&mut s, "Elbow") + 40.0).abs() < 1e-6, "{r}");
    // Given only the linked elbow, the reach turns it through its link's source.
    run(&mut s, "edit.undo", json!({}));
    let r = reach(&mut s, "Fore", tip, json!({"point": to, "joints": ["Elbow"]}));
    assert_eq!(r["reached"], true, "{r}");
    assert!((value(&mut s, "Shoulder") - 40.0).abs() < 1e-6, "{r}");
    // The elbow held at its own limit by the link stops the reach, and is named.
    run(&mut s, "edit.undo", json!({}));
    run(&mut s, "joint.limits", json!({"joint": "Elbow", "min": -30, "max": 30}));
    let r = reach(&mut s, "Fore", tip, json!({"point": to}));
    assert_eq!(r["reached"], false, "{r}");
    assert!(r["stopped_by_limit"].as_array().is_some_and(|l| l.contains(&json!({"joint": "Elbow", "value": "angle", "limit": "min"}))), "{r}");
}

/// A target straight across the pivot from the point (where the misses don't change to first
/// order) is still reached: the arm turns half round, the way its limits leave open.
#[test]
fn a_target_across_the_pivot_is_reached() {
    let tip = Vec3::new(100.0, 0.0, 0.0);
    for (limits, want) in [(None, None), (Some([-180, 0]), Some(-180.0)), (Some([0, 180]), Some(180.0))] {
        let mut s = arm(false, 0.0);
        if let Some([min, max]) = limits {
            run(&mut s, "joint.limits", json!({"joint": "Shoulder", "min": min, "max": max}));
        }
        let r = reach(&mut s, "Fore", tip, json!({"point": [-100, 0, 0], "joints": ["Shoulder"]}));
        assert_eq!(r["reached"], true, "{limits:?}: {r}");
        let got = value(&mut s, "Shoulder");
        assert!((got.abs() - 180.0).abs() < 1e-6 && want.is_none_or(|w| (got - w).abs() < 1e-6), "{limits:?}: {got}");
    }
}

/// A loop that only closes again after a full turn of a value geared up from the arm's (a joint
/// between two grounded parts, linked 36:1 to the shoulder) holds the arm: a step of a tenth of a
/// turn would close it at both ends, but not on the way.
#[test]
fn a_loop_geared_up_from_the_chain_holds_it() {
    let tip = Vec3::new(100.0, 0.0, 0.0);
    let mut s = arm(false, 0.0);
    part(&mut s, "Lock", [-1.0, 5.0, -1.0]);
    let lock = occ(&s, "Lock");
    run(&mut s, "occurrence.ground", json!({"occurrence": lock, "grounded": true}));
    pin(&mut s, "revolute", "Lock", "Base", "Lock", Vec3::ZERO, Vec3::Z);
    run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Lock", "ratio": 36}));
    let r = reach(&mut s, "Fore", tip, json!({"point": [0, 1e6, 0], "joints": ["Shoulder"]}));
    assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(false), &json!({"joint": "Lock"})), "{r}");
    assert!(value(&mut s, "Shoulder").abs() < 1e-6, "{r}");
    // Geared up so far that even the small kick off a target straight across the pivot would turn
    // the loop a whole turn: it holds too.
    run(&mut s, "joint.delete", json!({"link": 0}));
    run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Lock", "ratio": 36000}));
    let r = reach(&mut s, "Fore", tip, json!({"point": [-100, 0, 0], "joints": ["Shoulder"]}));
    assert_eq!(r["reached"], false, "{r}");
    assert!(value(&mut s, "Shoulder").abs() < 1e-6, "{r}");
}

/// A planar joint far from the point turns it and slides the long way round, and the answer
/// gives the values as joint.list does.
#[test]
fn a_planar_joint_far_from_the_point_turns_and_slides() {
    let mut s = Session::default();
    part(&mut s, "Base", [-1.0, -1.0, -1.0]);
    part(&mut s, "Puck", [99.0, -1.0, 2.0]);
    let base = occ(&s, "Base");
    run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true}));
    pin(&mut s, "planar", "Table", "Base", "Puck", Vec3::ZERO, Vec3::Z);
    let r = reach(&mut s, "Puck", Vec3::new(100.0, 0.0, 0.0), json!({"point": [100, 0, 0], "z": [0, 1, 0], "axis": [1, 0, 0]}));
    assert_eq!(r["reached"], true, "{r}");
    let l = run(&mut s, "joint.list", json!({"quiet": true}));
    let listed = l["joints"].as_array().and_then(|js| js.iter().find(|j| j["name"] == "Table")).map(|j| j["values"].clone());
    assert_eq!(Some(r["values"][0]["values"].clone()), listed, "{r}");
    assert!((r["values"][0]["values"][2]["value"].as_f64().unwrap_or(f64::NAN) - 90.0).abs() < 1e-6, "{r}");
}

/// A slider on its own (a chain with no size) still travels the whole way.
#[test]
fn a_lone_slider_travels_all_the_way() {
    let mut s = Session::default();
    part(&mut s, "Base", [-1.0, -1.0, -1.0]);
    part(&mut s, "Ram", [-1.0, -1.0, 2.0]);
    let base = occ(&s, "Base");
    run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true}));
    pin(&mut s, "slider", "Rail", "Base", "Ram", Vec3::ZERO, Vec3::Z);
    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 100]}));
    assert_eq!(r["reached"], true, "{r}");
    assert!((value(&mut s, "Rail") - 100.0).abs() < 1e-6, "{r}");
    // However far.
    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 3e7]}));
    assert_eq!(r["reached"], true, "{r}");
    // A limit on it is named whatever the chain's size.
    run(&mut s, "edit.undo", json!({}));
    run(&mut s, "joint.limits", json!({"joint": "Rail", "min": -50, "max": 100}));
    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 1e6]}));
    assert_eq!(r["stopped_by_limit"], json!([{"joint": "Rail", "value": "distance", "limit": "max"}]), "{r}");
    // And a reach of a thousandth of a millimetre still moves it.
    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 99.999]}));
    assert_eq!((&r["reached"], &r["moved"]), (&json!(true), &json!(true)), "{r}");
}

/// The four-bar's rocker reaches a point on its swing, the crank and coupler following with the
/// loop closed; past its toggle position it stops there, and the loop is named.
#[test]
fn a_four_bar_reaches_with_its_rocker() {
    let (mut s, _, q0) = four_bar();
    let o4 = Vec3::new(45.0, 0.0, 0.0);
    let start = q0.y.atan2(q0.x - 45.0);
    let swing = |deg: f64| o4 + Vec3::new((start + deg.to_radians()).cos(), (start + deg.to_radians()).sin(), 0.0) * 40.0;
    let r = run(&mut s, "joint.reach", json!({"occurrence": "Rocker", "at": pt(q0), "point": pt(swing(-10.0))}));
    assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(true), &Value::Null), "{r}");
    assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
    assert!(place(&s, "Rocker", q0).dist(swing(-10.0)) < 1e-6 && place(&s, "Coupler", q0).dist(swing(-10.0)) < 1e-6, "{r}");
    // The crank turned to where the loop closes with the rocker there (its pin on the coupler's circle).
    let th = (90.0 + value(&mut s, "Crank")).to_radians();
    let crank_pin = Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0);
    assert!((crank_pin.dist(swing(-10.0)) - 50.0).abs() < 1e-6 && place(&s, "Coupler", Vec3::new(0.0, 20.0, 0.0)).dist(crank_pin) < 1e-6, "{r}");
    // Past its toggle position (|Q| = 30) the loop stops it there.
    let (mut s, _, q0) = four_bar();
    let r = run(&mut s, "joint.reach", json!({"occurrence": "Rocker", "at": pt(q0), "point": pt(swing(100.0))}));
    assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(false), &json!({"joint": "Rocker pin"})), "{r}");
    // Its joints have no limits, so none is named.
    assert_eq!(r["stopped_by_limit"], Value::Null, "{r}");
    assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
    let limit = ((900.0f64 - 3625.0) / 3600.0).acos().to_degrees() - start.to_degrees();
    let at = value(&mut s, "Rocker");
    assert!(at > limit - 0.5 && at <= limit + 1e-6, "stopped at {at}°, the toggle is at {limit}°: {r}");
    let [m1, m2] = meet(o4, 40.0, Vec3::ZERO, 30.0);
    assert!(place(&s, "Rocker", q0).dist(m1).min(place(&s, "Rocker", q0).dist(m2)) < 0.5, "{r}");
}

#[test]
fn reach_refuses_bad_params_and_says_why_nothing_moves() {
    let mut s = arm(false, 30.0);
    for bad in [
        json!({"occurrence": "Fore"}),
        json!({"occurrence": "Nope", "point": [0, 0, 0]}),
        json!({"occurrence": "Fore", "point": [1e300, 0, 0]}),
        json!({"occurrence": "Fore", "point": [0, 0, "x"]}),
        json!({"occurrence": "Fore", "point": [0, 0, 0], "z": [0, 0, 0]}),
        json!({"occurrence": "Fore", "point": [0, 0, 0], "axis": [1, 0, 0]}),
        json!({"occurrence": "Fore", "point": [0, 0, 0], "joints": "Elbow"}),
        json!({"occurrence": "Fore", "point": [0, 0, 0], "joints": ["Nope"]}),
        json!({"occurrence": "Fore", "point": [0, 0, 0], "at": [f64::MAX, 0, 0]}),
    ] {
        assert!(s.execute("joint.reach", &bad).is_err(), "{bad}");
    }
    // A grounded part, a part no joint places, and joints that don't move it: nothing moves, and
    // the answer says why.
    part(&mut s, "Loose", [200.0, 0.0, 0.0]);
    for (p, why) in [
        (json!({"occurrence": "Base", "point": [5, 5, 5]}), "fixed in place"),
        (json!({"occurrence": "Loose", "point": [5, 5, 5]}), "not connected"),
        (json!({"occurrence": "Upper", "point": [5, 5, 5], "joints": ["Elbow"]}), "none of the joints given"),
    ] {
        let r = run(&mut s, "joint.reach", p.clone());
        assert_eq!((&r["reached"], &r["moved"]), (&json!(false), &json!(false)), "{p}: {r}");
        assert!(r["reason"].as_str().is_some_and(|w| w.contains(why)), "{p}: {r}");
    }
}