#! /usr/bin/python3
# Test program for module {move}
# Last edited on 2021-02-13 13:54:20 by jstolfi

import move
import hacks
import job_parms
import rn
import pyx
import sys
from math import sqrt, sin, cos, floor, ceil, inf, nan, pi

parms = job_parms.slow()

def test_make_width_is_jump():
  sys.stderr.write("--- testing {make,width,is_jump} -------------\n")

  p1 = (1,1)
  p2 = (3,1)
  p3 = (2,3)
  p4 = (4,4)
  p5 = (1,4)
  wd1 = parms['solid_raster_width']
  wd2 = 0.5*wd1

  mv12 = move.make(p1,p2, wd1, parms); 
  assert not move.is_jump(mv12); 
  assert move.width(mv12) == wd1

  mv23 = move.make(p2,p3,   0, parms); 
  assert move.is_jump(mv23);     
  assert move.width(mv23) == 0

  mv34 = move.make(p3,p4, wd2, parms); 
  assert not move.is_jump(mv34); 
  assert move.width(mv34) == wd2

  mv55 = move.make(p5,p5, wd1, parms); 
  assert not move.is_jump(mv55); 
  assert move.width(mv55) == wd1

def test_move_orient_unpack_rev():
  sys.stderr.write("--- testing {orient,unpack,rev,pini,pfin} -------------\n")

  p1 = (1,1)
  p2 = (3,1)
  wd1 = parms['solid_raster_width']
  mv12 = move.make(p1,p2, wd1, parms); 

  def check_pini_pfin(omvx):
    mvx, drx = move.unpack(omvx)
    assert mvx == mv12
    if drx == 0:
      assert move.pini(omvx) == p1;
      assert move.pfin(omvx) == p2;
    else:
      assert move.pini(omvx) == p2;
      assert move.pfin(omvx) == p1;
  
  for dr in range(4):
  
    omva = move.orient(mv12, dr)
    mva, dra = move.unpack(omva)
    assert dra == dr % 2
    check_pini_pfin(omva)
    
    omvb = move.orient(move.rev(mv12), dr)
    mvb, drb = move.unpack(omvb)
    assert drb == (dr + 1) % 2
    check_pini_pfin(omvb)

def test_displace():
  sys.stderr.write("--- testing {displace} -------------\n")

  p1 = (1,1)
  p2 = (3,1)
  wd1 = parms['solid_raster_width']
  mv12 = move.make(p1,p2, wd1, parms); 

  angr = pi/6
  vr = (2,3)
  mv12r = move.displace(mv12, angr, vr, parms)
  p1r = rn.add(rn.rotate2(p1, angr), vr)
  p2r = rn.add(rn.rotate2(p2, angr), vr)
  assert rn,dist(move.pini(mv12r), p1r) < 1.0e-8

