import math

from OpenGL.GL import *
from OpenGL.GLU import *

from agk.matrix44 import *
from agk.vector3 import *
from agk.quaternion import *


class RootedRigidColorMesh( object ):

    def __init__(self, matrix, display_list_id, colour):
        self.id = id(self)
        self.colour = colour
        self.matrix = matrix.copy()
        self.display_list_id = display_list_id

        self.default_orient()


    def default_orient(self):
        to_origin = Vector3( self.matrix.translate ) * -1.0

        if not to_origin:
            return          # if to_origin is a zero vector the object does not need to be rotated

        to_origin.normalise()
        myup = Vector3( self.matrix.y_axis )
        dot = myup.dot( to_origin )
        if dot < 0.998:
            orient_quat = Quaternion.from_two_vec3( myup, to_origin, dotprod=dot, unit=True ).get_normalised()
            self.matrix = self.matrix * orient_quat.to_matrix44()

    def render(self):
        glColor( self.colour )
        glPushMatrix()
        glMultMatrixf( self.matrix.to_opengl() )
        glCallList( self.display_list_id )
        glPopMatrix()



class MovingRigidColorMesh( RootedRigidColorMesh ):

    def __init__(self, acceleration_force, movement_speed, rotation_speed, *args ):
        super(MovingRigidColorMesh, self).__init__( *args[-3:] )

        self.movement_speed = movement_speed
        self.rotation_speed = rotation_speed
        self.acceleration_force = acceleration_force

        self.quat = Quaternion.from_matrix( self.matrix )

        self.velocity = Vector3( self.matrix.z_axis ) * self.movement_speed
        self.location = Vector3( self.matrix.translate )



    def update_rotation(self, TimeDelay):
        direction = Vector3( self.matrix.z_axis[:3] )
        velocity_dir = self.velocity.get_normalised()

        dp = direction.dot( velocity_dir )
        if dp > 0.997:      # we don't need to update, since we already facing the desired direction
            return False

        quat_roto = Quaternion.from_two_vec3( direction, velocity_dir, dotprod=dp, unit=True )
        koeff = self.rotation_speed * TimeDelay
        quat_roto.scale( koeff ).normalise()

        self.quat = quat_roto * self.quat
        return True

    def update_position(self, TimeDelay):
        delta_pos = self.velocity * TimeDelay
        quat_pos = Quaternion.from_two_vec3( self.location, self.location + delta_pos ).get_normalised()

        self.velocity = quat_pos.rotate_vec3( self.velocity )
        self.location = quat_pos.rotate_vec3( self.location )
        self.matrix.translate = self.location

        self.quat = quat_pos * self.quat

    def update_matrix(self):
        pos = self.matrix.translate     # save the translation component
        self.matrix = self.quat.normalise().to_matrix44()           # rebuild matrix, loosing the translation component
        self.matrix.translate = pos[:]

    def update(self, TimeDelay):
        self.update_rotation( TimeDelay )
        self.update_position( TimeDelay )
        self.update_matrix()
