#include <assert.h>
#include <stdio.h>
#include <string.h>

#include "parser/uni_hid_parser_ds4.h"
#include "parser/uni_hid_parser_psmove.h"
#include "parser/uni_hid_parser_switch.h"
#include "parser/uni_hid_parser_native_motion.h"
#include "uni_hid_device.h"
#include "uni_utils.h"

// Real staged parsers and normalization. Only radio, clock and platform edges
// are substituted; fixtures enter public setup/feature/input APIs.
static uint8_t command[128];
static unsigned command_len, ready_count;
void uni_log(const char* fmt, ...) { (void)fmt; }
void printf_hexdump(const void* data, int len) { (void)data; (void)len; }
void uni_hid_device_send_ctrl_report(uni_hid_device_t* d, const uint8_t* bytes, uint16_t len) {
    (void)d; assert(len <= sizeof(command)); memcpy(command, bytes, len); command_len = len;
}
void uni_hid_device_send_intr_report(uni_hid_device_t* d, const uint8_t* bytes, uint16_t len) {
    uni_hid_device_send_ctrl_report(d, bytes, len);
}
int l2cap_can_send_packet_now(uint16_t cid) { (void)cid; return 1; }
int l2cap_send(uint16_t cid, uint8_t* bytes, uint16_t len) {
    (void)cid; uni_hid_device_send_intr_report(NULL, bytes, len); return ERROR_CODE_SUCCESS;
}
uint8_t l2cap_request_can_send_now_event(uint16_t cid) { (void)cid; return 0; }
uint32_t btstack_run_loop_get_time_ms(void) { return 0; }
void btstack_run_loop_set_timer(btstack_timer_source_t* t, uint32_t ms) { (void)t; (void)ms; }
void btstack_run_loop_add_timer(btstack_timer_source_t* t) { (void)t; }
bool btstack_run_loop_remove_timer(btstack_timer_source_t* t) { (void)t; return false; }
void btstack_run_loop_set_timer_context(btstack_timer_source_t* t, void* p) { t->context = p; }
void btstack_run_loop_set_timer_handler(btstack_timer_source_t* t, void (*fn)(btstack_timer_source_t*)) { t->process = fn; }
void* btstack_run_loop_get_timer_context(btstack_timer_source_t* t) { return t->context; }
bool uni_hid_device_set_ready_complete(uni_hid_device_t* d) { (void)d; ++ready_count; return true; }
uni_hid_device_t* uni_hid_device_create_virtual(uni_hid_device_t* d) { (void)d; return NULL; }
void uni_hid_device_set_cod(uni_hid_device_t* d, uint32_t cod) { (void)d; (void)cod; }
void uni_hid_device_connect(uni_hid_device_t* d) { (void)d; }
void uni_hid_device_process_controller(uni_hid_device_t* d) { (void)d; }
void uni_hid_device_set_product_id(uni_hid_device_t* d, uint16_t pid) { d->product_id = pid; }
void uni_hid_device_set_vendor_id(uni_hid_device_t* d, uint16_t vid) { d->vendor_id = vid; }
uint8_t uni_hid_parser_hat_to_dpad(uint8_t hat) { return hat == 0 ? DPAD_UP : 0; }
void uni_hid_parser_wii_setup(uni_hid_device_t* d) { (void)d; assert(false); }
bool uni_hid_parser_wii_accel_snapshot(uni_hid_device_t* d, int32_t v[3], uint32_t* s) {
    (void)d; (void)v; (void)s; assert(false); return false;
}
bool uni_hid_parser_wii_gyro_snapshot(uni_hid_device_t* d, int32_t v[3], uint32_t* s) {
    (void)d; (void)v; (void)s; assert(false); return false;
}
static void put16(uint8_t* bytes, int value) {
    bytes[0] = (uint16_t)value; bytes[1] = (uint16_t)value >> 8;
}
static uni_native_motion_snapshot_t snapshot(uni_hid_device_t* d) {
    uni_native_motion_snapshot_t result;
    assert(uni_hid_parser_native_motion_snapshot(d, &result));
    return result;
}
static void ds4_seal(uint8_t bytes[78]) {
    const uint8_t transaction = 0xa1;
    uint32_t crc = ~uni_crc32_le(uni_crc32_le(UINT32_MAX, &transaction, 1), bytes, 74);
    for (unsigned i = 0; i < 4; ++i) bytes[74 + i] = crc >> (8 * i);
}
static void ds4_input(uni_hid_device_t* d, uint8_t bytes[78], uint16_t tick) {
    put16(bytes + 12, tick);
    ds4_seal(bytes);
    uni_hid_parser_ds4_init_report(d);
    uni_hid_parser_ds4_parse_input_report(d, bytes, 78);
}
static void ds4_provenance(void) {
    uni_hid_device_t d = {0};
    d.report_parser.setup = uni_hid_parser_ds4_setup;
    uni_hid_parser_ds4_setup(&d);
    uint8_t bytes[78] = {0x11};
    bytes[7] = 0x28; // Cross, neutral hat.
    put16(bytes + 19, 8192); // Gyro Z.
    put16(bytes + 23, 8192); // Acceleration Y.
    ds4_input(&d, bytes, 65530);
    uni_native_motion_snapshot_t first = snapshot(&d);
    assert(first.report_valid && !first.accel_valid && !first.gyro_valid);
    assert(d.controller.gamepad.buttons & BUTTON_A);
    uint8_t calibration[37] = {2};
    for (unsigned i = 0; i < 3; ++i) {
        put16(calibration + 7 + 2 * i, 100);
        put16(calibration + 13 + 2 * i, -100);
        put16(calibration + 23 + 4 * i, 8192);
        put16(calibration + 25 + 4 * i, -8192);
    }
    put16(calibration + 19, 100); put16(calibration + 21, 100);
    uni_hid_parser_ds4_parse_feature_report(&d, calibration, 36);
    ds4_input(&d, bytes, 65531);
    assert(!snapshot(&d).accel_valid);
    uni_hid_parser_ds4_parse_feature_report(&d, calibration, 37);
    ds4_input(&d, bytes, 65531);
    assert(!snapshot(&d).accel_valid); // Calibration cannot rejuvenate a cached tick.
    ds4_input(&d, bytes, 2); // Wrapped sensor clock is forward progress.
    first = snapshot(&d);
    assert(first.accel_valid && first.gyro_valid && first.accel_q13[1] == 8192);
    ds4_input(&d, bytes, 2);
    assert(snapshot(&d).accel_sequence == first.accel_sequence);
    ds4_input(&d, bytes, 1);
    assert(snapshot(&d).gyro_sequence == first.gyro_sequence);
    bytes[7] ^= 0x20; // Corrupt input must not be admitted as button release.
    uni_hid_parser_ds4_parse_input_report(&d, bytes, 78);
    assert(!snapshot(&d).report_valid);
    assert(snapshot(&d).accel_sequence == first.accel_sequence);
    uint8_t buttons[10] = {1};
    uni_hid_parser_ds4_init_report(&d);
    uni_hid_parser_ds4_parse_input_report(&d, buttons, sizeof(buttons));
    assert(snapshot(&d).report_valid && snapshot(&d).accel_sequence == first.accel_sequence);
    // Valid accel calibration with degenerate gyro extrema is accel-only.
    memset(calibration + 7, 0, 12);
    uni_hid_parser_ds4_parse_feature_report(&d, calibration, 37);
    ds4_input(&d, bytes, 3);
    assert(snapshot(&d).accel_valid && !snapshot(&d).gyro_valid);
    uni_hid_parser_native_motion_forget(&d);
    uni_hid_parser_ds4_setup(&d); // Same pointer, new parser lifetime.
    assert(!snapshot(&d).report_valid && !snapshot(&d).accel_valid);
    uni_hid_parser_ds4_parse_feature_report(&d, calibration, 37);
    ds4_input(&d, bytes, 3);
    assert(snapshot(&d).accel_sequence != first.accel_sequence);
    uni_hid_parser_native_motion_forget(&d);
}

