
    il                    r   d dl mZ d dlZd dlZd dlmZ d dlmZ d dlZd dl	Z	d dl
Z
d dlmZmZmZmZmZ d dlZd dlmZ d dlmZmZ d dlmZmZ d d	lmZmZmZ d d
lmZm Z m!Z!m"Z" d dl#m$Z$m%Z% d dl&m'Z'm(Z(m)Z) d dl*m+Z+ e G d d             Z, G d d      Z-ddZ.ddZ/ddZ0dddZ1ddZ2e3dk(  r e2        yy)    )annotationsN)	dataclass)
HTTPStatus)AnyDictOptionalSetTuple)WebSocketServerProtocol)ActionExecutorExecutionReport)	LLMClient	LLMConfig)PlanContextPlanRequestPlannerService)InvalidTransitionOrchestratorStateMachineOrchestratorStateTransitionRecord)EdgeProxyClientClientConfig)EventLogMessageFRDetectionsMessageNavStatusMessage)FRTemporalAnalyzerc                  P   e Zd ZU dZded<   dZded<   dZded<   d	Zded
<   dZded<   dZ	ded<   dZ
ded<   dZded<   dZded<   dZded<   dZded<   dZded<   dZded<   dZded<   dZded <   d!Zded"<   dZded#<   dZded$<   dZd%ed&<   dZd'ed(<   dZd%ed)<   dZded*<   dZded+<   y),ServerConfig0.0.0.0strhosti@  intportz/healthhealth_path/wsws_path
/dashboarddashboard_ws_path   ping_intervalping_timeoutFboolenable_plannerTenable_executorenable_edge_proxy	localhostedge_proxy_hosti  edge_proxy_port.https://modelapi.klass.dev/v1/chat/completionsllm_endpointQwen3-Next-80B-A3B-FP8	llm_modelNOptional[str]llm_api_key   llm_timeout_sllm_include_response_formatllm_accept_languagezOptional[int]llm_max_tokenszOptional[float]	llm_top_p	llm_top_kllm_use_openai_sdk
llm_stream)__name__
__module____qualname__r!   __annotations__r#   r$   r&   r(   r*   r+   r-   r.   r/   r1   r2   r4   r6   r8   r:   r;   r<   r=   r>   r?   r@   rA        :/home/nelsen/Projects/kognitive/orchestrator/src/server.pyr   r      s    D#D# K GS)s)M3L# ND  OT #t#&OS&OSHL#H-Is-!%K%M3(,,)--$(NM(!%I%#I}#$$JrG   r   c                  J   e Zd Z	 	 	 d	 	 	 	 	 	 	 	 	 ddZddZddZddZddZddZd dZ	d!d	Z
	 	 	 	 	 	 	 	 d"d
Zd#dZ	 	 	 	 	 	 	 	 	 	 	 	 d$dZ	 	 	 	 	 	 	 	 	 	 	 	 d$dZ	 	 	 	 	 	 	 	 d%dZd&dZddZd'dZd(dZd)dZddZd*dZd+dZd,dZd,dZd-dZ	 	 	 	 	 	 d.dZy)/OrchestratorServerNc                   || _         |xs
 t               | _        ||n| j                         | _        ||n|j
                  r
t               nd | _        t        j                  d      | _
        d | _        d | _        d | _        t               | _        t!               | _        t!               | _        t'        t)        t+        j,                  dd            d      | _        | j                  rE| j1                          | j3                          | j                  j5                  | j6                         | j9                          y )Nzorchestrator.server$ORCHESTRATOR_NAV_REASONING_TIMEOUT_S8      ?)configr   state_machine_build_default_plannerplannerr.   r   executorlogging	getLoggerlogger_edge_client_edge_client_connect_task_pending_planr   _fr_analyzerset_dashboard_clients_reported_nav_failuresmaxfloatosgetenv_nav_reasoning_timeout_s_setup_executor_listener_setup_edge_proxy_clientregister_broadcast_callbackbroadcast_to_dashboard_setup_confirmation_listener)selfrO   rP   rR   rS   s        rH   __init__zOrchestratorServer.__init__5   s    *H.F.H")"5w4;V;V;X$,$8SYSiSi~?Oos''(=> 8<GK&7; /0 AD03#(+"))BCHI)
% ==))+))+MM55d6Q6QR 	))+rG   c                @   | j                   j                  sy t        | j                   j                  | j                   j                  | j                   j
                  | j                   j                  | j                   j                  | j                   j                  | j                   j                  | j                   j                  | j                   j                  | j                   j                  | j                   j                        }t        t        |            S )N)endpointmodelapi_key	timeout_sinclude_response_formataccept_language
