# Implementation of module {move}
# Last edited on 2021-05-10 15:24:19 by jstolfi

import move

import move_parms
import hacks
import rn

import sys
import pyx 
from math import nan, inf, sqrt

class Move_IMP:
  # The field {mv.endpts} is a 2-tuple with the two endpoints of the 
  # axis, in arbitrary order.   An oriented move {(mv,dr)} means 
  # that the motion is from {mv.endpts[dr]} to {mv.endpts[1-dr]}.
  #
  # The field {mv.mp} is a {Move_Parms} object that specifies the width 
  # and timing parameters of the move.
  # The field {mv.extime} is the time (in seconds) needed to execute it.

  def __init__(self, p0, p1, mp, tex):
    self.endpts = (p0, p1)
    self.mp = mp
    self.extime = tex
    self.name = None
    # ----------------------------------------------------------------------
    # Fields for the {hotpath} module (see {move_hp_IMP.py):
    self.hp_paths = None  # List of input paths that own the move and its indices therein.

def make(p0, p1, mp):
  assert hacks.is_point(p0); p0 = tuple(p0) # Make sure it is immutable.
  assert hacks.is_point(p1); p1 = tuple(p1) # Make sure it is immutable.
  assert mp != None and isinstance(mp, move_parms.Move_Parms)
  dpq = rn.dist(p0, p1)
  tex = move_parms.nozzle_travel_time(dpq, None, mp)
  return move.Move(p0, p1, mp, tex)

def parameters(omv):
  mv, dr = unpack(omv)
  return mv.mp

def is_jump(omv):
  mv, dr = unpack(omv)
  return move_parms.width(mv.mp) == 0

def width(omv):
  mv, dr = unpack(omv)
  return move_parms.width(mv.mp)

def pini(omv):
  mv, dr = unpack(omv)
  return mv.endpts[dr]

def pfin(omv):
  mv, dr = unpack(omv)
  return mv.endpts[1-dr]
  # ----------------------------------------------------------------------

def endpoints(omv):
  mv, dr = unpack(omv)
  pts = (mv.endpts[dr], mv.endpts[1-dr])
  return pts
  # ----------------------------------------------------------------------

def length(omv):
  mv, dr = unpack(omv)
  L = rn.dist(mv.endpts[0], mv.endpts[1])
  return L
  # ----------------------------------------------------------------------

def get_name(omv):
  mv, dr = unpack(omv)
  name = mv.name
  if name == None: name = "J?" if is_jump(mv) else "T?"
  if dr == 1: name = "~" + name
  return name
  # ----------------------------------------------------------------------

def set_name(omv, name):
  assert type(name) is str
  mv, dr = unpack(omv)
  mv.name = name
  return
  # ----------------------------------------------------------------------

def bbox(OMVS):
  B = None
  for omv in OMVS:
    B = rn.box_include_point(B, pini(omv))
    B = rn.box_include_point(B, pfin(omv))
  return B
  # ----------------------------------------------------------------------

def rev(omv):  
  mv, dr = unpack(omv)
  return (mv, 1-dr)

def spin(omv, dr):
  mv1, dr1 = unpack(omv)
  return (mv1, (dr1 + dr) % 2)

def unpack(omv):
  if isinstance(omv, move.Move):
    return omv, 0
  else:
    # sys.stderr.write("omv =%s\n" % str(omv))
    assert type(omv) is tuple
    assert len(omv) == 2
    mv, dr = omv
    assert isinstance(mv, move.Move)
    assert dr == 0 or dr == 1
    return mv, dr

def displace(omv, ang, disp, mp):
  p = tuple(rn.add(rn.rotate2(pini(omv), ang), disp))
  q = tuple(rn.add(rn.rotate2(pfin(omv), ang), disp))
  return make(p, q, mp)
  
