# Implementation of module {move}
# Last edited on 2021-02-18 22:22:25 by jstolfi

import move
import hacks
import rn
import pyx 
from math import nan, inf, sqrt
import sys

class Move_IMP:
  # The field {mv.pt} is a 2-tuple with the two endpoints of the 
  # axis, in arbitrary order.   An oriented move {(mv,dr)} means 
  # that the motion is from {mv.pt[dr]} to {mv.pt[1-dr]}.
  #
  # The field {mv.width} is the nominal width.
  # The field {mv.extime} is the time (in seconds) needed to execute it.

  def __init__(self, p0, p1, wd, tex):
    self.pt = (p0, p1)
    self.width = wd
    self.extime = tex

def make(p0, p1, wd, parms):
  assert hacks.is_point(p0)
  assert hacks.is_point(p1)
  assert wd >= 0
  dpq = rn.dist(p0, p1)
  jmp = (wd == 0)
  tex = nozzle_travel_time(dpq, jmp, None, parms)
  return move.Move(p0, p1, wd, tex)
  
def connector(omvprev, omvnext, parms):
  p = pfin(omvprev)
  vprev = rn.sub(p, pini(omvprev))
  q = pini(omvnext)
  vnext = rn.sub(pfin(omvnext), q)
  assert p != q
  vpq = rn.sub(q, p)
  wdp = width(omvprev)
  wdq = width(omvnext)
  if wdp == 0 or wdq == 0 or wdp != wdq:
    # Use a jump.
    wd = 0
  else:
    # Decide based on geometry:
    assert wdp == wdq
    if connector_must_be_jump(vpq, vprev, vnext, wdp, parms):
      # Use a jump:
      wd = 0
    else:
      # Use a trace with the same width:
      wd = wdp
      
  mv = make(p, q, wd, parms)
  return mv
  
def connector_must_be_jump(vpq, vprev, vnext, wd, parms):
  
  dpq =  rn.norm(vpq)
  
  # If the distance to be covered is too big, use a jump:
  if dpq >= 3*wd: 
    # sys.stderr.write("dist = %12.0f too big\n" % (dpq/wd))
    return True
  
  # If a jump be faster, use a jump:
  tjmp = nozzle_travel_time(dpq, True,  None, parms)
  tlnk = nozzle_travel_time(dpq, False, None, parms)
  if tjmp < tlnk:
    return True

  # If either of the adjacent moves is too short, use a jump:
  if rn.norm(vprev) <= 2*wd or  rn.norm(vnext) <= 2*wd: 
    # sys.stderr.write("dists = %12.f %12.6f too small\n" % (rn.norm(vprev)/wd, rn.norm(vnext)/wd))
    return True
  
  # Compute the sign of the turning angles:
  eps2 = 1.0e-6*wd*wd
  s1 = rn.cross2d(vprev,vpq)
  s2 = rn.cross2d(vpq,vnext)
  if (s1 >= -eps2 and s2 <= +eps2) or (s1 <= +eps2 and s2 >= -eps2):
    # Angles have opposite signs, or nearly so -- use a jump:
    # sys.stderr.write("signs = %12.f %12.6f not consistent\n" % (s1,s2))
    return True
  
  # Risk a trace:
  return False

def displace(omv, ang, disp, parms):
  p = tuple(rn.add(rn.rotate2(pini(omv), ang), disp))
  q = tuple(rn.add(rn.rotate2(pfin(omv), ang), disp))
  return make(p, q, width(omv), parms)

def is_jump(omv):
  mv, dr = unpack(omv)
  return mv.width == 0

def width(omv):
  mv, dr = unpack(omv)
  return mv.width

def pini(omv):
  mv, dr = unpack(omv)
  return mv.pt[dr]

def pfin(omv):
  mv, dr = unpack(omv)
  return mv.pt[1-dr]

def bbox(omv):
  B = rn.box_from_point(pini(omv))
  B = rn.box_include_point(B, pfin(omv))
  return B
  # ----------------------------------------------------------------------

