# Implementation of the module {best_elem_path}.
# Last edited on 2021-12-26 21:00:12 by stolfi

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

def solve(OPHS, org, mp_jump):
 
  nph = len(OPHS) # Number of input path elements.
  sys.stderr.write("{best_elem_path.solve}: %d elements\n" % nph)
  assert nph >= 1 or org != None, "cannot solve with no path elements"
 
  NP = num_pot_cands(nph)
  sys.stderr.write("{best_elem_path.solve}: %d candidate paths\n" % NP)
  NPmax = 1000*1000*1000
  if NP > NPmax: 
    sys.stderr.write("{best_elem_path.solve}: ** too many candidate paths, chickened out\n")
    return None, 0
  
  # Initial list of elements in path prefix, and the cumulative fabtimes:
  OPHS_pref = [] 
  TFABS_pref = []
  if org != None:
    oph = path.make_trivial(org)
    OPHS_pref.append(oph)
    TFABS_pref.append(path.fabtime(oph))
    assert path.fabtime(oph) == 0
      
  # Compute the list {OPHS_sol} with the elements of {OPHS} in best order and orientation,
  # together with necessary connectors (links or jumps):
  if nph == 0:
    OPHS_sol = OPHS_pref
  else:
    # Convert {OPHS} into set {PHS} of {Path} objects, set their groups to {None}:
    PHS = set()
    for oph in OPHS:
      ph, dr = path.unpack(oph)
      assert not ph in PHS, "repeated path"
      path.set_group(ph, None)
      PHS.add(ph)

    # Get the set {CTS} of all contacts
    CTS = set()
    for oph in OPHS:
      for isd in 0, 1:
        CTSi = path.get_contacts(oph, isd)
        CTS |= CTSi
        
    # Paranoia:
    contact.check_side_paths(CTS,OPHS,ydir=None)

    # Find the optimum list {OPHS_sol} that begins with {OPHS_pref}:
    OPHS_opt = None
    Tfab_opt = +inf
    Tconn_opt = +inf
    CTSA = set()
    CTSI = CTS.copy()
    ncalls = 0
    Tconn_pref = 0
    OPHS_sol, Tfab_sol, Tconn_sol, ncalls, ncands = solve_aux \
      ( OPHS_pref, TFABS_pref, Tconn_pref,
        OPHS_opt, Tfab_opt, Tconn_opt, 
        PHS, CTSA, CTSI,
        mp_jump, 
        ncalls, NP
      )
    sys.stderr.write("{best_elem_path.solve}: returned with %d calls, %d candidates remaining\n" % (ncalls,ncands))
  
  
  # Convert {OPHS_sol} to path {fph}:
  if OPHS_sol == None:
    fph = None
  else:
    n_sol = len(OPHS_sol)
    assert n_sol >= 1
    
    for iph in range(n_sol):
      oph = OPHS_sol[iph]
      sys.stderr.write("  %03d %s\n" % (iph, oph)) 

    use_links = False # Links should be there already.
    fph = path.concat(OPHS_sol, use_links, None)

    # Check whether {Tfab_sol} is correct:
    Tfab_fph = path.fabtime(fph)
    assert abs(Tfab_fph - Tfab_sol) < 1.0e-8

  return fph, ncalls
  # ----------------------------------------------------------------------
  
