-- Functions
local function CheckDistance(pos1, pos2)
	return (pos1 - pos2).Magnitude
end

local function Raycast(origin, direction)
	return workspace:Raycast(origin, direction, RayParams)
end

local function RaycastToPoint(origin, destination)
	return Raycast(origin, destination - origin)
end

local function UpdateThrottleWithComp(throttle: number)
	Chassis.UpdateThrottle(Car.Chassis.FloorPanel.AssemblyLinearVelocity.Magnitude, throttle)
end

local function UpdateSteeringWithComp(steer: number)
	Chassis.UpdateSteering(steer, Car.Chassis.FloorPanel.AssemblyLinearVelocity.Magnitude)
	return steer
end

local function UpdateThrottleToSpeed(spsSpeed)
	Chassis.SetMotorVelocity(spsSpeed / (math.pi * 3) * math.rad(360))
end

local function FaceSteerToPosition(position: Vector3)
	local PositionDifference = (position - Car.Chassis.FloorPanel.Position).Unit

	if PositionDifference:Dot(Car.Chassis.FloorPanel.CFrame.LookVector) < -0.5 then --  Vehicle is behind the car!
		local RightResult = Raycast(Car.Chassis.FloorPanel.Position, Car.Chassis.FloorPanel.CFrame.RightVector * RaycastDistance)
		local LeftResult = Raycast(Car.Chassis.FloorPanel.Position, Car.Chassis.FloorPanel.CFrame.RightVector * -RaycastDistance)
		local FrontResult = Raycast(Car.Chassis.FloorPanel.Position, Car.Chassis.FloorPanel.CFrame.LookVector * RaycastDistance)

		local RightDistance = RaycastDistance
		local LeftDistance = RaycastDistance
		local FrontDistance = RaycastDistance

		if RightResult ~= nil then
			RightDistance = CheckDistance(Car.Chassis.FloorPanel.Position, RightResult.Position)
		end

		if LeftResult ~= nil then
			LeftDistance = CheckDistance(Car.Chassis.FloorPanel.Position, LeftResult.Position)
		end

		if FrontResult ~= nil then
			FrontDistance = CheckDistance(Car.Chassis.FloorPanel.Position, FrontResult.Position)
		end

		if (LeftDistance > 20 or RightDistance > 20) and FrontDistance > 25 then
			if LeftDistance > RightDistance then
				UpdateSteeringWithComp(1)
				return 1
			else
				UpdateSteeringWithComp(-1)
				return -1
			end
		else
			UpdateThrottleWithComp(-0.6)
			return 0
		end
	else -- Vehicle somewhere not behind; use default behavior
		local GoalFrontVector = Car.Chassis.FloorPanel.CFrame.RightVector
		local DotProduct = PositionDifference:Dot(GoalFrontVector)
		local TargetAngle1 = math.deg(math.acos(DotProduct))
		local TargetAngle = TargetAngle1 - 90
		if TargetAngle > 90 then
			TargetAngle = 90 - TargetAngle
		end

		UpdateSteeringWithComp(math.clamp(TargetAngle / 40, -1, 1))

		return math.abs(math.clamp(TargetAngle / 40, -1, 1))
	end
end

local function PIT(vehicle: Model)
	-- First, find the nearest wheel: left or right
	local vehLeft: BasePart = vehicle:FindFirstChild("SuspensionRL", true).Wheel
	local vehRight: BasePart = vehicle:FindFirstChild("SuspensionRR", true).Wheel

	if CheckDistance(vehLeft.Position, Car.Chassis.SuspensionFR.Wheel.Position) < 50 or CheckDistance(vehRight.Position, Car.Chassis.SuspensionFL.Wheel.Position) < 50 then
		if CheckDistance(vehLeft.Position, Car.Chassis.SuspensionFR.Wheel.Position) < CheckDistance(vehRight.Position, Car.Chassis.SuspensionFL.Wheel.Position) then
			UpdateThrottleWithComp(1)
			FaceSteerToPosition(vehLeft.Position --[[+ vehLeft.AssemblyLinearVelocity]])
		else
			UpdateThrottleWithComp(1)
			FaceSteerToPosition(vehRight.Position --[[+ vehRight.AssemblyLinearVelocity]])
		end
		return true
	else
		return false
	end
end