# Implementation of module {path}
# Last edited on 2021-05-10 18:36:47 by jstolfi

import path
import move
import move_parms
import block
import hacks
import rn 
from math import nan, inf, sqrt, sin, cos, pi, floor
import pyx 
import sys

class Path_IMP:
  # The initial and final points of the {Path} object, in its native
  # orientation, are {ph.pt[0]} and {ph.pt[1]}, respectively.
  #
  # The list of oriented moves (traces and jumps) that comprise a path
  # {ph}, in the native order, is {ph.OMVS}. Namely, tracing the oriented
  # path {(ph,0)} means executing the oriented moves
  # {ph.OMVS[0],ph.OMVS[1],...,ph.OMVS[nmv-1]}, in this sequence, where
  # {nmv=len(ph.OMVS)}. Tracing the oriented path {(ph,1)} means executing
  # {rev(ph.OMVS[nmv-1]),rev(ph.OMVS[nmv-2]),...,rev(ph.OMVS[0])}, in this
  # sequence.
  #
  # The field {ph.cumtex} is a list such that {ph.cumtex[k]} is the
  # cumulative time to execute moves {ph.OMVS[0]} through {ph.OMVS[k]},
  # for each {k} in {0..nmv-1}. These times include the penalty times for
  # each trace/move or move/trace transition. The transition penalty is
  # always assumed to be added  to the execution time of the jump.
  #
  # These times assume that the nozzle is stationary at the beginning
  # and end of each move, so that {ph.cumtex[k]} is just the sum of the
  # execution times of the individual moves plus the applicable
  # transition penalties.
  
  def __init__(self, p, q, OMVS, cumtex):
    # Read-only fields:
    self.OMVS = OMVS
    self.cumtex = cumtex
    self.pt = (p, q)
    self.name = None
    # Mutable fields for the {hotpath} heuristic:
    self.ctss = [[],[]] # Contact lists.
    self.bc = None # Owning block.

# ATTRIBUTES

def nelems(oph):
  ph, dr = unpack(oph)
  return len(ph.OMVS)
  
def elem(oph, imv): 
  ph, dr = unpack(oph)
  # sys.stderr.write("oph = %s ph = %s dr = %s\n" % (str(oph), str(ph), str(dr)))
  if dr == 0:
    return ph.OMVS[imv]
  else:
    nmv = len(ph.OMVS)
    return move.rev(ph.OMVS[nmv-1-imv])
    
def find(oph, omv):
  nmv = nelems(oph)
  mv, dr = move.unpack(omv)
  for imv in range(nmv):
    omvk = elem(oph,imv)
    mvk, drk = move.unpack(omvk)
    if mvk == mv: return imv
  return None

# GEOMETRY

def pini(oph):
  ph, dr = unpack(oph)
  return ph.pt[dr]
  
def pfin(oph):
  ph, dr = unpack(oph)
  return ph.pt[1-dr]

def bbox(OPHS):
  B = None
  for oph in OPHS:
    ph, dr = unpack(oph)
    B = rn.box_include_point(B, pini(oph)) # In case the path is empty.
    B = rn.box_join(B, move.bbox(ph.OMVS))
  return B
  # ----------------------------------------------------------------------

# CREATION

def make_empty(p):
  return path.Path(p, p, (), ())
  
def from_move(p, q, mp):
  mv = move.make(p, q, mp)
  return path.Path(p, q, ( mv, ), ( move.extime(mv), ) )

def from_moves(omvs):
  assert type(omvs) is list or type(omvs) is tuple
  nmv = len(omvs) # Number of moves.
  assert nmv >= 1, "must have at least one move"
  p = move.pini(omvs[0])   # Initial point of final path.
  q = move.pfin(omvs[nmv-1]) # Final point of final path.
  p_prev = p
  t_prev = 0 
  cumtex = []
  for imv in range(nmv):
    omvk = omvs[imv]
    mvk, drk = move.unpack(omvk) # For type checking.
    pk, qk = move.endpoints(omvk)
    assert pk == p_prev, "moves are not connected: %s %s" % (str(p_prev),str(pk))
    tfink = t_prev + move.extime(omvk)
    if move.is_jump(omvk):
      udk = move.ud_penalty(omvk)
      # Add the applicable move/jump transition penalties:
      if imv > 0 and not move.is_jump(omvs[imv-1]):
        tfink += udk
      if imv < nmv-1 and not move.is_jump(omvs[imv+1]):
        tfink += udk
    cumtex.append(tfink)
    p_prev = qk
    t_prev = tfink
  assert p_prev == q
  return path.Path(p, q, tuple(omvs), tuple(cumtex))
  
