#include "native_imu.h"

#include <array>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <limits>

namespace {
int failures = 0;
using Vector = std::array<double, 3>;
using Quaternion = std::array<double, 4>;

void expect(bool okay, const char* message) {
    if (!okay) {
        std::cerr << message << '\n';
        ++failures;
    }
}

// Deliberately decode bit-by-bit rather than mirroring the encoder's word
// boundaries. Double precision here is an independent host-side reference.
uint32_t bits(const uint8_t* bytes, unsigned start, unsigned width) {
    uint32_t result = 0;
    for (unsigned i = 0; i < width; ++i) {
        result |= static_cast<uint32_t>((bytes[(start + i) / 8] >> ((start + i) % 8)) & 1) << i;
    }
    return result;
}

int64_t signed_bits(const uint8_t* bytes, unsigned start, unsigned width) {
    const uint32_t value = bits(bytes, start, width);
    return value & (UINT32_C(1) << (width - 1)) ? static_cast<int64_t>(value) - (INT64_C(1) << width) : value;
}

struct Decoded {
    Quaternion q{};
    Vector acceleration{};
    std::array<int64_t, 3> acceleration_raw{};
    uint32_t counter = 0;
    uint32_t elapsed = 0;
    int64_t temperature = 0;
};

Decoded decode(const uint8_t* bytes) {
    Decoded result;
    expect(bytes[3] == 0x0c, "wire block must use the recovered one-sample format");
    result.counter = bits(bytes, 0, 12);
    result.elapsed = bits(bytes, 12, 12);
    result.temperature = signed_bits(bytes, 224, 16);
    const unsigned largest = bits(bytes, 32, 3);
    expect(largest < 4, "wire quaternion tag must identify an actual component");
    if (largest >= 4) return result;
    Vector ratios{};
    double length_squared = 1.0;
    for (unsigned i = 0; i < 3; ++i) {
        ratios[i] = bits(bytes, 35 + 31 * i, 31) / 1073741824.0 - 1.0;
        length_squared += ratios[i] * ratios[i];
        result.acceleration_raw[i] = signed_bits(bytes, 128 + 32 * i, 32);
        result.acceleration[i] = result.acceleration_raw[i] / 268435456.0;
    }
    result.q[largest] = 1.0 / std::sqrt(length_squared);
    for (unsigned i = 0; i < 3; ++i) result.q[(largest + i + 1) & 3] = ratios[i] * result.q[largest];
    return result;
}

Vector rotate(const Quaternion& q, const Vector& v) {
    const Vector cross{q[2] * v[2] - q[3] * v[1], q[3] * v[0] - q[1] * v[2], q[1] * v[1] - q[2] * v[0]};
    return {v[0] + 2.0 * (q[0] * cross[0] + q[2] * cross[2] - q[3] * cross[1]),
            v[1] + 2.0 * (q[0] * cross[1] + q[3] * cross[0] - q[1] * cross[2]),
            v[2] + 2.0 * (q[0] * cross[2] + q[1] * cross[1] - q[2] * cross[0])};
}

bool close(const Vector& a, const Vector& b, double tolerance = 0.00002) {
    for (unsigned i = 0; i < 3; ++i) {
        if (!std::isfinite(a[i]) || std::abs(a[i] - b[i]) > tolerance) return false;
    }
    return true;
}

Quaternion normalized(const float input[4]) {
    double squared = 0.0;
    for (unsigned i = 0; i < 4; ++i) squared += static_cast<double>(input[i]) * input[i];
    Quaternion result{};
    for (unsigned i = 0; i < 4; ++i) result[i] = input[i] / std::sqrt(squared);
    return result;
}

double camera_heading(const ProbeNativeMotion& motion) {
    const Vector forward = rotate(normalized(motion.quaternion()), {0.0, 1.0, 0.0});
    return std::atan2(forward[1], forward[0]);
}

double heading_change(const ProbeNativeMotion& motion, double initial_heading) {
    return std::remainder(camera_heading(motion) - initial_heading, 2.0 * std::acos(-1.0));
}

void check_encoded_orientation(const float q[4], const float accel[3]) {
    std::array<uint8_t, 32> guarded{};
    guarded.front() = 0x53;
    guarded.back() = 0x79;
    expect(probe_native_imu_pack(q, accel, 0xabc, 0xdef, -12345, guarded.data() + 1), "finite nonzero orientation must encode");
    expect(guarded.front() == 0x53 && guarded.back() == 0x79, "encoder must write exactly thirty bytes");
    const auto decoded = decode(guarded.data() + 1);
    expect(decoded.counter == 0xabc && decoded.elapsed == 0xdef && decoded.temperature == -12345,
           "wire counters and signed temperature must decode independently");
    for (unsigned axis = 0; axis < 3; ++axis) {
        Vector basis{};
        basis[axis] = 1.0;
        expect(close(rotate(decoded.q, basis), rotate(normalized(q), basis)),
               "wire orientation must preserve physical rotation for every largest-component choice");
    }
    expect(close(decoded.acceleration, {accel[0], accel[1], accel[2]}, 0.0000001),
           "wire acceleration must preserve signed Q28 g units");
}

void codec_wire_edges() {
    const float accel[3]{1.25f, -2.5f, 0.0625f};
    const float orientations[][4]{{4.0f, -1.0f, 2.0f, -3.0f}, {-0.5f, -3.0f, 0.25f, 1.0f},
                                  {1.0f, -2.0f, 4.0f, -0.5f}, {-1.0f, 0.5f, -2.0f, -4.0f},
                                  {1.0f, -1.0f, 1.0f, 1.0f}};
    for (const auto& q : orientations) check_encoded_orientation(q, accel);
    const float large = std::numeric_limits<float>::max();
    const float tiny = std::numeric_limits<float>::denorm_min();
    const float large_q[4]{large, -large, large, -large};
    const float tiny_q[4]{tiny, -tiny, tiny, -tiny};
    check_encoded_orientation(large_q, accel);
    check_encoded_orientation(tiny_q, accel);

    uint8_t bytes[30]{};
    const float identity[4]{1.0f, 0.0f, 0.0f, 0.0f};
    const float saturated[3]{large, -large, -8.0f};
    expect(probe_native_imu_pack(identity, saturated, 0, 4095, -32768, bytes), "finite out-of-range acceleration must saturate");
    const auto limits = decode(bytes);
    expect(limits.acceleration_raw == std::array<int64_t, 3>{INT32_MAX, INT32_MIN, INT32_MIN},
           "positive saturation must never wrap to negative acceleration");
    expect(limits.counter == 0 && limits.elapsed == 4095 && limits.temperature == -32768,
           "wire signed and unsigned field boundaries must survive encoding");
    const float ties[3]{0.5f / 268435456.0f, 1.5f / 268435456.0f, -2.5f / 268435456.0f};
    const float fine_q[4]{1.0f, 5.5f / 1073741824.0f, -5.5f / 1073741824.0f, 0.0f};
    expect(probe_native_imu_pack(fine_q, ties, 0, 0, 0, bytes), "sub-count finite inputs must encode");
    expect(decode(bytes).acceleration_raw == std::array<int64_t, 3>{0, 2, -2}, "Q28 halfway values use nearest-even rounding");
    expect(bits(bytes, 35, 31) == UINT32_C(0x40000006) && bits(bytes, 66, 31) == UINT32_C(0x3ffffffa),
           "adding the ratio midpoint must not erase low quaternion bits");
}

void codec_rejects_invalid_inputs() {
    float q[4]{1.0f, 0.0f, 0.0f, 0.0f};
    float accel[3]{0.0f, 0.0f, 1.0f};
    std::array<uint8_t, 30> output;
    output.fill(0xa5);
    const auto unchanged = output;
    const float invalid[]{std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::infinity(),
                          -std::numeric_limits<float>::infinity()};
    for (float value : invalid) {
        q[2] = value;
        expect(!probe_native_imu_pack(q, accel, 1, 1, 0, output.data()), "nonfinite quaternion must be rejected");
        q[2] = 0.0f;
        accel[1] = value;
        expect(!probe_native_imu_pack(q, accel, 1, 1, 0, output.data()), "nonfinite acceleration must be rejected, not saturated");
        accel[1] = 0.0f;
    }
    q[0] = 0.0f;
    expect(!probe_native_imu_pack(q, accel, 1, 1, 0, output.data()), "zero quaternion cannot identify a real orientation");
    q[0] = 1.0f;
    expect(!probe_native_imu_pack(q, accel, 4096, 1, 0, output.data()), "counter overflow must fail rather than overwrite elapsed bits");
    expect(!probe_native_imu_pack(q, accel, 1, 4096, 0, output.data()), "elapsed overflow must fail rather than overwrite format bits");
    expect(output == unchanged, "failed encoding must leave the caller's pending packet unchanged");
}

struct Rig {
    ProbeNativeMotion motion;
    ProbeNativeMotionSample sample{};
    uint32_t now;
    uint32_t generation = 1;
    static constexpr float residual[3]{0.0f, 0.0f, 0.0f};
    ProbeNativeMotionBias policy = ProbeNativeMotionBias::kAlreadyCalibrated;