def solve_aux(OPHS_pref, TFABS_pref, Tconn_pref, OPHS_opt, Tfab_opt, Tconn_opt, PHS, CTSA, CTSI, mp_jump, ncalls, ncands):
  # Input parameters:
  # 
  #   {OPHS_pref} should be a list of some of the given path elements, possibly
  #   reversed, that is to be exteded. Possibly empty.  See details below.
  #
  #   {TFABS_pref} should be a list such that {TFABS_pref[i]} is the the fabtime
  #   of all the paths in {OPHS_pref[0:i+1]}, including connectors.
  #
  #   {Tconn_pref} should be the total fabtime of the connectors in
  #   {OPHS_pref}.
  #
  #   {OPHS_opt} should be a list with all the given path elements,
  #   possibly reversed, with the necessary connectors, whose
  #   concatenation is the best valid tool path path found so far; or
  #   {None} if no path has been found. If not {None}, it will be
  #   non-empty. It doed not necessarily begin with {OPHS_pref} and its
  #   elements may not be elements of the input set {OPHS}.
  #
  #   {Tfab_opt} should be the fabtime of the concatenation of the paths in
  #   {OPHS_opt}; or {+inf} if {OPHS_opt} is {None}.
  #
  #   {Tconn_opt} should be the total fabtime of the connectors in
  #   {OPHS_opt}; or {+inf} if {OPHS_opt} is {None}.
  #
  #   {PHS} should be the set of {Path} objects that have not been used
  #   in {OPHS_pref}. The index group of every path in {PHS}, as
  #   returned by {path.get_group}, must be {None}.
  #
  #   {CTSA} should be the set of contacts that are covered but not closed by some 
  #   path in {OPHS_pref}.
  # 
  #   {CTSI} should be the set of contacts that are neither covered or closed
  #   by paths in {OPHS_pref}.
  # 
  #   {ncalls} should be the number of times {solve_aux} has been called so far
  #   since and including the call by {solve}.
  #
  #   {ncands} should be the number of candidate paths that remain to be examined.
  #
  # Enumerates all valid paths that start with {OPHS_pref}. Returns:
  #
  #   {OPHS_sol} a list with all the given path elements, possibly
  #   reversed, with the necessary connectors, whose concatenation is
  #   the best valid tool path path found that is better than {OPHS_opt};
  #   or {OPHS_opt} (which may be {None}) if no such path has been found.
  #   If not {OPHS_opt}, it will be non-empty and be an extension of
  #   {OPHS_pref}.
  #
  #   {Tfab_sol} is the total fabtime of the paths on {OPHS_sol},
  #   of {+inf} if {OPHS_sol} is {None}.
  #
  #   {Tconn_sol} is the total fabtime of the connectors in {OPHS_sol},
  #   of {+inf} if {OPHS_sol} is {None}.
  #
  #   {ncalls} is the input {ncalls} plus 1 plus all recursive calls
  #   made by this call.
  #
  #   {ncands} The input {ncands} minus the number of paths that were
  #   eliminaed or cheched by this call and all its recursive calls.
  #
  # The list {OPHS_pref} must be a subset of the original input
  # elements, intercalated with connectors (jumps of links) when
  # necessary. Every connector is a path, possibly consisting of a
  # single jump. The first and last elements of the list should be input
  # elements, and there should not be more than one connector between
  # any two elements. The concatenation {oph_pref} of all those paths
  # and connectors is assumed to satisfy the cooling times of all
  # contacts that it closes.  
  #
  # The returned list {OPHS_sol}, if not the same as {OPHS_opt},
  # will have the same properties.
  #
  # The group index of {OPHS_pref[i]}, as returned by {path.get_group},
  # must be {i}.
  #
  # The connector times {Tconn_pref} and {Tconn_opt} must include the
  # T/J penalties for each connector that is a jump. The same will be 
  # true of the returned {Tconn_sol}.
  #
  # Assumes that the contact sets {path.get_contacts(oph,isd)} are set
  # for every path {oph} in {PHS} or {OPHS_pref}, that
  # {path.get_contacts(ph,isd)} are properly set for all paths in {PHS}.
  # and {contact.get_side_paths} are consistent with them.
  
  nph_rest = len(PHS)
  assert OPHS_pref != None
  n_pref = len(OPHS_pref)

  def report():
    sys.stderr.write("  * ncalls = %12d ncands = %12d nph_rest = %5d" % (ncalls, ncands, nph_rest))
    sys.stderr.write(" Tfab_opt = %10.4f Tconn_opt = %10.4f\n" % (Tfab_opt, Tconn_opt))
    return
    # ....................................................................

  if (ncalls < 100) or (ncalls < 10000 and ncalls % 100 == 0) or (ncalls % 10000 == 0): report()
  
  ncalls += 1
  
  assert OPHS_pref != None
  assert len(TFABS_pref) == n_pref
  Tfab_pref = (0 if n_pref == 0 else TFABS_pref[-1]) # Total fabtime of {OPHS_pref}.

  if Tconn_pref >= Tconn_opt:
    # {OPHS_pref} has already too much connection time; return.
    ncands -= num_pot_cands(nph_rest)
    return OPHS_opt, Tfab_opt, Tconn_opt, ncalls, ncands
  elif nph_rest == 0:
    # We got a new complete tool-path that is valid:
    assert Tconn_pref < Tconn_opt # Paranoia.
    assert Tfab_pref < Tfab_opt + 1.0e-12 # Since the non-conn time must be the same.
    # Update the best path:
    OPHS_opt = OPHS_pref
    Tfab_opt = Tfab_pref
    Tconn_opt = Tconn_pref
    report()
    ncands -= 1
    return OPHS_opt, Tfab_opt, Tconn_opt, ncalls, ncands
  elif quick_est_prefix_max_rcool(OPHS_pref, TFABS_pref, CTSA, mp_jump) > 1:
    # Any extension of {OPHS_pref} would violate the cooling constraints.
    ncands -= num_pot_cands(nph_rest)
    return OPHS_opt, Tfab_opt, Tconn_opt, ncalls, ncands
  else:
    # {OPHS_pref} is okay but not complete yet. Try to extend it:
    # Get last element added to {OPH_pref}, or {None} if empty:
    oph_last = (None if len(OPHS_pref) == 0 else OPHS_pref[-1])
    # Note: cannot loop on {PHS} itself because the loop changes {PHS}.
    for ph in tuple(PHS):
      PHS.remove(ph)
      for dr in 0, 1:
        oph_cand = (ph, dr)
        # Get connection time, including T/J penalties:
        use_links = True
        if oph_last != None:
          Tfab_conn = path.connection_time(oph_last, oph_cand, use_links, mp_jump)
          assert Tfab_conn > 0 # Strictly.
        else:
          Tfab_conn = 0
        # Check if adding {oph_cand} is safe:
        Rmax = quick_cand_max_rcool(OPHS_pref, TFABS_pref, Tfab_conn, oph_cand, CTSA)
        if Rmax > 1: 
          ncands -= num_pot_cands(nph_rest - 1)
        else:
          # Appending {oph_cand} to {OPHS_pref}, with connector, will not violate any constraints.
          # Should use linked lists...
          OPHS_ext = OPHS_pref.copy()
          TFABS_ext = TFABS_pref.copy()
          Tconn_ext = Tconn_pref
          if Tfab_conn != 0: 
            # Must add a connector:
            assert oph_last != None
            conn = path.get_connector(oph_last, oph_cand, use_links, mp_jump)
            if isinstance(conn, move.Move):
              oph_conn = path.from_moves([conn,])
            else:
              oph_conn = conn
            OPHS_ext.append(oph_conn)
            TFABS_ext.append(Tfab_pref + Tfab_conn)
            Tconn_ext += Tfab_conn
          # Append the candidate:
          Tfab_oph = path.fabtime(oph_cand)
          path.set_group(ph, len(OPHS_ext))
          OPHS_ext.append(oph_cand)
          TFABS_ext.append(Tfab_pref + Tfab_conn + Tfab_oph)
          # Get contacts that are closed by adding {oph_cand} to 
          CTSA_ext, CTSI_ext = update_contacts(oph_cand, CTSA, CTSI)
          OPHS_opt, Tfab_opt, Tconn_opt, ncalls, ncands = solve_aux \
            ( OPHS_ext, TFABS_ext, Tconn_ext,
              OPHS_opt, Tfab_opt, Tconn_opt,
              PHS, CTSA_ext, CTSI_ext, 
              mp_jump, 
              ncalls, ncands
            )
          path.set_group(ph, None)
      # Restore {PHS} set for next candidate:
      PHS.add(ph)
    assert len(PHS) == nph_rest
  return OPHS_opt, Tfab_opt, Tconn_opt, ncalls, ncands

  # ----------------------------------------------------------------------

