-- Constants
local KEYBOARD_BUTTON_IMAGE = require(script.KeyboardButtonImage)
local KEYBOARD_BUTTON_ICON_MAPPING = require(script.KeyboardButtonIconMapping)
local KEYCODE_TO_TEXT_MAPPING = require(script.KeyCodeToTextMapping)

-- Services
local UserInputService = game:GetService("UserInputService")

return function(Scope : any, prompt, Config : {})
    local AddedChildren = {}
	local buttonTextString = UserInputService:GetStringForKeyCode(prompt.KeyboardKeyCode)

	local buttonTextImage = KEYBOARD_BUTTON_IMAGE[prompt.KeyboardKeyCode]
	if buttonTextImage == nil then
		buttonTextImage = KEYBOARD_BUTTON_ICON_MAPPING[buttonTextString]
	end

	if buttonTextImage == nil then
		local keyCodeMappedText = KEYCODE_TO_TEXT_MAPPING[prompt.KeyboardKeyCode]
		if keyCodeMappedText then
			buttonTextString = keyCodeMappedText
		end
	end

	AddedChildren[#AddedChildren + 1] = Scope:New("ImageLabel", {
		Name = "ButtonImage",
		BackgroundTransparency = 1,
		Size = UDim2.fromOffset(28, 30),
		AnchorPoint = Vector2.new(0.5, 0.5),
		Position = UDim2.fromScale(0.5, 0.5),
		Image = "rbxasset://textures/ui/Controls/key_single.png",
		ImageColor3 = Config.TextColor,
	})

	if buttonTextImage then
		AddedChildren[#AddedChildren + 1] = Scope:New("ImageLabel", {
			Name = "ButtonImage",
			AnchorPoint = Vector2.new(0.5, 0.5),
			Size = UDim2.fromOffset(36, 36),
			Position = UDim2.fromScale(0.5, 0.5),
			BackgroundTransparency = 1,
			Image = buttonTextImage,
			ImageColor3 = Config.TextColor,
		})
	elseif buttonTextString ~= nil and buttonTextString ~= "" then
		AddedChildren[#AddedChildren + 1] = Scope:New("TextLabel", {
			Name = "ButtonText",
			Size = UDim2.fromScale(1, 1),
			Position = UDim2.fromOffset(0, -1),
			Font = Enum.Font.GothamMedium,
			TextSize = if string.len(buttonTextString) > 2 then 12 else 14,
			BackgroundTransparency = 1,
			TextColor3 = Config.TextColor,
			TextXAlignment = Enum.TextXAlignment.Center,
			Text = buttonTextString,
		})
	else
		error(
			"ProximityPrompt '"
				.. prompt.Name
				.. "' has an unsupported keycode for rendering UI: "
				.. tostring(prompt.KeyboardKeyCode)
		)
	end

    return AddedChildren
end


return function(Scope : any, Prompt : ProximityPrompt, Config : {})
    return {
        Scope:New("ImageLabel", {
            Name = "ButtonImage",
            ImageColor3 = Config.TextColor,
            BackgroundTransparency = 1,
            Size = UDim2.fromOffset(25, 31),
            AnchorPoint = Vector2.new(0.5, 0.5),
            Position = UDim2.fromScale(0.5, 0.5),
            Image = "rbxasset://textures/ui/Controls/TouchTapIcon.png",
        })
    }
end


-- Constants
local INPUT_KEY_LABELS = {
    [Enum.ProximityPromptInputType.Gamepad] = require(script.Gamepad),
    [Enum.ProximityPromptInputType.Touch] = require(script.Touch),
    ["Default"] = require(script.Keyboard),
}

return function(Scope : any, inputType, prompt, Config : {})
    local Component = INPUT_KEY_LABELS[inputType] or INPUT_KEY_LABELS["Default"]

    return Component(Scope, prompt, Config)
end


-- NOTE: This module will be improved when the input connections are recoded

-- Imports
local Packages = script.Parent:FindFirstChild("Packages")
local Seam = if Packages then require(Packages.Seam) else require(script.Parent.Parent.Seam)

-- Variables
local Scope = Seam.Scope(Seam)

return {
   AppearSoundId = Scope:Value("rbxassetid://10066931761"),
   ClickSoundId = Scope:Value("rbxassetid://10128760939"),
   HoldSoundId = Scope:Value("rbxassetid://421058925"),
   TriggerSoundId = Scope:Value("rbxassetid://10128766965"),
   SoundVolume = Scope:Value(0.35),
}


--!native

-- really basic simulation logic
-- for the humanoid and world
-- but, i advise you to make your own!!

assert(not workspace.StreamingEnabled, "this simulation script assumes StreamingEnabled = false")

local Pausing = false
local HZ = 60

local ContextActionService = game:GetService("ContextActionService")
local ReplicatedStorage = game:GetService("ReplicatedStorage")
local RunService = game:GetService("RunService")
local Players = game:GetService("Players")

local LocalPlayer = assert(Players.LocalPlayer)

local LuauHumanoid = require(ReplicatedStorage["luau-humanoid"])
local LuauPhysics = require(ReplicatedStorage["luau-physics"])

local what = Instance.new("Hint", workspace)

local Humanoid = LuauHumanoid.Humanoid
local World = LuauPhysics.World

local CurrentHumanoid: LuauHumanoid.Humanoid
local RealHumanoid: Humanoid

local function characterAdded()
	local character = LocalPlayer.Character
	RealHumanoid = character:WaitForChild("Humanoid") :: Humanoid

	local rootPart = RealHumanoid.RootPart
	RealHumanoid:GetPropertyChangedSignal("MoveDirection")
		:Connect(function()
			CurrentHumanoid.moveDirection = RealHumanoid.MoveDirection
		end)

	RealHumanoid:GetPropertyChangedSignal("Jump")
		:Connect(function()
			CurrentHumanoid.jump = RealHumanoid.Jump
		end)

	-- create the humanoid from the root part
	RealHumanoid.EvaluateStateMachine = false
	RealHumanoid:ChangeState(Enum.HumanoidStateType.Running)
	rootPart.Anchored = true
	rootPart.CFrame = workspace.tp.CFrame

	local baseHeight = if RealHumanoid.RigType == Enum.HumanoidRigType.R15
		then 0.15
		else 3

	CurrentHumanoid = Humanoid.new(rootPart)
	CurrentHumanoid.hipHeight = RealHumanoid.HipHeight + baseHeight
end

-- debugging purposes
ContextActionService:BindAction(
	"pause-physics",
	function(_, state: Enum.UserInputState)
		if state == Enum.UserInputState.Begin then
			Pausing = not Pausing
		end
	end,
	false,
	Enum.KeyCode.Q
)

ContextActionService:BindAction(
	"step-physics",
	function(_, state: Enum.UserInputState)
		if state == Enum.UserInputState.Begin then
			workspace.Wireframe:Clear()

			local state = CurrentHumanoid.state
			CurrentHumanoid:update(2 / HZ)
			if CurrentHumanoid.state ~= state then
				RealHumanoid:ChangeState(CurrentHumanoid.state)
			end

			World.step(2 / HZ)
		end
	end,
	false,
	Enum.KeyCode.E
)

local previous = os.clock()
local function heartbeat(dt: number)

	local clock = os.clock()
	if not CurrentHumanoid or Pausing then
		previous = clock
		return
	end

	local ticks = (clock - previous) * HZ
	workspace.Wireframe:Clear()

	--World.bvh:draw()

	local t0 = 0.0
	local t1 = 0.0

	-- set state, whatever this is
	local state = CurrentHumanoid.state
	RealHumanoid.EvaluateStateMachine = false

	for i = 1, ticks do
		for j = 1, 4 do
			CurrentHumanoid:update(0.25 / HZ)
		end

		if CurrentHumanoid.state ~= state then
			RealHumanoid:ChangeState(CurrentHumanoid.state)
		end

		World.step(1 / HZ)
	end

	local remainder = ((ticks % 1) / HZ)
	CurrentHumanoid:update(remainder)
	if CurrentHumanoid.state ~= state then
		RealHumanoid:ChangeState(CurrentHumanoid.state)
	end

	t0 = os.clock()
	World.step(remainder)
	t1 = os.clock()

	what.Text = `{((t1-t0)*1e6)//1}us, {World.numActive} awake`

	previous = clock
end

if LocalPlayer.Character then
	characterAdded()
end

LocalPlayer.CharacterAdded:Connect(characterAdded)

task.wait(0.5)
previous = os.clock()
RunService.Heartbeat:Connect(heartbeat)

-- initialize the world
for _, v in workspace.static:GetDescendants() do
	if v:IsA("BasePart") then
		World.addStaticBody(v)
	end
end

for i, v in workspace.dynamic:GetChildren() do
	World.addDynamicBody(v)
end


--!strict

-- Modern humanoid physics reimplementation
-- in lua

local LuauPhysics = require("../luau-physics")
local World = LuauPhysics.World

export type RaycastResult = LuauPhysics.RaycastResult

local UPDATE_HZ = 480

local VEC_Y_FLIP = Vector3.new(1, -1, 1)
local VEC_XZ = Vector3.new(1, 0, 1)

local CAST_POINTS = {
	-- center line
	Vector3.new(0, -1, 0),
	Vector3.new(0, -1, 1),
	Vector3.new(0, -1, -1),

	-- corners
	Vector3.new(-1, -1, -1),
	Vector3.new(-1, -1, 1),
	Vector3.new(1, -1, -1),
	Vector3.new(1, -1, 1),
}

local States = {} :: { [Enum.HumanoidStateType]: State }
local Humanoid = {}
Humanoid.__index = Humanoid

export type State = {
	-- function to calculate the torque
	calculateTorque: (self: Humanoid, dt: number) -> Vector3,

	-- function to calculate the force
	calculateForce: (self: Humanoid, dt: number) -> Vector3,

	-- function to update
	update: (self: Humanoid, dt: number) -> (),

	-- function to enter
	enter: (self: Humanoid, dt: number) -> (),
}

export type LadderResult = {
	-- ladder normal
	normal: Vector3,

	-- is there a ladder?
	hasLadder: boolean,
}

export type FloorResult = {
	-- current floor, set this to nil
	-- if no floor is present
	part: BasePart?,

	-- has a floor?
	hasFloor: boolean,

	-- average raycast floor position
	position: Vector3,

	-- average raycast normal
	normal: Vector3,

	-- average raycast distance
	distance: number,

	-- velocity of the floor
	linearVelocity: Vector3,

	-- rotational velocity of the floor
	angularVelocity: Vector3,

	-- origin of the floor
	origin: Vector3,
}

export type Humanoid = typeof(setmetatable({} :: {
	-- other state
	ladderResult: LadderResult,
	floorResult: FloorResult,

	-- current state
	angularVelocity: Vector3,
	linearVelocity: Vector3,
	rootCoord: CFrame,

	-- physics state
	inertia: CFrame,
	mass: number,

	-- used for state
	state: Enum.HumanoidStateType,
	jumpDirection: Vector3,
	coyoteTimer: number,
	character: Model,

	-- used for movement
	moveDirection: Vector3,
	hipHeight: number,
	walkSpeed: number,
	jumpPower: number,

	-- mouselock
	lookAngle: Vector3,
	mouselock: boolean,

	-- physics body
	body: LuauPhysics.Body,

	-- used for physics
	rootPart: BasePart,
	clock: number,

	-- user jump
	jump: boolean,
}, Humanoid))

function Humanoid.bindState(
	enum: Enum.HumanoidStateType,
	state: State
): State
	-- binds a state onto the humanoid
	-- so we can use it
	States[enum] = state
	return state
end

function Humanoid.new(rootPart: BasePart): Humanoid
	local character = rootPart.Parent :: Model

	-- these are used for calculating state stuff
	local body = World.addDynamicBody(rootPart)
	World.markBodyActive(body, true)

	return setmetatable({
		-- other state
		ladderResult = {
			normal = Vector3.zero,
			hasLadder = false,
		} :: LadderResult,

		floorResult = {
			position = Vector3.zero,
			normal = Vector3.zero,
			distance = 0.0,

			linearVelocity = Vector3.zero,
			angularVelocity = Vector3.zero,
			origin = Vector3.zero,
		} :: FloorResult,

		-- current state
		angularVelocity = rootPart.AssemblyAngularVelocity,
		linearVelocity = rootPart.AssemblyLinearVelocity,
		rootCoord = rootPart.CFrame,

		-- physics state
		inertia = body.inertia,
		mass = body.mass,

		-- used for state
		state = Enum.HumanoidStateType.Running,
		character = character,

		-- used for movement
		moveDirection = Vector3.zero,
		hipHeight = 2.0,
		walkSpeed = 16.0,
		jumpPower = 50.0,

		jumpDirection = Vector3.zero,
		coyoteTimer = 0.0,

		-- mouselock
		lookAngle = Vector3.zAxis,
		mouselock = false,

		-- physics body
		body = body,

		-- used for physics
		rootPart = rootPart,
		clock = os.clock(),

		-- user jump
		jump = false,
	}, Humanoid)
end

local function angleBetween(a: Vector3, b: Vector3): number
	-- gives a signed angle from a to b
	local cos = math.clamp(a:Dot(b), -1.0, 1.0)

	-- angle sign is sign((a x y).b)
	return math.acos(cos) * math.sign(a.X*b.Z - a.Z*b.X)
end

function Humanoid.getDesiredVelocity(self: Humanoid): Vector3
	-- gets the desired velocity
	local desiredVelocity = self.moveDirection * self.walkSpeed

	local ladderResult = self.ladderResult
	local floorResult = self.floorResult

	local floorPart = floorResult.part
	if ladderResult.hasLadder then
		-- add the ladder velocity
		local dot = self.moveDirection:Dot(ladderResult.normal)
		desiredVelocity -= math.sign(dot) * self.walkSpeed * Vector3.yAxis
		desiredVelocity *= Vector3.yAxis
	elseif floorPart then
		-- add the floor velocity aswell
		desiredVelocity += floorResult.linearVelocity
			+ (floorResult.origin - self.rootCoord.Position):Cross(floorResult.angularVelocity)
	end

	return desiredVelocity
end

function Humanoid.computeBalanceTorque(self: Humanoid, dt: number)
	local kP = 2250
	local kD = 50

	-- get the angular offset from the y pole
	local tiltWorld = -Vector3.yAxis:Cross(self.rootCoord.UpVector)
	return self.inertia * (kP*tiltWorld - kD*self.angularVelocity)
end

