#! /usr/bin/python3 -t
# _*_ coding: iso-8859-1 _*_
# Last edited on 2024-07-27 00:32:59 by stolfi

PROG_NAME = "libgeo_test"
PROG_DESC = "Tests the Python {libgeo} package"
PROG_VERS = "1.0"

PROG_COPYRIGHT = "Copyright © 2009 State University of Campinas"

PROG_INFO = "!!! PROG_INFO to be written"

import sys;
import os;
import copy;
import perspective;
import hrn;
import rmxn;
import rn;
import math; from math import sqrt, sin, cos, floor, ceil, inf, nan, pi

def data_error(msg) :
  "Prints the error message {msg} about the current input line, and aborts."
  sys.stderr.write("%s:%d: %s\n" % (in_file_name, in_line_number, msg));
  sys.exit(1)
 
def arg_error(msg):
  "Prints the error message {msg} about the command line arguments, and aborts."
  sys.stderr.write("%s\n" % msg);
  sys.stderr.write("usage: %s\n" % PROG_HELP);
  sys.exit(1)

sys.stderr.write("------------------------------------------------------------\n");
sys.stderr.write("Testing {rn}:\n");

z = rn.add([1,2,3], [4,5,6]);
sys.stderr.write("rn.add([1,2,3], [4,5,6]) = %s\n" % str(z));

z = rn.sub([1,2,3], [4,5,6]);
sys.stderr.write("rn.sub([1,2,3], [4,5,6]) = %s\n" % str(z));

z = rn.scale(100, [4,5,6]);
sys.stderr.write("rn.scale(100, [4,5,6]) = %s\n" % str(z));

s = rn.norm_sqr([1,2,3]);
sys.stderr.write("rn.norm_sqr([1,2,3]) = %s\n" % str(s));

s = rn.norm([1,2,3]);
sys.stderr.write("rn.norm([1,2,3]) = %s\n" % str(s));

s = rn.dist([1,2,3,4], [4,5,6,7]);
sys.stderr.write("rn.dist([1,2,3,4], [4,5,6,7]) = %s\n" % str(s));

s = rn.dist_sqr([1,2,3,4], [4,5,6,7]);
sys.stderr.write("rn.dist_dqr([1,2,3,4], [4,5,6,7]) = %s\n" % str(s));

d,ds = rn.dir([1,2,3]);
sys.stderr.write("rn.dir([1,2,3]) = %s, %s\n" % (str(d),str(ds)));

s = rn.dot([1,2,3],[4,5,6]);
sys.stderr.write("rn.dot([1,2,3],[4,5,6]) = %s\n" % str(s));

w = rn.cross2d([1,2],[4,5]);
sys.stderr.write("rn.cross2d([1,2],[4,5]) = %s\n" % str(w));

w = rn.cross3d([1,2,3],[4,5,6]);
sys.stderr.write("rn.cross3d([1,2,3],[4,5,6]) = %s\n" % str(w));

sys.stderr.write("------------------------------------------------------------\n");
sys.stderr.write("Testing {rmxn}:\n");

M = rmxn.zero_matrix(2,3);
sys.stderr.write("rmxn.zero_matrix(2,3) = %s\n" % str(M));

M = rmxn.ident_matrix(3,4);
sys.stderr.write("rmxn.ident_matrix(3,4) = %s\n" % str(M));

M = rmxn.diag_matrix([2,3,4]);
sys.stderr.write("rmxn.diag_matrix([2,3,4]) = %s\n" % str(M));

A = [[1, 2, 3], [4, 5, 6]];
sys.stderr.write("A = %s\n" % str(A));

B = [[1, 2], [3, 4], [5, 6]];
sys.stderr.write("B = %s\n" % str(B));

M = rmxn.mul(A,B);
sys.stderr.write("rmxn.mul(A,B) = %s\n" % str(M));

v = rmxn.map_row([10,100],A);
sys.stderr.write("rmxn.map_row([10,100],A) = %s\n" % str(v));

v = rmxn.map_col(B,[10,100]);
sys.stderr.write("rmxn.map_col(B,[10,100]) = %s\n" % str(v));

sys.stderr.write("------------------------------------------------------------\n");
sys.stderr.write("Testing {hrn}:\n");