def shared_border(mv0, mv1):
  if is_jump(mv0) or is_jump(mv1): 
    # sys.stderr.write("one or both moves are jumps\n")
    p0 = None; p1 = None
  else:
    e00, e01 = mv0.endpts # Endpoints of axis segment {S0} of {mv0}.
    wd0 = width(mv0)
    
    e10, e11 = mv1.endpts # Endpoints of axis segment {S1} of {mv1}.
    wd1 = width(mv1)
    
    tol = 0.10*max(wd0,wd1)
    a00,a01, a10,a11 = rn.seg_seg_overlap(e00,e01, e10,e11, tol)
    if a00 == None or a01 == None or a10 == None or a11 == None:
      # sys.stderr.write("move axes do not overlap\n")
      p0 = None; p1 = None
    else:
      d0 = rn.dist(a00,a10)
      d1 = rn.dist(a01,a11)
      assert abs(d0 - d1) < 2*tol
      if min(d0,d1) > 1.13*(wd0 + wd1)/2:
        # sys.stderr.write("segments are too far apart d0 = %.7f d1 = %.7f\n" % (d0,d1))
        p0 = None; p1 = None
      else:
        r = wd0/(wd0 + wd1)
        p0 = rn.mix(1-r, a00, r, a10)
        p1 = rn.mix(1-r, a01, r, a11)
  return (p0,p1)
  # ----------------------------------------------------------------------

def connector(omv_prev, omv_next, use_jump, use_link, mp_jump):
  mp = connector_parameters(omv_prev, omv_next, use_jump, use_link, mp_jump)
  p = pfin(omv_prev)
  q = pini(omv_next)
  mv = make(p, q, mp)
  return mv
   
def extime(omv):
  mv, dr = unpack(omv)
  return mv.extime

def ud_penalty(omv):
  mv, dr = unpack(omv)
  ac, sp, ud = move_parms.dynamics(mv.mp)
  if not is_jump(omv): assert ud == 0
  return ud
 
def cover_time(omv, m):
  mv, dr = unpack(omv) # To type-check.
  ac, sp, ud = move_parms.dynamics(mv.mp)
  # Compute cover time {tomv} from start of move {omv}:
  p, q = endpoints(omv)
  vpm = rn.sub(m, p)
  vpq = rn.sub(q, p)
  dpq = rn.norm(vpq)
  dpm = rn.dot(vpm,vpq)/dpq  # Distance from {p} to point nearest to {m}
  # sys.stderr.write("dpq = %12.8f dpm = %12.8f ratio = %12.8f\n" % (dpq, dpm, dpm/dpq))
  if dpm < 0: dpm = 0
  if dpm > dpq: dpm = dpq
  tc = move_parms.nozzle_travel_time(dpq, dpm, mv.mp)
  return tc

def connector_extime(omv_prev, omv_next, use_jump, use_link, mp_jump):
  mp = connector_parameters(omv_prev, omv_next, use_jump, use_link, mp_jump)
  p = pfin(omv_prev)
  q = pini(omv_next)
  dpq = rn.dist(p, q)
  tex = move_parms.nozzle_travel_time(dpq, None, mp)
  tex += move_parms.transition_penalty(parameters(omv_prev), mp)
  tex += move_parms.transition_penalty(mp, parameters(omv_next))
  return tex

def connector_parameters(omv_prev, omv_next, use_jump, use_link, mp_jump):
  assert not (use_jump and use_link), "conflicting {use_jump,use_link}"
  assert mp_jump == None or isinstance(mp_jump, move_parms.Move_Parms)
  p = pfin(omv_prev)
  q = pini(omv_next)
  assert p != q
  mp_prev = parameters(omv_prev)
  mp_next = parameters(omv_next)
  if use_jump or mp_prev != mp_next or move_parms.is_jump(mp_prev):
    # Use a jump:
    mp = mp_jump
  elif use_link:
    # Use a link:
    assert mp_prev == mp_next
    mp = mp_prev
  else:
    # Decide based on geometry:
    assert mp_prev == mp_next
    vpq = rn.sub(q, p)
    v_prev = rn.sub(p, pini(omv_prev))
    v_next = rn.sub(pfin(omv_next), q)
    dmax = 3*move_parms.width(mp_prev)
    if connector_must_be_jump(vpq, v_prev, v_next, dmax, mp_jump, mp_prev):
      mp = mp_jump
    else:
      mp = mp_prev
  return mp
  
