collision

local ReplicatedStorage = game:GetService("ReplicatedStorage")

local Matter = require(ReplicatedStorage.Shared.Libs.Matter)
local Components = require(ReplicatedStorage.Shared.Components)

local RotateVector2 = require(ReplicatedStorage.Shared.Utils.RotateVector2)
local GetOverlappingEntities = require(ReplicatedStorage.Shared.Utils.GetOverlappingEntities)
local GetEntityGlobalPosition = require(ReplicatedStorage.Shared.Utils.GetEntityGlobalPosition)
local GetEntityGlobalRotation = require(ReplicatedStorage.Shared.Utils.GetEntityGlobalRotation)
local GetEntityGlobalScale = require(ReplicatedStorage.Shared.Utils.GetEntityGlobalScale)
local GetEntityCanvas = require(ReplicatedStorage.Shared.Utils.GetEntityCanvas)

local CheckCollisions = require(ReplicatedStorage.Shared.Systems.CheckCollisions)
local ApplyVelocity = require(ReplicatedStorage.Shared.Systems.ApplyVelocity)

local function sweptAABB(
    velocity,
    position1,
    size1,

    position2,
    size2
)
    local xInvEntry, yInvEntry
    local xInvExit, yInvExit

    if velocity.X > 0 then
        xInvEntry = position2.X - (position1.X + size1.X / 2)
        xInvExit = (position2.X + size2.X / 2) - position1.X
    else
        xInvEntry = (position2.X + size2.X / 2) - position1.X
        xInvExit = position2.X - (position1.X + size1.X / 2)
    end

    if velocity.Y > 0 then
        yInvEntry = position2.Y - (position1.Y + size1.Y / 2)
        yInvExit = (position2.Y + size2.Y / 2) - position1.Y
    else
        yInvEntry = (position2.Y + size2.Y / 2) - position1.Y
        yInvExit = position2.Y - (position1.Y + size1.Y / 2)
    end

    local xEntry, yEntry
    local xExit, yExit

    if velocity.X == 0 then
        xEntry = math.huge
        xExit = math.huge
    else
        xEntry = xInvEntry / velocity.X
        xExit = xInvExit / velocity.X
    end

    if velocity.X == 0 then
        yEntry = math.huge
        yExit = math.huge
    else
        yEntry = yInvEntry / velocity.Y
        yExit = yInvExit / velocity.Y
    end

    local entryTime = math.max(xEntry, yEntry)
    local exitTime = math.min(xExit, yExit)

    if entryTime > exitTime or (xEntry < 0 and yEntry < 0) or xEntry > 1 or yEntry > 1 then
        return 1, Vector2.new(0, 0)
    end

    local normal

    if xEntry > yEntry then
        if xInvEntry < 0 then
            normal = Vector2.new(1, 0)
        else
            normal = Vector2.new(-1, 0)
        end
    else
        if yInvEntry < 0 then
            normal = Vector2.new(0, 1)
        else
            normal = Vector2.new(0, -1)
        end
    end

    return entryTime, normal
end

local function globalToLocalPosition(id, position, world)
    local parentRotation = 0
    local parentPosition = Vector2.zero
    local parentScale = Vector2.one

    local child = world:get(id, Components.Child)
    if child and world:contains(child.Parent) then
        parentPosition = GetEntityGlobalPosition(child.Parent, world)
        parentRotation = GetEntityGlobalRotation(child.Parent, world)
        parentScale = GetEntityGlobalScale(child.Parent, world)
    end

    return RotateVector2(position - parentPosition, -math.rad(parentRotation)) / parentScale
end

local function RespondToCollisions(world, state)
    for id1, transform1, velocity1, collider1, physics1 in world:query(Components.Transform, Components.Velocity, Components.Collider, Components.Physics) do
        local colliding = world:get(id1, Components.Colliding)
        local collidingWith = if colliding
            then colliding.CollidingWith
            else {}

        local globalPosition1 = GetEntityGlobalPosition(id1, world)
        local globalSize1 = GetEntityGlobalScale(id1, world) * collider1.Size

        local lastPosition = physics1.LastPosition or globalPosition1
        local lastChange = globalPosition1 - lastPosition

        if #collidingWith == 0 then
            continue
        end

        local minCollisionTime = math.huge
        local minCollisionNormal = nil

        for _, id2 in collidingWith do
            local sprite2 = world:get(id2, Components.Sprite)
            if sprite2 then
                world:insert(id2, sprite2:patch({
                    ImageColor = Color3.new(0, 0, 1)
                }))
            end

            local collider2 = world:get(id2, Components.Collider)
            if not collider2 then
                continue
            end

            local transform2 = world:get(id2, Components.Transform)
            if not transform2 then
                continue
            end

            local globalPosition2 = GetEntityGlobalPosition(id2, world)
            local globalSize2 = GetEntityGlobalScale(id2, world) * collider2.Size
            local collisionTime, collisionNormal = sweptAABB(
                lastChange,
                lastPosition,
                globalSize1,
                globalPosition2,
                globalSize2
            )

            if collisionTime < minCollisionTime then
                minCollisionTime = collisionTime
                minCollisionNormal = collisionNormal
            end
        end

        local parentRotation = 0
        local parentPosition = Vector2.zero
        local parentScale = Vector2.one

        local newGlobalPosition = lastPosition + lastChange * minCollisionTime
        local newLocalPosition = globalToLocalPosition(id1, newGlobalPosition, world)

        local dotprod = (lastChange.X * minCollisionNormal.Y + lastChange.Y * minCollisionNormal.X) * (1 - minCollisionTime)

        world:insert(id1,
            transform1:patch({
                Position = newLocalPosition
            }),
            physics1:patch({
                LastPosition = newGlobalPosition
            }),
            velocity1:patch({
                Velocity = Vector2.new(
                    dotprod * minCollisionNormal.Y,
                    dotprod * minCollisionNormal.X
                )
            })
        )
    end
end

return {
    system = RespondToCollisions,
    after = {
        CheckCollisions,
        ApplyVelocity,
    },
}