# Path: imusim/io/asf_amc.py
# def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod):
#     """
#     Load motion capture data from an ASF and AMC file pair.
# 
#     @param asfFileName: Name of the ASF file containing the description of
#         the rigid body model
#     @param amcFileName: Name of the AMC file containing the motion data
#     @param scaleFactor: Scaling factor to convert lengths to m. For data
#         from the CMU motion capture corpus this should be 2.54/100 to
#         convert from inches to metres.
# 
#     @return: A {SampledBodyModel} representing the root of the rigid body
#         model structure.
#     """
#     with open(asfFileName, 'r') as asfFile:
#         data = asfParser.parseFile(asfFile)
#         scale = (1.0/data.units.get('length',1)) * scaleFactor
# 
#         bones = dict((bone.name,bone) for bone in data.bones)
#         asfModel = ASFRoot(data.root)
# 
#         for entry in data.hierarchy:
#             parent = asfModel.getBone(entry.parent)
#             for childName in entry.children:
#                 ASFBone(parent, bones[childName], scale)
# 
#         imusimModel = SampledBodyModel('root')
#         for subtree in asfModel.children:
#             for bone in subtree:
#                 if not bone.isDummy:
#                     offset = vector(0,0,0)
#                     ancestors = bone.ascendTree()
#                     while True:
#                         ancestor = ancestors.next().parent
#                         offset += ancestor.childoffset
#                         if not ancestor.isDummy:
#                             break
# 
#                     SampledJoint(parent=imusimModel.getJoint(ancestor.name),
#                             name=bone.name,
#                             offset=offset)
#                 if not bone.hasChildren:
#                     PointTrajectory(
#                             parent=imusimModel.getJoint(bone.name),
#                             name=bone.name+'_end',
#                             offset=bone.childoffset
#                             )
# 
#         with open(amcFileName) as amcFile:
#             motion = amcParser.parseFile(amcFile)
#             t = 0
#             for frame in motion.frames:
#                 for bone in frame.bones:
#                     bonedata = asfModel.getBone(bone.name)
#                     if bone.name == 'root':
#                         data = dict((chan.lower(), v) for chan,v in
#                             zip(bonedata.channels,bone.channels))
#                         position = convertCGtoNED(scale * vector(data['tx'],
#                             data['ty'],data['tz']))
#                         imusimModel.positionKeyFrames.add(t, position)
# 
#                     axes, angles = zip(*[(chan[-1], angle) for chan, angle in
#                         zip(bonedata.channels, bone.channels) if
#                                 chan.lower().startswith('r')])
#                     rotation = (bonedata.rotationOffset.conjugate *
#                             Quaternion.fromEuler(angles[::-1], axes[::-1]))
#                     joint = imusimModel.getJoint(bone.name)
#                     if joint.hasParent:
#                         parentRot = joint.parent.rotationKeyFrames.latestValue
#                         parentRotOffset = bonedata.parent.rotationOffset
#                         rotation = parentRot * parentRotOffset * rotation
#                     else:
#                         rotation = convertCGtoNED(rotation)
#                     joint.rotationKeyFrames.add(t, rotation)
#                 t += framePeriod
# 
#         return imusimModel
# 
# Path: imusim/io/bvh.py
# def loadBVHFile(filename, conversionFactor=1):
#     """
#     Load sampled body model data from a BVH file.
# 
#     @param filename: Name of file to load.
#     @param conversionFactor: Scale factor to apply to position values, e.g.
#         to convert meters to centimeters use conversionFactor=100.
# 
#     @return: A L{SampledBodyModel} object.
#     """
#     with open(filename,'r') as bvhFile:
#         loader = BVHLoader(bvhFile,conversionFactor)
#         loader._readHeader()
#         loader._readMotionData()
#     return loader.model


from imusim.io.asf_amc import loadASFFile
from imusim.io.bvh import loadBVHFile
from os import path
from numpy.testing import assert_almost_equal
from imusim.testing.quaternions import assertMaximumRMSErrorAngle
"""
Tests for ASF/AMC input
"""
# Copyright (C) 2009-2011 University of Edinburgh
#
# This file is part of IMUSim.
#
# IMUSim is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# IMUSim is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with IMUSim.  If not, see <http://www.gnu.org/licenses/>.


def testASFInput():
    dir = path.dirname(__file__)
    asfFile = path.join(dir, "16.asf")
    amcFile = path.join(dir, "16_15.amc")
    bvhFile = path.join(dir, "16_15.bvh")


