# Implementation of the module {hotpath}.
# Last edited on 2021-02-19 15:52:39 by jstolfi

import hotpath
import block
import path
import contact
import move
import move_parms
import hacks
import rn
import sys
from math import sqrt, sin, cos, log, exp, floor, ceil, inf, nan, pi

import pyx
import os

def best_path(o, BS, CS, Delta, maxcalls, parms, test_parms, c_box, wr):
  describe_input(wr, o, BS, CS, Delta, maxcalls, parms)
  wr.write("----------------------------------------------------------------------\n")
  mp_jump = move_parms.make(0, 3000, 100, 0.00)
  Q = path.make_empty(o)
  Qbest, ncalls = best_path_aux \
    ( Q, BS, CS, Delta, maxcalls, 
      Q_maxtc = 0, ncalls = 0, lev = 0, 
      Qbest = None, mp_jump = mp_jump, parms = parms, test_parms = test_parms, c_box = c_box, wr = wr
    )
  wr.write("made %d recursive calls to {best_path_aux}\n" % ncalls)
  if Qbest == None:
    wr.write("{best_path} failed to find a valid tool-path\n")
  else:
    assert contact.max_tcool(Qbest, CS) <= Delta
    # Report contact coverage and cooling times:
    wr.write("----------------------------------------------------------------------\n")
    wr.write("coverage times and cooling time of each contact\n")
    for k in range(len(CS)):
      ct = CS[k]
      wr.write("CS[%03d] (" % k) 
      tcs = contact.covtimes(Qbest, ct)
      for i in range(2):
        wr.write(" "+ ("   .   " if tcs[i] == None else ("%7.3f" % tcs[i])))
      tcool = contact.tcool(Qbest, ct)
      wr.write(" ) " + ("   .   " if tcool == None else ("%7.3f" % tcool)))
      wr.write("\n")
  return Qbest
  
def best_path_aux(Q, BSR, CSR, Delta, maxcalls, Q_maxtc, ncalls, lev, Qbest, mp_jump, parms, test_parms, c_box, wr):
  # Like {best_path}, but considers only paths that begin with the path
  # {Q}, which will be a {path.Path} object. 
  #
  # The set {BSR} must be what remains of the original block set {BS}
  # after removing all blocks already incorporated in {Q}. The set {CSR}
  # must be the contacts of the original set {CS} that are still
  # relevant: that is, which have at least one side that is a move of a
  # block in {BSR}.
  #
  # The parameter {Q_maxtc} must be the {contact.max_tcool(Q,CS')} where
  # {CS'=CS\setminus CRS} are the contacts from the original {CS} that
  # had both sides covered by {Q} (and hence are no longer in {CSR}).
  #
  # The parameter {ncalls} is the number of times this procedure has been
  # entered before the current call. 
  #
  # The parameter {lev} is the depth of recursion. It is how many blocks
  # were removed from the original set {BS} and added to the path {Q}.
  #
  # The parameter {Qbest} is the best valid path found so far, or {None}
  # if no path has been found yet.
  #
  # The parameters {maxcalls,Delta,parms} are as in {best_path}.
  #
  # The procedure returns two results: [0] the best valid path found so
  # far, or {None} if it could not find any such path; and [1] the given
  # number {ncalls} plus the count of all recursive calls made by this
  # call (at least 1). If the procedure can't find a better path than
  # {Qbest}, it returns {Qbest}.
  
  # We made one more call:
  ncalls += 1
  Q_extime = path.extime(Q)     # Time already taken by {t}
    
  # Is {Q} the beginning of a valid path? 
  if predtcool(Q, Q_maxtc, BSR, CSR, parms) > Delta:
    # No -- Not worth continuing, return what we got so far:
    return Qbest, ncalls
    
  # Coud {Q} be extended to a path better than {Qbest}?
  mex = rest_min_extime(BSR) # Minimum time needed to excute the remaing blocks.
  if Qbest != None and Q_extime + mex >= path.extime(Qbest):
    # No -- Not worth continuing, return what we got so far:
    return Qbest, ncalls
    
  # Is Q a full path?
  if len(BSR) == 0:
    # If we got here, it must be valid:
    assert len(CSR) == 0
    # If we got here, it must be better than {Qbest}:
    assert Qbest == None or Q_extime < path.extime(Qbest)
    wr.write("!! found a better path")
    wr.write(" ncalls = %d max_tcool = %.3f extime = %.3f\n"  % (ncalls,Q_maxtc,Q_extime))
    Qbest = Q
    return Qbest, ncalls
  
  # Did we do enough work?
  if Qbest != None and maxcalls != None and ncalls >= maxcalls: 
    return Qbest, ncalls
    
  # Try to extend {Q} with each of the blocks in {BSR}, in all possible choices:
  BCPS = candidates(Q, BSR, CSR, path.pfin(Q), Delta, parms, mp_jump) # Sorted in "most promising first" order
  if BCPS != None:
    for bcp in BCPS:
      bc, ip = bcp
      P = block.choice(bc, ip)# Get the candidate path.
      
      use_jumps = True
      use_links = False
      Qplus = path.concat((Q, P), use_jumps, use_links, mp_jump) # Extend the tentative path.
      Qplus_maxtc = max(Q_maxtc, contact.max_tcool(Qplus, CSR))
      
      block.set_used(bc, True)
      BSminus = remove_block(BSR, bc)
      CSminus = remove_contacts(CSR)
      
      if test_parms['debug']:
        plot_partial_solution(ncalls, Qplus, BSminus, CSminus, c_box, parms['solid_raster_width'])

      Qbest, ncalls = best_path_aux \
        ( Qplus, BSminus, CSminus, Delta, maxcalls, 
          Q_maxtc = Qplus_maxtc, ncalls = ncalls, lev = lev+1, 
          Qbest = Qbest, mp_jump = mp_jump, parms = parms, test_parms = test_parms, c_box = c_box, wr = wr
        )
      
      block.set_used(bc, False)

      # Did we do enough work:
      if Qbest != None and maxcalls != None and ncalls > maxcalls:
        return Qbest, ncalls
    
  # We tried all possible extensions of {Q}:
  return Qbest, ncalls

