import simbios.simtk as simtk

def main():
    # 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))
    lastBody = matter.getGround().getMobilizedBodyIndex()
    for i in range(10):
        pendulum = simtk.MobilizedBody.Ball(
            matter.updMobilizedBody(lastBody), 
            simtk.Transform((0, 0, 0)), 
            pendulumBody, 
            simtk.Transform((0, 1, 0)))
        lastBody = pendulum.getMobilizedBodyIndex()
    system.defaultSubsystem.addEventReporter(simtk.VTKEventReporter(system, 0.02))

    # Initialize the system and state.
    system.realizeTopology()
    state = system.getDefaultState()
    random = simtk.Random.Gaussian()
    for i in range(state.getNQ()):
        state.q[i] = random.getValue()

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

if __name__ == '__main__':
    main()