---------------------------------------------------------------------------
--- Utils functions
---------------------------------------------------------------------------

if not TowTruckMod then TowTruckMod = {} end
if not TowTruckMod.Utils then TowTruckMod.Utils = {} end

TowTruckMod.Utils.tempVector1 = Vector3f.new()
TowTruckMod.Utils.tempVector2 = Vector3f.new()
TowTruckMod.Utils.tempVector3 = Vector3f.new()
TowTruckMod.Utils.tempVector4 = Vector3f.new()

function TowTruckMod.Utils.isTrailer(vehicle)
	if vehicle == nil then return end
	
	return string.match(string.lower(vehicle:getScript():getName()), "trailer")
end

function TowTruckMod.Utils.isTowTruck(vehicle)
	if vehicle == nil then return end
	
	local script = vehicle:getScript()
	local hookAttachment = script:getAttachmentById("hook")
	return hookAttachment
end

-- Calculate distance between two vehicles using their hook/trailer attachment points
local function getHookDistance(towTruck, targetVehicle)
	local hookPos = towTruck:getAttachmentWorldPos("hook", TowTruckMod.Utils.tempVector1)
	if not hookPos then return 9999 end

	-- Try trailer attachment first, then trailerfront
	local targetPos = targetVehicle:getAttachmentWorldPos("trailer", TowTruckMod.Utils.tempVector2)
	if not targetPos then
		targetPos = targetVehicle:getAttachmentWorldPos("trailerfront", TowTruckMod.Utils.tempVector2)
	end
	if not targetPos then return 9999 end

	local dx = hookPos:x() - targetPos:x()
	local dy = hookPos:y() - targetPos:y()
	return math.sqrt(dx * dx + dy * dy)
end

function TowTruckMod.Utils.getAviableVehicles(mainVehicle)
	local vehicles = {}
	local square = mainVehicle:getSquare()
	if square == nil then return vehicles end

	-- Reduced search radius from 10 to 6 tiles for more reasonable range
	local searchRadius = 6
	for y = square:getY() - searchRadius, square:getY() + searchRadius do
		for x = square:getX() - searchRadius, square:getX() + searchRadius do
			local square2 = getCell():getGridSquare(x, y, square:getZ())
			if square2 then
				for i = 1, square2:getMovingObjects():size() do
					local obj = square2:getMovingObjects():get(i - 1)
					if obj ~= nil
							and instanceof(obj, "BaseVehicle")
							and obj ~= mainVehicle
							and #(TowTruckMod.Utils.getHookTypeVariants(mainVehicle, obj)) ~= 0
							then
						table.insert(vehicles, obj)
					end
				end
			end
		end
	end

	-- Sort by distance from tow truck hook to target vehicle
	table.sort(vehicles, function(a, b)
		return getHookDistance(mainVehicle, a) < getHookDistance(mainVehicle, b)
	end)

	return vehicles
end

--- Return a table with hookType options for vehicles.
function TowTruckMod.Utils.getHookTypeVariants(vehicleA, vehicleB)

	local hookTypeVariants = {}

	if vehicleA:getVehicleTowing() or vehicleA:getVehicleTowedBy()
			or vehicleB:getVehicleTowing() or vehicleB:getVehicleTowedBy() then 
		return hookTypeVariants 
	end

	local p1, p2 = TowTruckMod.Utils.getTowingAttachPoints(vehicleA, vehicleB)
	
	if TowTruckMod.Utils.isTrailer(vehicleB) then
		return hookTypeVariants
	end

	if p1 or p2 then
		if TowTruckMod.Utils.isTowTruck(vehicleA) then
			local hookType = {}
				hookType.name = getText("UI_Text_Towing_attach") .. "\n".. ISVehicleMenu.getVehicleDisplayName(vehicleB) .. "\n" .. getText("UI_Text_Towing_byHook")
				hookType.func = TowTruckMod.Hook.attachTowTruckAction
				hookType.towingVehicle = vehicleA
				hookType.towedVehicle = vehicleB
				hookType.towingPoint = p1
				hookType.towedPoint = p2					
				hookType.texture = getTexture("media/textures/tow_car_icon.png")
			table.insert(hookTypeVariants, hookType)			
		end
	end
	return hookTypeVariants
end