    explicit Rig(uint32_t start = 0, ProbeNativeMotionBias mode = ProbeNativeMotionBias::kAlreadyCalibrated)
        : now(start), policy(mode) {
        sample.accel_valid = sample.gyro_valid = true;
        sample.accel_g[2] = 1.0f;
        rest();
    }
    void rest() {
        for (unsigned i = 0; i < 3; ++i) sample.gyro_dps[i] = residual[i];
    }
    void fresh(uint32_t elapsed = 25000, bool accel = true, bool gyro = true) {
        now += elapsed;
        if (accel) {
            ++sample.accel_sequence;
            sample.accel_us = now;
        }
        if (gyro) {
            ++sample.gyro_sequence;
            sample.gyro_us = now;
        }
        motion.update(now, generation, sample, policy);
    }
    void settle() {
        for (unsigned i = 0; i < 65; ++i) fresh();
        expect(motion.ready(), "fresh stationary sensors must finish residual bias calibration");
    }
    Decoded packet() {
        uint8_t bytes[30]{};
        expect(motion.ready(), "only calibrated fresh motion may be packed for the consumer");
        expect(probe_native_imu_pack(motion.quaternion(), motion.acceleration(), 0, 0, 0, bytes), "live motion must be encodable");
        return decode(bytes);
    }
};

void background_bias_requires_quiet_samples_not_startup_delay() {
    Rig fast(0, ProbeNativeMotionBias::kTrackStationary);
    fast.sample.gyro_dps[2] = 1.0f;
    fast.fresh(0);
    expect(fast.motion.ready(), "Wii motion must be available on the first real sensor pair");
    for (unsigned i = 0; i < 63; ++i) fast.fresh(1000);
    expect(fast.motion.bias()[2] == 0.0f, "packet count alone cannot establish a bias target");
    for (unsigned i = 0; i < 56; ++i) fast.fresh();
    expect(fast.motion.bias()[2] == 0.0f, "partial quiet windows cannot change gyro correction");
    const double before = camera_heading(fast.motion);
    fast.fresh(37000);
    expect(fast.motion.ready() && fast.motion.bias()[2] == 0.0f, "learning a target must not reset or snap motion");
    expect(std::abs(heading_change(fast.motion, before) - 0.037 * std::acos(-1.0) / 180.0) < 0.000001,
           "calibration completion must preserve accumulated yaw");
    fast.fresh();
    expect(fast.motion.bias()[2] > 0.0f && fast.motion.bias()[2] <= 0.125001f,
           "a confirmed target must be applied with a bounded slew, not a jump");

    Rig slow(0, ProbeNativeMotionBias::kTrackStationary);
    slow.sample.gyro_dps[2] = 1.0f;
    slow.fresh(0);
    for (unsigned i = 0; i < 62; ++i) slow.fresh(30000);
    expect(slow.motion.ready() && slow.motion.bias()[2] == 0.0f,
           "elapsed time without enough distinct samples cannot learn bias");
    slow.fresh(30000);
    slow.fresh(0);
    expect(slow.motion.bias()[2] == 0.0f, "zero elapsed time cannot increase correction strength");
    slow.fresh(25000);
    expect(slow.motion.bias()[2] > 0.0f, "a complete quiet window must refine bias without gating output");
}

void quantized_wii_bias_calibrates_and_integrates() {
    Rig rig(0, ProbeNativeMotionBias::kTrackStationary);
    constexpr float device_bias[3]{-13.0625f, 12.4375f, -13.125f};
    constexpr int noise_q10[8]{-576, 192, -320, 576, -192, 320, -64, 64};
    const Vector gravity{8352.0 / 8192.0, 290.0 / 8192.0, -298.0 / 8192.0};
    // Deterministic Wii-scale Q13 steps and Q10 noise, with accel and gyro
    // arriving independently at 200 / 100 Hz. Neither noise nor bias is motion.
    for (unsigned i = 0; i < 4000; ++i) {
        rig.sample.accel_g[0] = (8352 + (i & 1 ? 80 : -80)) / 8192.0f;
        rig.sample.accel_g[1] = (290 + (i & 2 ? 40 : -40)) / 8192.0f;
        rig.sample.accel_g[2] = (-298 + (i & 4 ? 43 : -43)) / 8192.0f;
        if (i % 2 == 0) {
            for (unsigned axis = 0; axis < 3; ++axis) {
                rig.sample.gyro_dps[axis] = device_bias[axis] + noise_q10[(i / 2 + axis * 3) % 8] / 1024.0f;
            }
        }
        rig.fresh(5000, true, i % 2 == 0);
        expect(rig.motion.ready(), "quantized Wii motion must remain available throughout background learning");
    }
    expect(rig.motion.ready(), "a large measured Wii residual must remain correctable in the background");
    expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]},
                 {device_bias[0], device_bias[1], device_bias[2]}, 0.02),
           "quiet sensor observations must converge to the measured device bias");
    const Quaternion reference = rig.packet().q;
    const double gravity_length = std::sqrt(gravity[0] * gravity[0] + gravity[1] * gravity[1] + gravity[2] * gravity[2]);
    expect(close(rotate(reference, gravity), {0.0, 0.0, gravity_length}, 0.0002),
           "quantized gravity must establish the physical reference without rescaling acceleration");
    for (unsigned axis = 0; axis < 3; ++axis) {
        rig.sample.gyro_dps[axis] = device_bias[axis];
        rig.sample.accel_g[axis] = static_cast<float>(gravity[axis]);
    }
    rig.fresh(0);
    for (unsigned i = 0; i < 100; ++i) rig.fresh(10000);
    expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), rotate(reference, {1.0, 0.0, 0.0}), 0.001),
           "learned large residual bias must not become stationary orientation drift");
    rig.sample.gyro_dps[2] += 90.0f;
    rig.fresh(0);
    for (unsigned i = 1; i <= 100; ++i) {
        const double angle = i * std::acos(-1.0) / 200.0;
        rig.sample.accel_g[0] = static_cast<float>(std::cos(angle) * gravity[0] + std::sin(angle) * gravity[1]);
        rig.sample.accel_g[1] = static_cast<float>(-std::sin(angle) * gravity[0] + std::cos(angle) * gravity[1]);
        rig.fresh(10000);
    }
    expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), rotate(reference, {0.0, 1.0, 0.0}), 0.001),
           "one second of body rotation must subtract the measured Wii bias before integration");
}

