#!/usr/bin/env python3
"""Local UR RTDE gateway. pip install ur-rtde; deploy behind authenticated HTTPS.
UR_HOST, ASLA_BRIDGE_TOKEN (>=24 chars), ASLA_ENABLE_MOTION=1 (optional).
Never run on a robot before site-specific commissioning and safe target validation.
"""
import os,json,time,math,hmac,threading
if not __debug__:raise RuntimeError('Do not run the robot gateway with Python -O')
from http.server import ThreadingHTTPServer,BaseHTTPRequestHandler
class Gateway:
 def __init__(self,receiver,controller_factory=None,enable_motion=False):
  self.receiver=receiver;self.factory=controller_factory;self.control=None;self.enable_motion=enable_motion;self.commands={};self.lock=threading.Lock();self.last_controller_time=None;self.last_update=time.monotonic()
 def state(self):
  r=self.receiver;controller_time=r.getTimestamp();now=time.monotonic();bits=r.getSafetyStatusBits();fresh=r.isConnected();
  if self.last_controller_time!=controller_time:self.last_update=now;self.last_controller_time=controller_time
  fresh=fresh and now-self.last_update<2;safe=fresh and bool(bits&3) and not(bits&~3) and not r.isProtectiveStopped() and not r.isEmergencyStopped();return{'connected':fresh,'q':r.getActualQ(),'tcp':r.getActualTCPPose(),'timestamp':time.time(),'controller_timestamp':controller_time,'safety_status':'normal' if safe else 'stopped','motion_enabled':bool(self.enable_motion and safe)}
 def command(self,b):
  cid=b.get('command_id');assert isinstance(cid,str) and 8<=len(cid)<=100,'command_id required'
  with self.lock:
   if cid in self.commands:return self.commands[cid]
   assert self.enable_motion,'motion disabled locally'
   if b.get('kind')=='stop':
    if self.control:self.control.stopJ(1.0)
   else:
    assert b.get('kind')=='movej','unsupported command'
    assert self.state()['motion_enabled'],'robot safety stop'
    q=b.get('q');assert isinstance(q,list) and len(q)==6 and all(isinstance(v,(int,float)) and math.isfinite(v) and abs(v)<=(math.pi if i==2 else 2*math.pi) for i,v in enumerate(q)),'joint limits'
    # Bridge also requires a target envelope, independent of cloud validation.
    current=self.receiver.getActualQ();assert max(abs(a-b)for a,b in zip(q,current))<=.35,'target exceeds 0.35rad commissioning envelope'
    speed=b.get('speed',.1);acc=b.get('acceleration',.2);assert 0<speed<=.15 and 0<acc<=.2,'speed/acceleration limits'
    if self.control is None:self.control=self.factory()
    assert self.control.moveJ(q,speed,acc,True),'controller rejected target'
   result={'accepted':True,'completed':False,'command_id':cid};self.commands[cid]=result
   if len(self.commands)>1000:self.commands.pop(next(iter(self.commands)))
   return result

def serve(gateway,token,host='127.0.0.1',port=8088):
 class Handler(BaseHTTPRequestHandler):
  def log_message(self,*args):pass
  def reply(self,status,data):
   raw=json.dumps(data).encode();self.send_response(status);self.send_header('Content-Type','application/json');self.send_header('Content-Length',str(len(raw)));self.end_headers();self.wfile.write(raw)
  def authorized(self):return hmac.compare_digest(self.headers.get('Authorization',''),'Bearer '+token)
  def do_GET(self):
   if not self.authorized():return self.reply(401,{'error':'unauthorized'})
   try:
    if self.path=='/health':return self.reply(200,{'ok':gateway.receiver.isConnected(),'service':'asla-ur-bridge','motion_enabled':gateway.enable_motion})
    if self.path=='/state':return self.reply(200,gateway.state())
    self.reply(404,{'error':'not found'})
   except Exception as e:self.reply(503,{'error':str(e)})
  def do_POST(self):
   if not self.authorized():return self.reply(401,{'error':'unauthorized'})
   try:
    assert self.path=='/command','unknown route';n=int(self.headers.get('Content-Length','0'));assert 0<n<=8192,'body limit';self.reply(200,gateway.command(json.loads(self.rfile.read(n))))
   except (AssertionError,ValueError)as e:self.reply(422,{'error':str(e)})
   except Exception as e:self.reply(503,{'error':str(e)})
 return ThreadingHTTPServer((host,port),Handler)
if __name__=='__main__':
 robot_host=os.environ.get('UR_HOST');token=os.environ.get('ASLA_BRIDGE_TOKEN','');assert robot_host and len(token)>=24,'UR_HOST and ASLA_BRIDGE_TOKEN (24+ chars) required'
 from rtde_receive import RTDEReceiveInterface
 enabled=os.environ.get('ASLA_ENABLE_MOTION')=='1'
 def control():
  from rtde_control import RTDEControlInterface
  return RTDEControlInterface(robot_host)
 gateway=Gateway(RTDEReceiveInterface(robot_host),control,enabled)
 serve(gateway,token,port=int(os.environ.get('ASLA_BRIDGE_PORT','8088'))).serve_forever()