function TowTruckMod.Utils.getTowingAttachPoints(vehicleA, vehicleB)	
	if TowTruckMod.Utils.isTowTruck(vehicleA) then
		if vehicleA:canAttachTrailer(vehicleB, "hook", "trailer") then
			return "hook", "trailer"
		end
		if vehicleA:canAttachTrailer(vehicleB, "hook", "trailerfront") then
			return "hook", "trailerfront"
		end
	end
end

function TowTruckMod.Utils.lerpTowbarOffsets(playerObj, towingVehicle, towedVehicle, attachmentA, attachmentB)
    if towingVehicle == nil or towedVehicle == nil then return end

    -- Fetch the hook attachment from the towed vehicle's script
    local hookB = towedVehicle:getScript():getAttachmentById(attachmentB)
    if not hookB then 
        return 
    end
	
    -- Set a small threshold for when to stop interpolating
    local tolerance = 0.01

    -- Loop for alignment updates
    for i = 1, 20 do
        local pointA = towingVehicle:getAttachmentLocalPos("towbar", Vector3f.new())
        local pointB = towedVehicle:getAttachmentLocalPos("towbarfront", Vector3f.new())
        local yOffset = pointA:y() - pointB:y()

        if math.abs(yOffset) <= tolerance then
            break
        end

        local direction = yOffset >= 0 and 1 or -1
        local lerp = function(current, target, t)
            return current + (target - current) * t
        end

        local interpolationSpeed = 0.2
        local hookPosB = hookB:getOffset()
        local targetY = hookPosB:y() + (math.abs(yOffset) * direction)
        local newY = lerp(hookPosB:y(), targetY, interpolationSpeed)
        hookB:getOffset():set(hookPosB:x(), newY, hookPosB:z())
    end
	TowTruckMod.Hook.setTowingScript(towedVehicle)
		
    local args = { vehicleA = towingVehicle:getId(), vehicleB = towedVehicle:getId(), attachmentA = attachmentA, attachmentB = attachmentB }
	sendClientCommand(playerObj, 'towing', 'attachTowTruck', args)
end

function TowTruckMod.Utils.updateAttachmentsForRigidTow(playerObj, towingVehicle, towedVehicle, attachmentA, attachmentB)
	if towingVehicle == nil or towedVehicle == nil then return end

	local offset = towedVehicle:getScript():getAttachmentById(attachmentB):getOffset()
	local zShift = offset:z() > 0 and 0.5 or -0.5
	towedVehicle:getScript():getAttachmentById(attachmentB):getOffset():set(offset:x(), offset:y(), offset:z() + zShift)

	TowTruckMod.Utils.lerpTowbarOffsets(playerObj, towingVehicle, towedVehicle, attachmentA, attachmentB)
end

function TowTruckMod.Utils.updateAttachmentsOnDefaultValues(towingVehicle, towedVehicle, isWasTowTruck)
	local attachment = towingVehicle:getScript():getAttachmentById(towingVehicle:getTowAttachmentSelf())
	local zOffset = (towingVehicle:getTowAttachmentSelf() == "trailer") and -1 or 1
    attachment:setZOffset(zOffset)

	attachment = towedVehicle:getScript():getAttachmentById(towedVehicle:getTowAttachmentSelf())
	zOffset = (towedVehicle:getTowAttachmentSelf() == "trailer") and -1 or 1
	attachment:setZOffset(zOffset)

	local offset = attachment:getOffset()
	local zShift = offset:z() > 0 and -1 or 1
	local yShift = 0
	attachment:getOffset():set(offset:x(), offset:y() + yShift, offset:z() + zShift)
end

-- Is the vehicle upside down?

function TowTruckMod.Utils.isUpsideDownVehicle(vehicle)
   local topPos = vehicle:getWorldPos(0, 1, 0, TowTruckMod.Utils.tempVector1)
   local centerPos = vehicle:getWorldPos(0, 0, 0, TowTruckMod.Utils.tempVector2)

   return topPos:z() < (centerPos:z() + 0.3)
end

