"""Robot motion — DoA tracking + expressive actions."""

import asyncio
import math
import time
import logging
import re

import numpy as np

def create_head_pose(roll=0, pitch=0, yaw=0, degrees=False):
    if degrees:
        roll, pitch, yaw = math.radians(roll), math.radians(pitch), math.radians(yaw)
    cr, sr = math.cos(roll), math.sin(roll)
    cp, sp = math.cos(pitch), math.sin(pitch)
    cy, sy = math.cos(yaw), math.sin(yaw)
    R = np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr, 0],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr, 0],
        [-sp,   cp*sr,            cp*cr,             0],
        [0,     0,                0,                 1],
    ], dtype=np.float64)
    return R

from .config import DOA_SMOOTHING, CONTROL_LOOP_HZ

logger = logging.getLogger(__name__)

ACTION_RE = re.compile(r"\[ACTION:(\w+)\]", re.IGNORECASE)
SALE_RE = re.compile(r"\[SALE:([^\]]+)\]", re.IGNORECASE)


def parse_response(text: str) -> tuple[str, list[str], list[str]]:
    actions = [m.group(1).lower() for m in ACTION_RE.finditer(text)]
    sales = [m.group(1).strip() for m in SALE_RE.finditer(text)]
    clean = ACTION_RE.sub("", text)
    clean = SALE_RE.sub("", clean).strip()
    return clean, actions, sales


class MotionController:
    def __init__(self, mini):
        self.mini = mini
        self._running = False
        self._doa_angle: float | None = None
        self._speech_detected = False
        self._is_speaking = False
        self._action_queue: asyncio.Queue[str] = asyncio.Queue()

    async def start(self):
        self._running = True

    async def stop(self):
        self._running = False

    def set_speaking(self, val: bool):
        self._is_speaking = val

    def trigger(self, action: str):
        try:
            self._action_queue.put_nowait(action)
        except asyncio.QueueFull:
            pass

    async def doa_tracking_loop(self):
        loop = asyncio.get_event_loop()
        while self._running:
            try:
                result = await loop.run_in_executor(None, self.mini.media.get_DoA)
                if result:
                    angle, speech = result
                    self._speech_detected = speech
                    if self._doa_angle is None:
                        self._doa_angle = angle
                    else:
                        self._doa_angle = DOA_SMOOTHING * angle + (1 - DOA_SMOOTHING) * self._doa_angle
            except Exception:
                pass
            await asyncio.sleep(0.05)

    async def motion_loop(self):
        while self._running:
            t = time.monotonic()

            head_yaw = 0.0
            if self._doa_angle is not None and self._speech_detected:
                head_yaw = max(min((math.pi / 2) - self._doa_angle, 0.7), -0.7)

            ant_l, ant_r = 0.0, 0.0
            if self._is_speaking:
                amp = math.radians(25)
                ant_l = amp * math.sin(2 * math.pi * 1.5 * t)
                ant_r = amp * math.sin(2 * math.pi * 1.5 * t + math.pi / 3)

            head = create_head_pose(roll=0, pitch=0, yaw=head_yaw, degrees=False)
            self.mini.set_target(head=head, antennas=[ant_r, ant_l])
            await asyncio.sleep(1.0 / CONTROL_LOOP_HZ)

    async def action_loop(self):
        while self._running:
            try:
                action = await asyncio.wait_for(self._action_queue.get(), timeout=0.5)
                await self._execute(action)
            except asyncio.TimeoutError:
                continue

    async def _execute(self, action: str):
        m = self.mini
        if action == "wave":
            for _ in range(3):
                m.goto_target(antennas=[0, math.radians(-40)], duration=0.2)
                await asyncio.sleep(0.25)
                m.goto_target(antennas=[math.radians(-40), 0], duration=0.2)
                await asyncio.sleep(0.25)
            m.goto_target(antennas=[0, 0], duration=0.3)

        elif action == "nod":
            for angle in [-15, 10, -10, 0]:
                head = create_head_pose(pitch=math.radians(angle), degrees=False)
                m.goto_target(head=head, duration=0.2)
                await asyncio.sleep(0.25)

        elif action == "shake":
            for yaw in [-20, 20, -15, 0]:
                head = create_head_pose(yaw=math.radians(yaw), degrees=False)
                m.goto_target(head=head, duration=0.2)
                await asyncio.sleep(0.2)

        elif action == "happy":
            for _ in range(3):
                m.goto_target(antennas=[math.radians(-30), math.radians(-30)], duration=0.15)
                await asyncio.sleep(0.2)
                m.goto_target(antennas=[math.radians(10), math.radians(10)], duration=0.15)
                await asyncio.sleep(0.2)
            m.goto_target(antennas=[0, 0], duration=0.3)

        elif action == "dance":
            for _ in range(4):
                m.goto_target(body_yaw=math.radians(20), antennas=[math.radians(-30), math.radians(10)], duration=0.25)
                await asyncio.sleep(0.3)
                m.goto_target(body_yaw=math.radians(-20), antennas=[math.radians(10), math.radians(-30)], duration=0.25)
                await asyncio.sleep(0.3)
            m.goto_target(body_yaw=0, antennas=[0, 0], duration=0.3)

        elif action == "think":
            head = create_head_pose(roll=math.radians(5), pitch=math.radians(-10), yaw=math.radians(15), degrees=False)
            m.goto_target(head=head, antennas=[math.radians(-20), math.radians(-20)], duration=0.4)
            await asyncio.sleep(1.5)
            m.goto_target(head=create_head_pose(degrees=False), antennas=[0, 0], duration=0.4)

        elif action == "greet":
            head = create_head_pose(pitch=math.radians(-15), degrees=False)
            m.goto_target(head=head, antennas=[math.radians(-20), math.radians(-20)], duration=0.3)
            await asyncio.sleep(0.5)
            m.goto_target(head=create_head_pose(degrees=False), antennas=[0, 0], duration=0.3)