import direct.directbase.DirectStart
from direct.actor import Actor
from direct.task import Task
from pandac.PandaModules import *
import sys,os,time,string

from direct.gui.DirectGui import *
from direct.showbase import DirectObject

##########################################################
#
# A list of all the manipulable bones in the model.
# Each bone is accompanied by a specifier indicating 
# the "degrees of freedom" of the bone - ie, which axes
# you're supposed to rotate it around.
#
##########################################################

MOCAPBONES=[
("root",      "abcxyz", (   0,   0,   0)),
("lowerback", "xyz",    (   0,   0,   0)),
("upperback", "xyz",    (   0,   0,   0)),
("thorax",    "xyz",    (   0,   0,   0)),
("lowerneck", "xyz",    (   0,   0,   0)),
("upperneck", "xyz",    (   0,   0,   0)),
("head",      "xyz",    (   0,   0,   0)),
("rclavicle", "yz",     (   0,   0,   0)),
("rhumerus",  "xyz",    (-180,  30,  90)),
("rradius",   "x",      (   0,   0,   0)),
("rwrist",    "y",      (-180,  60,-180)),
("rhand",     "xz",     (   0,   0,   0)),
("rfingers",  "x",      (   0,   0,   0)),
("rthumb",    "xz",     (   0,   0,  45)),
("lclavicle", "yz",     (   0,   0,   0)),
("lhumerus",  "xyz",    (-180, -30, -90)),
("lradius",   "x",      (   0,   0,   0)),
("lwrist",    "y",      (-180, -60, 180)),
("lhand",     "xz",     (   0,   0,   0)),
("lfingers",  "x",      (   0,   0,   0)),
("lthumb",    "xz",     (   0,   0, -45)),
("rfemur",    "xyz",    (   0,   0, -20)),
("rtibia",    "x",      (   0,   0,   0)),
("rfoot",     "xz",     ( -45,   0,   0)),  # hand-edited data, used to be (-90,0,0)
("rtoes",     "x",      (   0,   0,   0)),
("lfemur",    "xyz",    (   0,   0,  20)),
("ltibia",    "x",      (   0,   0,   0)),
("lfoot",     "xz",     ( -45,   0,   0)),  # hand-edited data, used to be (-90,0,0)
("ltoes",     "x",      (   0,   0,   0)),
]

##########################################################
#
# Some handy utility routines
#
##########################################################

def errorexit(s):
  print s
  sys.exit(1)

##########################################################
#
# Mocap Actor
#
# Just a regular actor, with the following extras:
#
# self.positions - a list of positions for each bone
# self.controls - a list of joint controllers for each bone
#
# The actor is already running an idle animation, so you
# can start tweaking the joint controllers immediately.
#
##########################################################

class mocapActor(Actor.Actor):

  def __init__(self,fn):
    Actor.Actor.__init__(self,fn,{"idle":fn})
    exposed = Actor.Actor(fn,{"idle":fn})
    exposed.loop("idle")
    exposed.update()
    bundle = exposed.getPart("modelRoot").node().getBundle()
    self.matrices = []
    for bone,dofs,defrot in MOCAPBONES:
      matrix = Mat4()
      bundle.findChild(bone).getTransform(matrix)
      self.matrices.append(matrix)
    exposed.delete()
    self.controls = [self.controlJoint(None,"modelRoot",bone) for bone,dofs,defrot in MOCAPBONES]
    self.loop("idle")

  def applyMocapFrame(self, frame):
    rot = Mat4.rotateMat
    offs = 0
    for i in range(len(self.controls)):
      (bone,dofs,(rx,ry,rz)) = MOCAPBONES[i]
      mat = self.matrices[i]
      px = mat.getCell(3,0)
      py = mat.getCell(3,1)
      pz = mat.getCell(3,2)
      if (dofs=="abcxyz"):
        self.setPos(frame[offs+0]*0.2,frame[offs+2]*-0.2,frame[offs+1]*0.2)
	(tx,ty,tz)=(frame[offs+3],frame[offs+4],frame[offs+5])
      if (dofs=="x"):      (tx,ty,tz)=(frame[offs+0],0,0)
      if (dofs=="y"):      (tx,ty,tz)=(0,frame[offs+0],0)
      if (dofs=="xz"):     (tx,ty,tz)=(frame[offs+0],0,frame[offs+1])
      if (dofs=="yz"):     (tx,ty,tz)=(0,frame[offs+0],frame[offs+1])
      if (dofs=="xyz"):    (tx,ty,tz)=(frame[offs+0],frame[offs+1],frame[offs+2])
      mat = Mat4(1,0,0,0,0,1,0,0,0,0,1,0,px,py,pz,1)
      mat = rot(rx,Vec3(1,0,0)) * rot(ry,Vec3(0,0,1)) * rot(rz*(-1.0),Vec3(0,1,0)) * mat
      mat = rot(tx,Vec3(1,0,0)) * rot(ty,Vec3(0,0,1)) * rot(tz*(-1.0),Vec3(0,1,0)) * mat
      self.controls[i].setMat(mat)
      offs += len(dofs)


##########################################################
#
# readMocapFile
#
##########################################################

def readMocapFile(fn):
  try: f = file(fn,"r")
  except: errorexit("Cannot open "+fn+" for reading")
  ln = f.readline()
  if (ln[:9] != "#!OML:ASF"): errorexit("Mocap file doesn't start with #!OML:ASF")
  ln = f.readline().rstrip("\r\n")
  if (ln != ":FULLY-SPECIFIED"): errorexit("Mocap file doesn't say 'FULLY-SPECIFIED'")
  ln = f.readline().rstrip("\r\n")
  if (ln != ":DEGREES"): errorexit("Mocap file doesn't say 'DEGREES'")
  result = []
  next = 1
  while (1):
    ln = f.readline()
    if (ln == ""): break
    key = []
    ln.rstrip("\r\n")
    if (int(ln)!=next): errorexit("Mocap file contains irregular frame numbering")
    for bone,dofs,defrot in MOCAPBONES:
      words = f.readline().rstrip("\r\n").split()
      if (words[0] != bone): errorexit("Mocap file contains unexpected bone name")
      if (len(words) != len(dofs)+1): errorexit("Mocap file contains incorrect degrees of freedom")
      for i in range(len(dofs)):
        key.append(float(words[i+1]))
    next += 1
    result.append(key)
  print "Read "+str(len(result))+" frames from "+fn
  return(result)
