import move
import path
import block

import move_parms
import hacks
import rn 

import pyx 
from math import nan, inf, sqrt, sin, cos, pi, floor
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)

# 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)

# 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])
    
# 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
  # ----------------------------------------------------------------------

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

# ORIENTATION

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

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_standard(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