"""Python wrapper for the native LowCmd builder.

This module is intentionally kept outside the main go1_pro_sdk package until
the byte-equivalence and timing tests are stable on the robot board.
"""
from pathlib import Path

from go1_fast_lowcmd import FastLowCmdBuilder as _NativeFastLowCmdBuilder
from go1_pro_sdk import PowerProtectViolation


def default_state_path() -> str:
    return str(
        Path(__file__).resolve().parents[1]
        / "go1_pro_sdk"
        / "_data"
        / "blowfish_state.bin"
    )


class FastLowCmdBuilder:
    def __init__(self, state_path=None):
        self._native = _NativeFastLowCmdBuilder(str(state_path or default_state_path()))

    def build_plain_damping(self):
        return self._native.build_plain_damping()

    def build_encrypted_damping(self):
        return self._native.build_encrypted_damping()

    def build_plain_servo12(self, q, dq=None, tau=None, kp=None, kd=None):
        return self._native.build_plain_servo12(q, dq=dq, tau=tau, kp=kp, kd=kd)

    def build_encrypted_servo12(self, q, dq=None, tau=None, kp=None, kd=None):
        return self._native.build_encrypted_servo12(q, dq=dq, tau=tau, kp=kp, kd=kd)

    def build_encrypted_servo12_checked(
            self, q, kp, kd, actual_q=None, actual_tau=None, position_protect_limit=0.0):
        try:
            return self._native.build_encrypted_servo12_checked(
                q,
                kp,
                kd,
                actual_q=actual_q,
                actual_tau=actual_tau,
                position_protect_limit=position_protect_limit,
            )
        except RuntimeError as exc:
            message = str(exc)
            if message.startswith("power protect: "):
                raise PowerProtectViolation(message[len("power protect: "):]) from exc
            raise

    def build_plain_fields20(self, mode=None, q=None, dq=None, tau=None, kp=None, kd=None):
        return self._native.build_plain_fields20(mode=mode, q=q, dq=dq, tau=tau, kp=kp, kd=kd)

    def build_encrypted_fields20(self, mode=None, q=None, dq=None, tau=None, kp=None, kd=None):
        return self._native.build_encrypted_fields20(mode=mode, q=q, dq=dq, tau=tau, kp=kp, kd=kd)

    def build_plain_lowcmd(self, lowcmd):
        return self._native.build_plain_lowcmd(lowcmd)

    def build_encrypted_lowcmd(self, lowcmd):
        return self._native.build_encrypted_lowcmd(lowcmd)

    def encrypt(self, data):
        return self._native.encrypt(data)

    def decrypt_lowstate(self, data):
        return self._native.decrypt_lowstate(data)