#!/usr/bin/env python3
"""Compare independent and inverse-model feedback for two coupled outputs.

Python 3, standard library only. State, time, and inputs are normalized.
Plant: p' = B(actual)u, B(c) = [[1,c],[c,1]], p(0)=(0,0), target=(1,0).
Independent feedback: u=K(target-p). Decoupled: u=B(design)^-1 K(target-p).
The continuous-feedback trajectory uses an exact matrix exponential, not time stepping.
"""

import argparse
import json
import math


DT = 0.05
STEPS = 160
TARGET = (1.0, 0.0)
PRESETS = {
    "matched": (0.6, 0.6),
    "mismatch": (0.6, 0.8),
    "near-singular": (0.95, 0.95),
}


def finite(value):
    if not math.isfinite(value):
        raise ValueError("The calculation exceeds the finite numeric range.")
    return 0.0 if value == 0 else value


def validate_coupling(coupling):
    if not isinstance(coupling, (int, float)) or not math.isfinite(coupling) or not 0 <= coupling < 1:
        raise ValueError("Use a finite coupling from zero up to, but excluding, one.")


def validate_gains(k1, k2):
    if not all(isinstance(k, (int, float)) and math.isfinite(k) and 0.5 <= k <= 2 for k in (k1, k2)):
        raise ValueError("Use finite gains between 0.5 and 2.")


def validate_vector(vector):
    if (not isinstance(vector, (list, tuple)) or len(vector) != 2
            or not all(isinstance(v, (int, float)) and math.isfinite(v) for v in vector)):
        raise ValueError("Use two finite vector coordinates.")


def plant(coupling):
    validate_coupling(coupling)
    return ((1.0, coupling), (coupling, 1.0))


def condition(coupling):
    """Spectral condition number, with normalized coordinates."""
    validate_coupling(coupling)
    return (1 + coupling) / (1 - coupling)


def controller_matrix(design_coupling, k1, k2, controller):
    validate_coupling(design_coupling)
    validate_gains(k1, k2)
    if controller == "independent":
        return ((k1, 0.0), (0.0, k2))
    if controller != "decoupled":
        raise ValueError("Choose independent or decoupled feedback.")
    denominator = (1 - design_coupling) * (1 + design_coupling)
    return ((k1 / denominator, -design_coupling * k2 / denominator),
            (-design_coupling * k1 / denominator, k2 / denominator))


def multiply(left, right):
    return tuple(tuple(finite(left[i][0] * right[0][j] + left[i][1] * right[1][j])
                       for j in range(2)) for i in range(2))


def apply(matrix, vector):
    return tuple(finite(row[0] * vector[0] + row[1] * vector[1]) for row in matrix)


def decay_rates(matrix):
    """Positive real eigenvalues of the lesson's error matrices."""
    if not isinstance(matrix, (list, tuple)) or len(matrix) != 2:
        raise ValueError("Use a finite 2 by 2 matrix.")
    for row in matrix:
        validate_vector(row)
    (a, b), (c, d) = matrix
    half_difference = (a - d) / 2
    discriminant = finite(half_difference * half_difference + b * c)
    if discriminant < 0:
        raise ValueError("This helper requires real decay rates.")
    fast = finite((a + d) / 2 + math.sqrt(discriminant))
    if fast <= 0:
        raise ValueError("This helper requires strictly positive decay rates.")
    slow = finite((a * d - b * c) / fast)
    if slow <= 0:
        raise ValueError("This helper requires strictly positive decay rates.")
    return (min(slow, fast), max(slow, fast))


def error_at(matrix, time, initial=TARGET):
    """Exact exp(-M t)e(0), including repeated rates and large times."""
    validate_vector(initial)
    if not isinstance(time, (int, float)) or not math.isfinite(time) or time < 0:
        raise ValueError("Use a finite, nonnegative time.")
    slow, fast = decay_rates(matrix)
    if time == 0:
        return tuple(initial)
    decay = math.exp(-slow * time)
    gap = fast - slow
    coefficient = time * decay if gap == 0 else decay * (-math.expm1(-gap * time) / gap)
    shifted = ((matrix[0][0] - slow, matrix[0][1]), (matrix[1][0], matrix[1][1] - slow))
    correction = apply(shifted, initial)
    return tuple(finite(decay * initial[i] - coefficient * correction[i]) for i in range(2))