def test_plot():

  sys.stderr.write("--- testing {plot,bbox} -------------\n")

  p1 = (1,1)
  p2 = (3,1)
  p3 = (2,3)
  p4 = (4,4)
  p5 = (1,4)
  wd1 = parms['solid_raster_width']
  wd2 = 0.5*wd1
  mv12 = move.make(p1,p2, wd1, parms); 
  mv23 = move.make(p2,p3,   0, parms); 
  mv34 = move.make(p3,p4, wd2, parms); 
  mv55 = move.make(p5,p5, wd1, parms); 

  szx = 6 # Single plot width. 
  szy = 6 # Single plot height.

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

  def test_plot_bbox(omv, dp, wdbox):
    # Plots the bounding box of move {omv} displaced by {dp}.
    B = move.bbox(omv)
    dpm = rn.add(dp, B[0])
    szxm = B[1][0] - B[0][0]
    szym = B[1][1] - B[0][1]
    hacks.plot_frame(c, pyx.color.rgb.blue, wdbox, dpm, szxm, szym, -wdbox/2)
    return
    # ----------------------------------------------------------------------

  def test_plot_layer(dr, axes, dots, arrows):
    # Tests {move.plot_layer} for one combination of direction and options. 
    # sys.stderr.write("\n")
    # sys.stderr.write("--- enter {test_plot} ---\n")
    sys.stderr.write("test_plot_layer: dr = %d axes = %d dots = %d arrows = %d\n" % (dr,axes,dots,arrows))

    # Colors:
    cmatter = pyx.color.rgb(0.800, 0.750, 0.600) # Est. material footprint.
    ctraces = pyx.color.rgb(0.850, 0.050, 0.000)  # Sausages of traces.
    caxes = pyx.color.rgb(0.450, 0.050, 0.000)   # Axis lines of traces.
    cdots = caxes                                # End dots of traces.
    cjumps = pyx.color.rgb.black                  # Axis lines, dots, and arrowheads of jumps.
    
    # Relative to the nominal width:
    rmatter = 1.13; 
    rtraces =  0.80; 

    # Absolute (in mm):
    wref = 0.15*min(wd1,wd2) # Reference line width.
    waxes = wref if axes else 0 
    wdots = 2.5*wref if dots else 0
    wjump = wref
    szarrows = 6*wref if arrows else 0

    ix = 0
    iy = 8*int(axes) + 4*int(dots) + 2*int(arrows) + dr
    dp = (ix*szx,iy*szy)
    hacks.plot_frame(c, pyx.color.rgb.black, 0.03, dp, szx,szy, -0.02)
    hacks.plot_grid(c, None, 0.02, dp, szx,szy, +0.25, 1,1)
    tp = (dp[0]+0.5, dp[1]+szy-0.5,)
    c.text(tp[0], tp[1], "BYL dr:%d ax:%d dt:%d ar:%d" % (dr,int(axes),int(dots),int(arrows)))
    for layer in range(4):
      # sys.stderr.write("--- layer %d ---\n" % layer)
      for mv in ( mv12, mv34, mv55, mv23 ):
        # sys.stderr.write("\n")
        omv = move.orient(mv, dr)
        jmp = move.is_jump(mv)
        wd = move.width(omv)
        if layer == 0 and not jmp:
          # plots the estimate of actual material:
          wmatter = rmatter*wd
          move.plot_layer(c, omv, dp, clr=cmatter, waxis=wmatter, dashed=False, wdots=0, szarrow=None)
        elif layer == 1 and not jmp:
          # plots the nominal trace material:
          wtraces = rtraces*wd
          move.plot_layer(c, omv, dp, clr=ctraces, waxis=wtraces, dashed=False, wdots=0, szarrow=None)
        elif layer == 2 and not jmp:
          # Plots the trace axis:
          move.plot_layer(c, omv, dp, clr=caxes, waxis=waxes, dashed=False, wdots=wdots, szarrow=szarrows)
        elif layer == 3 and jmp:
          # Plots the jump:
          move.plot_layer(c, omv, dp, clr=cjumps, waxis=wjump, dashed=True, wdots=wdots, szarrow=szarrows)
        if layer == 0:
          test_plot_bbox(omv, dp, 0.25*waxes)
    return
    # ----------------------------------------------------------------------

  def test_plot_standard(dr, axes, dots, arrows):
    # Tests {move.plot_standard} for one combination of direction and options. 
    # sys.stderr.write("\n")
    # sys.stderr.write("--- enter {test_plot} ---\n")
    sys.stderr.write("test_plot_standard: dr = %d axes = %d dots = %d arrows = %d\n" % (dr,axes,dots,arrows))

    # Absolute (in mm):
    wref = 0.15*min(wd1,wd2) # Reference line width.
    waxes = wref;

    ix = 1
    iy = 8*int(axes) + 4*int(dots) + 2*int(arrows) + dr
    dp = (ix*szx,iy*szy)
    hacks.plot_frame(c, pyx.color.rgb.black, 0.03, dp, szx,szy, -0.02)
    hacks.plot_grid(c, None, 0.02, dp, szx,szy, +0.25, 1,1)
    tp = (dp[0]+0.5, dp[1]+szy-0.5,)
    c.text(tp[0], tp[1], "STD dr:%d ax:%d dt:%d ar:%d" % (dr,int(axes),int(dots),int(arrows)))
    if dr == 0:
      # Plot all layers for each move. Note that {mv23} is a jump:
      for mv in ( mv12, mv34, mv23, mv55, ):
        omv = move.orient(mv, dr)
        move.plot_standard \
          ( c, omv, dp, layer=None, ctrace=None, waxis=waxes, 
            axis=axes, dots=dots, arrow=arrows, matter=True
          )
        test_plot_bbox(omv, dp, 0.25*waxes)
    else:
      # Plot layer by layer:
      for layer in range(4):
        # sys.stderr.write("--- layer %d ---\n" % layer)
        for mv in ( mv12, mv23, mv34, mv55 ):
          omv = move.orient(mv, dr)
          move.plot_standard(c, omv, dp, layer, None, waxis=waxes, axis=axes, dots=dots, arrow=arrows, matter=True)
          if layer == 0:
            test_plot_bbox(omv, dp, 0.25*waxes)
    return
    # ----------------------------------------------------------------------
    
  for dr in range(2):
    for axes in False,True:
      for dots in False,True:
        for arrows in False,True:
          test_plot_layer(dr, axes, dots, arrows)
          test_plot_standard(dr, axes, dots, arrows)

  hacks.write_plot(c, "tests/out/move_TST_plot")
  return
  # ----------------------------------------------------------------------

