#define PY_SSIZE_T_CLEAN
#include <Python.h>
#include <array>
#include <cmath>
#include <cstdio>
#include <cstdint>
#include <cstring>
#include <fstream>
#include <string>
#include <vector>

namespace {

constexpr Py_ssize_t LOWCMD_SIZE = 616;
constexpr Py_ssize_t CRC_OFFSET = 612;
constexpr Py_ssize_t MOTOR_OFFSET = 22;
constexpr Py_ssize_t MOTOR_SIZE = 27;
constexpr int MOTOR_COUNT = 20;
constexpr int SERVO_COUNT = 12;
constexpr uint32_t CRC_POLY = 0x04c11db7u;
constexpr double TAU_MAX_SERVO[SERVO_COUNT] = {
    23.7, 23.7, 35.55,
    23.7, 23.7, 35.55,
    23.7, 23.7, 35.55,
    23.7, 23.7, 35.55,
};
constexpr double JOINT_LIMIT_LO[SERVO_COUNT] = {
    -0.78, -0.60, -2.70,
    -0.78, -0.60, -2.70,
    -0.78, -0.60, -2.70,
    -0.78, -0.60, -2.70,
};
constexpr double JOINT_LIMIT_HI[SERVO_COUNT] = {
    0.78, 3.50, -0.95,
    0.78, 3.50, -0.95,
    0.78, 3.50, -0.95,
    0.78, 3.50, -0.95,
};
constexpr const char* JOINT_NAMES_SERVO[SERVO_COUNT] = {
    "FR_0", "FR_1", "FR_2",
    "FL_0", "FL_1", "FL_2",
    "RR_0", "RR_1", "RR_2",
    "RL_0", "RL_1", "RL_2",
};

std::array<uint32_t, 256> make_crc_table() {
    std::array<uint32_t, 256> table{};
    for (uint32_t byte = 0; byte < 256; ++byte) {
        uint32_t crc = byte << 24;
        for (int i = 0; i < 8; ++i) {
            if (crc & 0x80000000u) {
                crc = (crc << 1) ^ CRC_POLY;
            } else {
                crc <<= 1;
            }
        }
        table[byte] = crc;
    }
    return table;
}

const std::array<uint32_t, 256> CRC_TABLE = make_crc_table();

inline void put_u16_le(uint8_t* p, uint16_t v) {
    p[0] = static_cast<uint8_t>(v & 0xffu);
    p[1] = static_cast<uint8_t>((v >> 8) & 0xffu);
}

inline void put_u32_le(uint8_t* p, uint32_t v) {
    p[0] = static_cast<uint8_t>(v & 0xffu);
    p[1] = static_cast<uint8_t>((v >> 8) & 0xffu);
    p[2] = static_cast<uint8_t>((v >> 16) & 0xffu);
    p[3] = static_cast<uint8_t>((v >> 24) & 0xffu);
}

inline uint32_t get_u32_le(const uint8_t* p) {
    return (static_cast<uint32_t>(p[0]))
        | (static_cast<uint32_t>(p[1]) << 8)
        | (static_cast<uint32_t>(p[2]) << 16)
        | (static_cast<uint32_t>(p[3]) << 24);
}

inline void put_f32_le(uint8_t* p, double value) {
    float f = static_cast<float>(value);
    uint32_t raw;
    static_assert(sizeof(raw) == sizeof(f), "float must be 32-bit");
    std::memcpy(&raw, &f, sizeof(raw));
    put_u32_le(p, raw);
}

uint32_t crc32_unitree(const uint8_t* data, Py_ssize_t len) {
    uint32_t crc = 0xffffffffu;
    Py_ssize_t words = len / 4;
    for (Py_ssize_t i = 0; i < words; ++i) {
        uint32_t word = get_u32_le(data + i * 4);
        crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ ((word >> 24) & 0xffu)) & 0xffu];
        crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ ((word >> 16) & 0xffu)) & 0xffu];
        crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ ((word >> 8) & 0xffu)) & 0xffu];
        crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ (word & 0xffu)) & 0xffu];
    }
    return crc;
}

double round_half_even(double value, double scale) {
    double scaled = value * scale;
    double floor_v = std::floor(scaled);
    double frac = scaled - floor_v;
    double rounded;
    constexpr double eps = 1e-12;
    if (frac > 0.5 + eps) {
        rounded = floor_v + 1.0;
    } else if (frac < 0.5 - eps) {
        rounded = floor_v;
    } else {
        rounded = (std::fmod(floor_v, 2.0) == 0.0) ? floor_v : floor_v + 1.0;
    }
    return rounded / scale;
}

void encode_tau(uint8_t* p, double tau) {
    tau = round_half_even(tau, 100.0);
    int integer_part = static_cast<int>(tau);
    double fractional_part = tau - static_cast<double>(integer_part);
    bool neg = false;
    if (tau < 0.0) {
        neg = true;
        integer_part = 255 + integer_part;
    }
    if (fractional_part == 0.0) {
        neg = false;
    }
    int hex_value = static_cast<int>(fractional_part * 256.0);
    if (neg) {
        hex_value = 255 + hex_value + 1;
    }
    p[0] = static_cast<uint8_t>(hex_value & 0xff);
    p[1] = static_cast<uint8_t>(integer_part & 0xff);
}