function Humanoid.computeRunningForce(self: Humanoid, dt: number): Vector3
	local maxVerticalForce = 10000
	local kP = 100

	local desiredVelocity = self:getDesiredVelocity()
	local desiredAccel = kP * (desiredVelocity - self.linearVelocity)

	local horizontal = desiredAccel * VEC_XZ
	local vertical = Vector3.yAxis * math.clamp(
		desiredAccel.Y,
		-maxVerticalForce,
		maxVerticalForce
	)

	local maxForce = if self.floorResult.hasFloor
		then 250.0
		else 143.0

	if horizontal:Dot(horizontal) > maxForce * maxForce then
		-- reach max force, clamp it!
		horizontal = horizontal.Unit * maxForce
	end

	local desiredPlane = if self.ladderResult.hasLadder
		then Vector3.one
		else VEC_XZ

	local gravity = Vector3.yAxis * World.Gravity
	local deltaForce = self.mass * (horizontal + vertical + gravity)
	if self.floorResult.hasFloor then
		-- apply friction
		deltaForce *= 0.9
	end

	return deltaForce * desiredPlane
end

function Humanoid.computeBalanceForce(self: Humanoid, dt: number): Vector3
	local kP = 20000
	local kD = 1100

	-- get the current floor
	local floorResult = self.floorResult
	if not floorResult.hasFloor then
		return Vector3.zero
	end

	-- calculate acceleration needed to stabilize our
	-- player onto the ground
	local dV = self.linearVelocity.Y - floorResult.linearVelocity.Y
	local dX = self.hipHeight - floorResult.distance
	local desiredAccel = kP * dX - kD * dV
	if desiredAccel < -1 then
		-- let gravity cause the player to sink
		-- to the floor
		return Vector3.zero
	end

	-- add upwards acceleration
	local gravity = World.Gravity
	local deltaAccel = (desiredAccel * 0.1)
	return Vector3.yAxis * self.mass * (deltaAccel + gravity)
end

function Humanoid.changeState(self: Humanoid, state: Enum.HumanoidStateType)
	-- changes the state to another one instead
	self.state = state
	States[state].enter(self, 0.0)
end

function Humanoid.computeOrientTorque(self: Humanoid, dt: number): Vector3
	local kMaxAccel = 10000
	local kTurn = 10
	local kP = 150

	local floorAngular = self.floorResult.angularVelocity.Y
	if self.moveDirection.Magnitude < 0.1 then
		-- no need to set angular velocity here
		local desiredAccel = kP * (floorAngular - self.angularVelocity.Y)
		desiredAccel = math.clamp(desiredAccel, -kMaxAccel, kMaxAccel)
		return self.inertia * Vector3.yAxis * desiredAccel
	end

	-- our desired angular velocity
	local desiredAngular = angleBetween(self.moveDirection, self.rootCoord.LookVector)
	if math.abs(desiredAngular) < 1e-3 then
		-- really small angle, we don't need to
		-- apply a correctional torque
		desiredAngular *= 0.0
	else
		-- apply turn
		desiredAngular *= kTurn
	end

	local desiredAccel = kP * (desiredAngular + floorAngular - self.angularVelocity.Y)
	desiredAccel = math.clamp(desiredAccel, -kMaxAccel, kMaxAccel)
	return self.inertia * Vector3.yAxis * desiredAccel
end

function Humanoid.updateLadder(self: Humanoid, dt: number)
	-- Also, this implementation is really naive!
	-- I'm currently working on a solution that
	-- just clips the potential candidates instead
	local rootCoord = self.rootCoord
	local displacement = 0.5
	local stepSize = 2.45

	local direction = rootCoord.LookVector
	local steps, possible, step, air = math.floor(4 * self.hipHeight), 0, 0, 0
	local collided = false

	-- reset the ladder state
	local ladderResult = self.ladderResult
	ladderResult.hasLadder = false

	for i = 1, steps do
		local offset = (i - 1) * displacement - self.hipHeight
		local origin = rootCoord.Position + Vector3.yAxis * offset

		local result = World.raycast(
			origin,
			direction,
			self.character
		) :: RaycastResult

		local didCollide = result ~= nil
		if didCollide ~= collided then
			-- collision differed, check the distance
			-- between the points
			local difference = (step - air) * displacement
			if didCollide then
				-- set it to the current
				-- ladder normal
				ladderResult.normal = result.Normal
			end

			collided = didCollide
			if math.abs(difference) < stepSize and i ~= 1 then
				possible += 1
			end

			-- make the step and air
			-- the same
			step = i
			air = i
		elseif collided then
			-- report the ladder rung
			step = i
		else
			-- report the air
			air = i
		end
	end

	-- calculate the ladder result
	ladderResult.hasLadder = possible >= 2
end

function Humanoid.updateCeiling(self: Humanoid): RaycastResult?
	-- checks for a ceiling, used in the jumping code
	local halfSize = self.rootPart.Size * 0.4
	local rotation = self.rootCoord.Rotation
	local origin = self.rootCoord.Position

	-- get raycast results
	local numResults = 0
	local direction = Vector3.yAxis * (self.hipHeight + 1)
	for i, castPoint in CAST_POINTS do
		local result = World.raycast(
			origin + rotation * (castPoint * halfSize * VEC_Y_FLIP),
			direction,
			self.character
		) :: RaycastResult

		if result then
			return result
		end
	end

	return nil
end

function Humanoid.updateFloor(self: Humanoid, dt: number)
	local halfSize = self.rootPart.Size * 0.4
	local rotation = self.rootCoord.Rotation
	local origin = self.rootCoord.Position

	local hysterisis = math.max(math.abs(self.linearVelocity.Y / 100), 0.5)
	hysterisis += self.hipHeight

	local floorResult = self.floorResult
	local floorPart: LuauPhysics.Body

	-- reset the floor result
	floorResult.part = nil
	floorResult.hasFloor = false
	floorResult.angularVelocity *= 0.0
	floorResult.linearVelocity *= 0.0
	floorResult.position *= 0.0
	floorResult.distance *= 0.0
	floorResult.origin *= 0.0
	floorResult.normal *= 0.0

	-- get raycast results
	local numResults = 0
	local direction = -Vector3.yAxis * hysterisis
	for i, castPoint in CAST_POINTS do
		if numResults ~= 0 and i == 4 then
			-- terminate early
			break
		end

		local result = World.raycast(
			origin + rotation * (castPoint * halfSize),
			direction,
			self.character
		) :: RaycastResult

		if result then
			-- add to the results table
			numResults += 1
			floorResult.position += result.Position
			floorResult.distance += result.Distance
			floorResult.normal += result.Normal
			floorPart = result.Instance
		end
	end

	if numResults ~= 0 and floorPart then
		-- if we did have results, make sure we
		-- average them in the end

		-- set floor result and velocity
		local invNumResults = 1.0 / numResults
		floorResult.position *= invNumResults
		floorResult.distance *= invNumResults
		floorResult.normal *= invNumResults

		floorResult.angularVelocity = floorPart.angularVelocity
		floorResult.linearVelocity = floorPart.linearVelocity
		floorResult.origin = floorPart.cframe.Position

		floorResult.part = floorPart :: BasePart
		floorResult.hasFloor = true
	end
end

function Humanoid.update(self: Humanoid, dt: number)
	self.angularVelocity = self.body.angularVelocity
	self.linearVelocity = self.body.linearVelocity
	self.rootCoord = self.body.cframe

	-- get initial state
	debug.profilebegin("humanoid::solve")

	-- first, we check for any floors
	self:updateFloor(dt)

	-- second, we update the state
	States[self.state].update(self, dt)

	local gravity = Vector3.yAxis * World.Gravity * self.mass
	local currentState = States[self.state]
	self.body.force = currentState.calculateForce(self, dt) - gravity
	self.body.torque = currentState.calculateTorque(self, dt)

	if self.mouselock then
		-- Set the mouselock rotation afterwards,
		-- so wallhops are possible
		local rootCoord = self.rootCoord
		local desiredAngular = angleBetween(
			self.lookAngle,
			(rootCoord.LookVector * VEC_XZ).Unit
		)

		local baseCFrame = CFrame.new(rootCoord.Position)
		local translation = CFrame.new(
			0.0, 0.0, 0.0,
			0.0, math.sin(desiredAngular), 0.0, math.cos(desiredAngular)
		)

		self.body.cframe = baseCFrame * (translation * rootCoord.Rotation)
	end

	debug.profileend()

	-- finalize state
	self.rootPart.AssemblyAngularVelocity = self.angularVelocity
	self.rootPart.AssemblyLinearVelocity = self.linearVelocity
end

return Humanoid


-- luau humanoid reaction :3
-- magicoal_nerb

local Humanoid = require("@self/Humanoid")

export type Humanoid = Humanoid.Humanoid
export type State = Humanoid.State

return {
	-- main humanoid
	Humanoid = require("@self/Humanoid"),

	-- states
	Freefall = require("@self/states/Freefall"),
	Climbing = require("@self/states/Climbing"),
	Jumping = require("@self/states/Jumping"),
	Running = require("@self/states/Running"),
}


--!strict
local Humanoid = require("../Humanoid")

local Climbing = {} :: Humanoid.State

function Climbing.calculateTorque(self: Humanoid.Humanoid, dt: number): Vector3
	-- return the torques so far
	return self:computeBalanceTorque(dt)
end

function Climbing.calculateForce(self: Humanoid.Humanoid, dt: number): Vector3
	-- return the current force so far
	return self:computeRunningForce(dt)
		+ self:computeBalanceForce(dt)
end

function Climbing.update(self: Humanoid.Humanoid, dt: number)
	-- Check the floor
	self:updateFloor(dt)
	self:updateLadder(dt)

	if not self.ladderResult.hasLadder then
		-- The user is not on a ladder
		return self:changeState(Enum.HumanoidStateType.Running)
	elseif self.jump then
		-- The user is requesting to jump
		return self:changeState(Enum.HumanoidStateType.Jumping)
	end
end

function Climbing.enter(self: Humanoid.Humanoid)
	-- pass :3
end

return Humanoid.bindState(Enum.HumanoidStateType.Climbing, Climbing)


--!strict

local Humanoid = require("../Humanoid")

local Freefall = {} :: Humanoid.State

function Freefall.calculateTorque(self: Humanoid.Humanoid, dt: number): Vector3
	-- return the torques so far
	return self:computeBalanceTorque(dt)
		+ self:computeOrientTorque(dt)
end

function Freefall.calculateForce(self: Humanoid.Humanoid, dt: number): Vector3
	-- return the current force so far
	return self:computeRunningForce(dt)
end

function Freefall.update(self: Humanoid.Humanoid, dt: number)
	-- look for the floor
	self:updateFloor(dt)
	self:updateLadder(dt)

	if self.floorResult.hasFloor and self.linearVelocity.Y <= 0.0 then
		-- change the state to running because we
		-- found a floor
		return self:changeState(Enum.HumanoidStateType.Running)
	elseif self.ladderResult.hasLadder then
		-- the user is on a ladder
		return self:changeState(Enum.HumanoidStateType.Climbing)
	end
end

function Freefall.enter(self: Humanoid.Humanoid)
	-- :3
end

return Humanoid.bindState(Enum.HumanoidStateType.Freefall, Freefall)


--!strict

local Humanoid = require("../Humanoid")

local JUMP_THRESHOLD = 5.0
local VEC_XZ = Vector3.new(1.0, 0.0, 1.0)

local Jumping = {} :: Humanoid.State

function Jumping.calculateTorque(self: Humanoid.Humanoid, dt: number): Vector3
	-- return the torques so far
	return self:computeBalanceTorque(dt)
		+ self:computeOrientTorque(dt)
end

function Jumping.calculateForce(self: Humanoid.Humanoid, dt: number): Vector3
	-- this is really silly, but roblox has a flag exposed
	-- about jump spring parameters, so we have to make jumps
	-- this way. because of this, lag highjumps exist
	local kP = 200

	local err = self.jumpPower - self.linearVelocity:Dot(self.jumpDirection)
	local cap = 1.1 * (self.jumpPower - self.linearVelocity:Dot(self.jumpDirection)) / dt
	local accel = math.min(kP * err, cap)

	if accel <= 0.0 then
		-- we reached the jump power
		self:changeState(Enum.HumanoidStateType.Freefall)
		return self:computeRunningForce(dt)
	end

	-- otherwise, exert the upward force
	-- until we reach the target jump power
	local jumpForce = self.jumpDirection * self.mass * accel
	return self:computeRunningForce(dt) + jumpForce
end

function Jumping.update(self: Humanoid.Humanoid, dt: number)
	-- check the floor and ceiling
	local hitCeiling = self:updateCeiling(dt)
	self:updateLadder(dt)
	self:updateFloor(dt)

	if self.jumpPower - JUMP_THRESHOLD <= self.linearVelocity:Dot(self.jumpDirection) then
		-- We reached the jump power
		self:changeState(Enum.HumanoidStateType.Freefall)
	elseif hitCeiling then
		-- We hit a ceiling
		self:changeState(Enum.HumanoidStateType.Freefall)
	end
end

function Jumping.enter(self: Humanoid.Humanoid)
	-- :3
	if self.floorResult.hasFloor then
		-- if we were on the floor, then jump off of it
		self.jumpDirection = Vector3.yAxis
	elseif self.ladderResult.hasLadder then
		-- if we were on a ladder, then jump off of it
		local lookDirection = -(self.rootCoord.LookVector * VEC_XZ).Unit
		self.jumpDirection = (Vector3.yAxis + lookDirection).Unit
	else
		-- otherwise, we just default to the
		-- up direction
		self.jumpDirection = Vector3.yAxis
	end
end

return Humanoid.bindState(Enum.HumanoidStateType.Jumping, Jumping)


--!strict
local Humanoid = require("../Humanoid")

local COYOTE_HEIGHT = 0.05
local COYOTE_TIMER = 0.1

local Running = {} :: Humanoid.State

function Running.calculateTorque(self: Humanoid.Humanoid, dt: number): Vector3
	-- return the torques so far
	return self:computeBalanceTorque(dt)
		+ self:computeOrientTorque(dt)
end

function Running.calculateForce(self: Humanoid.Humanoid, dt: number): Vector3
	-- return the current force so far
	return self:computeRunningForce(dt)
		+ self:computeBalanceForce(dt)
end

