#!C:/Python26/python.exe
# Based on TestPendulum.cpp SimTK example program

import sys
import simtk.molmodel as ss

class PositionReporter(ss.PeriodicEventReporter):

    def __init__(self, system, body, interval):
        super(PositionReporter, self).__init__(interval)
        self.system = system
        self.body = body

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

# Create the system. 
 
system = ss.MultibodySystem()
matter = ss.SimbodyMatterSubsystem(system) 
forces = ss.GeneralForceSubsystem(system) 
gravity = ss.Force.UniformGravity(forces, matter, (0, -9.8, 0)) 
pendulumBody = ss.Body.Rigid(ss.MassProperties(1.0, (0,0,0), ss.Inertia(1))) 
pendulumBody.addDecoration(ss.Transform(), ss.DecorativeSphere(0.1)) 
pendulum1 = ss.MobilizedBody.Pin(matter.Ground(), ss.Transform((0,0,0)), 
        pendulumBody, ss.Transform((0, 1, 0))) 
pendulum2 = ss.MobilizedBody.Pin(pendulum1, ss.Transform((0,0,0)), 
        pendulumBody, ss.Transform((0, 1, 0)))
        
vtk_reporter = ss.VTKEventReporter(system, 0.01)
system.updDefaultSubsystem().addEventReporter(vtk_reporter) 

# Does not work if I construct PositionReporter in place
# Dies with "TypeError: 'NoneType' object is not callable"
# system.defaultSubsystem().addEventReporter(PositionReporter(system, pendulum2, 0.10)) 

reporter = PositionReporter(system, pendulum2, 0.03)
system.updDefaultSubsystem().addEventReporter(reporter) 
 
# Initialize the system and state. 
 
system.realizeTopology()   
state = system.defaultState
pendulum2.setOneU(state, 0, 5.0) 
 
# Simulate it. 
 
integ = ss.VerletIntegrator(system) 
ts = ss.TimeStepper(system, integ) 
ts.initialize(state) 

# print "About to simulate..."
ts.stepTo(0.3) 
# print "Simulation complete"