def num_pot_cands(nph):
  # Returns {2^nph nph!}, the number of potential candidates for {nph} elements.
  Nmax = 1000*1000*1000*1000 # Just to stop if overflow.
  N = 1
  for i in range(nph):
    assert N < Nmax
    N = N*2*(i+1)
  return N
  # ----------------------------------------------------------------------

# ESTIMATING MAX COOLING TIME RATIO FOR ALL EXTENSIONS OF PREFIX

def quick_est_prefix_max_rcool(OPHS_pref, TFABS_pref, CTSA, mp_jump):
  # Assumes that {OPHS_pref} is the current element list prefix, that
  # {TFABS_pref} is its total fabrication time, and that {CTSA} is the
  # set of elements that are covered by it on only one side. 
  #
  # For each contact, estimates a lower bound for the cooling time ratio
  # of any tool-path that begins with {OPHS_pref}. Returns the maximum
  # of those ratios. If {CTSA} is empty, returns 0.
  #
  # The path groups and path-contact associations must be as in 
  # the entry to {solve_aux};
  #
  # The returned lower bound is fairly tight if it is at most 1.
  # but the procedure returns as soon as it concludes that it is 
  # greater than 1.
  
  Rmax = 0
  for ct in CTSA:
    # Get a lower bound for the cooling time of {ct} 
    # since its coverage by {oph_pref} to the coverage by some other path element:
    Tlob = est_tcool_of_contact(OPHS_pref, TFABS_pref, ct, mp_jump)
    # Lower bound for the relative cooling time:
    Rlob = Tlob/contact.tcool_limit(ct)
    Rmax = max(Rmax, Rlob)
    if Rmax > 1: break
  return Rmax
  # ----------------------------------------------------------------------
    
