import simbios.simtk as simtk
import sys

class PositionReporter (simtk.TriggeredEventReporter):
    def __init__(self, system, body):
        # It's important to call the constructor of the parent class
        super(PositionReporter, self).__init__(simtk.Stage.Velocity)
        self.system = system
        self.body = body
        self.getTriggerInfo().setTriggerOnRisingSignTransition(False)

    def getValue(self, state):
        vel = self.body.getBodyOriginVelocity(state)
        return vel[1]

    def handleEvent(self, state):
        self.system.realize(state, simtk.Stage.Position)
        pos = self.body.getBodyOriginLocation(state)
        print "%f\t%f\t%f" % (state.time, pos[0], pos[1])
        sys.stdout.flush()

# Create the system.
system = simtk.MultibodySystem()
matter = simtk.SimbodyMatterSubsystem(system)
forces = simtk.GeneralForceSubsystem(system)
gravity = simtk.Force.UniformGravity(forces, matter, (0, -9.8, 0))
pendulumBody = simtk.Body.Rigid(simtk.MassProperties(1.0, (0,0,0), simtk.Inertia(1)))
pendulumBody.addDecoration(simtk.Transform(), simtk.DecorativeSphere(0.1))
pendulum = simtk.MobilizedBody.Pin(matter.ground, simtk.Transform((0,0,0)), pendulumBody, simtk.Transform((0, 1, 0)))
system.defaultSubsystem.addEventReporter(simtk.VTKEventReporter(system, 0.01))
# PositionReporter must be constructed outside of method call
reporter = PositionReporter(system, pendulum)
system.defaultSubsystem.addEventReporter(reporter)

# Initialize the system and state.
system.realizeTopology()
state = system.defaultState
pendulum.setOneU(state, 0, 2.0)

# Simulate it.
integ = simtk.RungeKuttaMersonIntegrator(system)
ts = simtk.TimeStepper(system, integ)
ts.initialize(state)
ts.stepTo(5.0)