##########

def predtcool(Q, Q_maxtc, BSR, CSR, parms):
  # The procedure receives the current tentative path {Q}, the maximum
  # cooling time {Q_maxtc} of all the originsl contacts that are closed
  # by it, the list {BSR} of the blocks that have not yet been included
  # in it, and the list {CSR} of contacts that are still relevant for
  # {BSR}. It returns a float {tm) such that
  # {contact.max_tcool(Qex,CS,parms) >= tm} for every path {Qex} that is
  # an extension of {Q} with some choice from every block in {BSR}, in
  # any orders; where {CS} is the original set of contacts.
  #
  # Assumes that {Q_maxtc} is {contact.max_tcool(Q, CS \setminus CSR)}.
  # Estimate the worst cooling time for active contacts:
  maxtc_active = path.extime(Q) - contact.min_tcov(Q, CSR)
  return max(Q_maxtc, maxtc_active)

##########

def remove_block(BSR, bc):
  # The parameter {BSR} must be a list or tuple of {Block} objects,
  # and {bc} must one of its elements.
  #
  # Returns a copy of the block set {BSR} minus the block {bc}. Does not change {BSR}.
  assert type(BSR) is list or type(BSR) is tuple
  assert isinstance(bc, block.Block)
  BSminus = tuple(bcx for bcx in BSR if bcx != bc)
  assert len(BSminus) == len(BSR) - 1
  return BSminus

def remove_contacts(CSR):
  # The parameter {CSR} must be a tuple or list of contacts, {Q} must be a 
  # {Path} object, and {bcp = (bc,ip)} must be a block pick.
  #
  # Removes from the contact set {CSR} all contacts that will become
  # irrelevant once the path {Q} is extended by the oriented
  # path {P=block.choice(bc,ip)}.
  
  assert type(CSR) is list or type(CSR) is tuple
  CSminus = tuple(ctx for ctx in CSR if not exclude_contact(ctx))
  return CSminus