def rev(omv):  
  mv, dr = unpack(omv)
  return (mv, 1-dr)

def orient(omv, dr):
  mv1, dr1 = unpack(omv)
  return (mv1, (dr1 + dr) % 2)

def unpack(omv):
  if isinstance(omv, move.Move):
    return omv, 0
  else:
    # sys.stderr.write("omv =%s\n" % str(omv))
    assert type(omv) is tuple
    assert len(omv) == 2
    mv, dr = omv
    assert isinstance(mv, move.Move)
    assert dr == 0 or dr == 1
    return mv, dr

def extime(omv):
  mv, dr = unpack(omv)
  return mv.extime
  
def cover_time(omv, m, parms):
  mv, dr = unpack(omv) # To type-check.

  # Compute cover time {tomv} from start of move {omv}:
  p = move.pini(omv)
  q = move.pfin(omv)
  vpm = rn.sub(m, p)
  vpq = rn.sub(q, p)
  dpq = rn.norm(vpq)
  dpm = rn.dot(vpm,vpq)/dpq  # Relative position of {m}
  # sys.stderr.write("dpq = %12.8f dpm = %12.8f ratio = %12.8f\n" % (dpq, dpm, dpm/dpq))
  if dpm < 0: dpm = 0
  if dpm > dpq: dpm = dpq
  tc = move.nozzle_travel_time(dpq, move.is_jump(omv), dpm, parms)
  return tc

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 plot_standard(c, omv, dp, layer, ctrace, waxis, axis, dots, arrow, matter):
  
  assert waxis != None and waxis > 0
  
  # Get the move endpoints:
  p = pini(omv)
  q = pfin(omv)
  vpq = rn.sub(q,p)
  dpq = rn.norm(vpq)
  wd = width(omv)

  # Relative trace widths:
  rmatter = 1.13;  # Estimated material.
  rtraces =  0.80;  # Nominal trace area.

  # Absolute widths:
  wdots =  2.5*waxis;  # Dots at ends of moves.
  szarrow = 6*waxis   # Size of arrowhead.

  # Colors:
  if ctrace == None: ctrace = pyx.color.rgb(0.050, 0.800, 0.000)
  cmatter = pyx.color.rgb(0.850, 0.800, 0.750) # Material extent.
  caxis = pyx.color.rgb(0.6*ctrace.r, 0.6*ctrace.g, 0.6*ctrace.b)
  cjump = pyx.color.rgb.black             
  
  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,)

  for ly in lys:
    if ly == 0 and wd > 0 and matter:
      # Extent of material:
      wmatter = rmatter*wd
      plot_layer(c, omv, dp, clr=cmatter, waxis=wmatter, dashed=False, wdots=0, szarrow=None)
    elif ly == 1 and wd > 0 and ctrace != None:
      # Conventional trace sausage:
      wtrace = rtraces*wd
      plot_layer(c, omv, dp, clr=ctrace, waxis=wtrace, dashed=False, wdots=0, szarrow=None)
    elif ly == 2 and wd > 0 and (axis or dots or arrow):
      # Trace axis and/or dots and/or arrowhead:
      trc_waxis = waxis if axis else 0
      trc_wdots = wdots if dots else 0
      trc_szarrow = szarrow if arrow else 0
      plot_layer(c, omv, dp, clr=caxis, waxis=trc_waxis, dashed=False, wdots=trc_wdots, szarrow=trc_szarrow)
    elif ly == 3 and wd == 0:
      # Jump axis, dots, and arrowhead:
      jmp_waxis = waxis
      jmp_wdots = wdots
      jmp_szarrow = szarrow
      plot_layer(c, omv, dp, clr=cjump, waxis=jmp_waxis, dashed=True, wdots=jmp_wdots, szarrow=jmp_szarrow)