void encode_kp(uint8_t* p, double kp) {
    double base_d = std::floor(kp);
    int base = static_cast<int>(base_d);
    double frac_d = round_half_even(kp - base_d, 10.0);
    int frac = static_cast<int>(frac_d * 10.0);
    int val;
    if (frac < 5) {
        val = base * 32 + frac * 3;
    } else {
        val = base * 32 + (frac - 1) * 3 + 4;
    }
    put_u16_le(p, static_cast<uint16_t>(val));
}

void encode_kd(uint8_t* p, double kd) {
    int integer_part = static_cast<int>(kd);
    double fractional_part = round_half_even(kd - integer_part, 10.0);
    int frac = static_cast<int>(fractional_part * 10.0);
    uint8_t nibble = 0;
    switch (frac) {
        case 0: nibble = 0x0; break;
        case 1: nibble = 0x1; break;
        case 2: nibble = 0x3; break;
        case 3: nibble = 0x4; break;
        case 4: nibble = 0x6; break;
        case 5: nibble = 0x8; break;
        case 6: nibble = 0x9; break;
        case 7: nibble = 0xb; break;
        case 8: nibble = 0xc; break;
        case 9: nibble = 0xe; break;
        default: nibble = 0x0; break;
    }
    put_u16_le(p, static_cast<uint16_t>((integer_part << 4) | nibble));
}

void encode_motor(uint8_t* p, int mode, double q, double dq, double tau, double kp, double kd) {
    p[0] = static_cast<uint8_t>(mode & 0xff);
    put_f32_le(p + 1, q);
    put_f32_le(p + 5, dq);
    encode_tau(p + 9, tau);
    encode_kp(p + 11, kp);
    encode_kd(p + 13, kd);
    // reserve[0..2] are already zeroed by the packet initializer.
}

void encode_motor_with_reserve(
        uint8_t* p, int mode, double q, double dq, double tau, double kp, double kd,
        uint32_t reserve0, uint32_t reserve1, uint32_t reserve2) {
    encode_motor(p, mode, q, dq, tau, kp, kd);
    put_u32_le(p + 15, reserve0);
    put_u32_le(p + 19, reserve1);
    put_u32_le(p + 23, reserve2);
}

void init_lowcmd_header(uint8_t* cmd) {
    std::memset(cmd, 0, LOWCMD_SIZE);
    cmd[0] = 0xfe;
    cmd[1] = 0xef;
    cmd[2] = 0xff;
    cmd[20] = 0x3a;
    cmd[21] = 0xc0;
}

void finish_crc(uint8_t* cmd) {
    uint32_t crc = crc32_unitree(cmd, CRC_OFFSET);
    put_u32_le(cmd + CRC_OFFSET, crc);
}

bool read_double_sequence(PyObject* obj, double* out, int count, double default_value, const char* name) {
    for (int i = 0; i < count; ++i) {
        out[i] = default_value;
    }
    if (obj == nullptr || obj == Py_None) {
        return true;
    }
    if (PyNumber_Check(obj) && !PySequence_Check(obj)) {
        double v = PyFloat_AsDouble(obj);
        if (PyErr_Occurred()) {
            return false;
        }
        for (int i = 0; i < count; ++i) {
            out[i] = v;
        }
        return true;
    }
    PyObject* seq = PySequence_Fast(obj, name);
    if (!seq) {
        return false;
    }
    Py_ssize_t n = PySequence_Fast_GET_SIZE(seq);
    if (n < count) {
        Py_DECREF(seq);
        PyErr_Format(PyExc_ValueError, "%s must contain at least %d values", name, count);
        return false;
    }
    PyObject** items = PySequence_Fast_ITEMS(seq);
    for (int i = 0; i < count; ++i) {
        out[i] = PyFloat_AsDouble(items[i]);
        if (PyErr_Occurred()) {
            Py_DECREF(seq);
            return false;
        }
    }
    Py_DECREF(seq);
    return true;
}

bool read_int_sequence(PyObject* obj, int* out, int count, int default_value, const char* name) {
    for (int i = 0; i < count; ++i) {
        out[i] = default_value;
    }
    if (obj == nullptr || obj == Py_None) {
        return true;
    }
    if (PyLong_Check(obj)) {
        long v = PyLong_AsLong(obj);
        if (PyErr_Occurred()) {
            return false;
        }
        for (int i = 0; i < count; ++i) {
            out[i] = static_cast<int>(v);
        }
        return true;
    }
    PyObject* seq = PySequence_Fast(obj, name);
    if (!seq) {
        return false;
    }
    Py_ssize_t n = PySequence_Fast_GET_SIZE(seq);
    if (n < count) {
        Py_DECREF(seq);
        PyErr_Format(PyExc_ValueError, "%s must contain at least %d values", name, count);
        return false;
    }
    PyObject** items = PySequence_Fast_ITEMS(seq);
    for (int i = 0; i < count; ++i) {
        long v = PyLong_AsLong(items[i]);
        if (PyErr_Occurred()) {
            Py_DECREF(seq);
            return false;
        }
        out[i] = static_cast<int>(v);
    }
    Py_DECREF(seq);
    return true;
}

bool get_int_attr(PyObject* obj, const char* name, int* out) {
    PyObject* value = PyObject_GetAttrString(obj, name);
    if (!value) {
        return false;
    }
    long v = PyLong_AsLong(value);
    Py_DECREF(value);
    if (PyErr_Occurred()) {
        return false;
    }
    *out = static_cast<int>(v);
    return true;
}

