#! /usr/bin/python3
# Test program for module {move}
# Last edited on 2021-05-30 20:57:50 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)

  dp = (0, 0)

  c, szx,szy = hacks.make_canvas(B, dp, True, True, 4, 8)

  # 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:
  rwd_matter = 1.13; 
  rwd =  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 = 6*wd_ref
  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("  {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

    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 = rwd_matter*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 = rwd*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("  {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 ]

    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, rwd=rwd, 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, rwd=rwd, 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):
          dpk = rn.add(dp, ((2*dr+0)*szx, iy*szy))
          test_plot_do_layer(dpk, dr, axes, dots, arrows)
          dpk = rn.add(dp, ((2*dr+1)*szx, iy*szy))
          test_plot_do_standard(dpk, 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.

  B = ((1, 1), (1 + 2*R, 1 + 2*R))

  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
    rwd = 0.80
    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,], rwd, wd_axes, False, True, False, False)
    move.plot_standard(c, [mvp,], dp, None, [clr_pq,], rwd, wd_axes, False, True, False, False)
    move.plot_standard(c, [mvq,], dp, None, [clr_pq,], rwd, wd_axes, False, True, False, False)

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

    dp = (0, 0)
    c, szx,szy = hacks.make_canvas(B, dp, True, True, nq, nq)

    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:
          dpk = rn.add(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, dpk, 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,]
  rwd = 0.80
  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,], rwd, 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
          sys.stderr.write("c0  = ( %20.16f %20.16f ) c1  = ( %20.16f %20.16f )\n" % (c0[0],c0[1],c1[0],c1[1]))
          # Should do more checks...
        # Test indifference to orientation and order:
        for omva, omvb in (mv1,mv0), (mv0, move.rev(mv1)), (mv1, move.rev(mv0)): 
          c0r,c1r = move.shared_border(omva, omvb)
          if c0r != None and c1r != None:
            sys.stderr.write("c0r = ( %20.16f %20.16f ) c1r = ( %20.16f %20.16f )\n" % (c0r[0],c0r[1],c1r[0],c1r[1]))
            assert \
              ( hacks.same_point(c0r, c0, 1.0e-8) and hacks.same_point(c1r, c1, 1.0e-8) ) or \
              ( hacks.same_point(c0r, c1, 1.0e-8) and hacks.same_point(c1r, c0, 1.0e-8) )
          else:
            assert  c0r == None and c1r == None
        # 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,tag_names} ---\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"
  
  sys.stderr.write("  ... applying {move.tag_names} ...\n")
  move.tag_names([TRS[2], JMS[2]], "Tag.")
  
  xms = gname("TRS[2]", TRS[2])
  assert xms == "Tag.Tb0"
  
  xms = gname("~TRS[2]", move.rev(TRS[2]))
  assert xms == "~Tag.Tb0"
  
  xms = gname("~JMS[2]", move.rev(JMS[2]))
  assert xms == "~Tag.Jd0"

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

def test_show():
  sys.stderr.write("--- testing {show,show_list} ---\n");
  TRS, JMS = move_example.misc_C(mp_fill, mp_jump)
  sys.stderr.write("  ... {show} ...\n")
  wna = 5
  for omv in TRS[0], move.rev(TRS[0]), JMS[0], move.rev(JMS[0]):
    sys.stderr.write("[")
    move.show(sys.stderr, omv, 4, wna)
    sys.stderr.write("]\n")
    wna = wna + 5
  sys.stderr.write("]\n")
  
  sys.stderr.write("  ... {show_list} ...\n")
  move.show_list(sys.stderr, TRS, 2)
  sys.stderr.write("\n")
  move.show_list(sys.stderr, JMS, 2)
  return
  # ----------------------------------------------------------------------

# Run the tests:

test_name()
test_show()
test_make_width_is_jump()
test_move_orient_unpack_rev()
test_displace()
test_plot()
test_extime()
test_cover_time()
test_connector()
test_shared_border()