#include <assert.h>
#include <math.h>
#include <stdarg.h>
#include <stdint.h>
#include <string.h>

#include "controller_input.h"
#include "input/bluepad32_input_backend.h"
#include "input/wii_ir_pointer.h"
#include "platform/pico/bootsel_pairing_button.h"
#include "platform/pico/system_clock.h"
#include "profile/controller_profile_runtime.h"
#include "pico/stdlib.h"

static uint64_t now_us;
static uint32_t stage;
static Bluepad32WiiBridgeSnapshot source;
static bool swap_faces = true;
static bool publish_during_snapshot;
static uint32_t ir_sequence;
static uint32_t sensor_sequence;
static uint32_t last_sensor_us;
static uint16_t raw_ir_buttons;
static uint16_t ir_x[4] = {420, 620, 0, 0};
static uint16_t ir_y[4] = {384, 384, 0, 0};
static uint8_t ir_mask = 3;
static probe_controller_input controls;
static uint8_t packet[63];
static const int32_t bias_q10[3] = {1024, -2048, 512};

uint32_t time_us_32() { return static_cast<uint32_t>(now_us); }
absolute_time_t get_absolute_time() { return now_us; }
uint32_t to_ms_since_boot(absolute_time_t time) { return static_cast<uint32_t>(time / 1000); }
absolute_time_t make_timeout_time_ms(uint32_t ms) { return now_us + 1000ull * ms; }
bool time_reached(absolute_time_t deadline) { return now_us >= deadline; }
void sleep_ms(uint32_t ms) { now_us += 1000ull * ms; }
void system_clock_initialize() {}
extern "C" int probe_debug_printf(const char*, ...) { return 0; }
BootselPairingButtonEvent bootsel_pairing_button_task() { return BootselPairingButtonEvent::kNone; }

void bluepad32_input_backend_init() { stage = 1; wii_ir_pointer_init(); }
void bluepad32_input_backend_start() { stage = 2; }
void bluepad32_input_backend_diagnostics(Bluepad32BackendDiagnostics* out) { *out = {}; out->initialization_stage = stage; }
void bluepad32_input_backend_open_pairing_window() {}
void bluepad32_input_backend_select_wii_source(const uint8_t[6]) { wii_ir_pointer_reset(); }
void bluepad32_input_backend_wii_snapshot(Bluepad32WiiBridgeSnapshot* out) {
    if (publish_during_snapshot) {
        // Model Core1 publishing after Core0 entered the snapshot call.
        now_us += 300;
        source.accel_sequence = source.gyro_sequence = ++sensor_sequence;
        source.received_us = source.accel_received_us = source.gyro_received_us = time_us_32();
    }
    *out = source;
}
bool bluepad32_input_backend_set_wii_orientation(const ControllerIdentity&, uint32_t, bool) { return false; }
void bluepad32_input_backend_report_sent(uint8_t) {}
bool bluepad32_input_backend_wii_sample_request(uint8_t, uint64_t*) { return false; }
int bluepad32_input_backend_wii_sample_result(uint64_t) { return -1; }
void bluepad32_input_backend_wii_sample_cancel() {}
void bluepad32_input_backend_queue_profile_feedback(uint8_t, uint32_t, uint8_t, ControllerProfileConfirmationPolicy) {}
void controller_profile_runtime_reset() {}
bool controller_profile_runtime_take_initial_profile_indication(uint8_t, ControllerProfileRuntimeProfileChangeEvent*) { return false; }
bool controller_profile_runtime_take_profile_change(uint8_t, ControllerProfileRuntimeProfileChangeEvent*) { return false; }
ControllerProfileTransformResult controller_profile_runtime_transform(
    uint8_t, const Bluepad32SlotSnapshot& input, uint32_t, AdapterUsbMode) {
    ControllerProfileTransformResult result{};
    result.state = input.state;
    if (swap_faces) {
        result.state.button_south = input.state.button_east;
        result.state.button_east = input.state.button_south;
    }
    return result;
}