def plot_layer(c, omv, dp, clr, waxis, dashed, wdots, szarrow):
  
  if clr == None: return
  
  # Simplifications:
  if waxis == None: waxis = 0 
  if wdots == None: wdots = 0 
  if szarrow == None: szarrow = 0
  assert waxis >= 0 and wdots >= 0 and szarrow >= 0 
  
  # Get the move endpoints:
  p = pini(omv)
  q = pfin(omv)
  vpq = rn.sub(q,p)
  dpq = rn.norm(vpq)
  
  # Omit the arrowhead if not enough space for it:
  if dpq <= 2*szarrow: szarrow = 0
  
  # Nothing to plot if nothing is requested:
  if waxis == 0 and wdots == 0 and szarrow == 0: return
  
  arrowpos = 0.5  # Position of arrow along axis.

  # Perturbation to force painting of zero-length lines.
  eps = 0.0001*max(waxis,wdots,szarrow)  # For plotting the dots.
  assert eps > 0
  
  if waxis == 0 and szarrow > 0:
    # We want an invisible line but still with the arrowhead.
    # Unfortunately setting linewidth(0) still draws a thin line.
    # So we cook things up, by moving {p} and {q} right next 
    # to the arrowhead.
    m = rn.mix(1-arrowpos, p, arrowpos, q)  # Posititon of arrow.
    shaft = 0.9*szarrow  # Length of reduced arrow shaft.
    pa = arrowpos*shaft/dpq
    qa = (1-arrowpos)*shaft/dpq
    paxis = rn.mix(1, m, -pa, vpq)
    qaxis = rn.mix(1, m, +qa, vpq)
  elif dpq == 0:
    # Perturb {p,q} to ensure that the zero-length line is drawn as a dot:
    paxis = rn.sub(p,(eps,eps))
    qaxis = rn.add(q,(eps,eps))
  else:
    paxis = p; qaxis = q
  
  # Define styles {sty_axis} for the axis, {sty_dots} for the dots:
  sty_comm = [ pyx.style.linecap.round, pyx.style.linejoin.round, clr, ] # Common style.
  
  sty_axis = sty_comm + [ pyx.style.linewidth(waxis) ]
  if dp != None: sty_axis.append(pyx.trafo.translate(dp[0], dp[1]))

  if szarrow > 0: 
    # Define the arrow style {sty_deco}:
    wdarrow = szarrow/5 # Linewidth for stroking the arrowhead (guess).
    sty_deco = sty_comm + [ pyx.style.linewidth(wdarrow) ]
    sty_deco = sty_deco + [ pyx.deco.stroked([pyx.style.linejoin.round]), pyx.deco.filled([]) ]
    
    # Add an arrowhead in style {sty_deco} to {sty_axis}:
    sty_axis = sty_axis + [ 
      pyx.deco.earrow(sty_deco, size=szarrow, constriction=None, pos=arrowpos, angle=35)
    ]
  
  if wdots > 0:
    # Plot dots:
    sty_dots = sty_comm + [ pyx.style.linewidth(wdots) ]
    if dp != None: sty_dots.append(pyx.trafo.translate(dp[0], dp[1]))
    c.stroke(pyx.path.line(p[0]+eps, p[1]+eps, p[0]-eps, p[1]-eps), sty_dots)
    c.stroke(pyx.path.line(q[0]+eps, q[1]+eps, q[0]-eps, q[1]-eps), sty_dots)

  if waxis > 0 and dashed: 
    # Define dash pattern, or turn off dashing if too short:
    dashed, dashpat = hacks.adjust_dash_pattern(dpq/waxis, 1.75, 2.00)
    if dashed:
      # Make the line style dashed:
      sty_axis = sty_axis + [ pyx.style.linestyle(pyx.style.linecap.round, pyx.style.dash(dashpat)) ]

  if waxis > 0 or szarrow > 0:
    # Stroke the axis, with fixed endpoints:
    c.stroke(pyx.path.line(paxis[0], paxis[1], qaxis[0], qaxis[1]), sty_axis)