bool get_double_attr(PyObject* obj, const char* name, double* out) {
    PyObject* value = PyObject_GetAttrString(obj, name);
    if (!value) {
        return false;
    }
    double v = PyFloat_AsDouble(value);
    Py_DECREF(value);
    if (PyErr_Occurred()) {
        return false;
    }
    *out = v;
    return true;
}

bool copy_bytes_attr(PyObject* obj, const char* name, uint8_t* out, Py_ssize_t expected_len) {
    PyObject* value = PyObject_GetAttrString(obj, name);
    if (!value) {
        return false;
    }
    char* data = nullptr;
    Py_ssize_t len = 0;
    if (PyBytes_AsStringAndSize(value, &data, &len) < 0) {
        Py_DECREF(value);
        return false;
    }
    if (len != expected_len) {
        Py_DECREF(value);
        PyErr_Format(PyExc_ValueError, "%s must be %zd bytes", name, expected_len);
        return false;
    }
    std::memcpy(out, data, static_cast<size_t>(expected_len));
    Py_DECREF(value);
    return true;
}

bool read_reserve3(PyObject* motor, uint32_t reserve[3]) {
    reserve[0] = reserve[1] = reserve[2] = 0;
    PyObject* value = PyObject_GetAttrString(motor, "reserve");
    if (!value) {
        return false;
    }
    PyObject* seq = PySequence_Fast(value, "motor.reserve must be a sequence");
    Py_DECREF(value);
    if (!seq) {
        return false;
    }
    Py_ssize_t n = PySequence_Fast_GET_SIZE(seq);
    if (n < 3) {
        Py_DECREF(seq);
        PyErr_SetString(PyExc_ValueError, "motor.reserve must contain at least 3 values");
        return false;
    }
    PyObject** items = PySequence_Fast_ITEMS(seq);
    for (int i = 0; i < 3; ++i) {
        unsigned long v = PyLong_AsUnsignedLong(items[i]);
        if (PyErr_Occurred()) {
            Py_DECREF(seq);
            return false;
        }
        reserve[i] = static_cast<uint32_t>(v);
    }
    Py_DECREF(seq);
    return true;
}

PyObject* build_plain_from_lowcmd_obj(PyObject* lowcmd) {
    PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
    if (!result) {
        return nullptr;
    }
    auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
    init_lowcmd_header(cmd);

    int value = 0;
    if (!copy_bytes_attr(lowcmd, "head", cmd, 2)) {
        Py_DECREF(result);
        return nullptr;
    }
    if (!get_int_attr(lowcmd, "levelFlag", &value)) {
        Py_DECREF(result);
        return nullptr;
    }
    cmd[2] = static_cast<uint8_t>(value & 0xff);
    if (!get_int_attr(lowcmd, "frameReserve", &value)) {
        Py_DECREF(result);
        return nullptr;
    }
    cmd[3] = static_cast<uint8_t>(value & 0xff);
    if (!copy_bytes_attr(lowcmd, "SN", cmd + 4, 8)) {
        Py_DECREF(result);
        return nullptr;
    }
    if (!copy_bytes_attr(lowcmd, "version", cmd + 12, 8)) {
        Py_DECREF(result);
        return nullptr;
    }
    if (!get_int_attr(lowcmd, "bandWidth", &value)) {
        Py_DECREF(result);
        return nullptr;
    }
    cmd[20] = static_cast<uint8_t>((value >> 8) & 0xff);
    cmd[21] = static_cast<uint8_t>(value & 0xff);

    PyObject* motors = PyObject_GetAttrString(lowcmd, "motorCmd");
    if (!motors) {
        Py_DECREF(result);
        return nullptr;
    }
    PyObject* seq = PySequence_Fast(motors, "lowcmd.motorCmd must be a sequence");
    Py_DECREF(motors);
    if (!seq) {
        Py_DECREF(result);
        return nullptr;
    }
    if (PySequence_Fast_GET_SIZE(seq) < MOTOR_COUNT) {
        Py_DECREF(seq);
        Py_DECREF(result);
        PyErr_SetString(PyExc_ValueError, "lowcmd.motorCmd must contain at least 20 motors");
        return nullptr;
    }
    PyObject** items = PySequence_Fast_ITEMS(seq);
    for (int i = 0; i < MOTOR_COUNT; ++i) {
        PyObject* motor = items[i];
        int mode = 0;
        double q = 0.0, dq = 0.0, tau = 0.0, kp = 0.0, kd = 0.0;
        uint32_t reserve[3];
        if (!get_int_attr(motor, "mode", &mode)
            || !get_double_attr(motor, "q", &q)
            || !get_double_attr(motor, "dq", &dq)
            || !get_double_attr(motor, "tau", &tau)
            || !get_double_attr(motor, "Kp", &kp)
            || !get_double_attr(motor, "Kd", &kd)
            || !read_reserve3(motor, reserve)) {
            Py_DECREF(seq);
            Py_DECREF(result);
            return nullptr;
        }
        encode_motor_with_reserve(
            cmd + MOTOR_OFFSET + i * MOTOR_SIZE,
            mode, q, dq, tau, kp, kd,
            reserve[0], reserve[1], reserve[2]);
    }
    Py_DECREF(seq);

    if (!copy_bytes_attr(lowcmd, "wirelessRemote", cmd + 566, 40)) {
        Py_DECREF(result);
        return nullptr;
    }
    if (!copy_bytes_attr(lowcmd, "reserve", cmd + 606, 4)) {
        Py_DECREF(result);
        return nullptr;
    }
    finish_crc(cmd);
    return result;
}

