import math

"""
Vector3 Class, used for 3d maths and whatnot
"""
RADTODEG = 180 / 3.1415926
DEGTORAD = 3.1415926 / 180

class V3():
  def __init__(self, x = 0, y = 0, z = 0):
    self.x = x
    self.y = y
    self.z = z    

  def move(self,by):
    if type(by) is V3:
      self.x += by.x
      self.y += by.y
      self.x += by.z
    else:
      self.x += by
      self.y += by
      self.z += by

  def length(self):
    return math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)

  def transform(self, m):
    '''
    Transforms the vector by a 4x4 matrix
    '''
    self.x = m.m00 * self.x + m.m10 * self.y + m.m20 * self.z + m.m30
    self.y = m.m01 * self.x + m.m11 * self.y + m.m21 * self.z + m.m31
    self.z = m.m02 * self.x + m.m12 * self.y + m.m22 * self.z + m.m32

  """
  Static methods
  """
  @staticmethod
  def Normalize(v):
    '''
    Takes a vector and puts it into the 0 - 1 range
    '''
    vlen = v.length()
    if (vlen > 0):
      return V3(v.x / vlen, v.y / vlen, v.z / vlen)
    else:
      return V3()
    
  @staticmethod
  def Cross(a, b):
    return V3(
      a.y * b.z - a.z * b.y,
      a.z * b.x - a.x * b.z,
      a.x * b.y - a.y * b.x
    ) 

  @staticmethod
  def Dot(a, b):
    return a.x * b.x + a.y * b.y + a.z * b.z

  """
  Operators
  """
  def __add__(self, b):  
    if type(b) != float:
      return V3(
        self.x + b.x,
        self.y + b.y,
        self.z + b.z
      )
    else:
      return V3(
        self.x + b,
        self.y + b,
        self.z + b
      )

  def __sub__(self, b):
    if type(b) != float:
      return V3(
        self.x - b.x,
        self.y - b.y,
        self.z - b.z
      )
    else:
      return V3(
        self.x - b,
        self.y - b,
        self.z - b
      )

  def __mul__(self, b):  
    if type(b) != float:
      return V3(
        self.x * b.x,
        self.y * b.y,
        self.z * b.z
      )
    else:
      return V3(
        self.x * b,
        self.y * b,
        self.z * b
      )

  def __div__(self, b):  
    if type(b) != float:
      return V3(
        self.x / b.x,
        self.y / b.y,
        self.z / b.z
      )
    else:
      return V3(
        self.x / b,
        self.y / b,
        self.z / b
      )
  def __neg__(self):
    return V3(
      -self.x,
      -self.y,
      -self.z
    )

  def __iadd__(self, b):  
    self.x += b.x
    self.y += b.y
    self.z += b.z
    return self

  def __isub__(self, b):  
    self.x -= b.x
    self.y -= b.y
    self.z -= b.z
    return self

  def __imul__(self, b):  
    self.x *= b.x
    self.y *= b.y
    self.z *= b.z
    return self

  def __idiv__(self, b):  
    self.x /= b.x
    self.y /= b.y
    self.z /= b.z
    return self

  """
  Metamethods
  """
  def __str__(self):
    return "v.xyz = " + str(self.x) + " " + str(self.y) + " " + str(self.z) + "\n"

