local class = {}
class.__index = class

----

local STIFFNESS = 400
local DAMPING = 40
local PERCISION = 0.001

local MOVE_TO_PADDING = 2

local V3_FLAT = Vector3.new(1, 0, 1)
local V3_ZERO = Vector3.new(0, 0, 0)
local V3_UP = Vector3.new(0, 1, 0)

----

local function solveSpring(deltaTime, position, velocity, destination)
    local displacement = position - destination
    local springForce = -STIFFNESS * displacement
    local dampForce = -DAMPING * velocity

    local acceleration = springForce + dampForce
    local newVelocity = velocity + acceleration * deltaTime
    local newPosition = position + velocity * deltaTime

    if newVelocity.magnitude < PERCISION and (destination - newPosition).magnitude < PERCISION then
        return destination, velocity * 0
    else
        return newPosition, newVelocity
    end
end

local function getMass(instance)
    local mass = 0

    for _, object in next, instance:GetDescendants() do
        if object:IsA("BasePart") then
            mass = mass + object:GetMass()
        end
    end

    return mass
end

----

function class.new(instance, controller)
    local self = {}
    self.Controller = controller
    self.Instance = instance

    self.RootPart = instance.PrimaryPart
    self.Humanoid = instance:WaitForChild("Humanoid")

    self.MoveForce = self.RootPart:WaitForChild("VectorForce")
    self.TurnForce = self.RootPart:WaitForChild("BodyGyro")
    self.HeightForce = self.RootPart:WaitForChild("BodyPosition")

    self.CurrentAcceleration = Vector3.new()
    self.MassCache = getMass(self.Instance)
    self.Brain = {}

    return setmetatable(self, class)
end

----

function class:Destroy()
    self.Step = function() end
    self.Brain = {}

    self.RootPart = nil
    self.Instance = nil
    self.Humanoid = nil

    self.MoveForce = nil
    self.TurnForce = nil
    self.HeightForce = nil
end

function class:ResetMass()
    self.MassCache = getMass(self.Instance)
end

function class:SyncBrain(newStates)
    for index, value in next, newStates do
        self.Brain[index] = value
    end
end

function class:Step(deltaTime)
    -- floor getting, height setting
        -- raycasting is just a lazy library for common raycasting cases
        -- :CharacterGroundCast() just finds the ground info below the character
        -- it's almost always the point directly below the RootPart
    local hipHeight = self.Humanoid.HipHeight + 1
    local ground, groundPos, groundNorm = raycasting:CharacterGroundCast(self.RootPart.CFrame, hipHeight)
    local hipOffset = V3_UP * (hipHeight)

    -- handle height popping and falling
    if ground then
        self.RootPart.BodyPosition.MaxForce = V3_UP * 10000000
    else
        self.RootPart.BodyPosition.MaxForce = V3_ZERO
    end

    self.RootPart.BodyPosition.Position = groundPos + hipOffset

    -- move direction setting
    local currentPosition = self.RootPart.CFrame.p
    local currentVelocity = self.RootPart.Velocity

    local moveTargetPosition = nil;
    local moveTargetSpeed = 0

    local moveDirection = self.RootPart.CFrame.LookVector
    local moveSpeed = self.Brain.MoveSpeed or 0

    -- figure where to go
    if self.Brain.TargetPosition then
        moveTargetPosition = self.Brain.TargetPosition
    end

    -- if we've got somewhere to go
    if moveTargetPosition then
        local vectorTo = (moveTargetPosition - currentPosition) * V3_FLAT
        local distance = vectorTo.Magnitude

        -- we're close enough to stop now
        if distance <= MOVE_TO_PADDING then
            moveSpeed = moveTargetSpeed

        -- set walk direction to target
        else
            moveDirection = vectorTo.unit
        end

    -- no where to go
    else
        moveSpeed = 0
    end

    -- thanks luanoid
    currentVelocity, self.CurrentAcceleration = solveSpring(
        deltaTime,
        currentVelocity,
        self.CurrentAcceleration,
        moveDirection * moveSpeed
    )

    self.MoveForce.Force = self.MassCache * self.CurrentAcceleration * V3_FLAT

    -- move rotation
    if moveSpeed > 0 then
        self.TurnForce.CFrame = CFrame.new(moveDirection * 0, moveDirection)
    end

    -- humanoid force state
    self.Humanoid:ChangeState(Enum.HumanoidStateType.Physics)
end

----

return class