def test_connector():

  sys.stderr.write("--- testing {connector} -------------\n")

  nang = 7 # Number of angles for the second move.
  nq = 5  # Number of starting positions in {X} and {Y} for the second move.
  hq = (nq-1)/2
  wdr = parms['solid_raster_width']   # Distance between adjacent solid fill rasters.
  wdc = parms['contour_trace_width']  # Distance between adjacent contour traces.
  pstep = 1.5*wdc # {X,Y} increment between starting points.
  L = 6*wdr # Length of each move.
  R = ceil(L + (nq/2)*pstep) # max extent of second move 
  p1 = (1+R, 1+R)         # End of first move.
  p0 = rn.sub(p1, (L,0))  # Start of first move.
  szx = 2 + 2*R # Single plot width. 
  szy = szx     # Single plot height.

  def test_connector_one(c, dp, p0,p1, q0,q1,wd):
    # Tests a connector between the traces {p0-->p1} and {q0-->q1}.
    mvp = move.make(p0, p1, wd, parms)
    mvq = move.make(q0, q1, wd, parms)

    # Try creating a connector that is not a jump:
    cn = move.connector(mvp, mvq, parms)
    wc = move.width(cn)

    # Plot style:
    wref = 0.05*wdr
    waxes =  wref 
    clr_pq = pyx.color.rgb(0.050, 0.850, 0.000)
    clr_cn = pyx.color.rgb(0.000, 0.150, 0.850)

    # Draw the traces:
    move.plot_standard(c, cn,  dp, None, clr_cn, waxes, False, True, False, False)
    move.plot_standard(c, mvp, dp, None, clr_pq, waxes, False, True, False, False)
    move.plot_standard(c, mvq, dp, None, clr_pq, waxes, False, True, False, False)

  for iang in range(nang):
    ang = 2*pi*iang/nang

    c = pyx.canvas.canvas()
    pyx.unit.set(uscale=0.25, wscale=0.25, vscale=0.25)
    hacks.plot_frame(c, pyx.color.rgb.white, 0.30, None, nq*szx, nq*szy, -0.15)

    for iqx in range(nq):
      for iqy in range(nq):

        # Choose the trace widths:
        wd = wdr if (iqx+iqy) % 2 == 0 else wdc;
          
        # Avoid {q0--q1} crossing {p0--p1}:
        ok = True
        if iqy == hq and iqx <= hq: ok = False
        if iqy == hq and abs(ang - pi) < 1.0e-8: ok = False
        if iqy > hq and iqx < hq and ang > pi: ok = False
        if iqy < hq and iqx < hq and ang < pi: ok = False
        if ok:
          # Compute the endpoints of the second move:
          dp = (iqx*szx, iqy*szy)
          q0 = rn.add(p1, rn.scale(pstep, (iqx-hq,iqy-hq)))
          q1 = rn.add(q0, (L*cos(ang),L*sin(ang)))

          test_connector_one(c, dp, p0,p1, q0,q1, wd)

    hacks.write_plot(c, "tests/out/move_TST_cn%02d" % iang)

def test_nozzle_travel_time():
  sys.stderr.write("--- testing {nozzle_travel_time} -------------\n")

  pfile = open("tests/out/move_TST_times.dat", "w")  # Timing plot file.

  def write_time_dist_plot(jmp,dpq):
    nd = 200
    for id in range(nd+1):
      dpm = (dpq*id)/nd
      tpm = move.nozzle_travel_time(dpq, jmp, dpm, parms)
      pfile.write("%8.4f %8.4f\n" % (dpm, tpm))
    pfile.write("\n")

  for jmp in False,True:
    write_time_dist_plot(jmp, 5.0)
    write_time_dist_plot(jmp, 0.3)

  pfile.close()

