"""Appendix H deterministic algebra checks; no acquisition, localization MC or orbit acceptance.

Run: python check_shared_timestamp_poisoning.py. Tolerance: 1e-10 unless stated.
"""
import numpy as np


def check(actual, expected):
    np.testing.assert_allclose(actual, expected, atol=1e-10, rtol=1e-10)


def predict(blocks, offsets, errors):
    sizes = [len(a) for a in blocks]
    A = np.zeros((sum(sizes), 3*len(blocks)))
    P = np.eye(sum(sizes))
    start = 0
    for k, a in enumerate(blocks):
        rows = slice(start, start+len(a))
        assert np.linalg.matrix_rank(a) == 3
        A[rows, 3*k:3*k+3] = a
        P[rows, rows] -= a@np.linalg.pinv(a)
        start += len(a)
    B, e = np.vstack(offsets), np.concatenate(errors)
    Z, b = P@B, P@e
    assert np.linalg.matrix_rank(Z, tol=1e-10) == B.shape[1]
    delta = np.linalg.lstsq(Z, b, rcond=None)[0]
    positions = np.linalg.lstsq(A, e-B@delta, rcond=None)[0]
    r = e-A@positions-B@delta
    D = np.column_stack((A, B))
    direct = np.linalg.lstsq(D, e, rcond=None)[0]
    check(np.r_[positions, delta], direct)
    check(r, P@(e-B@delta))
    S, gamma = Z.T@Z, Z.T@b
    check(delta, np.linalg.solve(S, gamma))
    check(b@b-r@r, gamma@np.linalg.solve(S, gamma))
    return positions.reshape(-1, 3), delta, r, D, P, B, e