static void put_pair(uint8_t* out, uint16_t x, uint16_t y) {
    out[0] = static_cast<uint8_t>(x);
    out[1] = static_cast<uint8_t>((x >> 8) | (y << 4));
    out[2] = static_cast<uint8_t>(y >> 4);
}
static uint16_t stick_x(const uint8_t* p) { return p[5] | ((p[6] & 15u) << 8); }
static uint16_t stick_y(const uint8_t* p) { return (p[6] >> 4) | (p[7] << 4); }
static int32_t signed32(const uint8_t* p) {
    const uint32_t raw = p[0] | (uint32_t(p[1]) << 8) | (uint32_t(p[2]) << 16) | (uint32_t(p[3]) << 24);
    return raw <= INT32_MAX ? static_cast<int32_t>(raw) : -1 - static_cast<int32_t>(UINT32_MAX - raw);
}
static uint32_t get_bits(const uint8_t* p, unsigned start, unsigned count) {
    uint32_t result = 0;
    for (unsigned i = 0; i < count; ++i) result |= uint32_t((p[(start+i)/8] >> ((start+i)%8)) & 1) << i;
    return result;
}
static void decode_quaternion(const uint8_t* p, double out[4]) {
    const uint8_t* imu = p + 16;
    const unsigned index = get_bits(imu, 32, 3);
    assert(index < 4);
    double ratios[3], norm = 1;
    for (unsigned i = 0; i < 3; ++i) {
        ratios[i] = get_bits(imu, 35 + 31*i, 31) / 1073741824.0 - 1;
        norm += ratios[i] * ratios[i];
    }
    out[index] = 1 / sqrt(norm);
    for (unsigned i = 0; i < 3; ++i) out[(index+i+1)&3] = ratios[i] * out[index];
}
static void publish(bool gyro_fresh = true) {
    source.received_us = time_us_32();
    ++source.state_generation;
    if (time_us_32() - last_sensor_us >= 10000) {
        last_sensor_us = time_us_32();
        ++sensor_sequence;
        source.accel_sequence = sensor_sequence;
        source.accel_received_us = time_us_32();
        if (gyro_fresh) {
            source.gyro_sequence = sensor_sequence;
            source.gyro_received_us = time_us_32();
        }
    }
    wii_ir_pointer_observe(source.slot, source.controller.connection_generation,
                          ++ir_sequence, raw_ir_buttons, ir_x, ir_y, ir_mask, false, 0.0f, true);
}
static uint32_t poll(bool commit = true, bool gyro_fresh = true) {
    now_us += 4000;
    publish(gyro_fresh);
    probe_controller_input_poll(0, to_ms_since_boot(now_us), &controls);
    const uint32_t token = probe_controller_input_peek_native_report(0, to_ms_since_boot(now_us), packet);
    if (commit && token) assert(probe_controller_input_commit_native_report(0, token));
    return token;
}

