# 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/trajectories/rigid_body.py
# class SampledBodyModel(SampledPositionTrajectory, SampledJoint):
#     """A body model with sampled positions and rotations."""
# 
#     def __init__(self, name=None):
#         """
#         Initialise body model.
# 
#         @param name: The name of the root L{Joint} of the body model
#         """
#         SampledJoint.__init__(self, None, name)
#         SampledPositionTrajectory.__init__(self)
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         model = cls(ref.name)
#         for child in ref.children:
#             if child.isJoint:
#                 SampledJoint.structureCopy(child, model)
#             else:
#                 PointTrajectory(model, child.name, child.positionOffset)
#         return model
# 
# class SplinedBodyModel(SplinedPositionTrajectory, SplinedJoint):
#     """A body model with positions and rotations splined from sampled data."""
# 
#     def __init__(self, sampled, **kwargs):
#         """
#         Initialise splined body model.
# 
#         @param sampled: The L{SampledBodyModel} from which to generated
#             splined trajectories
#         """
#         SplinedJoint.__init__(self, None, sampled, **kwargs)
#         SplinedPositionTrajectory.__init__(self, sampled, **kwargs)
# 
#     @property
#     def startTime(self):
#         return max(self._positionStartTime, self._rotationStartTime)
# 
#     @property
#     def endTime(self):
#         return min(self._positionEndTime, self._rotationEndTime)
# 
# Path: imusim/algorithms/position.py
# class PositionEstimator(object):
# class ConstantPosition(PositionEstimator):
# class RootAccelerationIntegrator(PositionEstimator):
# class LowestPoint(PositionEstimator):
# class ContactTrackingKalmanFilter(PositionEstimator):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1))):
#     def __call__(self, data, t):
#     def _update(self, data, dt, t):
#     def getParameter(self, data, jointName, parameter, default=None):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)),
#             **kwargs):
#     def _update(self, data, dt, t):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)),
#             initialVelocity=np.zeros((3,1)),
#             integrationMethod=integrators.TrapeziumRule, **kwargs):
#     def _update(self, data, dt, t):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)),
#             **kwargs):
#     def _lowestPoint(self, t):
#     def _update(self, data, dt, t):
#     def __init__(self, model, initialTime=0,
#             initialPosition=np.zeros((3,1)),
#             initialVelocity=np.zeros((3,1)),
#             accelerationVar=0.2, velocityVar=0.5,
#             rejectionProbabilityThreshold=0.4,
#             **kwargs):
#     def _update(self, data, dt, t):
#     def estimateVelocityProbabilities(self, dt, t):
#     def transitionMatrix(self, dt):
#     def controlMatrix(self, dt):
#     def processCovariance(self, dt):
#         A = np.matrix([ [1, 0, 0, dt, 0, 0],
#                         [0, 1, 0, 0, dt, 0],
#                         [0, 0, 1, 0, 0, dt],
#                         [0, 0, 0, 1, 0, 0 ],
#                         [0, 0, 0, 0, 1, 0 ],
#                         [0, 0, 0, 0, 0, 1 ]])
#         B = np.matrix([ [a, 0, 0],
#                         [0, a, 0],
#                         [0, 0, a],
#                         [b, 0, 0],
#                         [0, b, 0],
#                         [0, 0, b]])
#         Q = np.matrix([ [a, 0, 0, b, 0, 0],
#                         [0, a, 0, 0, b, 0],
#                         [0, 0, a, 0, 0, b],
#                         [b, 0, 0, c, 0, 0],
#                         [0, b, 0, 0, c, 0],
#                         [0, 0, b, 0, 0, c]])


from os import path
from imusim.io.bvh import loadBVHFile
from imusim.trajectories.rigid_body import SampledBodyModel, SplinedBodyModel
from imusim.algorithms import position
from imusim.testing.inspection import getImplementations
import numpy as np


referenceModel = SplinedBodyModel(
    loadBVHFile(path.join(path.dirname(__file__), 'test.bvh'), 0.01))
dt = 0.01
sampleTimes = np.arange(referenceModel.startTime, referenceModel.endTime, dt)

def checkAlgorithm(algorithm):
    sampledModel = SampledBodyModel.structureCopy(referenceModel)

    for t in sampleTimes:
        for referenceJoint, sampledJoint in zip(referenceModel.joints,
                sampledModel.joints):
            sampledJoint.rotationKeyFrames.add(t, referenceJoint.rotation(t))

    positionEstimator = algorithm(sampledModel,
            initialTime = referenceModel.startTime,
            initialPosition = referenceModel.position(referenceModel.startTime),
            initialVelocity = referenceModel.velocity(referenceModel.startTime))

    for t in sampleTimes[1:]:
        data = [{'jointName' : referenceModel.name,
                'linearAcceleration' : referenceModel.acceleration(t)}]
        positionEstimator(data, t)

    result = sampledModel.positionKeyFrames.values
    assert np.shape(result) == referenceModel.position(sampleTimes).shape
    assert np.all(np.isfinite(result))

def testPositionAlgorithms():