def test_extime():
  sys.stderr.write("--- testing {extime} -------------\n")

  p2 = (3,1)
  p3 = (2,3)
  p4 = (4,4)

  wd1 = parms['solid_raster_width']
  wd2 = 0.5*wd1

  mv23 = move.make(p2,p3,   0, parms); 
  mv34 = move.make(p3,p4, wd2, parms); 

  d23 = rn.dist(p2,p3)
  t23a = move.nozzle_travel_time(d23, move.is_jump(mv23), None, parms)
  t23b = move.extime(mv23)
  assert t23a == t23b

  u34 = rn.sub(p4,p3)
  v34 = (-u34[1], +u34[0]) # A vector perpendicular to the segment {p3--p4}.
  d34 = rn.dist(p3,p4)
  r34a = 0.75
  p34a = rn.mix(r34a, p3, 1-r34a, p4) # A point on the segment {p3--p4}, 3/4 of the way.
  q34a = rn.mix(0.01, v34, 1.0, p34a) # Displace p34a off the segment a bit.
  t34a = move.nozzle_travel_time(d34, move.is_jump(mv34), r34a*d34, parms)
  t34b = move.nozzle_travel_time(d34, move.is_jump(mv34), (1-r34a)*d34, parms)
  assert abs(t34a + t34b - move.extime(mv34)) < 1.0e-8

def test_cover_time():
  sys.stderr.write("--- testing {cover_time} -----------------\n")

  wdfill = parms['solid_raster_width']

  # Make two moves:

  p11 = (1,1)
  p12 = (6,1)
  mv1 = move.make(p11, p12, wdfill, parms)
  
  p21 = (3, 1-wdfill)
  p22 = (7, 1-wdfill)
  mv2 = move.make(p21, p22, wdfill, parms)

  # Pick a contact point {ctm} between them:
  v0 = (4, 1-wdfill/2) # Start of contact.
  v1 = (6, 1-wdfill/2) # End of contact.
  ctm = rn.mix(0.5, v0, 0.5, v1) # Midpoint of contact.

  szx = 8
  szy = 2
  c = pyx.canvas.canvas()
  pyx.unit.set(uscale=2.0, wscale=2.0, vscale=2.0)
  hacks.plot_frame(c, pyx.color.rgb.white, 0.02, (0,0), szx, szy, -0.01)
  ctrace1 = None
  ctrace2 = pyx.color.rgb.red
  wref = 0.05*wdfill
  waxes = wref

  move.plot_standard(c, mv1, None, None, ctrace1, waxes, axis=True, dots=True, arrow=True, matter=False)
  move.plot_standard(c, mv2, None, None, ctrace2, waxes, axis=True, dots=True, arrow=True, matter=False)

  wcont =   0.15*wdfill;  # Contact line.
  ccont = pyx.color.rgb.blue

  sty = [pyx.style.linewidth(wcont), pyx.style.linecap.round, ccont]
  eps = 1.0e-6 * wdfill
  c.stroke(pyx.path.line(ctm[0]-eps, ctm[1]-eps, ctm[0]+eps, ctm[1]+eps), sty)

  hacks.write_plot(c, "tests/out/move_TST_cover_time")

  # Check contact cover time on moves:
  
  for omv in mv1, mv2, move.rev(mv1), move.rev(mv2):
    p = move.pini(omv)
    q = move.pfin(omv)
    dpq = rn.dist(p, q)
    rmq = abs(ctm[0] - p[0])/abs(q[0] - p[0]) # Rel pos of contact on {mv1}
    tca = move.nozzle_travel_time(dpq, False, rmq*dpq, parms)
    tcb = move.cover_time(omv, ctm, parms)
    tcc = move.cover_time(move.rev(omv), ctm, parms)
    assert abs(tca - tcb) < 1.0e-8
    assert abs((tca + tcc) - move.extime(omv)) < 1.0e-8

  return
  # ----------------------------------------------------------------------

# Run the tests:

test_extime()
test_plot()
test_cover_time()
test_nozzle_travel_time()
test_make_width_is_jump()
test_move_orient_unpack_rev()
test_displace()
test_connector()