void measured_gravity_sets_reference() {
    const float poses[][3]{{1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, -1.0f},
                           {0.000001f, 0.000002f, -1.0f}, {0.3f, 0.4f, 0.8660254f}};
    for (const auto& pose : poses) {
        Rig rig;
        for (unsigned i = 0; i < 3; ++i) rig.sample.accel_g[i] = pose[i];
        rig.settle();
        const auto decoded = rig.packet();
        expect(close(rotate(decoded.q, decoded.acceleration), {0.0, 0.0, 1.0}),
               "measured gravity, including anti-parallel gravity, must align with reference +Z");
        expect(close(decoded.acceleration, {pose[0], pose[1], pose[2]}, 0.0000001),
               "reference initialization must not replace the real body acceleration");
    }
}

void bias_corrected_body_rotation_reaches_wire() {
    Rig rig;
    rig.sample.accel_g[0] = 1.0f;
    rig.sample.accel_g[2] = 0.0f;
    rig.settle();
    for (unsigned i = 0; i < 100; ++i) rig.fresh(10000);
    expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {0.0, 0.0, 1.0}),
           "stationary residual bias must not rotate the initialized orientation");
    rig.sample.gyro_dps[2] += 90.0f;
    rig.fresh(0);
    for (unsigned i = 1; i <= 100; ++i) {
        const double angle = i * std::acos(-1.0) / 200.0;
        rig.sample.accel_g[0] = static_cast<float>(std::cos(angle));
        rig.sample.accel_g[1] = static_cast<float>(-std::sin(angle));
        rig.fresh(10000);
    }
    const auto turned = rig.packet();
    expect(close(rotate(turned.q, {1.0, 0.0, 0.0}), {0.0, 1.0, 0.0}) &&
           close(rotate(turned.q, {0.0, 1.0, 0.0}), {0.0, 0.0, -1.0}) &&
           close(rotate(turned.q, {0.0, 0.0, 1.0}), {-1.0, 0.0, 0.0}),
           "one second of corrected body-Z rotation must compose after the nonidentity gravity alignment");
    rig.sample.accel_g[0] = 1.6f;
    rig.sample.accel_g[1] = -0.3f;
    rig.sample.accel_g[2] = 0.8f;
    rig.fresh(0);
    expect(close(rig.packet().acceleration, {1.6, -0.3, 0.8}, 0.0000001), "ready packets must retain real dynamic acceleration");
    expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]},
                 {Rig::residual[0], Rig::residual[1], Rig::residual[2]}, 0.000001),
           "motion after calibration must not relearn or chase the gyro bias");
}

