# PyEPL: hardware/vr/avatar.pyx
#
# Copyright (C) 2003-2005 Michael J. Kahana
# Authors: Ian Schleifer, Per Sederberg, Aaron Geller, Josh Jacobs
# URL: http://memory.psych.upenn.edu/programming/pyepl
#
# Distributed under the terms of the GNU Lesser General Public License
# (LGPL). See the license.txt that came with this file.

"""
This modules contains the LowVAvatar class.
"""

import pyepl.hardware.vr
import eyes
import itertools
import math
import ode
import numpy

cdef object unitz
unitz = numpy.array((0.0, 0.0, 1.0))

cdef extern from "ode/ode.h":
    ctypedef double dReal
    ctypedef dReal dMatrix3[4*3]
    void dRFromEulerAngles(dMatrix3 R, dReal phi, dReal theta, dReal psi)

def RFromEulerAngles(phi, theta, psi):
    """
    Calculate the rotation matrix for the given Euler angles. 
    """
    cdef dMatrix3 R
    dRFromEulerAngles(R, phi, theta, psi)
    return [R[0], R[1], R[2], R[4], R[5], R[6], R[8], R[9], R[10]]

class LowVAvatar:
    """
    A LowVAvatar represents a user's position, orientation, and body
    shape in the virtual environment.
    """
    def __init__(self, env, posorient = None):
        """
        Create a LowVAvatar.
        """
        self.env = env
        self.eyevector = (0, 0, 0)
        self.eyes = []
        if posorient is not None:
            self.posorient = posorient
        else:
            self.posorient = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
    def travel(self, **controls):
        """
        """
        self.posorient = self.getNewPosOrient(**controls)
        if controls.has_key("view_pitch"):
            vp = controls["view_pitch"]
        else:
            vp = 0.0
        self.posorient = (self.posorient[0] + self.eyevector[0], self.posorient[1] + self.eyevector[1], self.posorient[2] + self.eyevector[2],
                          self.posorient[3], self.posorient[4] + vp, self.posorient[5])
        for eye in self.eyes:
            eye.reposition(*self.posorient)
    def getNewPosOrient(self):  # To be overridden
        """
        """
        pass
    def positionOrientation(self):
        """
        Return a 6-tuple of position and orientation data: x, y, z,
        yaw, pitch, roll.
        """
        return self.posorient
    def teleport(self, pos = None, angle = None, tilt = None, roll = None):#FIX!!
        """
        Set the absolute position, angle, tilt, and roll.
        """
        pass #...
    def attachEye(self, eye):
        """
        Lock motion of eye with the position of this avatar.
        """
        self.eyes.append(eye)
    def dettachEye(self, eye):
        """
        Unlock motion of eye.
        """
        self.eyes.remove(eye)

class LowVAvatarSpeedBubble(LowVAvatar):
    """
    A LowVAvatar represents a user's position, orientation, and body
    shape in the virtual environment.
    """
    def __init__(self, env, posorient = None, radius = 0.5, eyeheight = 3.8, density = 1.0):
        """
        Create a LowVAvatar.
        """
        LowVAvatar.__init__(self, env, posorient)
        self.eyevector = (0, eyeheight - radius, 0)
        self.body = ode.Body(env.world)
        M = ode.Mass()
        M.setSphere(density, radius)
        self.body.setMass(M)
        self.geom = ode.GeomSphere(env.space, radius)
        self.geom.setBody(self.body)
        self.body.setPosition((self.posorient[0], self.posorient[1] + radius, self.posorient[2]))
        self.body.setRotation(RFromEulerAngles(self.posorient[4] * (numpy.pi / 180.0),
                                               self.posorient[3] * (numpy.pi / 180.0),
                                               self.posorient[5] * (numpy.pi / 180.0)
                                               )  # order...?  Only sure about yaw...
                              )
        self.geom.mu = 0
    def getNewPosOrient(self, forward_speed = 0.0, yaw_speed = 0.0):
        """
        """
        global unitz
        cdef float x
        cdef float y
        cdef float z
        cdef float yaw
        cdef float pitch
        cdef float roll

        rot = numpy.reshape(numpy.array(self.body.getRotation()), (3, 3))
        unitdir = -numpy.dot(rot, unitz)
        self.body.setLinearVel(unitdir * forward_speed)
        self.body.setAngularVel((0.0, -yaw_speed, 0.0))
        self.env.dynamicStep(30)

        rot = numpy.reshape(numpy.array(self.body.getRotation()), (3, 3))
        unitdir = numpy.dot(rot, unitz)

	# NB. for now all this only works for yaw

        xunit, yunit, zunit = unitdir

	# the following gives yaw values ranging from -90 to 270 deg.,
	# with 90 directly ahead, and -90 directly behind
        yaw = numpy.arctan(xunit/zunit) * (180.0 / numpy.pi)
        if zunit < 0.0:
            yaw = yaw + 180.0

        roll=0
        pitch=0
        x, y, z = self.body.getPosition()
        return (x, y, z, yaw, pitch, roll)