int main() {
    probe_controller_input_init();
    assert(probe_controller_input_start());
    uint8_t calibration[9];
    put_pair(calibration, 2000, 2100);
    put_pair(calibration+3, 1500, 1400);
    put_pair(calibration+6, 1600, 1700);
    probe_controller_input_set_stick_calibration(calibration);
    probe_controller_input_set_native_features(0x37);
    probe_controller_input_set_native_stream(0, true);
    source.slot = 0;
    source.controller.active = true;
    source.controller.connection_generation = 7;
    source.layout = Bluepad32ControllerLayout::kWiiNunchuk;
    source.controller.state.button_south = true; // Profile maps this to native A.
    source.accel_valid = source.gyro_valid = true;
    source.accel_q13[1] = 8192; // SDL up -> virtual native rail-down +X.
    memcpy(source.gyro_q10, bias_q10, sizeof(bias_q10));
    source.battery = 128;
    assert(poll());
    assert(controls.active && packet[15] == 30); // Wii IMU starts before background bias learning.
    assert(packet[1] == 0x15); // Measured half battery, USB powered, not charging.
    for (unsigned i = 1; i < 450; ++i) assert(poll());
    assert(controls.active && packet[15] == 30 && packet[19] == 0x0c);
    assert(signed32(packet+32) == (1 << 28));
    assert(signed32(packet+36) == 0 && signed32(packet+40) == 0);
    assert(packet[2] == 2 && packet[3] == 0 && packet[13] == 20);
    assert(stick_x(packet) == 2000 && stick_y(packet) == 2100);
    double initial[4]; decode_quaternion(packet, initial);

    raw_ir_buttons = 0x000c; // Physical A+B in pointer telemetry are not click overlays.
    for (unsigned i = 0; i < 8; ++i) assert(poll());
    assert(packet[2] == 2 && (packet[2] & 0x30) == 0);
    source.controller.state.left_stick_x = INT16_MAX;
    source.controller.state.left_stick_y = INT16_MIN;
    assert(poll());
    assert(stick_x(packet) == 3500 && stick_y(packet) == 3500);
    source.controller.state.left_stick_x = source.controller.state.left_stick_y = 0;

    // Native IR displacement must survive failed submission, then be consumed once.
    raw_ir_buttons = 0x0002; // A mapped game button must not clutch native IR movement.
    bool horizontal_motion = false;
    int32_t horizontal_total = 0, horizontal_cross_axis = 0;
    for (unsigned i = 0; i < 24; ++i) {
        ir_x[0] += 2; ir_x[1] += 2;
        assert(poll());
        horizontal_motion = horizontal_motion || packet[9] != 0 || packet[10] != 0;
        const uint16_t x = packet[9] | (uint16_t(packet[10]) << 8);
        const uint16_t y = packet[11] | (uint16_t(packet[12]) << 8);
        horizontal_total += x <= INT16_MAX ? int32_t(x) : int32_t(x) - 65536;
        horizontal_cross_axis += y <= INT16_MAX ? int32_t(y) : int32_t(y) - 65536;
    }
    assert(horizontal_motion);
    assert(horizontal_total < 0);
    assert(horizontal_cross_axis * 4 > horizontal_total && horizontal_cross_axis * 4 < -horizontal_total);
    ir_x[0] += 8; ir_x[1] += 8;
    const uint32_t ticket = poll(false);
    uint8_t retry[63];
    assert(ticket && probe_controller_input_peek_native_report(0, to_ms_since_boot(now_us), retry) == ticket);
    assert(memcmp(packet, retry, sizeof(packet)) == 0);
    assert(probe_controller_input_commit_native_report(0, ticket));
    assert(!probe_controller_input_commit_native_report(0, ticket));

    // Tilting the Wii up moves camera spots down. Native Joy-Con Y must
    // reverse the desktop-pointer convention without changing consumption.
    for (unsigned i = 0; i < 80; ++i) assert(poll());
    int32_t vertical_totals[2]{};
    for (unsigned direction = 0; direction < 2; ++direction) {
        for (unsigned i = 0; i < 104; ++i) {
            if (i < 24) {
                ir_y[0] += direction == 0 ? 2 : -2;
                ir_y[1] += direction == 0 ? 2 : -2;
            }
            assert(poll());
            const uint16_t y = packet[11] | (uint16_t(packet[12]) << 8);
            vertical_totals[direction] += y <= INT16_MAX ? int32_t(y) : int32_t(y) - 65536;
            WiiIrMouseReport pending{};
            (void)wii_ir_mouse_peek(&pending, INT16_MAX);
            assert(pending.dx == 0 && pending.dy == 0);
        }
    }
    assert(vertical_totals[0] > 0 && vertical_totals[1] < 0);
    // Upstream radial smoothing has a positional dead zone, so returning the
    // camera to its origin need not return the filtered pointer exactly there.
    // Per-report checks above instead defend once-only native consumption.

    // A visible, stationary bar must anchor native heading against residual drift.
    for (unsigned i = 0; i < 3000; ++i) assert(poll());
    decode_quaternion(packet, initial);
    source.gyro_q10[1] = bias_q10[1] + 614; // ~0.6dps warming drift about vertical.
    for (unsigned i = 0; i < 5000; ++i) assert(poll());
    double optically_held[4]; decode_quaternion(packet, optically_held);
    double heading_dot = 0;
    for (unsigned i = 0; i < 4; ++i) heading_dot += initial[i] * optically_held[i];
    assert(fabs(heading_dot) > cos(2.0 * acos(-1.0) / 360.0));
    memcpy(source.gyro_q10, bias_q10, sizeof(bias_q10));

    // Gyro-only motion remains available while the sensor bar is out of view.
    ir_mask = 0;
    // Give background correction a fresh quiet window after the simulated cooling.
    for (unsigned i = 0; i < 750; ++i) assert(poll());
    decode_quaternion(packet, initial);
    for (unsigned i = 0; i < 250; ++i) assert(poll());
    double after_bias[4]; decode_quaternion(packet, after_bias);
    double dot = 0; for (unsigned i=0;i<4;++i) dot += initial[i]*after_bias[i];
    assert(fabs(dot) > .99999);
    // SDL yaw -> native mouse-frame +X; one second at90dps rotates90 degrees.
    source.gyro_q10[1] = bias_q10[1] + 90 * 1024;
    for (unsigned i = 0; i < 250; ++i) assert(poll());
    double turned[4]; decode_quaternion(packet, turned);
    dot = 0; for (unsigned i=0;i<4;++i) dot += initial[i]*turned[i];
    assert(fabs(fabs(dot) - sqrt(.5)) < .015);
    memcpy(source.gyro_q10, bias_q10, sizeof(bias_q10));

    ir_mask = 0;
    assert(poll());
    assert(packet[9] == 0 && packet[10] == 0 && packet[11] == 0 && packet[12] == 0 && packet[13] == 0xff);
    ir_x[0] = 500; ir_x[1] = 700; ir_mask = 3;
    assert(poll());
    assert(packet[9] == 0 && packet[10] == 0 && packet[11] == 0 && packet[12] == 0);

    // Fresh controller/accelerometer traffic cannot rejuvenate stale MotionPlus.
    for (unsigned i = 0; i < 42; ++i) assert(poll(true, false));
    assert(controls.active && packet[15] == 0 && packet[13] == 0xff);
    const uint32_t obsolete = poll(false, false);
    ++source.controller.connection_generation;
    assert(poll(false));
    assert(!probe_controller_input_commit_native_report(0, obsolete));
    probe_controller_input_set_native_stream(0, false);
    assert(probe_controller_input_peek_native_report(0, to_ms_since_boot(now_us), retry) == 0);
    probe_controller_input_set_native_stream(0, true);
    publish_during_snapshot = true;
    for (unsigned i = 0; i < 450; ++i) assert(poll());
    assert(packet[15] == 30);
    source.controller.active = false;
    now_us += 4000;
    probe_controller_input_poll(0, to_ms_since_boot(now_us), &controls);
    assert(!controls.active);
    assert(probe_controller_input_peek_native_report(0, to_ms_since_boot(now_us), retry) == 0);
    return 0;
}