void elapsed_time_not_packet_count_controls_rotation() {
    for (uint32_t interval : {5000u, 25000u}) {
        Rig rig;
        rig.settle();
        rig.sample.gyro_dps[2] += 120.0f;
        rig.fresh(0);
        for (uint32_t elapsed = interval; elapsed <= 1000000; elapsed += interval) {
            rig.fresh(interval, true, elapsed % 100000 == 0);
        }
        expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {-0.5, std::sqrt(0.75), 0.0}),
               "a fresh held gyro rate must integrate elapsed time regardless of polling cadence");
    }
    Rig split;
    split.settle();
    split.sample.gyro_dps[2] += 90.0f;
    ++split.sample.gyro_sequence;
    split.sample.gyro_us = split.now + 10000;
    split.fresh(40000, true, false);
    const double angle = 2.7 * std::acos(-1.0) / 180.0;
    expect(close(rotate(split.packet().q, {1.0, 0.0, 0.0}), {std::cos(angle), std::sin(angle), 0.0}),
           "a rate arriving inside an update interval must only affect time after its receipt");
}

void moving_startup_does_not_calibrate() {
    for (unsigned movement = 0; movement < 4; ++movement) {
        Rig rig(0, ProbeNativeMotionBias::kTrackStationary);
        for (unsigned i = 0; i < 100; ++i) {
            rig.rest();
            rig.sample.accel_g[0] = 0.0f;
            rig.sample.accel_g[1] = 0.0f;
            rig.sample.accel_g[2] = 1.0f;
            if (movement == 0) {
                // A real slow body-X turn changes gravity even with a constant
                // gyro reading. Its per-packet tilt is smaller than Wii noise.
                const float angle = static_cast<float>(i) * 0.025f * 2.0f * static_cast<float>(std::acos(-1.0) / 180.0);
                rig.sample.gyro_dps[0] += 2.0f;
                rig.sample.accel_g[1] = std::sin(angle);
                rig.sample.accel_g[2] = std::cos(angle);
            }
            if (movement == 1) rig.sample.accel_g[2] = 0.5f;
            if (movement == 2) rig.sample.accel_g[0] = i & 1 ? 0.04f : -0.04f;
            if (movement == 3) rig.sample.gyro_dps[0] = i & 1 ? 1.0f : -1.0f;
            rig.fresh();
        }
        expect(rig.motion.ready() && rig.motion.bias()[0] == 0.0f && rig.motion.bias()[1] == 0.0f &&
               rig.motion.bias()[2] == 0.0f, "moving startup must stay live without learning movement as bias");
        rig.rest();
        rig.sample.accel_g[0] = 0.0f;
        rig.sample.accel_g[1] = 0.0f;
        rig.sample.accel_g[2] = 1.0f;
        rig.sample.gyro_dps[2] = 1.0f;
        for (unsigned i = 0; i < 128; ++i) rig.fresh();
        expect(rig.motion.ready() && rig.motion.bias()[2] > 0.0f, "background correction must recover after movement");
    }
    Rig contaminated(0, ProbeNativeMotionBias::kTrackStationary);
    contaminated.sample.gyro_dps[2] = 1.0f;
    for (unsigned i = 0; i < 40; ++i) contaminated.fresh();
    contaminated.sample.gyro_dps[0] = 4.0f;
    contaminated.fresh();
    contaminated.rest();
    contaminated.sample.gyro_dps[2] = 1.0f;
    for (unsigned i = 0; i < 40; ++i) contaminated.fresh();
    expect(contaminated.motion.ready() && contaminated.motion.bias()[2] == 0.0f,
           "a gyro impulse must discard, not pool, the partial bias window");
    for (unsigned i = 0; i < 24; ++i) contaminated.fresh();
    expect(contaminated.motion.bias()[2] == 0.0f, "a completed estimate must not snap the current correction");
    contaminated.fresh();
    expect(contaminated.motion.bias()[2] > 0.0f, "a replacement quiet window must eventually refine bias");

    Rig interleaved(0, ProbeNativeMotionBias::kTrackStationary);
    interleaved.sample.gyro_dps[2] = 1.0f;
    for (unsigned i = 0; i < 55; ++i) interleaved.fresh();
    interleaved.sample.accel_g[0] = 0.1f;
    interleaved.fresh(5000, true, false);
    interleaved.sample.accel_g[0] = 0.0f;
    interleaved.fresh(5000, true, false);
    for (unsigned i = 0; i < 20; ++i) interleaved.fresh();
    expect(interleaved.motion.ready() && interleaved.motion.bias()[2] == 0.0f,
           "acceleration-only movement must reset bias collection without blocking motion");
    interleaved.settle();
}

