import pygame
from pygame.locals import *
import ode
import math
import random
import time

def draw():
	srf.fill((255,255,255))
	for c in connections:
		draw_connection(c)
	for b in bodies:
		if b.isEnabled():
			draw_body(b)	
	pygame.display.flip()
		
def coord(x,y):
    "Convert world coordinates to pixel coordinates."
    return int(zoom*x), int(-(zoom*y))

def decoord((x,y)):
	"Convert pixel coordinates to world coordinates"
	return int(x/zoom), int(-(y/zoom))
	
def create_ball(density, radius, (x,y)):
	global bodies, counter, objcount
	global lastbody
	body=ode.Body(world)
	lastbody=body
	M=ode.Mass()
	M.setSphere(density, radius)
	body.setMass(M)
	body.shape = "sphere"
	body.radius=radius
	body.color = (random.randint(0,255),random.randint(0, 255),random.randint(0, 255))
	body.setPosition( (x,y,0) )
        bodies.append(body)
        counter=0
        objcount+=1
	#print "ball created"
	return body

def draw_body(body):
	x,y,z = body.getPosition();
	pygame.draw.circle(srf, body.color, coord(x,y), body.radius*zoom*ballzoom, 0)
	
def draw_connection((joint, body)):
	x1,y1,z1=joint.getAnchor()
	x2,y2,z2=body.getPosition()
	draw_line(x1,y1,x2,y2)
def draw_jconn(((x1,y1),body)):
	x2,y2,z2=body.getPosition()
	draw_line(x1,y1,x2,y2)
def draw_line(x1,y1,x2,y2):
	pygame.draw.line(srf, (0,0,0), coord(x1,y1), coord(x2,y2), 2)	
	pygame.draw.circle(srf,(0,0,0), coord(x1,y1), 5, 0)
	pygame.draw.circle(srf,(0,0,0), coord(x2,y2), 5, 0)

def attach_env(body, (x,y)):
	z=0
	joint=ode.BallJoint(world)
	joint.attach(body, ode.environment)
	joint.setAnchor((x,y,z))
	joints.append(joint)
	connections.append((joint, body))
	jconn.append(((x,y),body))
	lastjoint=joint
	#print joint
	return joint

def attach_bod(body1, body2):
	z=0
	joint=ode.BallJoint(world)
	joint.attach(body2, body1)
	joint.setAnchor(body1.getPosition())
	joints.append(joint)
	connections.append((joint, body2))
	lastjoint=joint
	#print joint
	return joint

def ball_near(x,y):
	for b in bodies:
		bx,by,bz=b.getPosition()
		dist = math.sqrt((math.fabs(x-bx))**2+(math.fabs(y+by)**2))
		print (x,y), (bx,by)
		if dist<mindis:
			return b
	
def handle_events():
	events=pygame.event.get()
	for e in events:
		if e.type==QUIT:
			loopFlag=False
		if e.type==KEYDOWN: 
			if e.key==112:
				pause()
			elif e.key==122:
				lastbody.disable()
			elif e.key==270:
				inc_gravity(0.2)
			elif e.key==269:
				inc_gravity(-0.2)
			elif e.key==268:
				inc_worldsteps(10)
			elif e.key==267:
				inc_worldsteps(-10)
			#print e.key
			
			
		if e.type==MOUSEBUTTONDOWN:
			if e.button==3:
				startposR=e.pos
				mx,my=decoord(startposR)
				kugla=False
				kugla2=False
				wait=True
				for b in bodies:
					bx,by,bz=b.getPosition()
					distance = math.sqrt((math.fabs(mx-bx))**2+(math.fabs(my-by)**2))
					print (mx,my), (bx,by), distance
					if distance<mindis:
						kugla=True
						body1=b
				while wait:
					for e in events:
						if e.type==MOUSEBUTTONUP and e.button==3:
							endposR=e.pos
							mx,my=decoord(endposR)
							for b in bodies:
								bx,by,bz=b.getPosition()
								distance = math.sqrt((math.fabs(mx-bx))**2+(math.fabs(my-by)**2))
								if distance<mindis:
									kugla2=True
									body2=b	
							if kugla2==False:
								body2=create_ball(2500, 0.1, decoord(endposR))
							if kugla and body1!=body2:
								attach_bod(body1, body2)
							else:
								attach_env(body2, decoord(startposR))
							wait=False
							kugla=False
							kugla2=False
					events=pygame.event.get()
	
			elif e.button==1:
				startposL=e.pos
				mx,my=decoord(startposL)
				wait=True
				kugla=False
				for b in bodies:
					bx,by,bz=b.getPosition()
					distance = math.sqrt((math.fabs(mx-bx))**2+(math.fabs(my-by)**2))
					print (mx,my), (bx,by)
					if distance<mindis:
						kugla=True
						body1=b
				while wait and kugla:
					for e in events:
						if e.type==MOUSEBUTTONUP and e.button==1:
							endposL=e.pos
							m1x,m1y=decoord(endposL)
							body1.addForce( ((m1x-mx)*100000,(m1y-my)*100000,0) )
							print m1x,m1y
							wait=False
					events=pygame.event.get()


def debug():
	for j in joints:
		return j," ",j.getFeedback()
def damp():
	for b in bodies:
		dx,dy,dz=b.getForce()
		dx*=dampratio
		dy*=dampratio
		b.addForce((dx,dy,dz))
def pause():
	paused=True
	while paused:
		handle_events()
		clk.tick()
		draw()
		events=pygame.event.get()
		for e in events:
			if e.type==KEYDOWN and e.key==112:
				paused=False	
	
def change_energy(ratio):
	for b in bodies:
		fx,fy,fz=b.getForce()
		fx=fx*ratio
		fy=fy*ratio
		fz=fz*ratio
		b.addForce((fx,fy,fz))

def inc_gravity(factor):
	gx, gy, gz=world.getGravity()
	gy-=factor
	world.setGravity((gx,gy,gz))
	print world.getGravity()
def inc_worldsteps(factor):
	global worldsteps
	worldsteps+=factor
	print worldsteps
# Initialize pygame
pygame.init()

# Open a display
srf = pygame.display.set_mode((1024,768))

# Create a world object
world = ode.World()
world.setGravity((0,-9.81,0))
zoom=20
ballzoom=10
bodies=[]
joints=[]
connections=[]
jconn=[]
objcount=0
mindis=zoom/ballzoom+0.2
dampratio=0.00000001
worldsteps=75
global lastbody

fps = 50
dt = 1.0/fps

loopFlag = True
clk = pygame.time.Clock()
while loopFlag:
    handle_events()
    # Next simulation step
    clk.tick(fps)
    for i in range(worldsteps):
    	world.step(dt/worldsteps)
    #print debug()
    draw()