--[[
    Universal Vehicle Enter Fix
    
    This hook fixes the vehicle entry problem when the character
    is at certain angles or the calculated distance is slightly greater than 2,
    OR when the pathfinder fails to reach the door (common on large vehicles
    like the W900 where the seat "outside" position may be unreachable).
    
    Original problem 1: ISEnterVehicle:start() checks if distance > 2 and aborts.
    Original problem 2: ISPathFindAction fails to path to the seat → forceStop()
                        cancels the entire queue including ISEnterVehicle.
    Solution: 
      1. Override ISPathFindAction:pathToVehicleSeat so a pathfinding failure
         does NOT abort the queue (runActionsAfterFailing = true). The action
         completes and ISEnterVehicle:start() still runs.
      2. In ISEnterVehicle:start(), increase the distance tolerance and snap the
         player to the outside seat position when they are still too far.
]]--

require "Vehicles/TimedActions/ISEnterVehicle";
require "Vehicles/TimedActions/ISPathFindAction";

-- Configuration for Universal Vehicle Enter Fix
VehicleEnterFix = VehicleEnterFix or {};
VehicleEnterFix.maxDistance = 4; -- Increased maximum distance (vanilla = 2)
VehicleEnterFix.failSafeEnabled = true; -- Enable fail-safe teleport
VehicleEnterFix.enterAnimationTimeoutMs = 1800;
VehicleEnterFix.debug = true; -- Debug logs

-- ============================================
-- PATHFINDING FIX
-- Override pathToVehicleSeat so that if the pathfinder cannot reach the seat
-- (BehaviorResult.Failed), it does NOT abort the action queue.
-- Instead, the action "completes" and ISEnterVehicle:start() still runs;
-- the teleport fail-safe below handles placing the player close enough.
-- ============================================
local originalPathToVehicleSeat = ISPathFindAction.pathToVehicleSeat;