static void move_calibration(uni_hid_device_t* d, bool zcm2, bool valid) {
    uint8_t blob[143] = {0};
    const uint8_t lo1[] = {0x0a, 0x24, 0x14}, hi1[] = {0x16, 0x1e, 0x08};
    const uint8_t lo2[] = {0x08, 0x16, 0x24}, hi2[] = {0x02, 0x10, 0x1e};
    const uint8_t bias1[] = {0x2a, 0x2c, 0x2e}, high1[] = {0x46, 0x50, 0x5a};
    const uint8_t bias2[] = {0x26, 0x28, 0x2a}, high2[] = {0x30, 0x38, 0x40}, low2[] = {0x42, 0x4a, 0x52};
    for (unsigned i = 0; i < 3; ++i) {
        int center = zcm2 ? 0 : 0x8000;
        put16(blob + (zcm2 ? lo2[i] : lo1[i]), center - (valid ? 1000 : 0));
        put16(blob + (zcm2 ? hi2[i] : hi1[i]), center + (valid ? 1000 : 0));
        put16(blob + (zcm2 ? bias2[i] : bias1[i]), center);
        put16(blob + (zcm2 ? high2[i] : high1[i]), center + 1000);
        if (zcm2) put16(blob + low2[i], -1000);
    }
    blob[0] = 0x10;
    uni_hid_parser_psmove_parse_feature_report(d, blob, 49);
    uint8_t continuation[49] = {0x10, zcm2 ? 0x81 : 1};
    memcpy(continuation + 2, blob + 49, 47);
    uni_hid_parser_psmove_parse_feature_report(d, continuation, 49);
    if (!zcm2) {
        continuation[1] = 0x82;
        memcpy(continuation + 2, blob + 96, 47);
        uni_hid_parser_psmove_parse_feature_report(d, continuation, 49);
    }
}
static void move_provenance(bool zcm2) {
    uni_hid_device_t d = {0};
    d.report_parser.setup = uni_hid_parser_psmove_setup;
    d.product_id = zcm2 ? 0x0c5e : 0x03d5;
    uni_hid_parser_psmove_setup(&d);
    uint8_t report[49] = {1};
    report[2] = 0x40; // Cross.
    report[43] = 1;
    for (unsigned i = 0; i < 12; ++i)
        put16(report + 13 + 2 * i, (zcm2 ? 0 : 0x8000) + 1000);
    uni_hid_parser_psmove_parse_input_report(&d, report, sizeof(report));
    assert(snapshot(&d).report_valid && !snapshot(&d).accel_valid);
    move_calibration(&d, zcm2, false);
    report[43] = 2;
    uni_hid_parser_psmove_parse_input_report(&d, report, sizeof(report));
    assert(!snapshot(&d).accel_valid && !snapshot(&d).gyro_valid);
    move_calibration(&d, zcm2, true);
    uni_hid_parser_psmove_parse_input_report(&d, report, sizeof(report));
    assert(!snapshot(&d).accel_valid);
    report[43] = 3;
    uni_hid_parser_psmove_parse_input_report(&d, report, sizeof(report));
    uni_native_motion_snapshot_t first = snapshot(&d);
    assert(first.accel_valid && first.gyro_valid && first.accel_q13[0] == 8192);
    assert(first.gyro_q10[0] == (zcm2 ? 540 : 480) * 1024);
    uni_hid_parser_psmove_init_report(&d);
    uni_hid_parser_psmove_parse_input_report(&d, report, sizeof(report));
    assert(snapshot(&d).gyro_sequence == first.gyro_sequence);
    report[43] = 2;
    uni_hid_parser_psmove_parse_input_report(&d, report, sizeof(report));
    assert(snapshot(&d).accel_sequence == first.accel_sequence);
    uni_hid_parser_psmove_parse_input_report(&d, report, 43);
    assert(!snapshot(&d).report_valid && snapshot(&d).gyro_sequence == first.gyro_sequence);
    uni_hid_parser_native_motion_forget(&d);
}