def est_tcool_of_contact(OPHS_pref, TFABS_pref, ct, mp_jump):
  # The contact {ct} must have only one of its sides covered by the
  # implicit prefix path {oph_pref}, the concatenation of all elements
  # of {OPHS_pref}.
  #
  # The procedure computes a lower bound for {contact.tcool(oph,ct)} for any path 
  # that begins with {oph_pref}. 
  #
  # The path groups and path-contact associations must be as in 
  # the entry to {solve_aux};
  #
  # This estimate does NOT take into account the min time needed to
  # connect to {oph_pref} to the other path that covers {ct}, since it
  # may get there faster by using some other path first than by jumpight
  # straight to it.
  
  Tcool_pref = tcool_in_pref(OPHS_pref, TFABS_pref, ct)
  Tcov_min = tcov_min_by_unused(ct)
  assert Tcov_min != None
  assert Tcool_pref != None
  return Tcool_pref + Tcov_min
  # ----------------------------------------------------------------------
  
# ESTIMATING MAX COOLING RATIOS FOR A SPECIFIC EXTENSION OF PREFIX

def quick_cand_max_rcool(OPHS_pref, TFABS_pref, Tfab_conn, oph, CTSA):
  # Computes the maximum relative cooling time ratio among every active contact {ct}
  # in {CTSA} that has one side covered by the current prefix path {oph_pref}, that is
  # the concatenation of all paths in {OPHS_pref}, and the other side covered
  # by {oph}. Assumes that {oph} will be appended to {oph_pref} with a connector
  # whose fabtime (including T/J penalties) is {Tfab_conn}.
  #
  # Assumes that all other parametes and data structures are as in the 
  # entry to {solve_aux}. In particular, the group index of {oph}
  # must be {None}.
  #
  # The returned lower bound is fairly tight if it is at most 1.
  # but the procedure returns as soon as it concludes that it is 
  # greater than 1.
  
  Rmax = 0
  for ct in CTSA:
    Tcov_cand = tcov_by_cand(oph, ct)
    if Tcov_cand != None:
      Tcool_pref = tcool_in_pref(OPHS_pref, TFABS_pref, ct)
      Rct = (Tcool_pref + Tfab_conn + Tcov_cand)/contact.tcool_limit(ct)
      Rmax = max(Rmax, Rct)
  return Rmax
  # ----------------------------------------------------------------------

# COOLING AND COVERING TIMES OF A CONTACT FOR A SPECIFIC PATH

def tcool_in_pref(OPHS_pref, TFABS_pref, ct):
  # The contact {ct} must have only one of its sides covered by the
  # implicit prefix path {oph_pref}, the concatenation of all elements
  # of {OPHS_pref}.
  #
  # The procedure computes the cooling time of that contact since its coverage
  # by {oph_pref} to the end of {oph_pref}. 
  #
  # The path groups and path-contact associations must be as in 
  # the entry to {solve_aux}.

  n_pref = len(OPHS_pref)
  Tfab_pref = (0 if n_pref == 0 else TFABS_pref[-1]) # Total fabtime of {oph_pref}.
  Tcool_pref = None
  for isd in 0, 1:
    CPHS = contact.get_side_paths(ct, isd)
    assert len(CPHS) == 1
    for ph_ct, dr_ct, imv_ct in CPHS:
      oph_ct = (ph_ct, dr_ct) # Covering path as stored in {ct}. 
      nmv = path.nelems(ph_ct)
      igr = path.get_group(ph_ct)
      if igr != None:
        # This side of {ct} is covered by the implicit prefix path.
        assert Tcool_pref == None, "active contact with both sides covered by {oph_pref}"
        # Get the element in the prefix element list {OPHS_pref}:
        assert igr >= 0 and igr < len(OPHS_pref)
        oph_us = OPHS_pref[igr]
        ph_us, dr_us = path.unpack(oph_us)
        assert ph_us == ph_ct
        # The a lower bound for its cooling time:
        # Get fabtime {Tfab_ant} of all elements in {OPHS_pref} before {oph_us}:
        Tfab_ant = (0.0 if igr == 0 else TFABS_pref[igr-1])
        # Get fabtime of {oph_us} until covering {ct}:
        imv_us = (imv_ct if dr_ct == dr_us else nmv-1-imv_ct)
        Tcov_us = contact.path_tcov(oph_us, imv_us, ct, isd)
        # Elapsed time since coverage 
        Tcool_pref = Tfab_pref - Tfab_ant - Tcov_us
  assert Tcool_pref != None, "active contact with neither side covered by {oph_pref}"
  return Tcool_pref
  # ----------------------------------------------------------------------

