import pybullet as p
import pybullet_data as pd
import time
import math

usePhysX = True
useMaximalCoordinates = True
if usePhysX:
  p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
  p.loadPlugin("eglRendererPlugin")
else:
  p.connect(p.GUI)

p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)

p.setAdditionalSearchPath(pd.getDataPath())
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71

p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100

num=64
radius=0.1
numDominoes=0

for i in range (int(num*50)):
  num=(radius*2*math.pi)/0.08
  radius += 0.05/float(num)
  orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)])
  pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03]
  sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
  numDominoes+=1
  
pos=[pos[0],pos[1],pos[2]+0.3]
orn = p.getQuaternionFromEuler([0,0,-math.pi/4.])
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)

print("numDominoes=",numDominoes)


#for j in range (20):
#    for i in range (100):
#        if (i<99):
#          sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
#        else:
#          orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
#          sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
    
    
print("loaded!")


#p.changeDynamics(sphere ,-1, mass=1000)

door = p.loadURDF("door.urdf",[0,0,-11])
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door))


p.setGravity(0,0,-10)
position_control = True

angle = math.pi*0.25
p.resetJointState(door,1,angle)  
angleread = p.getJointState(door,1)
print("angleread = ",angleread)
prevTime = time.time()

angle = math.pi*0.5

count=0
while (1):
  count+=1
  if (count==12):
      p.stopStateLogging(logId)
      p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)

  curTime = time.time()
  
  diff = curTime - prevTime
  #every second, swap target angle
  if (diff>1):
    angle = - angle
    prevTime = curTime
  
  if position_control:
    p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001)
  else:  
    p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
  #contacts = p.getContactPoints()
  #print("contacts=",contacts)
  p.stepSimulation()
  #time.sleep(1./240.)
