from __future__ import annotations
from math import atan2, degrees, hypot


def execute(payload: dict) -> dict:
    pose, goal = payload["pose"], payload["goal"]
    quality = float(payload.get("localizationQuality", 0))
    threshold = float(payload.get("minimumQuality", 0.75))
    dx, dy = float(goal["x"]) - float(pose["x"]), float(goal["y"]) - float(pose["y"])
    distance = hypot(dx, dy)
    desired = degrees(atan2(dy, dx)) if distance else float(pose.get("headingDeg", 0))
    heading_error = (desired - float(pose.get("headingDeg", 0)) + 180) % 360 - 180
    accepted = quality >= threshold
    arrived = accepted and distance <= 0.1
    speed = 0.0 if not accepted or arrived else min(float(payload.get("maxSpeedMps", 0.8)), max(0.15, distance * 0.4))
    status = "LOCALIZATION_DEGRADED" if not accepted else ("ARRIVED" if arrived else "NAVIGATING")
    return {"component": "daedongrobotics/greenhouse-lps-navigation", "accepted": accepted,
            "status": status, "target": "lpu", "distanceM": round(distance, 3),
            "headingErrorDeg": round(heading_error, 2), "speedMps": round(speed, 3),
            "command": "STOP" if speed == 0 else "DRIVE"}