# Implementation of module {contact}
# Last edited on 2021-05-10 15:23:59 by jstolfi

import contact
import move
import move_parms
import path
import block
import hacks
import rn
import pyx 
from math import nan, inf, sqrt
import sys

class Contact_IMP:
  # The endpoints of the contact are {ct.pt[0]} and {ct.pt[1]}, in 
  # arbitrary order.
  #
  # The contact is between traces {ct.mv[0]} and {ct.mv[1]}, 
  # two distinct unoriented {Move} objects that are traces (not jumps).
  #
  # The field {ct.tcov} is a pair (2-tuple) of times, such
  # that {ct.tcov[i]} is the time for the nozzle to go past the midpont of
  # the contact when tracing the move {ct.mv[i]} in its native direction.

  def __init__(self, p0, p1, mv0, tc0, mv1, tc1):
    # Read-only fields:
    self.pts = (p0, p1)
    self.mv = (mv0, mv1)
    self.tcov = (tc0, tc1)
    self.name = None
    # Mutable fields for the {hotpath} module:
    self.iscov = [False, False] # Status of contact rel to current {Q}.
    self.bc = [None, None]      # Input blocks that contain the sides of this contact.
    self.ophss = [[], []]       # Input block choices that contain sides of this contact.
    self.rl = [None,None]       # Elis?
    
def make(p0, p1, mv0, mv1):
  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 isinstance(mv0, move.Move) and not move.is_jump(mv0)
  assert isinstance(mv1, move.Move) and not move.is_jump(mv1)
  assert mv0 != mv1
  m = rn.mix(0.5, p0, 0.5, p1)
  tc0 = move.cover_time(mv0, m)
  tc1 = move.cover_time(mv1, m)
  ct = contact.Contact(p0, p1, mv0, tc0, mv1, tc1)
  return ct

def make_elis(p0, p1, mv0, mv1, bc0, bc1, rl0, rl1):
  ct = make(p0, p1, mv0, mv1)
  ct.bc = (bc0, bc1)
  ct.rl = (rl0, rl1)
  return ct 

def endpoints(ct):
  assert isinstance(ct, contact.Contact)
  return ct.pts

def pmid(ct):
  return rn.mix(0.5, ct.pts[0], 0.5, ct.pts[1])

def side(ct, i):
  return ct.mv[i]

def tcov(ct, i):
  return ct.tcov[i]

def which_side(mv, ct):
  assert isinstance(mv, move.Move)
  for i in range(2):
    if ct.mv[i] == mv:
      return i
  return None 
 
def from_moves(mv0, mv1, szmin, rszmin):
  assert isinstance(mv0, move.Move)
  assert isinstance(mv1, move.Move)
  if move.is_jump(mv0) or move.is_jump(mv1): return None
  p0, p1 = move.shared_border(mv0, mv1)
  if p0 == None: return None
  assert p1 != None
  dp = rn.dist(p0, p1)
  if dp == 0: return None
  if szmin > 0 and dp < szmin: return None
  L0 = move.length(mv0)
  L1 = move.length(mv1)
  if rszmin > 0 and dp < rszmin*min(L0,L1): return None
  ct = make(p0, p1, mv0, mv1)
  return ct
  # ----------------------------------------------------------------------

def from_move_lists(MVS0, MVS1, szmin, rszmin):
  assert type(MVS0) is tuple or type(MVS0) is list
  assert type(MVS1) is tuple or type(MVS1) is list
  CTS = []
  for mv0 in MVS0:
    for mv1 in  MVS1:
      ct = from_moves(mv0, mv1, szmin, rszmin)
      if ct != None: CTS.append(ct)
  return CTS
  # ----------------------------------------------------------------------

def from_paths(oph0, oph1, szmin, rszmin):
  ph0, dr0 = path.unpack(oph1) # For the typechecking.
  ph1, dr1 = path.unpack(oph1) # For the typechecking.
  CTS = []
  for imv0 in range(path.nelems(oph0)):
    for imv1 in range(path.nelems(oph1)):
      mv0, dr0 = move.unpack(path.elem(oph0, imv0)) 
      mv1, dr1 = move.unpack(path.elem(oph1, imv1)) 
      ct = from_moves(mv0, mv1, szmin, rszmin)
      if ct != None: CTS.append(ct)
  return CTS
  # ----------------------------------------------------------------------

