import ode, time

class Physics:
	def __init__(self):
		# Create a world object
		self.world = ode.World()
		self.world.setGravity((0, -9.81, 0))
		#self.world.setERP(0.8)
		#self.world.setCFM(1E-5)
		
		# Create a space object
		self.space = ode.Space()
		
		# Create a plane geom which prevent the objects from falling forever
		self.floor = ode.GeomPlane(self.space, (0, 1, 0), 0)
		
		# A joint group for the contact joints that are generated whenever
		# two bodies collide
		self.contactgroup = ode.JointGroup()
		
		# set lasttime variable to zero
		self.lasttime = time.time()
	
	# Collision callback
	def CollisionCallback(self, args, geom1, geom2):
		"""
		Callback function for the collide() method.
		
		This function checks if the given geoms do collide and
		creates contact joints if they do.
		"""
	
		# Check if the objects do collide
		contacts = ode.collide(geom1, geom2)
		
		# loop over any contacts yay
		for c in contacts:		
			bodies = [geom1.getBody(), geom2.getBody()]
			friction = [2500, 2500]
			
			c.setBounce(0.9)
			c.setMu(friction[0] + friction[1])
			j = ode.ContactJoint(self.world, self.contactgroup, c)
			
			j.attach(bodies[0], bodies[1])
	
	def Step(self, size = 0):
		for i in range(2):
			# Detect collisions and create contact joints
			self.space.collide((), self.CollisionCallback)
		
		if size:
			self.world.quickStep(size)
		else:
			self.world.quickStep(time.time() - self.lasttime)
		
		self.lasttime = time.time()
		
		# Remove all contact joints
		self.contactgroup.empty()
	
	# add something to the physics world
	def MakeBody(self, position, density, size, gravity, shape):
		# create a new mass
		M = ode.Mass()
	
		# Create a box geom for collision detection
		if (shape == "sphere"):
			geom = ode.GeomSphere(self.space, radius=size)
			M.setSphere(density, size)
		elif (shape == "box"):
			geom = ode.GeomBox(self.space, lengths=size[:])
			M.setBox(density, size[0], size[1], size[2])
		
		body = ode.Body(self.world)
		body.setMass(M)
		body.setGravityMode(gravity)
		body.setPosition(position)
		
		geom.setBody(body)
		return geom.getBody()
	
	def MakePlane(self, orientation, position):
		# Create a plane geom which prevent the objects from falling forever
		self.floor = ode.GeomPlane(self.space, orientation, position)
	
	def MakeGeom(self, position, size, gravity, shape):
		# Create a box geom for collision detection
		if (shape == "sphere"):
			geom = ode.GeomSphere(self.space, radius=size)
		elif (shape == "box"):
			geom = ode.GeomBox(self.space, lengths=size[:])
		
		return geom

if __name__ == '__main__':
	from Physics import Physics
	from Gfx2d import gfx
	from pygame import time, draw
	
	c = time.Clock()
	p = Physics()
	a = p.MakeBody([0.5, 0.5, 0], 10, 0.01, 1, "sphere")
	b = p.MakeBody([0.51, 0.8, 0], 10, 0.01, 1, "sphere")
	
	while 1:
	        p.Step()
	        gfx.screen.fill([0, 0, 0])
	        draw.circle(gfx.screen, [255, 255, 255], [int(v * gfx.width) for v in a.getPosition()[:-1]], int(0.01 * gfx.width))
	        draw.circle(gfx.screen, [255, 255, 255], [int(v * gfx.width) for v in b.getPosition()[:-1]], int(0.01 * gfx.width))
	        c.tick(30)
	        gfx.Flip()
	
	def draw_body(body):
	        x,y,z = body.getPosition()
	        R = body.getRotation()
	
	        rot = [R[0], R[3], R[6], 0.,
	        R[1], R[4], R[7], 0.,
	        R[2], R[5], R[8], 0.,
	        x, y, z, 1.0]
	
	        if body.shape=="box":
	                sx,sy,sz = body.boxsize