detectRange = 2.5

vehicle = {}

maxSpeed = 20

goalPos = Vec(0, 0, 0)
SPOTMARKED = false

gCost = 1

testHeight = 2
drivePower = 0.6

detectPoints = {
	[1] = Vec(0, 0, -detectRange * 2),
	[2] = Vec(detectRange, 0, -detectRange),
	[3] = Vec(-detectRange, 0, -detectRange),
	[4] = Vec(-detectRange, 0, 0),
	[5] = Vec(detectRange, 0, 0),
	[6] = Vec(0, 0, detectRange)
}

weights = {
	[1] = 0.85,
	[2] = 0.85,
	[3] = 0.85,
	[4] = 0.5,
	[5] = 0.5,
	[6] = 0.25
}

targetMoves = {
	list = {},
	target = Vec(0, 0, 0),
	targetIndex = 1
}

hitColour = Vec(1, 0, 0)
clearColour = Vec(0, 1, 0)

function init()
	for i = 1, 10 do
		targetMoves.list[i] = Vec(0, 0, 0)
	end

	vehicle.id = FindVehicle("cfg")
	updatePlayerPosTimer = 5
end

function tick(dt)
	updatePlayerPosTimer = updatePlayerPosTimer - dt
	if updatePlayerPosTimer <= 0 then
		updatePlayerPosTimer = 5
		goalPos = VecAdd(GetPlayerTransform().pos, Vec(0,1,0))
	end
	--markLoc()
end

function markLoc()
	if InputPressed("g") then
		local camera = GetCameraTransform()
		local aimpos = TransformToParentPoint(camera, Vec(0, 0, -300))
		local hit, dist, normal = QueryRaycast(camera.pos, VecNormalize(VecSub(aimpos, camera.pos)), 200, 0)
		if hit then
			goalPos = TransformToParentPoint(camera, Vec(0, 0, -dist))
		end
	end

	if VecLength(goalPos) ~= 0 then
		SpawnParticle("smoke", goalPos, Vec(0, 5, 0), 0.5, 1)
	end
end

function update(dt)
	if GetInt("level.tankdispatch") == 0 then
		return
	end
	targetCost = vehicleDetection3()
	targetCost.target = MAV(targetCost.target)
	controlVehicle(targetCost)
end

function vehicleDetection3()
	local vehicleBody = GetVehicleBody(vehicle.id)
	local vehicleTransform = GetVehicleTransform(vehicle.id)
	local min, max = GetBodyBounds(vehicleBody)
	vehicleTransform.pos = TransformToParentPoint(vehicleTransform, Vec(0, testHeight, 0))
	local vehicleTransformOrig = TransformCopy(vehicleTransform)
	local fwd = TransformToParentPoint(vehicleTransform, Vec(0, 0, -detectRange * 1.5))
	local fwdL = TransformToParentPoint(vehicleTransform, Vec(detectRange, 0, -detectRange))
	local fwdR = TransformToParentPoint(vehicleTransform, Vec(-detectRange, 0, -detectRange))
	local boundsSize = VecSub(max, min)

	costs = {}
	bestCost = {key = 0, val = 1000, target = Vec(0, 0, 0)}

	if (VecLength(goalPos) > 0.5 and VecLength(VecSub(GetVehicleTransform(vehicle.id).pos, goalPos)) > 3) then
		for key, detect in ipairs(detectPoints) do
			vehicleTransform = GetVehicleTransform(vehicle.id)
			vehicleTransform.pos = TransformToParentPoint(vehicleTransform, Vec(0, testHeight, 0))
			if (detect[3] < 0) then
				vehicleTransform.pos = TransformToParentPoint(vehicleTransform, Vec(0, 0, -boundsSize[3] * .4))
			elseif (detect[3] > 0) then
				vehicleTransform.pos = TransformToParentPoint(vehicleTransform, Vec(0, 0, boundsSize[3] * .4))
			end
			if (detect[1] < 0) then
				vehicleTransform.pos = TransformToParentPoint(vehicleTransform, Vec(-boundsSize[1] * .25), 0, 0)
			elseif (detect[1] > 0) then
				vehicleTransform.pos = TransformToParentPoint(vehicleTransform, Vec(boundsSize[1] * .25), 0, 0)
			end
			--QueryRequire("physical static large")
			QueryRejectVehicle(vehicle.id)
			local fwdPos = TransformToParentPoint(vehicleTransform, detect)
			local direction = VecSub(fwdPos, vehicleTransform.pos)
			hit, dist = QueryRaycast(vehicleTransform.pos, direction, VecLength(direction) * .5, boundsSize[1] * .5)
			local lineColour = clearColour

			costs[key] = costFunc(TransformToParentPoint(vehicleTransform, detect), hit, key)

			if hit and dist < detectRange then
				lineColour = hitColour
			else
				if costs[key] < bestCost.val then
					bestCost.key = key
					bestCost.val = costs[key]
					bestCost.target = detect
				end
			end
			--DebugLine(vehicleTransform.pos, fwdPos, lineColour[1], lineColour[2], lineColour[3])
		end
	end
	return bestCost
end

function MAV(targetCost)
	targetMoves.targetIndex = (targetMoves.targetIndex % #targetMoves.list) + 1
	targetMoves.target = VecSub(targetMoves.target, targetMoves.list[targetMoves.targetIndex])
	targetMoves.target = VecAdd(targetMoves.target, targetCost)
	targetMoves.list[targetMoves.targetIndex] = targetCost
	return VecScale(targetMoves.target, (#targetMoves.list / 100))
end

function controlVehicle(targetCost)
	if (VecLength(goalPos) > 0.5) then
		local targetMove = VecNormalize(targetCost.target)
		if (VecLength(VecSub(GetVehicleTransform(vehicle.id).pos, goalPos)) > 5) then
			if (targetMove[1] ~= 0 and targetMove[3] == 0) then
				targetMove[3] = 1

				targetMove[1] = -targetMove[1]
			end
			DriveVehicle(vehicle.id, -targetMove[3] * drivePower, -targetMove[1], false)
		else
			DriveVehicle(vehicle.id, 0, 0, true)
		end
	end
end

function costFunc(testPos, hit, key)
	local cost = 100
	if (not hit) then
		cost = VecLength(VecSub(testPos, goalPos)) * (1 - weights[key])
	end
	return cost
end

function vehicleMovement(vel, angVel)
	local vehicleTransform = GetBodyTransform(vehicle.body)
	local targetVel = TransformToParentPoint(vehicleTransform, vel)
	targetVel = VecSub(targetVel, vehicleTransform.pos)
	local targetAngVel = angVel
	local currentVel = GetBodyVelocity(vehicle.body)
	local currentAngVel = GetBodyAngularVelocity(vehicle.body)

	if (VecLength(currentVel) < maxSpeed) then
		SetBodyVelocity(vehicle.body, VecAdd(currentVel, targetVel))
	end
	SetBodyAngularVelocity(vehicle.body, VecAdd(currentAngVel, targetAngVel))
end