void duplicate_sequences_and_interleaved_recovery() {
    Rig rig;
    rig.fresh(0);
    for (unsigned i = 0; i < 400; ++i) {
        // A caller cannot renew sensor lifetime by relabelling receipt time
        // while leaving the actual parser sequence unchanged.
        rig.sample.accel_us = rig.sample.gyro_us = rig.now + 5000;
        rig.fresh(5000, false, false);
    }
    expect(!rig.motion.ready(), "duplicate samples cannot establish calibrated sensor readiness");
    for (unsigned i = 0; i < 128; ++i) rig.fresh(25000, i % 2 == 0, i % 2 != 0);
    expect(rig.motion.ready(), "fresh independently interleaved sensors must recover after a stale reset");
    rig.sample.gyro_dps[2] += 100.0f;
    rig.fresh(10000, false, false);
    expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}),
           "mutating a duplicate sequence must not inject a new angular rate");
}

void lifecycle_invalidates_bias_and_orientation() {
    for (unsigned fault = 0; fault < 7; ++fault) {
        Rig rig(0, ProbeNativeMotionBias::kTrackStationary);
        rig.sample.gyro_dps[2] = 1.0f;
        for (unsigned i = 0; i < 400; ++i) rig.fresh(10000);
        expect(rig.motion.bias()[2] == 1.0f, "quiet samples must establish a correction before a lifecycle fault");
        if (fault == 0) rig.sample.accel_valid = false;
        if (fault == 1) rig.sample.gyro_valid = false;
        if (fault == 2) rig.sample.accel_g[0] = std::numeric_limits<float>::quiet_NaN();
        if (fault == 3) rig.sample.gyro_dps[0] = std::numeric_limits<float>::infinity();
        if (fault == 4) ++rig.generation;
        if (fault == 5) rig.motion.reset();
        rig.fresh(fault == 6 ? 50001 : 1000);
        expect(rig.motion.ready() == (fault >= 4), "invalid sensors fail closed; fresh sensors after reset recover immediately");
        expect(rig.motion.bias()[2] == 0.0f, "faults must retire the old correction and its target");
        ++rig.generation;
        rig.sample.accel_valid = rig.sample.gyro_valid = true;
        rig.sample.accel_g[0] = rig.sample.accel_g[1] = 0.0f;
        rig.sample.accel_g[2] = -1.0f;
        rig.sample.gyro_dps[0] = -0.25f;
        rig.sample.gyro_dps[1] = 0.1f;
        rig.sample.gyro_dps[2] = -0.35f;
        rig.fresh();
        expect(rig.motion.ready() && close(rotate(rig.packet().q, {0.0, 0.0, -1.0}), {0.0, 0.0, 1.0}),
               "a fresh connection must initialize from its real pose without waiting");
        for (unsigned i = 0; i < 400; ++i) rig.fresh(10000);
        expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]}, {-0.25, 0.1, -0.35}, 0.000001),
               "background recovery must learn the new source, not resume a retired target");
    }
}

void stale_boundaries_and_hidden_gaps() {
    for (bool stale_gyro : {false, true}) {
        Rig rig;
        rig.settle();
        rig.fresh(50000, stale_gyro, !stale_gyro);
        rig.fresh(50000, stale_gyro, !stale_gyro);
        rig.fresh(49999, stale_gyro, !stale_gyro);
        expect(rig.motion.ready(), "a held sensor remains usable strictly below the 150ms age limit");
        rig.fresh(1, stale_gyro, !stale_gyro);
        expect(!rig.motion.ready(), "either sensor reaching the 150ms limit must invalidate motion");
        rig.settle();
    }
    Rig hidden;
    hidden.settle();
    hidden.sample.gyro_dps[2] += 90.0f;
    hidden.fresh(0);
    hidden.fresh(50000, false, false);
    hidden.fresh(50000, false, false);
    expect(hidden.motion.ready(), "a 50ms update gap is still within the integration limit");
    hidden.fresh(50000);
    expect(hidden.motion.ready(), "a replacement exactly at expiry must preserve the continuously covered interval");
    hidden.fresh(50000, false, false);
    hidden.fresh(50000, false, false);
    hidden.fresh(49999, false, false);
    hidden.fresh(2);
    expect(hidden.motion.ready() && close(rotate(hidden.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}),
           "fresh replacements after a hidden gap must reinitialize, not integrate stale angular displacement");
    hidden.rest();
    hidden.fresh(0);
    hidden.settle();
    expect(close(rotate(hidden.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}),
           "stale angular displacement must not replay after recalibration");
}

void clocks_sequences_and_connections_wrap() {
    Rig rig(UINT32_MAX - 2200000u);
    rig.sample.accel_sequence = rig.sample.gyro_sequence = UINT32_MAX - 30u;
    rig.settle();
    rig.sample.gyro_dps[2] += 90.0f;
    rig.fresh(0);
    for (unsigned i = 0; i < 80; ++i) rig.fresh(12500);
    expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {0.0, 1.0, 0.0}),
           "microsecond rollover must preserve one second of physical angular displacement");
    ++rig.generation;
    rig.sample.accel_sequence = rig.sample.gyro_sequence = 0;
    rig.rest();
    rig.fresh(0);
    expect(rig.motion.ready(), "a new connection must start immediately from its own fresh samples");
    rig.settle();
    expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}),
           "a new connection must establish its own reference orientation");

    Rig calibration_wrap(UINT32_MAX - 750000u);
    calibration_wrap.settle();
    expect(calibration_wrap.motion.ready(), "the stationary window must span microsecond rollover safely");
}

