from pathlib import Path
import sys,re,math,heapq,json,uuid,shutil
R=Path(__file__).resolve().parent; B=R.parent
sys.path.insert(0,str(B/'parity_metadata_closure_A22'))
from prepare_candidates import forms,run
from align_and_verify import footprint_rows
P=R/'board/led_driver.kicad_pcb'; s=P.read_text()
if (R/'checks.json').exists():raise RuntimeError('Final candidate present: experimental route replay requires an isolated input copy')
shutil.copy2(P,R/'before_grid_board.kicad_pcb')
step=.1;nx=940;ny=900;x0=1.;y0=1.
tasks=[('/LED_K29',(40.,25.24),(44.,46.85),1,1)]
def padpoints(text):
 out={}
 for _,_,f,ref,_ in footprint_rows(text):
  m=re.search(r'\(at ([\d.-]+) ([\d.-]+)(?: ([\d.-]+))?\)',f);fx,fy=float(m[1]),float(m[2]);fa=math.radians(float(m[3] or 0));ca,sa=math.cos(fa),math.sin(fa)
  for _,_,p in forms(f,'pad'):
   pin=re.match(r'\(pad "([^"]*)"',p)[1];m=re.search(r'\(at ([\d.-]+) ([\d.-]+)',p);px,py=map(float,m.groups());net=re.search(r'\(net "([^"]+)"',p)
   if net:out[(ref,pin)]=(fx+px*ca+py*sa,fy-px*sa+py*ca,net[1])
 return out
def distseg(x,y,a,b):
 dx=b[0]-a[0];dy=b[1]-a[1];t=max(0,min(1,((x-a[0])*dx+(y-a[1])*dy)/(dx*dx+dy*dy))) if dx*dx+dy*dy else 0
 return math.hypot(x-a[0]-t*dx,y-a[1]-t*dy)
def obstacles(net):
 obs=[set(),set()];vo=[set(),set()]
 def raster(bounds,layers,pred,margin,target):
  lo,hi,bo,to=bounds
  for i in range(max(0,math.floor((lo-margin-x0)/step)),min(nx,math.ceil((hi+margin-x0)/step))+1):
   for j in range(max(0,math.floor((bo-margin-y0)/step)),min(ny,math.ceil((to+margin-y0)/step))+1):
    if pred(x0+i*step,y0+j*step,margin):
     for l in layers:target[l].add((i,j))
 def add(bounds,layers,pred):
  raster(bounds,layers,pred,.31,obs);raster(bounds,layers,pred,.51,vo)
 for _,_,z in forms(s,'segment'):
  if '(net "'+net+'")' in z:continue
  a=tuple(map(float,re.search(r'\(start ([\d.-]+) ([\d.-]+)',z).groups()));b=tuple(map(float,re.search(r'\(end ([\d.-]+) ([\d.-]+)',z).groups()));w=float(re.search(r'\(width ([\d.-]+)',z)[1]);l=0 if '(layer "F.Cu")' in z else 1
  add((min(a[0],b[0])-w/2,max(a[0],b[0])+w/2,min(a[1],b[1])-w/2,max(a[1],b[1])+w/2),[l],lambda x,y,m,a=a,b=b,w=w:distseg(x,y,a,b)<=m+w/2)
 for _,_,z in forms(s,'via'):
  same_net='(net "'+net+'")' in z
  xy=tuple(map(float,re.search(r'\(at ([\d.-]+) ([\d.-]+)',z).groups()));rad=float(re.search(r'\(size ([\d.-]+)',z)[1])/2
  bounds=(xy[0]-rad,xy[0]+rad,xy[1]-rad,xy[1]+rad)
  pred=lambda x,y,m,xy=xy,rad=rad:math.hypot(x-xy[0],y-xy[1])<=rad+m
  if same_net:raster(bounds,[0,1],pred,.56,vo)
  else:add(bounds,[0,1],pred)
 for _,_,f,ref,_ in footprint_rows(s):
  m=re.search(r'\(at ([\d.-]+) ([\d.-]+)(?: ([\d.-]+))?\)',f);fx,fy=float(m[1]),float(m[2]);fa=math.radians(float(m[3] or 0));ca,sa=math.cos(fa),math.sin(fa)
  for _,_,p in forms(f,'pad'):
   if '(net "'+net+'")' in p:continue
   if not any(x in p for x in ['"F.Cu"','"B.Cu"','"*.Cu"']):continue
   pm=re.search(r'\(at ([\d.-]+) ([\d.-]+)(?: ([\d.-]+))?\)',p);px,py=float(pm[1]),float(pm[2]);cx=fx+px*ca+py*sa;cy=fy-px*sa+py*ca;w,h=map(float,re.search(r'\(size ([\d.-]+) ([\d.-]+)',p).groups());angle=math.radians(float(pm[3] or 0));co,si=math.cos(angle),math.sin(angle)
   ls=[0,1] if '"*.Cu"' in p else [0] if '"F.Cu"' in p else [1]
   def pred(x,y,m,cx=cx,cy=cy,w=w,h=h,co=co,si=si):
    xx=(x-cx)*co-(y-cy)*si;yy=(x-cx)*si+(y-cy)*co
    return math.hypot(max(abs(xx)-w/2,0),max(abs(yy)-h/2,0))<=m
   rad=math.hypot(w,h)/2;add((cx-rad,cx+rad,cy-rad,cy+rad),ls,pred)
 return obs,vo