def bbox(CTS):
  B = None
  for ct in CTS:
    B = rn.box_include_point(B, ct.pts[0])
    B = rn.box_include_point(B, ct.pts[1])
  return B
  # ----------------------------------------------------------------------
  
def covindices(oph, ct):
  ixs = [None, None]
  n = path.nelems(oph)
  for imv in range(n):
    omv = path.elem(oph, imv)
    mv, dr = move.unpack(omv)
    for i in range(2):
      if side(ct, i) == mv:
        assert ixs[i] == None, "repeated move in path"
        ixs[i] = imv
  return tuple(ixs)

def covtime(oph, imv, ct, i):
  assert isinstance(ct, contact.Contact)
  ph, dr_ph = path.unpack(oph) # For the typechecking.
  if imv == None:
    tc = None
  else:
    mv = side(ct, i)
    omv = path.elem(oph, imv)
    mv, dr = move.unpack(omv)
    if dr == 0:
      tc =  path.tini(oph, imv) + ct.tcov[i]
    else:
      tc = path.tfin(oph, imv) - ct.tcov[i]
  return tc

def covtimes(oph, ct):
  ixs = covindices(oph, ct)
  assert len(ixs) == 2
  tcs = [ None, None ]
  for i in range(2):
    imv = ixs[i]
    if imv != None:
      omv = path.elem(oph, imv)
      mv, dr = move.unpack(omv)
      if dr == 0:
        tcs[i] =  path.tini(oph, imv) + ct.tcov[i]
      else:
        tcs[i] = path.tfin(oph, imv) - ct.tcov[i]
  return tuple(tcs)

def tcool(oph, ct):
  tcs = covtimes(oph, ct)
  assert type(tcs) is list or type(tcs) is tuple
  assert len(tcs) == 2
  if tcs[0] != None and tcs[1] != None:
    return abs(tcs[0] - tcs[1])
  else:
    return None

def pair_tcool(oph0, tconn, oph1, ct):
  # Get the two sides of {ct}:
  imv0 = path.find(oph0,side(ct,0))
  imv1 = path.find(oph1,side(ct,1))
  if imv0 == None or imv1 == None:
    tc = None
  else:
    tc0 = covtime(oph0,imv0,ct,0)
    tc1 = covtime(oph1,imv1,ct,1)
    tc1 += path.extime(oph0) + tconn
    assert tc1 >= tc0
    tc = tc1 - tc0
  return tc
  # ----------------------------------------------------------------------

def max_tcool(oph, CTS):
  assert type(CTS) is list or type(CTS) is tuple
  tmax = -inf
  for ct in CTS:
    tc = tcool(oph, ct)
    if tc != None and tc > tmax: 
      tmax = tc
  return tmax
  # ----------------------------------------------------------------------

def pair_max_tcool(oph0, tconn, oph1, CTS):
  assert type(CTS) is list or type(CTS) is tuple
  tmax = -inf
  for ct in CTS:
    tc = pair_tcool(oph0, tconn, oph1, ct)
    if tc != None and tc > tmax: 
      tmax = tc
  return tmax
  # ----------------------------------------------------------------------
  
def min_tcov(oph, CTS):
  assert type(CTS) is list or type(CTS) is tuple
  tmin = +inf
  for ct in CTS:
    tcs = covtimes(oph, ct)
    if (tcs[0] == None) != (tcs[1] == None):
      tci = tcs[0] if tcs[0] != None else tcs[1]
      if tci < tmin: tmin = tci
  return tmin

# PLOTTING

def plot_to_files(fname, CTS, clr, OPHS, CLRS, wd_axes, tics, arrows):

  assert type(CTS) is list or type(CTS) is tuple
  assert type(OPHS) is list or type(OPHS) is tuple
  assert len(OPHS) > 0

  # Compute the plot's bounding box:
  B = path.bbox(OPHS)
  B = rn.box_join(B, bbox(CTS))
  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)

  # Plot the paths:
  axes = True
  dots = True
  ph_arrows = True
  matter = True

  path.plot_standard(c, OPHS, None, None, CLRS, wd_axes, axes, dots, ph_arrows, matter)
  
  # Plot the contacts:
  wd_ct = 1.5*wd_axes
  sz_tics = wd_ct if tics else 0
  ct_arrows = arrows
  for ct in CTS:
    plot_single(c, ct, None, clr, wd_line=wd_ct, sz_tic=sz_tics, arrow=ct_arrows)
  hacks.write_plot(c, fname)
  return
  # ----------------------------------------------------------------------