function TowTruckMod.Utils.getUpsideDownVehicle(mainVehicle)
	local vehicles = {}
	local square = mainVehicle:getSquare()
	if square == nil then return vehicles end

	-- Reduced search radius from 10 to 6 tiles
	local searchRadius = 6
	for y = square:getY() - searchRadius, square:getY() + searchRadius do
		for x = square:getX() - searchRadius, square:getX() + searchRadius do
			local square2 = getCell():getGridSquare(x, y, square:getZ())
			if square2 then
				for i = 1, square2:getMovingObjects():size() do
					local obj = square2:getMovingObjects():get(i - 1)
					if obj ~= nil
							and instanceof(obj, "BaseVehicle")
							and obj ~= mainVehicle
							and TowTruckMod.Utils.isUpsideDownVehicle(obj)
							and #(TowTruckMod.Utils.getCenterHook(mainVehicle, obj)) ~= 0
							then
						table.insert(vehicles, obj)
					end
				end
			end
		end
	end

	-- Sort by distance from tow truck hook
	table.sort(vehicles, function(a, b)
		return getHookDistance(mainVehicle, a) < getHookDistance(mainVehicle, b)
	end)

	return vehicles
end

--- Return a table with hookType options for flipped vehicles.
function TowTruckMod.Utils.getCenterHook(vehicleA, vehicleB)
	local centerHook = {}

	if vehicleA:getVehicleTowing() or vehicleA:getVehicleTowedBy()
			or vehicleB:getVehicleTowing() or vehicleB:getVehicleTowedBy() then
		return centerHook
	end

	if TowTruckMod.Utils.isTowTruck(vehicleA) and (vehicleA:canAttachTrailer(vehicleB, "hook", "flipNode")) then
		local hookType = {}
			hookType.name = getText("UI_Text_Towing_flip") .. "\n".. ISVehicleMenu.getVehicleDisplayName(vehicleB) .. "\n" .. getText("UI_Text_Towing_byHook")
			hookType.func = TowTruckMod.Hook.flipVehicleAction
			hookType.towingVehicle = vehicleA
			hookType.towedVehicle = vehicleB
			hookType.texture = getTexture("media/textures/flipVehicle.png")
		table.insert(centerHook, hookType)
	end
	return centerHook
end

-----------------------------------------------------------

function TowTruckMod.Utils.getTowableVehicleNear(square, ignoreVehicle, attachmentA, attachmentB)
	for y=square:getY() - 6,square:getY()+6 do
		for x=square:getX()-6,square:getX()+6 do
			local square2 = getCell():getGridSquare(x, y, square:getZ())
			if square2 then
				for i=1,square2:getMovingObjects():size() do
					local obj = square2:getMovingObjects():get(i-1)
					if instanceof(obj, "BaseVehicle") and obj ~= ignoreVehicle and ignoreVehicle:canAttachTrailer(obj, attachmentA, attachmentB) then
						return obj
					end
				end
			end
		end
	end
	return nil
end

-----------------------------------------------------------

--- Fix mods that add vehicles without tow attachments
local function fixTowAttachmentsForOtherVehicleMods()
	local scriptManager = getScriptManager()
	local vehicleScripts = scriptManager:getAllVehicleScripts()

	for i = 0, vehicleScripts:size()-1 do
		local script = vehicleScripts:get(i)
		local wheelCount = script:getWheelCount()

		local attachHeightOffset = -0.5
		if wheelCount > 0 then
			attachHeightOffset = script:getWheel(0):getOffset():y() + 0.1
		end

		local trailerAttachment = script:getAttachmentById("trailer")
		if trailerAttachment == nil then
			local attach = ModelAttachment.new("trailer")
			attach:getOffset():set(0, attachHeightOffset, -script:getPhysicsChassisShape():z()/2 - 0.1)
			attach:setZOffset(-1)

			script:addAttachment(attach)
		end

		local trailerAttachment = script:getAttachmentById("trailerfront")
		if trailerAttachment == nil then
			local attach = ModelAttachment.new("trailerfront")
			attach:getOffset():set(0, attachHeightOffset, script:getPhysicsChassisShape():z()/2 + 0.1)
			attach:setZOffset(1)

			script:addAttachment(attach)
		end

		local trailerAttachment = script:getAttachmentById("flipNode")
		if trailerAttachment == nil then
			local attach = ModelAttachment.new("flipNode")
			attach:getOffset():set(0, attachHeightOffset, 0)
			attach:setZOffset(0)

			script:addAttachment(attach)
		end
	end
end

Events.OnGameBoot.Add(fixTowAttachmentsForOtherVehicleMods)