def exclude_contact(ct):
  # The parameter {ct} must be a {Contact} object.
  #
  # Returns {True} iff the contact {ct} is covered on both sides.
  assert isinstance(ct, contact.Contact)

  bc0, ip = contact.side_block(ct, 0)
  bc1, ip = contact.side_block(ct, 1)
  
  done0 = block.used(bc0)
  done1 = block.used(bc1)

  if (done0 and done1) == False:
    return False
  
  return True

##########

def candidates(Q, BSR, CSR, p, Delta, parms, mp_jump):
  # Given the list {BSR} of blocks that have not been incorporated yet in
  # {Q}, the final point {p} of {Q}, and the list of still-relevant
  # contacts {CSR}, returns the list {BCPS} of candidates for the next
  # element to be added to {Q}. That is alist of block picks, containing
  # all possible picks from all blocks of {BSR}, sorted by priority order.
  
  def opdist(bcp):
    bc, ip = bcp
    oph = block.choice(bc, ip)
    d = rn.dist(p, path.pini(oph))
    return d

  def check_block(bc, ip, Delta, ct):
    B_aux = block.choice(bc, ip)
    use_jumps = True
    use_links = False
    Q_aux = path.concat((Q, B_aux), use_jumps, use_links, mp_jump)
    tcool = contact.max_tcool(Q_aux, [ct])

    if tcool < Delta:
      return True
    
    return False

  def oc_list(Q, CSR, Delta):
    BOC = []  # Blocks with open contacts.

    for ct in CSR:
      bc0, ip = contact.side_block(ct, 0)
      bc1, ip = contact.side_block(ct, 1)
      
      done0 = block.used(bc0)
      done1 = block.used(bc1)

      if (done0 == True and done1 == False) or (done0 == False and done1 == True):
        tcov = path.extime(Q) - contact.min_tcov(Q, [ct])
        if tcov >= Delta: # Open contact exceeded {Delta}.
          return None

        bc = bc0 if done0 == False else bc1
        
        len_BOC = len(BOC)
        np = block.nchoices(bc)

        for ip in range(np):
          if check_block(bc, ip, Delta, ct):
            bcp = (bc, ip)
            BOC.append(bcp)
        
        if len(BOC) == len_BOC:
          return None
    
    BOC.sort(key = opdist)

    return BOC

  def group_blocks(Q, BSR, p, Delta, parms, mp_jump):
    BCC = []  # Blocks without open contacts.
    
    for bc in BSR:
      np = block.nchoices(bc)
      for ip in range(np):
        bcp = (bc, ip)
        BCC.append(bcp)
    
    BCC.sort(key = opdist)

    return BCC

  BOC = oc_list(Q, CSR, Delta)
  if BOC == None: # At least one open contact exceeded {Delta}.
    return None
  elif len(BOC) > 0:
    return tuple(BOC)

  BCC = group_blocks(Q, BSR, p, Delta, parms, mp_jump)
  return tuple(BCC)

##########

def describe_input(wr, o, BS, CS, Delta, maxcalls, parms):
  # Prints main attributes of the {best_path} input data to 
  # file {wr} in human-readable form.

  wr.write('------- CONTACT x BLOCK TABLE:\n')
  wr.write("\n")
  print_contact_table(wr, BS, CS, None)
  wr.write("\n")

  wr.write("number of blocks = %d\n" % len(BS))
  wr.write("average choices per block = %.3f\n" % blocks_avg_choices(BS))
  wr.write("lower bound to execution time = %.3f s\n" % (blocks_min_starttime(o,BS,parms) + rest_min_extime(BS)))
  wr.write("number of contacts = %d\n" % len(CS))
  minmaxtc = blocks_min_max_tcool(CS,BS,parms)
  wr.write("lower bound to max cooling time = %.3f s\n" % minmaxtc)
  if Delta == +inf:
    wr.write("no limit on cooling time\n")
  else:
    wr.write("max allowed cooling time = %.3f s\n" % Delta)
  assert minmaxtc <= Delta, "** oops - {Delta} is too small for {CS,BS}"
  if maxcalls == None:
    wr.write("no limit on number of calls\n")
  else:
    wr.write("max allowed calls = %d\n" % maxcalls)
  return
  # ----------------------------------------------------------------------