def plot_single(c, ct, dp, clr, wd_line, sz_tic, arrow): 
  p = ct.pts[0]
  q = ct.pts[1]
  dpq = rn.dist(p,q)
  peps = 0.01*wd_line if dpq < 1.0e-6 else 0 # Perturbation for equal points.
  sty_basic = [
    pyx.style.linecap.round, 
    clr,
  ]
  sty_line = sty_basic + [ pyx.style.linewidth(wd_line), ]
  if dp != None: sty_line.append(pyx.trafo.translate(dp[0], dp[1]))
  c.stroke(pyx.path.line(p[0]-peps, p[1]-peps, q[0]+peps, q[1]+peps), sty_line)

  # Should we plot a transversal tic or arrowhead?
  if sz_tic == None: sz_tic = 0 # Simplification.
  if sz_tic > 0 or arrow:
    # Plot the transversal tic or arrowhead:
    m = rn.mix(0.5, p, 0.5, q)  # Midpoint.
    u = get_perp_dir(m, ct.mv[0], ct.mv[1])
    sz_arrow = 3*wd_line if arrow else 0
    # We need a tic with a certain min size for the arrowhead:
    sz_tic = max(sz_tic, 0.80*sz_arrow)
    a = rn.mix(1.0, m, -0.5*sz_tic, u)
    b = rn.mix(1.0, m, +0.5*sz_tic, u)
    sty_tic = sty_basic
    if sz_arrow > 0:
      # Add the arrowhead to the tic.
      arrowpos = 0.5  # Position of arrow on transversal line.
      wd_arrow = sz_arrow/5 # Linewidth for stroking the arrowhead (guess).
      sty_arrow = sty_basic + [ 
        pyx.deco.stroked([pyx.style.linewidth(wd_arrow), pyx.style.linejoin.round]),
        pyx.deco.filled([])
      ]
      sty_tic = sty_tic + \
        [ 
          pyx.deco.earrow(sty_arrow, size=sz_arrow, constriction=None, pos=arrowpos, angle=35)
        ]
      sys.stderr.write("sz_arrow = %.3f wd_arrow = %3f sz_tic = %.3f\n" % (sz_arrow, wd_arrow, sz_tic))
    
    sty_tic = sty_tic + [ pyx.style.linewidth(wd_line), ]
    if dp != None: sty_tic.append(pyx.trafo.translate(dp[0], dp[1]))
    c.stroke(pyx.path.line(a[0], a[1], b[0], b[1]), sty_tic)

def get_perp_dir(m, mv0, mv1):
  # Returns the direction from trace {mv0} towards trace{mv1}
  # at the point {m}, assumed to be the midpoint of a contact
  # between them.

  sys.stderr.write("m = ( %.3f %.3f )\n" % ( m[0], m[1],))
  assert hacks.is_point(m)
  assert mv0 != mv1, "both sides on same move?"
  a = [None,None]
  for i in range(2):
    mvi = (mv0,mv1)[i]
    p0i, p1i = move.endpoints(mvi)
    r = min(1, max(0, rn.pos_on_line(p0i, p1i, m))) # Nearest rel pos in move to {m}
    a[i] = rn.mix(1-r, p0i, r, p1i)
  assert a[0] != a[1]
  sys.stderr.write("a = ( %.3f %.3f ) ( %.3f %.3f )\n" % ( a[0][0], a[0][1], a[1][0], a[1][1],))
  u, da = rn.dir(rn.sub(a[1],a[0]))
  return u
  # ----------------------------------------------------------------------