struct FastLowCmdBuilder {
    PyObject_HEAD
    uint32_t P[18];
    uint32_t S[4][256];
};

uint32_t bf_f(FastLowCmdBuilder* self, uint32_t x) {
    return (((self->S[0][(x >> 24) & 0xffu] + self->S[1][(x >> 16) & 0xffu]) & 0xffffffffu)
            ^ self->S[2][(x >> 8) & 0xffu]) + self->S[3][x & 0xffu];
}

void encrypt_block_le(FastLowCmdBuilder* self, const uint8_t* in, uint8_t* out) {
    uint32_t L = get_u32_le(in);
    uint32_t R = get_u32_le(in + 4);
    for (int i = 0; i < 16; ++i) {
        L ^= self->P[i];
        R ^= bf_f(self, L);
        uint32_t tmp = L;
        L = R;
        R = tmp;
    }
    uint32_t tmp = L;
    L = R;
    R = tmp;
    R ^= self->P[16];
    L ^= self->P[17];
    put_u32_le(out, L);
    put_u32_le(out + 4, R);
}

void decrypt_block_le(FastLowCmdBuilder* self, const uint8_t* in, uint8_t* out) {
    uint32_t L = get_u32_le(in);
    uint32_t R = get_u32_le(in + 4);
    for (int i = 17; i > 1; --i) {
        L ^= self->P[i];
        R ^= bf_f(self, L);
        uint32_t tmp = L;
        L = R;
        R = tmp;
    }
    uint32_t tmp = L;
    L = R;
    R = tmp;
    R ^= self->P[1];
    L ^= self->P[0];
    put_u32_le(out, L);
    put_u32_le(out + 4, R);
}

inline float get_f32_le(const uint8_t* p) {
    uint32_t raw = get_u32_le(p);
    float value;
    static_assert(sizeof(raw) == sizeof(value), "float must be 32-bit");
    std::memcpy(&value, &raw, sizeof(value));
    return value;
}

inline int16_t get_i16_le(const uint8_t* p) {
    return static_cast<int16_t>(
        static_cast<uint16_t>(p[0]) | (static_cast<uint16_t>(p[1]) << 8));
}

inline int32_t get_i32_le(const uint8_t* p) {
    return static_cast<int32_t>(get_u32_le(p));
}

PyObject* encrypt_bytes(FastLowCmdBuilder* self, const uint8_t* data, Py_ssize_t len) {
    if (len % 8 != 0) {
        PyErr_SetString(PyExc_ValueError, "data length must be a multiple of 8");
        return nullptr;
    }
    PyObject* result = PyBytes_FromStringAndSize(nullptr, len);
    if (!result) {
        return nullptr;
    }
    auto* out = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
    for (Py_ssize_t i = 0; i < len; i += 8) {
        encrypt_block_le(self, data + i, out + i);
    }
    return result;
}

PyObject* make_float_list(const double* values, int count) {
    PyObject* list = PyList_New(count);
    if (!list) {
        return nullptr;
    }
    for (int i = 0; i < count; ++i) {
        PyObject* v = PyFloat_FromDouble(values[i]);
        if (!v) {
            Py_DECREF(list);
            return nullptr;
        }
        PyList_SET_ITEM(list, i, v);
    }
    return list;
}

PyObject* make_int_list(const int* values, int count) {
    PyObject* list = PyList_New(count);
    if (!list) {
        return nullptr;
    }
    for (int i = 0; i < count; ++i) {
        PyObject* v = PyLong_FromLong(values[i]);
        if (!v) {
            Py_DECREF(list);
            return nullptr;
        }
        PyList_SET_ITEM(list, i, v);
    }
    return list;
}

bool dict_set_steals(PyObject* dict, const char* key, PyObject* value) {
    if (!value) {
        return false;
    }
    int ok = PyDict_SetItemString(dict, key, value);
    Py_DECREF(value);
    return ok == 0;
}

bool dict_set_long(PyObject* dict, const char* key, long value) {
    PyObject* obj = PyLong_FromLong(value);
    return dict_set_steals(dict, key, obj);
}

PyObject* parse_remote_pressed(uint16_t btn) {
    struct ButtonName {
        uint16_t mask;
        const char* name;
    };
    static constexpr ButtonName buttons[] = {
        {0x0001, "R1"}, {0x0002, "L1"}, {0x0004, "START"}, {0x0008, "SELECT"},
        {0x0010, "R2"}, {0x0020, "L2"}, {0x0040, "F1"}, {0x0080, "F2"},
        {0x0100, "A"}, {0x0200, "B"}, {0x0400, "X"}, {0x0800, "Y"},
        {0x1000, "UP"}, {0x2000, "RIGHT"}, {0x4000, "DOWN"}, {0x8000, "LEFT"},
    };
    PyObject* list = PyList_New(0);
    if (!list) {
        return nullptr;
    }
    for (const auto& b : buttons) {
        if ((btn & b.mask) == 0) {
            continue;
        }
        PyObject* s = PyUnicode_FromString(b.name);
        if (!s) {
            Py_DECREF(list);
            return nullptr;
        }
        if (PyList_Append(list, s) < 0) {
            Py_DECREF(s);
            Py_DECREF(list);
            return nullptr;
        }
        Py_DECREF(s);
    }
    return list;
}