max_tokenstop_ptop_kuse_openai_sdkstream)rO   )
llm_client)rO   r-   r   r4   r6   r8   r:   r;   r<   r=   r>   r?   r@   rA   r   r   )rh   
llm_configs     rH   rQ   z)OrchestratorServer._build_default_planner[   s    {{))[[--++''KK++kk//$(KK$K$K KK;;{{11++''++'';;99;;))

 *)EFFrG   c                H     d fd} j                   j                  |       y)zGSet up state machine listener to trigger executor on EXECUTING_ACTIONS.c                   | j                   t        j                  k(  rj                  j	                  d       d }| j
                  r| j
                  j                  d      }|j                  }|r%t        j                  j                  |             y j                  j                  d       t        j                  j                                y y )Nz<State transitioned to EXECUTING_ACTIONS, triggering executorplanz-No plan to execute, transitioning to SPEAKING)nextr   EXECUTING_ACTIONSrV   infopayloadgetrY   asynciocreate_task_execute_plan_and_transitionwarning_send_actions_complete)recordrz   rh   s     rH   on_state_transitionzHOrchestratorServer._setup_executor_listener.<locals>.on_state_transitionp   s    {{/AAA  R >>!>>--f5D<--D''(I(I$(OPKK''(WX''(C(C(EF! BrG   Nr   r   returnNonerP   add_listenerrh   r   s   ` rH   rc   z+OrchestratorServer._setup_executor_listenern   s     	G( 	''(;<rG   c                H     d fd} j                   j                  |       y)zESet up state machine listener to broadcast plans on AWAITING_CONFIRM.c                    | j                   t        j                  k(  r@j                  j	                  d       t        j                  j                  |              y y )NzFState transitioned to AWAITING_CONFIRM, broadcasting plan to dashboard)r{   r   AWAITING_CONFIRMrV   r}   r   r   _broadcast_confirmation_request)r   rh   s    rH   r   zLOrchestratorServer._setup_confirmation_listener.<locals>.on_state_transition   sL    {{/@@@  \ ##D$H$H$PQ ArG   Nr   r   r   s   ` rH   rg   z/OrchestratorServer._setup_confirmation_listener   s     	R 	''(;<rG   c                   | j                   j                  sy| j                  s| j                  j	                  d       y	 t        | j                   j                  | j                   j                  d      }t        || j                        | _	        | j                  j                  | j                         | j                  j                  | j                         | j                  j                  | j                         | j                  j!                  | j"                         | j                  j%                  | j&                         | j                  j)                  d| j                   j                  | j                   j                         y# t*        $ r&}| j                  j-                  d|       Y d}~yd}~ww xY w)z$Set up Edge Proxy client if enabled.Nz-Edge Proxy enabled but executor not availablez/edge)r!   r#   r&   )rO   rV   z&Edge Proxy client configured for %s:%sz&Failed to set up Edge Proxy client: %s)rO   r/   rS   rV   r   r   r1   r2   r   rW   register_edge_proxy_clientrZ   on_consistent_detection_on_consistent_fr_detectionon_fr_detections_on_fr_detectionson_nav_status_on_nav_statuson_event_log_on_edge_event_logr}   	Exceptionerror)rh   edge_configexcs      rH   rd   z+OrchestratorServer._setup_edge_proxy_client   sN   {{,,}}KK OP	M&[[00[[00K
 !0{4;; WDMM44T5F5FG 55d6V6VW..t/E/EF++D,?,?@**4+B+BCKK8++++
  	MKKFLL	Ms   EF 	GF<<Gc                   K   | j                   j                  |j                  |j                         d{    y7 w)zCForward FR detections from the edge proxy to the temporal analyzer.N)rZ   feed
detections	timestamprh   msgs     rH   r   z$OrchestratorServer._on_fr_detections   s*     $$S^^S]]CCCs   4><>c                H   K   | j                  |dd       d{    y7 w)z/Handle live nav_status updates from edge proxy.live_nav_statusFsourcereplayN)$_handle_navigation_failure_reasoningr   s     rH   r   z!OrchestratorServer._on_nav_status   s,     77$ 8 
 	
 	
s   " "c                h  K   | j                  d|j                  |j                  |j                  |j                  |j
                  |j                  |j                  d       d{    |j                  dk(  r^| j                  d|j                  |j                  |j                  |j
                  |j                  |j                  d       d{    y|j                  dk7  ryt        |j                  t              r|j                  ni }t        |j                  |j                  t        |j                  dd	            t        |j                  d
d            |j                  d      |j                  d            }|j                  r0| j                  $	 | j                  j                  |       d{    | j'                  ||j                  rdnd|j                         d{    y7 7 '7 ?# t         $ r&}| j"                  j%                  d|       Y d}~gd}~ww xY w7 ?w)z+Handle replay-safe edge event_log messages.
edge_event)typeevent_id
event_type
request_idstatusr   r   r~   Nscan_area_captureedge_artifact)r   r   r   r   r   r   artifact
nav_statusdestination progressg        reason
error_code)r   r   r   r   r   r   z3Failed applying replayed nav status to executor: %sreplay_event_log	event_logr   )rf   r   r   r   r   r   r   r~   
isinstancedictr   r    r   r_   rS   _handle_nav_statusr   rV   r   r   )rh   r   r~   nav_msgr   s        rH   r   z%OrchestratorServer._on_edge_event_log   s    ))$LL!nn!nn** ]]**;;	
 	
 	
 >>00--+ #"%..!jj!$!jj #
 
 
 >>\)!+CKK!>#++B"~~::GKKr:;7;;z378;;x({{<0
 ::$--3`mm66w??? 77),%:: 8 
 	
 	
_	

: @ `##$Y[^__`	
sn   A$H2&G6'A+H2G9C
H2G> <G<=G> /H20H01H29H2<G> >	H-H(#H2(H--H2c                 K   |j                   dk7  ry | j                  |      sy dj                  |j                  xs d|j                  xs d|j
                  xs d|j                  xs dg      }|| j                  v ry | j                  j                  |       t        | j                        dkD  r| j                  j                          | j                  |j                  |j                  |j
                  ||       d {   }| j                  dd|d|j                  xs d	 d