# 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/trajectories/rigid_body.py
# class SampledBodyModel(SampledPositionTrajectory, SampledJoint):
#     """A body model with sampled positions and rotations."""
# 
#     def __init__(self, name=None):
#         """
#         Initialise body model.
# 
#         @param name: The name of the root L{Joint} of the body model
#         """
#         SampledJoint.__init__(self, None, name)
#         SampledPositionTrajectory.__init__(self)
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         model = cls(ref.name)
#         for child in ref.children:
#             if child.isJoint:
#                 SampledJoint.structureCopy(child, model)
#             else:
#                 PointTrajectory(model, child.name, child.positionOffset)
#         return model
# 
# class SplinedBodyModel(SplinedPositionTrajectory, SplinedJoint):
#     """A body model with positions and rotations splined from sampled data."""
# 
#     def __init__(self, sampled, **kwargs):
#         """
#         Initialise splined body model.
# 
#         @param sampled: The L{SampledBodyModel} from which to generated
#             splined trajectories
#         """
#         SplinedJoint.__init__(self, None, sampled, **kwargs)
#         SplinedPositionTrajectory.__init__(self, sampled, **kwargs)
# 
#     @property
#     def startTime(self):
#         return max(self._positionStartTime, self._rotationStartTime)
# 
#     @property
#     def endTime(self):
#         return min(self._positionEndTime, self._rotationEndTime)
# 
# Path: imusim/algorithms/position.py
# class PositionEstimator(object):
# class ConstantPosition(PositionEstimator):
# class RootAccelerationIntegrator(PositionEstimator):
# class LowestPoint(PositionEstimator):
# class ContactTrackingKalmanFilter(PositionEstimator):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1))):
#     def __call__(self, data, t):
#     def _update(self, data, dt, t):
#     def getParameter(self, data, jointName, parameter, default=None):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)),
#             **kwargs):
#     def _update(self, data, dt, t):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)),
#             initialVelocity=np.zeros((3,1)),
#             integrationMethod=integrators.TrapeziumRule, **kwargs):
#     def _update(self, data, dt, t):
#     def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)),
#             **kwargs):
#     def _lowestPoint(self, t):
#     def _update(self, data, dt, t):
#     def __init__(self, model, initialTime=0,
#             initialPosition=np.zeros((3,1)),
#             initialVelocity=np.zeros((3,1)),
#             accelerationVar=0.2, velocityVar=0.5,
#             rejectionProbabilityThreshold=0.4,
#             **kwargs):
#     def _update(self, data, dt, t):
#     def estimateVelocityProbabilities(self, dt, t):
#     def transitionMatrix(self, dt):
#     def controlMatrix(self, dt):
#     def processCovariance(self, dt):
#         A = np.matrix([ [1, 0, 0, dt, 0, 0],
#                         [0, 1, 0, 0, dt, 0],
#                         [0, 0, 1, 0, 0, dt],
#                         [0, 0, 0, 1, 0, 0 ],
#                         [0, 0, 0, 0, 1, 0 ],
#                         [0, 0, 0, 0, 0, 1 ]])
#         B = np.matrix([ [a, 0, 0],
#                         [0, a, 0],
#                         [0, 0, a],
#                         [b, 0, 0],
#                         [0, b, 0],
#                         [0, 0, b]])
#         Q = np.matrix([ [a, 0, 0, b, 0, 0],
#                         [0, a, 0, 0, b, 0],
#                         [0, 0, a, 0, 0, b],
#                         [b, 0, 0, c, 0, 0],
#                         [0, b, 0, 0, c, 0],
#                         [0, 0, b, 0, 0, c]])


# Path: imusim/tests/algorithms/position_test.py
from os import path
from imusim.io.bvh import loadBVHFile
from imusim.trajectories.rigid_body import SampledBodyModel, SplinedBodyModel
from imusim.algorithms import position
from imusim.testing.inspection import getImplementations
import numpy as np



referenceModel = SplinedBodyModel(
    loadBVHFile(path.join(path.dirname(__file__), 'test.bvh'), 0.01))
dt = 0.01
sampleTimes = np.arange(referenceModel.startTime, referenceModel.endTime, dt)

def checkAlgorithm(algorithm):
    sampledModel = SampledBodyModel.structureCopy(referenceModel)

    for t in sampleTimes:
        for referenceJoint, sampledJoint in zip(referenceModel.joints,
                sampledModel.joints):
            sampledJoint.rotationKeyFrames.add(t, referenceJoint.rotation(t))

    positionEstimator = algorithm(sampledModel,
            initialTime = referenceModel.startTime,
            initialPosition = referenceModel.position(referenceModel.startTime),
            initialVelocity = referenceModel.velocity(referenceModel.startTime))

    for t in sampleTimes[1:]:
        data = [{'jointName' : referenceModel.name,
                'linearAcceleration' : referenceModel.acceleration(t)}]
        positionEstimator(data, t)

    result = sampledModel.positionKeyFrames.values
    assert np.shape(result) == referenceModel.position(sampleTimes).shape
    assert np.all(np.isfinite(result))

def testPositionAlgorithms():

    for algorithm in getImplementations(position, position.PositionEstimator):