def from_points(pts, mp_trace, mp_jump):
  assert type(pts) is list or type(pts) is tuple
  m = len(pts) # Number of points or lists.
  assert m >= 1, "must have at least one point"
  p_prev = None
  omvs = []
  cumtex = []
  jmp = None # True if previous element of {pts} was list, false if it was point.
  for ptj in pts:
    if hacks.is_point(ptj):
      # Append a single move with the given width:
      if p_prev != None: 
        mv = move.make(p_prev, ptj, mp_trace)
        omvs.append(mv)
      p_prev = ptj
      jmp = False
    else:
      # {ptj}  must be a list of points. Appends a sequence of moves from {p_prev}
      # to those points.  The first move, if any, will be a jump, the rest 
      # will be traces.  Then forces a jump to whatever comes next (if any).
      assert type(ptj) is list or type(ptj) is tuple
      mpjk = mp_jump # Parameters of of next move.
      for ptjk in ptj:
        assert hacks.is_point(ptjk)
        if p_prev != None: 
          mv = move.make(p_prev, ptjk, mpjk)
          omvs.append(mv)
        p_prev = ptjk
        mpjk = mp_trace
      mpjk = mp_jump
  assert p_prev != None # Since there must have been at least one point.
  if len(omvs) == 0:
    return path.make_empty(p_prev)
  else:
    return path.from_moves(omvs)

def concat(ophs, use_jumps, use_links, mp_jump):
  assert type(ophs) is list or type(ophs) is tuple
  assert type(use_jumps) is bool
  assert type(use_links) is bool
  assert mp_jump == None or isinstance(mp_jump, move_parms.Move_Parms)
  
  m = len(ophs) # Number of paths.
  assert m >= 1, "must have at least one path"
  p = pini(ophs[0])   # Initial point of final path.
  q = pfin(ophs[m-1]) # Final point of final path.
  p_prev = p
  omvs = []
  for ophj in ophs:
    phj, drj = unpack(ophj) # For type checking.
    nmvj = nelems(ophj)
    pj = pini(ophj)
    qj = pfin(ophj)
    if p_prev != pj:
      # Needs a connector. Get one:
      if use_jumps or nmvj == 0 or len(omvs) == 0:
        # Use a jump:
        cnj = move.make(p_prev, pj, mp_jump)
      else:
        # Use a link if possible and desired or favorable
        assert nmvj > 0
        use_jump = False
        use_link = use_links
        omv_prev = omvs[-1]
        omv_next = path.elem(ophj,0)
        cnj = move.connector(omv_prev, omv_next, use_jump, use_link, mp_jump)
      # Insert the connector:
      assert isinstance(cnj, move.Move)
      omvs.append(cnj)
      p_prev = pj
    # Append the moves of {ophj}:
    for imv in range(nmvj):
      omvk = elem(ophj, imv)
      pk, qk = move.endpoints(omvk)
      assert p_prev == pk
      omvs.append(omvk)
      p_prev = qk
  assert p_prev == q
  return path.from_moves(omvs)

def displace(oph, ang, v):
  nmv = nelems(oph)
  if (nmv == 0):
    p = pini(oph)
    return empty(p)
  else:
    omvs = []
    for imv in range(nmv):
      omvk = elem(oph, imv)
      mpk = move.parameters(omvk)
      omvs.append(move.displace(omvk, ang, v, mpk))
    return from_moves(omvs)

# ORIENTATION

def rev(oph):
  ph, dr = unpack(oph)
  return (ph, 1-dr)

def spin(oph, dr):
  ph1, dr1 = unpack(oph)
  return (ph1, (dr1 + dr) % 2)

def unpack(oph):
  # sys.stderr.write("oph = %s\n" % str(oph))
  if isinstance(oph, path.Path):
    # sys.stderr.write("returning %s, 0\n" % str(oph))
    return oph, 0
  else:
    assert type(oph) is tuple
    assert len(oph) == 2
    ph, dr = oph
    # sys.stderr.write("ph = %s dr = %s\n" % (str(ph), str(dr)))
    assert isinstance(ph, path.Path)
    assert dr == 0 or dr == 1
    return ph, dr