|j                  |j                  |j
                  |j                  ||ddd       d {    y 7 g7 w)Nfailed|r   i  r   r   r   r   r   	reasoningnavigation_failurez!NAVIGATE failed for destination 'unknown')r   r   r   r   r   r   )intentr   planSummarymetadatar   data)r   _is_localization_failurejoinr   r   r   r   r]   addlenclear&_generate_navigation_failure_reasoningrf   )rh   r   r   r   
dedupe_keyr   s         rH   r   z7OrchestratorServer._handle_navigation_failure_reasoning   sd     ::!,,S1XX$"%2$"

 b	

 444##''
3t**+d2''--/EE::~~ F 
 
	 ))#2!*%FsGcZcFdde#f&)nn'*&)nn"%**"("(!	
 	
 	

	
s%   DE1E-A!E1'E/(E1/E1c                    |j                   xs dj                         dk(  ry|j                  xs dj                         }d|v ryd|v ryy)Nr   NAV_LOCALIZATION_LOSTTlocalizationrosbridge_disconnectedF)r   upperr   lower)rh   r   r   s      rH   r   z+OrchestratorServer._is_localization_failure*  sP    NN b'')-DD**"))+V##v-rG   c          	        	
K    j                   j                  	 j                   j                  
	r
s j                        S d	
 fd}	 t	        j
                  t	        j                  |       j                  dz          d {   }|r|S 	  j                        S 7 # t        $ r&} j                  j                  d|       Y d }~Bd }~ww xY ww)Nr   c                 *   dd l } 	dddddxs d d
xs d d	xs d d
 d ddgddd}ddi}j                  j                  rdj                  j                   |d<   | j                  ||j                        }|j                          |j                         }|j                  di g      d   j                  di       j                  dd      }t        |t              r|j                         S t        |      j                         S )Nr   systemz~You are the navigation reasoning module for a field robot. Explain localization failures for operators in one short paragraph.)rolecontentuserz+Navigation failure details:
- destination: r   z
- reason: z
- error_code: z
- source: z
- replayed_after_reconnect: z5
Provide likely root cause and immediate next checks.g?   )rl   messagestemperaturerq   Content-Typezapplication/jsonzBearer Authorization)jsonheaderstimeoutchoicesmessager   r   )requestsrO   r8   postrb   raise_for_statusr   r   r   r    strip)r   r~   r   responsebodyr   r   rk   r   rl   r   r   rh   r   s         rH   	_call_llmzLOrchestratorServer._generate_navigation_failure_reasoning.<locals>._call_llmH  s]     !)b !'..9.FY-G H))/)<9(= >--7-D9,E F))/ 1;;A( CSS*  #!1G4 &'9:G{{&&-4T[[5L5L4M+N(}}55	 % H %%'==?DRD)!,Y#Y# 
 '3'}}&w<%%''rG   rN   )r   z0Navigation-failure LLM reasoning unavailable: %s)r   r    )rO   r4   r6   &_fallback_navigation_failure_reasoningr   wait_for	to_threadrb   r   rV   r   )rh   r   r   r   r   r   r   r   r   rk   rl   s   ``````   @@rH   r   z9OrchestratorServer._generate_navigation_failure_reasoning4  s      ;;++%%u>>'% ?  0	( 0	(d	Y%..!!),55; I    
 ::#! ; 
 	
  	YKK RTWXX	YsB   AC-:B; B9B; !C-9B; ;	C*C% C-%C**C-c          	     D    |rdnd}d|xs d d| d|xs d d|xs d	 d
	S )Nz after reconnect replayr   zNavigation to 'r   z' failedz with unknown_errorz (zno reason providedz). Likely localization/pose freshness issue; check rosbridge connection, robot pose updates, and waypoint_handler feedback before retrying.rF   )rh   r   r   r   r   r   replay_texts          rH   r   z9OrchestratorServer._fallback_navigation_failure_reasoning  sM     4:/rk6Y7x}F,_-R0N:N/O PQQ	