int FastLowCmdBuilder_init(FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
    const char* state_path = nullptr;
    static const char* kwlist[] = {"state_path", nullptr};
    if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s", const_cast<char**>(kwlist), &state_path)) {
        return -1;
    }

    std::ifstream f(state_path, std::ios::binary);
    if (!f) {
        PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path);
        return -1;
    }
    std::array<uint8_t, 4168> buf{};
    f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
    if (f.gcount() < static_cast<std::streamsize>(buf.size())) {
        PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path);
        return -1;
    }
    const uint8_t* p = buf.data();
    for (int i = 0; i < 18; ++i) {
        self->P[i] = get_u32_le(p + i * 4);
    }
    p += 18 * 4;
    for (int s = 0; s < 4; ++s) {
        for (int i = 0; i < 256; ++i) {
            self->S[s][i] = get_u32_le(p + (s * 256 + i) * 4);
        }
    }
    return 0;
}

PyObject* FastLowCmdBuilder_build_plain_damping(FastLowCmdBuilder*, PyObject*) {
    PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
    if (!result) {
        return nullptr;
    }
    auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
    init_lowcmd_header(cmd);
    finish_crc(cmd);
    return result;
}

PyObject* FastLowCmdBuilder_build_encrypted_damping(FastLowCmdBuilder* self, PyObject*) {
    uint8_t cmd[LOWCMD_SIZE];
    init_lowcmd_header(cmd);
    finish_crc(cmd);
    return encrypt_bytes(self, cmd, LOWCMD_SIZE);
}

PyObject* FastLowCmdBuilder_build_plain_servo12(FastLowCmdBuilder*, PyObject* args, PyObject* kwargs) {
    PyObject* q_obj = nullptr;
    PyObject* dq_obj = nullptr;
    PyObject* tau_obj = nullptr;
    PyObject* kp_obj = nullptr;
    PyObject* kd_obj = nullptr;
    static const char* kwlist[] = {"q", "dq", "tau", "kp", "kd", nullptr};
    if (!PyArg_ParseTupleAndKeywords(
            args, kwargs, "O|OOOO", const_cast<char**>(kwlist),
            &q_obj, &dq_obj, &tau_obj, &kp_obj, &kd_obj)) {
        return nullptr;
    }

    double q[SERVO_COUNT], dq[SERVO_COUNT], tau[SERVO_COUNT], kp[SERVO_COUNT], kd[SERVO_COUNT];
    if (!read_double_sequence(q_obj, q, SERVO_COUNT, 0.0, "q")) return nullptr;
    if (!read_double_sequence(dq_obj, dq, SERVO_COUNT, 0.0, "dq")) return nullptr;
    if (!read_double_sequence(tau_obj, tau, SERVO_COUNT, 0.0, "tau")) return nullptr;
    if (!read_double_sequence(kp_obj, kp, SERVO_COUNT, 0.0, "kp")) return nullptr;
    if (!read_double_sequence(kd_obj, kd, SERVO_COUNT, 0.0, "kd")) return nullptr;

    PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
    if (!result) {
        return nullptr;
    }
    auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
    init_lowcmd_header(cmd);
    for (int i = 0; i < SERVO_COUNT; ++i) {
        encode_motor(cmd + MOTOR_OFFSET + i * MOTOR_SIZE, 0x0a, q[i], dq[i], tau[i], kp[i], kd[i]);
    }
    finish_crc(cmd);
    return result;
}

PyObject* FastLowCmdBuilder_build_encrypted_servo12(FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
    PyObject* plain = FastLowCmdBuilder_build_plain_servo12(self, args, kwargs);
    if (!plain) {
        return nullptr;
    }
    PyObject* result = encrypt_bytes(
        self,
        reinterpret_cast<const uint8_t*>(PyBytes_AS_STRING(plain)),
        PyBytes_GET_SIZE(plain));
    Py_DECREF(plain);
    return result;
}