def connector_must_be_jump(vpq, v_prev, v_next, dmax, mp_jump, mp_link):
  
  dpq =  rn.norm(vpq)
  
  # If the distance to be covered is too big, use a jump:
  if dpq >= dmax: 
    # sys.stderr.write("dist = %12.0f too big\n" % (dpq/dmax))
    return True
  
  # If either of the adjacent moves is too short, use a jump:
  if rn.norm(v_prev) <= dmax/2 or  rn.norm(v_next) <= dmax/2: 
    # sys.stderr.write("dists = %12.f %12.6f too small\n" % (rn.norm(v_prev)/dmax, rn.norm(v_next)/dmax))
    return True
  
  # Compute the sign of the turning angles:
  eps2 = 1.0e-6*dmax*dmax
  s1 = rn.cross2d(v_prev,vpq)
  s2 = rn.cross2d(vpq,v_next)
  if (s1 >= -eps2 and s2 <= +eps2) or (s1 <= +eps2 and s2 >= -eps2):
    # Angles have opposite signs, or nearly so -- use a jump:
    # sys.stderr.write("signs = %12.f %12.6f not consistent\n" % (s1,s2))
    return True
  
  # Risk a trace:
  return False

def plot_to_files(fname, OMVS, CLRS, wd_axes):  
  assert type(OMVS) is list or type(OMVS) is tuple
  
  # Compute the plot's bounding box:
  B = bbox(OMVS)
  pbox = hacks.round_box(B, 1)
  szx, szy = rn.box_size(pbox)
  sz_max = max(szx, szy)

  c = pyx.canvas.canvas()
  pyx.unit.set(uscale=1.00, wscale=1.00, vscale=1.00)

  wd_grid = 0.002*sz_max
  hacks.plot_grid(c, None, wd_grid, None, pbox, +5*wd_grid, 1,1)

  wd_frame = 0.003*sz_max
  hacks.plot_frame(c, pyx.color.rgb.black, wd_frame, None, pbox, -0.75*wd_frame)

  axes = True
  dots = True
  arrows = True
  matter = True

  plot_standard(c, OMVS, None, None, CLRS, wd_axes, axes, dots, arrows, matter)
  hacks.write_plot(c, fname)
  return
  # ----------------------------------------------------------------------

