import math as math
import numpy as np


def lookAt(eyeX,eyeY,eyeZ,cX,cY,cZ,upX,upY,upZ):
    F = np.matrix([cX-eyeX,cY-eyeY,cZ-eyeZ])
    UP = np.matrix([upX,upY,upZ])
    f = F / math.sqrt(np.sum(np.square(F)))
    UP = UP / math.sqrt(np.sum(np.square(UP)))
    s = np.cross(f,UP)
    u = np.cross((s/math.sqrt(np.sum(np.square(s)))),f)
    mat = np.matrix([
        [s[0,0],s[0,1],s[0,2],0],
        [u[0,0],u[0,1],u[0,2],0],
        [-f[0,0],-f[0,1],-f[0,2],0],
        [0,0,0,1]
    ])

    return np.transpose(mat)

def orthogonalMatrix(r,l,t,b,f,n):
    mat = np.matrix([
        [2/(r-l),0,0,-(r+l)/(r-l)],
        [0,2/(t-b),0,-(t+b)/(t-b)],
        [0,0,-2/(f-n),-(f+n)/(f-n)],
        [0,0,0,1]
    ])

    return np.transpose(mat)


def perspectiveMatrix(fovy,aspect,znear,zfar):
    fovy_rads = fovy*math.pi/180
    f = math.cos(fovy_rads/2.0)/math.sin(fovy_rads/2.0)
    a = (zfar+znear)/(znear-zfar)
    b = (2 * zfar * znear) / (znear - zfar)

    mat = np.matrix([[f/aspect,0,0,0],
                     [0,f,0,0],
                     [0,0,a,b],
                     [0,0,-1,0]],np.float32)

    return np.transpose(mat)

def translate(x, y, z):
    mat = np.matrix([[1, 0, 0, x],
                     [0, 1, 0, y],
                     [0, 0, 1, z],
                     [0, 0, 0, 1]], np.float32)

    return np.transpose(mat)

def rotate(x,y,z,d4 = False):
    if not d4:
        mat = np.matrix([[1, 0, 0], [0, math.cos(x), -math.sin(x)], [0, math.sin(x), math.cos(x)]], np.float32)
        mat = mat * np.matrix([[math.cos(y), 0, -math.sin(y)], [0, 1, 0], [math.sin(y), 0, math.cos(y)]], np.float32)
        mat = mat * np.matrix([[math.cos(z), -math.sin(z), 0], [math.sin(z), math.cos(z), 0], [0, 0, 1]], np.float32)
    else:
        mat = np.matrix([[1, 0, 0, 0], [0, math.cos(x), -math.sin(x), 0], [0, math.sin(x), math.cos(x), 0], [0, 0, 0, 1]], np.float32)
        mat = mat * np.matrix([[math.cos(y), 0, -math.sin(y), 0], [0, 1, 0, 0], [math.sin(y), 0, math.cos(y), 0], [0, 0, 0, 1]], np.float32)
        mat = mat * np.matrix([[math.cos(z), -math.sin(z), 0, 0], [math.sin(z), math.cos(z), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
    return np.transpose(mat)