"""Planar two-link kinematics. Python standard library only; no physical robot."""
import json
import math


def fk(q, lengths=(1.0, 0.8)):
    q1, q2 = q
    a, b = lengths
    return (a * math.cos(q1) + b * math.cos(q1 + q2),
            a * math.sin(q1) + b * math.sin(q1 + q2))


def ik(x, y, elbow=1, lengths=(1.0, 0.8)):
    a, b = lengths
    c = (x * x + y * y - a * a - b * b) / (2 * a * b)
    if abs(c) > 1.0 + 1e-12:
        raise ValueError("target outside reachable annulus")
    c = max(-1.0, min(1.0, c))
    q2 = elbow * math.acos(c)
    q1 = math.atan2(y, x) - math.atan2(b * math.sin(q2), a + b * math.cos(q2))
    return q1, q2


def jacobian(q, lengths=(1.0, 0.8)):
    q1, q2 = q
    a, b = lengths
    return ((-a * math.sin(q1) - b * math.sin(q1 + q2), -b * math.sin(q1 + q2)),
            (a * math.cos(q1) + b * math.cos(q1 + q2), b * math.cos(q1 + q2)))


def main():
    q = ik(1.1, 0.6)
    p = fk(q)
    jac = jacobian(q)
    eps = 1e-6
    errors = []
    for col in range(2):
        plus, minus = list(q), list(q)
        plus[col] += eps
        minus[col] -= eps
        fp, fm = fk(plus), fk(minus)
        for row in range(2):
            errors.append(abs(jac[row][col] - (fp[row] - fm[row]) / (2 * eps)))
    result = {
        "target_m": [1.1, 0.6], "q_rad": q,
        "fk_error_m": math.hypot(p[0] - 1.1, p[1] - 0.6),
        "jacobian_max_error": max(errors),
        "singular_determinant": 1.0 * 0.8 * math.sin(0.0),
    }
    assert result["fk_error_m"] < 1e-12
    assert result["jacobian_max_error"] < 1e-8
    try:
        ik(2.0, 0.0)
    except ValueError:
        result["unreachable_target_rejected"] = True
    else:
        raise AssertionError("unreachable target accepted")
    print(json.dumps(result, indent=2))


if __name__ == "__main__":
    main()