function Running.update(self: Humanoid.Humanoid, dt: number)
	-- check the floor
	self:updateLadder(dt)
	self:updateFloor(dt)

	if not self.floorResult.hasFloor then
		-- the user is not on the floor, increment
		-- the coyote timer
		self.coyoteTimer -= dt
		if self.coyoteTimer < 0.0 then
			-- set to freefall, coyote timer ended
			self:changeState(Enum.HumanoidStateType.Freefall)
		end
	elseif self.jump and self.floorResult.distance <= self.hipHeight + COYOTE_HEIGHT then
		-- the user is requesting to jump.
		self:changeState(Enum.HumanoidStateType.Jumping)
	elseif self.ladderResult.hasLadder then
		-- the user is on a ladder
		self:changeState(Enum.HumanoidStateType.Climbing)
	else
		self.coyoteTimer = COYOTE_TIMER
	end
end

function Running.enter(self: Humanoid.Humanoid)
	-- :3
	self.coyoteTimer = COYOTE_TIMER
end

return Humanoid.bindState(Enum.HumanoidStateType.Running, Running)


--!strict
local GRAVITY = 196.2

local Hull = require("./narrowphase/Hull")
local Mat3 = require("./Mat3")

local Body = {}
Body.__index = Body

export type Hull = Hull.Hull
export type Mat3 = Mat3.Mat3

export type Body = {
	-- body
	part: BasePart,
	hull: Hull,

	-- sleep
	sleep: number,
	flags: number,

	-- queries
	smooth: boolean,
	min: Vector3,
	max: Vector3,

	-- state
	cframe: CFrame,
	size: Vector3,

	-- velocities
	linearVelocity: Vector3,
	angularVelocity: Vector3,

	-- momentum
	linearMomentum: Vector3,
	angularMomentum: Vector3,

	-- other state if necessary lol
	torque: Vector3,
	force: Vector3,

	-- inertia
	invInertia: CFrame,
	inertia: CFrame,

	-- mass
	invMass: number,
	mass: number,

	-- surface props
	restitution: number,
	friction: number,
	beta: number,
}

local function getWedgeInertia(mass: number, size: Vector3): Vector3
	-- https://freedium.cfd/https://rjallain.medium.com/the-moment-of-inertia-tensor-for-a-triangle-18482978a938
	local x = size.X
	local y = size.Y
	local z = size.Z

	return Vector3.new(
		mass * z*z,
		mass * x*x,
		mass * y*y
	) / 6
end

local function getCubeInertia(mass: number, size: Vector3): Vector3
	-- gets moment of inertia of a cube, we assume this
	-- for all of the parts in our body
	local x = size.X
	local y = size.Y
	local z = size.Z

	return Vector3.new(
		mass * (y*y + z*z),
		mass * (x*x + z*z),
		mass * (x*x + y*y)
	) / 12
end

local function getSphereInertia(mass: number, radius: number): Vector3
	return Vector3.one * 0.4 * mass * radius * radius
end

local function getExtentsSize(cframe: CFrame, size: Vector3): (Vector3, Vector3)
	local halfSize = size * 0.5
	local halfAbsSize = cframe.RightVector:Abs() * halfSize.X
		+ cframe.UpVector:Abs() * halfSize.Y
		+ cframe.LookVector:Abs() * halfSize.Z

	-- gets the position
	local position = cframe.Position
	return position - halfAbsSize,
		position + halfAbsSize
end

