#! /usr/bin/python3
# ===================================================================
# Test combining multiple transformation matrices
# into a single matrix
#
# Notes:
#
#  45 deg  = 0.7854 rad
#  90 deg  = 1.5708 rad
# 135 deg  = 2.3562 rad
# 180 deg  = 3.1416 rad
#
# sin(45)  = 0.7071
# cos(45)  = 0.7071 
# sin(90)  = 1.0
# cos(90)  = 0.0
# sin(135) = 0.7071
# cos(135) = 0.7071
# sin(180) = 0.0
# cos(180) = 1.0
#
# ===================================================================

import transformation_matrix as tr
import numpy as np
import sys

x   = 100.0                    # x coordinate
y   = 100.0                    # y coordinate
z   = 100.0                    # z coordinate
tx  = -50.0                    # x translation
ty  =   0.0                    # y translation
tz  =   0.0                    # z translation
deg =  90.0                    # Z rotation angle

# ---- return array with round array values
def xyz(ary):
    return np.round_(ary,4)

# ---- point coordinate array
C = np.array([x,y,z,1])

# ---- Z rotation matrix 
R = tr.get_z_rotation_matrix_3d(deg)

# ---- translate matrix
T = tr.get_translation_matrix_3d(tx,ty,tz)

# -------------------------------------------------------------------
# ---- round array values
# -------------------------------------------------------------------

def xyz(ary):
    return np.round_(ary,4)

# -------------------------------------------------------------------
# ---- main
# -------------------------------------------------------------------

print()
print('==== Test Data ============================')
print('---- rotation angle (deg) -----------------')
print(deg)
print('---- test coords --------------------------')
print(xyz(C))
print(type(C))
print('---- test T (translation) -----------------')
print(xyz(T))
print(type(T))
print('---- test R (rotation) --------------------')
print(xyz(R))
print(type(R))

print()
print('==== Test 1 ===============================')

C1  = C@T
C2  = T@C
C3  = C@R
C4  = R@C

print('---- C@T ----------------------------------')
print(xyz(C1))
print(type(C1))
print('---- T@C ----------------------------------')
print(xyz(C2))
print(type(C2))
print('---- C@R ----------------------------------')
print(xyz(C3))
print(type(C3))
print('---- R@C ----------------------------------')
print(xyz(C4))
print(type(C4))

print()
print('==== Test 2 ===============================')

TM1 = R@T
C5  = C @ TM1
C6  = TM1 @ C

print('----  R@T ---------------------------------')
print(xyz(TM1))
print('---- C @ (R@T) ----------------------------')
print(xyz(C5))
print('---- (R@T) @ C ----------------------------')
print(xyz(C6))

print()
print('==== Test 3 ===============================')

TM2 = T@R
C7  = C @ TM2
C8  = TM2 @ C

print('----  T@R ---------------------------------')
print(xyz(TM2))
print('---- C @ (T@R) ----------------------------')
print(xyz(C7))
print('---- (T@R) @ C ----------------------------')
print(xyz(C8))