PyObject* FastLowCmdBuilder_build_encrypted_servo12_checked(
        FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
    PyObject* q_obj = nullptr;
    PyObject* actual_q_obj = nullptr;
    PyObject* actual_tau_obj = nullptr;
    double kp_scalar = 0.0;
    double kd_scalar = 0.0;
    double position_protect_limit = 0.0;
    static const char* kwlist[] = {
        "q", "kp", "kd", "actual_q", "actual_tau", "position_protect_limit", nullptr};
    if (!PyArg_ParseTupleAndKeywords(
            args, kwargs, "Odd|OOd", const_cast<char**>(kwlist),
            &q_obj, &kp_scalar, &kd_scalar,
            &actual_q_obj, &actual_tau_obj, &position_protect_limit)) {
        return nullptr;
    }

    double q[SERVO_COUNT], actual_q[SERVO_COUNT], actual_tau[SERVO_COUNT];
    if (!read_double_sequence(q_obj, q, SERVO_COUNT, 0.0, "q")) return nullptr;

    bool have_actual_q = actual_q_obj != nullptr && actual_q_obj != Py_None;
    bool have_actual_tau = actual_tau_obj != nullptr && actual_tau_obj != Py_None;
    if (have_actual_q && !read_double_sequence(actual_q_obj, actual_q, SERVO_COUNT, 0.0, "actual_q")) {
        return nullptr;
    }
    if (have_actual_tau && !read_double_sequence(actual_tau_obj, actual_tau, SERVO_COUNT, 0.0, "actual_tau")) {
        return nullptr;
    }

    if (have_actual_tau) {
        for (int i = 0; i < SERVO_COUNT; ++i) {
            if (std::fabs(actual_tau[i]) > TAU_MAX_SERVO[i]) {
                char buf[192];
                std::snprintf(
                    buf,
                    sizeof(buf),
                    "power protect: 电机 %s 实测 tauEst=%.2f 已超 max %.2f, 立即停机!",
                    JOINT_NAMES_SERVO[i], actual_tau[i], TAU_MAX_SERVO[i]);
                PyObject* message = PyUnicode_FromString(buf);
                if (message) {
                    PyErr_SetObject(PyExc_RuntimeError, message);
                    Py_DECREF(message);
                }
                return nullptr;
            }
        }
    }

    uint8_t cmd[LOWCMD_SIZE];
    init_lowcmd_header(cmd);
    for (int i = 0; i < SERVO_COUNT; ++i) {
        double target = q[i];
        if (target < JOINT_LIMIT_LO[i]) {
            target = JOINT_LIMIT_LO[i];
        } else if (target > JOINT_LIMIT_HI[i]) {
            target = JOINT_LIMIT_HI[i];
        }
        double kp = kp_scalar;
        double kd = kd_scalar;
        if (have_actual_q && position_protect_limit > 0.0
                && std::fabs(target - actual_q[i]) > position_protect_limit) {
            kp = 0.0;
            kd = 0.0;
        }
        encode_motor(cmd + MOTOR_OFFSET + i * MOTOR_SIZE, 0x0a, target, 0.0, 0.0, kp, kd);
    }
    finish_crc(cmd);
    return encrypt_bytes(self, cmd, LOWCMD_SIZE);
}

PyObject* FastLowCmdBuilder_build_plain_fields20(FastLowCmdBuilder*, PyObject* args, PyObject* kwargs) {
    PyObject* mode_obj = nullptr;
    PyObject* q_obj = nullptr;
    PyObject* dq_obj = nullptr;
    PyObject* tau_obj = nullptr;
    PyObject* kp_obj = nullptr;
    PyObject* kd_obj = nullptr;
    static const char* kwlist[] = {"mode", "q", "dq", "tau", "kp", "kd", nullptr};
    if (!PyArg_ParseTupleAndKeywords(
            args, kwargs, "|OOOOOO", const_cast<char**>(kwlist),
            &mode_obj, &q_obj, &dq_obj, &tau_obj, &kp_obj, &kd_obj)) {
        return nullptr;
    }

    int mode[MOTOR_COUNT];
    double q[MOTOR_COUNT], dq[MOTOR_COUNT], tau[MOTOR_COUNT], kp[MOTOR_COUNT], kd[MOTOR_COUNT];
    if (!read_int_sequence(mode_obj, mode, MOTOR_COUNT, 0, "mode")) return nullptr;
    if (!read_double_sequence(q_obj, q, MOTOR_COUNT, 0.0, "q")) return nullptr;
    if (!read_double_sequence(dq_obj, dq, MOTOR_COUNT, 0.0, "dq")) return nullptr;
    if (!read_double_sequence(tau_obj, tau, MOTOR_COUNT, 0.0, "tau")) return nullptr;
    if (!read_double_sequence(kp_obj, kp, MOTOR_COUNT, 0.0, "kp")) return nullptr;
    if (!read_double_sequence(kd_obj, kd, MOTOR_COUNT, 0.0, "kd")) return nullptr;

    PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
    if (!result) {
        return nullptr;
    }
    auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
    init_lowcmd_header(cmd);
    for (int i = 0; i < MOTOR_COUNT; ++i) {
        encode_motor(cmd + MOTOR_OFFSET + i * MOTOR_SIZE, mode[i], q[i], dq[i], tau[i], kp[i], kd[i]);
    }
    finish_crc(cmd);
    return result;
}

PyObject* FastLowCmdBuilder_build_encrypted_fields20(FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
    PyObject* plain = FastLowCmdBuilder_build_plain_fields20(self, args, kwargs);
    if (!plain) {
        return nullptr;
    }
    PyObject* result = encrypt_bytes(
        self,
        reinterpret_cast<const uint8_t*>(PyBytes_AS_STRING(plain)),
        PyBytes_GET_SIZE(plain));
    Py_DECREF(plain);
    return result;
}

PyObject* FastLowCmdBuilder_build_plain_lowcmd(FastLowCmdBuilder*, PyObject* args) {
    PyObject* lowcmd = nullptr;
    if (!PyArg_ParseTuple(args, "O", &lowcmd)) {
        return nullptr;
    }
    return build_plain_from_lowcmd_obj(lowcmd);
}

