from simbios.simtk import *
import math

def main():
    
    # Create the system.
    system = MultibodySystem()
    matter = SimbodyMatterSubsystem(system)
    gearBody = Body.Rigid(MassProperties(1.0, Vec3(0), Inertia(1)))
    gearBody.addDecoration(Transform(Rotation(0.5*math.pi, CoordinateAxis.XCoordinateAxis())), DecorativeCylinder(1.0, 0.1))
    gear1 = MobilizedBody.Pin(matter.updGround(), Transform(Vec3(1, 0, 0)), gearBody, Transform())
    gear2 = MobilizedBody.Pin(matter.updGround(), Transform(Vec3(-1, 0, 0)), gearBody, Transform())
    rodBody = Body.Rigid(MassProperties(1.0, Vec3(0), Inertia(1)))
    rodBody.addDecoration(Transform(Vec3(0, 1, 0)), DecorativeCylinder(0.05, 1.0))
    rod = MobilizedBody.Pin(gear2, Transform(Vec3(0, 0.8, 0.1)), rodBody, Transform())
    Constraint.ConstantSpeed(gear1, 0.1)
    Constraint.NoSlip1D(matter.updGround(), Vec3(0), UnitVec3(0, 1, 0), gear1, gear2)
    Constraint.PointOnLine(matter.updGround(), UnitVec3(0, 1, 0), Vec3(0, 0, 0.1), rod, Vec3(0, 2, 0))
    system.updDefaultSubsystem().addEventReporter(VTKEventReporter(system, 0.05))
    
    # Initialize the system and state.
    system.realizeTopology()
    state = system.getDefaultState()
    
    # Simulate it.
    integ = RungeKuttaMersonIntegrator(system)
    ts = TimeStepper(system, integ)
    ts.initialize(state)
    ts.stepTo(50.0)

if __name__ == "__main__":
    main()