#include "switch2_reports.h"

// Report layouts are pinned to:
// https://github.com/ndeadly/switch2_controller_research/blob/d1c5a7f7ba298f83017fae84952a4e6d2ef8fc92/hid_reports.md
namespace {

void pack_stick(uint8_t* destination, uint16_t x, uint16_t y) {
    const uint16_t packed_x = x >> 4;
    const uint16_t packed_y = y >> 4;
    destination[0] = static_cast<uint8_t>(packed_x & 0xFF);
    destination[1] = static_cast<uint8_t>(
        ((packed_x >> 8) & 0x0F) | ((packed_y & 0x0F) << 4));
    destination[2] = static_cast<uint8_t>(packed_y >> 4);
}

}  // namespace

Switch2InputReport switch2_build_input_report(
    Switch2InputReportId id,
    const SwitchInputState& state,
    uint32_t counter) {
    Switch2InputReport report{id, {}};
    std::array<uint8_t, 63>& payload = report.payload;

    switch (id) {
        case Switch2InputReportId::Common:
            payload[0] = static_cast<uint8_t>(counter);
            payload[1] = static_cast<uint8_t>(counter >> 8);
            payload[2] = static_cast<uint8_t>(counter >> 16);
            payload[3] = static_cast<uint8_t>(counter >> 24);
            payload[4] = static_cast<uint8_t>(
                (state.button_zr ? 0x80 : 0) |
                (state.button_r ? 0x40 : 0) |
                (state.button_a ? 0x08 : 0) |
                (state.button_b ? 0x04 : 0) |
                (state.button_x ? 0x02 : 0) |
                (state.button_y ? 0x01 : 0));
            payload[5] = static_cast<uint8_t>(
                (state.button_capture ? 0x20 : 0) |
                (state.button_home ? 0x10 : 0) |
                (state.button_l3 ? 0x08 : 0) |
                (state.button_r3 ? 0x04 : 0) |
                (state.button_plus ? 0x02 : 0) |
                (state.button_minus ? 0x01 : 0));
            payload[6] = static_cast<uint8_t>(
                (state.button_zl ? 0x80 : 0) |
                (state.button_l ? 0x40 : 0) |
                (state.dpad_left ? 0x08 : 0) |
                (state.dpad_right ? 0x04 : 0) |
                (state.dpad_up ? 0x02 : 0) |
                (state.dpad_down ? 0x01 : 0));
            pack_stick(&payload[10], state.lx, state.ly);
            pack_stick(&payload[13], state.rx, state.ry);
            payload[0x29] = 0x01;
            break;

        case Switch2InputReportId::Pro:
            payload[0] = static_cast<uint8_t>(counter);
            // USB external power is known; charging and battery bits stay zero.
            payload[1] = 0x01;
            payload[2] = static_cast<uint8_t>(
                (state.button_r3 ? 0x80 : 0) |
                (state.button_plus ? 0x40 : 0) |
                (state.button_zr ? 0x20 : 0) |
                (state.button_r ? 0x10 : 0) |
                (state.button_x ? 0x08 : 0) |
                (state.button_y ? 0x04 : 0) |
                (state.button_a ? 0x02 : 0) |
                (state.button_b ? 0x01 : 0));
            payload[3] = static_cast<uint8_t>(
                (state.button_l3 ? 0x80 : 0) |
                (state.button_minus ? 0x40 : 0) |
                (state.button_zl ? 0x20 : 0) |
                (state.button_l ? 0x10 : 0) |
                (state.dpad_up ? 0x08 : 0) |
                (state.dpad_left ? 0x04 : 0) |
                (state.dpad_right ? 0x02 : 0) |
                (state.dpad_down ? 0x01 : 0));
            payload[4] = static_cast<uint8_t>(
                (state.button_capture ? 0x02 : 0) |
                (state.button_home ? 0x01 : 0));
            pack_stick(&payload[5], state.lx, state.ly);
            pack_stick(&payload[8], state.rx, state.ry);
            payload[11] = 0x30;
            break;
    }

    return report;
}