#! /usr/bin/python3
# Test program for module {move}
# Last edited on 2021-03-21 09:49:49 by jstolfi

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

parms = job_parms.slow()

mp_fill = move_parms.make_for_fillings(parms)
mp_cont = move_parms.make_for_contours(parms)
mp_jump = move_parms.make_for_jumps(parms)

wd_fill = move_parms.width(mp_fill)
wd_cont = move_parms.width(mp_cont)

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)

  mv12 = move.make(p1,p2, mp_fill); 
  assert not move.is_jump(mv12); 
  assert move.width(mv12) == wd_fill

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

  mv34 = move.make(p3,p4, mp_cont); 
  assert not move.is_jump(mv34); 
  assert move.width(mv34) == wd_cont

  mv55 = move.make(p5,p5, mp_fill); 
  assert not move.is_jump(mv55); 
  assert move.width(mv55) == wd_fill

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

  p1 = (1,1)
  p2 = (3,1)

  mv12 = move.make(p1,p2, mp_fill); 

  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
      assert move.endpoints(omvx) == (p1, p2)
    else:
      assert move.pini(omvx) == p2
      assert move.pfin(omvx) == p1
      assert move.endpoints(omvx) == (p2, p1)
    assert move.length(omvx) == rn.dist(p1,p2)
  
  for dr in range(4):
    omva = move.spin(mv12, dr)
    mva, dra = move.unpack(omva)
    assert dra == dr % 2
    check_pini_pfin(omva)
    
    omvb = move.spin(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)

  mv12 = move.make(p1,p2, mp_fill); 

  angr = pi/6
  vr = (2,3)
  mv12r = move.displace(mv12, angr, vr, mp_fill)
  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
  assert rn,dist(move.pfin(mv12r), p2r) < 1.0e-8