q = hrn.pt_scale([2, 3, 4, 5], [10,100,1000]);
sys.stderr.write("hrn.pt_scale([2, 3, 4, 5], [10,100,1000]) = %s\n" % str(q));

M = hrn.trans_matrix([2, 3, 4, 5]);
sys.stderr.write("hrn.trans_matrix([2, 3, 4, 5]) = %s\n" % str(M));

M = hrn.scale_matrix([3, 4, 5]);
sys.stderr.write("hrn.scale_matrix([3, 4, 5]) = %s\n" % str(M));

sys.stderr.write("------------------------------------------------------------\n");
sys.stderr.write("Testing {perspective}:\n");

att =    [1,2,3];
obs = [1, 4,6,5];
upd =    [0,0,1];
mag =    [+240,-240,+240];
ctr =    [+300,+400,0000];

M = perspective.camera_matrix(att,obs,upd,mag,ctr);
sys.stderr.write("rmxn.map_col(B,[10,100]) = %s\n" % str(v));

pt = [
  [ 1, 00, 00, 00 ],
  [ 1, -1, -1, 00 ],
  [ 1, -1, +1, 00 ],
  [ 1, +1, +1, 00 ],
  [ 1, +1, -1, 00 ],
  [ 0, +1, 00, 00 ],
  [ 0, 00, +1, 00 ],
  [ 0, 00, 00, +1 ]];
  
if (len(att) == 3) : att = copy.copy(att); att[0:0] = [ 1 ]
assert len(att) == 4, "{att} must be a point of R^3 or H^3";
for i in range(len(pt)) :
  pi = hrn.pt_pt_add(pt[i], att);
  qi = rmxn.map_row(pi,M);
  sys.stderr.write("%20s --> %s\n" % (str(pi), str(qi)));

sys.stderr.write("------------------------------------------------------------\n");
sys.stderr.write("Testing {seg_seg_ovelap}:\n");

p0 = (3,1,2)
p1 = (7,3,1)
p01 = rn.sub(p1, p0)
u, um = rn.dir(p01)  # Unit vector of {p0-->p1}.
v, vm = rn.dir((-u[2],0,+u[0])) # A unit vector orthogonal to {u}.
w, wm = rn.dir((-u[1],+u[0],0)) # Another unit vector orthogonal to {u,v}.
tol = 1.0e-6

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), 
  ):
  sys.stderr.write("testing with r0 = %.7f r1 = %.7f\n" % (r0, r1))
  assert r0 <= r1
  # Tests with vectors parallel and antiparallel:
  q0 = rn.mix4(1.0, p0, 0.1 + 0.001*tol, v, 0.2 + 0.001*tol, w, r0, p01)
  q1 = rn.mix4(1.0, p0, 0.1 - 0.003*tol, v, 0.2 - 0.003*tol, w, r1, p01)
  for k in range(2):
    a0,a1, b0,b1 = rn.seg_seg_overlap(p0,p1, q0,q1, tol)
    if r1 <= 0 or r0 >= 1 or r1 - r0 < tol:
      # Should have no overlap:
      assert a0 == None and a1 == None and b0 == None and b1 == None
    else:
      # Should have non-trivial overlap:
      assert a0 != None and a1 != None and b0 != None and b1 != None
      assert abs(rn.dist(a0,a1) - rn.dist(b0,b1)) <= tol
      assert abs(rn.dist(a0,b0) - rn.dist(a1,b1)) <= tol
      assert abs(rn.dist(a0,b1) - rn.dist(b0,a1)) <= tol
      # Should do more checks...
    # Flip segment {q0--q1}:
    q0,q1 = q1,q0
  # Tests with vectors not parallel:
  q0 = rn.mix4(1.0, p0, 0.5, v, 0.2, w, r0, p01)
  q1 = rn.mix4(1.0, p0, 0.1, v, 0.5, w, r1, p01)
  for k in range(2):
    a0,a1, b0,b1 = rn.seg_seg_overlap(p0,p1, q0,q1, tol)
    assert a0 == None and a1 == None and b0 == None and b1 == None
    # Flip segment {q0--q1}:
    q0,q1 = q1,q0

# ----------------------------------------------------------------------