# Implementation of the module {hotpath_elis}.
# Last edited on 2021-06-04 03:19:42 by jstolfi

import hotpath_elis
import path
import path_hp
import block
import block_hp
import contact
import contact_hp
import move
import move_parms
import hacks
import rn
import job_parms
import plot_data

import sys
from math import sqrt, sin, cos, log, exp, floor, ceil, inf, nan, pi

def concat_paths(A, B, mp_jump):
  use_jumps = True
  use_links = True
  Q = path.concat((A, B), use_jumps, use_links, mp_jump)

  return Q

def select_contours(OCRS, ocr):
  OCRS_aux = []

  while path.outer_contours(ocr) != []:
    ocr = path.outer_contours(ocr)[0]

  OCRS_aux.append(ocr)
  OCRS = remove_contour(OCRS, ocr)

  for ocr_in in path.inner_contours(ocr):
    OCRS_aux.append(ocr_in)
    OCRS = remove_contour(OCRS, ocr_in)

  return OCRS_aux, OCRS

def remove_contour(OCRS, ocr):
  OCRSminus = tuple(ocrx for ocrx in OCRS if ocrx != ocr)
  return OCRSminus

def select_contacts(BCS):
  CTS = []

  for bc in BCS:
    cts = block.get_cts(bc)
    for ct in cts:
      if ct not in CTS:
        CTS.append(ct)
        
  return CTS
  

def find_new_qend(qEnd, BCS):
  def distance (p, q):
    dx = p[0] - q[0]
    dy = p[1] - q[1]
    d = sqrt(dx*dx + dy*dy)
    return d 

  x_min = None
  y_min = None

  x_max = None
  y_max = None

  for bc in BCS:
    oph, d = block.choice(bc, 0)
    pini = path.pini(oph)
    pfin = path.pfin(oph)

    x = min(pini[0], pfin[0])
    y = min(pini[1], pfin[1])

    if x_min == None or x_min > x:
      x_min = x

    if y_min == None or y_min > y:
      y_min = y

    x = max(pini[0], pfin[0])
    y = max(pini[1], pfin[1])

    if x_max == None or x_max < x:
      x_max = x

    if y_max == None or y_max < y:
      y_max = y
  
  dMin = distance(qEnd, (x_min, y_min))
  dMax = distance(qEnd, (x_max, y_max))

  if dMax > dMin:
    return (x_min, y_min)
  else:
    return (x_max, y_max)

def best_path(o, qEnd, BCS, CTS, mp_cont, mp_jump, Delta, maxcalls, parms, debug, B, wr):
  Q = path.make_empty(o)

  Qbest, ncalls = best_path_aux \
    ( Q, BCS, CTS, Delta, maxcalls, 
      Q_maxtc = 0, ncalls = 0, lev = 0, 
      Qbest = None, mp_cont = mp_cont, mp_jump = mp_jump, 
      parms = parms, debug = debug, B = B, wr = wr
    )

  if Qbest != None and contact.max_tcool(Qbest, CTS) <= Delta:
    generic_path.describe_solution(wr, Qbest, CTS, Delta, o, qEnd, ncalls)

  return Qbest