"""
Matrix Maths, things are about to get crazy...
"""
class M4():
  def __init__(self, m00=0, m10=0, m20=0, m30=0, 
                     m01=0, m11=0, m21=0, m31=0,
                     m02=0, m12=0, m22=0, m32=0,
                     m03=0, m13=0, m23=0, m33=0):
    self.m00=m00; self.m10=m10; self.m20=m20; self.m30=m30
    self.m01=m01; self.m11=m11; self.m21=m21; self.m31=m31
    self.m02=m02; self.m12=m12; self.m22=m22; self.m32=m32
    self.m03=m03; self.m13=m13; self.m23=m23; self.m33=m33

  def getGLArray(self):
    return [
      self.m00, self.m01, self.m02, self.m03,
      self.m10, self.m11, self.m12, self.m13,
      self.m20, self.m21, self.m22, self.m23,
      self.m30, self.m31, self.m32, self.m33
    ]
  """
  Static Methods
  """
  @staticmethod
  def Transpose(m):
    return M4(
      m.m00, m.m01, m.m02, m.m03,
      m.m10, m.m11, m.m12, m.m13,
      m.m20, m.m21, m.m22, m.m23,
      m.m30, m.m31, m.m32, m.m33
    )

  @staticmethod
  def Identity():
    return M4(
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1
    )

  @staticmethod
  def Translation(v):
    return M4(
    1, 0, 0, v.x,
    0, 1, 0, v.y,
    0, 0, 1, v.z,
    0, 0, 0, 1
    )

  @staticmethod
  def Scale(v):
    return M4(
    v.x, 0, 0, 0,
    0, v.y, 0, 0,
    0, 0, v.z, 0,
    0, 0, 0,   1 
    )

  @staticmethod
  def RotationX(rad):
    s = math.sin(rad); c = math.cos(rad)
    return M4(
    1,  0,  0, 0,
    0,  c, -s, 0,
    0,  s,  c, 0,
    0,  0,  0, 1,
    )

  @staticmethod
  def RotationY(rad):
    s = math.sin(rad); c = math.cos(rad)
    return M4(
    c,  0, s, 0,
    0,  1, 0, 0,
    -s, 0, c, 0,
    0,  0, 0, 1, 
    )

  @staticmethod
  def RotationZ(rad):
    s = math.sin(rad); c = math.cos(rad)
    return M4(
    c, -s, 0, 0,
    s,  c, 0, 0,
    0,  0, 1, 0,
    0,  0, 0, 1
    )

  @staticmethod
  def Ortho(l, r, b, t, n, f):
    tx = -(r+l)/(r-l)
    ty = -(t+b)/(t-b)
    tz = -(f+n)/(f-n)
    return M4(
    2.0/(r-l), 0, 0, tx,
    0, 2.0/(t-b), 0, ty,
    0, 0, 2.0/f(-n), tz,
    0, 0, 0, 1
    )

  @staticmethod
  def Perspective(fov, aspect, near, far):
    fov_r = fov / 180 * 3.1415926
    f = 1.0 / math.tan(fov_r / 2.0)
    ar = aspect
    nd = near; fd = far
    return M4(
    f/ar, 0, 0, 0,
    0, f, 0, 0,
    0, 0, (fd+nd)/(nd-fd), (2 * fd*nd)/(nd-fd),
    0, 0, -1,   
    )

  @staticmethod
  def LookAt(fr, to, up):
    z = -V3.Normalize(to - fr)
    x = -V3.Normalize(V3.Cross(up, z))
    y = V3.Cross(z, x)

    return M4(
      x.x, x.y, x.z, -V3.Dot(fr, x),
      y.x, y.y, y.z, -V3.Dot(fr, y),
      z.x, z.y, z.z, -V3.Dot(fr, z),
      0, 0, 0, 1 
    )

  """
  Metamethods
  """
  def __str__(self):
    builder ="M4 = {\n"
    builder+=" " + str(self.m00) + " " + str(self.m10) + " " + str(self.m20) + " " + str(self.m30) + "\n" 
    builder+=" " + str(self.m01) + " " + str(self.m11) + " " + str(self.m21) + " " + str(self.m31) + "\n"
    builder+=" " + str(self.m02) + " " + str(self.m12) + " " + str(self.m22) + " " + str(self.m32) + "\n"
    builder+=" " + str(self.m03) + " " + str(self.m13) + " " + str(self.m23) + " " + str(self.m33) + "\n"
    builder+="}\n"
    return builder

if __name__=='__main__':
  """
  Preform simple tests here
  """
  
  a = V3(10, 23, 1)
  b = V3(4, 2, 8)

  m = M4.Identity()
  print m
  print(M4.Transpose(m))

  print(m.getGLArray())