rG   c           	        K   | j                   j                  d|||       | j                  ddd| d|dddd	d
       d{    y7 w)z6Called when the FR analyzer detects a consistent face.z5Consistent detection: %s for %.1fs (confidence: %.2f)r   person_detectedz	Detected z consistently for z.0fsr   )r   r   r   r   N)rV   r}   rf   )rh   identity
confidencedurations       rH   r   z.OrchestratorServer._on_consistent_fr_detection  sh      	C		
 ))#/#H:-?~QO#%	
 	
 	
s   AAA
Ac           	     J   K    j                   s5 j                  j                  d        j                  d       d{    ydd fd}	  j                  j	                  d|j                  dd              j                   j                           j                   j                  ||       d{   } j                   j                  d	d
d       d{    	 t        i       }t        j                   j                  j                  |j                  dd      ||j                  d      |j                  d       d{   } j                  j	                  d|j                  j                  dd              j                   j                           j                   j                  |j                         d{   }|j"                  r4 j                  j	                  d        j%                          d{    y j                  j'                  d|j                          j                  |j                  r|j                  d   nd       d{    y7 -7 7 7 &7 # t         $ rC} j                  j                  d|        j                  d|        d{  7   Y d}~yd}~ww xY w7 7 d# t         $ rI} j                  j                  d|        j                  t)        |             d{  7   Y d}~yd}~ww xY ww)zxExecute plan and transition state based on result.

        Args:
            plan: Plan dictionary to execute.
        z)Executor not available for plan executionexecutor_not_availableNc                    Y| j                   j                  d| j                  d      | j                  d             j                  j	                  d       y y )Nz2Dynamic replanning requested at step=%s reasons=%sstep_idreasonsdynamic_replan_requested)rV   r}   r   rS   cancel)signalreplan_signalrh   s    rH   _on_replan_requestedzMOrchestratorServer._execute_plan_and_transition.<locals>._on_replan_requested  sU    $ &  HJJy)JJy)
 $$%?@ %rG   zExecuting plan: %splan_idr   )on_replan_requestedplan_updatedynamic_replan)r   phasetriggerr   r   )r  previous_plan_idprevious_errorszDynamic replan generated: %szDynamic replan failed: %szdynamic_replan_failed: z%Plan execution completed successfullyzPlan execution failed: %sr   r   zPlan execution error: %s)r  Dict[str, Any]r   r   )rS   rV   r   _send_actions_failedr}   r   reset_cancelexecute_planrR   rf   _build_contextr   r   refine_planerrorsrz   r   successr   r   r    )rh   rz   r  reportbase_contextrefined_graphr   r  s   `      @rH   r   z/OrchestratorServer._execute_plan_and_transition  s     }}KKIJ++,DEEE26		A0	6KK1488Iy3QRMM&&(==55$8 6  F (T\\-E11 -!1#0  #1"#5L*1*;*;002.$'4040C/5}}		+ 	%M KK$$6%**..y)D MM..0#'==#=#=m>P>P#QQF ~~  !HI11333##$?O//FMMa0@Wfggg{ F&	% R  KK%%&A3G336McU4STTT 4 h 	6KK8#>++CH555	6s   <L# I-L#A&K 6I07,K #I3$K )A'I; I6A<I; I9I; :K K
K L#AK 'K(K ,L#0K 3K 6I; 9I; ;	K3K7J:8K=K L#KK K 	L 9LLLL#L  L#c                   K   	 | j                   j                  d       y# t        $ r&}| j                  j	                  d|       Y d}~yd}~ww xY ww)z-Send actions_complete event to state machine.actions_completez*Invalid transition on actions_complete: %sNrP   handle_eventr   rV   r   rh   r   s     rH   r   z)OrchestratorServer._send_actions_complete  sJ     	S++,>?  	SKK LcRR	Ss*   A  A	AA
A
AAc                   K   	 | j                   j                  dd|i       y# t        $ r&}| j                  j	                  d|       Y d}~yd}~ww xY ww)z|Send actions_failed event to state machine.

        Args:
            error: Error message describing the failure.
        actions_failedr   z(Invalid transition on actions_failed: %sNr  )rh   r   r   s      rH   r  z'OrchestratorServer._send_actions_failed  sQ     	Q++,<w>NO  	QKK JCPP	Qs*   A# A	AAAAAc                  K   | j                   s| j                  j                  d       yd}|j                  r|j                  j	                  d      }|| j
                  }|| j                  j                  d       yd||j                  d}t               }| j                   D ]U  }	 |j                  t        j                  |             d{    | j                  j                  d|j                         W | xj                   |z  c_         |r&| j                  j                  d	t!        |             yy7 j# t        $ rB}| j                  j                  d|j                  |       |j                  |       Y d}~d}~ww xY ww)
zBroadcast plan confirmation request to all connected dashboard clients.

        Args:
            record: Transition record containing the plan in its payload.
        z;No dashboard clients connected to send confirmation requestNrz   z*No plan available for confirmation requestuser_confirmation)r   rz   r   z0Sent confirmation request to dashboard client %sz-Failed to send confirmation request to %s: %sz)Removed %d disconnected dashboard clients)r\   rV   r   r~   r   rY   r   r[   sendr   dumpsdebugremote_addressr   r   r}   r   )rh   r   rz   r   disconnectedclientr   s          rH   r   z2OrchestratorServer._broadcast_confirmation_request  s_     &&KK ]^ >>>>%%f-D<%%D<KK LM ())
 u-- 	)F)kk$**W"5666!!"TV\VkVkl	) 	</KKH#lJ[\  7 )##$SU[UjUjlop  (()sC   B'F*'D>D<*D><A F<D>>	F	8F?FF		Fc                r  K   | j                   s| j                  j                  d       yt               }| j                   D ]e  }	 |j	                  t        j                  |             d{    | j                  j                  d|j                  d      |j                         g | xj                   |z  c_         |r&| j                  j                  dt        |             yy7 z# t        $ rB}| j                  j                  d|j                  |       |j                  |       Y d}~d}~ww xY ww)a'  Broadcast a message to all connected dashboard WebSocket clients.

        Serializes the message to JSON and sends it to every client in
        ``_dashboard_clients``. Removes any clients that fail to receive.

        Args:
            message: Dictionary to serialize and broadcast.
        z,broadcast_to_dashboard: no clients connectedNz%broadcast_to_dashboard: sent %s to %sr   z0broadcast_to_dashboard: failed to send to %s: %sz7broadcast_to_dashboard: removed %d disconnected clients)r\   rV   r&  r[   r$  r   r%  r   r'  r   r   r   r}   r   )rh   r   r(  r)  r   s        rH   rf   z)OrchestratorServer.broadcast_to_dashboard:  s     &&KKLMu-- 	)F)kk$**W"5666!!;KK'))	)  	</KKIL!  7  )##F))
   (()sC   AD7'C),C'-:C)'A D7'C))	D428D/*D7/D44D7c                $  K   | j                   j                  d| j                  j                  | j                  j                         | j
                  c| j                  Wt        j                  | j
                  j                               | _        | j                  j                  | j                         	 t        j                  | j                  | j                  j                  | j                  j                  | j                  j                  | j                  j                   | j"                        4 d {    t        j$                          d {    d d d       d {    | j
                  #	 | j
                  j'                          d {    | j                  K| j                  j-                         s0| j                  j/                          	 | j                   d {    y y y 7 7 7 # 1 d {  7  sw Y   xY w7 w# t(        $ r&}| j                   j+                  d|       Y d }~d }~ww xY w7 V# t        j0                  $ r Y y t(        $ r&}| j                   j+                  d|       Y d }~y d }~ww xY w# | j
                  W	 | j
                  j'                          d {  7   n2# t(        $ r&}| j                   j+                  d|       Y d }~nd }~ww xY w| j                  | j                  j-                         sw| j                  j/                          	 | j                   d {  7   w # t        j0                  $ r Y w t(        $ r&}| j                   j+                  d|       Y d }~w d }~ww xY ww w xY ww)Nz%Starting orchestrator server on %s:%s)r*   r+   process_requestz Edge Proxy disconnect failed: %sz1Edge Proxy connect task error during shutdown: %s)rV   r}   rO   r!   r#   rW   rX   r   r   connectadd_done_callback_on_edge_client_connect_done
websocketsserve_handlerr*   r+   _process_requestFuture
disconnectr   r   doner  CancelledErrorr  s     rH   r1  zOrchestratorServer.serve_  s    3T[[5E5Et{{GWGW	
 (T-K-K-S-4-@-@ARARAZAZA\-]D***<<T=^=^_	b!''    "kk77![[55 $ 5 5 ' ' nn&&&' '   ,Q++66888 --9$B`B`BeBeBg..557b8888 Ch9#' '' ' ' ' 9  QKK''(JCPPQ 9--   bKK''([]`aab   ,Q++66888  QKK''(JCPPQ --9$B`B`BeBeBg..557b8888--   bKK''([]`aab Ch9sw  B5N8BJ
 ;G3<J
 ?G9G5G9J
 'G7(J
 ,N9H HH A NI +I,I 0N3J
 5G97J
 9H?H HJ
 H 	H?H:5N:H??NI JNJ!J=NJN
NJ<5J86J<;N<	K+K&!N&K++AN/M>M?MNNNN"N>NNNNc                    	 |j                          y # t        j                  $ r Y y t        $ r&}| j                  j                  d|       Y d }~y d }~ww xY w)Nz"Edge Proxy connect task failed: %s)resultr   r7  r   rV   r   )rh   taskr   s      rH   r/  z/OrchestratorServer._on_edge_client_connect_done  sL    	IKKM%% 	 	IKKBCHH	Is    AAAAc                  K   |d| j                   j                  fv r{| j                  j                  d|j                         	 |2 3 d {   }| j                  |       d {   }|%|j                  t        j                  |             d {    R|| j                   j                  k(  r| j                  j                  d|j                         | j                  j                  |       	 |j                  t        j                  d| j                  j                  j                   d             d {    |2 3 d {   }| j#                  |       d {   }|%|j                  t        j                  |             d {    R|j'                  d
d       d {    y 7 f7 P7 $6 n2# t        $ r&}| j                  j                  d|       Y d }~nd }~ww xY w| j                  j                  d|j                         y # | j                  j                  d|j                         w xY w7 7 7 7 6 n2# t        $ r&}| j                  j                  d|       Y d }~nd }~ww xY w| j                  j%                  |       | j                  j                  d	|j                         y # | j                  j%                  |       | j                  j                  d	|j                         w xY w7 Zw)N/zClient connected from %szConnection error: %szClient disconnected from %sz"Dashboard client connected from %sstate_updater   statezDashboard connection error: %sz%Dashboard client disconnected from %si  zUnsupported path)coder   )rO   r&   rV   r}   r'  _handle_messager$  r   r%  r   r   r(   r\   r   rP   r?  value_handle_dashboard_messagediscardclose)rh   	websocketpathr   r   r   s         rH   r2  zOrchestratorServer._handler  s    C,,--KK79Q9QRZ%. C C'%)%9%9'%BBH+'nnTZZ-ABBB 4;;000KKA9C[C[\##''	2dnnTZZ*!//55;;1 &   
 &/ C C'%)%C%CG%LLH+'nnTZZ-ABBB oo40BoCCCACBB &/  A##$:C@@A   !>	@X@XY   !>	@X@XY
CLB &/  K##$DcJJK ''//	:  !H)JbJbc ''//	:  !H)JbJbc 	DsH  ALF; F9
F0F9F; #F3$F; +'F; F6F; AL3AI
 ;H?<I
 III
I
 I I
 ''I
 II
 L*L+L0F93F; 6F; 9F; :H ;	G*G% H %G**H -'L(H<<L?I
 II
 I
 I
 	J> 
	I9I4/J> 4I99J> <AL>ALLc                x  K   t        |t        t        f      r	 |j                  dd      }t        |t
              sdddS 	 t        j                  |      }t        |t              sdd	dS |j                  d
      }|sdddS |dk(  r#d| j                  j                  j                  dS |dk(  rF|j                  d      r| j                  d|      S d| j                  j                  j                  dS |dk(  r| j                  sdddS |j                  dd      j!                         }|sdddS t#        |t%        |j                  d            |j                  dd            }t'        j(                  | j                  j*                  |       d {   }d
dit-        |      S |dk(  rl| j.                  sdddS |j                  d      }|dd dS | j.                  j1                  ||j                  d      !       d {   }d
d"it3        |      S |d#k(  r.|j                  d      }|r|| _        | j                  d#d|i      S |d$k(  r| j                  d$|      S |d%v r| j                  ||      S dd&|d'S # t        $ r dddcY S w xY w# t        j                  $ r dddcY S w xY w7 7 w)(Nutf-8replacer  r   invalid_bytesr   r   unsupported_messageinvalid_jsoninvalid_payloadr   missing_typepingpongr>  
transcriptis_finalfinal_transcriptackplan_requestplanner_disabledr   r   missing_intentcontextr   request)r   r[  r   plan_resultr  executor_disabledrz   missing_plan)r[  execution_reportuser_confirmeduser_rejected>   resetr   barge_inllm_responsetts_completeaction_completeprocessing_failedwake_word_detectedunknown_messager   r   detail)r   bytes	bytearraydecoder   r    r   loadsJSONDecodeErrorr   r   rP   r?  rB  _advance_staterR   r   r   r  r   r   rz   _plan_result_payloadrS   r  _report_payloadrY   )	rh   r   r~   msg_typer   r\  r9  rz   r  s	            rH   rA  z"OrchestratorServer._handle_message  s    gy12C!...C '3'#.CDD	>jj)G '4(#.?@@;;v&#n==v"T-?-?-E-E-K-KLL|#{{:&**+=wGG!D,>,>,D,D,J,JKK~%<< '2DEE[[2.446F '2BCC!&w{{9'=>";;|Y?G
 #,,T\\->->HHFMJ-A&-IJJ~%== '2EFF;;v&D| '.AA==55dGKKPYDZ5[[F.J/&2IJJ'';;v&D%)"&&'7&$HH&&&@@ 	
 	
 &&x99*;xPPO  C '/BBC ## 	>#n==	>: I \sl   J:J J:J E J:J5A0J:	J8
A9J:JJ:JJ:J2/J:1J22J:8J:c                b  K   t        |t        t        f      r	 |j                  dd      }t        |t
              sdddS 	 t        j                  |      }t        |t              sdd	dS |j                  d
      }|sdddS |dk(  r#d| j                  j                  j                  dS |dk(  r.|j                  d      }|r|| _        | j                  dd|i      S |dk(  r| j                  d|      S dd|dS # t        $ r dddcY S w xY w# t        j                  $ r dddcY S w xY ww)a=  Handle messages from dashboard clients.

        Dashboard clients can send confirmation responses (user_confirmed/user_rejected)
        that are forwarded to the state machine.

        Args:
            message: WebSocket message from dashboard.

        Returns:
            Optional response dictionary.
        rI  rJ  rK  r   rL  rM  rN  rO  rP  r   rQ  rR  rS  r>  ra  rz   rb  rj  rk  )r   rm  rn  ro  r   r    r   rp  rq  r   r   rP   r?  rB  rY   rr  )rh   r   r~   ru  rz   s        rH   rC  z,OrchestratorServer._handle_dashboard_message   sY     gy12C!...C '3'#.CDD	>jj)G '4(#.?@@;;v&#n== v"T-?-?-E-E-K-KLL '';;v&D%)"&&'7&$HH&&&@@*;xPPA  C '/BBC ## 	>#n==	>sL   D/C= D/D B%D/=DD/DD/D,)D/+D,,D/c                    	 | j                   j                  ||      }d||j
                  dS # t        $ r8}ddt        |      | j                   j                  j
                  dcY d }~S d }~ww xY w)Nr   invalid_transition)r   r   rl  r?  r=  )r   eventr?  )rP   r  r   r    r?  rB  )rh   ry  r~   	new_stater   s        rH   rr  z!OrchestratorServer._advance_state1  sp    	**77wGI 'QQ ! 	-c(++1177	 	s   . 	A/-A*$A/*A/c                H   || j                   j                  k(  r-d}ddt        t        |            fg}t        j
                  ||fS |d| j                   j                  | j                   j                  fvr-d}ddt        t        |            fg}t        j                  ||fS y )Ns   OK)r   z
text/plainzContent-Lengthr<  s	   Not Found)	rO   r$   r    r   r   OKr&   r(   	NOT_FOUND)rh   rG  _request_headersr   r   s        rH   r3  z#OrchestratorServer._process_request=  s     4;;***D.!3s4y>2G =='4//T[[00$++2O2OPPD.!3s4y>2G ''$66rG   )NNN)
rO   r   rP   z"Optional[OrchestratorStateMachine]rR   Optional[PlannerService]rS   zOptional[ActionExecutor]r   r   )r   r  r   r   )r   r   r   r   )r   r   r   r   )r   r   r   r   )r   r   r   r    r   r,   r   r   )r   r   r   r,   )r   r    r   r7   r   r7   r   r    r   r,   r   r    )r   r    r   r_   r   r_   r   r   )rz   r  r   r   )r   r    r   r   r   )r   r  r   r   )r:  zasyncio.Task[None]r   r   )rF  r   rG  r    r   r   )r   r   r   Optional[Dict[str, Any]])ry  r    r~   r  r   r  )rG  r    r~  r   r   z(Optional[Tuple[HTTPStatus, list, bytes]])rB   rC   rD   ri   rQ   rc   rg   rd   r   r   r   r   r   r   r   r   r   r   r  r   rf   r1  r/  r2  rA  rC  rr  r3  rF   rG   rH   rJ   rJ   4   s    =A,0-1$,$, :$, *	$,
 +$, 
$,LG&=0=M@D
5
n2
2
 	2

 2
 
2
hV
 V
 	V

 "V
 V
 V
 
V
p
 
 	

 "
 
 
 

"

).
:?
	
.H6TS	Q(]T#J"bHI%DNKQZ/Qb
R+.	1rG   rJ   c                *   | xs i } t        | j                  dd      | j                  dd      t        | j                  dd            | j                  d      xs g d| j                  d	      | j                  d
      | j                  di             S )Nrobot_idrobotcurrent_locationr   battery_leveld   available_actions)CHECK_BATTERYNAVIGATE	SCAN_AREAVERIFY_OBJECTSPEAKWAITIDENTIFY_PERSONALERT_OPERATORvenue_idmission_typeadditional_constraints)r  r  r  r  r  r  r  )r   r   r"   )r~   s    rH   r  r  S  s    mGZ1 %7C'++os;<!++&9: 

	
 Z([[0&{{+CRH# rG   c                
   | j                   | j                  | j                  | j                  j	                         D ci c]5  \  }}||j
                  |j                  |j                  |j                  d7 c}}dS c c}}w )N)actionr   outputr   )r  r  r  results)	r  r  r  r  itemsr  r   r  r   )r  r  outcomes      rH   rt  rt  j  sw    >>>>-- %+NN$8$8$:
 ! !..!..!.. 	 
	 
s   :A?c                    | j                   | j                  | j                  | j                  | j                  | j
                  | j                  dS )Nr  rz   is_safe_noopattemptstotal_latency_mstrace_idfailure_reasonr  )r9  s    rH   rs  rs  {  sC    >>++OO"33OO // rG   c                t    t        j                  |       }||S |j                         j                         dv S )N>   1onyestrue)r`   ra   r   r   )namedefaultrB  s      rH   	_env_flagr    s4    IIdOE};;= $>>>rG   c            
     L
   t        j                  d      } | j                  dt        j                  dd             | j                  dt
        t        t        j                  dd	            
       | j                  dt        j                  dd             | j                  dt        j                  dd             | j                  ddt        dd             | j                  ddt        dd             | j                  ddt        dd             | j                  dt        j                  dd             | j                  dt
        t        t        j                  dd            
       | j                  d t        j                  d!d"             | j                  d#t        j                  d$d%             | j                  d&t        j                  d'd(             | j                  d)t        j                  d*             | j                  d+t
        t        t        j                  d,d-            
       | j                  d.dt        d/d             | j                  d0t        j                  d1             | j                  d2t
        t        j                  d3      rt        t        j                  d3            nd 
       | j                  d4t        t        j                  d5      rt        t        j                  d5            nd 
       | j                  d6t
        t        j                  d7      rt        t        j                  d7            nd 
       | j                  d8dt        d9d             | j                  d:dt        d;d             | j                         }t        j                  |j                  <       t        dQi d=|j                  d>|j                  d?|j                  d@|j                   dA|j"                  dB|j$                   dC|j&                  dD|j(                  dE|j*                  dF|j,                  dG|j.                  dH|j0                  dI|j2                  dJ|j4                   dK|j6                  dL|j8                  dM|j:                  dN|j<                  dO|j>                  dP|j@                  }tC        |      }	 tE        jF                  |jI                                y # tJ        $ r Y y w xY w)RNzOrchestrator WebSocket server)descriptionz--hostORCHESTRATOR_HOSTr   )r  z--portORCHESTRATOR_PORT8000)r   r  z	--ws-pathORCHESTRATOR_WS_PATHr%   z--dashboard-ws-pathORCHESTRATOR_DASHBOARD_WS_PATHr'   z--enable-planner
