import pybullet as p
import time
import math

p.connect(p.GUI)
useMaximalCoordinates = False

p.setGravity(0,0,-10)
plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates)

p.setRealTimeSimulation(0)


velocity = 1
num = 40
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.setPhysicsEngineParameter(enableConeFriction=1)

for i in range (num):
	print("progress:",i,num)
	
	x = velocity*math.sin(2.*3.1415*float(i)/num)
	y = velocity*math.cos(2.*3.1415*float(i)/num)
	print("velocity=",x,y)
	sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates)
	p.changeDynamics(sphere,-1,lateralFriction=0.02)
	#p.changeDynamics(sphere,-1,rollingFriction=10)
	p.changeDynamics(sphere,-1,linearDamping=0)
	p.changeDynamics(sphere,-1,angularDamping=0)
	p.resetBaseVelocity(sphere,linearVelocity=[x,y,0])

	prevPos = [0,0,0]
	for i in range (2048):
		p.stepSimulation()
		pos = p.getBasePositionAndOrientation(sphere)[0]
		if (i&64):
			p.addUserDebugLine(prevPos,pos,[1,0,0],1)
			prevPos = pos

p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)

	
while (1):
	time.sleep(0.01)