import numpy as np
import math


def load_values():
    i1, i2, i3 = [], [], []
    f = open("i1.txt", "r")
    for x in f:
        i1.append([float(ele) for ele in x.split("\t")])
    f.close()

    f = open("i2.txt", "r")
    for x in f:
        i2.append([float(ele) for ele in x.split("\t")])
    f.close()

    f = open("i3.txt", "r")
    for x in f:
        i3.append([float(ele) for ele in x.split("\t")])
    f.close()


    return i1, i2, i3



def get_input(type = 'static', input = None):
    '''
        takes x,y,z values
    '''

    if type == 'static':
        inputs = [
                [[270.4434, 522.7805, 322.9194],
                [331.8686, 472.3892, 283.1944],
                [300.1669, 508.5782, 219.0040]]
        ]


        return inputs

    else:
        i1, i2, i3 = load_values()
        n = len(i1)
        inputs = []
        for i in range(n):
            inputs.append([i1[i], i2[i], i3[i]])

        return inputs
        

def calc_transposed_values(input):
    '''
        computes new cordinates on new axes (?)
    '''
    i2 = input[1]
    i1 = input[0]
    i3 = input[2]
    i4 = np.subtract(i2,i1)
    i5 = np.subtract(i3,i2)
    i6 = [i4[1]*i5[2] - i5[1]*i4[2], i4[0]*i5[2] - i5[0]*i4[2], i4[0]*i5[1] - i5[0]*i4[1]]
    i7 = [i4[1]*i6[2] - i6[1]*i4[2], i4[0]*i6[2] - i6[0]*i4[2], i4[0]*i6[1] - i6[0]*i4[1]]

    return i4, i5, i6, i7

    
def find_angle(i, j, ele):
    '''
    finds angle between two set of points
    '''

    a = math.sqrt(i[0]**2 + i[1]**2 + i[2]**2)

    cos_theta = float(ele)/a
    sin_theta = math.sqrt(1 - cos_theta**2)

    #print(f"angle between {i} and {j} is costheta: {cos_theta} and sintheta {sin_theta}")

    return cos_theta , sin_theta


def comp_matrices(x_sin, x_cos, y_sin, y_cos, z_sin, z_cos, tx, ty, tz):
    X = [
        [1, 0 , 0 , 0],
        [0, x_cos, x_sin, 0],
        [0, -x_sin, x_cos, 0],
        [0, 0, 0, 1]
    ]

    Y = [
        [y_cos, 0, -y_sin, 0],
        [0, 1, 0, 0],
        [y_sin, 0, y_cos, 0],
        [0, 0, 0, 1]
    ]

    Z = [
        [z_cos, z_sin, 0, 0],
        [-z_sin, z_cos, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ]

    T = [
        [1, 0, 0, tx],
        [0, 1, 0, ty],
        [0, 0, 1, tz],
        [0, 0, 0, 1]
    ]

    return X, Y, Z, T

def mat_mul(A, B):
    """
     does A*B.T operation
    """
    return np.matmul(A, np.array(B).transpose())

if __name__=='__main__':
	ip = []

	for i in range(3):
		ip.append([])
		ip[-1]=[int(x) for x in input(f"Enter x{i+1}, y{i+1}, z{i+1} value: ").split(",")]
		print(i)	
	ip = [ip]

	inputs = get_input('static', ip)

	for inp in inputs:
	    i4, i5, i6, i7 = calc_transposed_values(inp)
	    x_cos, x_sin = find_angle(i4, [1,0,0], i4[0])
	    y_cos, y_sin = find_angle(i6, [0,1,0], i6[1])
	    z_cos, z_sin = find_angle(i7, [0,0,1], i7[2])
	    tx, ty, tz = inp[1][0], inp[1][1], inp[1][2]
	    X, Y, Z, T = comp_matrices(x_sin, x_cos, y_sin, y_cos, z_sin, z_cos, tx, ty, tz)
	    result = np.matmul(np.matmul(np.matmul(X, Y), Z), T)
	    print(result)