routes=[]
for k,(net,a,b,al,bl) in enumerate(tasks):
 print('Routing',k,net,a,b,flush=True);obs,vo=obstacles(net)
 portals={}
 for _,_,z in forms(s,'via'):
  if '(net "'+net+'")' not in z:continue
  xy=tuple(map(float,re.search(r'\(at ([\d.-]+) ([\d.-]+)',z).groups()))
  portals[(round((xy[0]-x0)/step),round((xy[1]-y0)/step))]=xy
 def pt(xy,l):return(round((xy[0]-x0)/step),round((xy[1]-y0)/step),l)
 for _,_,f,ref,_ in footprint_rows(s):
  for _,_,pad in forms(f,'pad'):
   if 'thru_hole' not in pad or '(net "'+net+'")' not in pad:continue
   pin=re.match(r'\(pad "([^"]*)"',pad)[1];xy=padpoints(s)[(ref,pin)][:2]
   portals[(round((xy[0]-x0)/step),round((xy[1]-y0)/step))]=xy
 start=pt(a,al);goal=pt(b,bl);
 if start[:2] in obs[al] or goal[:2] in obs[bl]:
  print('blocked endpoint; skipped',flush=True);continue
 q=[(0,0,start)];cost={start:0};prev={};found=None
 while q:
  _,c,p=heapq.heappop(q)
  if c!=cost[p]:continue
  if p==goal:found=p;break
  i,j,l=p
  for di,dj in [(1,0),(-1,0),(0,1),(0,-1),(1,1),(-1,1),(1,-1),(-1,-1)]:
   if di==dj==0:
    if (i,j) not in portals and ((i,j) in vo[0] or (i,j) in vo[1]):continue
    v=(i,j,1-l);inc=25
   else:
    ii,jj=i+di,j+dj
    if not 0<=ii<=nx or not 0<=jj<=ny or (ii,jj) in obs[l]:continue
    if di and dj and ((i+di,j) in obs[l] or (i,j+dj) in obs[l]):continue
    v=(ii,jj,l);inc=math.hypot(di,dj)
   cc=c+inc
   if cc>=cost.get(v,1e30):continue
   cost[v]=cc;prev[v]=p;hh=math.hypot(v[0]-goal[0],v[1]-goal[1])+(10 if v[2]!=goal[2] else 0);heapq.heappush(q,(cc+hh,cc,v))
 
 if not found:
  print('No route; skipped',flush=True);continue
 path=[found]
 while path[-1]!=start:path.append(prev[path[-1]])
 path.reverse();simple=[path[0]]
 for i in range(1,len(path)-1):
  if tuple(path[i][j]-path[i-1][j] for j in range(3))!=tuple(path[i+1][j]-path[i][j] for j in range(3)):simple.append(path[i])
 simple.append(path[-1]);coords=[(*(portals[(i,j)] if (i,j) in portals else (round(x0+i*step,5),round(y0+j*step,5))),l) for i,j,l in simple];coords=[(*a,al)]+coords+[(*b,bl)]
 # A bottom-plane termination requires an actual plated via, even if A* ended
 # immediately after a layer transition elsewhere then a short bottom trace.
 if net=="/GND":coords.append((*b,1))
 adds=[]
 for u,v in zip(coords,coords[1:]):
  if u==v:continue
  uid=str(uuid.uuid4())
  if u[2]!=v[2] and any(math.hypot(u[0]-xy[0],u[1]-xy[1])<.00001 for xy in portals.values()):continue
  if u[2]!=v[2]:adds.append('(via (at %.5f %.5f) (size 0.6) (drill 0.3) (layers "F.Cu" "B.Cu") (net "%s") (uuid "%s"))'%(*u[:2],net,uid))
  else:adds.append('(segment (start %.5f %.5f) (end %.5f %.5f) (width 0.2) (layer "%s") (net "%s") (uuid "%s"))'%(*u[:2],*v[:2],'F.Cu' if u[2]==0 else 'B.Cu',net,uid))
 s=s.rstrip()[:-1]+'\n'+'\n'.join(adds)+'\n)\n';P.write_text(s);routes.append({'net':net,'coords':coords});print('done',len(coords),'points',flush=True)
(R/'route_points.json').write_text(json.dumps(routes,indent=2)+'\n')