void fresh_gravity_bounds_warm_gyro_tilt_drift() {
    Rig normal, fast;
    for (Rig* rig : {&normal, &fast}) {
        rig->sample.accel_g[0] = 1.0f;
        rig->sample.accel_g[2] = 0.0f;
        rig->settle();
        // The console trace drifted roughly 0.6 degrees/s after startup.
        rig->sample.gyro_dps[1] += 0.6f;
        rig->fresh(0);
    }
    double maximum_tilt = 0.0;
    for (unsigned millisecond = 1; millisecond <= 60000; ++millisecond) {
        fast.fresh(1000, millisecond % 10 == 0, millisecond % 10 == 0);
        if (millisecond % 10 == 0) normal.fresh(10000);
        if (millisecond % 1000 == 0) {
            const Vector gravity = rotate(normal.packet().q, {1.0, 0.0, 0.0});
            const double tilt = std::atan2(std::hypot(gravity[0], gravity[1]), gravity[2]);
            if (tilt > maximum_tilt) maximum_tilt = tilt;
        }
    }
    expect(maximum_tilt < std::acos(-1.0) / 180.0,
           "fresh stationary gravity must keep warming gyro tilt drift below one degree");
    expect(close(rotate(normal.packet().q, {1.0, 0.0, 0.0}),
                 rotate(fast.packet().q, {1.0, 0.0, 0.0}), 0.0002),
           "duplicate accelerometer polling must not strengthen gravity correction");
}

void dynamic_acceleration_does_not_steer_orientation() {
    Rig rig;
    rig.settle();
    rig.sample.gyro_dps[2] += 60.0f;
    rig.sample.accel_g[0] = 2.0f;
    rig.fresh(0);
    for (unsigned i = 0; i < 100; ++i) rig.fresh(10000);
    const auto packet = rig.packet();
    expect(close(rotate(packet.q, {1.0, 0.0, 0.0}), {0.5, std::sqrt(0.75), 0.0}),
           "non-gravity acceleration must not steer gyro orientation");
    expect(close(packet.acceleration, {2.0, 0.0, 1.0}),
           "gravity correction must not replace the consumer's dynamic acceleration");
}

void optical_heading_bounds_drift_without_polling_gain() {
    Rig slow, frequent, gyro_only;
    for (Rig* rig : {&slow, &frequent, &gyro_only}) {
        rig->settle();
        rig->sample.gyro_dps[2] += 0.6f;
        rig->fresh(0);
    }
    const double initial = camera_heading(slow.motion);
    slow.motion.observe_optical_heading(slow.now, 1, 0, slow.now, 0.2f, true);
    frequent.motion.observe_optical_heading(frequent.now, 1, 0, frequent.now, 0.2f, true);
    double maximum_drift = 0.0;
    for (uint32_t step = 1; step <= 6000; ++step) {
        slow.fresh(10000);
        frequent.fresh(10000);
        gyro_only.fresh(10000);
        // The same physical reference at 10Hz and 50Hz, with extra polling of
        // duplicate 50Hz reports, must give essentially the same drift bound.
        if (step % 10 == 0) {
            slow.motion.observe_optical_heading(slow.now, 1, step / 10, slow.now, 0.2f, true);
        }
        frequent.motion.observe_optical_heading(frequent.now, 1, step / 2,
                                                frequent.now, 0.2f, true);
        if (step % 10 == 0) {
            const double drift = std::abs(heading_change(slow.motion, initial));
            if (drift > maximum_drift) maximum_drift = drift;
        }
    }
    expect(maximum_drift < 1.3 * std::acos(-1.0) / 180.0,
           "stable optical bearing must bound a minute of warming gyro yaw drift");
    expect(std::abs(heading_change(slow.motion, camera_heading(frequent.motion))) < 0.0002,
           "optical correction strength must follow sample time, not sample or polling count");
    expect(std::abs(heading_change(gyro_only.motion, initial) - 36.0 * std::acos(-1.0) / 180.0) < 0.0002,
           "without optical observations gravity must leave yaw gyro-derived");
}

void optical_anchor_preserves_pose_and_corrects_only_world_yaw() {
    Rig rig;
    rig.sample.accel_g[0] = 0.3f;
    rig.sample.accel_g[1] = 0.4f;
    rig.sample.accel_g[2] = std::sqrt(0.75f);
    rig.settle();
    // Establish arbitrary nonzero world yaw while preserving this physical tilt.
    for (unsigned i = 0; i < 3; ++i) rig.sample.gyro_dps[i] += 60.0f * rig.sample.accel_g[i];
    rig.fresh(0);
    for (unsigned i = 0; i < 20; ++i) rig.fresh(25000);
    rig.rest();
    rig.fresh(0);
    const Quaternion initial = normalized(rig.motion.quaternion());
    const double initial_heading = camera_heading(rig.motion);
    rig.motion.observe_optical_heading(rig.now, 1, 1, rig.now, 0.45f, true);
    expect(normalized(rig.motion.quaternion()) == initial,
           "the first optical sample must preserve arbitrary existing yaw and tilt exactly");
    rig.sample.accel_g[0] = 1.8f;
    rig.sample.accel_g[1] = -0.2f;
    rig.sample.accel_g[2] = 0.6f;
    rig.fresh(0);
    for (uint32_t step = 1; step <= 400; ++step) {
        rig.fresh(25000);
        if (step % 4 == 0) {
            rig.motion.observe_optical_heading(rig.now, 1, step / 4 + 1, rig.now, 0.8f, true);
        }
    }
    expect(std::abs(heading_change(rig.motion, initial_heading) + 0.35) < 0.004,
           "increasing aim-right optical yaw must converge to negative world yaw, not reanchor away motion");
    const Quaternion corrected = normalized(rig.motion.quaternion());
    for (unsigned axis = 0; axis < 3; ++axis) {
        Vector basis{};
        basis[axis] = 1.0;
        expect(std::abs(rotate(initial, basis)[2] - rotate(corrected, basis)[2]) < 0.000002,
               "optical correction must preserve every body axis's gravity-aligned tilt");
    }
    expect(close(rig.packet().acceleration, {1.8, -0.2, 0.6}, 0.0000001),
           "optical correction must retain real dynamic acceleration in the native packet");
    expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]},
                 {Rig::residual[0], Rig::residual[1], Rig::residual[2]}, 0.000001),
           "optical correction must not modify the calibrated raw gyro bias");
}