# TIMING

def extime(oph):
  ph, dr = unpack(oph)
  nmv = len(ph.OMVS)
  return 0 if nmv == 0 else ph.cumtex[nmv-1]
  
def tini(oph, imv):
  ph, dr = unpack(oph)
  nmv = len(ph.OMVS)
  assert imv >= 0 and imv <= nmv
  if imv == 0: return 0
  if imv == nmv: return extime(oph)
  if dr == 0:
    # Native direction:
    return ph.cumtex[imv-1]
  else:
    # Reverse direction
    return ph.cumtex[nmv-1] - ph.cumtex[nmv-1-imv] 
  
def tfin(oph, imv):
  return tini(oph, imv+1)

# PLOTTING

def plot_to_files(fname, OPHS, CLRS, wd_axes):

  assert type(OPHS) is list or type(OPHS) is tuple
  assert len(OPHS) > 0

  # Compute the plot's bounding box:
  B = bbox(OPHS)
  pbox = hacks.round_box(B, 1)
  szx, szy = rn.box_size(pbox)
  sz_max = max(szx, szy)

  c = pyx.canvas.canvas()
  pyx.unit.set(uscale=1.00, wscale=1.00, vscale=1.00)

  wd_grid = 0.002*sz_max
  hacks.plot_grid(c, None, wd_grid, None, pbox, +5*wd_grid, 1,1)

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

  axes = True
  dots = True
  arrows = True
  matter = True

  plot_standard(c, OPHS, None, None, CLRS, wd_axes, axes, dots, arrows, matter)
  hacks.write_plot(c, fname)
  return
  # ----------------------------------------------------------------------

def plot_standard(c, OPHS, dp, layer, CLRS, wd_axes, axes, dots, arrows, matter):

  assert type(OPHS) is list or type(OPHS) is tuple
  nph = len(OPHS)
  assert nph > 0

  assert wd_axes != None and wd_axes > 0, "invalid wd_axes"
  
  if CLRS == None: 
    CLRS = [ pyx.color.rgb(0.050, 0.800, 0.000), ] # Default trace color.
  else:
    assert type(CLRS) is list or type(CLRS) is tuple
  nclr = len(CLRS)
  assert nclr == 1 or nclr == nph

  def pick_colors(k):
    # Returns the colors for trace sausages and axes of path {OPHS[k]}.
    if nclr == 1:
      ctraces = CLRS[0]
    else:
      ctraces = CLRS[k]
    caxes =   pyx.color.rgb(0.6*ctraces.r, 0.6*ctraces.g, 0.6*ctraces.b) # Color of trace axis, dots, arrow.
    return ctraces, caxes

  # Colors:
  cmatter = pyx.color.rgb(0.850, 0.800, 0.750)  # Material footprint color.
  cjumps =  pyx.color.rgb.black  # Color of jump axes, dots, arrows.

  # Dimensions relative to nominal trace widths:
  rmatter = 1.13; # Material footprint.
  rtraces = 0.80; # Trace sausage width.

  # Absolute dimensions (mm):
  wd_dots =   2.5*wd_axes;  # Dots at ends of moves.
  sz_arrows = 8*wd_axes     # Size of arrows. 

  # Get list {lys} of layers to plot:
  if layer == None:
    # Plot all four layers.
    lys = (range(4))
  else:
    # Plot only the selected layer.
    assert type (layer) is int
    assert layer >= 0 and layer < 4
    lys = (layer,)

  # Plot the layers:
  for ly in lys:
    for k in range(nph):
      oph = OPHS[k]
      ctraces, caxes = pick_colors(k)
      if ly == 0 and matter:
        # plots the estimate of actual material:
        path.plot_layer\
          ( c, oph, dp, jmp=False, \
            clr=cmatter, rwd=rmatter, wd=0, 
            dashed=False, wd_dots=0, sz_arrows=None
          )
      elif ly == 1:
        # plots the nominal trace material:
        path.plot_layer \
          ( c, oph, dp, jmp=False,
            clr=ctraces, rwd=rtraces, wd=0, 
            dashed=False, wd_dots=0, sz_arrows=None
          )
      elif ly == 2 and (axes or dots or arrows):
        # Plots the trace axis:
        t_wd = wd_axes if axes else 0
        t_wd_dots = wd_dots if dots else 0
        t_sz_arrows = sz_arrows if arrows else 0
        path.plot_layer \
          ( c, oph, dp, jmp=False, 
            clr=caxes, rwd = 0, wd=t_wd, 
            dashed=False, wd_dots=t_wd_dots, sz_arrows=t_sz_arrows
          )
      elif ly == 3:
        # Plots the jump:
        j_wd = wd_axes;
        j_wd_dots = wd_dots;
        j_sz_arrows = sz_arrows
        path.plot_layer \
          ( c, oph, dp, jmp=True, 
            clr=cjumps, rwd = 0, wd=j_wd, 
            dashed=True, wd_dots=j_wd_dots, sz_arrows=j_sz_arrows
          )

