''
''	Example of how to make a local gravity entity
''

#include "../easyChipmunk.bi"
#include "fbgfx.bi"
using fb

#define GRABABLE_MASK_BIT (1 shl 31)
#define NOT_GRABABLE_MASK (not GRABABLE_MASK_BIT)

type myPlanet
	body				as cpBody ptr
	shape				as cpShape ptr
	gravityStrength		as cpFloat
end type

type shapeData
	shape					as cpShape ptr
	body					as cpBody ptr
	colour					as uinteger
end type

declare sub setupScreen( byval w as integer, byval h as integer, byval instance as easyChipmunk ptr )
declare function generateShapeData( list() as shapeData, byval shape as cpShape ptr, byval colour as uinteger ) as integer
declare function createNewCircle( list() as shapeData, byval mass as cpFloat, byval radius as cpFloat, byval colour as uinteger, byval isStatic as ubyte = 0, byval offset as cpVect = cpvzero ) as cpShape ptr
declare function createNewPolygon( list() as shapeData, byval mass as cpFloat, byval vertices as cpVect ptr, byval nVertices as integer, byval colour as uinteger, byval isStatic as ubyte = 0, byval offset as cpVect = cpvzero ) as cpShape ptr
declare function createNewPolygon2( list() as shapeData, byval mass as cpFloat, byval nVertices as integer, byval radius as cpFloat, byval colour as uinteger, byval isStatic as ubyte = 0, byval offset as cpVect = cpvzero ) as cpShape ptr
declare sub removeShape( list() as shapeData, byval instance as easyChipmunk ptr, byval shape as cpShape ptr )
declare sub addShapeToSpace( list() as shapeData, byval instance as easyChipmunk ptr, byval shape as cpShape ptr )
declare sub drawShapeList( list() as shapeData )

dim shared planet as myPlanet

' Update function for local gravity - see chipmunk's official demos, specifically planet.c for more information
sub planetGravityVelocityFunc cdecl( byval body as cpBody ptr, byval gravity as cpVect, byval damping as cpFloat, byval dt as cpFloat )
	' Offset body
	var p = cpvsub( body->p, planet.body->p )
	var sqdist = cpvlengthsq( p )
	var g = cpvadd( cpvmult( p, -planet.gravityStrength / ( sqdist * cpfsqrt( sqdist ) ) ), gravity )
	
	cpBodyUpdateVelocity( body, g, damping, dt )
end sub

' Not a necessary function, but is good to implement in the long run
sub updateFunc( byval ec as any ptr )
	dim physics as easyChipmunk ptr = cptr( easyChipmunk ptr, ec )
	dim planet as myPlanet ptr = cptr( myPlanet ptr, physics->userData )
	cpBodyUpdatePosition( planet->body, physics->deltaTime )
end sub

dim physics as easyChipmunk = easyChipmunk()
redim shapes(0) as shapeData

' Create a planet
planet.body = cpBodyNew( INFINITY, INFINITY )
planet.body->w = 0.2
planet.body->p = cpv( 400, 300 )
planet.shape = cpCircleShapeNew( planet.body, 70.0, cpvzero )
planet.shape->e = 1.0
planet.shape->u = 1.0
planet.shape->layers = NOT_GRABABLE_MASK
planet.gravityStrength = 1000000

physics.lock()
	setupScreen( 800, 600, @physics )
	physics.space->gravity = cpv( 0, 20 )
	physics.space->iterations = 20
	physics.space->sleepTimeThreshold = 0.05
	physics.space->idleSpeedThreshold = 0
	physics.onUpdate = cptr( ecUpdateFunc, @updateFunc )			' * Set update function
	planet.shape = cpSpaceAddShape( physics.space, planet.shape )	' * Add planet shape to simulation
	physics.userData = @planet										' * Set the planet as the user data
	physics.pause = 0
physics.unlock()

dim as integer mx, my, mb
dim as double spawnTimer = Timer
dim as cpConstraint ptr mouseSpring = NULL
dim as cpBody ptr mousePointer = cpBodyNew( INFINITY, INFINITY )
dim as integer frameTimer = Timer + 1
dim as integer framecount = 0, fps = 0, shapecount = 0
dim as double deleteTimer = Timer