def best_path_aux(Q, BCS_R, CTS_R, Delta, maxcalls, Q_maxtc, ncalls, lev, Qbest, mp_cont, mp_jump, parms, debug, B, wr):
  # Like {best_path}, but considers only paths that begin with the path
  # {Q}, which will be a {path.Path} object. 

  # The set {BCS_R} must be what remains of the original block set {BCS}
  # after removing all blocks already incorporated in {Q}. The set {CTS_R}
  # must be the contacts of the original set {CTS} that are still
  # relevant: that is, which have at least one side that is a move of a
  # block in {BCS_R}.

  # The parameter {Q_maxtc} must be the {contact.max_tcool(Q,CTS')} where
  # {CTS'=CTS\setminus OCRS} are the contacts from the original {CTS} that
  # had both sides covered by {Q} (and hence are no longer in {CTS_R}).

  # 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 {BCS} 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, BCS_R, CTS_R, 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 = min_tot_extime(BCS_R) # 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(BCS_R) == 0:
    # If we got here, it must be valid:
    assert len(CTS_R) == 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 == 0: 
    return Qbest, ncalls
  elif maxcalls != None and maxcalls != 0 and ncalls >= maxcalls:
    return Qbest, ncalls
    
  # Try to extend {Q} with each of the blocks in {BCS_R}, in all possible choices:
  BCPS = candidates(Q, BCS_R, CTS_R, path.pfin(Q), Delta, parms, mp_cont, mp_jump) # Sorted in "most promising first" order
  if BCPS != None:
    for bcp in BCPS:
      bc, ip, rasterLink = bcp
      P = block.choice(bc, ip)# Get the candidate path.
      
      use_jumps = True
      use_links = False

      if rasterLink == None:
        Qplus = path.concat((Q, P), use_jumps, use_links, mp_jump) # Extend the tentative path.
        Qplus_maxtc = max(Q_maxtc, contact.max_tcool(Qplus, CTS_R))
      
      else:
        Qplus = path.concat((Q, rasterLink), use_jumps, use_links, mp_jump)
        Qplus = path.concat((Qplus, P), use_jumps, use_links, mp_jump) 
        Qplus_maxtc = max(Q_maxtc, contact.max_tcool(Qplus, CTS_R))
      
      block_hp.set_used_choice(bc,ip)
      BCSminus = remove_block(BCS_R, bc)
      CTSminus = remove_contacts(CTS_R)
      
      if debug:
        plot_data.plot_debug(None, BCSminus, CTSminus, Qplus, tag, ncalls, B)

      Qbest, ncalls = best_path_aux \
        ( Qplus, BCSminus, CTSminus, Delta, maxcalls, 
          Q_maxtc = Qplus_maxtc, ncalls = ncalls, lev = lev+1, 
          Qbest = Qbest, mp_cont = mp_cont, mp_jump = mp_jump, 
          parms = parms, debug = debug, B = B, wr = wr
        )
      
      block_hp.set_used_choice(bc, None)

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