PyObject* FastLowCmdBuilder_build_encrypted_lowcmd(FastLowCmdBuilder* self, PyObject* args) {
    PyObject* lowcmd = nullptr;
    if (!PyArg_ParseTuple(args, "O", &lowcmd)) {
        return nullptr;
    }
    PyObject* plain = build_plain_from_lowcmd_obj(lowcmd);
    if (!plain) {
        return nullptr;
    }
    PyObject* result = encrypt_bytes(
        self,
        reinterpret_cast<const uint8_t*>(PyBytes_AS_STRING(plain)),
        PyBytes_GET_SIZE(plain));
    Py_DECREF(plain);
    return result;
}

PyObject* FastLowCmdBuilder_encrypt(FastLowCmdBuilder* self, PyObject* args) {
    Py_buffer view;
    if (!PyArg_ParseTuple(args, "y*", &view)) {
        return nullptr;
    }
    PyObject* result = encrypt_bytes(self, reinterpret_cast<const uint8_t*>(view.buf), view.len);
    PyBuffer_Release(&view);
    return result;
}

PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* args) {
    Py_buffer view;
    if (!PyArg_ParseTuple(args, "y*", &view)) {
        return nullptr;
    }
    if (view.len < 807) {
        PyBuffer_Release(&view);
        Py_RETURN_NONE;
    }
    Py_ssize_t aligned = (view.len / 8) * 8;
    if (aligned < 807) {
        PyBuffer_Release(&view);
        Py_RETURN_NONE;
    }

    const uint8_t* cipher = reinterpret_cast<const uint8_t*>(view.buf);
    std::vector<uint8_t> data(static_cast<size_t>(aligned));
    for (Py_ssize_t i = 0; i < aligned; i += 8) {
        decrypt_block_le(self, cipher + i, data.data() + i);
    }
    PyBuffer_Release(&view);

    if (data[0] != 0xfe || data[1] != 0xef || data[2] != 0xff || data[3] != 0x00) {
        Py_RETURN_NONE;
    }

    double quat[4], gyro[3], rpy[3];
    for (int i = 0; i < 4; ++i) {
        quat[i] = static_cast<double>(get_f32_le(data.data() + 22 + i * 4));
    }
    for (int i = 0; i < 3; ++i) {
        gyro[i] = static_cast<double>(get_f32_le(data.data() + 22 + 16 + i * 4));
        rpy[i] = static_cast<double>(get_f32_le(data.data() + 22 + 40 + i * 4));
    }

    double q[MOTOR_COUNT], dq[MOTOR_COUNT], tau[MOTOR_COUNT], q_raw[MOTOR_COUNT], dq_raw[MOTOR_COUNT];
    int mode[MOTOR_COUNT], ddq[MOTOR_COUNT], ddq_raw[MOTOR_COUNT], temperature[MOTOR_COUNT];
    int reserve0[MOTOR_COUNT], reserve1[MOTOR_COUNT];
    for (int i = 0; i < MOTOR_COUNT; ++i) {
        const uint8_t* m = data.data() + 75 + i * 32;
        mode[i] = static_cast<int>(m[0]);
        q[i] = static_cast<double>(get_f32_le(m + 1));
        dq[i] = static_cast<double>(get_f32_le(m + 5));
        ddq[i] = static_cast<int>(get_i16_le(m + 9));
        tau[i] = static_cast<double>(get_i16_le(m + 11)) * 0.00390625;
        q_raw[i] = static_cast<double>(get_f32_le(m + 13));
        dq_raw[i] = static_cast<double>(get_f32_le(m + 17));
        ddq_raw[i] = static_cast<int>(get_i16_le(m + 21));
        temperature[i] = static_cast<int>(m[23]);
        reserve0[i] = static_cast<int>(get_u32_le(m + 24));
        reserve1[i] = static_cast<int>(get_u32_le(m + 28));
    }

    const uint8_t* bms = data.data() + 715;
    int bms_current = static_cast<int>(get_i32_le(bms + 4));
    int cell_vol[10];
    for (int i = 0; i < 10; ++i) {
        cell_vol[i] = static_cast<int>(
            static_cast<uint16_t>(bms[14 + i * 2])
            | (static_cast<uint16_t>(bms[15 + i * 2]) << 8));
    }

    const uint8_t* remote = data.data() + 759;
    uint16_t btn = static_cast<uint16_t>(remote[2]) | (static_cast<uint16_t>(remote[3]) << 8);
    double remote_vals[5] = {
        static_cast<double>(get_f32_le(remote + 4)),   // lx
        static_cast<double>(get_f32_le(remote + 20)),  // ly
        static_cast<double>(get_f32_le(remote + 8)),   // rx
        static_cast<double>(get_f32_le(remote + 12)),  // ry
        static_cast<double>(get_f32_le(remote + 16)),  // L2
    };

    PyObject* out = PyDict_New();
    if (!out) {
        return nullptr;
    }
    if (!dict_set_steals(out, "imu_quaternion", make_float_list(quat, 4))
        || !dict_set_steals(out, "imu_gyroscope", make_float_list(gyro, 3))
        || !dict_set_steals(out, "imu_rpy", make_float_list(rpy, 3))
        || !dict_set_steals(out, "motor_q", make_float_list(q, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_dq", make_float_list(dq, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_tau", make_float_list(tau, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_q_raw", make_float_list(q_raw, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_dq_raw", make_float_list(dq_raw, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_mode", make_int_list(mode, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_ddq", make_int_list(ddq, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_ddq_raw", make_int_list(ddq_raw, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_temperature", make_int_list(temperature, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_reserve0", make_int_list(reserve0, MOTOR_COUNT))
        || !dict_set_steals(out, "motor_reserve1", make_int_list(reserve1, MOTOR_COUNT))
        || !dict_set_steals(out, "bms_cell_vol", make_int_list(cell_vol, 10))
        || !dict_set_steals(out, "remote_axes", make_float_list(remote_vals, 5))
        || !dict_set_steals(out, "remote_pressed", parse_remote_pressed(btn))) {
        Py_DECREF(out);
        return nullptr;
    }

    if (!dict_set_long(out, "head", static_cast<long>(data[0] | (data[1] << 8)))
        || !dict_set_long(out, "levelFlag", data[2])
        || !dict_set_long(out, "frameReserve", data[3])
        || !dict_set_long(out, "bandWidth", static_cast<long>(data[20] | (data[21] << 8)))
        || !dict_set_long(out, "bms_version_h", bms[0])
        || !dict_set_long(out, "bms_version_l", bms[1])
        || !dict_set_long(out, "bms_status", bms[2])
        || !dict_set_long(out, "bms_soc", bms[3])
        || !dict_set_long(out, "bms_current", bms_current)
        || !dict_set_long(out, "bms_cycle", static_cast<long>(bms[8] | (bms[9] << 8)))
        || !dict_set_long(out, "bms_bq_ntc0", bms[10])
        || !dict_set_long(out, "bms_bq_ntc1", bms[11])
        || !dict_set_long(out, "bms_mcu_ntc0", bms[12])
        || !dict_set_long(out, "bms_mcu_ntc1", bms[13])
        || !dict_set_long(out, "remote_btn", btn)) {
        Py_DECREF(out);
        return nullptr;
    }
    return out;
}

PyMethodDef FastLowCmdBuilder_methods[] = {
    {"build_plain_damping", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_damping), METH_NOARGS,
     "Build a plain all-damping LowCmd packet."},
    {"build_encrypted_damping", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_damping), METH_NOARGS,
     "Build an encrypted all-damping LowCmd packet."},
    {"build_plain_servo12", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_servo12),
     METH_VARARGS | METH_KEYWORDS, "Build a plain LowCmd packet with first 12 motors in servo mode."},
    {"build_encrypted_servo12", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_servo12),
     METH_VARARGS | METH_KEYWORDS, "Build an encrypted LowCmd packet with first 12 motors in servo mode."},
    {"build_encrypted_servo12_checked",
     reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_servo12_checked),
     METH_VARARGS | METH_KEYWORDS,
     "Build an encrypted servo12 packet with native safety checks."},
    {"build_plain_fields20", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_fields20),
     METH_VARARGS | METH_KEYWORDS, "Build a plain LowCmd packet from 20 motor field arrays."},
    {"build_encrypted_fields20", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_fields20),
     METH_VARARGS | METH_KEYWORDS, "Build an encrypted LowCmd packet from 20 motor field arrays."},
    {"build_plain_lowcmd", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_lowcmd), METH_VARARGS,
     "Build a plain LowCmd packet from a Python LowCmd object."},
    {"build_encrypted_lowcmd", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_lowcmd), METH_VARARGS,
     "Build an encrypted LowCmd packet from a Python LowCmd object."},
    {"encrypt", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_encrypt), METH_VARARGS,
     "Encrypt a bytes-like object with the loaded Blowfish state."},
    {"decrypt_lowstate", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_decrypt_lowstate), METH_VARARGS,
     "Decrypt and parse a LowState UDP packet into primitive Python fields."},
    {nullptr, nullptr, 0, nullptr},
};

