import pybullet as p
import time
import pybullet_data

p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)

terrain_mass = 0
terrain_visual_shape_id = -1
terrain_position = [0, 0, 0]
terrain_orientation = [0, 0, 0, 1]
terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
                                                    fileName="terrain.obj",
                                                    flags=p.GEOM_FORCE_CONCAVE_TRIMESH |
                                                    p.GEOM_CONCAVE_INTERNAL_EDGE,
                                                    meshScale=[0.5, 0.5, 0.5])
p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
                  terrain_position, terrain_orientation)

useMaximalCoordinates = True
sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,
                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])

mass = 1
visualShapeId = -1

for i in range(5):
  for j in range(5):
    for k in range(5):
      #if (k&2):
      sphereUid = p.createMultiBody(
          mass,
          colSphereId,
          visualShapeId, [-i * 5 * sphereRadius, j * 5 * sphereRadius, k * 2 * sphereRadius + 1],
          useMaximalCoordinates=useMaximalCoordinates)
      #else:
      #	sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
      p.changeDynamics(sphereUid,
                       -1,
                       spinningFriction=0.001,
                       rollingFriction=0.001,
                       linearDamping=0.0)
      p.changeDynamics(sphereUid, -1, ccdSweptSphereRadius=0.002)

p.setGravity(0, 0, -10)

pts = p.getContactPoints()
print("num points=", len(pts))
print(pts)
while (p.isConnected()):
  time.sleep(1. / 240.)
  p.stepSimulation()