local glm = require 'honey.glm'
local Vec3       = glm.Vec3
local Mat4       = glm.Mat4
local Quaternion = glm.Quaternion
local ode = honey.ode

local script = require 'honey.ecs.script'

local module = {}
setmetatable(module, {__index=_G})
setfenv(1, module)


-- create a new mass
local function createMass(tbl)
	local mass = ode.MassCreate()
	local class = tbl.class
	if not class then
		-- configure mass manually
	elseif class == "sphere" then
		ode.MassSetSphere(
			mass, 
			tbl.density, 
			tbl.radius
		)
	elseif class == "capsule" then
		if tbl.mass then
			ode.MassSetCapsuleTotal(
				mass,
				tbl.mass,
				tbl.direction,
				tbl.radius,
				tbl.length
			)
		else
			ode.MassSetCapsule(
				mass,
				tbl.density,
				tbl.direction,
				tbl.radius,
				tbl.length
			)
		end
	end
	return mass
end


-- create a new physics body
local function createPhysicsBody(db, world, id, component)
	-- initialize the body with garbage collector
	print("add component body for "..id)
	local body = ode.BodyCreate(world)
	component._gc = honey.util.gc_canary(function() 
		print("releasing component body for " .. id)
		ode.BodyDestroy(body) 
		body = nil
	end)

	-- connect the body with a collision geom
	local collision = db:getComponent(id, "collision")
	if collision then
		print(id, collision.class)
		ode.GeomSetBody(collision._geom, body)
	end

	-- set the mass of the body
	ode.BodySetMass(body, createMass(component.mass))

	-- set the transform of the body
	local m = db:getComponent(id, "node").matrix
	ode.BodySetPosition(
		body,
		m[1][4], m[2][4], m[3][4]
	)
	ode.BodySetRotation(
		body,
		m[1][1], m[1][2], m[1][3],
		m[2][1], m[2][2], m[2][3],
		m[3][1], m[3][2], m[3][3]
	)

	-- set the linear velocity
	local vel = component.velocity or Vec3{0,0,0}
	ode.BodySetLinearVel(
		body, vel[1], vel[2], vel[3]
	)
	component.velocity = vel

	-- set the angular velocity
	local avel = component.angularVelocity or Vec3{0,0,0}
	ode.BodySetAngularVel(
		body, avel[1], avel[2], avel[3]
	)
	component.angularVelocity = avel

	-- optionally set a maximum angular speed
	if component.maxAngularSpeed then
		ode.BodySetMaxAngularSpeed(body, component.maxAngularSpeed)
	end

	-- put the body into the physics component
	component._body = body
end


local function handleCollision(db, self, other, collision)
	local handler = db:getComponent(self, "onCollision")
	if handler then
		h = script.getFunction(handler)
		h(db, self, other, collision)
	end
end


local function collide(self, a, b, collision)
	-- check for collision handlers
	local idA = ode.GeomGetData(a)
	local idB = ode.GeomGetData(b)
	handleCollision(self.db, idA, idB, collision)
	handleCollision(self.db, idB, idA, collision)

	local physicsA = self.db:getComponent(idA, "physics")
	local physicsB = self.db:getComponent(idB, "physics")

	local surface = (physicsA and physicsA.surface) or
		(physicsB and physicsB.surface)

	if surface then
		-- set up the joint params
		local contact = ode.CreateContact{ surface={
			mode = ode.ContactBounce + ode.ContactSoftCFM,
			mu = ode.Infinity,
			bounce = 0.90,
			bounce_vel = 0.1,
			soft_cfm = 0.001,
		}}
		ode.ContactSetGeom(contact, collision)
		-- create the joint
		local joint = ode.JointCreateContact(
			self.world,
			self.contactGroup,
			contact
		)
		-- attach the two bodies
		local bodyA = ode.GeomGetBody(a)
		local bodyB = ode.GeomGetBody(b)
		ode.JointAttach(joint, bodyA, bodyB)
	end
end

--===== physics =====--


system = function(params)
	local interval = params.interval or 0.016
	local groupSize = params.groupSize or 20
	local refs = {}
	return {
		db=params.db,
		space=params.space,
		world=params.world,
		contactGroup=ode.JointGroupCreate(groupSize),
		time=interval,

		priority=1,
		update=function(self, dt)
			for i, ref in ipairs(refs) do
				print(i, ref.tbl, ref.physics)
			end
			local query = self.db:queryComponent("physics")

			-- check for new physics entities
			for id, physics in pairs(query) do
				if not physics._body then
					createPhysicsBody(self.db, self.world, id, physics)
				end
			end

			self.time = self.time + dt
			-- only run the update every [interval] seconds
			if self.time > interval then
				self.time = self.time - interval

				-- check for near collisions between geoms
				ode.SpaceCollide(self.space, function(a, b)
					-- check for actual collisions
					local collisions = ode.Collide(a, b, 1)
					if #collisions > 0 then
						collide(self, a, b, collisions[1])
					end
				end)
				-- update the world
				ode.WorldQuickStep(self.world, interval)
				-- remove all contact joints
				ode.JointGroupEmpty(self.contactGroup)

				-- update entity nodes
				for id, physics in pairs(query) do
					local x,y,z = ode.BodyGetPosition(physics._body)
					local d,a,b,c = ode.BodyGetQuaternion(physics._body)
					local node = self.db:getComponent(id, "node")
					local q = Quaternion{a,b,c,d}
					node.matrix
						:identity()
						:translate(Vec3{x,y,z})
						:mul(Quaternion{a,b,c,d}:toMat4())

					local vel = physics.velocity
					vel[1], vel[2], vel[3] = ode.BodyGetLinearVel(physics._body)
					local avel = physics.angularVelocity
					avel[1], avel[2], avel[3] = ode.BodyGetAngularVel(physics._body)
				end
			end
		end,
	}
end



return module