def print_contact_table(wr, BS, CS, MS):
  nbc = len(BS)
  nct = len(CS)
  nmv = (0 if MS == None else len(MS))
  
  def fmt1(i, c, n):
    fmt = c + "%0" + ("2" if n < 100 else "3")+ "d"
    return fmt % i
    
  def fmt2(i, j, c, n):
    fmt = c + "%0" + ("2" if n < 100 else "3")+ "d:%d"
    return fmt % (i,j)

  # Table header:
  ctsp = " "*len(fmt2(0,0, "C", nct))
  mvsp = " "*len(fmt1(0, "M", nmv))
  bcds = "-"*len(fmt2(0,0, "B", nbc))
  wr.write("%s %s " % (ctsp,mvsp))
  for ib in range(nbc):
    bci = BS[ib]
    npi = block.nchoices(bci)
    for jpi in range(npi):
      wr.write(" " + (fmt2(ib,jpi, "B", nbc)))
  wr.write("\n")
  for i in range(nct):
    cti = CS[i]
    for j in range(2):
      ctID = fmt2(i, j, "C", nct)
      mvij = contact.side(cti,j)
      if MS == None:
        mvID = mvsp
      else:
        r = MS.index(mvij)
        mvID = fmt1(r, "M", nmv)
      wr.write("%s %s " % (ctID,mvID))
      for k in range(nbc):
        bck = BS[k]
        npk = block.nchoices(bck)
        for l in range(npk):
          ophkl = block.choice(bck, l)
          t = path.find(ophkl, mvij)
          if t == None:
            wr.write(" " + bcds)
          else:
            wr.write(" %*d" % (len(bcds), t))
      wr.write("\n")
    wr.write("\n")
  return
  # ----------------------------------------------------------------------

def blocks_avg_choices(BS):
  # Returns the geometric average of the number of choices 
  # of the blocks in the list {BS}.
  slog = 0
  for bc in BS:
    np = block.nchoices(bc)
    assert np >=1
    slog += log(np)
  avg = exp(slog/len(BS))
  return avg

def blocks_min_starttime(p, BSR, parms):
  # Given a point {p} and the list {BSR} of blocks that have not been yet
  # included in the tentative tool-path, returns the minimum time needed
  # to jump to the nearest starting point of any choice of any of those
  # blocks.
  dmin = +inf
  for bc in BSR:
    for ip in range(block.nchoices(bc)):
      oph = block.choice(bc, ip)
      di = rn.dist(p, path.pini(oph))
      if di < dmin: dmin = di
  assert dmin != +inf
  tmin = nozzle_travel_time(dmin, True, None, parms)
  return tmin
  # ----------------------------------------------------------------------