def main():
    # Synthetic local summary blocks: delay first, then Doppler and angles.
    a = np.array([[1., .2, 0.], [.3, -.4, .2], [.4, 1., .1], [-.2, .3, 1.],
                  [1., 0., 0.], [0., 1., 0.], [0., 0., 1.], [.2, .1, .7]])
    b = np.vstack((np.eye(2), np.zeros((6, 2))))
    e = np.array([1., -.4, .7, -.2, 0., 0., 0., 0.])
    p0, d0, _, _, _, _, _ = predict([a], [b], [e])
    x, delta, r, D, P, B, error = predict([a]*4, [b]*4, [e]+[np.zeros(8)]*3)
    check(delta, d0/4)
    clean_x = -np.linalg.pinv(a)@b@d0/4
    check(x[1:], np.tile(clean_x, (3, 1)))
    check(x[0], np.linalg.pinv(a)@e+clean_x)
    pa = P[:8, :8]
    L = b.T@pa[:, :4]
    check(L[:, 2:], -b.T@a@np.linalg.pinv(a)[:, 2:4])
    assert np.linalg.norm(L[:, 2:]) > .01, "Doppler must expose indirect offset coupling"
    gain1 = np.linalg.norm(pa@e)**2-np.linalg.norm(predict([a], [b], [e])[2])**2
    check(np.linalg.norm(P@error)**2-r@r, gain1/4)
    S = B.T@P@B
    metric = L.T@np.linalg.solve(S, L)
    cross = 2*e[:2]@metric[:2, 2:]@e[2:4]
    check(e[:2]@metric[:2, :2]@e[:2]+cross+e[2:4]@metric[2:, 2:]@e[2:4], gain1/4)
    assert abs(cross) > .01, "Joint error must expose a nonzero cross term"
    common = np.array([.4, -.3])
    x_common, d_common, r_common, *_ = predict([a]*4, [b]*4, [b@common]*4)
    check(x_common, np.zeros((4, 3)))
    check(d_common, common)
    check(r_common, np.zeros(32))
    check(predict([a], [b], [b@common])[0], np.zeros((1, 3)))

    gamma = b.T@pa@e
    assert np.linalg.norm(gamma) > .01
    cancelling = np.linalg.svd(L)[2][-1]
    x_cancel, d_cancel, _, *_ = predict([a]*4, [b]*4,
                                       [np.r_[cancelling, np.zeros(4)]]+[np.zeros(8)]*3)
    check(d_cancel, np.zeros(2))
    check(x_cancel[1:], np.zeros((3, 3)))
    orthogonal = np.vstack((np.zeros((2, 3)), np.eye(3)))
    orthogonal_b = np.vstack((np.eye(2), np.zeros((3, 2))))
    orthogonal_e = orthogonal_b@common
    check(predict([orthogonal]*2, [orthogonal_b]*2,
                  [orthogonal_e, np.zeros(5)])[0], np.zeros((2, 3)))

    H = D@np.linalg.pinv(D)
    selection = np.eye(len(error))[:, :4]
    check(r@r, e[:4]@(np.eye(4)-selection.T@H@selection)@e[:4])
    assert np.linalg.matrix_rank(D[4:]) == D.shape[1]
    spike = np.eye(len(error))[:, 0]
    h = H[0, 0]
    spike_r = (np.eye(len(error))-H)@spike
    check(spike_r[0], 1-h)
    check(spike_r@spike_r, 1-h)
    check(spike_r[1:]@spike_r[1:], h*(1-h))
    assert np.linalg.norm(pa@e) > np.linalg.norm(r[:8])
    left_out_delta = predict([a]*3, [b]*3, [np.zeros(8)]*3)[1]
    check(left_out_delta, np.zeros(2))
    check(pa@(e-b@left_out_delta), pa@e)

    # Exact absorption counterexample: other users cannot anchor two offset directions.
    attacked = np.array([[1., 0., .4], [0., 1., -.3], [.2, .4, 1.],
                         [1., 0., 0.], [0., 1., 0.]])
    clean = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.],
                     [0., 0., 1.], [0., 0., 1.]])
    bc = np.vstack((np.eye(2), np.zeros((3, 2))))
    corrupted = attacked@np.array([0., 0., .7])+bc@common
    assert np.linalg.norm(corrupted[2]) > 0 and np.linalg.norm(corrupted[3:]) == 0
    xc, dc, rc, Dc, *_ = predict([attacked]+[clean]*3, [bc]*4,
                                [corrupted]+[np.zeros(5)]*3)
    check(dc, common)
    check(xc[1:], np.tile(np.r_[-common, 0.], (3, 1)))
    check(rc, np.zeros(20))
    assert np.linalg.matrix_rank(Dc[3:]) < Dc.shape[1]
    check((np.eye(5)-clean@np.linalg.pinv(clean))@bc, np.zeros((5, 2)))
    # A common offset changes neither user contrasts nor median-centered residuals.
    samples = np.array([.2, -.3, .4, .1])
    check(samples-np.median(samples), (samples+4)-np.median(samples+4))
    small = np.array([-3.4e-5, -3.3e-5, -3.3e-5, 1e-4])
    center, width = -3.35e-5, .5e-6
    difference = small[-1]-center
    contrast = abs(small[-1]-np.median(small))/(1.4826*np.median(abs(small-np.median(small)))+1e-9)
    lower = (abs(difference)-width)/(2*1.4826*width+1e-9)
    assert max(abs(small)) < .001 and contrast >= lower > 50
    # Free 3-D geometry includes position-dependent Earth-fixed user velocity.
    spin = np.array([[0., -7.292115e-5, 0.], [7.292115e-5, 0., 0.], [0., 0., 0.]])
    satellites = np.array([[6.9e6, 1e5, 2e5], [6.8e6, -3e5, 4e5], [7e6, 4e5, -2e5]])
    velocities = np.array([[0., 7200., 400.], [1200., 6800., -500.], [-1500., 7000., 100.]])
    position = np.array([6.37e6, 3e4, 7e4])
    def summaries(p):
        ranges = np.linalg.norm(p-satellites, axis=1)
        directions = (p-satellites)/ranges[:, None]
        doppler = np.sum(directions*(velocities-spin@p), axis=1)
        return np.r_[(ranges[1:]-ranges[0])/299792458e-6,
                      2*np.pi*1e-6/.2*(doppler[1:]-doppler[0])]
    ranges = np.linalg.norm(position-satellites, axis=1)
    directions = (position-satellites)/ranges[:, None]
    tangents = [(np.eye(3)-np.outer(u, u))/r for u, r in zip(directions, ranges)]
    doppler_jac = np.array([(v-spin@position)@t-u@spin
                            for v, t, u in zip(velocities, tangents, directions)])
    analytic = np.vstack(((directions[1:]-directions[0])/299792458e-6,
                          2*np.pi*1e-6/.2*(doppler_jac[1:]-doppler_jac[0])))
    step = .1
    numeric = np.column_stack([(summaries(position+step*basis)-summaries(position-step*basis))/(2*step)
                               for basis in np.eye(3)])
    geometry_error = np.linalg.norm(analytic-numeric)/np.linalg.norm(analytic)
    assert geometry_error < 1e-7, geometry_error
    print(f"Free-3D delay/Doppler Jacobian relative error: {geometry_error:.3g}")
    print("PASS: block maps, joint interactions, 1/K law, zero-coupling/cancellation,")
    print("      single-row recovery, deletion rank, exact absorption and consensus invariance.")
    print("Synthetic linear blocks only; real masks/scales and nonlinear prediction remain code-side checks.")


if __name__ == "__main__":
    main()