def tcov_min_by_unused(ct):
  # The contact {ct} must have only one of its sides covered by the
  # implicit prefix path {oph_pref}, the concatenation of all elements
  # of {OPHS_pref}.
  #
  # The procedure computes the minimum time it will take for the path element {oph_un} 
  # on the other side of {ct}, or its reverse, to cover that side, counting from
  # the beginning of that path.
  #
  # The path groups and path-contact associations must be as in 
  # the entry to {solve_aux}.
  
  Tcov_min = None
  for isd in 0, 1:
    CPHS = contact.get_side_paths(ct, isd)
    assert len(CPHS) == 1
    for ph_ct, dr_ct, imv_ct in CPHS:
      igr = path.get_group(ph_ct)
      if igr == None:
        # This side of {ct} is covered by an unused path, {ph_ct}.
        assert Tcov_min == None, "active contact with both sides uncovered"
        Tcov_min = +inf
        # Try both orientations of {oph_ct}:
        nmv = path.nelems(ph_ct)
        oph_ct = (ph_ct, dr_ct) # Covering path as stored in {ct}. 
        for oph_r, imv_r in (oph_ct, imv_ct), (path.rev(oph_ct), nmv-1-imv_ct):
          Tcov_r = contact.path_tcov(oph_r, imv_r, ct, isd)
          assert Tcov_r != None and Tcov_r > 0
          Tcov_min = min(Tcov_min, Tcov_r)
  assert Tcov_min != None, "active contact with neither side covered by {oph_pref}"
  return Tcov_min
  # ----------------------------------------------------------------------

def tcov_by_cand(oph, ct):
  # The contact {ct} must have only one of its sides covered by the
  # implicit prefix path {oph_pref}, the concatenation of all elements
  # of {OPHS_pref}.
  #
  # If the other side is covered by {oph}, the procedure computes the time it will take for {oph},
  # in the given orientation, to cover that side.  If the orther side of {ct}
  # is covered by some other unused path, returns {None}.
  #
  # The path groups and path-contact associations must be as in 
  # the entry to {solve_aux}.
  
  ph, dr = path.unpack(oph)
  assert path.get_group(ph) == None
  for isd in 0, 1:
    CPHS = contact.get_side_paths(ct, isd)
    assert len(CPHS) == 1
    for ph_ct, dr_ct, imv_ct in CPHS:
      if path.get_group(ph_ct) == None:
        # The path {path_ct} is still unused.
        if ph_ct != ph: return None
        imv = (imv_ct if dr_ct == dr else path.nelems(ph_ct)-1-imv_ct)
        Tcov = contact.path_tcov(oph, imv, ct, isd)
        assert Tcov != None and Tcov > 0
        return Tcov
  assert False, "active contact with both sides covered by {oph_pref}"
  # ----------------------------------------------------------------------

def update_contacts(oph, CTSA, CTSI):
  # Updates the active and inactive contacts to account for inclusion of path {oph}.
  CTSA_upd = CTSA.copy()
  CTSI_upd = CTSI.copy()
  
  # Remove from {CTSA_upd} the contacts of {CTSA} closed by {oph}:
  for ct in CTSA:
    ixs = contact.path_ixcovs(oph, ct)
    assert ixs[0] == None or ixs[1] == None, "path covers both sides of contact"
    if ixs[0] != None or ixs[1] != None:
      # Active contact is closed by {oph}
      CTSA_upd.remove(ct)
  # Move from {CTSI_upd} to {CTSA _upd} contacts that are covered by {upd}:
  for ct in CTSI:
    ixs = contact.path_ixcovs(oph, ct)
    assert ixs[0] == None or ixs[1] == None, "path covers both sides of contact"
    if ixs[0] != None or ixs[1] != None:
      # Inactive contact is covered by {oph}
      CTSA_upd.add(ct)
      CTSI_upd.remove(ct)
  return CTSA_upd, CTSI_upd
  # ----------------------------------------------------------------------

def describe_and_check_input(wr, OPHS, org, mp_jump):
  wr.write("### INPUT DATA ######################################################################\n")
  wr.write("org = %s\n" % ("None" if org == None else ("( %12.6f %12.6f )" % org)))
  wr.write("!! Not implemented !!\n")
  wr.write("#####################################################################################\n")
  return
  # ----------------------------------------------------------------------