do
	physics.simulate()
	physics.lock()
		screenlock()
			cls
			locate 1, 2: print "Left click to move around objects"
			locate 2, 2: print "Right click to spawn a random object"
			locate 3, 2: print "Right click + Ctrl to remove an object"
			locate 4, 2 : Print Using "Simulating #### shapes"; shapecount
			locate 1, LoWord( width() ) - 9 : print using "##### fps"; fps
			drawShapeList( shapes() )
			if mouseSpring <> NULL then
				line( mouseSpring->a->p.x, mouseSpring->a->p.y )-( mouseSpring->b->p.x, mouseSpring->b->p.y ), rgb( 200, 200, 255 )
			end if
			circle( planet.body->p.x, planet.body->p.y ), cpCircleShapeGetRadius( planet.shape ), rgba( 255, 0, 0, 255 )
			paint( planet.body->p.x, planet.body->p.y ), rgba( 255, 200, 200, 100 ), rgba( 255, 0, 0, 255 )
		screenunlock()
		getmouse mx, my, , mb
		if mb = -1 then mb = 0
		if ( mx <> -1 ) and ( my <> -1 ) then mousePointer->p = cpv( mx, my )
		if ( mb And 1 ) then
			if mouseSpring = NULL then
				var query = cpSpacePointQueryFirst( physics.space, mousePointer->p, GRABABLE_MASK_BIT, 0 )
				if query <> NULL then
					mouseSpring = cpDampedSpringNew( mousePointer, query->body, cpvzero, cpBodyWorld2Local( query->body, mousePointer->p ), 1.0, 100.0, 2.0 )
					mouseSpring->biasCoef = 1.0
					mouseSpring->maxForce = INFINITY
					cpSpaceAddConstraint( physics.space, mouseSpring )
				end if
			end if
		else
			if mouseSpring <> NULL then
				cpSpaceRemoveConstraint( physics.space, mouseSpring )
				cpConstraintFree( mouseSpring )
				mouseSpring = NULL
			end if
		end if
		if (mb And 2 ) then
			if multikey( SC_CONTROL ) then
				var query = cpSpacePointQueryFirst( physics.space, mousePointer->p, GRABABLE_MASK_BIT, 0 )
				if query <> NULL then
					removeShape( shapes(), @physics, query )
				end if
			else
				if ( Timer > spawnTimer ) And ( ( mousePointer->p.x > 0 ) And ( mousePointer->p.y > 0 ) And ( mousePointer->p.x < 800 ) And ( mousePointer->p.y < 600 ) ) then
					spawnTimer = Timer + 0.05
					var shape = createNewCircle( shapes(), 10, 10.0, rgb( 100 + int(rnd*150), 100 + int(rnd*150), 100 + int(rnd*150) ) )
					'var shape = createNewPolygon2( shapes(), 10.0, 3 + int(rnd*7), 10.0, rgb( 100 + int(rnd*150), 100 + int(rnd*150), 100 + int(rnd*150) ) )
					shape->e = 0.9
					shape->u = 5.0
					shape->body->p = mousePointer->p
					var pull = cpvsub( shape->body->p, planet.body->p )
					var r = cpvlength( pull )
					var v = cpfsqrt( planet.gravityStrength / r) / r
					shape->body->v = cpvmult( cpvperp( pull ), v )
					shape->body->w = v
					cpBodySetAngle( shape->body, cpfatan2( shape->body->p.y, shape->body->p.x ) )
					addShapeToSpace( shapes(), @physics, shape )
				end if
			end if
			shapecount = ubound( shapes )
		end if
		if multikey( SC_C ) And ( lbound(shapes) > 0 ) And ( Timer > deleteTimer ) then
			removeShape( shapes(), @physics, shapes(ubound(shapes)).shape )
			shapecount = ubound( shapes )
			deleteTimer = Timer + 0.05
		end if
	physics.unlock()
	
	framecount += 1
	if Timer > frameTimer then
		fps = framecount
		framecount = 0
		frameTimer = Timer + 1
	end if
loop until multikey( &h01 )

physics.lock()
	cpSpaceRemoveShape( physics.space, planet.shape )
	if planet.body then
		cpSpaceRemoveBody( physics.space, planet.body )
		cpBodyFree( planet.body )
	end if
	cpShapeFree( planet.shape )
physics.unlock()

end 0

sub setupScreen( byval w as integer, byval h as integer, byval instance as easyChipmunk ptr )
	screenres w, h, 16, , GFX_ALPHA_PRIMITIVES
	Randomize Timer
	
	dim wallVerts(0 to 3) as cpVect = { cpv(0, 0), cpv(w,0), cpv(w,h), cpv(0, h) }
	dim j as integer
	for i as integer = 0 to 3
		j = i + 1
		if j > 3 then j = 0
		dim ws as cpShape ptr
		ws = cpSegmentShapeNew( NULL, wallVerts(i), wallVerts(j), 15.0 )
		ws->u = 0.1
		ws->e = 1.0
		ws->layers = NOT_GRABABLE_MASK
		cpSpaceAddStaticShape( instance->space, ws )
	next i
	
	
end sub

