"""
zmq_api.py — ZMQ velocity + command interface to Holosoma
"""
import json
import time
import zmq
from Core.config_loader import load_config
from Core.logger import log

_cfg = load_config("ZMQ")

ZMQ_HOST        = _cfg["zmq_host"]
ZMQ_PORT        = _cfg["zmq_port"]
STOP_ITERATIONS = _cfg["stop_iterations"]
STOP_DELAY      = _cfg["stop_delay"]
STEP_PAUSE      = _cfg["step_pause"]

ctx  = zmq.Context()
sock = ctx.socket(zmq.PUB)
sock.bind(f"tcp://{ZMQ_HOST}:{ZMQ_PORT}")
time.sleep(0.5)
log(f"ZMQ PUB bound on tcp://{ZMQ_HOST}:{ZMQ_PORT}", "info", "zmq")


def get_socket():
    """Return the shared ZMQ PUB socket (for odometry to reuse)."""
    return sock


def send_vel(vx: float = 0.0, vy: float = 0.0, vyaw: float = 0.0):
    """Send velocity to Holosoma. vx m/s | vy m/s | vyaw rad/s"""
    sock.send_string(json.dumps({"vel": {"vx": vx, "vy": vy, "vyaw": vyaw}}))


def gradual_stop():
    """Smooth deceleration to zero over ~1 second."""
    for _ in range(STOP_ITERATIONS):
        send_vel(0.0, 0.0, 0.0)
        time.sleep(STOP_DELAY)


def send_cmd(cmd: str):
    """Send Holosoma state command: start | walk | stand | stop"""
    sock.send_string(json.dumps({"cmd": cmd}))


# Load MOVE_MAP from navigation config
_nav = load_config("Navigation")
MOVE_MAP = {k: tuple(v) for k, v in _nav["move_map"].items()}