# Motion control — cmd_vel, obstacle detection, safe movement
import asyncio
import math
import time

from .config import (
    log, _ros2_cache, _ros2_lock, _estop_event,
    OBSTACLE_DISTANCE, OBSTACLE_WAIT_TIMEOUT,
)
import ai_teammate_ros2_bridge.config as _cfg


def publish_cmd_vel(linear_x: float, angular_z: float, duration: float = 1.0):
    """Publish cmd_vel via rclpy native publisher at 10Hz for duration (blocking).
    Returns 'ok', 'estop' (interrupted), or 'error'."""
    from geometry_msgs.msg import Twist

    pub = _cfg._cmd_vel_pub
    if not pub:
        log.warning("ROS2 node not ready, cannot publish cmd_vel")
        return "error"

    _estop_event.clear()
    rate_hz = 10
    try:
        msg = Twist()
        msg.linear.x = float(linear_x)
        msg.angular.z = float(angular_z)

        interrupted = False
        if duration > 0:
            ticks = int(duration * rate_hz)
            for i in range(ticks):
                if _estop_event.is_set():
                    log.warning(f"E-STOP: interrupted at tick {i}/{ticks}")
                    interrupted = True
                    break
                pub.publish(msg)
                time.sleep(1.0 / rate_hz)
            if not interrupted:
                log.info(f"Published cmd_vel: linear={linear_x}, angular={angular_z} for {duration:.1f}s ({ticks} ticks)")
        else:
            pub.publish(msg)
            log.info(f"Published cmd_vel: linear={linear_x}, angular={angular_z}")

        # Always send STOP after movement
        stop = Twist()
        pub.publish(stop)
        log.info("Published cmd_vel STOP")

        return "estop" if interrupted else "ok"
    except Exception as e:
        log.error(f"cmd_vel publish error: {e}")
        return "error"


def _check_front_obstacle(threshold: float) -> tuple[bool, float | None]:
    """Check front obstacle from LiDAR cache. Returns (blocked, distance)."""
    with _ros2_lock:
        scan = _ros2_cache.get("scan", {})
    front = scan.get("front_min_distance")
    if front is None:
        return False, None
    return front < threshold, front


def _wait_for_clear(threshold: float) -> str:
    """Block until obstacle clears or E-STOP or timeout. Returns 'clear', 'estop', 'timeout'."""
    start = time.time()
    log.info(f"Waiting for obstacle to clear (threshold={threshold}m, timeout={OBSTACLE_WAIT_TIMEOUT}s)...")
    while True:
        if _estop_event.is_set():
            return "estop"
        if time.time() - start > OBSTACLE_WAIT_TIMEOUT:
            return "timeout"
        blocked, dist = _check_front_obstacle(threshold)
        if not blocked:
            log.info(f"Obstacle cleared (front={dist}m)")
            return "clear"
        time.sleep(0.2)


async def _handle_safe_move(distance: float, speed: float, step: float, threshold: float) -> dict:
    """Move with periodic obstacle checks every `step` meters."""
    loop = asyncio.get_event_loop()
    direction = 1.0 if distance > 0 else -1.0
    remaining = abs(distance)
    moved = 0.0

    while remaining > 0.01:
        if _estop_event.is_set():
            return {"status": "estop", "moved": round(moved, 3), "remaining": round(remaining, 3)}

        if direction > 0:
            blocked, front_dist = _check_front_obstacle(threshold)
            if blocked:
                log.warning(f"SAFE_MOVE: obstacle at {front_dist:.2f}m after {moved:.2f}m — waiting for clear")
                wait_result = await loop.run_in_executor(None, _wait_for_clear, threshold)
                if wait_result == "estop":
                    return {"status": "estop", "moved": round(moved, 3), "remaining": round(remaining, 3)}
                if wait_result == "timeout":
                    return {"status": "obstacle_timeout", "moved": round(moved, 3), "remaining": round(remaining, 3),
                            "front_distance": front_dist, "threshold": threshold,
                            "message": f"Obstacle not cleared after {OBSTACLE_WAIT_TIMEOUT}s"}

        chunk = min(step, remaining)
        duration = chunk / speed
        result = await loop.run_in_executor(None, publish_cmd_vel, direction * speed, 0.0, duration)

        if result == "estop":
            return {"status": "estop", "moved": round(moved + chunk, 3), "remaining": round(remaining - chunk, 3)}

        moved += chunk
        remaining -= chunk

    return {"status": "ok", "moved": round(moved, 3), "distance": distance, "speed": speed}