def nozzle_travel_time(dpq, jmp, dpm, parms):
  acc = parms['acceleration']  # Acceleration/deceleration at ends (mm/s^2).
  if jmp:
    vel_cru = parms['job_jump_speed'] # Cruise speed(mm/s).
    tud = parms['extrusion_on_off_time']  # Nozzle up/down time (s).
  else:
    # ??? Speed should be an attribute of the move.???
    vel_cru = parms['job_filling_speed'] # Cruise speed(mm/s).
    tud = 0

  # Figure out acceleration, cruise, and deceleration times and distances:
  acc_time = 0 if acc == inf else vel_cru/acc  # Time accelerating or decelerating to{vel}.
  if acc != inf and vel_cru*acc_time >= dpq:
    # Not enough space to reach cruise speed:
    acc_time = sqrt(dpq/acc)
    acc_dist = dpq/2
    vel_max = acc*acc_time
    cru_dist = 0
    cru_time = 0
  else:
    # Enough distance to reach cruise speed:
    acc_dist = 0 if acc == inf else vel_cru*acc_time/2 # Distance while accelerating or decelerating.
    vel_max = vel_cru
    cru_dist = dpq - 2*acc_dist # Cruise distance.
    cru_time = cru_dist/vel_cru
  # if dpm != None and dpm <= 0:
  #   sys.stderr.write("acc_dist = %8.2f cru_dist = %8.2f\n" % (acc_dist, cru_dist))
  #   sys.stderr.write("acc_time = %8.2f cru_time = %8.2f\n" % (acc_time, cru_time))
  #   sys.stderr.write("vel_max = %8.2f\n" % vel_max)
  # Compute the passage time {ttot}:
  ttot = tud
  if dpm == None or dpm >= dpq:
    # Acceleration, cruise, deceleration
    ttot += 2 * acc_time + cru_time + tud
  elif dpm > acc_dist + cru_dist:
    # Passage time is during deceleration:
    ttot += acc_time + cru_time
    dcm = dpm - (acc_dist + cru_dist)  # Distance after start of decel.
    delta = vel_max*vel_max - 2*dcm*acc
    # sys.stderr.write("%8.2f %8.2f %8.2f %8.2f\n" % (dcm, vel_max, acc, delta))
    tcm = (vel_max - sqrt(delta))/acc # Extra time to passage.
    ttot += tcm
  elif dpm >= acc_dist:
    # Passage time is during cruising phase:
    ttot += acc_time
    dam = dpm - acc_dist # Distance after acceleration.
    tam = dam/vel_cru # Extra time to passage.
    ttot += tam
  elif dpm >= 0:
    # Passage during acceleration:
    tpm = sqrt(2*dpm/acc) # Extra time to passage.
    ttot += tpm
  else:
    # Passage at very start:
    pass
  
  return ttot

def rest_min_extime(BSR):
  # Given the list{BSR} of blocks that have not been yet included in the
  # tentative tool-path, returns an estimate of the minimum time needed to
  # execute them, even if the best alternative could be chosen in each
  # block.
  mex = 0
  for bc in BSR:
    mex += block.min_extime(bc)
  return mex

def blocks_min_max_tcool(CSR,BSR,parms):
  # Given a list of contacts {CSR} and a list of blocks {BSR}, returns a
  # lower bound to the value of {contact.max_tcool(P,CSR)} for any path {P}
  # built from those blocks.
  #
  # The list {CSR} must contain only relevant contacts, that have at least 
  # one side covered by one of blocks of {BSR}.
  #
  # For each contact {ct} of {CSR}, it finds the two blocks {bc0} and {bc1}
  # of {BSR} that include the sides of {ct}. They must both exist, be
  # unique, and distinct. Computes a min cooling time {tcm(ct)} assuming
  # that the best possible choices {ph0,ph1} of those blocks that contain
  # the sides of {ct} are added in sequence to the tool-path. Ignores any
  # choice of either block that does not contain those sides, assuming
  # that the contact {ct} will become irrelevant if that choice is
  # selected for the tool-path.
  #
  # Then it returns the maximum of {tcm(ct)} among all contacts {ct} of {CSR}.
  # If {CSR} is empty, returns 0.
  tcmax = 0
  for ct in CSR:
    tcmin = blocks_min_tcool(BSR, ct, parms)
    assert tcmin < +inf, "some side of a contact is not covered by {BSR}"
    if tcmin > tcmax: tcmax = tcmin
  return tcmax
  # ----------------------------------------------------------------------

def blocks_min_tcool(BS, ct, parms):
  mv0 = contact.side(ct,0)
  mv1 = contact.side(ct,1)
  # Find the two blocks that have {mv0} and {mv1}:
  bc0 = None
  bc1 = None
  for bc in BS:
    has0 = block.find_choice_with_move(bc, mv0)
    has1 = block.find_choice_with_move(bc, mv1)
    assert not (has0 != None and has1 != None), "contact has two sides in same block"
    if has0 != None: 
      assert bc0 == None, "same move occurs in two different blocks"
      bc0 = bc
    if has1 != None: 
      assert bc1 == None, "same move occurs in two different blocks"
      bc1 = bc
    if bc0 != None and bc1 != None: break
  # Now {bc0} and {bc1} are the blocks that contain {mv0} and {mv1}, if any.
  assert bc0 != None or bc1 != None, "contact is not relevant to blocks of {BS}"
  if bc0 != None and bc1 != None:
    assert bc0 != bc1, "contact has two sides on the same block"
    tcmin = block_pair_min_tcool(bc0, bc1, ct, parms)
  else:
    tcmin = +inf
  return tcmin