def test_plot():

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

  OMVS = move_example.misc_A(mp_fill, mp_cont, mp_jump)
  nmv = len(OMVS)
  CLRS = hacks.trace_colors(nmv)
  nclr = len(CLRS)
 
  # Testing all combinations of options:
  
  # Compute the plot's bounding box:
  B = move.bbox(OMVS)
  pbox = hacks.round_box(B, 1)
  szx, szy = rn.box_size(pbox)

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

  # 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):
  wd_ref = 0.15*min(wd_fill,wd_cont) # Reference line width.
  wd_axes = wd_ref
  wd_dots = 2.5*wd_ref
  wd_jump = wd_ref
  sz_arrows = 8*wd_ref

  wd_frame = 0.03
  wd_grid = 0.02
  wd_bbox = 0.25*wd_ref

  def test_plot_do_bbox(omv, dp, wd_bbox):
    # Plots the bounding box of move {omv} displaced by {dp}.
    B = move.bbox([omv,])
    hacks.plot_frame(c, pyx.color.rgb.blue, wd_bbox, dp, B, -wd_bbox/2)
    return
    # ----------------------------------------------------------------------

  def test_plot_do_layer(dp, 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_do_layer: dr = %d axes = %d dots = %d arrows = %d\n" % (dr,axes,dots,arrows))

    def pick_colors(k):
      # 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

    OMVSdr = [ move.spin(omv, dr) for omv in OMVS ]

    wd_axes_f = wd_axes if axes else 0 
    wd_dots_f = wd_dots if dots else 0
    sz_arrows_f = sz_arrows if arrows else 0

    hacks.plot_frame(c, pyx.color.rgb.black, wd_frame, dp, pbox, -0.75*wd_frame)
    hacks.plot_grid(c, None, wd_grid, dp, pbox, +5*wd_grid, 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 k in range(nmv):
        omv = OMVSdr[k]
        # sys.stderr.write("\n")
        jmp = move.is_jump(omv)
        wd = move.width(omv)
        if layer == 0 and not jmp:
          # plots the estimate of actual material:
          wd_matter = rmatter*wd
          move.plot_layer \
            (c, omv, dp, clr=cmatter, wd=wd_matter, dashed=False, wd_dots=0, sz_arrow=None)
        elif layer == 1 and not jmp:
          # plots the nominal trace material:
          wd_trace = rtraces*wd
          ctrace, caxis = pick_colors(k)
          move.plot_layer \
            (c, omv, dp, clr=ctrace, wd=wd_trace, dashed=False, wd_dots=0, sz_arrow=None)
        elif layer == 2 and not jmp:
          # Plots the trace axis:
          ctrace, caxis = pick_colors(k)
          move.plot_layer \
            (c, omv, dp, clr=caxis, wd=wd_axes_f, dashed=False, wd_dots=wd_dots_f, sz_arrow=sz_arrows_f)
        elif layer == 3 and jmp:
          # Plots the jump:
          move.plot_layer \
            (c, omv, dp, clr=cjumps, wd=wd_jump, dashed=True, wd_dots=wd_dots_f, sz_arrow=sz_arrows_f)
        if layer == 0:
          test_plot_do_bbox(omv, dp, wd_bbox)
    return
    # ----------------------------------------------------------------------

  def test_plot_do_standard(dp, 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_do_standard: dr = %d axes = %d dots = %d arrows = %d\n" % (dr,axes,dots,arrows))

    OMVSdr = [ move.spin(omv, dr) for omv in OMVS ]

    hacks.plot_frame(c, pyx.color.rgb.black, wd_frame, dp, pbox, -0.75*wd_frame)
    hacks.plot_grid(c, None, wd_grid, dp, pbox, +5*wd_grid, 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:
      move.plot_standard \
        ( c, OMVSdr, dp, None, CLRS, wd_axes=wd_axes, 
          axes=axes, dots=dots, arrows=arrows, matter=True
        ) 
      for omv in OMVSdr:
        test_plot_do_bbox(omv, dp, wd_bbox)
    else:
      # Plot layer by layer:
      for layer in range(4):
        # sys.stderr.write("--- layer %d ---\n" % layer)
        move.plot_standard \
          ( c, OMVSdr, dp, layer, CLRS, wd_axes=wd_axes, 
            axes=axes, dots=dots, arrows=arrows, matter=True
          )
    return
    # ----------------------------------------------------------------------
    
  iy = 0  # Plot row index.
  for axes in False,True:
    for dots in False,True:
      for arrows in False,True:
        for dr in range(2):
          dp = ((2*dr+0)*szx, iy*szy)
          test_plot_do_layer(dp, dr, axes, dots, arrows)
          dp = ((2*dr+1)*szx, iy*szy)
          test_plot_do_standard(dp, dr, axes, dots, arrows)
        iy += 1

  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
  pstep = 1.5*wd_cont # {X,Y} increment between starting points.
  L = 6*wd_fill # 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, mp):
    # Tests a connector between the traces {p0-->p1} and {q0-->q1}.
    mvp = move.make(p0, p1, mp)
    mvq = move.make(q0, q1, mp)

    # Create a connector.  Let it choose between move or jump:
    use_jump = False
    use_link = False
    cn = move.connector(mvp, mvq, use_jump, use_link, mp_jump)
    mpc = move.connector_parameters(mvp, mvq, use_jump, use_link, mp_jump)
    tex = move.connector_extime(mvp, mvq, use_jump, use_link, mp_jump)
    assert mpc == move.parameters(cn)
    assert tex == move.extime(cn) \
      + move_parms.transition_penalty(move.parameters(mvp), mpc) \
      + move_parms.transition_penalty(mpc, move.parameters(mvq))
    
    # Since both moves are traces with same parameters, the decision must have been geometric:
    p0, p1 = move.endpoints(mvp)
    q0, q1 = move.endpoints(mvq)
    vpq = rn.sub(q0, p1)
    v_prev = rn.sub(p1, p0)
    v_next = rn.sub(q1, q0)
    dmax = 3*move_parms.width(mp)
    jmp = move.connector_must_be_jump(vpq, v_prev, v_next, dmax, mp_jump, mp)
    assert jmp == (mpc == mp_jump)

    # Plot style:
    wd_ref = 0.05*wd_fill
    wd_axes =  wd_ref 
    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,], wd_axes, False, True, False, False)
    move.plot_standard(c, [mvp,], dp, None, [clr_pq,], wd_axes, False, True, False, False)
    move.plot_standard(c, [mvq,], dp, None, [clr_pq,], wd_axes, False, True, False, False)

  Pbox = ((0,0), (nq*szx, nq*szy))
  
  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)
    wd_grid = 0.03
    hacks.plot_frame(c, pyx.color.rgb.white, wd_grid, None, Pbox, -0.15)

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

        # Choose the parameter record:
        mp = mp_fill if (iqx+iqy) % 2 == 0 else mp_cont;
          
        # 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, mp)

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

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

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

  mv23 = move.make(p2,p3, mp_jump); 
  mv34 = move.make(p3,p4, mp_cont); 

  d23 = rn.dist(p2,p3)
  t23a = move_parms.nozzle_travel_time(d23, None, mp_jump)
  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_parms.nozzle_travel_time(d34, r34a*d34, mp_cont)
  t34b = move_parms.nozzle_travel_time(d34, (1-r34a)*d34, mp_cont)
  assert abs(t34a + t34b - move.extime(mv34)) < 1.0e-8
  
  ud23 = move.ud_penalty(mv23); assert ud23 == parms['extrusion_on_off_time']
  ud34 = move.ud_penalty(mv34); assert ud34 == 0

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

  # Make two moves:

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

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

  OMVS = [mv1, mv2,]
  wd_axes = 0.05*wd_fill
  ctraces = pyx.color.rgb(1.000, 0.200, 1.000)
  move.plot_to_files("tests/out/move_TST_cover_time", OMVS, [ctraces,], wd_axes)

  # Check contact cover time on moves:
  
  for omv in mv1, mv2, move.rev(mv1), move.rev(mv2):
    mp = move.parameters(omv)
    p, q = move.endpoints(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_parms.nozzle_travel_time(dpq, rmq*dpq, mp)
    tcb = move.cover_time(omv, ctm)
    tcc = move.cover_time(move.rev(omv), ctm)
    assert abs(tca - tcb) < 1.0e-8
    assert abs((tca + tcc) - move.extime(omv)) < 1.0e-8

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

def test_shared_border():
  sys.stderr.write("--- testing {shared_border} ---\n");

  p0 = (3,1)
  p1 = (7,3)
  u, um = rn.dir(rn.sub(p1, p0))  # Unit vector of {mv0}.
  v = (-u[1],+u[0])                 # Unit vector orthogonal to {u}.

  mp0 = mp_fill; wd0 = move_parms.width(mp0)
  mp1 = mp_cont; wd1 = move_parms.width(mp1)
  tol = 0.10*max(wd0,wd1)

  mv0 = move.make(p0, p1, mp0)

  for r0,r1 in (
      (-0.04, -0.02), 
      (-0.02, -0.02), 
      (-0.02, +0.02), 
      (-0.02, +0.50), 
      (-0.02, +0.98), 
      (-0.02, +1.02), 
      (+0.02, +0.02), 
      (+0.02, +0.04), 
      (+0.02, +0.50), 
      (+0.02, +0.98), 
      (+0.02, +1.02), 
      (+0.50, +0.50), 
      (+0.50, +0.98), 
      (+0.50, +1.02), 
      (+0.96, +0.98), 
      (+0.98, +0.98), 
      (+0.98, +1.02), 
      (+1.02, +1.02), 
      (+1.02, +1.04), 
    ):
    for rsep in (0.99, 1.01, 2.0):
      sys.stderr.write("testing with r0 = %.7f r1 = %.7f rsep = %.7f\n" % (r0, r1, rsep))
      sep = rsep*(wd0+wd1)/2
      assert r0 <= r1
      # Tests with moves parallel and antiparallel:
      q0 = rn.mix3(1-r0, p0, r0, p1, sep + 0.001*(wd0+wd1), v)
      q1 = rn.mix3(1-r1, p0, r1, p1, sep - 0.003*(wd0+wd1), v)
      for k in range(2):
        mv1 = move.make(q0, q1, mp1)
        c0, c1 = move.shared_border(mv0, mv1)
        if r1 <= 0 or r0 >= 1 or r1 - r0 < 0.001 or rsep > 1.2:
          # Should have no shared border:
          assert c0 == None and c1 == None
        else:
          # Should have non-trivial shared border:
          assert c0 != None and c1 != None
          # Should do more checks...
        # Flip segment {q0--q1}:
        q0,q1 = q1,q0
      # Tests with vectors not parallel:
      q0 = rn.mix3(1-r0, p0, r0, p1, 0.5, v)
      q1 = rn.mix3(1-r1, p0, r1, p1, 0.1, v)
      for k in range(2):
        mv1 = move.make(q0, q1, mp1)
        c0, c1 = move.shared_border(mv0, mv1)
        assert c0 == None and c1 == None
        # Flip segment {q0--q1}:
        q0,q1 = q1,q0

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

def test_name():
  sys.stderr.write("--- testing {set_name,get_name} ---\n");
  
  def gname(var, omv):
    # Returns {get_name(omv)}, printing to {stderr}.
    xms = move.get_name(omv)
    sys.stderr.write("  get_name(%s) = %s\n" % (var,str(xms)))
    return xms
    # ............................................................

  tra = move.make((1,1), (3,2), mp_fill)
  xms = gname("tra", tra)
  assert xms == "T?"
  xms = gname("~tra", move.rev(tra))
  assert xms == "~T?"

  move.set_name(tra, "BB")
  xms = gname("tra", tra)
  assert xms == "BB"
  xms = gname("~tra", move.rev(tra))
  assert xms == "~BB"

  jma = move.make((1,1), (3,2), mp_jump)
  xms = gname("jma", jma)
  assert xms == "J?"
  xms = gname("~jma", move.rev(jma))
  assert xms == "~J?"
  
  move.set_name(move.rev(jma), "XY")
  xms = gname("jma", jma)
  assert xms == "XY"
  xms = gname("~jma", move.rev(jma))
  assert xms == "~XY"

  TRS, JMS = move_example.misc_C(mp_fill, mp_jump)
  
  xms = gname("TRS[2]", TRS[2])
  assert xms == "Tb0"
  
  xms = gname("~JMS[2]", move.rev(JMS[2]))
  assert xms == "~Jd0"
  
  return
  # ----------------------------------------------------------------------

def test_describe():
  sys.stderr.write("--- testing {describe} ---\n");
  TRS, JMS = move_example.misc_C(mp_fill, mp_jump)
  move.describe(sys.stderr, TRS)
  sys.stderr.write("\n")
  move.describe(sys.stderr, JMS)
  return
  # ----------------------------------------------------------------------

# Run the tests:

test_name()
test_describe()
# test_make_width_is_jump()
# test_move_orient_unpack_rev()
# test_displace()
# test_plot()
# test_extime()
# test_cover_time()
# test_connector()
# test_shared_border()