void optical_duplicates_and_hidden_intervals_do_not_gain_weight() {
    Rig rig;
    rig.settle();
    const double initial_heading = camera_heading(rig.motion);
    uint32_t sequence = 10;
    rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, 0.0f, true);
    rig.sample.gyro_dps[2] += 30.0f;
    rig.fresh(0);
    for (unsigned i = 0; i < 40; ++i) {
        rig.fresh(25000);
        rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, false);
    }
    rig.rest();
    rig.fresh(0);
    expect(std::abs(heading_change(rig.motion, initial_heading) - std::acos(-1.0) / 6.0) < 0.00002,
           "hidden or inferred optical reports must not invent a heading correction");
    const double hidden_heading = camera_heading(rig.motion);
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
    expect(std::abs(heading_change(rig.motion, hidden_heading)) < 0.000001,
           "reacquisition must not spend correction weight accumulated while hidden");
    for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
    expect(heading_change(rig.motion, hidden_heading) < -0.02,
           "continuous reacquisition must correct toward the retained original relative anchor");
    const double corrected_heading = camera_heading(rig.motion);
    for (unsigned i = 0; i < 200; ++i) {
        rig.fresh(1000);
        // A changed value and a newly labelled timestamp are still one packet.
        rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, -1.0f, true);
    }
    expect(std::abs(heading_change(rig.motion, corrected_heading)) < 0.000001,
           "duplicate optical sequence identities cannot strengthen correction or inject changed bearings");
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
    expect(std::abs(heading_change(rig.motion, corrected_heading)) < 0.000001,
           "duplicate timestamps cannot keep optical validity alive across a stale interval");
}

void optical_rejects_bad_samples_and_clock_order() {
    Rig rig;
    rig.settle();
    rig.motion.observe_optical_heading(rig.now, 1, 1, rig.now, 0.0f, true);
    uint32_t sequence = 1;
    const double initial_heading = camera_heading(rig.motion);
    // Each rejected identity must remain rejected if its fields are rewritten.
    for (unsigned fault = 0; fault < 4; ++fault) {
        for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
        const float bearing = fault == 0 ? std::numeric_limits<float>::quiet_NaN() : 1.0f;
        const uint32_t timestamp = fault == 1 ? rig.now - 150000u :
                                   fault == 2 ? rig.now + 1u : rig.now;
        rig.motion.observe_optical_heading(rig.now, 1, ++sequence, timestamp, bearing, fault != 3);
        for (unsigned i = 0; i < 4; ++i) {
            rig.fresh(25000);
            rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, 1.0f, true);
        }
        expect(std::abs(heading_change(rig.motion, initial_heading)) < 0.000001,
               "nonfinite, stale, future or unavailable optical identities cannot later be relabelled valid");
    }
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
    const uint32_t last_timestamp = rig.now;
    rig.fresh(25000);
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, last_timestamp - 1u, 1.0f, true);
    rig.motion.observe_optical_heading(rig.now, 1, sequence - 1u, rig.now, 1.0f, true);
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, last_timestamp, 1.0f, true);
    expect(std::abs(heading_change(rig.motion, initial_heading)) < 0.000001,
           "backwards clocks, out-of-order sequences and zero-time samples must not correct heading");
    // A subsequent ordered sample still contributes its own sensor-time
    // interval after the rejected observations.
    rig.fresh(25000);
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now - 1u, 1.0f, true);
    expect(heading_change(rig.motion, initial_heading) < -0.02,
           "a later fresh unique optical sample must still correct after rejected clock order");
}

void optical_generation_and_motion_lifecycle_reanchor() {
    for (unsigned transition = 0; transition < 4; ++transition) {
        Rig rig;
        rig.settle();
        rig.motion.observe_optical_heading(rig.now, UINT32_MAX, 100, rig.now, 0.0f, true);
        for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
        rig.motion.observe_optical_heading(rig.now, UINT32_MAX, 101, rig.now, 1.0f, true);
        expect(heading_change(rig.motion, std::acos(-1.0) / 2.0) < -0.04,
               "the old optical reference must be active before lifecycle replacement");
        uint32_t optical_generation = UINT32_MAX;
        uint32_t sequence = 102;
        if (transition == 0) {
            optical_generation = 0;
            sequence = 0;
        } else {
            if (transition == 1) {
                rig.sample.gyro_valid = false;
                rig.fresh(0);
                rig.sample.gyro_valid = true;
            }
            if (transition == 2) rig.motion.reset();
            if (transition == 3) ++rig.generation;
            rig.settle();
        }
        const Quaternion before = normalized(rig.motion.quaternion());
        const double heading = camera_heading(rig.motion);
        rig.motion.observe_optical_heading(rig.now, optical_generation, sequence, rig.now, -0.8f, true);
        expect(normalized(rig.motion.quaternion()) == before,
               "optical generation, motion invalidation, reset and reconnect must anchor without an initial jump");
        for (unsigned i = 0; i < 40; ++i) {
            rig.fresh(25000);
            if (i % 4 == 3) {
                rig.motion.observe_optical_heading(rig.now, optical_generation, ++sequence, rig.now, -0.8f, true);
            }
        }
        expect(std::abs(heading_change(rig.motion, heading)) < 0.000002,
               "a replaced optical reference must not pull toward the preceding lifecycle's bearing");
    }
}