def plot_standard(c, OMVS, dp, layer, CLRS, wd_axes, axes, dots, arrows, matter):
  
  assert type(OMVS) is list or type(OMVS) is tuple
  nmv = len(OMVS)
  assert nmv > 0
  
  assert wd_axes != None and wd_axes > 0
  
  if CLRS == None: 
    CLRS = [ pyx.color.rgb(0.050, 0.800, 0.000), ] # Default trace color.
  else:
    assert type(CLRS) is list or type(CLRS) is tuple
  nclr = len(CLRS)
  assert nclr == 1 or nclr == nmv
  
  def pick_colors(k):
    # Returns the colors for trace sausages and axes of move {OMVS[k]}.
    if nclr == 1:
      ctrace = CLRS[0]
    else:
      ctrace = CLRS[k]
    caxis =   pyx.color.rgb(0.6*ctrace.r, 0.6*ctrace.g, 0.6*ctrace.b) # Color of trace axis, dots, arrow.
    return ctrace, caxis

  # Colors:
  cmatter = pyx.color.rgb(0.800, 0.750, 0.600) # Est. material footprint.
  cjumps = pyx.color.rgb.black                 # Axis lines, dots, and arrowheads of jumps.

  # Dimensions relative to trace nominal widths:
  rmatter = 1.13;  # Estimated material.
  rtraces =  0.80;  # Nominal trace area.
  
  # Absolute dimenstons (in mm):
  wd_dots = 2.5*wd_axes
  sz_arrows = 8*wd_axes

  # Get list {lys} of layers to plot:
  if layer == None:
    # Plot all four layers.
    lys = (range(4))
  else:
    # Plot only the selected layer.
    assert type (layer) is int
    assert layer >= 0 and layer < 4
    lys = (layer,)

  # Plot the layers:
  for ly in lys:
    # sys.stderr.write("{move.plot_standard}: layer %d\n" % ly)
    for k in range(nmv):
      omv = OMVS[k]
      mv, dr = unpack(omv) # For the typechecking.
      jmp = is_jump(omv)
      wd = width(omv)
      if ly == 0 and not jmp and matter:
        # plots the estimate of actual material:
        wd_matter = rmatter*wd
        plot_layer \
          (c, omv, dp, clr=cmatter, wd=wd_matter, dashed=False, wd_dots=0, sz_arrow=None)
      elif ly == 1 and not jmp:
        # plots the nominal trace material:
        wd_trace = rtraces*wd
        ctrace, caxis = pick_colors(k)
        plot_layer \
          (c, omv, dp, clr=ctrace, wd=wd_trace, dashed=False, wd_dots=0, sz_arrow=None)
      elif ly == 2 and not jmp and (axes or dots or arrows):
        # Trace axis and/or dots and/or arrowhead:
        ctrace, caxis = pick_colors(k)
        t_wd_axis = wd_axes if axes else 0
        t_wd_dots = wd_dots if dots else 0
        t_sz_arrow = sz_arrows if arrows else 0
        plot_layer(c, omv, dp, clr=caxis, wd=t_wd_axis, dashed=False, wd_dots=t_wd_dots, sz_arrow=t_sz_arrow)
      elif ly == 3 and jmp:
        # Jump axis, dots, and arrowhead:
        j_wd_axis = wd_axes
        j_wd_dots = wd_dots
        j_sz_arrow = sz_arrows
        plot_layer(c, omv, dp, clr=cjumps, wd=j_wd_axis, dashed=True, wd_dots=j_wd_dots, sz_arrow=j_sz_arrow)
  return
  # ----------------------------------------------------------------------