function generateShapeData( list() as shapeData, byval shape as cpShape ptr, byval colour as uinteger ) as integer
	dim idx as integer = -1
	for i as integer = 1 to ubound( list )
		if list(i).shape = NULL then
			idx = i
			exit for
		end if
	next
	if idx = -1 then
		idx = ubound( list ) + 1
		redim preserve list(1 to idx) as shapeData
	end if
	list(idx).shape = shape
	list(idx).body = NULL
	list(idx).colour = colour
	return idx
end function

function createNewCircle( list() as shapeData, byval mass as cpFloat, byval radius as cpFloat, byval colour as uinteger, byval isStatic as ubyte = 0, byval offset as cpVect = cpvzero ) as cpShape ptr
	dim shape as cpShape ptr
	dim body as cpBody ptr = NULL
	if isStatic = 0 then
		body = cpBodyNew( mass, cpMomentForCircle( mass, radius, 0.0, offset ) )
		body->velocity_func = @planetGravityVelocityFunc
	end if
	shape = cpCircleShapeNew( body, radius, offset )
	var i = generateShapeData( list(), shape, colour )
	list(i).body = body
	return shape
end function

function createNewPolygon( list() as shapeData, byval mass as cpFloat, byval vertices as cpVect ptr, byval nVertices as integer, byval colour as uinteger, byval isStatic as ubyte = 0, byval offset as cpVect = cpvzero ) as cpShape ptr
	dim shape as cpShape ptr = NULL
	dim body as cpBody ptr = NULL
	if cpPolyValidate( vertices, nVertices ) = cpFalse then
		open cons for output as #5
		print #5, "Warning: polygon isn't valid!"
		close #5
	end if
	if isStatic = 0 then
		body = cpBodyNew( mass, cpMomentForPoly( mass, nVertices, vertices, offset ) )
		body->velocity_func = @planetGravityVelocityFunc
	end if
	shape = cpPolyShapeNew( body, nVertices, vertices, offset )
	var i = generateShapeData( list(), shape, colour )
	list(i).body = body
	return shape
end function

function createNewPolygon2( list() as shapeData, byval mass as cpFloat, byval nVertices as integer, byval radius as cpFloat, byval colour as uinteger, byval isStatic as ubyte = 0, byval offset as cpVect = cpvzero ) as cpShape ptr
	dim as cpVect verts(0 to nVertices-1)
	dim as cpFloat angIncrement = 360.0 / nVertices
	for i as integer = 0 to nVertices-1
		verts(i).x = cos( csng(i) * angIncrement ) * radius
		verts(i).y = sin( csng(i) * angIncrement ) * -radius
	next
	return createNewPolygon( list(), mass, @verts(0), nVertices, colour, isStatic, offset )
end function

sub removeShape( list() as shapeData, byval instance as easyChipmunk ptr, byval shape as cpShape ptr )
	for i as integer = 1 to ubound( list )
		if list(i).shape = shape then
			if list(i).body then cpSpaceRemoveBody( instance->space, list(i).body )
			cpSpaceRemoveShape( instance->space, list(i).shape )
			list(i).shape = NULL
			for r as integer = i to ubound(list)-1
				list(r) = list(r+1)
			next
			if ubound(list) > 1 then
				redim preserve list(1 to ubound(list)-1) as shapeData
			else
				redim list(0) as shapeData
			end if
			return
		end if
	next
end sub

sub addShapeToSpace( list() as shapeData, byval instance as easyChipmunk ptr, byval shape as cpShape ptr )
	for i as integer = 1 to ubound( list )
		if list(i).shape = shape then
			if list(i).body then cpSpaceAddBody( instance->space, list(i).body )
			cpSpaceAddShape( instance->space, list(i).shape )
			return
		end if
	next
end sub

sub drawShapeList( list() as shapeData )
	dim node as cpShape ptr
	for i as integer = 1 to ubound( list )
		node = list(i).shape
		if node = NULL then continue for
		select case node->klass->type
		case CP_CIRCLE_SHAPE
			circle( node->body->p.x, node->body->p.y ), cpCircleShapeGetRadius( node ), list(i).colour
			pset( node->body->p.x, node->body->p.y ), rgb(255,200,200)
		case CP_POLY_SHAPE
			var center = node->body->p
			var vert = cpPolyShapeGetVert( node, 0 )
			pset( center.x + vert.x, center.y + vert.y ), list(i).colour
			for v as integer = 1 to cpPolyShapeGetNumVerts( node )-2
				var vert = cpPolyShapeGetVert( node, v )
				line -( center.x + vert.x, center.y + vert.y ), list(i).colour
			next
			vert = cpPolyShapeGetVert( node, 0 )
			line -( center.x + vert.x, center.y + vert.y ), list(i).colour
		end select
	next
end sub

