"""ROS2 lookup facade. Existing Navigate proxy still owns physical stop proof."""
import json,threading,time,math
from .catalog import Catalog

def duration_seconds(value):
    if type(value.sec) is not int or type(value.nanosec) is not int or value.sec<0 or not 0<=value.nanosec<1_000_000_000:
        raise ValueError('invalid normalized Duration')
    seconds=value.sec+value.nanosec/1e9
    if not 0<seconds<=3600:raise ValueError('Duration must be in (0,3600] seconds')
    return seconds

def main():
    import rclpy
    from rclpy.node import Node
    from rclpy.action import ActionServer,ActionClient,GoalResponse,CancelResponse
    from rclpy.callback_groups import ReentrantCallbackGroup
    from rclpy.executors import MultiThreadedExecutor
    from bt_skill_interfaces.action import Navigate,NavigateSemantic
    class Proxy(Node):
        def __init__(self):
            super().__init__('navigation_semantic_proxy');self.group=ReentrantCallbackGroup();self.lock=threading.Lock();self.reserved=False;self.faulted=False
            path=self.declare_parameter('site_config_file','').value
            with open(path) as f:self.catalog=Catalog(json.load(f))
            self.client=ActionClient(self,Navigate,self.declare_parameter('navigate_action','skills/navigate').value,callback_group=self.group)
            self.server=ActionServer(self,NavigateSemantic,'skills/navigate_semantic',execute_callback=self.execute,goal_callback=self.admit,cancel_callback=lambda _:CancelResponse.ACCEPT,callback_group=self.group)
        def admit(self,g):
            try:
                self.catalog.resolve(g.kind,g.reference,g.registry_version,shelf=g.shelf_id,side=g.side_id,column=g.column_id,tier=g.tier_id)
                duration_seconds(g.timeout)
                if any(not math.isfinite(x) or x<=0 for x in (g.position_tolerance,g.orientation_tolerance)) or g.orientation_tolerance>math.pi:return GoalResponse.REJECT
                with self.lock:
                    if self.reserved or self.faulted:return GoalResponse.REJECT
                    self.reserved=True;self.accepted_at=time.monotonic()
                return GoalResponse.ACCEPT
            except (ValueError,KeyError):return GoalResponse.REJECT
        def execute(self,h):
            result=NavigateSemantic.Result();downstream=None;future=None
            try:
                g=h.request;lookup=self.catalog.resolve(g.kind,g.reference,g.registry_version,shelf=g.shelf_id,side=g.side_id,column=g.column_id,tier=g.tier_id)
                deadline=self.accepted_at+duration_seconds(g.timeout)
                q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.orientation_tolerance=g.orientation_tolerance
                p=lookup['pose'];q.target_pose.header.frame_id=p['frame_id'];q.target_pose.header.stamp=self.get_clock().now().to_msg()
                for k in ('x','y','z'):setattr(q.target_pose.pose.position,k,float(p[k]))
                for k in ('x','y','z','w'):setattr(q.target_pose.pose.orientation,k,float(p['q'+k]))
                if not self.client.server_is_ready():raise RuntimeError('NAVIGATE_UNAVAILABLE')
                remaining_ns=int((deadline-time.monotonic())*1e9)
                if remaining_ns<=0 or h.is_cancel_requested:raise RuntimeError('EXPIRED_BEFORE_DISPATCH')
                q.timeout.sec=remaining_ns//1_000_000_000;q.timeout.nanosec=remaining_ns%1_000_000_000
                future=self.client.send_goal_async(q);acceptance_deadline=min(deadline,time.monotonic()+3.);sequence=0;last_feedback=0.
                while not future.done():
                    if h.is_cancel_requested or time.monotonic()>acceptance_deadline:
                        def late(f):
                            try:
                                accepted=f.result()
                                if accepted.accepted:accepted.cancel_goal_async()
                            except Exception:pass
                        future.add_done_callback(late);raise RuntimeError('ACCEPTANCE_UNKNOWN')
                    time.sleep(.01)
                downstream=future.result()
                if not downstream.accepted:raise RuntimeError('NAVIGATE_REJECTED')
                done=downstream.get_result_async();cancel_at=None
                while not done.done():
                    now=time.monotonic()
                    if (h.is_cancel_requested or now>=deadline) and cancel_at is None:downstream.cancel_goal_async();cancel_at=now
                    if cancel_at is not None and now-cancel_at>5:raise RuntimeError('STOP_UNKNOWN')
                    if now-last_feedback>=.2:
                        sequence+=1;f=NavigateSemantic.Feedback();f.stamp=self.get_clock().now().to_msg();f.sequence=sequence;f.phase=2 if cancel_at else 1;f.message='awaiting downstream result';h.publish_feedback(f);last_feedback=now
                    time.sleep(.01)
                wrapped=done.result();m=wrapped.result
                for field in ('result','final_pose','pose_valid','final_position_error','final_orientation_error','errors_valid'):setattr(result,field,getattr(m,field))
                if wrapped.status==4 and m.result.status==0:h.succeed()
                elif wrapped.status==5 and h.is_cancel_requested:h.canceled()
                else:h.abort()
            except Exception as ex:
                self.faulted=True
                if downstream is not None and downstream.accepted:
                    try:downstream.cancel_goal_async()
                    except Exception:pass
                result.result.status=1;result.result.stop_state=0;result.result.error_code=str(ex);h.abort()
            finally:
                with self.lock:self.reserved=False
            return result
    rclpy.init();node=Proxy();executor=MultiThreadedExecutor(num_threads=4);executor.add_node(node)
    try:executor.spin()
    finally:executor.shutdown();node.destroy_node();rclpy.shutdown()
if __name__=='__main__':main()