class Simulation(object):
    """
    Main IMUSim simulation environment.

    @ivar rng: L{np.random.RandomState} instance for random number generation.
    @ivar environment: L{Environment} in use for simulation.
    @ivar engine: L{SimPy.Simulation} instance driving simulation.
    """

    def __init__(self, seed=None, environment=None,
            engine=SimPy.Simulation.Simulation):
        """
        Initialise simulation.

        @param seed: Seed value for L{np.random.RandomState}.
        @param environment: L{Environment} to simulate in.
        @param engine: L{SimPy.Simulation} subclass to drive simulation.
        """
        self.environment = Environment() if environment is None else environment
        self.engine = engine()
        self.engine.initialize()
        self.rng = np.random.RandomState(seed)

    @property
    def time(self):
        """ The current true time within the simulation (float). """
        assert hasattr(self, '_startTime'), 'Simulation time has not been set.'
        return self.engine.now() + self._startTime

    @time.setter
    def time(self, time):
        assert not hasattr(self, '_startTime'), 'Simulation time already set.'
        self._startTime = time

    class ProgressMonitor(SimPy.Simulation.Process):
        """
        Simple process to print status and remaining simulation time at 5%
        progress intervals.
        """
        def __init__(self, sim, duration):
            SimPy.Simulation.Process.__init__(self, sim=sim)
            self.startTime = sim.now()
            self.duration = duration
            sim.activate(self, self.printProgress())

        def printProgress(self):
            for i in range(0,20):
                start = time.time()
                yield SimPy.Simulation.hold, self, self.duration / 20.0
                now = self.sim.now() - self.startTime
                end = time.time()

                remaining = (20-i) * (end-start)
                print ("Simulated %.1fs of %.1fs (%3.0f%%). " +
                        "Estimated time remaining %.1fs") \
                        %(now, self.duration, (i + 1) * 5, remaining)

    def run(self, endTime, printProgress=True):
        """
        Advance the simulation up to the given time.

        @param endTime: Simulation time at which to stop (float).
        @param printProgress: Whether to print progress information (bool).
        """

        if printProgress:
            print "Simulating..."
            startTime = self.time
            duration = endTime - startTime
            progressMonitor = self.ProgressMonitor(self.engine, duration)

        startWallTime = time.time()
        self.engine.simulate(endTime - self._startTime)
        endWallTime = time.time()

        if printProgress:
            print "Simulation complete."
            print "Simulated %.1f seconds in %.1f seconds." % (
                    endTime - startTime, endWallTime - startWallTime)

    def subrng(self):
        """
        Obtain a new random number generator seeded from the main RNG.

        @return: A L{np.random.RandomState} instance.
        """
        return np.random.RandomState(seed=int(self.rng.uniform(0,2**31))) 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 class DistLinAccelCF(OrientationFilter):
    """
    Implementation of the multi-IMU orientation algorithm by Young & Ling.

    See A. D. Young, M. J. Ling and D. K. Arvind. "Distributed Estimation of
    Linear Acceleration for Improved Accuracy in Wireless Inertial Motion
    Capture", in Proc. 9th ACM/IEEE International Conference on Information
    Processing in Sensor Networks, ACM, 2010, pp. 256-267.

    @ivar qMeas: Result of latest vector observation (L{Quaternion}).
    @ivar vectorObservation: L{TimeSeries} of vector observation results.
    @ivar jointAccel: Latest estimate of joint acceleration
        (3x1 L{np.ndarray}).
    @ivar jointAcceleration: L{TimeSeries} of acceleration estimates.
    """
    GRAVITY_VECTOR = vectors.vector(0, 0, STANDARD_GRAVITY)

    @prepend_method_doc(BachmannCF)
    def __init__(self, initialTime, initialRotation, k, joint, offset,
            **kwargs):
        """
        @param joint: The L{Joint} the IMU is attached to.
        @param offset: The offset of the IMU in the joint's co-ordinate frame
            (3x1 L{np.ndarray})
        """
        OrientationFilter.__init__(self, initialTime, initialRotation)
        self.rotation.add(initialTime, initialRotation)
        self.qHat = initialRotation.copy()
        self.k = k
        self.joint = joint
        self.offset = offset

        self._vectorObservation = vector_observation.GramSchmidt()
        self.qMeas = Quaternion.nan()
        self.jointAccel = vectors.nan()

        self.gyroFIFO = collections.deque([], maxlen=3)
        self.accelFIFO = collections.deque([], maxlen=2)
        self.magFIFO = collections.deque([], maxlen=2)

        self.isRoot = not joint.hasParent
        self.children = joint.childJoints

        self.vectorObservation = TimeSeries()
        self.jointAcceleration = TimeSeries()

    def handleLinearAcceleration(self, jointAcceleration, dt):
        """
        Perform drift correction based on incoming joint acceleration estimate.

        @param jointAcceleration: Acceleration estimate (3x1 L{np.ndarray}).
        """
        self.jointAccel = jointAcceleration

        if len(self.gyroFIFO) < 3:
            return

        # Estimate linear acceleration
        o = self.offset
        w = self.gyroFIFO
        q = self.qHat_minus_1
        a = (w[0] - w[2]) / (2 * dt)
        lt = vectors.cross(a, o)
        lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
        l = (q.rotateFrame(self.jointAccel) + lt + lr)

        # Apply drift correction
        self.qMeas = self._vectorObservation(-(self.accelFIFO[1]- l),
                self.magFIFO[1])
        if q.dot(self.qMeas) < 0:
            self.qMeas.negate()
        q = q + 1.0/self.k * dt * (self.qMeas - q)
        dotq = 0.5 * self.qHat * Quaternion(0,*w[0])
        self.qHat = q + dotq * dt
        self.qHat.normalise()

    def childAcceleration(self, o, dt):
        """
        Compute acceleration for child joint.

        @param o: Offset of child joint (3x1 L{np.ndarray}).
        """
        w = self.gyroFIFO
        if len(w) < 3:
            return None
        q = self.qHat_minus_1
        a = (w[0] - w[2]) / (2 * dt)
        lt = vectors.cross(a, o)
        lr = vectors.dot(w[1], o) * w[1] - o * vectors.norm(w[1])**2
        l = self.jointAccel + q.rotateVector(lt + lr)
        return l

    def _update(self, accel, mag, gyro, dt, t):
        # Store measurements in queues for processing
        self.gyroFIFO.appendleft(gyro)
        self.accelFIFO.appendleft(accel)
        self.magFIFO.appendleft(mag)

        # save current qHat for drift correction
        self.qHat_minus_1 = copy.copy(self.qHat)

        # Inertial update
        dotq = 0.5 * self.qHat * Quaternion(0,*gyro)
        self.qHat += dotq * dt
        self.qHat.normalise()

        if self.isRoot and len(self.accelFIFO) >= 2:
            # Root can't receive acceleration data so must estimate it.
            measuredAccel = self.qHat_minus_1.rotateVector(self.accelFIFO[1])
            # Assume root joint acceleration was zero.
            self.jointAccel = np.zeros((3,1))
            # Calculate tangential acceleration for root joint IMU offset.
            linAccel = self.childAcceleration(self.offset, dt)
            if linAccel is not None:
                # Calculate expected measurement.
                expectedAccel = linAccel - self.GRAVITY_VECTOR
                # Set root joint acceleration to the residual.
                self.jointAccel = measuredAccel - expectedAccel
                self.handleLinearAcceleration(self.jointAccel, dt)

        self.rotation.add(t, self.qHat)
        self.vectorObservation.add(t, self.qMeas)
        self.jointAcceleration.add(t, self.jointAccel) class IdealIMU(StandardIMU):
    """
    An IMU with idealised models for all components.
    """

    def __init__(self, simulation=None, trajectory=None):
        self.accelerometer = IdealAccelerometer(self)
        self.magnetometer = IdealMagnetometer(self)
        self.gyroscope = IdealGyroscope(self)
        self.adc = IdealADC(self)
        self.timer = IdealTimer(self)
        self.radio = IdealRadio(self)
        StandardIMU.__init__(self, simulation, trajectory) class ScaleAndOffsetCalibrator(Calibrator):
    """
    Simple calibration procedure that obtains scale and offset factors.

    This procedure simulates testing the device in 18 placements:

        1. Stationary on a flat surface in 6 orthogonal orientations, such
           that gravity is aligned with each direction of each IMU axis.

        2. Stationary on an inclined surface in 6 orthogonal orientations,
           such that magnetic north is aligned with each direction of each
           IMU axis.

        3. On a rotating stage of known angular velocity in 6 orthogonal
           orientations, such that the axis of rotation is aligned with each
           direction of each IMU axis.
    """

    @prepend_method_doc(Calibrator)
    def __init__(self, environment, samples, samplingPeriod,
            rotationalVelocity):
        """
        @param samples: Number of samples to take in each placement (int).
        @param samplingPeriod: Time between samples (float, s).
        @param rotationalVelocity: Angular rate of rotations (float, rad/s).
        """
        Calibrator.__init__(self, environment)
        self.samples = samples
        self.samplingPeriod = samplingPeriod
        self.rotationalVelocity = rotationalVelocity

    def testTrajectories(self):
        position = vectors.vector(0,0,0)
        # Accelerometer test trajectories
        for angles in [
            (0,-90,0),(0,90,0), # X down, X up (pitch -/+90 deg)
            (0,0,90),(0,0,-90), # Y down, Y up (roll +/-90 deg)
            (0,0,0),(0,0,180)]: # Z down, Z up (roll 0/180 deg)
            rotation = Quaternion().setFromEuler(angles)
            yield StaticTrajectory(rotation=rotation)
        # Magnetometer test trajectories
        field = self.environment.magneticField(position, 0)
        N = field / vectors.norm(field)
        E = vectors.vector(-N[1,0], N[0,0], 0)
        E /= vectors.norm(E)
        D = vectors.cross(N, E)
        for vectorset in [
            (N,E,D), (-N,E,-D), # X north, X south
            (E,-N,D), (-E,N,D), # Y north, Y south
            (D,E,-N), (-D,E,N)]: # Z north, Z south
            rotation = Quaternion().setFromVectors(*vectorset)
            yield StaticTrajectory(rotation=rotation)
        # Gyro test trajectories
        for axis in range(3):
            omega = vectors.vector(0,0,0)
            omega[axis] = self.rotationalVelocity
            yield StaticContinuousRotationTrajectory(omega)
            yield StaticContinuousRotationTrajectory(-omega)

    def calibrate(self, imu):
        calibration = {}
        position = vectors.vector(0,0,0)
        expected = {}
        measured = {}
        for sensor in imu.sensors:
            expected[sensor] = []
            measured[sensor] = []
        for trajectory in self.testTrajectories():
            sim = Simulation(environment=self.environment)
            sim.time = 0
            imu.simulation = sim
            imu.trajectory = trajectory
            BasicIMUBehaviour(imu, self.samplingPeriod)
            sim.run(self.samplingPeriod * self.samples,
                    printProgress=False)
            t = sensor.rawMeasurements.timestamps
            for sensor in imu.sensors:
                expected[sensor].append(sensor.trueValues(t))
                measured[sensor].append(sensor.rawMeasurements.values)
        for sensor in imu.sensors:
            calibration[sensor] = ScaleAndOffsetCalibration.fromMeasurements(
                np.hstack(expected[sensor]), np.hstack(measured[sensor]))
        return calibration class BasicIMUBehaviour(object):
    """
    Basic behaviour for an IMU that performs periodic sampling.

    @ivar imu: L{IMU} on which the behaviour executes.
    @ivar samplingPeriod: Interval at which to sample and process (float).
    @ivar calibration: Mapping from IMU L{Sensor}s to L{SensorCalibration}s
        (dict).
    @ivar filter: L{OrientationFilter} to update with each set of samples.
    @ivar sampleCallback: Function to call after each sample. The behaviour
        object will be passed as a single argument.
    @ivar timerMux: L{TimerMultiplexer} for IMU timer.
    """

    __metaclass__ = ABCMeta

    def __init__(self, imu, samplingPeriod, calibration=None, filter=None,
            sampleCallback=None, initialTime=0):
        """
        Initialise IMU behaviour.

        @param imu: L{IMU} on which the behaviour executes.
        @param samplingPeriod: Interval at which to sample and process (float).
        @param calibration: Mapping from IMU L{Sensor}s to
            L{SensorCalibration}s (dict).
        @param filter: L{OrientationFilter} to update with each set of samples.
        @param sampleCallback: Function to call after each sample. The
            behaviour object will be passed as a single argument.
        @param initialTime: Initial time for local timekeeping.
        """

        self.imu = imu
        self.samplingPeriod = samplingPeriod
        self.calibration = calibration
        self.filter = filter

        for sensor in imu.sensors:
            sensor.rawMeasurements = TimeSeries()
            if self.calibration is not None:
                sensor.calibratedMeasurements = TimeSeries()

        self.sampleCallback = sampleCallback

        self.timerMux = TimerMultiplexer(imu.timer)

        PeriodicSampler(imu.adc, imu.sensors, VirtualTimer(self.timerMux),
                samplingPeriod, self._handleSample)

        self._time = initialTime

    def _handleSample(self, *measurements):
        measurements = list(measurements)
        self._time += self.samplingPeriod
        for sensor, measurement in zip(self.imu.sensors, measurements):
            sensor.rawMeasurements.add(self._time, measurement)
            if self.calibration is not None:
                measurement = self.calibration[sensor].apply(measurement)
                sensor.calibratedMeasurements.add(self._time, measurement)
        if self.filter is not None:
            self.filter(*measurements + [self._time])
        if self.sampleCallback != None:
            self.sampleCallback(self) class TimerMultiplexer(object):
    """
    Multiplexer for running multiple virtual timers on one hardware timer.
    """

    __metaclass__ = ABCMeta

    def __init__(self, timer):
        """
        Create timer multiplexer.

        @param timer: L{Timer} to multiplex.
        """
        self._timer = timer
        self._vtimers = []
        self._period = None
        self._inHandler = False
        timer.clear()
        timer.callback = self._timerHandler

    def _timerHandler(self):
        """
        Timer event handler.
        """

        self._inHandler = True
        for vtimer in self._vtimers:
            if vtimer._period is not None and not vtimer._done:
                vtimer._remaining -= self._period
                if vtimer._remaining <= 0:
                    if vtimer._repeat:
                        vtimer._remaining = vtimer._period
                    else:
                        vtimer._period = None
                    if vtimer.callback is not None:
                        vtimer.callback()
        self._inHandler = False

        for vtimer in self._vtimers:
            vtimer._done = False

        waits = [vtimer._remaining for vtimer in self._vtimers
                if vtimer._period is not None]

        if len(waits) > 0:
            self._period = min(waits)
            self._timer.start(self._period, self._timerHandler)
        else:
            self._period = None class InterSlaveSchedule(Schedule):
    """
    Transmission schedule including an auxillary slave-to-slave channel.
    """

    @prepend_method_doc(Schedule)
    def __init__(self, dataSlots, dataSlotTime, syncSlotTime, auxTxSlots, auxRxSlots, dataChannel=0, auxChannel=1):
        """
        @param auxTxSlots: List of device IDs indexed by allocated
            auxillary TX slot.
        @param auxRxSlots: List of device IDs indexed by allocated
            auxillary RX slot.
        @param dataChannel: Channel identifier of master-slave channel.
        @param auxChannel: Channel identifier of slave-slave channel.
        """
        Schedule.__init__(self, dataSlots, dataSlotTime, syncSlotTime)
        self.auxTxSlots = auxTxSlots
        self.auxRxSlots = auxRxSlots
        self.dataChannel = dataChannel
        self.auxChannel = auxChannel

    def _auxSlots(self, sequence, id):
        return list(np.nonzero(np.array(sequence)==id)[0])

    def slaveAuxTxSlots(self, id):
        return self._auxSlots(self.auxTxSlots, id)

    def slaveAuxRxSlots(self, id):
        return self._auxSlots(self.auxRxSlots, id)

    def auxSlot(self, source, dest):
        return np.nonzero((np.array(self.auxTxSlots)==source)
                & (np.array(self.auxRxSlots)==dest))[0][0] class MasterMAC(MAC):
    """
    Master MAC implementation.
    """

    @prepend_method_doc(MAC)
    def __init__(self, radio, timerMux, schedule, frameHandler=None):
        """
        @param schedule: L{Schedule} object giving transmission scheduling.
        @param frameHandler: Handler function to call at the end of each frame.
            The packets received in that frame will be passed as a list.
        """
        MAC.__init__(self, radio, timerMux)
        self.schedule = schedule
        self.frameHandler = frameHandler
        self.frameTimer = VirtualTimer(timerMux, self._frameTimerHandler)
        self.packets = []
        self.frameTimer.start(self.schedule.framePeriod, repeat=True)

    def _frameTimerHandler(self):
        if self.frameHandler is not None:
            self.frameHandler(self.packets)
        self.packets = []
        self.radio.transmit(SyncPacket())

    def queuePacket(self, packet):
        raise NotImplementedError

    def handleIncomingPacket(self, packet):
        if isinstance(packet, DataPacket):
            self.packets.append(packet) class InterSlaveMAC(SlaveMAC):
    """
    Slave MAC implementation with support for slave-slave packets.
    """

    @prepend_method_doc(SlaveMAC)
    def __init__(self, radio, timerMux, schedule, id, packetHandler=None):
        """
        @param packetHandler: Handler to call on reception of a slave-slave
            packet. The packet will be passed as an argument.
        """
        SlaveMAC.__init__(self, radio, timerMux, schedule, id)
        self.auxTxSlots = self.schedule.slaveAuxTxSlots(self.id)
        self.auxRxSlots = self.schedule.slaveAuxRxSlots(self.id)
        self.auxSlotTimer = VirtualTimer(timerMux, self._auxSlotTimerHandler)
        self.auxPackets = {}
        self.packetHandler = packetHandler
        self.radio.channel = schedule.dataChannel

    def _slotTimerHandler(self):
        SlaveMAC._slotTimerHandler(self)
        if self.slot == 0:
            self.auxSlot = 0
        else:
            self.auxSlot += 1
        if self.slot < self.schedule.dataSlots:
            self.auxSlotTimer.start(self.schedule.dataSlotTime/2)
            if self.auxSlot in self.auxTxSlots + self.auxRxSlots:
                self.radio.channel = self.schedule.auxChannel

    def _auxSlotTimerHandler(self):
        if self.auxSlot in self.auxTxSlots:
            packet = self.auxPackets.pop(self.auxSlot, None)
            if packet is not None:
                self.radio.transmit(packet)
        if self.slot + 1 in (self.dataTxSlot, len(self.schedule.dataSlots)):
            self.radio.channel = self.schedule.dataChannel

    def queueAuxPacket(self, packet):
        """
        Queue a packet for transmission to another slave.

        @param packet: L{RadioPacket} to transmit.
        """
        if isinstance(packet, AuxPacket):
            slot = self.schedule.auxSlot(packet.source, packet.dest)
            self.auxPackets[slot] = packet
        else:
            raise ValueError

    def handleIncomingPacket(self, packet):
        SlaveMAC.handleIncomingPacket(self, packet)
        if isinstance(packet, AuxPacket) and packet.dest == self.id:
            if self.packetHandler is not None:
                self.packetHandler(packet) class DataPacket(RadioPacket):
    """
    Data packet from slave to master.

    @ivar source: Source device ID.
    """
    def __init__(self, source, data):
        """
        Initialise data packet.
        @param source: Source device ID.
        @param data: Payload data.
        """
        RadioPacket.__init__(self, data)
        self.source = source

    @property
    def bytes(self):
        size = 2
        for key in self.keys():
            data = self[key]
            if isinstance(data, Quaternion):
                size += array(data.components).nbytes
            elif isinstance(data, numpy.ndarray):
                size += data.nbytes
        return size class AuxPacket(DataPacket):
    """
    Auxillary packet from slave to slave.

    @ivar source: Source device ID.
    @ivar dest: Destination device ID.
    """
    def __init__(self, source, dest, data):
        """
        Initialise auxillary packet.

        @param source: Source device ID.
        @param dest: Destination device ID.
        @param data: Payload data.
        """
        DataPacket.__init__(self, source, data)
        self.dest = dest class StaticTrajectory(ConstantPositionTrajectory, ConstantRotationTrajectory):
    """
    Represents the trajectory of a static object.
    """

    def __init__(self, position=None, rotation=None):
        """
        Construct static trajectory.

        @param position: the constant position of the object (3x1 L{np.ndarray})
        @param rotation: the constant rotation of the object (L{Quaternion})
        """
        ConstantPositionTrajectory.__init__(self, position)
        ConstantRotationTrajectory.__init__(self, rotation)

    @property
    def startTime(self):
        return 0

    @property
    def endTime(self):
        return np.inf class OffsetTrajectory(PositionTrajectory, RotationTrajectory):
    """
    Trajectory at an offset from another trajectory.

    @ivar parent: the L{AbstractTrajectory} from which this one inherits
    @ivar positionOffset: the offset from the parent trajectory in the parent
        local co-ordinate frame
    @ivar rotationOffset: the rotation offset from the parent trajectory
    """

    def __init__(self, parent, positionOffset=None, rotationOffset=None):
        """
        Initialise trajectory.

        @param parent: the L{AbstractTrajectory} which this trajectory follows.
        @param positionOffset: the offset vector from the parent trajectory in the
            co-ordinate frame of the parent. (3x1 L{np.ndarray})
        @param rotationOffset: the rotational offset from the parent trajectory
            (L{Quaternion})
        """
        self.parent = parent
        if positionOffset is None:
            self.positionOffset = np.zeros((3,1))
        else:
            self.positionOffset = positionOffset
        if rotationOffset is None:
            self.rotationOffset = Quaternion()
        else:
            self.rotationOffset = rotationOffset

    @CacheLastValue()
    def position(self, t):
        p = self.parent.position(t)
        r = self.parent.rotation(t)
        return p + r.rotateVector(self.positionOffset)

    @CacheLastValue()
    def velocity(self, t):
        v = self.parent.velocity(t)
        r = self.parent.rotation(t)
        o = r.rotateVector(self.positionOffset)
        omega = self.parent.rotationalVelocity(t)
        rv = vectors.cross(omega, o)
        return v + rv

    @CacheLastValue()
    @prepend_method_doc(PositionTrajectory)
    def acceleration(self, t):
        """
        Uses the algorithm from Young et al. 2010 'Distributed Estimation
        of Linear Acceleration for Improved Accuracy in Wireless Inertial
        Motion Capture' to calculate linear acceleration from rotational
        velocity and acceleration
        """
        a = self.parent.acceleration(t)
        r = self.parent.rotation(t)
        o = r.rotateVector(self.positionOffset)
        omega = self.parent.rotationalVelocity(t)
        alpha = self.parent.rotationalAcceleration(t)
        lt = vectors.cross(alpha, o)
        lr = vectors.dot(o, omega) * omega - o * vectors.norm(omega)**2
        return a + lt + lr

    @CacheLastValue()
    def rotation(self, t):
        return self.parent.rotation(t) * self.rotationOffset

    def rotationalVelocity(self, t):
        return self.parent.rotationalVelocity(t)

    def rotationalAcceleration(self, t):
        return self.parent.rotationalAcceleration(t)

    @property
    def startTime(self):
        return self.parent.startTime

    @property
    def endTime(self):
        return self.parent.endTime class IdealBasestation(Platform):
    """
    A simple basestation with ideal components.

    @ivar timer: The basestation L{Timer}.
    @ivar radio: The basestation L{Radio}
    """

    def __init__(self, simulation=None, trajectory=None):
        self.timer = IdealTimer(self)
        self.radio = IdealRadio(self)
        Platform.__init__(self, simulation, trajectory)

    @property
    def components(self):
        return [self.timer, self.radio] 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) class BodyModelReconstructor(object):
    """
    Reconstructs body model movements from individual joint data.

    This behaviour combines a L{PostureReconstructor} and
    a L{PositionEstimator} to update a L{SampledBodyModel}.

    @ivar bodyModel: L{SampledBodyModel} being updated.
    """

    def __init__(self, bodyModel, initialPosition=np.zeros((3,1)),
            postureReconstructor=SimpleForwardKinematics,
            positionEstimator=ConstantPosition, initialTime=0):
        """
        Initialise behaviour.

        @param bodyModel: L{SampledBodyModel} to update.
        @param postureReconstructor: L{PostureReconstructor} subclass to use.
        @param positionEstimator: L{PositionEstimator} subclass to use.
        @param initialTime: Initial time.
        """
        self.bodyModel = bodyModel
        self._postureEstimator = postureReconstructor(self.bodyModel)
        self._positionEstimator = positionEstimator(self.bodyModel,
                initialPosition = initialPosition)
        self._time = initialTime

    def handleData(self, data, dt):
        """
        Update the body model with new data.

        @param data: A L{dict} containing data, passed directly to both the
            posture and position estimators.
        @param dt: Time elapsed since last call to this method or __init__.
        """
        self._time += dt
        self._postureEstimator(data, self._time)
        self._positionEstimator(data, self._time) class TimeSeries(object):
    """
    A series of values taken at consecutive points in time.

    The values may be scalars, column vectors, or L{Quaternion} objects.

    Each value may have a (co)variance associated with it.

    Timestamps may be irregularly spaced but must be monotonically increasing.
    """

    def __init__(self, timestamps=None, values=None, variances=None):
        """
        Construct time series.

        A time series can be constructed empty, or with initial values if
        passed to the contstructor.

        @param timestamps: Times at which values were taken.
        @param values: Either a length N array of scalars or other data types,
            an MxN column vector array, or a length N L{QuaternionArray}.
        @param variances : Variances, either a length N sequence of variances
            for scalar data, a NxMxM sequence of covariances for MxN column
            vector data, or an Nx4x4 sequence of covariances for quaternion
            data.
        """
        if (timestamps is None) != (values is None):
            raise ValueError, "Both or neither of timestamps and values must be provided"
        self._dtype = None
        self._timestampsArray = None
        self._valuesArray = None
        self._variancesArray = None
        if timestamps is not None:
            assert all(np.diff(timestamps) > 0), "Timestamps must be in order"
            if len(np.shape(values)) == 1:
                values = list(np.asarray(values))
            else:
                values = list(np.asarray(np.hsplit(values, values.shape[-1])))
            if variances is not None:
                [self.add(t,v,V) for t,v,V in zip(timestamps, values, variances)]
            else:
                [self.add(t,v) for t,v in zip(timestamps, values)]

    def __getstate__(self):
        if self._hasVariances:
            return (self.timestamps, self.values, self.variances)
        else:
            return (self.timestamps, self.values, None)

    def __setstate__(self, state):
        self.__init__(*state)

    def __len__(self):
        if self._dtype is None:
            return 0
        else:
            return len(self._timestamps)

    def _initTypes(self, value):
        self._dtype = type(value)
        self._dshape = np.shape(value)
        if isinstance(value, np.ndarray):
            value = np.asarray(value)
            self._dims = value.shape[0]
        elif isinstance(value, Quaternion):
            self._dims = 4
        else:
            self._dims = 1

    def _varianceShape(self):
        if self._dims == 1:
            return ()
        else:
            return (self._dims, self._dims)

    @property
    def dtype(self):
        """
        Type of data in this time series.
        """
        return self._dtype

    @property
    def timestamps(self):
        """
        Array of times at which the values were taken.
        """
        if self._timestampsArray is None:
            self._timestampsArray = np.array(self._timestamps)
        return self._timestampsArray

    def _makeValuesArray(self, values):
        if self.dtype is Quaternion:
            return QuaternionArray(values)
        elif issubclass(self._dtype, np.ndarray) and len(self._dshape) == 2 and self._dshape[1] == 1:
            return np.hstack(values)
        else:
            return np.array(values)

    @property
    def values(self):
        """
        Array of values of the time series in chronological order.

        The returned array has shape (N) for scalars, shape (n, N) for
        vectors, or is a L{QuaternionArray} of length N for quaternions,
        where N is the number of values.
        """
        if self._valuesArray is None:
            self._valuesArray = self._makeValuesArray(self._values)
        return self._valuesArray

    @property
    def variances(self):
        """
        The (co)variances of the values in chronological order.

        The returned array has shape (N) if the values are scalar,
        (N, n, n) for vectors, or (N, 4, 4) for quaternions.
        """
        if not self._hasVariances:
            shape = tuple([len(self)] + list(self._varianceShape()))
            variances = np.empty(shape)
            variances[:] = np.nan
            return variances
        if self._variancesArray is None:
            self._variancesArray = np.array(self._variances)
        return self._variancesArray

    def __call__(self, t, returnVariance=False):
        """
        Obtain the closest previous value to time t.

        If returnVariance is True then the value and variance are returned.
        """
        indices = np.searchsorted(self.timestamps, t, 'right') - 1
        if issubclass(self._dtype, np.ndarray) and len(self._dshape) == 2 and self._dshape[1] == 1:
            values = self.values[:,np.atleast_1d(indices)]
        else:
            values = self.values[indices]
        if not returnVariance:
            return values
        else:
            if not self._hasVariances:
                raise ValueError, "This time series has no variance data."
            return values, np.array(self._variances[indices])

    def __iter__(self):
        return itertools.izip(self._timestamps, self._values, self._variances)

    def add(self, time, value, variance=None):
        """
        Add a timestamped value to the data.

        @param time: Time at which the value was taken.
        @param value: Value to store. The value must support the __copy__
            operation as used by the copy module.
        @param variance: (Co)variance of the value. The variance must support
            the __copy__ operation as used by the copy module.
        """
        if isinstance(value, np.matrix):
            value = np.asarray(value)
        if np.shape(value) == (1,):
            value = value[0]
        if self._dtype is None:
            self._initTypes(value)
            self._timestamps = [time]
            self._values = [value]
            if variance is not None:
                self._hasVariances = True
                assert np.shape(variance) == self._varianceShape(), \
                        "Variance shape must match series dimensions"
                self._variances = [variance]
            else:
                self._hasVariances = False
        else:
            assert time > self.latestTime, \
                    "Time must be after existing entries"
            self._timestamps.append(copy(time))
            assert type(value) == self._dtype, \
                    "Data type must match series data type"
            assert np.shape(value) == self._dshape, \
                    "Data shape must match series data shape"
            if self._dims > 1 and self._dtype != Quaternion:
                value = np.asarray(value)
            self._values.append(copy(value))
            if self._hasVariances:
                assert np.shape(variance) == self._varianceShape(), \
                        "Variance shape must match series dimensions"
                if self._dims > 1:
                    variance = np.asarray(variance)
                self._variances.append(copy(variance))

        self._latestTime = time
        self._latestValue = value
        self._timestampsArray = None
        self._valuesArray = None
        self._variancesArray = None

    @property
    def earliestTime(self):
        """ The earliest time stamp in this time series. """
        return self._timestamps[0]

    @property
    def earliestValue(self):
        """ The earliest value in this time series. """
        return self._values[0]

    @property
    def latestTime(self):
        """ The latest time stamp in this time series. """
        return self._timestamps[-1]

    @property
    def latestValue(self):
        """ The latest value in this time series. """
        return self._values[-1]