def plot_elis(c, ct, dp, clr, wd_line, sz_tic, arrow): 
  p = ct.pts[0]
  q = ct.pts[1]
  dpq = rn.dist(p,q)
  peps = 0.01*wd_line if dpq < 1.0e-6 else 0 # Perturbation for equal points.
  sty_basic = [
    pyx.style.linecap.round, 
    clr,
  ]
  sty_line = sty_basic + [ pyx.style.linewidth(wd_line), ]
  if dp != None: sty_line.append(pyx.trafo.translate(dp[0], dp[1]))
  c.stroke(pyx.path.line(p[0]-peps, p[1]-peps, q[0]+peps, q[1]+peps), sty_line)

  # Should we plot a transversal tic or arrowhead?
  if sz_tic == None: sz_tic = 0 # Simplification.
  if sz_tic > 0 or arrow:
    # Plot the transversal tic or arrowhead:
    m = rn.mix(0.5, p, 0.5, q)  # Midpoint.
    u = get_perp_dir(m, ct.mv[0], ct.mv[1])
    sz_arrow = 3*wd_line if arrow else 0
    # We need a tic with a certain min size for the arrowhead:
    sz_tic = max(sz_tic, 0.80*sz_arrow)
    a = rn.mix(1.0, m, -0.5*sz_tic, u)
    b = rn.mix(1.0, m, +0.5*sz_tic, u)
    sty_tic = sty_basic
    if sz_arrow > 0:
      # Add the arrowhead to the tic.
      arrowpos = 0.5  # Position of arrow on transversal line.
      wd_arrow = sz_arrow/5 # Linewidth for stroking the arrowhead (guess).
      sty_arrow = sty_basic + [ 
        pyx.deco.stroked([pyx.style.linewidth(wd_arrow), pyx.style.linejoin.round]),
        pyx.deco.filled([])
      ]
      sty_tic = sty_tic + \
        [ 
          pyx.deco.earrow(sty_arrow, size=sz_arrow, constriction=None, pos=arrowpos, angle=35)
        ]
      sys.stderr.write("sz_arrow = %.3f wd_arrow = %3f sz_tic = %.3f\n" % (sz_arrow, wd_arrow, sz_tic))
    
    sty_tic = sty_tic + [ pyx.style.linewidth(wd_line), ]
    if dp != None: sty_tic.append(pyx.trafo.translate(dp[0], dp[1]))
    c.stroke(pyx.path.line(a[0], a[1], b[0], b[1]), sty_tic)

def plot_link_elis(c, oph, clr, wd):
  for k in range(path.nelems(oph)):
    omvk = path.elem(oph, k)
    move.plot_layer(c, omvk, None, clr, wd, False, 0, 0)

  return

def get_name(ct):
  assert isinstance(ct, contact.Contact)
  name = ct.name
  if name == None: name = "C?"
  return name
  # ----------------------------------------------------------------------

def set_name(ct, name):
  assert type(name) is str
  ct.name = name
  return
  # ----------------------------------------------------------------------

def show(wr, ct, ind, wna):
  wr.write("  "*ind)
  wr.write("%-*s" % (wna,get_name(ct)))
  for i in range(2):
    pti = ct.pts[i]
    wr.write(" ( %6.3f, %.3f )" % (pti[0], pti[1]))
  wr.write(" ")
  for i in range(2):
    mvi = ct.mv[i]
    if i > 0:  wr.write(",")
    wr.write(move.get_name(mvi))
  return
  # ----------------------------------------------------------------------

def show_list(wr, CTS):
  assert type(CTS) is list or type (CTS) is tuple
  nct = len(CTS)
  if nct == 0: return
  wna = 4 # Width of "name" column; min 4 because of the header.
  for ct in CTS: wna = max(wna, len(get_name(ct)))
  wix = len(str(nct-1)) # Num digits in index.
  
  wr.write("\n")

  # Write header:
  wr.write("%*s %-*s %*s %*s sides\n" % (wix,"k",wna,"name",15,"pini",15,"pfin"))
  wr.write("%s %s %s %s --------------\n" % ("-"*wix,"-"*wna,"-"*15,"-"*15))
  
  # Write contacts:
  for k in range(len(CTS)):
    ct = CTS[k]
    wr.write("%*d " % (wix,k))
    show(wr, ct, 0, wna)
    wr.write("\n")

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