void optical_angles_and_sample_clocks_wrap() {
    Rig rig(UINT32_MAX - 1750000u);
    rig.settle();
    const float pi = std::acos(-1.0f);
    const float initial_bearing = pi - 0.02f;
    const double initial_heading = camera_heading(rig.motion);
    uint32_t sequence = UINT32_MAX - 1u;
    rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, initial_bearing, true);
    // Matching physical yaw and optical motion cross both wrap boundaries.
    rig.sample.gyro_dps[2] -= 30.0f;
    rig.fresh(0);
    for (unsigned step = 1; step <= 20; ++step) {
        rig.fresh(25000);
        if (step % 4 == 0) {
            const float bearing = std::remainder(initial_bearing + step * 0.025f * pi / 6.0f, 2.0f * pi);
            rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, bearing, true);
        }
    }
    expect(std::abs(heading_change(rig.motion, initial_heading) + std::acos(-1.0) / 12.0) < 0.00002,
           "optical angle, sequence and microsecond wrap must preserve intentional yaw direction without a jump");
    rig.rest();
    rig.fresh(0);
    const float final_bearing = std::remainder(initial_bearing + pi / 12.0f + 0.1f, 2.0f * pi);
    for (unsigned step = 1; step <= 400; ++step) {
        rig.fresh(25000);
        if (step % 4 == 0) {
            rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, final_bearing, true);
        }
    }
    expect(std::abs(heading_change(rig.motion, initial_heading) + std::acos(-1.0) / 12.0 + 0.1) < 0.001,
           "wrapped optical observations must continue correcting toward the original relative anchor");
}

void optical_vertical_forward_defers_the_anchor() {
    Rig rig;
    rig.sample.accel_g[1] = 1.0f;
    rig.sample.accel_g[2] = 0.0f;
    rig.settle();
    const Quaternion vertical = normalized(rig.motion.quaternion());
    uint32_t sequence = 0;
    for (unsigned i = 0; i < 20; ++i) {
        rig.fresh(25000);
        rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, i * 0.1f, true);
    }
    expect(close(rotate(normalized(rig.motion.quaternion()), {1.0, 0.0, 0.0}),
                 rotate(vertical, {1.0, 0.0, 0.0})),
           "near-vertical camera forward must reject ill-conditioned optical yaw");
    // Tilt smoothly away from vertical with matching physical gravity.
    rig.sample.gyro_dps[0] -= 90.0f;
    rig.fresh(0);
    for (unsigned i = 1; i <= 40; ++i) {
        const float angle = i * std::acos(-1.0f) / 80.0f;
        rig.sample.accel_g[1] = std::cos(angle);
        rig.sample.accel_g[2] = std::sin(angle);
        rig.fresh(25000);
    }
    rig.rest();
    rig.fresh(0);
    const Quaternion horizontal = normalized(rig.motion.quaternion());
    const double heading = camera_heading(rig.motion);
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, -0.6f, true);
    expect(normalized(rig.motion.quaternion()) == horizontal,
           "the first well-conditioned optical observation must establish the anchor without jumping");
    for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
    rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, -0.3f, true);
    expect(heading_change(rig.motion, heading) < -0.01,
           "heading correction must recover after the forward projection leaves vertical");
}

void background_tracking_preserves_motion_and_sensor_time() {
    Rig turn(0, ProbeNativeMotionBias::kTrackStationary);
    turn.sample.gyro_dps[2] = 90.0f;
    turn.fresh(0);
    for (unsigned i = 1; i <= 1000; ++i) {
        turn.fresh(10000);
        expect(turn.motion.ready(), "continuous movement must never block Wii motion");
        if (i == 100)
            expect(close(rotate(turn.packet().q, {1.0, 0.0, 0.0}), {0.0, 1.0, 0.0}),
                   "startup rotation must reach native output immediately");
    }
    expect(turn.motion.bias()[2] == 0.0f, "large steady yaw must not be learned as a stationary offset");

    Rig sparse(0, ProbeNativeMotionBias::kTrackStationary);
    Rig frequent(0, ProbeNativeMotionBias::kTrackStationary);
    sparse.sample.gyro_dps[2] = frequent.sample.gyro_dps[2] = 1.0f;
    sparse.fresh(0);
    frequent.fresh(0);
    for (unsigned ms = 1; ms <= 4000; ++ms) {
        frequent.fresh(1000, ms % 10 == 0, ms % 10 == 0);
        if (ms % 10 == 0) sparse.fresh(10000);
    }
    expect(sparse.motion.bias()[2] == 1.0f && frequent.motion.bias()[2] == 1.0f,
           "stationary gyro offset must converge under either polling cadence");
    expect(std::abs(heading_change(sparse.motion, camera_heading(frequent.motion))) < 0.000001,
           "duplicate polling must not increase background correction or change accumulated yaw");
    const double corrected_heading = camera_heading(sparse.motion);
    for (unsigned i = 0; i < 100; ++i) sparse.fresh(10000);
    expect(std::abs(heading_change(sparse.motion, corrected_heading)) < 0.000001,
           "learned correction must stop further stationary yaw drift without recentering");
}

}  // namespace

int main() {
    codec_wire_edges();
    codec_rejects_invalid_inputs();
    background_bias_requires_quiet_samples_not_startup_delay();
    background_tracking_preserves_motion_and_sensor_time();
    quantized_wii_bias_calibrates_and_integrates();
    measured_gravity_sets_reference();
    bias_corrected_body_rotation_reaches_wire();
    elapsed_time_not_packet_count_controls_rotation();
    moving_startup_does_not_calibrate();
    duplicate_sequences_and_interleaved_recovery();
    lifecycle_invalidates_bias_and_orientation();
    stale_boundaries_and_hidden_gaps();
    clocks_sequences_and_connections_wrap();
    fresh_gravity_bounds_warm_gyro_tilt_drift();
    dynamic_acceleration_does_not_steer_orientation();
    optical_heading_bounds_drift_without_polling_gain();
    optical_anchor_preserves_pose_and_corrects_only_world_yaw();
    optical_duplicates_and_hidden_intervals_do_not_gain_weight();
    optical_rejects_bad_samples_and_clock_order();
    optical_generation_and_motion_lifecycle_reanchor();
    optical_angles_and_sample_clocks_wrap();
    optical_vertical_forward_defers_the_anchor();
    return failures ? 1 : 0;
}