store_trueORCHESTRATOR_ENABLE_PLANNERF)r  r  z--disable-executorORCHESTRATOR_DISABLE_EXECUTORz--enable-edge-proxyORCHESTRATOR_ENABLE_EDGE_PROXYz--edge-proxy-hostORCHESTRATOR_EDGE_PROXY_HOSTr0   z--edge-proxy-portORCHESTRATOR_EDGE_PROXY_PORT8080z--log-levelORCHESTRATOR_LOG_LEVELINFOz--llm-endpointORCHESTRATOR_LLM_ENDPOINTr3   z--llm-modelORCHESTRATOR_LLM_MODELr5   z--llm-api-keyORCHESTRATOR_LLM_API_KEYz--llm-timeout-sORCHESTRATOR_LLM_TIMEOUT_S30z--llm-no-response-format#ORCHESTRATOR_LLM_NO_RESPONSE_FORMATz--llm-accept-language ORCHESTRATOR_LLM_ACCEPT_LANGUAGEz--llm-max-tokensORCHESTRATOR_LLM_MAX_TOKENSz--llm-top-pORCHESTRATOR_LLM_TOP_Pz--llm-top-kORCHESTRATOR_LLM_TOP_Kz--llm-use-openai-sdkORCHESTRATOR_LLM_USE_OPENAI_SDKz--llm-streamORCHESTRATOR_LLM_STREAM)levelr!   r#   r&   r(   r-   r.   r/   r1   r2   r4   r6   r8   r:   r;   r<   r=   r>   r?   r@   rA   rF   )&argparseArgumentParseradd_argumentr`   ra   r"   r  r_   
parse_argsrT   basicConfig	log_levelr   r!   r#   r&   r(   r-   disable_executorr/   r1   r2   r4   r6   r8   r:   llm_no_response_formatr<   r=   r>   r?   r@   rA   rJ   r   runr1  KeyboardInterrupt)parserargsrO   servers       rH   mainr    s    $$1PQF
"))4G*ST
BII16:;  
 		0%8   		:LI   7?  
 95A  
 :EB  
 		8+F   BII<fEF  
 		2F;   		57gh   		24LM   		45   BII:DAB  
 "?G  
 		<=   ACKhAiBII;<=os  
 >@iiH`>abii 89:gk  
 <>IIF^<_BII678ei  
 ;UC  
 3U;   Ddnn- YYYY  00	
 ** !111 00 ,, ,, && .. $$ (( )-(C(C$C !44  **!" ..#$ ..%&  22'( ??)F.  'FFLLN# s   3#T 	T#"T#__main__)r~   r  r   r   )r  r   r   r  )r9  r   r   r  )F)r  r    r  r,   r   r,   r  )4
__future__r   r  r   dataclassesr   httpr   r   rT   r`   typingr   r   r   r	   r
   r0  websockets.serverr   executor.action_executorr   r   planner.llm_clientr   r   planner.plannerr   r   r   rP   r   r   r   r   edge_proxy.clientr   r   edge_proxy.messagesr   r   r   services.fr_analyzerr   r   rJ   r  rt  rs  r  r  rB   rF   rG   rH   <module>r     s    "   !    	 2 2  5 D 3 D D j j ; V V 3   6\ \~."	?D zF rG   