def plot_layer(c, oph, dp, jmp, clr, rwd, wd, dashed, wd_dots, sz_arrows):
  # Get the {Path} object {ph} and the direction {dr} ({None} if un-oriented):
  ph, dr = unpack(oph)

  if clr == None: return

  # Simplfications:
  if rwd == None: rwd = 0
  if wd == None: wd = 0
  if wd_dots == None: wd_dots = 0
  if sz_arrows == None: sz_arrows = 0

  assert rwd >= 0 and wd >= 0 and wd_dots >= 0 and sz_arrows >= 0

  for imv in range(nelems(oph)):
    omvk = elem(oph, imv)
    if move.is_jump(omvk) == jmp:
      wdk = rwd*move.width(omvk) + wd
      move.plot_layer(c, omvk, dp, clr, wdk, dashed, wd_dots, sz_arrows)
  return
  # ----------------------------------------------------------------------

def plot_elis(c, OPHS, CLRS, wd_axes, d):
  assert isinstance(OPHS, path.Path)

  def pick_colors(k):
    nclr = len(CLRS)

    # Returns the colors for trace sausages and axes of move {OMVS[k]}.
    if nclr == 1:
      ctrace = CLRS[0]
    else:
      ctrace = CLRS[k]
    caxis =   pyx.color.rgb(0.6*ctrace.r, 0.6*ctrace.g, 0.6*ctrace.b) # Color of trace axis, dots, arrow.

    return ctrace, caxis

  moves_list = split_at_jumps(OPHS, d)
  block_id = 0
  isBlock = False

  # Dimensions relative to nominal trace widths:
  rtraces = 0.80; # Trace sausage width.

  # Absolute dimensions (mm):
  j_wd_dots =   2.5*wd_axes;  # Dots at ends of moves.
  j_sz_arrows = 8*wd_axes     # Size of arrows. 

  if CLRS == None:
    CLRS = hacks.trace_colors(len(moves_list))

  for moves in moves_list:
    if len(moves) == 1 and move.is_jump(moves[0]):
      isBlock = False
      clr = pyx.color.rgb.black
      rwd = 0
      wd = wd_axes
      dashed = True
      wd_dots = j_wd_dots
      sz_arrows = j_sz_arrows

    else:
      pini = move.pini(moves[0])
      pfin = move.pfin(moves[-1])

      if pini == pfin:
        isBlock = False
        clr = pyx.color.rgb.black

      else:
        isBlock = True
        clr, caxis = pick_colors(block_id)
        block_id += 1
      
      rwd = rtraces
      wd = 0
      dashed = False
      wd_dots = 0
      sz_arrows = 0
    
    for mv in moves:  
      wdk = rwd*move.width(mv) + wd    
      move.plot_layer(c, mv, None, clr, wdk, dashed, wd_dots, sz_arrows)

      if isBlock:
        move.plot_layer(c, mv, None, caxis, wd_axes, True, j_wd_dots, j_sz_arrows)

  return moves_list

def split_at_jumps(oph, d):
  moves_list = []
  moves_aux = []

  for k in range(nelems(oph)):
    omvk_aux = elem(oph, k)
    omvk = omvk_aux

    if not isinstance(omvk_aux, move.Move):
      omvk_aux = omvk_aux[0]
    elif d != None:
      omvk = (omvk, d)

    if move.is_jump(omvk_aux):
      if len(moves_aux) > 0:
        moves_list.append(moves_aux)
      moves_list.append([omvk])
      moves_aux = []

    else:
      moves_aux.append(omvk)

  if len(moves_aux) > 0:
    moves_list.append(moves_aux)

  return moves_list

# DEBUGGING AND TESTING

