#include "switch_uart_protocol.h"

namespace {

SwitchInputState make_neutral_state() {
    SwitchInputState state{};
    state.lx = SWITCH_PRO_JOYSTICK_MID;
    state.ly = SWITCH_PRO_JOYSTICK_MID;
    state.rx = SWITCH_PRO_JOYSTICK_MID;
    state.ry = SWITCH_PRO_JOYSTICK_MID;
    state.imu_sample_count = 0;
    return state;
}

}  // namespace

bool switch_uart_decode_input_frame(
    const uint8_t* packet,
    uint8_t length,
    SwitchInputState* out_state) {
    if (length < 12) {
        return false;
    }
    if (packet[0] != 0xAA) {
        return false;
    }
    if (packet[1] != 0x02) {
        return false;
    }

    const uint8_t payload_len = packet[2];
    if (static_cast<uint16_t>(payload_len) + 4u != length) {
        return false;
    }

    uint16_t sum = 0;
    for (uint16_t index = 0; index < static_cast<uint16_t>(3u + payload_len); ++index) {
        sum += packet[index];
    }
    if ((sum & 0xFF) != packet[length - 1]) {
        return false;
    }

    if (payload_len < 8) {
        return false;
    }

    const uint16_t buttons = static_cast<uint16_t>(packet[3]) |
                             (static_cast<uint16_t>(packet[4]) << 8);
    const uint8_t hat = packet[5];
    const uint8_t lx = packet[6];
    const uint8_t ly = packet[7];
    const uint8_t rx = packet[8];
    const uint8_t ry = packet[9];
    uint8_t imu_count = packet[10];
    if (imu_count > 3) {
        imu_count = 3;
    }

    const uint16_t required_payload_len =
        static_cast<uint16_t>(8u + static_cast<uint16_t>(imu_count) * 12u);
    if (payload_len < required_payload_len) {
        return false;
    }

    const auto expand_axis = [](uint8_t value) -> uint16_t {
        return static_cast<uint16_t>(value) << 8 | value;
    };
    const auto read_int16 = [](const uint8_t* source) -> int16_t {
        return static_cast<int16_t>(
            static_cast<uint16_t>(source[0]) |
            (static_cast<uint16_t>(source[1]) << 8));
    };

    SwitchInputState state = make_neutral_state();
    state.imu_sample_count = imu_count;
    for (uint8_t index = 0; index < imu_count; ++index) {
        const uint8_t* base = &packet[11 + index * 12];
        state.imu_samples[index].accel_x = read_int16(base);
        state.imu_samples[index].accel_y = read_int16(base + 2);
        state.imu_samples[index].accel_z = read_int16(base + 4);
        state.imu_samples[index].gyro_x = read_int16(base + 6);
        state.imu_samples[index].gyro_y = read_int16(base + 8);
        state.imu_samples[index].gyro_z = read_int16(base + 10);
    }

    switch (hat) {
        case SWITCH_PRO_HAT_UP: state.dpad_up = true; break;
        case SWITCH_PRO_HAT_UPRIGHT: state.dpad_up = true; state.dpad_right = true; break;
        case SWITCH_PRO_HAT_RIGHT: state.dpad_right = true; break;
        case SWITCH_PRO_HAT_DOWNRIGHT: state.dpad_down = true; state.dpad_right = true; break;
        case SWITCH_PRO_HAT_DOWN: state.dpad_down = true; break;
        case SWITCH_PRO_HAT_DOWNLEFT: state.dpad_down = true; state.dpad_left = true; break;
        case SWITCH_PRO_HAT_LEFT: state.dpad_left = true; break;
        case SWITCH_PRO_HAT_UPLEFT: state.dpad_up = true; state.dpad_left = true; break;
        default: break;
    }

    state.button_y = buttons & SWITCH_PRO_MASK_Y;
    state.button_x = buttons & SWITCH_PRO_MASK_X;
    state.button_b = buttons & SWITCH_PRO_MASK_B;
    state.button_a = buttons & SWITCH_PRO_MASK_A;
    state.button_r = buttons & SWITCH_PRO_MASK_R;
    state.button_zr = buttons & SWITCH_PRO_MASK_ZR;
    state.button_plus = buttons & SWITCH_PRO_MASK_PLUS;
    state.button_minus = buttons & SWITCH_PRO_MASK_MINUS;
    state.button_r3 = buttons & SWITCH_PRO_MASK_R3;
    state.button_l3 = buttons & SWITCH_PRO_MASK_L3;
    state.button_home = buttons & SWITCH_PRO_MASK_HOME;
    state.button_capture = buttons & SWITCH_PRO_MASK_CAPTURE;
    state.button_zl = buttons & SWITCH_PRO_MASK_ZL;
    state.button_l = buttons & SWITCH_PRO_MASK_L;

    state.lx = expand_axis(lx);
    state.ly = expand_axis(ly);
    state.rx = expand_axis(rx);
    state.ry = expand_axis(ry);

    if (!out_state) {
        return false;
    }
    *out_state = state;
    return true;
}