def predtcool(Q, Q_maxtc, BCS_R, CTS_R, 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 {BCS_R} of the blocks that have not yet been included
  # in it, and the list {CTS_R} of contacts that are still relevant for
  # {BCS_R}. It returns a float {tm) such that
  # {contact.max_tcool(Qex,CTS,parms) >= tm} for every path {Qex} that is
  # an extension of {Q} with some choice from every block in {BCS_R}, in
  # any orders; where {CTS} is the original set of contacts.

  # Assumes that {Q_maxtc} is {contact.max_tcool(Q, CTS \setminus CTS_R)}.
  # Estimate the worst cooling time for active contacts:
  maxtc_active = path.extime(Q) - contact.min_tcov(Q, CTS_R)
  return max(Q_maxtc, maxtc_active)

def candidates(Q, BCS_R, CTS_R, p, Delta, parms, mp_cont, mp_jump):
  # Given the list {BCS_R} of blocks that have not been incorporated yet in
  # {Q}, the final point {p} of {Q}, and the list of still-relevant
  # contacts {CTS_R}, 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 {BCS_R}, sorted by priority order.
  
  def opdist(bcp):
    bc, ip, rasterLink = bcp
    oph = block.choice(bc, ip)
    d = rn.dist(p, path.pini(oph))
    return d

  def opdist_raster_link(bcp):
    bc, ip, rasterLink = bcp
    oph = block.choice(bc, ip)
    d = rn.dist(p, path.pini(oph))
    if rasterLink == None:
      d += 1
    return d

  def find_raster_link(Q, bc, ip, ct):
    B_aux = block.choice(bc, ip)

    rasterLink = None

    if contact_hp.get_raster_links(ct) != [None, None]:
      qEnd = path.pfin(Q)
      pIni = path.pini(B_aux)
      links = contact_hp.get_raster_links(ct)

      if links[0] != None:
        p0 = path.pini(links[0])
        q0 = path.pfin(links[0])

        if qEnd == p0 and pIni == q0:
          rasterLink = links[0]
        
        elif qEnd == q0 and pIni == p0:
          rasterLink = path.rev(links[0])       

      if links[1] != None:
        p1 = path.pini(links[1])
        q1 = path.pfin(links[1])

        if qEnd == p1 and pIni == q1:
          rasterLink = links[1]
        
        elif qEnd == q1 and pIni == p1:
          rasterLink = path.rev(links[1])    
    
    return rasterLink

  def group_blocks(Q, BCS_C, CTS_R):
    BOC = []  # Blocks with open contacts.
    BCC = []  # Blocks without open contacts.

    for ct in CTS_R:
      bc0 = contact_hp.side_block(ct, 0)
      bc1 = contact_hp.side_block(ct, 1)
      
      done0 = block_hp.used_choice(bc0)
      done1 = block_hp.used_choice(bc1)

      if (done0 == None) != (done1 == None):
        # Exactly one side of {ct} is covered by {Q}
        bc = bc0 if done0 == None else bc1
        np = block.nchoices(bc)

        for ip in range(np):
          rasterLink = find_raster_link(Q, bc, ip, ct)
          bcp = (bc, ip, rasterLink)
          BOC.append(bcp)
          
          if rasterLink != None:
            bcp = (bc, ip, None)
            BOC.append(bcp)
      
        BCS_C = remove_block(BCS_C, bc)
  
    for bc in BCS_C:
      np = block.nchoices(bc)
      for ip in range(np):
        bcp = (bc, ip, None)
        BCC.append(bcp)
    
    BOC.sort(key = opdist_raster_link)
    BCC.sort(key = opdist)

    return BOC + BCC

  NEW_BS = group_blocks(Q, BCS_R, CTS_R)
  
  return tuple(NEW_BS)

def remove_block(BCS_R, bc):
  # The parameter {BCS_R} must be a list or tuple of {Block} objects,
  # and {bc} must one of its elements.

  # Returns a copy of the block set {BCS_R} minus the block {bc}. Does not change {BCS_R}.
  assert type(BCS_R) is list or type(BCS_R) is tuple
  assert isinstance(bc, block.Block)
  BSminus = tuple(bcx for bcx in BCS_R if bcx != bc)
  assert len(BSminus) == len(BCS_R) - 1
  return BSminus

def remove_contacts(CTS_R):
  # The parameter {CTS_R} 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 {CTS_R} all contacts that will become
  # irrelevant once the path {Q} is extended by the oriented
  # path {P=block.choice(bc,ip)}.
  
  assert type(CTS_R) is list or type(CTS_R) is tuple
  CTSminus = tuple(ctx for ctx in CTS_R if not exclude_contact(ctx))
  return CTSminus

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 = contact_hp.side_block(ct, 0)
  bc1 = contact_hp.side_block(ct, 1)
  
  done0 = block_hp.used_choice(bc0)
  done1 = block_hp.used_choice(bc1)

  if (done0 == None) or (done1 == None):
    return False
  
  return True

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 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 print_contact_table(wr, BS, CTS, MS):
  nbc = len(BS)
  nct = len(CTS)
  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 = CTS[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_move(ophkl, mvij)
          if t == None:
            wr.write(" " + bcds)
          else:
            wr.write(" %*d" % (len(bcds), t))
      wr.write("\n")
    wr.write("\n")
  return
  # ----------------------------------------------------------------------

def check_input(BS, CTS, Delta, mp_jump):
  if Delta == +inf:
    return True
  
  minmaxtc = contact.min_max_tcool(CTS, BS, mp_jump)

  if minmaxtc <= Delta:
    return True

  return False