def validate(oph):
  nmv = nelems(oph)
  ph, dr = unpack(oph)
  assert len(ph.OMVS) == nmv
  assert len(ph.cumtex) == nmv
  # sys.stderr.write("dr = %d cumtex = %s\n" % (dr, str(ph.cumtex)))
  # Validate the list of moves:
  p_prev = pini(oph)  # Final point of previous move.
  t_prev = 0 # Cumulative execution time.
  for imv in range(nmv):
    omvk = elem(oph, imv)
    # sys.stderr.write("omvk = %s\n" % str(omvk))
    mvk, drk = move.unpack(omvk) # Just to typecheck.
    # sys.stderr.write("mvk = %s drk = %s\n" % (str(mvk), str(drk)))
    mv_pini, mv_pfin = move.endpoints(omvk)
    # sys.stderr.write("p_prev = %s pini = %s\n" % (str(p_prev), str(mv_pini)))
    assert p_prev == mv_pini
    mv_tini = tini(oph, imv)
    mv_tfin = tfin(oph, imv)
    mv_tex = move.extime(omvk)
    # Account for up/down penalties:
    if move.is_jump(omvk):
      # Add the applicable move/jump transition penalties:
      udk = move.ud_penalty(omvk)
      if imv > 0 and not move.is_jump(elem(oph,imv-1)):
        mv_tex += udk
      if imv < nmv-1 and not move.is_jump(elem(oph,imv+1)):
        mv_tex += udk
    # sys.stderr.write("imv = %d" % imv)
    # sys.stderr.write(" t_prev = %12.8f tini = %12.8f tfin = %12.8f" % (t_prev,mv_tini,mv_tfin))
    # sys.stderr.write(" tex = %12.8f\n" % mv_tex)
    assert abs(t_prev - mv_tini) < 1.0e-8
    assert abs((mv_tfin - mv_tini) - mv_tex) < 1.0e-8
    p_prev = mv_pfin
    t_prev = mv_tfin
  assert p_prev == pfin(oph)
  assert abs(t_prev - extime(oph)) < 1.0e-8

def get_name(oph):
  ph, dr = unpack(oph)
  name = ph.name
  if name == None: name = "P?"
  if dr == 1: name = "~" + name
  return name
  # ----------------------------------------------------------------------

def set_name(oph, name):
  assert type(name) is str
  ph, dr = unpack(oph)
  ph.name = name
  return
  # ----------------------------------------------------------------------

def show(wr, oph, moves, ind, wna, wnm):
  ph, dr = unpack(oph)
  wr.write("  "*ind)
  wr.write(" " if dr == 0 else "~")
  wr.write("%-*s" % (wna,get_name(ph)))
  nmv = nelems(oph)
  wr.write(" %*d" % (wnm,nmv))
  p = pini(oph)
  q = pfin(oph)
  wr.write(" (%6.1f,%6.1f)" % (p[0],p[1]))
  wr.write(" (%6.1f,%6.1f)" % (q[0],q[1]))
  if moves:
    wr.write(" ")
    for imv in range(nmv):
      omv = elem(oph, imv)
      mvname = move.get_name(omv)
      if imv > 0: wr.write(",")
      wr.write(mvname)
  return
  # ----------------------------------------------------------------------

def show_list(wr, OPHS, moves):
  assert type(OPHS) is list or type (OPHS) is tuple
  nph = len(OPHS)
  if nph == 0: return
  wna = 3 # Width of "name" column; min 3 because of the header.
  wnm = 1 # Width of "number of moves" column.
  for oph in OPHS: 
    ph, dr = unpack(oph)
    wna = max(wna, len(get_name(ph)))
    wnm = max(wnm, len(str(nelems(oph))))
  wix = len(str(nph-1)) # Num digits in index.
  
  wr.write("\n")

  # Write header:
  wr.write("%*s %-*s %*s %*s %*s moves \n" % (wix,"k",wna+1,"name",wnm,"n",15,"pini",15,"pfin"))
  wr.write("%s %s %s %s %s %s\n" % ("-"*wix,"-"*(wna+1),"-"*wnm,"-"*15,"-"*15,"-"*15))
  
  # Write paths:
  for k in range(len(OPHS)):
    oph = OPHS[k]
    wr.write("%*d " % (wix,k))
    show(wr, oph, moves, 0, wna, wnm)
    wr.write("\n")

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