local CF_ZERO = CFrame.new(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
function Body.newStatic(part: Part, hull: Hull.Hull): Body
	-- if we're anchored, then we have infinite mass
	-- so we cannot apply linear velocity or angular velocity
	-- at all to this
	local cframe = part.CFrame
	local size = part.Size

	-- get the other stuff please :D
	local min, max = getExtentsSize(cframe, size)
	return {
		-- body
		part = part,
		hull = hull,

		-- sleep
		sleep = 0.0,
		flags = 0.0,

		-- queries
		smooth = part.Shape ~= Enum.PartType.Block,
		min = min,
		max = max,

		-- state
		cframe = part.CFrame,
		size = size,

		-- velocities
		angularVelocity = part.AssemblyAngularVelocity,
		linearVelocity = part.AssemblyLinearVelocity,

		-- momentum
		angularMomentum = Vector3.zero,
		linearMomentum = Vector3.zero,

		-- other state
		force = Vector3.zero,
		torque = Vector3.zero,

		-- inertia
		invInertia = CF_ZERO,
		inertia = CF_ZERO,

		-- mass
		invMass = 0.0,
		mass = math.huge,

		-- surface props
		restitution = 0.45,
		friction = 0.4,
		beta = 0.32,
	}
end

function Body.newDynamic(part: Part, hull: Hull.Hull): Body
	-- if we're anchored, then we have infinite mass
	-- so we cannot apply linear velocity or angular velocity
	-- at all to this
	local mass = part:GetMass()
	local cframe = part.CFrame
	local size = part.Size

	-- get the other stuff please :D
	local min, max = getExtentsSize(cframe, size)
	local inertia = getCubeInertia(mass, size)

	local inertiaMat = Mat3.fromVector(inertia)
	return {
		-- body
		part = part,
		hull = hull,

		-- sleep
		sleep = 0.0,
		flags = 0.0,

		-- queries
		smooth = part.Shape ~= Enum.PartType.Block,
		min = min,
		max = max,

		-- state
		cframe = part.CFrame,
		size = size,

		-- velocities
		angularVelocity = part.AssemblyAngularVelocity,
		linearVelocity = part.AssemblyLinearVelocity,

		-- momentum
		angularMomentum = Vector3.zero,
		linearMomentum = Vector3.zero,

		-- other state
		force = Vector3.new(0, -mass * GRAVITY, 0),
		torque = Vector3.zero,

		-- inertia
		invInertia = Mat3.toCFrame(Mat3.inverse(inertiaMat)),
		inertia = Mat3.toCFrame(inertiaMat),

		-- mass
		invMass = 1.0 / mass,
		mass = mass,

		-- surface props
		restitution = 0.45,
		friction = 0.4,
		beta = 0.32,
	}
end

function Body.getSystemProps(parts: { BasePart }): (Vector3, number, Mat3)
	-- gets the system properties of a specific system, good for
	-- offsets and stuff ig lol
	local mass = 0.0
	local inertia = Mat3.identity
	local centroid = Vector3.zero

	for i, part in parts do
		-- get the center of mass
		local partMass = part:GetMass()
		centroid += part.Position * partMass
		mass += partMass
	end

	centroid /= mass
	for i, part in parts do
		local partMass = part:GetMass()
		local r = part.Position - centroid

		-- apply parallel axis theorem
		local partInertia = getCubeInertia(partMass, part.Size)
		inertia += (Mat3.identity * r:Dot(r) - Mat3.outerProduct(r)) * partMass
		inertia += Mat3.fromVector(partInertia)
	end

	return centroid, mass, inertia
end

Body.getExtentsSize = getExtentsSize

return Body


--!strict
--!native

-- used to do matrix operations

local Mat3 = {}
Mat3.__index = Mat3

export type Mat3 = typeof(setmetatable({} :: {{ number }}, Mat3))

Mat3.zero = setmetatable({
	{0, 0, 0},
	{0, 0, 0},
	{0, 0, 0},
}, Mat3)

Mat3.identity = setmetatable({
	{1, 0, 0},
	{0, 1, 0},
	{0, 0, 1},
}, Mat3)

function Mat3.fromCFrame(cframe: CFrame): Mat3
	local _, _, _, r00, r01, r02, r10, r11, r12, r20, r21, r22 = cframe:GetComponents()
	return setmetatable({
		{ r00, r01, r02 },
		{ r10, r11, r12 },
		{ r20, r21, r22 },
	}, Mat3)
end

function Mat3.fromVector(vec: Vector3): Mat3
	return setmetatable({
		{vec.X, 0, 0},
		{0, vec.Y, 0},
		{0, 0, vec.Z},
	}, Mat3)
end

function Mat3.outerProduct(vec: Vector3): Mat3
	-- v^T * v
	local x = vec.X
	local y = vec.Y
	local z = vec.Z

	return setmetatable({
		{x*x, x*y, x*z},
		{y*x, y*y, y*z},
		{z*x, z*y, z*z},
	}, Mat3)
end

function Mat3.numMul(self: Mat3, x: number): Mat3
	-- scalar multiplication
	return setmetatable({
		{self[1][1] * x, self[1][2] * x, self[1][3] * x},
		{self[2][1] * x, self[2][2] * x, self[2][3] * x},
		{self[3][1] * x, self[3][2] * x, self[3][3] * x},
	}, Mat3)
end

function Mat3.vecMul(self: Mat3, what: Vector3): Vector3
	-- multiply a mat3 by a vector
	local x = what.X
	local y = what.Y
	local z = what.Z
	return Vector3.new(
		self[1][1] * x + self[2][1] * y + self[3][1] * z,
		self[1][2] * x + self[2][2] * y + self[3][2] * z,
		self[1][3] * x + self[2][3] * y + self[3][3] * z
	)
end

function Mat3.matMul(self: Mat3, what: Mat3): Mat3
	-- we're a matrix
	local product = setmetatable({
		{0, 0, 0},
		{0, 0, 0},
		{0, 0, 0},
	}, Mat3)

	for i = 1, 3 do
		for j = 1, 3 do
			product[i][j] = self[i][1] * what[1][j]
				+ self[i][2] * what[2][j]
				+ self[i][3] * what[3][j]
		end
	end

	return product :: Mat3
end

function Mat3.vecAdd(self: Mat3, what: Vector3): Mat3
	-- it's a vector
	local x = what.X
	local y = what.Y
	local z = what.Z
	return setmetatable({
		{self[1][1] + x, self[1][2], self[1][3]},
		{self[2][1], self[2][2] + y, self[2][3]},
		{self[3][1], self[3][2], self[3][3] + z},
	}, Mat3)
end

function Mat3.matSub(self: Mat3, what: Mat3): Mat3
	-- we're a matrix
	local product = setmetatable({
		{0, 0, 0},
		{0, 0, 0},
		{0, 0, 0}
	}, Mat3)

	for i = 1, 3 do
		for j = 1, 3 do
			product[i][j] = self[i][j] - what[i][j]
		end
	end

	return product
end

function Mat3.matAdd(self: Mat3, what: Mat3): Mat3
	-- we're a matrix
	local product = setmetatable({
		{0, 0, 0},
		{0, 0, 0},
		{0, 0, 0}
	}, Mat3)

	for i = 1, 3 do
		for j = 1, 3 do
			product[i][j] = self[i][j] + what[i][j]
		end
	end

	return product
end

function Mat3.det(self: Mat3): number
	-- A^-1
	local i11 = self[2][2] * self[3][3] - self[2][3] * self[3][2]
	local i21 = self[2][3] * self[3][1] - self[2][1] * self[3][3]
	local i31 = self[2][1] * self[3][2] - self[2][2] * self[3][1]

	-- precompute determinant
	return self[1][1] * i11
		+ self[1][2] * i21
		+ self[1][3] * i31
end

function Mat3.inverse(self: Mat3): Mat3
	-- A^-1-- A^-1
	local i11 = self[2][2] * self[3][3] - self[2][3] * self[3][2]
	local i21 = self[2][3] * self[3][1] - self[2][1] * self[3][3]
	local i31 = self[2][1] * self[3][2] - self[2][2] * self[3][1]

	-- precompute determinant
	local det = self[1][1] * i11
		+ self[1][2] * i21
		+ self[1][3] * i31

	if math.abs(det) < 1e-3 then
		error("lol singular matrix")
	end

	-- calculates it through cofactors + determinant expansion
	-- i dont like it
	local out = {
		{0, 0, 0},
		{0, 0, 0},
		{0, 0, 0},
	}

	local invDet = 1.0 / det
	out[1][1] = i11 * invDet
	out[1][2] = (self[1][3] * self[3][2] - self[1][2] * self[3][3]) * invDet
	out[1][3] = (self[1][2] * self[2][3] - self[1][3] * self[2][2]) * invDet
	out[2][1] = i21 * invDet
	out[2][2] = (self[1][1] * self[3][3] - self[1][3] * self[3][1]) * invDet
	out[2][3] = (self[1][3] * self[2][1] - self[1][1] * self[2][3]) * invDet
	out[3][1] = i31 * invDet
	out[3][2] = (self[1][2] * self[3][1] - self[1][1] * self[3][2]) * invDet
	out[3][3] = (self[1][1] * self[2][2] - self[1][2] * self[2][1]) * invDet

	return setmetatable(out, Mat3)
end

function Mat3.fromMatrix(
	a: Vector3,
	b: Vector3,
	c: Vector3
): Mat3
	return setmetatable({
		{ a.X, b.X, c.X },
		{ a.Y, b.Y, c.Y },
		{ a.Z, b.Z, c.Z },
	}, Mat3)
end

function Mat3.new(
	a: number, b: number, c: number,
	d: number, e: number, f: number,
	g: number, h: number, i: number
): Mat3
	return setmetatable({
		{ a, b, c },
		{ d, e, f },
		{ g, h, i },
	}, Mat3)
end

function Mat3.toCFrame(self: Mat3): CFrame
	-- converts to a cframe for slightly faster
	-- computation
	return CFrame.new(
		0, 0, 0,
		self[1][1], self[1][2], self[1][3],
		self[2][1], self[2][2], self[2][3],
		self[3][1], self[3][2], self[3][3]
	)
end

function Mat3.transpose(self: Mat3): Mat3
	-- A^T
	return setmetatable({
		{ self[1][1], self[2][1], self[3][1]},
		{ self[1][2], self[2][2], self[3][2]},
		{ self[1][3], self[2][3], self[3][3]}
	}, Mat3)
end

function Mat3.__mul(self: Mat3, what: number | Mat3): Mat3
	if typeof(what) == "number" then
		-- scalar multiply
		return Mat3.numMul(self, what)
	else
		-- matrix multiply
		return Mat3.matMul(self, what)
	end
end

Mat3.__sub = Mat3.matSub
Mat3.__add = Mat3.matAdd

return Mat3


--!strict

local BvhBonsaiPrune = require("./builders/BvhBonsaiPrune")
local BvhBinnedSah = require("./builders/BvhBinnedSah")

local Queue = require("./Queue")

local VEC_INF = Vector3.one * math.huge
local FAT_AABB = Vector3.one

-- Truthfully we only care if a node's id
-- is solely FLAG_BRANCH, because that is an internal
-- node made by the bvh. Flags that don't follow this
-- are leaves by default.
local FLAG_BRANCH = 0x1
local FLAG_LEAF = 0x2

local Bvh = {}
Bvh.__index = Bvh

export type Bin = Box & { count: number }
export type Box = {
	center: Vector3,
	min: Vector3,
	max: Vector3,
}

local function sah(box: Box): number
	-- the main cost function of our tree
	local delta = box.max - box.min
	return 2*(delta.X*delta.Y + delta.Z*delta.X + delta.Z*delta.Y)
end

local function unionCost(
	box0: Box,
	box1: Box
): number
	-- gets the cost of combinine two boxes
	-- together. the cost function is just the surface
	-- area of the boxes.
	local delta = box0.max:Max(box1.max) - box0.min:Min(box1.min)
	return 2*(delta.X*delta.Y + delta.Z*delta.X + delta.Z*delta.Y)
end

local function doesAABBCollidePoint(
	min: Vector3,
	max: Vector3,
	point0: Vector3
): boolean
	-- checks if a bounding box and point collides.
	-- consider the case:
	-- A___P___A

	-- in this case, A_min < P < A_max. basically this just
	-- asks if P is bounded within A_min and A_max. also, we're
	-- still using the number line analogy, so this will generalize
	-- to all 3 axes.
	return min.X < point0.X and point0.X < max.X
		and min.Y < point0.Y and point0.Y < max.Y
		and min.Z < point0.Z and point0.Z < max.Z
end

local function doesAABBCollideAABB(aabb0: Box, aabb1: Box): boolean
	-- checks if two aabbs collide
	local min0 = aabb0.min
	local max0 = aabb0.max

	local min1 = aabb1.min
	local max1 = aabb1.max

	return min0.X <= max1.X and max0.X >= min1.X
		and min0.Y <= max1.Y and max0.Y >= min1.Y
		and min0.Z <= max1.Z and max0.Z >= min1.Z
end

local function doesAABBCollideRay(
	node: Box,
	origin: Vector3,
	invDir: Vector3,
	size: Vector3
): number
	-- We know about a + bt = c, where t = (c - a)b
	-- We use this relationship to calculate vMin, vMax
	-- and we assume the T value is within [0, 1]
	local halfSize = size * 0.5
	local min = node.min - halfSize
	local max = node.max + halfSize
	if doesAABBCollidePoint(min, max, origin) then
		-- Premature check
		return 0.0
	end

	local vMin = (min - origin) * invDir
	local vMax = (max - origin) * invDir

	-- Quick branchless optimization
	local rMin = vMin:Min(vMax)
	local rMax = vMax:Max(vMin)

	local tMin = math.max(rMin.X, rMin.Y, rMin.Z)
	local tMax = math.min(rMax.X, rMax.Y, rMax.Z)
	return tMax >= 0
		and tMin <= tMax
		and tMin <= 1
		and tMin >= 0
		and tMin
		or -1
end

export type BvhNode<T> = {
	center: Vector3,
	min: Vector3,
	max: Vector3,

	flag: number,
	left: number,
	right: number,
	id: number,
	parent: number,
	free: number,

	data: T,
}

export type Bvh<T> = typeof(setmetatable({} :: {
	queue: Queue.Queue<number>,
	freeIndex: number,
	nodes: { BvhNode<T> },
	count: number,
	root: number,
}, Bvh))

function Bvh.new<T>(): Bvh<T>
	return setmetatable({
		queue = Queue.new(16, 0),
		freeIndex = 0,
		nodes = {},
		count = 0,
		root = 1,
	}, Bvh)
end

local BvhProxy: Box = {
	center = Vector3.zero,
	min = Vector3.one,
	max = Vector3.one,
}

function Bvh.queryPoint<T>(
	self: Bvh<T>,
	min: Vector3,
	max: Vector3,
	point: Vector3
): { number }
	local nodes = self.nodes
	if not nodes[self.root] then
		-- nothing
		return {}
	end

	local output = {} :: { number }
	local queue = self.queue
	queue:clear()
	queue:enqueue(self.root)

	while not queue:empty() do
		local id = queue:dequeue()
		local node = nodes[id]
		if node.flag ~= FLAG_BRANCH then
			-- leaf node => output
			if doesAABBCollidePoint(node.min, node.max, point) then
				-- add node
				table.insert(output, id)
			end
		else
			-- branch node => queue
			local l = nodes[node.left]
			if l and doesAABBCollidePoint(l.min, l.max, point) then
				-- add left node
				queue:enqueue(node.left)
			end

			local r = nodes[node.right]
			if r and doesAABBCollidePoint(r.min, r.max, point) then
				-- add right node
				queue:enqueue(node.right)
			end
		end
	end

	return output
end

function Bvh.query<T>(
	self: Bvh<T>,
	min: Vector3,
	max: Vector3
): { number }
	local nodes = self.nodes
	if not nodes[self.root] then
		-- nothing
		return {}
	end

	local output = {}
	local queue = self.queue
	queue:clear()
	queue:enqueue(self.root)

	BvhProxy.min = min
	BvhProxy.max = max
	while not queue:empty() do
		local id = queue:dequeue()
		local node = nodes[id]
		if node.flag ~= FLAG_BRANCH then
			-- leaf node => output
			if doesAABBCollideAABB(node, BvhProxy) then
				-- add node
				table.insert(output, id)
			end
		else
			-- branch node => queue
			local l = node.left
			if l ~= 0 and doesAABBCollideAABB(nodes[l], BvhProxy) then
				-- add left node
				queue:enqueue(l)
			end

			local r = node.right
			if r ~= 0 and doesAABBCollideAABB(nodes[r], BvhProxy) then
				-- add right node
				queue:enqueue(r)
			end
		end
	end

	return output
end

function Bvh.trace<T>(
	self: Bvh<T>,
	origin: Vector3,
	direction: Vector3,
	size: Vector3
): { number }
	-- box raycast operation
	local invDir = Vector3.one / direction
	local nodes = self.nodes
	if not nodes[self.root] then
		-- Don't query.
		return {}
	end

	debug.profilebegin("bvh::trace")

	local queue = self.queue
	queue:clear()
	queue:enqueue(self.root)

	local output = {}
	while not queue:empty() do
		local id = queue:dequeue()
		local node = nodes[id]
		if node.flag ~= FLAG_BRANCH then
			-- leaf node => output
			local t = doesAABBCollideRay(node, origin, invDir, size)
			if 0 <= t and t <= 1 then
				-- add left node
				table.insert(output, id)
			end
		else
			-- branch node
			local t0 = doesAABBCollideRay(nodes[node.left], origin, invDir, size)
			if 0 <= t0 and t0 <= 1 then
				-- add left node
				queue:enqueue(node.left)
			end

			local t1 = doesAABBCollideRay(nodes[node.right], origin, invDir, size)
			if 0 <= t1 and t1 <= 1 then
				-- add right node
				queue:enqueue(node.right)
			end
		end
	end

	debug.profileend()

	return output
end

function Bvh.rotate<T>(self: Bvh<T>, index: number)
	assert(index ~= 0, "index cannot be 0")

	if index == self.root then
		-- dont rotate the root
		-- node
		return
	end

	-- collect nodes
	local nodes = self.nodes
	local node = nodes[index]

	local parent = node.parent
	local right = node.right
	local left = node.left

	local parentNode = nodes[parent]
	local rightNode = nodes[right]
	local leftNode = nodes[left]

	local sibling
	if parentNode.left == index then
		sibling = parentNode.right
	else
		sibling = parentNode.left
	end

	local siblingNode = nodes[sibling]
	local leftArea = unionCost(siblingNode, leftNode)
	local rightArea = unionCost(siblingNode, rightNode)
	local area = unionCost(leftNode, rightNode)
	if area <= leftArea and area <= rightArea then
		-- dont rotate
		return
	end

	if leftArea < rightArea then
		-- swap right and sibling
		if parentNode.left == index then
			parentNode.right = right
		else
			parentNode.left = right
		end

		node.right = sibling
		rightNode.parent = parent
	else
		-- swap left and sibling
		if parentNode.left == index then
			parentNode.right = left
		else
			parentNode.left = left
		end

		node.left = sibling
		leftNode.parent = parent
	end

	siblingNode.parent = index
end

function Bvh.refit<T>(self: Bvh<T>, index: number)
	-- stage 3: refit
	local nodes = self.nodes
	while index ~= 0 do
		-- rotate
		local node = nodes[index]
		self:rotate(index)

		local leftNode = nodes[node.left]
		local rightNode = nodes[node.right]
		node.min = leftNode.min:Min(rightNode.min)
		node.max = leftNode.max:Max(rightNode.max)

		index = node.parent
	end
end

function Bvh.remove<T>(self: Bvh<T>, node: BvhNode<T>)
	-- removes a node
	local nodes = self.nodes
	local index = node.id

	local parent = node.parent
	local parentNode = nodes[parent]

	if self.root == index then
		-- add to freelist
		node.free = self.freeIndex
		node.parent = 0

		self.freeIndex = index
		self.root = 0
		return
	end

	local sibling

	-- obtain the sibling index
	if nodes[parentNode.left] == node then
		sibling = parentNode.right
	else
		sibling = parentNode.left
	end

	local newParent = parentNode.parent
	if newParent ~= 0 then
		-- grandparent is a branch node
		local newParentNode = nodes[newParent]
		if newParentNode.left == parent then
			newParentNode.left = sibling
		else
			newParentNode.right = sibling
		end

		-- refit if possible
		-- set sibling parent
		nodes[sibling].parent = newParent

		-- stage 3: refit
		local nodes = self.nodes
		local cursor = newParent
		while cursor ~= 0 do
			-- rotate
			local node = nodes[cursor]
			local leftNode = nodes[node.left]
			local rightNode = nodes[node.right]
			node.min = leftNode.min:Min(rightNode.min)
			node.max = leftNode.max:Max(rightNode.max)

			cursor = node.parent
		end
	else
		-- sibling becomes root node
		-- set sibling parent
		nodes[sibling].parent = 0
		self.root = sibling
	end

	-- add to freelist
	node.free = self.freeIndex
	nodes[parent] = nil

	self.freeIndex = index
end

function Bvh.getFreeIndex<T>(self: Bvh<T>): number
	-- we use this so we dont
	-- need to keep on creating
	-- redundant data. this acts similarly
	-- to a freelist
	local id = self.freeIndex
	if id ~= 0 then
		self.freeIndex = self.nodes[self.freeIndex].free
		return id
	else
		self.count += 1
		return self.count
	end
end

function Bvh.createNode<T>(object: T & {min: Vector3, max: Vector3}): BvhNode<T>
	return {
		center = (object.min + object.max) * 0.5,
		min = object.min,
		max = object.max,

		flag = FLAG_LEAF,
		left = 0,
		right = 0,
		id = 0,
		parent = 0,

		free = 0,
		data = object,
	}
end

function Bvh.getObject<T>(self: Bvh<T>, id: number): T
	-- provides an object given the id
	return self.nodes[id].data
end

function Bvh.insert<T>(self: Bvh<T>, leaf: BvhNode<T>): number
	-- Stage 1: find best sibling
	-- based off of erin catto's dynamic aabb tree
	-- insertion method, where we just find a good enough
	-- sibling node to insert into
	local siblingIndex = self.root
	local nodes = self.nodes

	local leafIndex = self:getFreeIndex()
	leaf.id = leafIndex
	leaf.free = 0

	if not nodes[siblingIndex] then
		leaf.parent = 0
		self.root = leafIndex

		nodes[self.root] = leaf
		return leafIndex
	end

	local area = sah(leaf)
	while siblingIndex do
		local sibling = nodes[siblingIndex]
		if sibling.flag ~= FLAG_BRANCH then
			break
		end

		-- the inherit cost is the surface area added
		-- when attempting to insert a node here.
		local cost = 2*sah(sibling)
		local inheritCost = 2*unionCost(sibling, leaf) - cost
		local r = nodes[sibling.right]
		local l = nodes[sibling.left]

		local leftCost = unionCost(leaf, l) + inheritCost
		if l.flag == FLAG_BRANCH then
			leftCost -= sah(l)
		end

		local rightCost = unionCost(leaf, r) + inheritCost
		if r.flag == FLAG_BRANCH then
			rightCost -= sah(r)
		end

		if cost <= leftCost and cost <= rightCost then
			break
		end

		if leftCost < rightCost then
			siblingIndex = sibling.left
		else
			siblingIndex = sibling.right
		end
	end

	-- stage 2: create new parent
	local sibling = nodes[siblingIndex]
	local parentIndex = sibling.parent
	local branchIndex = self:getFreeIndex()

	-- create aabb
	local branch = {
		min = VEC_INF,
		max = -VEC_INF,
		flag = FLAG_BRANCH,
		right = leafIndex,
		left = siblingIndex,
		parent = parentIndex,
		free = 0,
	} :: BvhNode<T>

	sibling.parent = branchIndex
	leaf.parent = branchIndex

	nodes[branchIndex] = branch
	nodes[leafIndex] = leaf

	if parentIndex ~= 0 then
		-- parent is an existing node
		local parent = nodes[parentIndex]
		if parent.left == siblingIndex then
			parent.left = branchIndex
		else
			parent.right = branchIndex
		end

		self:refit(branchIndex)
	else
		-- parent was the root node
		branch.min = leaf.min:Min(sibling.min)
		branch.max = leaf.max:Max(sibling.max)
		self.root = branchIndex
	end

	return leafIndex
end

export type BvhWork = {
	nodeIndex: number,
	parent: number,
	leftIndex: number,
	rightIndex: number,
}

function Bvh.build<T>(self: Bvh<T>, leaves: { BvhNode<T> })
	-- Do this when the bvh is empty.
	local nodes = {}
	local size = 1

	local buildQueue: Queue.Queue<BvhWork> = Queue.new(16, {} :: BvhWork)
	buildQueue:clear()
	buildQueue:enqueue({
		nodeIndex = 1,
		parent = 0,

		leftIndex = 1,
		rightIndex = #leaves,
	})

	while not buildQueue:empty() do
		local pair = buildQueue:dequeue()
		local count = (pair.rightIndex - pair.leftIndex) + 1
		if count == 1 then
			-- leaf case
			local node = leaves[pair.leftIndex]
			node.flag = FLAG_LEAF
			node.parent = pair.parent
			nodes[pair.nodeIndex] = node
			continue
		elseif count == 2 then
			-- branch case
			local right = leaves[pair.rightIndex]
			local left = leaves[pair.leftIndex]

			local min = left.min:Min(right.min)
			local max = left.max:Max(right.max)
			local branch = {
				min = min,
				max = max,
				center = (min + max) * 0.5,
				parent = pair.parent,
				left = size + 1,
				right = size + 2,
				flag = FLAG_BRANCH,
			} :: BvhNode<T>

			right.parent = pair.nodeIndex
			left.parent = pair.nodeIndex

			right.flag = FLAG_LEAF
			left.flag = FLAG_LEAF

			nodes[pair.nodeIndex] = branch
			nodes[size + 1] = left
			nodes[size + 2] = right

			size += 2
			continue
		elseif count < 2 then
			-- invalid case (count = 0)
			error("bvh! invalid partition lol")
		end

		-- create a branch node
		local branch = {
			max = -VEC_INF,
			min = VEC_INF,
			center = Vector3.zero,
			parent = pair.parent,
			left = size + 1,
			right = size + 2,
			flag = FLAG_BRANCH,
		} :: BvhNode<T>

		-- find the split, which in this case
		-- we'll use a binned sah
		local partition = BvhBinnedSah(
			leaves :: { BvhBinnedSah.Node },
			branch,
			pair.leftIndex,
			pair.rightIndex
		)

		-- add to work
		buildQueue:enqueue({
			nodeIndex = size + 1,
			parent = pair.nodeIndex,

			leftIndex = pair.leftIndex,
			rightIndex = partition - 1,
		})

		buildQueue:enqueue({
			nodeIndex = size + 2,
			parent = pair.nodeIndex,

			leftIndex = partition,
			rightIndex = pair.rightIndex,
		})

		size += 2
		nodes[pair.nodeIndex] = branch
	end

	assert(#nodes == size, "bvh! num nodes != size")
	self.nodes = nodes
	self.count = size
	self.root = 1

	-- do the bonsai prune to improve static tree cost
	BvhBonsaiPrune(
		self :: BvhBonsaiPrune.Bvh,
		nodes :: { BvhBonsaiPrune.Box },
		self.queue
	)
end

function Bvh.getCost<T>(self: Bvh<T>): number
	-- get the bvh cost
	local nodes = self.nodes
	local queue = self.queue
	queue:clear()
	queue:enqueue(self.root)

	local cost = 0.0
	while #queue > 0 do
		local pop = nodes[queue:dequeue()]
		if pop and pop.flag == FLAG_BRANCH then
			queue:enqueue(pop.left)
			queue:enqueue(pop.right)
			cost += sah(pop)
		end
	end

	return cost
end

function Bvh.forEach<T>(self: Bvh<T>, callback: (leaf: BvhNode<T>) -> ())
	local queue = self.queue
	queue:clear()
	queue:enqueue(self.root)

	local nodes = self.nodes
	while not queue:empty() do
		local id = queue:dequeue()
		local object = nodes[id]
		if object.flag ~= FLAG_BRANCH then
			callback(object)
		else
			if object.left ~= 0 then
				queue:enqueue(object.left)
			end

			if object.right ~= 0 then
				queue:enqueue(object.right)
			end
		end
	end
end

function Bvh.draw<T>(self: Bvh<T>)
	local dbg = Instance.new("Folder", workspace)
	-- debugger
	local nodes = self.nodes
	local queue = self.queue
	queue:enqueue(self.root)
	dbg:ClearAllChildren()

	while not queue:empty() do
		local id = queue:dequeue()
		local element = nodes[id]
		if not element then
			continue
		end

		local min = element.min
		local max = element.max

		local part = Instance.new("Part")
		part.Anchored = true
		part.CFrame = CFrame.new((min+max)*0.5)
		part.Size = max - min
		part.CanCollide = false
		part.CanQuery = false
		part.CanTouch = false
		part.Transparency = 1.0

		local sel = Instance.new("SelectionBox")
		sel.LineThickness = 0.05
		sel.Adornee = part
		sel.Parent = part
		part.Parent = dbg

		if element.flag == FLAG_BRANCH then
			queue:enqueue(element.left)
			queue:enqueue(element.right)
		end
	end
end

function Bvh.update<T>(
	self: Bvh<T>,
	id: number,
	min0: Vector3,
	max0: Vector3
)
	local node = self.nodes[id]
	local min1 = node.min - FAT_AABB
	local max1 = node.max + FAT_AABB

	if min1.X <= min0.X and min1.Y <= min0.Y and min1.Z <= min0.Z
		and max0.X <= max1.X and max0.Y <= max1.Y and max0.Z <= max1.Z then
		-- no need to update because the fat aabb still persists
		return
	end

	-- reinsert because it is outside of the aabb range
	node.min = min0 - FAT_AABB
	node.max = max0 + FAT_AABB

	self:remove(node)
	self:insert(node)
end

return Bvh


--!strict

-- Queue.lua
-- Handles the queue stuff i guess

local Queue = {}
Queue.__index = Queue

export type Queue<T> = typeof(setmetatable({} :: {
	left: number,
	right: number,
	count: number,

	mask: number,
	data: { T },
}, Queue))

function Queue.new<T>(capacityPow: number, initial: T): Queue<T>
	local mask = bit32.lshift(1, capacityPow) - 1
	local data = table.create(mask + 1, initial)

	return setmetatable({
		left = 0,
		right = 0,
		count = 0,

		data = data,
		mask = mask,
	}, Queue)
end

function Queue.clear<T>(self: Queue<T>)
	self.count = 0
	self.right = 0
	self.left = 0
end

function Queue.empty<T>(self: Queue<T>)
	return self.count <= 0
end

function Queue.peek<T>(self: Queue<T>)
	return self.data[self.left + 1]
end

function Queue.dequeue<T>(self: Queue<T>)
	local id = self.left + 1
	self.left = bit32.band(self.left + 1, self.mask)
	self.count -= 1

	return self.data[id]
end

function Queue.enqueue<T>(self: Queue<T>, data: T)
	local id = self.right + 1
	self.right = bit32.band(self.right + 1, self.mask)
	self.count += 1

	assert(self.count <= self.mask, "queue overflow")

	self.data[id] = data
end

return Queue


--!strict

-- binned sah builder ejemplo

local VEC_INF = Vector3.one * math.huge
local BINS = 8

export type Bin = Node & { count: number }
export type Node = {
	center: Vector3,
	min: Vector3,
	max: Vector3,
}

local BvhSahAxes: { Vector3 } = table.freeze({
	Vector3.new(1, 0, 0),
	Vector3.new(0, 1, 0),
	Vector3.new(0, 0, 1),
})

local function getNodeCost(node: Bin): number
	-- gets the cost of a node, if it has no leaves
	-- then it has the highest cost
	if node.count == 0 then
		return math.huge
	end

	local delta = node.max - node.min
	return 2*(delta.X*delta.Y + delta.Z*delta.X + delta.Z*delta.Y)*node.count
end

local function createEmptyBbox(): Bin
	return {
		center = Vector3.zero,
		count = 0,
		max = -VEC_INF,
		min = VEC_INF,
	}
end

return function(
	leaves: { Node },
	box: Node,
	leftIndex: number,
	rightIndex: number
): number
	for i = leftIndex, rightIndex do
		local node = leaves[i]
		box.min = box.min:Min(node.min)
		box.max = box.max:Max(node.max)
	end

	local delta = box.max - box.min
	local costs = {}

	local bestCost = math.huge
	local bestAxis = 1
	local bestBin = 1

	for i, axis in BvhSahAxes do
		-- Create bins
		local dot = delta:Dot(axis)
		if dot < 1e-2 then
			-- Don't use this.
			continue
		end

		local invSplitT = (BINS - 1) / dot
		local bins = {} :: { Bin }
		for j = 1, BINS do
			bins[j] = createEmptyBbox()
		end

		-- Populate bins
		local minBounds = box.min:Dot(axis)
		for j = leftIndex, rightIndex do
			local node = leaves[j]
			local index = (node.center:Dot(axis) - minBounds) * invSplitT
			index = (index // 1) + 1

			local bin = bins[index]
			bin.min = bin.min:Min(node.min)
			bin.max = bin.max:Max(node.max)
			bin.count += 1
		end

		-- Populate costs
		local rightBox = createEmptyBbox()
		local leftBox = createEmptyBbox()
		for j = 1, BINS do
			local leftBin = bins[j]
			if leftBin.count > 0 then
				leftBox.min = leftBox.min:Min(leftBin.min)
				leftBox.max = leftBox.max:Max(leftBin.max)
				leftBox.count += leftBin.count
			end

			costs[j] = getNodeCost(leftBox)
		end

		-- Add costs
		for j = BINS, 1, -1 do
			costs[j] += getNodeCost(rightBox)

			local rightBin = bins[j]
			if rightBin.count > 0 then
				rightBox.min = rightBox.min:Min(rightBin.min)
				rightBox.max = rightBox.max:Max(rightBin.max)
				rightBox.count += rightBin.count
			end
		end

		-- Get minimum cost
		for j = 1, BINS do
			local cost = costs[j]
			if cost < bestCost then
				bestCost = cost
				bestAxis = i
				bestBin = j
			end
		end
	end

	local axis = BvhSahAxes[bestAxis]
	assert(bestBin)

	-- Partition data along the split position through swaps.
	local splitPosition = (box.min + (bestBin / (BINS + 1)) * delta):Dot(axis)
	local left = leftIndex
	local right = rightIndex
	while left < right do
		if leaves[left].center:Dot(axis) >= splitPosition then
			while left < right and leaves[right].center:Dot(axis) >= splitPosition do
				right -= 1
			end

			-- swap candidates
			leaves[left], leaves[right] = leaves[right], leaves[left]
		end

		left += 1
	end

	if left <= leftIndex + 1 or left >= rightIndex - 1 then
		return (leftIndex + rightIndex) // 2
	else
		return left
	end
end


--!strict

-- optimization phase during construction
-- where we just rotate through the entire tree
-- and make pruned mini trees. good for static stuff

local Queue = require("../Queue")

local FLAG_LEAF = 0x2

export type Box = {
	flag: number,
	center: Vector3,
	min: Vector3,
	max: Vector3,
	free: number,
	left: number,
	right: number,
	parent: number,
}

local function sah(box: Box): number
	-- the main cost function of our tree
	local delta = box.max - box.min
	return delta.X*delta.Y + delta.Z*delta.X + delta.Z*delta.Y
end

export type Bvh = typeof(setmetatable({} :: {
	root: number,
	freeIndex: number,
}, {
	__index = {} :: {
		rotate: (self: Bvh, id: number) -> (),
		insert: (self: Bvh, node: Box) -> (),
	}
}))

return function<T>(
	bvh: Bvh,
	nodes: { Box },
	queue: Queue.Queue<number>
)
	-- do rotations from the entire tree, bottom-up
	local function dfs(id: number)
		local node = nodes[id]
		if node.flag == FLAG_LEAF then
			return
		end

		dfs(node.left)
		dfs(node.right)

		bvh:rotate(id)

		local leftNode = nodes[node.left]
		local rightNode = nodes[node.right]
		node.min = leftNode.min:Min(rightNode.min)
		node.max = leftNode.max:Max(rightNode.max)
	end

	dfs(bvh.root)

	-- create a queue
	local root = bvh.root
	local threshold = sah(nodes[root]) * 0.05
	local mini = {} :: { number }

	while not queue:empty() do
		local id = queue:dequeue()
		local node = nodes[id]
		assert(node)

		if node.flag == FLAG_LEAF then
			-- dont continue if its a leaf node
			-- but do track the parent
			table.insert(mini, id)
			continue
		elseif sah(node) > threshold then
			-- branch node violates the property. continue
			-- dfs
			queue:enqueue(node.right)
			queue:enqueue(node.left)
		else
			-- branch node that does not violate property
			-- use this as your new mini tree
			local parent = node.parent
			while nodes[parent] do
				local parentNode = nodes[parent]
				nodes[parent] = nil

				-- add to freelist
				parentNode.free = bvh.freeIndex
				bvh.freeIndex = parent

				parent = parentNode.parent
			end
			table.insert(mini, id)
		end
	end

	for i, nodeId in mini do
		-- Insert a tiny subset of nodes
		-- in there.
		local node = nodes[nodeId]
		bvh:insert(node)
	end
end


--!strict

-- sweep sah has a nlogn penality because of
-- sorting the data. however, it minimizes costs the most
-- using bonsai prune

local VEC_INF = Vector3.one * math.huge

export type Node = {
	center: Vector3,
	min: Vector3,
	max: Vector3,
}

local function sah(delta: Vector3): number
	-- the main cost function of our tree
	return delta.X*delta.Y + delta.Z*delta.X + delta.Z*delta.Y
end

local function quickSort(
	array: { Node },
	l: number,
	r: number,
	axis: Vector3
)
	if l >= r then
		return
	end

	-- i chose the middle partition
	local pivot = array[(l+r)//2].center:Dot(axis)
	local eq = l
	local lt = l
	local gt = r

	while eq <= gt do
		local dot = array[eq].center:Dot(axis)
		if dot < pivot then
			-- left hand side
			array[eq], array[lt] = array[lt], array[eq]
			lt += 1
			eq += 1
		elseif dot > pivot then
			-- right hand side
			array[eq], array[gt] = array[gt], array[eq]
			gt -= 1
		else
			-- middle
			eq += 1
		end
	end

	quickSort(array, l, lt-1, axis)
	quickSort(array, gt+1, r, axis)
end

local BvhAxes = {
	Vector3.new(1, 0, 0),
	Vector3.new(0, 1, 0),
	Vector3.new(0, 0, 1),
}

export type Box = {
	min: Vector3,
	max: Vector3,
}

return function(
	box: Box,
	leaves: { Node },
	start: number,
	finish: number
): number
	for i = start, finish do
		-- union box
		local leaf = leaves[i]
		box.min = box.min:Min(leaf.min)
		box.max = box.max:Max(leaf.max)
	end

	local bestIndex = -1
	local bestCost = math.huge
	local bestAxis

	local length = (finish - start)+1
	local costs = table.create(length + 1, 0)
	for j, axis in BvhAxes do
		-- sort indicies
		quickSort(
			leaves,
			start,
			finish,
			axis
		)

		local boxMax = -VEC_INF
		local boxMin = VEC_INF
		for i = 1, length do
			-- Go from the right to the left side
			local leaf = leaves[finish + 1 - i]
			boxMin = boxMin:Min(leaf.min)
			boxMax = boxMax:Max(leaf.max)

			costs[length - i + 1] = sah(boxMax - boxMin) * i
		end

		boxMax = -VEC_INF
		boxMin = VEC_INF
		for i = 1, length - 1 do
			-- go from the left to the right side
			local leaf = leaves[start + i - 1]
			boxMin = boxMin:Min(leaf.min)
			boxMax = boxMax:Max(leaf.max)

			local cost = sah(boxMax - boxMin)*i + costs[i+1]
			if cost < bestCost then
				bestCost = cost
				bestAxis = axis
				bestIndex = leaf.center:Dot(axis)
			end
		end
	end

	-- unstable partition
	local ptr = start
	while ptr < finish and leaves[ptr].center:Dot(bestAxis) <= bestIndex do
		ptr += 1
	end

	for i = ptr + 1, finish do
		if leaves[i].center:Dot(bestAxis) <= bestIndex then
			-- swap to preserve partition property
			-- that is, <=[start, ptr-1] >[ptr, finish]
			leaves[i], leaves[ptr] = leaves[ptr], leaves[i]
			ptr += 1
		end
	end

	-- 	LHS			RHS
	-- [start l-1] [l finish]
	return math.clamp(ptr-1, start, finish-1)
end


local World = require("@self/World")

export type Support = World.Support
export type World = World.World
export type Body = World.Body
export type Bvh = World.Bvh

return {
	World = require("@self/World"),
}


--!native
--!strict

local Body = require("../Body")

local SupportArbiter = require("./hulls/SupportArbiter")
local MeshArbiter = require("./hulls/MeshArbiter")

local Arbiter = {}

type Body = Body.Body

export type Arbiter = {
	-- abstract base class :3
	new: (Body, Body) -> Arbiter?,
	update: (Arbiter, number) -> Arbiter?,
	solve: (Arbiter, number) -> (),
}

return function(bodyA: Body, bodyB: Body): Arbiter?
	if bodyA.smooth or bodyB.smooth then
		return SupportArbiter.new(bodyA, bodyB) :: Arbiter
	else
		return MeshArbiter.new(bodyA, bodyB) :: Arbiter
	end
end


--!native
--!strict

local Body = require("../Body")

--[[
	The high level overview of what I'm doing here is
	basically making our old state vector[ie. the previous velocities of A and B]
	no longer violate a constraint. A constraint is satisfied when it is 0, or if it
	is a conditional constraint, when that condition is satisfied.

	We derive the impulse model through the following:
		Jv + b = 0
		M(v_2 - v_1) = L*lambda

	where J is the Jacobian matrix, and b is the baumgarte stabilization term.
	B accounts for positional error, and J defines what the constraints for the state
	velocities are.

	A key insight is that L = J^T, because Jv + b defines some hyperplane. And, we know
	that the direction of most increase in perpendicular to the plane, which is towards the
	normal of the plane. Thus, we get:
		(v_2 - v_1) = M^-1 * (J^T * lambda)
		v_2 = M^-1 * (J^T * lambda) + v_1

	After finding the next state vector, we essentially find a new state vector that
	does satisfy the constraint plane. From backsubstitution, we get:
		J(M^-1 * J^T * lambda + v_1) + b = 0
		lambda = -(Jv_1 + b) / (J * M^-1 * J^T)

	A crucial insight is that J a row vector that represents where we want our system state
	to move to(so it's 1 x N). Moreover, M^-1 is actually in the inverse mass matrix, ie:
		M = [ m_a 0 0 0 ]
		    [ 0 i_a 0 0 ]
		    [ 0 0 m_b 0 ]
		    [ 0 0 0 i_b ]

	which is (N x N). Thus, if we did:
		J * (M^-1 * J^T) <=> [1 x N] x [(N x N) x (N x 1)]
		<=> [1 x 1], a scalar!

	We call this scalar the effective mass matrix.
	Since we can now compute lambda, our next state vector is:
		v_2 = M^-1 * (J^T * lambda) + v_1
]]

local FLAG_NORMAL = 0x1
local FLAG_TANGENT = 0x2

local ROLL_DAMPEN = 0.05

local Jacobian = {}
local Contact = {}
Contact.__index = Contact

export type Body = Body.Body
export type Contact = typeof(setmetatable({} :: {
	bodyA: Body,
	bodyB: Body,

	-- Normal
	friction: number,
	normal: Vector3,

	-- Offsets
	rA: Vector3,
	rB: Vector3,

	-- Relative
	relativeA: Vector3,
	relativeB: Vector3,

	-- Jacobians
	jN: Jacobian,
	jT: Jacobian,
	jB: Jacobian,
}, Contact))

export type Jacobian = {
	-- State variables
	flag: number,
	jva: Vector3,
	jwa: Vector3,
	jvb: Vector3,
	jwb: Vector3,

	-- Final bias
	bias: number,
	effMass: number,
	totLambda: number,
}

local function getOrthogonal(normal: Vector3): (Vector3, Vector3)
	if math.abs(normal.Y) > 0.99 then
		-- It's up, so just pick the other two
		-- basis vectors.
		return Vector3.xAxis, Vector3.zAxis
	else
		-- tangent = y x normal
		-- bitangent = tangent x normal
		local tangent = Vector3.new(normal.Z, 0, -normal.X)
		local bitangent = tangent:Cross(normal)
		return tangent.Unit, bitangent.Unit
	end
end

local function safeUnit(v: Vector3): Vector3
	-- Only get the unit vector if
	-- its length is greater than sqrt(1e-3)
	if v:Dot(v) < 1e-3 then
		return Vector3.zero
	else
		return v.Unit
	end
end

local function solveJacobian(
	self: Jacobian,
	contact: Contact,
	dt: number
)
	-- lambda = -(J*v_1 + b) / (J * M^-1 * J^T)
	-- calculate J*v_1
	local a = contact.bodyA
	local b = contact.bodyB
	local jv = self.jva:Dot(a.linearVelocity)
		+ self.jwa:Dot(a.angularVelocity)
		+ self.jvb:Dot(b.linearVelocity)
		+ self.jwb:Dot(b.angularVelocity)

	-- Ensure that sum lambda_i >= 0, cannot clamp
	-- lambda on its own. Lambda is the difference between
	-- the clamped and previous lambda
	local effMass = self.effMass

	local lambda = -(jv + self.bias) * effMass
	local previousLambda = self.totLambda
	if self.flag == FLAG_TANGENT then
		-- apply friction
		local friction = contact.friction * contact.jN.totLambda
		self.totLambda = math.clamp(self.totLambda + lambda, -friction, friction)
		a.angularVelocity -= a.invInertia * safeUnit(a.angularVelocity) * ROLL_DAMPEN * friction
		b.angularVelocity -= b.invInertia * safeUnit(b.angularVelocity) * ROLL_DAMPEN * friction
	else
		-- clamp lambda >= 0
		self.totLambda = math.max(self.totLambda + lambda, 0.0)
	end

	lambda = self.totLambda - previousLambda
	a.linearVelocity += a.invMass * self.jva * lambda
	a.angularVelocity += a.invInertia * self.jwa * lambda

	b.linearVelocity += b.invMass * self.jvb * lambda
	b.angularVelocity += b.invInertia * self.jwb * lambda
end

local function createJacobian(): Jacobian
	return {
		-- state variables
		flag = 0,
		jva = Vector3.zero,
		jwa = Vector3.zero,
		jvb = Vector3.zero,
		jwb = Vector3.zero,

		-- warm starting
		bias = 0.0,
		totLambda = 0.0,
		effMass = 0.0,
	}
end

local function initJacobian(
	jacobian: Jacobian,
	a: Body,
	b: Body,

	normal: Vector3,
	rA: Vector3,
	rB: Vector3,
	flag: number,
	depth: number
)
	-- state variables
	local jva = -normal
	local jwa = -rA:Cross(normal)
	local jvb = normal
	local jwb = rB:Cross(normal)

	--[[
		C: (b - a) . n >= 0
		C': ((v_b + w_b x r_b) - (v_a + w_a x r_a)). n >= 0
		C': ((v_b - r_b x w_b) - (v_a - r_a x w_a)). n >= 0

		Know: a x b . c = c x a . b
		C': -(r_b x w_b).n + (r_a x w_a).n + (v_b - v_a).n
		C': -(n x r_b).w_b + (n x r_a).w_a + (v_b - v_a).n

		Want: C' = JV + b
		J = [ -n^T (n x r_a)^T n^T -(n x r_b)^T ]
		J = [ -n^T -(r_a x n)^T n^T (r_b x n)^T ]
		V = [ v_a ]
			[ w_a ]
			[ v_b ]
			[ w_b ]

		M_eff = 1.0 / (J * M^-1 * J^T => J_i.(M_i * J_i))
	]]

	local effMass = a.invMass
		+ (a.invInertia * jwa):Dot(jwa)
		+ b.invMass
		+ (b.invInertia * jwb):Dot(jwb)

	local bias = 0.0
	if flag == FLAG_NORMAL then
		--[[
			Doing the bias term
			C: (b - a) . n >= 0

			Baumgarte stabilization term(error = C)
			b = -B/dt * C
			b = -B/dt * (b-a).n

			Restitution(dependant on C')
			c = C'.n
		]]

		local velocityConstraint = -a.linearVelocity
			- a.angularVelocity:Cross(rA)
			+ b.linearVelocity
			+ b.angularVelocity:Cross(rB)

		local projected = velocityConstraint:Dot(normal)
		local restitution = a.restitution * b.restitution
		local beta = a.beta * b.beta

		bias = restitution * projected - 120 * beta * depth
	end

	-- state variables
	jacobian.flag = flag
	jacobian.jva = jva
	jacobian.jwa = jwa
	jacobian.jvb = jvb
	jacobian.jwb = jwb

	-- warm starting
	jacobian.bias = bias
	jacobian.totLambda = 0.0
	jacobian.effMass = 1.0 / effMass
end

function Contact.update(
	self: Contact,
	rA: Vector3,
	rB: Vector3,
	normal: Vector3,
	depth: number
)
	local tangent, bitangent = getOrthogonal(normal)
	local bodyA = self.bodyA
	local bodyB = self.bodyB

	-- Set state
	self.relativeA = bodyA.cframe:VectorToObjectSpace(rA)
	self.relativeB = bodyB.cframe:VectorToObjectSpace(rB)
	self.normal = normal
	self.rA = rA
	self.rB = rB

	initJacobian(self.jN, bodyA, bodyB, normal, rA, rB, FLAG_NORMAL, depth)
	initJacobian(self.jB, bodyA, bodyB, bitangent, rA, rB, FLAG_TANGENT, depth)
	initJacobian(self.jT, bodyA, bodyB, tangent, rA, rB, FLAG_TANGENT, depth)
end

function Contact.new(
	bodyA: Body,
	bodyB: Body,

	normal: Vector3,
	rA: Vector3,
	rB: Vector3,

	depth: number
): Contact
	local tangent, bitangent = getOrthogonal(normal)
	local jN = createJacobian()
	local jT = createJacobian()
	local jB = createJacobian()

	initJacobian(jB, bodyA, bodyB, bitangent, rA, rB, FLAG_TANGENT, depth)
	initJacobian(jT, bodyA, bodyB, tangent, rA, rB, FLAG_TANGENT, depth)
	initJacobian(jN, bodyA, bodyB, normal, rA, rB, FLAG_NORMAL, depth)

	return setmetatable({
		relativeA = bodyA.cframe:VectorToObjectSpace(rA),
		relativeB = bodyB.cframe:VectorToObjectSpace(rB),

		bodyA = bodyA,
		bodyB = bodyB,

		friction = bodyA.friction * bodyB.friction,
		normal = normal,

		rA = rA,
		rB = rB,

		jN = jN,
		jB = jB,
		jT = jT,
	}, Contact)
end

function Contact.solve(self: Contact, dt: number)
	-- Solve the jacobian
	solveJacobian(self.jN, self, dt)
	solveJacobian(self.jT, self, dt)
	solveJacobian(self.jB, self, dt)
end

return Contact


--!strict

local SupportHull = require("./hulls/SupportHull")
local MeshHull = require("./hulls/MeshHull")

export type Hull = {
	-- Hull abstract base class :3
	new: (...any) -> Hull,
	update: (Hull, CFrame, Vector3) -> (),
	raycast: (Hull, Vector3, Vector3) -> (number, Vector3, Vector3),

	-- fields
	support: (Vector3) -> Vector3,
	center: Vector3,
}

export type Support = SupportHull.Support
export type Face = MeshHull.Face

export type SupportHull = SupportHull.SupportHull
export type MeshHull = MeshHull.MeshHull

return {
	SupportHull = SupportHull,
	MeshHull = MeshHull,
}


--!strict

--[[
	MPR attempts to find a closest shape that collides with a ray.
	ie, like a GJK which is biased on hitting a ray.

	In essence, we are raycasting through the minkowski sum of an entire shape, so
	think of the way Quake derived games do a "box trace," which in reality,
	is just a raycast through an inflated version of the scene geometry.
]]

-- see https://arxiv.org/pdf/2304.07357
local EPSILON = 1e-3

-- This needs to be precise.
local MPR_DISCOVER_STEPS = 6
local MPR_REFINE_STEPS = 16

local SQR_ITERATIONS = 5

local function sameSide(a, b)
	-- This allows us to check dot products
	-- without needing to fix them first.
	return (a<0) == (b<0)
end

local function tripleProduct(a: Vector3, b: Vector3, c: Vector3)
	-- a x (b x c) = b(a.c) - c(a.b)
	return b*a:Dot(c) - c*a:Dot(b)
end

export type Support = (direction: Vector3) -> Vector3

return function(
	origin: Vector3,
	supp: Support,
	direction: Vector3
): (number, Vector3)
	local v0 = direction.Unit

	-- Finds a candidate portal.
	-- The portal is a triangle, but the
	-- overall simplex is a tetrahedron.
	local v1 = supp(-v0) - origin
	local v2 = supp(v1:Cross(v0)) - origin
	local v3 = supp((v2-v0):Cross(v1-v0)) - origin

	-- Now, try to find a portal
	-- whose triangle will collide
	-- with the origin ray.
	for i = 1, MPR_DISCOVER_STEPS do
		-- In this step, we check if our tetrahedron
		-- planes would collide with the origin.

		-- We only care about cases where the triangle
		-- shares a vertex with V0.
		local n012 = (v1-v0):Cross(v2-v0)
		local n023 = (v2-v0):Cross(v3-v0)
		local n031 = (v3-v0):Cross(v1-v0)

		-- If find a triangle which can see the origin point, make sure the normals
		-- are outwards facing(ie. away from the centeroid of the tetrahedron)
		-- such that we can replace the complement vertex with the proposed support point
		if sameSide(n012:Dot(v3), -n012:Dot(v0)) then
			-- Our origin lies on t_012
			if n012:Dot(v0) < 0.0 then
				v1, v2 = v2, v1
				n012 *= -1.0
			end

			v3 = supp(n012) - origin
			if v3:Dot(n012) < 0.0 then
				return 1, Vector3.zero
			end
		elseif sameSide(n023:Dot(v1), -n023:Dot(v0)) then
			-- Our origin lies on t_023
			if n023:Dot(v0) < 0.0 then
				v2, v3 = v3, v2
				n023 *= -1.0
			end

			v1 = supp(n023) - origin
			if v1:Dot(n023) < 0.0 then
				return 1, Vector3.zero
			end
		elseif sameSide(n031:Dot(v2), -n031:Dot(v0)) then
			-- Our origin lies on t_031
			if n031:Dot(v0) < 0.0 then
				v3, v1 = v1, v3
				n031 *= -1.0
			end

			v2 = supp(n031) - origin
			if v2:Dot(n031) < 0.0 then
				return 1, Vector3.zero
			end
		else
			-- This means our ray collides with our tetrahedron
			-- shape.
			break
		end
	end

	-- Check if our figure can generate a root
	local v12 = v2-v1
	local v13 = v3-v1

	local normal = v12:Cross(v13)
	local proj = v0 * (v1:Dot(normal) / v0:Dot(normal))
	if tripleProduct(v12, v12, v13):Dot(proj-v1) > 0.0 then
		-- Facing towards line V1, V2
		return 1, Vector3.zero
	elseif tripleProduct(-v13, v12, v13):Dot(proj-v1) > 0.0 then
		-- Facing towards line V1, V3
		return 1, Vector3.zero
	elseif tripleProduct(v3-v2, v12, v13):Dot(proj-v2) > 0.0 then
		-- Facing towards line V2, V3
		return 1, Vector3.zero
	end

	-- Since we found a triangle that collides with our ray,
	-- we can refine it so it minimizes the distance from the ray
	-- and the plane the triangle will form.
	for i = 1, MPR_REFINE_STEPS do
		normal = (v3 - v1):Cross(v2 - v1)
		if normal:Dot(v0) > 0.0 then
			v2, v3 = v3, v2
			normal *= -1.0
		end

		local p = supp(normal) - origin
		if v1:Dot(normal) > (p:Dot(normal) + 1e-3) then
			break
		elseif math.abs((p - v1):Dot(normal)) < 1e-3 then
			break
		end

		-- This is the second phase, where
		-- we just check which triangles are either
		-- inside or outside.

		-- Triangle v4,v0,v1
		local cross = p:Cross(v1)
		if cross:Dot(v0) > 0.0 then
			-- Inside v4,v0,v1
			cross = p:Cross(v2)
			if cross:Dot(v0) < 0.0 then
				-- Inside triangles
				-- v4,v0,v1
				-- v4,v2,v0
				v3 = p
			else
				-- Inside triangles
				-- v4,v0,v1
				-- v4,v0,v3
				v1 = p
			end
		else
			-- Outside v4,v0,v1
			cross = p:Cross(v3)
			if cross:Dot(v0) < 0.0 then
				-- Outside triangles
				-- v4,v0,v1
				-- v4,v0,v3
				v1 = p
			else
				-- Outside triangles
				-- v4,v0,v1
				-- v4,v0,v2
				v2 = p
			end
		end
	end

	-- dot(v0 * lambda, normal) = dot(v1, normal)
	-- lambda = dot(v1, normal) / dot(v0, normal)
	local t = v1:Dot(normal) / (v0:Dot(normal) * direction.Magnitude)
	if 0.0 <= t and t <= 1.0 then
		return t, normal.Unit
	end

	return 1, Vector3.zero
end


--!native
--!strict

--[[
	This uses Casey's GJK shortcut which is optimized
	for yes/no intersection tests. This gives pretty poor
	closest point simplexes though.

	Works on both types of hulls.
]]

local MAX_GJK_ITRS = 8

export type Support = (direction: Vector3) -> Vector3

local function tripleProduct(
	a: Vector3,
	b: Vector3,
	c: Vector3
): Vector3
	-- (a x b) x c
	-- -c x (a x b)
	-- b(c.a) - a(c.b)
	return b * c:Dot(a) - a * c:Dot(b)
end

return function(
	suppA: Support,
	suppB: Support,
	direction: Vector3
): boolean
	local simplex = { suppA(direction) - suppB(-direction) }
	local length = 1
	direction *= -1.0

	for i = 1, MAX_GJK_ITRS do
		local a = suppA(direction) - suppB(-direction)
		if a:Dot(direction) <= 0.1 then
			-- behind the direction, that means that we found
			-- a separation, so no collision
			return false
		end

		table.insert(simplex, a)
		length += 1

		if length == 1 then
			-- point case
			direction = -a
		elseif length == 2 then
			-- line case
			local b = simplex[1]
			local ab = b - a
			local ao = -a

			-- ab x ao x ab
			direction = ab * ab:Dot(a)
				- a*ab:Dot(ab)
		elseif length == 3 then
			-- triangle case
			local b = simplex[2]
			local c = simplex[1]

			local ab = b - a
			local ac = c - a

			-- ac x ab x ab
			local abPerp = tripleProduct(ac, ab, ab)

			-- ab x ac x ac
			local acPerp = tripleProduct(ab, ac, ac)
			if abPerp:Dot(a) < 0.0 then
				table.remove(simplex, 1)
				direction = abPerp
				length -= 1
			elseif acPerp:Dot(a) < 0.0 then
				table.remove(simplex, 2)
				direction = acPerp
				length -= 1
			else
				-- ontop or beneath the triangle
				local abc = ab:Cross(ac)
				direction = -abc * math.sign(abc:Dot(a))
			end
		elseif length == 4 then
			-- tetrahedron case
			local b, c, d = simplex[3], simplex[2], simplex[1]
			local ab, ac, ad = b - a, c - a, d - a
			local abc = ab:Cross(ac)
			local acd = ac:Cross(ad)
			local adb = ad:Cross(ab)

			abc *= -math.sign(abc:Dot(ad))
			acd *= -math.sign(acd:Dot(ab))
			adb *= -math.sign(adb:Dot(ac))

			if abc:Dot(a) < 0.0 then
				-- facing infront of abc
				table.remove(simplex, 1)
				direction = abc
				length -= 1
			elseif acd:Dot(a) < 0.0 then
				-- facing infront of acd
				table.remove(simplex, 2)
				direction = acd
				length -= 1
			elseif adb:Dot(a) < 0.0 then
				-- facing infront of adb
				table.remove(simplex, 3)
				direction = adb
				length -= 1
			else
				-- inside of tetrahedron, end this
				return true
			end
		end
	end

	return false
end


--!native
--!strict

--[[
	Utility to clip faces to generate a proper contact manifold.
	This is used for the GJK/SAT hybrid, and the main SAT callback.

	Moreover, this optimizes our clipped verticies to only include 4 points
	who maximize the area of the contact manifold. This is to improve stability
]]

local Hull = require("../Hull")

export type MeshHull = Hull.MeshHull
export type Support = Hull.Support
export type Face = Hull.Face

local function draw(simplex: { Vector3 })
	local length = #simplex
	for i = 1, length do
		local j = (i % length) + 1
		workspace.Wireframe:AddLine(
			simplex[i],
			simplex[j]
		)
	end
end

local function cleanupVerticies(inputList: { Vector3 }): { Vector3 }
	-- inplace vertex cleanup
	local hashmap = {} :: { [Vector3]: boolean }
	local left = 1

	for i, point in inputList do
		local hash = (point * 1e4):Floor()
		if hashmap[hash] then
			-- we have a similar vertex!
			continue
		end

		hashmap[hash] = true
		inputList[left] = point
		left += 1
	end

	for i = left, #inputList do
		table.remove(inputList)
	end

	return inputList
end

local function optimizeManifold(
	inputList: { Vector3 },
	normal: Vector3
): { Vector3 }
	if #inputList <= 4 then
		return cleanupVerticies(inputList)
	end

	-- there's more than 4 contact points, so we will have
	-- to find a combination that maximizes area
	local a = table.remove(inputList) :: Vector3
	local bi = 0
	local ci = 0
	local di = 0

	-- then, find the furthest point away from A.
	local dot = 0.0
	for j, point in inputList do
		local ap = point - a
		local apap = ap:Dot(ap)

		if dot < apap then
			dot = apap
			bi = j
		end
	end

	assert(bi ~= 0)

	-- then, find the point which would create
	-- the largest/smallest triangular area.
	local b = table.remove(inputList, bi) :: Vector3
	local maxArea = -math.huge
	local minArea = math.huge
	for j, p in inputList do
		local pa = a - p
		local pb = b - p

		-- calculate the area of a triangle, and
		-- check if they are the furthest away from the triangle.
		local area = pa:Cross(pb):Dot(normal)
		if maxArea < area then
			-- maximum area created.
			maxArea = area
			ci = j
		end

		if minArea > area then
			-- minimum area created.
			minArea = area
			di = j
		end
	end

	assert(ci ~= 0)
	assert(di ~= 0)

	return {
		a, b,
		inputList[ci],
		inputList[di],
	}
end

return function(
	hullA: MeshHull,
	hullB: MeshHull,
	faceA: Face,
	clipMargin: number
): { Vector3 }
	-- gather the input list
	local initialVerts = #faceA.verticies
	local inputList = table.create(initialVerts) :: { Vector3 }
	for i, vertex in faceA.verticies do
		inputList[i] = hullA.verticies[vertex]
	end

	-- extension of the sutherland-hodgman algorithm into 3D,
	-- clipping edges and planes. assumes the initial verticies
	-- maintain their winding order, which will be maintained
	-- throughout the algorithm
	local vertsB = hullB.verticies
	for i, face in hullB.faces do
		local normal = face.normal
		local w = vertsB[face.verticies[1]]:Dot(normal) + clipMargin

		local outputList = {} :: { Vector3 }
		local length = #inputList
		for j = 1, length do
			local k = if j == 1
				then length
				else j - 1

			-- current vertex
			-- previous vertex
			local vj = inputList[j]
			local vk = inputList[k]

			-- calculate dot products to check
			-- which side j and k lie on the plane
			local vjk = vj - vk
			local vjd = vj:Dot(normal)
			local vkd = vk:Dot(normal)

			if vjd < w then
				if w < vkd then
					-- the previous vertex is infront of the plane, so insert
					-- the intersection from the previous to the current plane(cuz IVT.. yk)

					-- dot(vk + (vj-vk)t, normal) = w
					-- dot(vk, normal) + dot(vj-vk,normal)t = w
					-- t = (w - dot(vk, normal)) / dot(vj-vk, normal)
					-- t = (w - vkd) / (vjd - vkd)
					local t = (w - vkd)/(vjd - vkd)
					table.insert(outputList, vk + vjk*t)
				end

				-- our current vertex is behind the plane, so add it
				table.insert(outputList, vj)
			elseif vkd < w then
				-- the previous vertex was behind the plane, and the current
				-- vertex is infront of the plane, so insert an intersection point(also cuz IVT)

				-- dot(vk + (vj-vk)t, normal) = w
				-- dot(vk, normal) + dot(vj-vk,normal)t = w
				-- t = (w - dot(vk, normal)) / dot(vj-vk, normal)
				-- t = (w - vkd) / (vjd - vkd)
				local t = (w - vkd)/(vjd - vkd)
				table.insert(outputList, vk + vjk*t)
			end
		end

		inputList = outputList
	end

	return optimizeManifold(inputList, faceA.normal)
end


--!strict

--[[
	Used to get the world space offsets
	of feature points in the GJK/EPA subroutines
]]

local EPSILON = 1e-3

export type Feature = {
	pA: Vector3,
	pB: Vector3,
	delta: Vector3,
}

local function saturate(x: number): number
	-- Helper clamp function
	return math.clamp(x, 0.0, 1.0)
end

local function cartesian(simplex: { Vector3 }, barycentric: { number })
	-- Converts barycentric coordinates back to the
	-- cartesian coordinates
	local length = #simplex
	if length == 3 then
		-- Triangle case
		return simplex[1] * barycentric[1]
			+ simplex[2] * barycentric[2]
			+ simplex[3] * barycentric[3]
	elseif length == 2 then
		-- Line case
		return simplex[1] * barycentric[1]
			+ simplex[2] * barycentric[2]
	else
		-- Point case
		return simplex[1]
	end
end

local function barycentric(simplex: { Feature }): { number }
	local length = #simplex
	if length == 3 then
		local a, b, c = simplex[1].delta, simplex[2].delta, simplex[3].delta

		local ab = b - a
		local ac = c - a
		local bc = c - b

		local d00 = ab:Dot(ab)
		local d01 = ab:Dot(ac)
		local d11 = ac:Dot(ac)
		local d20 = -a:Dot(ab)
		local d21 = -a:Dot(ac)
		local d22 = bc:Dot(bc)

		-- Degeneracy tests
		local d12 = ac:Dot(bc)
		if math.abs(d00 * d11 - d01 * d01) < EPSILON
			or math.abs(d11 * d22 - d12 * d12) < EPSILON
		then
			-- One of the edges is degenerate, so
			-- we just choose the largest edge and return
			if d11 > d22 and d11 > d00 then
				-- Edge AC
				table.remove(simplex, 2)
				return barycentric(simplex)
			elseif d22 > d00 and d22 > d11 then
				-- Edge BC
				table.remove(simplex, 1)
				return barycentric(simplex)
			else
				-- Edge AB
				table.remove(simplex, 3)
				return barycentric(simplex)
			end
		end

		-- From Christer Ericson's Real-Time Collision Detection book
		local denom = d00 * d11 - d01 * d01
		local v = (d11 * d20 - d01 * d21) / denom
		local w = (d00 * d21 - d01 * d20) / denom
		local u = 1.0 - v - w

		if u < 0.0 then
			-- Edge BC
			local t = saturate(-b:Dot(bc) / bc:Dot(bc))
			u = 0.0
			v = 1.0 - t
			w = t
		elseif v < 0.0 then
			-- Edge AC
			local t = saturate(c:Dot(ac) / ac:Dot(ac))
			u = t
			v = 0.0
			w = 1.0 - t
		elseif w < 0.0 then
			-- Edge AB
			local t = saturate(-ab:Dot(a) / ab:Dot(ab))
			u = 1.0 - t
			v = t
			w = 0.0
		end

		return { u, v, w }
	elseif length == 2 then
		-- Line
		local a, b = simplex[1].delta, simplex[2].delta
		local ab = b - a

		local abab = ab:Dot(ab)
		if abab < EPSILON * EPSILON then
			-- Line is degenerate
			if a:Dot(a) < b:Dot(b) then
				-- A is closer
				return { 1.0, 0.0 }
			else
				-- B is closer
				return { 0.0, 1.0 }
			end
		else
			local t = saturate(-a:Dot(ab) / ab:Dot(ab))
			return { 1 - t, t }
		end
	else
		return { }
	end
end

return {
	cartesian = cartesian,
	barycentric = barycentric,
}


--!strict

--[[
	The EPA algorithm is an addition to the GJK algorithm where
	we can extract the minimal translation vector from a terminating
	GJK simplex. This is possible because(in my case atleast) GJK
	enclosing a point results in the simplex being a tetrahedron.

	The job of the EPA algorithm is to keep on sampling points that
	will eventually hit the surface of the shape. If we found a triangle
	closest to the origin that is on the surface, then we give back that triangle's
	position, and its normal is our MTV normal!
]]

local Hull = require("../Hull")

local extractCoordinates = require("./extractCoordinates")

local cartesian = extractCoordinates.cartesian
local barycentric = extractCoordinates.barycentric

type Support = (Vector3) -> (Vector3)
type Feature = extractCoordinates.Feature

export type Contact = {
	sep: number,
	axis: Vector3,

	contactA: { Vector3 },
	contactB: { Vector3 },
}

local EPA_MAX_ITRS = 12
local EPSILON = 1e-3

local function encode(a: number, b: number): number
	-- Encodes the verticies
	return bit32.lshift(a, 16) + b
end

local function decode(a: number): (number, number)
	-- Gives back the verticies
	return bit32.rshift(a, 16), bit32.band(a, 0xFFFF)
end

local function dual(a: number): number
	-- Gets the dual of the edge
	return bit32.lshift(bit32.band(a, 0xFFFF), 16) + bit32.rshift(a, 16)
end

return function(
	simplex: { Feature },
	hullA: Hull.Hull,
	hullB: Hull.Hull
): Contact?
	local suppA = hullA.support
	local suppB = hullB.support

	assert(#simplex == 4, "simplex should be a tetrahedron")

	local normals = table.create(4, Vector3.zero)
	local mesh: {{ number }} = {
		-- Create a basic tetrahedron mesh
		-- from our GJK termination condition
		{ 1, 2, 3 },
		{ 1, 4, 2 },
		{ 1, 3, 4 },
		{ 2, 4, 3 },
	}

	local bestDistance = math.huge
	local bestTriangle = 1
	for i, triangle in mesh do
		-- First, we check for the closest triangle
		-- that encloses this simplex
		local a = simplex[triangle[1]].delta
		local b = simplex[triangle[2]].delta
		local c = simplex[triangle[3]].delta

		local normal = (c - a):Cross(b - a).Unit
		if normal:Dot(a) < 0 then
			-- Flip normal if the vertex
			-- is pointing outside of it
			normal *= -1
		end

		local distance = a:Dot(normal)
		if distance < bestDistance then
			bestDistance = distance
			bestTriangle = i
		end

		normals[i] = normal.Unit
	end

	for i = 1, EPA_MAX_ITRS do
		-- If the current triangle is on the surface
		-- of the mesh, then we can safely terminate
		local normal = normals[bestTriangle]
		local featureB = suppB(-normal)
		local featureA = suppA(normal)
		local point = featureA - featureB

		local distance = math.abs(normal:Dot(point) - bestDistance)
		if math.abs(distance) < EPSILON then
			-- We converged!
			break
		end

		-- Otherwise, we will add this new point by creating
		-- a triangle fan around the point. We do this by removing
		-- triangles facing it, and then triangulating around the boundary
		-- edges
		local newNormals: { Vector3 } = {}
		local newMesh: {{ number }} = {}
		local edgelist: { number } = {}

		for j, triangle in mesh do
			local a = triangle[1]
			if normals[j]:Dot(point - simplex[a].delta) < 1e-3 then
				-- It is behind the simplex point, so we
				-- can keep this
				table.insert(newMesh, triangle)
				table.insert(newNormals, normals[j])

				continue
			end

			-- Otherwise, add its entries to the edgelist
			local b = triangle[2]
			local c = triangle[3]

			edgelist[encode(a, b)] = 1
			edgelist[encode(b, c)] = 1
			edgelist[encode(c, a)] = 1
		end

		-- Repair by building a triangle fan
		-- around this discovered point
		table.insert(simplex, {
			pA = featureA,
			pB = featureB,
			delta = point,
		})

		local id = #simplex
		for edge in edgelist do
			if edgelist[dual(edge)] then
				-- This cannot be a boundary edge
				-- because the dual of it exists
				continue
			end

			local a, b = decode(edge)
			local va = simplex[a].delta
			local vb = simplex[b].delta

			local normal = (point - va):Cross(vb - va)
			if normal:Dot(normal) < 1e-3 then
				-- The triangle is degenerate, so do not
				-- use it.
				continue
			elseif normal:Dot(va) < 0 then
				-- The triangle is pointing away the point
				table.insert(newNormals, -normal.Unit)
			else
				-- The triangle is pointing towards the point
				table.insert(newNormals, normal.Unit)
			end

			table.insert(newMesh, { a, b, id })
		end

		-- Update our current state
		normals = newNormals
		mesh = newMesh

		-- Compute the best triangle
		-- for the next iteration
		if #mesh == 0 then
			return nil
		end

		local bestDistance = normals[1]:Dot(simplex[mesh[1][1]].delta)
		bestTriangle = 1

		for j = 2, #normals do
			local normal = normals[j]
			local point = simplex[mesh[j][1]].delta

			local distance = point:Dot(normal)
			if bestDistance > distance then
				bestDistance = distance
				bestTriangle = j
			end
		end
	end

	-- We will assume this triangle is on the surface
	-- in order to terminate the algorithm. This may not be
	-- entirely correct, but we can fix it next tick.
	local pointA = simplex[mesh[bestTriangle][1]]
	local pointB = simplex[mesh[bestTriangle][2]]
	local pointC = simplex[mesh[bestTriangle][3]]

	local normal = normals[bestTriangle]
	local coordinates = barycentric({ pointA, pointB, pointC })
	local simplexA = { pointA.pA, pointB.pA, pointC.pA }
	local simplexB = { pointA.pB, pointB.pB, pointC.pB }

	local pA = cartesian(simplexA, coordinates)
	local pB = cartesian(simplexB, coordinates)
	local sep = (pA - pB):Dot(normal)

	return {
		sep = sep,
		axis = -normal,

		contactA = { pA },
		contactB = { pB },
	}
end


--!native
--!strict

--[[
	Gets a one-shot contact manifold between
	different shapes by using the separating axis theorem.
]]

local Hull = require("../Hull")

local clipHullFaces = require("./clipHullFaces")

local COLLISION_SLOP = 1e-2
local EPSILON = 1e-3

export type MeshHull = Hull.MeshHull
export type Support = Hull.Support
export type Face = Hull.Face

export type Contact = {
	sep: number,
	axis: Vector3,

	contactA: { Vector3 },
	contactB: { Vector3 },
}

local function twin(edge: number): number
	-- Gets the dual
	local a = bit32.rshift(edge, 16)
	local b = bit32.band(edge, 0xFFFF)

	return bit32.lshift(b, 16) + a
end

local function queryEdge(
	verticies: { Vector3 },
	edge: number
): (Vector3, Vector3)
	-- Query edges of the verticies
	return verticies[bit32.band(edge, 0xFFFF)],
		verticies[bit32.rshift(edge, 16)]
end

local function areArcsIntersecting(
	bxa: Vector3,
	dxc: Vector3,

	a: Vector3,
	b: Vector3,
	c: Vector3,
	d: Vector3
): boolean
	-- Checks if the arc spanning a->b and c->d are
	-- colliding. If it is, then we can test this pair
	local cba = c:Dot(bxa)
	local dba = d:Dot(bxa)
	local adc = a:Dot(dxc)
	local bdc = b:Dot(dxc)

	local sameHemisphere = 0.0 < cba * bdc
	local arcIntersection = cba * dba < 0.0
		and adc * bdc < 0.0

	return sameHemisphere and arcIntersection
end

local function queryEdgeDirections(hullA: MeshHull, hullB: MeshHull): (Vector3, number, number, number)
	-- Check all possible edge combinations of A and B as potential separating
	-- axes. Only happens in 3D
	local facesA = hullA.faces
	local facesB = hullB.faces

	local vertsA = hullA.verticies
	local vertsB = hullB.verticies

	local normsA = hullA.normals
	local normsB = hullB.normals

	local bestDirection = Vector3.zero
	local bestDistance = -math.huge
	local bestEdgeA = 0
	local bestEdgeB = 0

	local origin = hullA.center
	for i, edgeA in hullA.edges do
		-- get the edge from A
		local normalB = facesA[normsA[twin(edgeA)]].normal
		local normalA = facesA[normsA[edgeA]].normal

		local pointA, pointB = queryEdge(vertsA, edgeA)
		local dirAB = pointB - pointA

		for j, edgeB in hullB.edges do
			-- Get the edge from B
			local normalD = facesB[normsB[twin(edgeB)]].normal
			local normalC = facesB[normsB[edgeB]].normal

			local pointC, pointD = queryEdge(vertsB, edgeB)
			local dirCD = pointD - pointC

			-- The idea is that we only care for edges
			-- that lie on the minkowski sum of both the hulls.
			-- We can check this through seeing if the gauss map of
			-- these edges collide.
			if not areArcsIntersecting(
				-dirAB,
				-dirCD,
				normalA,
				normalB,
				-normalC,
				-normalD
			) then
				-- A and B are not on the minkowski sum
				continue
			end

			-- Compute the separating axis
			local axis = dirAB:Cross(dirCD)
			if axis:Dot(axis) < 0.005^2 then
				-- Don't calculate the axis
				continue
			elseif axis:Dot(pointA - origin) < 0.0 then
				-- Assume that we maintain
				-- consistent normal orientation
				axis = -axis
			end

			-- Check the support point of B onto A
			local direction = axis.Unit
			local distance = (pointC - pointA):Dot(direction)
			if EPSILON < distance then
				-- Do an early exit
				return direction, distance,
					edgeA, edgeB
			elseif bestDistance < distance then
				-- Mark our direction and edges
				bestDirection = direction
				bestDistance = distance
				bestEdgeA = edgeA
				bestEdgeB = edgeB
			end
		end
	end

	if bestEdgeA == 0 then
		return Vector3.zero, -math.huge, 1, 1
	end

	return bestDirection, bestDistance,
		bestEdgeA, bestEdgeB
end

local function queryFaceDirections(hullA: MeshHull, hullB: MeshHull): (Face, number)
	local bestDistance = -math.huge
	local bestFace = 0

	local delta = hullB.center - hullA.center

	-- It is known for a convex shape, support(face.normal) lies on
	-- the current face
	local vertsA = hullA.verticies
	for i, face in hullA.faces do
		-- query the support point
		local normal = face.normal
		if normal:Dot(delta) < 0.0 then
			continue
		end

		local vertexA = vertsA[face.verticies[1]]
		local vertexB = hullB.support(-normal)

		local distance = (vertexB - vertexA):Dot(normal)
		if EPSILON < distance then
			-- Terminate early because we
			-- already hit something!
			return face, distance
		elseif bestDistance < distance then
			bestDistance = distance
			bestFace = i
		end
	end

	-- return a record of the current face
	assert(bestFace ~= 0, "hull had no faces!")
	return hullA.faces[bestFace], bestDistance
end

return function(
	hullA: MeshHull,
	hullB: MeshHull
): Contact?
	-- the possible separating axes between two convex
	-- polyhedra are between: face normals of A and B
	-- 		edge cross product combinations of A and B
	debug.profilebegin("narrowphase::sat")

	local faceA, distanceA = queryFaceDirections(hullA, hullB)
	if EPSILON < distanceA then
		-- we found a separating axis
		debug.profileend()
		return nil
	end

	local faceB, distanceB = queryFaceDirections(hullB, hullA)
	if EPSILON < distanceB then
		-- we found a separating axis
		debug.profileend()
		return nil
	end

	local edgeAB, distanceAB, edgeA, edgeB = queryEdgeDirections(hullA, hullB)
	if EPSILON < distanceAB then
		-- we found a separating axis
		debug.profileend()
		return nil
	end

	if distanceA >= distanceAB and distanceA >= distanceB then
		-- Face contact with B
		local contact = clipHullFaces(hullA, hullB, faceA, COLLISION_SLOP)
		debug.profileend()
		return {
			sep = -distanceA,
			axis = -faceA.normal,

			contactA = contact,
			contactB = contact,
		}
	elseif distanceB >= distanceAB then
		-- Face contact with B
		local contact = clipHullFaces(hullB, hullA, faceB, COLLISION_SLOP)
		debug.profileend()
		return {
			sep = -distanceB,
			axis = faceB.normal,

			contactA = contact,
			contactB = contact,
		}
	end

	-- use an edge contact, there only exists 1 contact point
	-- and it is the closest point between the two edges
	local a, b = hullA:queryEdge(edgeA)
	local c, d = hullB:queryEdge(edgeB)

	-- a + ab*t_1 = c + cd*t_2
	-- a - c = cd*t_2 - ab*t_1
	local ab = b - a
	local cd = d - c
	local ca = a - c

	-- ca x cd = (cd*t_2 - ab*t_1) x cd
	-- ca x cd = -(ab x cd)t_1
	-- (ca x cd) . (ab x cd) = -((ab x cd) . (ab x cd))t_1
	-- -((ca x cd) . (ab x cd)) / |ab x cd|^2 = t_1
	local cacd = ca:Cross(cd)
	local abcd = ab:Cross(cd)
	local t1 = math.clamp(-cacd:Dot(abcd) / abcd:Dot(abcd), 0.0, 1.0)

	-- ca x ab = (cd*t_2 - ab*t_1) x ab
	-- ca x ab = (cd x ab)t_2
	-- (ca x ab) . (cd x ab) = ((cd x ab) . (cd x ab))t_2
	-- ((ca x ab) . (cd x ab)) / |cd x ab|^2 = t_2
	local caab = ca:Cross(ab)
	local cdab = cd:Cross(ab)
	local t2 = math.clamp(caab:Dot(cdab) / cdab:Dot(cdab), 0.0, 1.0)

	local rootA = a + ab * t1
	local rootB = c + cd * t2

	debug.profileend()
	return {
		sep = -distanceAB,
		axis = -edgeAB,
		contactA = { rootA },
		contactB = { rootB },
	}
end