# Implementation of module {contact}
# Last edited on 2021-10-03 15:58:57 by stolfi

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 {ctside[0]} and {ctside[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 {ctside[i]} in its native direction.

  def __init__(self, p0, p1, mv0, tcov0, mv1, tcov1, tclim):
    # Read-only fields:
    self.pts = (p0, p1)           # Endpoints.
    self.side = (mv0, mv1)        # {Move} objects on each side.
    self.tclim = tclim            # Max allowed cooling time.
    self.tcov = (tcov0, tcov1)    # {otcov[i]] is fabtime from start of side {i} to contact midpoint.
    self.name = None              # Name of contact, for debugging.
    # Mutable fields for the {hotpath} heuristic:
    self.side_paths = ([],[])     # {side_paths[i]} is list of input block choices that contain {side[i]}.
    self.blocks = [None, None]    # Two {Block} objects that contain the sides of this contact.
    self.ophss = [[], []]         # Input block choices that contain sides of this contact.
    
def make(p0, p1, omv0, omv1, tclim):
  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.
  mv0,dr0 = move.unpack(omv0); assert not move.is_jump(mv0)
  mv1,dr1 = move.unpack(omv1); assert not move.is_jump(mv1)
  assert mv0 != mv1
  m = rn.mix(0.5, p0, 0.5, p1)
  tcov0 = move.cover_time(mv0, m)
  tcov1 = move.cover_time(mv1, m)
  assert type(tclim) is float, "invalid {tclim}"
  assert tclim >= 0.001, "{tclim} too small"
  ct = contact.Contact(p0, p1, mv0, tcov0, mv1, tcov1, tclim)
  return ct

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

def tcool_limit(ct):
  return ct.tclim

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

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

def which_side(omv, ct):
  mv, dr = move.unpack(omv)
  for i in range(2):
    if ct.side[i] == mv:
      return i
  return None 

def from_moves(omv0, omv1, szmin, rszmin, tclim):
  if move.is_jump(omv0) or move.is_jump(omv1): return None
  p0, p1 = move.shared_border(omv0, omv1)
  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(omv0)
  L1 = move.length(omv1)
  if rszmin > 0 and dp < rszmin*min(L0,L1): return None
  ct = make(p0, p1, omv0, omv1, tclim)
  return ct
  # ----------------------------------------------------------------------

def from_move_lists(OMVS0, OMVS1, szmin, rszmin, tclim):
  assert type(OMVS0) is tuple or type(OMVS0) is list
  assert type(OMVS1) is tuple or type(OMVS1) is list
  CTS = []
  for omv0 in OMVS0:
    for omv1 in  OMVS1:
      ct = from_moves(omv0, omv1, szmin, rszmin, tclim)
      if ct != None: CTS.append(ct)
  for i in range(len(CTS)): contact.set_name(CTS[i], "C%d" % i)
  return CTS
  # ----------------------------------------------------------------------

def from_paths(oph0, oph1, szmin, rszmin, tclim):
  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)):
      omv0 = path.elem(oph0, imv0) 
      omv1 = path.elem(oph1, imv1) 
      ct = from_moves(omv0, omv1, szmin, rszmin, tclim)
      if ct != None: CTS.append(ct)
  for i in range(len(CTS)): contact.set_name(CTS[i], "C%d" % i)
  return CTS
  # ----------------------------------------------------------------------