def experiment(preset_id="matched", k1=1.0, k2=1.0):
    if preset_id not in PRESETS:
        raise ValueError("Choose a listed experiment preset.")
    validate_gains(k1, k2)
    design, actual = PRESETS[preset_id]
    actual_plant = plant(actual)
    runs = {}
    for controller in ("independent", "decoupled"):
        feedback = controller_matrix(design, k1, k2, controller)
        closed_loop = multiply(actual_plant, feedback)
        rows = []
        for index in range(STEPS + 1):
            time = index * DT
            error = error_at(closed_loop, time)
            rows.append({"time": time, "state": [TARGET[i] - error[i] for i in range(2)],
                         "error": error, "command": apply(feedback, error)})
        runs[controller] = {
            "controller": controller, "controllerMatrix": feedback, "closedLoopMatrix": closed_loop,
            "rows": rows, "initialVelocity": apply(actual_plant, rows[0]["command"]),
            "decayRates": decay_rates(closed_loop),
            "peakCrossTrack": max(abs(row["state"][1]) for row in rows),
            "peakCommand": max(abs(value) for row in rows for value in row["command"]),
            "finalError": math.hypot(*rows[-1]["error"]),
        }
    return {"preset": preset_id, "gains": (k1, k2), "designCondition": condition(design),
            "actualCondition": condition(actual), "runs": runs}


def check():
    """Numerical regressions, independent of the all-time eigenvalue argument."""
    def close(actual, expected, tolerance=1e-10):
        assert math.isclose(actual, expected, rel_tol=tolerance, abs_tol=tolerance), (actual, expected)

    for coupling in (0, 0.6, 0.8, 0.95):
        identity = multiply(plant(coupling), controller_matrix(coupling, 1, 1, "decoupled"))
        for i in range(2):
            for j in range(2):
                close(identity[i][j], float(i == j))
    default = experiment()
    close(default["designCondition"], 4)
    for row in default["runs"]["independent"]["rows"]:
        time = row["time"]
        close(row["error"][0], (math.exp(-0.4 * time) + math.exp(-1.6 * time)) / 2)
        close(row["error"][1], (math.exp(-1.6 * time) - math.exp(-0.4 * time)) / 2)
    for row in default["runs"]["decoupled"]["rows"]:
        close(row["state"][0], 1 - math.exp(-row["time"]))
        close(row["state"][1], 0)
    for preset_id in PRESETS:
        for k1 in (0.5, 1, 2):
            for k2 in (0.5, 1, 2):
                report = experiment(preset_id, k1, k2)
                for run in report["runs"].values():
                    assert run["decayRates"][0] > 0
                    for time in (0.1, 1, 3):
                        matrix = run["closedLoopMatrix"]
                        h = 1e-5
                        before, after = error_at(matrix, time - h), error_at(matrix, time + h)
                        slope = apply(matrix, error_at(matrix, time))
                        for i in range(2):
                            close((after[i] - before[i]) / (2 * h), -slope[i], 1e-7)
    close(experiment("near-singular")["runs"]["decoupled"]["rows"][0]["command"][0], 1 / 0.0975)
    close(experiment("mismatch")["runs"]["decoupled"]["initialVelocity"][1], 0.3125)
    close(error_at(((1, 2), (0, 1)), 2, (1, 1))[0], -3 * math.exp(-2))
    assert error_at(((1, 0), (0, 1)), 1000) == (0, 0)


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--check", action="store_true", help="run checked values and equation regressions")
    parser.add_argument("--json", action="store_true", help="print all sampled results as JSON")
    parser.add_argument("--preset", choices=PRESETS, default="matched")
    parser.add_argument("--k1", type=float, default=1.0)
    parser.add_argument("--k2", type=float, default=1.0)
    args = parser.parse_args()
    if args.check:
        check()
        print("Checks passed: inverse, analytic modes, exact ODE, unequal gains, mismatch, and repeated rates.")
        return
    try:
        report = experiment(args.preset, args.k1, args.k2)
    except ValueError as error:
        parser.error(str(error))
    if args.json:
        print(json.dumps(report, allow_nan=False))
        return
    design, actual = PRESETS[args.preset]
    print(f"Normalized coupled plant: design c={design:g}, actual c={actual:g}, gains=({args.k1:g}, {args.k2:g})")
    print(f"Condition numbers: design={report['designCondition']:.6f}, actual={report['actualCondition']:.6f}")
    for name, run in report["runs"].items():
        print(f"\n{name}: positive decay rates = {run['decayRates'][0]:.6f}, {run['decayRates'][1]:.6f}")
        print("time       p1        p2        u1        u2")
        for time in (0, 1, 2, 8):
            row = run["rows"][round(time / DT)]
            p1, p2 = row["state"]
            u1, u2 = row["command"]
            print(f"{time:4.1f} {p1:9.6f} {p2:9.6f} {u1:9.6f} {u2:9.6f}")
        print(f"Sampled maxima (0 to 8): |p2|={run['peakCrossTrack']:.6f}, |u|={run['peakCommand']:.6f}")
        print(f"Final error norm at t=8: {run['finalError']:.6f}")
    print("\nNo saturation, noise, inertia, or delay. Inputs can be signed and have unlimited magnitude.")
    print("Plot samples illustrate behavior; positive decay rates establish convergence for this linear model.")


if __name__ == "__main__":
    main()