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))
    pendulum1 = simtk.MobilizedBody.Pin(matter.ground, 
            simtk.Transform((0,0,0)), 
            pendulumBody, 
            simtk.Transform((0, 1, 0)))
    pendulum2 = simtk.MobilizedBody.Pin(pendulum1, 
            simtk.Transform((0,0,0)), 
            pendulumBody, 
            simtk.Transform((0, 1, 0)))
    pointOnLine = simtk.Constraint.PointOnLine(matter.ground, 
            simtk.UnitVec3(0, 1, 0), 
            (0,0,0), 
            pendulum2, 
            (0,0,0) )
    system.defaultSubsystem.addEventReporter(simtk.VTKEventReporter(system, 0.01))

    # Initialize the system and state.
    system.realizeTopology()
    state = system.defaultState
    pendulum2.setOneU(state, 0, 5.0)

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

if __name__ == '__main__':
    main()