def from_blocks(bc0, bc1, szmin, rszmin, tclim):
  # Collect the {Move} object lists:
  MVS = [None,None]
  for i in range(2):
    bci = (bc0, bc1)[i]
    MVS[i] = block.moves(bci)
  # Now get the contacts:
  CTS = from_move_lists(MVS[0],MVS[1], szmin,rszmin, tclim)
  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 path_ixcovs(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 is_relevant(ct, BCS, ich):
  assert ich == None or type(ich) is int
  assert type(BCS) is list or type(BCS) is tuple
  assert isinstance(ct, contact.Contact)
  for bc in BCS:
    nch = nchoices(bc)
    if ich == None or ich < nch:
      choices = (ich,) if ich != None else range(nch)
      for jch in choices:
        oph = choice(bc, jch)
        ixs = path_ixcovs(oph, ct)
        if ixs[0] != None or ixs[1] != None:
          return True
  return False
  # ----------------------------------------------------------------------

def path_tcov(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 path_tcovs(oph, ct):
  ixs = path_ixcovs(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)
  # ----------------------------------------------------------------------

# PATHS ASSOCIATED TO CONTACT SIDE

def clear_side_paths(ct):
  assert isinstance(ct, contact.Contact)
  ct.side_paths = ([],[])
  # ----------------------------------------------------------------------

def add_side_path(ct, i, oph):
  assert isinstance(ct, contact.Contact)
  mv = ct.side[i]
  assert path.find_move(oph, mv) != None, "path does not cover side {i} of {ct}"
  ct.side_paths[i].append(oph)
  # ----------------------------------------------------------------------

def get_side_paths(ct, i):
  assert isinstance(ct, contact.Contact)
  return tuple(ct.side_paths[i])
  # ----------------------------------------------------------------------

# COOLING ESTIMATORS

def rcool_closed(ct, tcs):
  # Returns the cooling time ratio of {ct} in some path {oph} that closes {ct},
  # given the corresponding cover times {tcs[0..1]}.  Fails if either of 
  # the times is {None}.
  assert tcs[0] != None and tcs[1] != None
  tc_lim = tcool_limit(ct)
  if tc_lim == +inf: return 0
  tcool = abs(tcs[0] - tcs[1])
  rcool = tcool/tc_lim
  return rcool
  # ----------------------------------------------------------------------

def est_rcool_closed_by_path(ct, i0, oph0, tc0, oph1, tc1, mp_jump):
  # Returns a lower bound for the cooling time ratio of {ct} in some path that includes {oph0} and {oph1}, 
  # in that order and direction; where {oph0} and {oph1} cover sides {i0} and {1-i0} of {ct}, respectively.
  # 
  # Assumes that {tc0} is the fabtime of {oph0} from the midpoint of
  # {ct} to the end of {oph0}, and {tc1} is the fabtime of {oph1} from
  # the beginning to the midpoint of {ct}. Fails if either is {None}.
  tc_lim = tcool_limit(ct)
  if tc_lim == +inf: return 0
  use_links = True
  tconn_est = path.connection_time(oph0, oph1, use_links, mp_jump) 
  tcool_min = tc0 + tconn_est + tc1
  rcool_min = tcool_min/tc_lim 
  return rcool_min
  # ----------------------------------------------------------------------
  
def est_rcool_closed_by_other_paths(ct, oph, i, tc, mp_jump, quick):
  # Assumes that the oriented path {oph} covers side {i} of {ct}.
  # Returns a lower bound for the cooling time ratio of {ct} among all
  # paths {P} that start with {oph} and include some paths {oph1} among
  # the paths that have been associated to side {1-i} of {ct}. The
  # procedure fails if there are no such paths.
  # 
  # Assumes that {tc} (which must not be {None}) is the fabtime of
  # {oph} from the midpoint of {ct} to the end of {oph}.
  # 
  # The {quick} parameter has the same meaning as in {est_rcool}.

  tc_lim = tcool_limit(ct)
  if tc_lim == +inf: return 0

  if quick and tc > tc_lim: return +inf  # Time to finish {oph} already exceeds limit.

  # Compute the min fabtime {tc1} to close {ct} with some choice of {bc} after finishing {oph}:
  OPHS1 = get_side_paths(ct, 1-i)
  rcool_min = +inf 
  for oph1 in OPHS1:
    tcs1 = path_tcovs(oph1, ct)
    assert tcs1[i] == None and tcs1[1-i] != None
    tc1 = tcs1[1-i]
    rcool_min = min(rcool_min, est_rcool_closed_by_path(ct, i, oph, tc, oph1, tc1, mp_jump))
  assert rcool_min < +inf, "no paths in {get_side_paths} closes {ct}"
  return rcool_min 
  # ----------------------------------------------------------------------

def est_rcool(oph, ct, mp_jump, quick):
  tcs = path_tcovs(oph, ct)
  assert type(tcs) is list or type(tcs) is tuple
  assert len(tcs) == 2
  # Compute a lower bound {tc_est} for the cooling time:
  if tcs[0] != None and tcs[1] != None:
    # Contact {ct} is closed by {oph}, the exact cooling time is known:
    rcool = rcool_closed(ct, tcs)
  elif tcs[0] != None or tcs[1] != None:
    # Only one side of {ct} is covered bt {oph}.
    # Get the fabtime from {ct} to the end of {oph}:
    i = 0 if tcs[0] != None else 1
    tc = path.fabtime(oph) - tcs[i]
    # Compute the cloosing time ratio for the best choice of {bc}
    rcool = est_rcool_closed_by_other_paths(ct, oph, i, tc, mp_jump, quick)
  else:
    rcool = -inf
  return rcool
  # ----------------------------------------------------------------------

def est_max_rcool(oph, CTS, mp_jump, quick):
  assert type(CTS) is list or type(CTS) is tuple
  est_max_rc = -inf
  for ct in CTS:
    tcs = path_tcovs(oph, ct)
    # Compute a lower bound {rc_ct} for the cooling time ratio:
    if tcs[0] != None and tcs[1] != None:
      # Contact {ct} is closed by {oph}, the exact cooling time is known:
      rc_ct = rcool_closed(ct, tcs)
    elif tcs[0] != None or tcs[1] != None:
      # Contact is partially closed by {oph}:
      i = 0 if tcs[0] != None else 1
      tc = path.fabtime(oph) - tcs[i]
      rc_ct = est_rcool_closed_by_other_paths(ct, oph, i, tc, mp_jump, quick)
    else:
      assert False, ("contact %s is not covered by path %s" % (contact.name(ct),path.name(oph)))
    est_max_rc = max(est_max_rc, rc_ct)
    if quick and est_max_rc > 1: 
      est_max_rc = +inf; break
  return est_max_rc
  # ----------------------------------------------------------------------

def plot_to_files(fname, CTS, clr, OPHS, CLRS, rwd, 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))

  dp = None

  # No frame, because it may confuse with contour:
  c, szx, szy = hacks.make_canvas(B, None, False, True, 1, 1)

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

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

def plot_single(c, ct, dp, clr, wd, sz_tic, arrow): 
  p = ct.pts[0]
  q = ct.pts[1]
  dpq = rn.dist(p,q)
  peps = 0.01*wd 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), ]
  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.side[0], ct.side[1])
    sz_arrow = 3*wd 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), ]
    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, omv0, omv1):
  # 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)
  mv0, dr0 = move.unpack(omv0)
  mv1, dr1 = move.unpack(omv1)
  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_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 tag_names(CTS, tag):
  if tag != None and tag != "":
    assert type(tag) is str
    for ct in CTS:
      ct.name = tag + get_name(ct)
  return
  # ----------------------------------------------------------------------

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

def show_list(wr, CTS, ind):
  xind = "  "*ind
  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 %*s %*s sides\n" % (xind,wix,"k",wna,"name",15,"pini",15,"pfin",5,"tclim"))
  wr.write("%s%s %s %s %s %s --------------\n" % (xind,"-"*wix,"-"*wna,"-"*15,"-"*15,"-"*5))
  
  # Write contacts:
  for k in range(len(CTS)):
    ct = CTS[k]
    wr.write("%s%*d " % (xind,wix,k))
    show(wr, ct, 0, wna)
    wr.write("\n")

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