async def _handle_safe_navigate_path(waypoints: list, speed: float, step: float, threshold: float) -> dict:
    """Navigate waypoints with obstacle checks between segments."""
    loop = asyncio.get_event_loop()
    results = []
    prev = waypoints[0]

    for i, wp in enumerate(waypoints[1:], 1):
        if _estop_event.is_set():
            results.append({"wp": i, "status": "estop"})
            break

        dx = wp[0] - prev[0]
        dy = wp[1] - prev[1]
        distance = math.sqrt(dx * dx + dy * dy)

        if distance > 0.01:
            target_angle = math.atan2(dy, dx)
            angle_deg = math.degrees(target_angle)
            if abs(angle_deg) > 1:
                turn_duration = abs(math.radians(angle_deg)) / 0.5
                result = await loop.run_in_executor(None, publish_cmd_vel, 0.0, (0.5 if angle_deg > 0 else -0.5), turn_duration)
                if result == "estop":
                    results.append({"wp": i, "status": "estop"})
                    break

            move_result = await _handle_safe_move(distance, speed, step, threshold)
            results.append({"wp": i, "x": wp[0], "y": wp[1], **move_result})

            if move_result["status"] in ("obstacle", "estop"):
                break
        else:
            results.append({"wp": i, "status": "skipped"})

        prev = wp

    completed = all(r.get("status") == "ok" for r in results)
    return {"status": "ok" if completed else "partial", "waypoints_executed": len(results),
            "total": len(waypoints) - 1, "results": results}


async def _handle_navigate_path(params: dict) -> dict:
    """Navigate through a sequence of (x, y) waypoints using cmd_vel dead reckoning."""
    waypoints = params.get("waypoints", [])
    speed = float(params.get("speed", 0.3))
    if not waypoints or len(waypoints) < 2:
        return {"status": "error", "message": "at least 2 waypoints required"}

    loop = asyncio.get_event_loop()
    results = []
    prev = waypoints[0]

    for i, wp in enumerate(waypoints[1:], 1):
        if _estop_event.is_set():
            results.append({"wp": i, "status": "estop"})
            break

        dx = wp[0] - prev[0]
        dy = wp[1] - prev[1]
        distance = math.sqrt(dx * dx + dy * dy)

        if distance > 0.01:
            target_angle = math.atan2(dy, dx)
            angle_deg = math.degrees(target_angle)
            if abs(angle_deg) > 1:
                turn_duration = abs(math.radians(angle_deg)) / 0.5
                result = await loop.run_in_executor(None, publish_cmd_vel, 0.0, (0.5 if angle_deg > 0 else -0.5), turn_duration)
                if result == "estop":
                    results.append({"wp": i, "status": "estop"})
                    break

            duration = distance / speed
            result = await loop.run_in_executor(None, publish_cmd_vel, speed, 0.0, duration)
            results.append({"wp": i, "x": wp[0], "y": wp[1], "distance": round(distance, 3), "status": result})
        else:
            results.append({"wp": i, "x": wp[0], "y": wp[1], "status": "skipped"})

        prev = wp

    completed = all(r.get("status") == "ok" for r in results)
    return {"status": "ok" if completed else "partial", "waypoints_executed": len(results), "total": len(waypoints) - 1, "results": results}
