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

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

--===== 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")

			for id, physics in pairs(query) do
				if not physics._body then
					print("add physics body for "..id)
					local body = ode.BodyCreate(self.world)
					physics._gc = honey.util.gc_canary(function() 
						print("releasing physics body for " .. id)
						ode.BodyDestroy(body) 
						body = nil
					end)

					local collision = self.db:getComponent(id, "collision")
					if collision then
						print(id, collision.class)
						ode.GeomSetBody(collision._geom, body)
					end

					local mass = ode.MassCreate()
					local class = physics.mass.class
					if not class then
						-- configure mass manually
					elseif class == "sphere" then
						ode.MassSetSphere(
							mass, 
							physics.mass.density, 
							physics.mass.radius
						)
					elseif class == "capsule" then
						ode.MassSetCapsule(
							mass,
							physics.mass.density,
							physics.mass.direction,
							physics.mass.radius,
							physics.mass.length
						)
					end
					ode.BodySetMass(body, mass)
					local m = self.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]
					)
					local vel = physics.velocity or Vec3{0,0,0}
					ode.BodySetLinearVel(
						body, vel[1], vel[2], vel[3]
					)
					physics.velocity = vel

					local avel = physics.angularVelocity or Vec3{0,0,0}
					ode.BodySetAngularVel(
						body, avel[1], avel[2], avel[3]
					)
					physics.angularVelocity = avel

					if physics.maxAngularSpeed then
						ode.BodySetMaxAngularSpeed(body, physics.maxAngularSpeed)
					end

					physics._body = body
				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
						-- 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, collisions[1])
						-- 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)
				-- 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