#include "../config/events.lua"
#include "../config/constants.lua"
#include "helpers/mathfr.lua"
#include "helpers/string.lua"

local HORIZONTAL_MP = 11
local HORIZONTAL_MAX = 15
local HORIZONTAL_SPEED = 0.7

local VERTICAL_MP = 2.8
local VERTICAL_MAX = 10
local VERTICAL_SPEED = 0.4

local last_vel = nil
local offsetAngle = 0.0
local offsetVerticalAngle = 0.0

force_x = 0.0
force_y = 0.0
force_z = 0.0

local camera_loc_checked, camera_local_pos
local layer = LAYERS_TITLES["camera-collision"]

function init()
    lastOffset = Vec()
    RegisterListenerTo(EVENTS.GAMEMODE_INIT, "OnGamemodeInit")
end

function OnGamemodeInit()
    UnregisterListener(EVENTS.GAMEMODE_INIT, "OnGamemodeInit")

    local gamemode = GetString("level.gamemode")
    if gamemode == "derby" then
        HORIZONTAL_MP = 1.5
        HORIZONTAL_MAX = 2
        HORIZONTAL_SPEED = 0.5

        VERTICAL_MP = 1.9
        VERTICAL_MAX = 7
        VERTICAL_SPEED = 0.3
    end
end

function tick(dt)
    local veh = GetPlayerVehicle()
    if not (veh and veh > 0) then return end

    local body = GetVehicleBody(veh)
    local tr = GetBodyTransform(body)
    local bodyPos, bodyRot = tr.pos, tr.rot
    local focusAt

    if not camera_loc_checked then
        local camera_loc = HasTag(veh, "camera_handle") and GetTagValue(veh, "camera_handle")
        if camera_loc then
            camera_local_pos = GetTagValue(camera_loc, "local_pos"):as_vec()
        end
        camera_loc_checked = true
    end
    if camera_local_pos then
        focusAt = TransformToParentPoint(tr, camera_local_pos)
    else
        focusAt = bodyPos
    end

    local velocity = GetBodyVelocity(body)
    local angularVelocity = GetBodyAngularVelocity(body)

    local bodyForwardVec = QuatRotateVec(bodyRot, Vec(0,0,-1))
    local bodyUpVec = QuatRotateVec(bodyRot, Vec(0,1,0))

    local dot = VecDot(VecNormalize(angularVelocity), bodyUpVec)

    SetValue("offsetAngle", math:Clamp(VecLength(angularVelocity) * HORIZONTAL_MP, -HORIZONTAL_MAX, HORIZONTAL_MAX) * sign(dot), "cosine", HORIZONTAL_SPEED)

    local quat_hor = QuatAxisAngle(Vec(0, 1, 0), offsetAngle)

    local vel_dot = VecDot(VecNormalize(velocity), bodyForwardVec)
    local acc
    if last_vel then
        acc = (VecLength(velocity) - VecLength(last_vel)) * sign(vel_dot) * 10
    else
        acc = 0
    end

    local acc_3d = TransformToLocalVec(GetBodyTransform(body), VecSub(velocity, last_vel))
    acc_3d = VecScale(acc_3d, 10)

    local force_interp_speed = 1.0
    SetValue("force_x", math:Clamp(-acc_3d[1], -0.5, 0.5), "cosine", 1.4)
    SetValue("force_y", math:Clamp(-acc_3d[2], -2.0, 2.0), "cosine", 1.8)
    SetValue("force_z", math:Clamp(-acc_3d[3], -0.8, 0.8), "cosine", 1.2)

    last_vel = velocity

    SetValue("offsetVerticalAngle", math:Clamp(acc * VERTICAL_MP, -VERTICAL_MAX, VERTICAL_MAX), "cosine", VERTICAL_SPEED)
    local quat_vert = QuatAxisAngle(Vec(1, 0, 0), offsetVerticalAngle)

    local quat = QuatRotateQuat(quat_hor, quat_vert)

    local cam = GetCameraTransform()
    local vehPos = tr.pos

    local camPos = VecSub(cam.pos, lastOffset)
    local first_person = math:Distance2Vec(camPos, TransformToParentPoint(tr, GetVehicleDriverPos(veh))) <= 2
    SetBool("level.first_person_cam", first_person)
    if first_person then return end -- offset and its calculations not needed in first person view, exiting

    local vecFromVehToCam = VecSub(camPos, vehPos)
    local dirToCamera = VecNormalize(vecFromVehToCam)
    local len = VecLength(vecFromVehToCam)
    local vec = Vec(force_x,force_y,force_z)
    local wallPos

    -- Trace from cam to veh
	QueryLayerFilter(layer)
    local hit, d, _, s = QueryRaycast(vehPos, dirToCamera, len + 3)
    if hit then
        wallPos = VecAdd(vehPos, VecScale(dirToCamera, d - 1))
        wallPos = VecSub(wallPos, camPos) -- Offset in world coordinates
        lastOffset = wallPos
        wallPos = TransformToLocalVec(cam, wallPos) -- Offset in camera local coordinates
        -- SetCameraOffsetTransform(Transform(vec), false)
    else
        lastOffset = Vec()
    end

    vec = VecAdd(wallPos, vec)
    SetCameraOffsetTransform(Transform(vec, quat))
end

function sign(val)
    if val >= 0 then
        return 1
    else
        return -1
    end
end