PyTypeObject FastLowCmdBuilderType = {
    PyVarObject_HEAD_INIT(nullptr, 0)
};

PyModuleDef module = {
    PyModuleDef_HEAD_INIT,
    "go1_fast_lowcmd",
    "Native CPython LowCmd builder/encrypter for go1_pro_sdk.",
    -1,
    nullptr,
};

}  // namespace

PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) {
    FastLowCmdBuilderType.tp_name = "go1_fast_lowcmd.FastLowCmdBuilder";
    FastLowCmdBuilderType.tp_basicsize = sizeof(FastLowCmdBuilder);
    FastLowCmdBuilderType.tp_flags = Py_TPFLAGS_DEFAULT;
    FastLowCmdBuilderType.tp_doc = "Native LowCmd builder with persistent Blowfish state.";
    FastLowCmdBuilderType.tp_init = reinterpret_cast<initproc>(FastLowCmdBuilder_init);
    FastLowCmdBuilderType.tp_new = PyType_GenericNew;
    FastLowCmdBuilderType.tp_methods = FastLowCmdBuilder_methods;

    if (PyType_Ready(&FastLowCmdBuilderType) < 0) {
        return nullptr;
    }

    PyObject* m = PyModule_Create(&module);
    if (!m) {
        return nullptr;
    }
    Py_INCREF(&FastLowCmdBuilderType);
    if (PyModule_AddObject(m, "FastLowCmdBuilder", reinterpret_cast<PyObject*>(&FastLowCmdBuilderType)) < 0) {
        Py_DECREF(&FastLowCmdBuilderType);
        Py_DECREF(m);
        return nullptr;
    }
    PyModule_AddIntConstant(m, "LOWCMD_SIZE", LOWCMD_SIZE);
    return m;
}