#!/usr/bin/python
# -*- coding: utf-8 -*-

## Modified version of the BVH importer borrowed from the makehuman project
## some parts borrowed from bvhplay


"""
**Project Name:**      MakeHuman

**Product Home Page:** http://www.makehuman.org/

**Code Home Page:**    http://code.google.com/p/makehuman/

**Authors:**           Marc Flerackers, Jonas Hauquier

**Copyright(c):**      MakeHuman Team 2001-2012

**Licensing:**         GPL3 (see also http://sites.google.com/site/makehumandocs/licensing)

**Coding Standards:**  See http://sites.google.com/site/makehumandocs/developers-guide

Abstract
--------

BVH importer
"""

# Libraries from makehuman
from aljabr import vadd, makeUnit, degree2rad, makeTranslation, mmul, euler2matrix

# Standard libraries
from math import radians, cos, sin, pi
import math

# Numpy
from numpy import array, dot


#######################################
# JOINT class (formerly BONE)
# A BVH "joint" is a single vertex with potentially MULTIPLE
# edges.  It's not accurate to call these "bones" because if
# you rotate the joint, you rotate ALL attached bones.

class Joint:
  def __init__(self, name):
    self.name = name
    self.children = []
    # Ordered list of channels: each
    # list entry is one of [XYZ]position, [XYZ]rotation
    self.channels = []
    self.frames = []
    self.parent = None
    self.strans = array([0.,0.,0.]) # static translation vector

    self.order = None

    # Transformation matrix:
    self.stransmat = array([ [0.,0.,0.,0.],[0.,0.,0.,0.],    \
                               [0.,0.,0.,0.],[0.,0.,0.,0.] ])


  def info(self):
    print("Joint name:", self.name)
    print(" %s is connected to " % self.name)
    if(len(self.children) == 0):
        print("nothing")
    else:
      for child in self.children:
        print("%s " % child.name)
      print()
    for child in self.children:
      child.info()


  def updateFrame(self, frame, scale=1):
    xpos = 0.0
    ypos = 0.0
    zpos = 0.0

    xrot = 0.0
    yrot = 0.0
    zrot = 0.0

    # We have to build up drotmat one rotation value at a time so that
    # we get the matrix multiplication order correct.
    drotmat = array([ [1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.] ])

    # Build a translation matrix for this keyframe
    dtransmat = array([ [1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.] ])

    # Apply orientation for frame of motion
    if frame >= 0 and frame < len(self.frames):
        for index, channel in enumerate(self.channels):
            if channel == 'Xposition':
                xpos = scale*self.frames[frame][index]
                dtransmat[0,3] = xpos
            elif channel == 'Yposition':
                ypos = scale*self.frames[frame][index]
                dtransmat[1,3] = ypos
            elif channel == 'Zposition':
                zpos = scale*self.frames[frame][index]
                dtransmat[2,3] = zpos
              
            if channel == 'Xrotation':
                xrot = self.frames[frame][index]
                theta = radians(xrot)
                mycos = cos(theta)
                mysin = sin(theta)
                drotmat2 = array([ [1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.], [0.,0.,0.,1.] ])
                drotmat2[1,1] = mycos
                drotmat2[1,2] = -mysin
                drotmat2[2,1] = mysin
                drotmat2[2,2] = mycos
                drotmat = dot(drotmat, drotmat2)
            elif channel == 'Yrotation':
                yrot = self.frames[frame][index]
                theta = radians(yrot)
                mycos = cos(theta)
                mysin = sin(theta)
                drotmat2 = array([ [1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.], [0.,0.,0.,1.] ])
                drotmat2[0,0] = mycos
                drotmat2[0,2] = mysin
                drotmat2[2,0] = -mysin
                drotmat2[2,2] = mycos
                drotmat = dot(drotmat, drotmat2)
            elif channel == 'Zrotation':
                zrot = self.frames[frame][index]
                theta = radians(zrot)
                mycos = cos(theta)
                mysin = sin(theta)
                drotmat2 = array([ [1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.], [0.,0.,0.,1.] ])
                drotmat2[0,0] = mycos
                drotmat2[0,1] = -mysin
                drotmat2[1,0] = mysin
                drotmat2[1,1] = mycos
                drotmat = dot(drotmat, drotmat2)

    self.drotmat = drotmat
    self.dtransmat = dtransmat

    # At this point we should have computed:
    # Static (s)
    #  stransmat  (computed previously in __readJoint subroutine)
    # Dynamic (d)
    #  dtransmat (only non-zero if we're the hips)
    #  drotmat
    # We now have enough to compute joint.trtr and also to convert
    # the position of this joint (vertex) to worldspace.

    # trtr[time]  A premultiplied series of translation and rotation matrices

    if self.parent:
        self.localtoworld = dot(self.parent.trtr, self.stransmat)
    else:
        self.localtoworld = dot(self.stransmat, self.dtransmat)

    # Add rotation of this joint to stack to use for determining children positions
    # Note that position of this joint is not affected by its rotation
    self.trtr = dot(self.localtoworld,self.drotmat)

    # Position is the translation part of the mat (fourth column)
    self.worldpos = array([ self.localtoworld[0,3],
                            self.localtoworld[1,3],
                            self.localtoworld[2,3], 
                            self.localtoworld[3,3] ])

    for child in self.children:
        child.updateFrame(frame)


  def parseChannels(self, channels):
        self.order = ""     # order in which rotation angles are stored
        for channel in channels:
            if channel == "Xrotation":
                self.order += "x"
            elif channel == "Yrotation":
                self.order += "z"
            elif channel == "Zrotation":
                self.order += "y"
        return