function ISPathFindAction:pathToVehicleSeat(character, vehicle, seat)
    local o = originalPathToVehicleSeat(self, character, vehicle, seat);
    -- Setting runActionsAfterFailing + a no-op onFailFunc means:
    -- on BehaviorResult.Failed → forceComplete() instead of forceStop()
    o.runActionsAfterFailing = true;
    o.onFailFunc = function() end; -- no-op; required by the runActionsAfterFailing branch
    o.onFailArgs = {};
    if VehicleEnterFix.debug then
        ----print(("[VehicleEnterFix] pathToVehicleSeat overridden – will not abort queue on pathfind failure");
    end
    return o;
end

-- Override the start function with fail-safe
function ISEnterVehicle:start()
    ----print(("enter start")
    if not isServer() then
        local playerNum = self.character:getPlayerNum();
        getCell():setDrag(nil, playerNum);
        local contextMenu = getPlayerContextMenu(playerNum);
        if contextMenu and contextMenu:isAnyVisible() then
            contextMenu:hideAndChildren();
        end
    end

    -- NOTE: self.started is intentionally set AFTER the distance/fail-safe
    -- block.  If we set it at the top and then return early (before calling
    -- vehicle:enter), isValid() would immediately see started=true but the
    -- character not yet in the seat → returns false → action aborts → flash.

    local outside = self.vehicle:getPassengerPosition(self.seat, "outside");
    local worldPos = Vector3f.new();
    self.vehicle:getWorldPos(outside:getOffset(), worldPos);

    local distance = self.character:DistTo(worldPos:x(), worldPos:y());
    
    -- Use increased maximum distance for all vehicles
    local maxDistance = VehicleEnterFix.maxDistance or 4;
    
    -- Debug log
    if VehicleEnterFix.debug then
        --print(("ISEnterVehicle:start() - Distance: " .. tostring(distance) ..", MaxDistance: " .. tostring(maxDistance) ..", Vehicle: " .. tostring(self.vehicle:getScript():getFullName()));
    end

    -- Check distance with increased tolerance
    if distance > maxDistance then
        if VehicleEnterFix.failSafeEnabled then
            if VehicleEnterFix.debug then
                ----print(("  -> Fail-safe activated! Teleporting to seat outside position...");
            end
            
            -- Snap player to the seat's outside position.
            -- We do NOT re-measure distance afterwards: setX/setY may not be
            -- reflected by DistTo until the next game tick (cached value), so
            -- we simply trust the teleport and proceed.
            self.character:setX(worldPos:x());
            self.character:setY(worldPos:y());
        else
            -- Fail-safe disabled – set started=true so isValid() detects the
            -- character is not in the seat and cleanly cancels the action.
            self.started = true;
            return;
        end
    end

    -- Mark started only now – vehicle:enter() is about to be called so
    -- isValid()'s started-branch (getCharacter == character) will be valid.
    self.started = true;
    self.vehicleEnterFixStartMs = getTimestampMs and getTimestampMs() or nil;

    -- Continue with normal entry
    self.action:setBlockMovementEtc(true); -- ignore 'E' while entering
    self.vehicle:enter(self.seat, self.character);
    self.vehicle:playPassengerSound(self.seat, "enter");
    self.character:SetVariable("bEnteringVehicle", "true");
    self.character:triggerMusicIntensityEvent("VehicleEnter");
    
    local heavyItem = (self.character:getPrimaryHandItem() and self.character:getPrimaryHandItem():hasTag(ItemTag.HEAVY_ITEM)) 
        or (self.character:getSecondaryHandItem() and self.character:getSecondaryHandItem():hasTag(ItemTag.HEAVY_ITEM));
    
    if heavyItem then 
        forceDropHeavyItems(self.character);
    end
end

function ISEnterVehicle:update()
    self.vehicle:playPassengerAnim(self.seat, "enter");
    if self.character:GetVariable("EnterAnimationFinished") == "true" then
        self.character:ClearVariable("EnterAnimationFinished");
        self.character:ClearVariable("bEnteringVehicle");
        self.vehicleEnterFixStartMs = nil;
        self:forceComplete();
        return;
    end

    local startedAt = self.vehicleEnterFixStartMs;
    local timeoutMs = VehicleEnterFix.enterAnimationTimeoutMs or 1800;
    local now = getTimestampMs and getTimestampMs() or nil;
    if not startedAt or not now or now - startedAt < timeoutMs then
        return;
    end

    if self.vehicle:getCharacter(self.seat) ~= self.character then
        return;
    end

    if VehicleEnterFix.debug then
        --print(("ISEnterVehicle:update() - Enter animation timed out, forcing completion for " .. tostring(self.vehicle:getScript():getFullName())));
    end

    self.character:ClearVariable("EnterAnimationFinished");
    self.character:ClearVariable("bEnteringVehicle");
    self.vehicleEnterFixStartMs = nil;
    self:forceComplete();
end

-- ============================================
-- AUTOMATIC RETRY SYSTEM
-- ============================================

-- Override isValid to be more tolerant
function ISEnterVehicle:isValid()
    -- If already started, use original logic
    if self.started then
        return self.vehicle:getCharacter(self.seat) == self.character;
    end
    
    -- Basic checks
    if self.character:getVehicle() ~= nil then
        return false;
    end
    
    if self.vehicle:isSeatOccupied(self.seat) then
        -- Give a second chance for all vehicles
        -- (sometimes MP synchronization incorrectly reports occupied)
        -- Check if there's really someone in the seat
        local occupant = self.vehicle:getCharacter(self.seat);
        if occupant == nil then
            return true; -- Seat is free, just was desynchronized
        end
        return false;
    end
    
    return true;
end

-- ============================================
-- VEHICLE UNSTUCK CONTEXT MENU
-- Right-click a vehicle → "Unstuck" performs a deterministic
-- recovery: physics off, optional nearby XY landing offset, upright yaw reset,
-- safe debugZ restore, then one physics re-enable at the end.
-- ============================================

VehicleEnterFix.activeRecoveries = VehicleEnterFix.activeRecoveries or {}

local function VehicleEnterFix_IsMoveUpAdminOnly()
    return SandboxVars
        and SandboxVars.rSemiTruck
        and SandboxVars.rSemiTruck.AdminOnlyMoveUp == true
end

local function VehicleEnterFix_IsAdmin(playerObj)
    if not playerObj or not playerObj.getAccessLevel then
        return false
    end

    local accessLevel = playerObj:getAccessLevel() or ""
    return string.lower(accessLevel) == "admin"
end

local function VehicleEnterFix_PlayerCanUseOwnVehicle(playerObj, vehicle)
    if not playerObj or not vehicle then
        return false
    end

    if vehicle.isDriver and vehicle:isDriver(playerObj) then
        return true
    end

    local inventory = playerObj.getInventory and playerObj:getInventory() or nil
    local keyId = vehicle.getKeyId and vehicle:getKeyId() or -1
    if inventory and inventory.haveThisKeyId and keyId and keyId >= 0 and inventory:haveThisKeyId(keyId) then
        return true
    end

    if vehicle.isKeysInIgnition and vehicle:isKeysInIgnition() then
        return true
    end

    return false
end

local function VehicleEnterFix_CanUseMoveUp(playerObj, vehicle)
    if VehicleEnterFix_IsAdmin(playerObj) then
        return true
    end

    if VehicleEnterFix_IsMoveUpAdminOnly() then
        return false
    end

    return playerObj ~= nil and vehicle ~= nil
end

local function VehicleEnterFix_GetRecoverySquareZ(vehicle)
    local square = vehicle and vehicle:getSquare() or nil
    if square then
        return square:getZ()
    end

    local z = vehicle and vehicle:getZ() or 0
    return math.floor(z or 0)
end

local function VehicleEnterFix_FindRecoveryPosition(vehicle)
    local baseX = vehicle:getX()
    local baseY = vehicle:getY()
    local cell = getCell()
    local z = VehicleEnterFix_GetRecoverySquareZ(vehicle)
    local offsets = {
        { 0.00, 0.00 },
        { 1.50, 0.00 },
        { -1.50, 0.00 },
        { 0.00, 1.50 },
        { 0.00, -1.50 },
        { 0.75, 0.00 },
        { -0.75, 0.00 },
        { 0.00, 0.75 },
        { 0.00, -0.75 },
        { 2.50, 0.00 },
        { -2.50, 0.00 },
        { 0.00, 2.50 },
        { 0.00, -2.50 },
        { 1.25, 0.00 },
        { -1.25, 0.00 },
        { 0.00, 1.25 },
        { 0.00, -1.25 },
        { 0.75, 0.75 },
        { 0.75, -0.75 },
        { -0.75, 0.75 },
        { -0.75, -0.75 },
        { 1.50, 1.50 },
        { 1.50, -1.50 },
        { -1.50, 1.50 },
        { -1.50, -1.50 },
    }

    if not cell then
        return baseX, baseY
    end

    for _, offset in ipairs(offsets) do
        local targetX = baseX + offset[1]
        local targetY = baseY + offset[2]
        local square = cell:getGridSquare(targetX, targetY, z)
        if square then
            return targetX, targetY
        end
    end

    return baseX, baseY
end

local function VehicleEnterFix_RefreshVehicleParts(vehicle, rebuildPhysics)
    if not vehicle then
        return
    end

    if vehicle.updatePartStats then
        pcall(vehicle.updatePartStats, vehicle)
    end

    if vehicle.updateBulletStats then
        pcall(vehicle.updateBulletStats, vehicle)
    end

    if rebuildPhysics and vehicle.createPhysics then
        pcall(vehicle.createPhysics, vehicle)
    end
end

local function VehicleEnterFix_ForceDetachTrailer(vehicle)
    if not vehicle or not vehicle.breakConstraint then
        return false
    end

    local hasTowLink = (vehicle.getVehicleTowing and vehicle:getVehicleTowing())
        or (vehicle.getVehicleTowedBy and vehicle:getVehicleTowedBy())

    if not hasTowLink then
        return false
    end

    pcall(vehicle.breakConstraint, vehicle, true, false)
    return true
end

local function VehicleEnterFix_ClearActiveRecovery(vehicleId)
    if not vehicleId then
        return
    end

    local recovery = VehicleEnterFix.activeRecoveries[vehicleId]
    if recovery and recovery.onTick then
        Events.OnTick.Remove(recovery.onTick)
    end

    VehicleEnterFix.activeRecoveries[vehicleId] = nil
end

local function VehicleEnterFix_ResolveWorldTransform(vehicle)
    if not vehicle or not vehicle.getWorldTransform then
        return nil
    end

    local transform = nil

    if Transform and Transform.new then
        local okNew, result = pcall(Transform.new)
        if okNew then
            transform = result
        end
    end

    if not transform and vehicle.jniTransform then
        transform = vehicle.jniTransform
    end

    if not transform then
        return nil
    end

    local okGet = pcall(vehicle.getWorldTransform, vehicle, transform)
    if not okGet then
        return nil
    end

    return transform
end

local function VehicleEnterFix_ApplyWorldTransform(vehicle, targetX, targetY, targetZ)
    if not vehicle or not vehicle.setWorldTransform then
        return false
    end

    local transform = VehicleEnterFix_ResolveWorldTransform(vehicle)
    if not transform or not transform.origin or not transform.origin.set then
        return false
    end

    local okSet = pcall(transform.origin.set, transform.origin, targetX, targetY, targetZ)
    if not okSet then
        return false
    end

    local okApply = pcall(vehicle.setWorldTransform, vehicle, transform)
    if not okApply then
        return false
    end

    if vehicle.setPosition then
        pcall(vehicle.setPosition, vehicle, targetX, targetY, targetZ)
    else
        if vehicle.setX then
            pcall(vehicle.setX, vehicle, targetX)
        end
        if vehicle.setY then
            pcall(vehicle.setY, vehicle, targetY)
        end
        if vehicle.setZ then
            pcall(vehicle.setZ, vehicle, targetZ)
        end
    end

    return true
end

local function VehicleEnterFix_RunRecoveryTick(state)
    local vehicle = state and state.vehicle or nil
    if not vehicle or (vehicle.isRemovedFromWorld and vehicle:isRemovedFromWorld()) then
        VehicleEnterFix_ClearActiveRecovery(state and state.vehicleId or nil)
        return false
    end

    state.tick = state.tick + 1

    if state.tick == 1 then
        if vehicle.setForceBrake then
            pcall(vehicle.setForceBrake, vehicle)
        end
        if vehicle.setSpeedKmHour then
            pcall(vehicle.setSpeedKmHour, vehicle, 0)
        end
        VehicleEnterFix_ForceDetachTrailer(vehicle)
    end

    if state.tick <= state.liftTicks then
        vehicle:setPhysicsActive(false)

        if vehicle.setDebugZ then
            vehicle:setDebugZ(state.targetDebugZ)
        end

        if vehicle.setAngles then
            pcall(vehicle.setAngles, vehicle, state.initialAngleX, state.initialAngleY, state.initialAngleZ)
        end

        if state.tick == 1 then
            VehicleEnterFix_RefreshVehicleParts(vehicle, false)
        end

        return true
    end

    if state.tick <= state.liftTicks + state.levelTicks then
        vehicle:setPhysicsActive(false)

        if vehicle.setDebugZ then
            vehicle:setDebugZ(state.targetDebugZ)
        end

        if vehicle.setAngles then
            pcall(vehicle.setAngles, vehicle, state.angleX, state.angleY, state.angleZ)
        end

        if state.tick == state.liftTicks + 1 then
            VehicleEnterFix_RefreshVehicleParts(vehicle, false)
        end

        return true
    end

    if state.tick == state.liftTicks + state.levelTicks + 1 then
        if vehicle.setDebugZ then
            vehicle:setDebugZ(state.baseDebugZ)
        end

        if vehicle.setAngles then
            pcall(vehicle.setAngles, vehicle, state.angleX, state.angleY, state.angleZ)
        end

        vehicle:setPhysicsActive(true)

        return true
    end

    if state.tick <= state.liftTicks + state.levelTicks + state.dropTicks then
        return true
    end

    VehicleEnterFix_ClearActiveRecovery(state.vehicleId)
    return false
end

local function VehicleEnterFix_PerformUnstuckRecovery(vehicle, player)
    if not vehicle then
        return
    end

    VehicleEnterFix_ForceDetachTrailer(vehicle)

    local vehicleId = vehicle.getId and vehicle:getId() or nil
    if not vehicleId then
        return
    end

    local initialAngleX = vehicle.getAngleX and vehicle:getAngleX() or 0
    local initialAngleY = vehicle.getAngleY and vehicle:getAngleY() or 0
    local initialAngleZ = vehicle.getAngleZ and vehicle:getAngleZ() or 0
    local angleX = 0
    local angleY = initialAngleY
    local angleZ = 0
    local baseDebugZ = 1
    local targetDebugZ = 3

    if VehicleEnterFix.debug then
        --print(("[VehicleEnterFix] Recovery reposition: "
            --.. tostring(vehicle:getScript():getFullName())
            --.. " angleX=" .. tostring(angleX)
            --.. " angleY=" .. tostring(angleY)
            --.. " angleZ=" .. tostring(angleZ)
            --.. " debugZ=" .. tostring(targetDebugZ));
    end

    VehicleEnterFix_ClearActiveRecovery(vehicleId)

    local state = {
        vehicle = vehicle,
        vehicleId = vehicleId,
        playerObj = player and getSpecificPlayer(player) or nil,
        baseDebugZ = baseDebugZ,
        targetDebugZ = targetDebugZ,
        initialAngleX = initialAngleX,
        initialAngleY = initialAngleY,
        initialAngleZ = initialAngleZ,
        angleX = angleX,
        angleY = angleY,
        angleZ = angleZ,
        liftTicks = 4,
        levelTicks = 4,
        dropTicks = 3,
        serverFinalizeSent = false,
        tick = 0,
    }

    local function onTick()
        local ok, err = pcall(VehicleEnterFix_RunRecoveryTick, state)
        if not ok then
            VehicleEnterFix_ClearActiveRecovery(vehicleId)
            if VehicleEnterFix.debug then
                --print("[VehicleEnterFix] Recovery staged reset failed: " .. tostring(err))
            end
        end
    end

    state.onTick = onTick
    VehicleEnterFix.activeRecoveries[vehicleId] = state

    onTick()
    if VehicleEnterFix.activeRecoveries[vehicleId] then
        Events.OnTick.Add(onTick)
    end
end

local function VehicleEnterFix_PerformMoveVehicleImpulse(vehicle)
    if not vehicle then
        return
    end

    if VehicleEnterFix.debug then
        --print(("[VehicleEnterFix] Pushing vehicle up via impulse: "
            --.. tostring(vehicle:getScript():getFullName())))
    end

    VehicleEnterFix_ForceDetachTrailer(vehicle)

    if vehicle.setPhysicsActive then
        pcall(vehicle.setPhysicsActive, vehicle, true)
    end

    if not (vehicle.addImpulse and Vector3f and Vector3f.new) then
        return
    end

    local mass = vehicle.getMass and vehicle:getMass() or 0
    local lift = 800
    local forceVec = Vector3f.new(0, mass * lift, 0)
    local pushPoint = Vector3f.new(0, 0, 0)

    pcall(vehicle.addImpulse, vehicle, forceVec, pushPoint)
end

local function VehicleEnterFix_UnstuckVehicle(player, vehicle)
    return
end

local function VehicleEnterFix_MoveVehicleUp(player, vehicle)
    if not vehicle then
        return
    end

    local playerObj = getSpecificPlayer(player)
    if not playerObj then
        return
    end

    if isClient() then
        sendClientCommand(playerObj, "W900", "moveVehicleImpulse", { vehicle = vehicle:getId() })
        return
    end

    VehicleEnterFix_PerformMoveVehicleImpulse(vehicle)
end

local function VehicleEnterFix_UnstuckVehicleRadial(playerObj, vehicle)
    return
end

local function VehicleEnterFix_ShouldShowTiltRecovery(vehicle)
    if not vehicle then
        return false
    end

    local angleX = vehicle.getAngleX and vehicle:getAngleX() or 0
    local angleZ = vehicle.getAngleZ and vehicle:getAngleZ() or 0

    return (angleX > -155 and angleX < 155) or (angleZ > 45 and angleZ < -45)
end

local function VehicleEnterFix_OnServerCommand(module, command, args)
    if module ~= "W900" or not args then
        return
    end

    if not args.vehicle then
        return
    end

    local vehicle = getVehicleById(args.vehicle)
    if not vehicle then
        return
    end

    if command == "performMoveVehicleImpulse" then
        VehicleEnterFix_PerformMoveVehicleImpulse(vehicle)
    end
end

local function VehicleEnterFix_OnFillWorldObjectContextMenu(player, context, worldobjects, test)
    -- Ignore while the player is sitting inside a vehicle
    local playerObj = getSpecificPlayer(player);
    if playerObj:getVehicle() ~= nil then return end;

    -- Pick the vehicle under the mouse cursor
    local vehicle = IsoObjectPicker.Instance:PickVehicle(getMouseXScaled(), getMouseYScaled());
    if not vehicle then return end;

    -- Only show option when player is within 5 tiles of the vehicle
    local vx, vy = vehicle:getX(), vehicle:getY();
    local dx = playerObj:getX() - vx;
    local dy = playerObj:getY() - vy;
    if (dx * dx + dy * dy) > 25 then return end;

    if not VehicleEnterFix_CanUseMoveUp(playerObj, vehicle) then return end;

    -- When test == true the engine only checks whether this handler *would*
    -- add something; we must return true without actually adding.
    if test then return true end;

    local moveUpOption = context:addOption("Move Up", player, VehicleEnterFix_MoveVehicleUp, vehicle);
    moveUpOption.iconTexture = getTexture("media/ui/vehicles/v_up.png");

end

local function VehicleEnterFix_HasTowingSlice(menu)
    if not menu or not menu.slices then
        return false
    end

    for _, slice in ipairs(menu.slices) do
        local command = slice and slice.command
        local callback = command and command[1]
        if callback == ISVehicleMenu.onAttachTrailer or callback == ISVehicleMenu.onDetachTrailer then
            return true
        end
    end

    return false
end

local function VehicleEnterFix_EnsureTowingSlice(playerObj, vehicle)
    if not playerObj or not vehicle then
        return
    end

    local menu = getPlayerRadialMenu(playerObj:getPlayerNum())
    if not menu or VehicleEnterFix_HasTowingSlice(menu) then
        return
    end

    if ISVehicleMenu and ISVehicleMenu.doTowingMenu then
        ISVehicleMenu.doTowingMenu(playerObj, vehicle, menu)
    end
end

if not VehicleEnterFix.old_showRadialMenuOutside then
    VehicleEnterFix.old_showRadialMenuOutside = ISVehicleMenu.showRadialMenuOutside

    function ISVehicleMenu.showRadialMenuOutside(playerObj)
        VehicleEnterFix.old_showRadialMenuOutside(playerObj)

        if not playerObj or playerObj:getVehicle() ~= nil then
            return
        end

        local vehicle = ISVehicleMenu.getVehicleToInteractWith(playerObj)
        if not vehicle then
            return
        end

        -- Compatibility fallback: some menu wrappers can drop the vanilla
        -- towing slices; re-run towing injection only when they are missing.
        VehicleEnterFix_EnsureTowingSlice(playerObj, vehicle)

        if not VehicleEnterFix_CanUseMoveUp(playerObj, vehicle) then
            return
        end
    end
end

Events.OnFillWorldObjectContextMenu.Add(VehicleEnterFix_OnFillWorldObjectContextMenu);
Events.OnServerCommand.Add(VehicleEnterFix_OnServerCommand);