def block_pair_min_tcool(bc0, bc1, ct, parms):
  tcmin = +inf
  # Enumerate choices of {bc0}:
  for ip0 in range(block.nchoices(bc0)):
    # Get choice {oph0} and the cooling time {tc0} to the end of it:
    oph0 = block.choice(bc0, ip0)
    tcs0 = contact.covtimes(path.rev(oph0), ct)
    assert tcs0[0] == None or tcs0[1] == None, "contact has both sides on {bc0}"
    tc0 = tcs0[0] if tcs0[0] != None else tcs0[1]
    if tc0 != None:
      p0 = path.pfin(oph0)
      # Enumerate choices of {bc1}:
      for ip1 in range(block.nchoices(bc1)):
        # Get choice {oph1} and the cooling time {tc1} to the end of it:
        oph1 = block.choice(bc1, ip1)
        tcs1 = contact.covtimes(oph1, ct)
        assert tcs1[0] == None or tcs1[1] == None, "contact has both sides on {bc1}"
        tc1 = tcs1[0] if tcs1[0] != None else tcs1[1]
        if tc1 != None:
          # Compute min time for jump or link between them:
          p1 = path.pini(oph1)
          dist = rn.dist(p0, p1)
          tjmp = nozzle_travel_time(dist, True, None, parms)  # Jump time.
          tlnk = nozzle_travel_time(dist, False, None, parms) # Link time. Assumes possible.
          # Cooling time for this pair of choices:
          tc = tc0 + min(tjmp, tlnk) + tc1
          if tc < tcmin: tcmin = tc
  return tcmin

##########

def plot_partial_solution(ncalls, Q, BS, CS, c_box, wd):
  c = pyx.canvas.canvas()

  pbox = c_box[0]
  sz_max = c_box[1]

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

  wd_grid = 0.002*sz_max
  hacks.plot_grid(c, pyx.color.rgb(0.800, 0.750, 0.600), 0.03, None, pbox, +5*wd_grid, 1,1)

  pyx.unit.set(uscale=1.00, wscale=1.00, vscale=1.00)
  
  ctrace =  pyx.color.rgb( 0.000, 0.710, 1.000 ) # Color of nominal trace area.
  waxes = 0.05*wd; 
  axes = False
  dots = False
  arrows = False
  matter = True

  # Plot origin.
  wstart = 10*waxes; cstart = pyx.color.rgb( 0.800, 0.200, 0.000 ) # Initial nozzle position.
  o = (0,0)
  hacks.plot_line(c, cstart, wstart, o, o)

  # Plot solution.
  path.plot_standard(c, [Q], o, None, [ctrace], waxes, axes, dots, arrows, matter)

  # Plot blocks.
  ctraces = pyx.color.rgb( 1.000, 0.412, 0.705 )
  for bc in BS:
    oph = block.choice(bc, 0)
    layer = 1

    path.plot_standard \
      ( c, [oph], None, layer, [ctraces], waxes,
        axes = True, dots = True, arrows = True, matter = True
      )

  # Plot contacts.
  wconts = 0.15*wd
  ccontsopen = pyx.color.rgb( 0.000, 0.000, 0.000 )
  cconts = pyx.color.rgb(0.200, 0.600, 0.000)

  for ct in CS:
    bc0, ip = contact.side_block(ct, 0)
    bc1, ip = contact.side_block(ct, 1)
    
    done0 = block.used(bc0)
    done1 = block.used(bc1)

    if (done0 == True and done1 == False) or (done0 == False and done1 == True):
      contact.plot_single(c, ct, None, clr = ccontsopen, wd_line = wconts, sz_tic = 0, arrow = False)
    elif done0 == False and done1 == False:
      contact.plot_single(c, ct, None, clr = cconts, wd_line = wconts, sz_tic = 0, arrow = False)
 
  # Write files.
  hacks.write_plot(c, c_box[-1] + str(ncalls).zfill(7))
  os.remove(c_box[-1] + str(ncalls).zfill(7) + '.eps')

  return