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

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);
}

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;
}

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;
}

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."},
    {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;
}