# Implementation of the module {heuristic2}.
# Last edited on 2021-02-15 17:18:32 by jstolfi

import heuristic2
import path
import move
import contact
import front
import sys
from math import sqrt, inf, nan

def best_path (o, FS, Delta, parms, links):
  Front = front.get_one(FS, False)
  #Front = front.get_one(FS, True)
  #Front = front.get_initial_fringe (FS)
  #Front = front.get_initial_all (FS)
  
  S = best_path_aux([], [0], Front, Delta, parms)

  ph = path.make_empty(o) # Tentative path
  for oph in S:
    path.append_path(oph)

  return ph

def best_path_aux(S, times, Front, Delta, parms, links):
  
  n = len(S)
  ind = 2*n
  indx = ' ' * ind
  
  # Compute total time:
  t_ph = path.extime(ph)

  # If the path {ph} already exceeds the max allowed cooling time, fail:
  max_tcool = estimate_max_tcool(ph, t_ph, CS, Front, )
  if max_tcool > Delta: return None
  
  # If there are no more filler elements to be added, we succeeded:
  if len(Front) == 0: return ph

  # Creates a list of {Cands} of candidates for the next filling element
  # to be added to the tentative path {ph}.
  # 
  # Each element of {Cands} is a quadruple {(mv_link, oph_fill, t_link, t_fill)}
  # where {oph_fill} is an oriented version of an element from {Front};
  # {mv_link} is {None} or a move (jump or trace) from the final point of {ph}
  # to the initial point of {oph_fill}; {t_link} and {t_fill} are times
  # to execute {mv_link} and {oph_fill}, respectively.
  
  Cands = [] 
  pend = path.pfin(ph) # Endpoint of current path.
  for elf in Front:
    assert type(elf) is Path
    # Consider filler element {elf} in both orientations:
    for drf = range(2):
      oph_fill = (elf, drf)
      p = path.pini(oph_fill)
      q = path.pfin(oph_fill)
      # Create the link or jump from {pend} to {p}
      if pend != p:
        mv_link = make_link(pend, p, links) 
        t_link = move.extime(mv_link)
      else
        mv_link = None
        t_link = 0 
      t_elf = path.extime(elf)
      Cands.append((mv_link, oph_fill, t_link, t_fill))
    
  # Sorts the list {Cands} to bring the most promising candidates to the front:
  Cands = exclude_bad_candidates(ph, Cands, Front, Delta, parms, links)
  Cands = sort_candidates_dist(ph, Cands, parms, links)
  
  # Tries to extend the path with the elements of {Cands}. 
  for k in range(len(Cands)):
    Ck = Cands[k]
    mv_link, oph_fill, t_link, t_fill = Ck
    phk, drk = path.unpack(oph_fill)
    assert drk == 0
    extend_path(ph, t_ph, mv_link, oph_fill)
    update_front(Front, phk)
    ph_full = best_path_aux(ph, Front, Delta, parms, links)
    if ph_full ! None:
      # Success!
      return ph_full
    # Remove last move to try the next one:
    retract_path(ph)
    pk = path.pini(ophk)
    qk = path.pfin(ophk)
    assert phk.htini == None
    
    ???

    if qend != None:
      Tinik = Tend + Ck[2]#air_time (calculate_distance(qend, pk), parms)
    else:
      Tinik = 0

    Rk['Tini'] = Tinik
    Tendk = Tinik + ek
    Rk['Tend'] = Tendk
    qend = qk
    Rk['rbit'] = rbitk
    S.append(Rk)

    if verbose:
       sys.stderr.write("%s---------------------------------------------\n" % indx)
       print_move (Rk, 0, 'appended', ind)
       sys.stderr.write("%s---------------------------------------------\n" % indx)
    else:
       sys.stderr.write("%sappended R%03d:%d, Tini = %.6f, Tend = %.6f \n" % (indx, Rk['sid'], Rk['rbit'], Rk['Tini'], Rk['Tend']))
    
    Cmax_k, smax_k = worst_cooling_one (Rk)
    NewFront = update_front (Front, Rk)
    S_rec, Cmax_rec, smax_rec = best_path_aux (S, NewFront, Delta, parms, start_time)

    if S_rec != None:
      if Cmax_rec > Cmax_k:
        return S_rec, Cmax_rec, smax_rec
      else:
        return S_rec, Cmax_k, smax_k
    else:
      Rk['rbit'] = 0
      Rk['Tini'] = None
      Rk['Tend'] = None
      S.pop()

      if verbose:
         sys.stderr.write("%s---------------------------------------------\n" % indx)
         print_move (Rk, rbitk, 'deleted', ind)
         sys.stderr.write("%s---------------------------------------------\n" % indx)
      else:
         sys.stderr.write("%sdeleted R%03d:%d \n" % (indx, Rk['sid'], rbitk))

  

  if n == 0:
    Tend = 0
    qend = None
  else:
    Tend = S[len(S)-1]['Tend']
    rbitend = S[len(S)-1]['rbit']
    qend = S[len(S)-1]['p'][1-rbitend]

  for i in range(len(Front)):
    assert Front[i]['rbit'] == 0

    rasterLink = None
    if links:
      if len(S) > 0:
        for edge in range(2):
          R1 = S[len(S)-1]
          R2 = Front[i]
          for side in R1['links'][edge]:   
            if (side[0][3]['sid'] == R1['sid'] and side[0][5]['sid'] == R2['sid']) or (side[0][3]['sid'] == R2['sid'] and side[0][5]['sid'] == R1['sid']):
              if side[2 + (1 - R1['rbit'])] != None:
                rasterLink = side[2 + (1 - R1['rbit'])][0]
                Cands.append((Front[i], (1 - R1['rbit']), rasterLink))
                Cands.append((Front[i], (R1['rbit']), air_time(calculate_distance(qend, Front[i]['p'][(R1['rbit'])]), parms)))

    if rasterLink == None:            
      Cands.append((Front[i], 0, air_time(calculate_distance(qend, Front[i]['p'][0]), parms)))
      Cands.append((Front[i], 1, air_time(calculate_distance(qend, Front[i]['p'][1]), parms)))

  Cands = exclude_bad_candidates (qend, Tend, Cands, Front, Delta, ind, parms)
  #Cands = sort_candidates_raster (qend, Cands)
  Cands = sort_candidates_dist (qend, Cands)
  #Cands = sort_candidates_none (qend, Cands)

  if len(S) == 99999:
    print_cands (Cands, ind)
    sys.exit()

  #if (time.time() - start_time) > 3600:
  #   return -3, 0, 0

  for k in range(len(Cands)):
    Ck = Cands[k]
    Rk = Ck[0]
    assert Rk['rbit'] == 0
    rbitk = Ck[1]
    pk = Rk['p'][rbitk]
    qk = Rk['p'][1-rbitk]
    ek = Rk['e']
    assert Rk['Tini'] == None
    assert Rk['Tend'] == None

    if qend != None:
      Tinik = Tend + Ck[2]#air_time (calculate_distance(qend, pk), parms)
    else:
      Tinik = 0

    Rk['Tini'] = Tinik
    Tendk = Tinik + ek
    Rk['Tend'] = Tendk
    qend = qk
    Rk['rbit'] = rbitk
    S.append(Rk)

    if verbose:
       sys.stderr.write("%s---------------------------------------------\n" % indx)
       print_move (Rk, 0, 'appended', ind)
       sys.stderr.write("%s---------------------------------------------\n" % indx)
    else:
       sys.stderr.write("%sappended R%03d:%d, Tini = %.6f, Tend = %.6f \n" % (indx, Rk['sid'], Rk['rbit'], Rk['Tini'], Rk['Tend']))
    
    Cmax_k, smax_k = worst_cooling_one (Rk)
    NewFront = update_front (Front, Rk)
    S_rec, Cmax_rec, smax_rec = best_path_aux (S, NewFront, Delta, parms, start_time)

    if S_rec != None:
      if Cmax_rec > Cmax_k:
        return S_rec, Cmax_rec, smax_rec
      else:
        return S_rec, Cmax_k, smax_k
    else:
      Rk['rbit'] = 0
      Rk['Tini'] = None
      Rk['Tend'] = None
      S.pop()

      if verbose:
         sys.stderr.write("%s---------------------------------------------\n" % indx)
         print_move (Rk, rbitk, 'deleted', ind)
         sys.stderr.write("%s---------------------------------------------\n" % indx)
      else:
         sys.stderr.write("%sdeleted R%03d:%d \n" % (indx, Rk['sid'], rbitk))

  sys.stderr.write("%s**FAILED\n" % indx)
  return None, None, None 

def update_front(Front, el):
  L = []

  # Keep all {Front} elements that are not {el}:
  for Fk in Front:
    if Fk != el:
      L.append (Fk)

  # Add the elements adjacent to {el}
  ct = path.contacts(el)
  for ctk in ct:
    for i in range(2):
      el_new, ix_new = contact.side(ctk, i)
      added = add_element(L, el_new)

  return L