def plot_layer(c, omv, dp, clr, wd, dashed, wd_dots, sz_arrow):
  
  if clr == None: return
  
  # Simplifications:
  if wd == None: wd = 0 
  if wd_dots == None: wd_dots = 0 
  if sz_arrow == None: sz_arrow = 0
  assert wd >= 0 and wd_dots >= 0 and sz_arrow >= 0 
  
  # Get the move endpoints:
  p, q = endpoints(omv)
  vpq = rn.sub(q,p)
  dpq = rn.norm(vpq)
  
  # Omit the arrowhead if not enough space for it:
  if dpq <= 2*sz_arrow: sz_arrow = 0
  
  # Nothing to plot if nothing is requested:
  if wd == 0 and wd_dots == 0 and sz_arrow == 0: return
  
  arrowpos = 0.5  # Position of arrow along axis.

  # Perturbation to force painting of zero-length lines.
  eps = 0.0001*max(wd,wd_dots,sz_arrow)  # For plotting the dots.
  assert eps > 0
  
  if wd == 0 and sz_arrow > 0:
    # We want an invisible line but still with the arrowhead.
    # Unfortunately setting linewidth(0) still draws a thin line.
    # So we cook things up, by moving {p} and {q} right _next 
    # to the arrowhead.
    m = rn.mix(1-arrowpos, p, arrowpos, q)  # Posititon of arrow.
    shaft = 0.9*sz_arrow  # Length of reduced arrow shaft.
    pa = arrowpos*shaft/dpq
    qa = (1-arrowpos)*shaft/dpq
    paxis = rn.mix(1, m, -pa, vpq)
    qaxis = rn.mix(1, m, +qa, vpq)
  elif dpq == 0:
    # Perturb {p,q} to ensure that the zero-length line is drawn as a dot:
    paxis = rn.sub(p,(eps,eps))
    qaxis = rn.add(q,(eps,eps))
  else:
    paxis = p; qaxis = q
  
  # Define styles {sty_axis} for the axis, {sty_dots} for the dots:
  sty_comm = [ pyx.style.linecap.round, pyx.style.linejoin.round, clr, ] # Common style.
  
  sty_axis = sty_comm + [ pyx.style.linewidth(wd) ]
  if dp != None: sty_axis.append(pyx.trafo.translate(dp[0], dp[1]))

  if sz_arrow > 0: 
    # Define the arrow style {sty_deco}:
    wdarrow = sz_arrow/8 # Linewidth for stroking the arrowhead (guess).
    sty_deco = sty_comm + [ pyx.style.linewidth(wdarrow) ]
    sty_deco = sty_deco + [ pyx.deco.stroked([pyx.style.linejoin.round]), pyx.deco.filled([]) ]
    
    # Add an arrowhead in style {sty_deco} to {sty_axis}:
    sty_axis = sty_axis + [ 
      pyx.deco.earrow(sty_deco, size=sz_arrow, constriction=None, pos=arrowpos, angle=35)
    ]
  
  if wd_dots > 0:
    # Plot dots:
    sty_dots = sty_comm + [ pyx.style.linewidth(wd_dots) ]
    if dp != None: sty_dots.append(pyx.trafo.translate(dp[0], dp[1]))
    c.stroke(pyx.path.line(p[0]+eps, p[1]+eps, p[0]-eps, p[1]-eps), sty_dots)
    c.stroke(pyx.path.line(q[0]+eps, q[1]+eps, q[0]-eps, q[1]-eps), sty_dots)

  if wd > 0 and dashed: 
    # Define dash pattern, or turn off dashing if too short:
    dashed, dashpat = hacks.adjust_dash_pattern(dpq/wd, 1.75, 2.00)
    if dashed:
      # Make the line style dashed:
      sty_axis = sty_axis + [ pyx.style.linestyle(pyx.style.linecap.round, pyx.style.dash(dashpat)) ]

  if wd > 0 or sz_arrow > 0:
    # Stroke the axis, with fixed endpoints:
    c.stroke(pyx.path.line(paxis[0], paxis[1], qaxis[0], qaxis[1]), sty_axis)

# DEBUGGING AND TESTING

def describe(wr, OMVS):
  nmv = len(OMVS)
  ndg = 1 if nmv <= 10 else 2 if nmv <= 100 else 3 if nmv < 1000 else 4 # Digits for index.
  nna = 4
  for omv in OMVS: nna = max(nna, len(get_name(omv))) # Max len of name.
  
  wr.write("\n")

  # Write header:
  wr.write("%*s %-*s d %-*s %-*s  wd obs\n" % (ndg,"k",nna,"name",15,"pini",15,"pfin"))
  wr.write("%s %s - %s %s --- %s\n" % ("-"*ndg,"-"*nna,"-"*15,"-"*15,"-"*15))
  
  # Write moves:
  for k in range(len(OMVS)):
    omv = OMVS[k]
    name = get_name(omv)
    wr.write("%d %-*s" % (k,nna,name))
    mv, dr = unpack(omv)
    wr.write(" %d" % dr)
    p = pini(omv)
    q = pfin(omv)
    wr.write(" (%6.1f,%6.1f)" % (p[0],p[1]))
    wr.write(" (%6.1f,%6.1f)" % (q[0],q[1]))
    wd = width(mv)
    if wd == 0:
      wr.write(" jmp")
    else:
      wr.write(" %3.1f" % wd)
    if p == q:
      wr.write(" trivial")
    elif p[0] == q[0]:
      wr.write(" vertical")
    elif p[1] == q[1]:
      wr.write(" horizontal")
    wr.write("\n")

  wr.write("\n")
  return
  # ----------------------------------------------------------------------