static void switch_setup(uni_hid_device_t* d, uint8_t type, bool calibrated) {
    memset(d, 0, sizeof(*d));
    d->report_parser.setup = uni_hid_parser_switch_setup;
    d->conn.interrupt_cid = 0x40;
    unsigned ready_before = ready_count;
    uni_hid_parser_switch_setup(d);
    for (unsigned step = 0; ready_count == ready_before && step < 12; ++step) {
        assert(command_len >= 12);
        uint8_t reply[49] = {0x21};
        reply[13] = 0x80;
        reply[14] = command[11];
        if (reply[14] == 2) reply[17] = type;
        if (reply[14] == 0x10) {
            memcpy(reply + 15, command + 12, 5);
            // Centered sticks, nonzero spans; no user calibration magic.
            if (little_endian_read_32(reply, 15) == 0x603d || little_endian_read_32(reply, 15) == 0x6046)
                memset(reply + 20, 0x80, reply[19]);
            if (little_endian_read_32(reply, 15) == 0x6020) {
                for (unsigned i = 0; i < 3; ++i) {
                    put16(reply + 26 + 2 * i, calibrated ? 16384 : 0);
                    put16(reply + 38 + 2 * i, calibrated ? 13371 : 0);
                }
                // Truncation with a convincing declared length cannot certify sensors.
                uni_hid_parser_switch_parse_input_report(d, reply, 22);
                assert(!snapshot(d).accel_valid);
            }
        }
        uni_hid_parser_switch_parse_input_report(d, reply, sizeof(reply));
    }
    assert(ready_count != ready_before);
}
static void switch_provenance(uint8_t type, bool calibrated) {
    uni_hid_device_t d;
    switch_setup(&d, type, calibrated);
    uint8_t report[49] = {0x30, 254};
    memset(report + 6, 0x80, 6);
    put16(report + 37, 4096); // Latest accelerometer sample, native X.
    put16(report + 43, 1000); // Latest gyro sample, native X.
    uni_hid_parser_switch_parse_input_report(&d, report, sizeof(report));
    uni_native_motion_snapshot_t first = snapshot(&d);
    assert(first.report_tracked && first.report_valid);
    if (type == 0x0b || !calibrated) {
        assert(!first.accel_valid && !first.gyro_valid);
    } else {
        assert(first.accel_valid && first.gyro_valid && first.accel_q13[2] == -8192);
        uni_hid_parser_switch_parse_input_report(&d, report, sizeof(report));
        assert(snapshot(&d).accel_sequence == first.accel_sequence);
        report[1] = 1;
        uni_hid_parser_switch_parse_input_report(&d, report, sizeof(report));
        assert(snapshot(&d).gyro_sequence != first.gyro_sequence);
        first = snapshot(&d);
        report[1] = 0;
        uni_hid_parser_switch_parse_input_report(&d, report, sizeof(report));
        assert(snapshot(&d).gyro_sequence == first.gyro_sequence);
    }
    uni_hid_parser_switch_parse_input_report(&d, report, 48);
    assert(!snapshot(&d).report_valid && snapshot(&d).accel_sequence == first.accel_sequence);
    uint8_t buttons[12] = {0x3f};
    uni_hid_parser_switch_parse_input_report(&d, buttons, sizeof(buttons));
    assert(snapshot(&d).report_valid && snapshot(&d).accel_sequence == first.accel_sequence);
    uni_hid_parser_switch_teardown(&d);
    assert(!uni_hid_parser_native_motion_snapshot(&d, &first));
}
int main(void) {
    ds4_provenance();
    move_provenance(false);
    move_provenance(true);
    switch_provenance(3, true);
    switch_provenance(1, true);
    switch_provenance(2, true);
    switch_provenance(3, false);
    switch_provenance(0x0b, false);
    uni_hid_device_t sensorless = {0};
    uni_native_motion_snapshot_t absent;
    memset(&absent, 0xff, sizeof(absent));
    assert(!uni_hid_parser_native_motion_snapshot(&sensorless, &absent));
    assert(!absent.report_tracked && !absent.accel_valid && !absent.gyro_valid && absent.gyro_sequence == 0);
    puts("Native DS4, Move, Switch motion provenance and sensorless absence passed");
    return 0;
}