from imusim.simulation.base import Simulation
from imusim.io.bvh import loadBVHFile
from imusim.algorithms.orientation import DistLinAccelCF
from imusim.platforms.imus import IdealIMU
from imusim.simulation.calibrators import ScaleAndOffsetCalibrator
from imusim.behaviours.imu import BasicIMUBehaviour
from imusim.behaviours.timing import TimerMultiplexer
from imusim.behaviours.orient_tdma import InterSlaveSchedule
from imusim.behaviours.orient_tdma import MasterMAC
from imusim.behaviours.orient_tdma import InterSlaveMAC
from imusim.behaviours.orient_tdma import DataPacket
from imusim.behaviours.orient_tdma import AuxPacket
from imusim.trajectories.base import StaticTrajectory
from imusim.trajectories.offset import OffsetTrajectory
from imusim.platforms.basestations import IdealBasestation
from imusim.trajectories.rigid_body import SampledBodyModel
from imusim.trajectories.rigid_body import SplinedBodyModel
from imusim.behaviours.basestation import BodyModelReconstructor
from imusim.testing.quaternions import assert_quaternions_correlated
from imusim.testing.random_data import randomPosition
from imusim.utilities.time_series import TimeSeries
from os import path
import numpy as np
"""
Test of simulation with distributed linear acceleration estimator.
"""
# 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 testDistLinAccSimulation():
    sim = Simulation()
    samplingPeriod = 1.0/100

    calibrator = ScaleAndOffsetCalibrator(sim.environment, 1000,