# Path: imusim/io/asf_amc.py
# def loadASFFile(asfFileName, amcFileName, scaleFactor, framePeriod):
#     """
#     Load motion capture data from an ASF and AMC file pair.
# 
#     @param asfFileName: Name of the ASF file containing the description of
#         the rigid body model
#     @param amcFileName: Name of the AMC file containing the motion data
#     @param scaleFactor: Scaling factor to convert lengths to m. For data
#         from the CMU motion capture corpus this should be 2.54/100 to
#         convert from inches to metres.
# 
#     @return: A {SampledBodyModel} representing the root of the rigid body
#         model structure.
#     """
#     with open(asfFileName, 'r') as asfFile:
#         data = asfParser.parseFile(asfFile)
#         scale = (1.0/data.units.get('length',1)) * scaleFactor
# 
#         bones = dict((bone.name,bone) for bone in data.bones)
#         asfModel = ASFRoot(data.root)
# 
#         for entry in data.hierarchy:
#             parent = asfModel.getBone(entry.parent)
#             for childName in entry.children:
#                 ASFBone(parent, bones[childName], scale)
# 
#         imusimModel = SampledBodyModel('root')
#         for subtree in asfModel.children:
#             for bone in subtree:
#                 if not bone.isDummy:
#                     offset = vector(0,0,0)
#                     ancestors = bone.ascendTree()
#                     while True:
#                         ancestor = ancestors.next().parent
#                         offset += ancestor.childoffset
#                         if not ancestor.isDummy:
#                             break
# 
#                     SampledJoint(parent=imusimModel.getJoint(ancestor.name),
#                             name=bone.name,
#                             offset=offset)
#                 if not bone.hasChildren:
#                     PointTrajectory(
#                             parent=imusimModel.getJoint(bone.name),
#                             name=bone.name+'_end',
#                             offset=bone.childoffset
#                             )
# 
#         with open(amcFileName) as amcFile:
#             motion = amcParser.parseFile(amcFile)
#             t = 0
#             for frame in motion.frames:
#                 for bone in frame.bones:
#                     bonedata = asfModel.getBone(bone.name)
#                     if bone.name == 'root':
#                         data = dict((chan.lower(), v) for chan,v in
#                             zip(bonedata.channels,bone.channels))
#                         position = convertCGtoNED(scale * vector(data['tx'],
#                             data['ty'],data['tz']))
#                         imusimModel.positionKeyFrames.add(t, position)
# 
#                     axes, angles = zip(*[(chan[-1], angle) for chan, angle in
#                         zip(bonedata.channels, bone.channels) if
#                                 chan.lower().startswith('r')])
#                     rotation = (bonedata.rotationOffset.conjugate *
#                             Quaternion.fromEuler(angles[::-1], axes[::-1]))
#                     joint = imusimModel.getJoint(bone.name)
#                     if joint.hasParent:
#                         parentRot = joint.parent.rotationKeyFrames.latestValue
#                         parentRotOffset = bonedata.parent.rotationOffset
#                         rotation = parentRot * parentRotOffset * rotation
#                     else:
#                         rotation = convertCGtoNED(rotation)
#                     joint.rotationKeyFrames.add(t, rotation)
#                 t += framePeriod
# 
#         return imusimModel
# 
# Path: imusim/io/bvh.py
# def loadBVHFile(filename, conversionFactor=1):
#     """
#     Load sampled body model data from a BVH file.
# 
#     @param filename: Name of file to load.
#     @param conversionFactor: Scale factor to apply to position values, e.g.
#         to convert meters to centimeters use conversionFactor=100.
# 
#     @return: A L{SampledBodyModel} object.
#     """
#     with open(filename,'r') as bvhFile:
#         loader = BVHLoader(bvhFile,conversionFactor)
#         loader._readHeader()
#         loader._readMotionData()
#     return loader.model


# Path: imusim/tests/io/asf_amc_test.py
from imusim.io.asf_amc import loadASFFile
from imusim.io.bvh import loadBVHFile
from os import path
from numpy.testing import assert_almost_equal
from imusim.testing.quaternions import assertMaximumRMSErrorAngle

"""
Tests for ASF/AMC input
"""
# Copyright (C) 2009-2011 University of Edinburgh
#
# This file is part of IMUSim.
#
# IMUSim is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# IMUSim is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with IMUSim.  If not, see <http://www.gnu.org/licenses/>.


def testASFInput():
    dir = path.dirname(__file__)
    asfFile = path.join(dir, "16.asf")
    amcFile = path.join(dir, "16_15.amc")
    bvhFile = path.join(dir, "16_15.bvh")


    asfModel = loadASFFile(asfFile, amcFile, scaleFactor=2.54/100,