#include "../../script/helpers/entity.lua"
#include "../../script/vehicle.lua"

-- ===================================
-- Config:
MAX_SPEED               = 20.0 -- In prefabs top speed = 15
MAX_SPEED_WHEN_TURNING  = 20.0
TIME_BEFORE_STUCK       = 10.0
pNum_of_combine         = GetIntParam("num_of_combine", 1)
pNext_wp                = GetIntParam("next_wp", 1)

id = nil
vehicle = nil
target = nil
waypoints = nil
timer = nil
next_wp = nil
prev_wp = nil
end_line_pos = nil
start_line_pos = nil
is_stuck = nil
time_to_stuck = nil
disabled = nil
is_sandbox = nil

function init()
    id = FindVehicle()
    if not IsHandleValid(id) then
        return
    end

    on_init()
    target          = Vec()
    waypoints       = FindWaypoints()
    next_wp         = pNext_wp
    prev_wp         = PrevWP()
    end_line_pos    = waypoints[next_wp]
    start_line_pos  = waypoints[prev_wp]
    is_stuck        = false
    time_to_stuck   = TIME_BEFORE_STUCK
    disabled        = false
    is_sandbox      = GetBool("level.sandbox")
    timer           = math.random(250) / 1000
end

function on_init()
	vehicle = c_vehicle:new(id)
	vehicle:SetSteeringController({2, 0.9, 0.5})
	vehicle:SetAccelerationController({})
end

function handleCommand(str)
    if str == "quickload" then
        on_init()
    end
end

function update(dt)
    if disabled then return end
    timer = timer - dt
    if timer <= 0 then timer = 0.25 else return end
    dt = 0.25

    if not IsHandleValid(id) or GetVehicleHealth(id) == 0 or is_stuck or (is_sandbox and GetPlayerVehicle() == id) then
        disabled = cEntity:EnqueueDeletion()
        return
    end
    vehicle:Tick(dt)

    CheckWayPoint()
    FindTarget()
	vehicle:TurnToPositionByController(dt, target)
    if next_wp == 1 or next_wp == 3 then
        vehicle:UpdateSpeed(dt, MAX_SPEED_WHEN_TURNING)
    else
        vehicle:UpdateSpeed(dt, MAX_SPEED)
    end
    -- VEHICLE_DRIVE           = 0.3
	-- vehicle:TurnToPosition(target)
    -- vehicle.control.drive = VEHICLE_DRIVE
    -- vehicle:Drive()

    if time_to_stuck < 0 then
        is_stuck = true
    elseif vehicle:GetVehicleVelocity() < 5.0 then
        time_to_stuck = time_to_stuck - dt
    else
        time_to_stuck = TIME_BEFORE_STUCK
    end
end

function tick(dt)
    if disabled then return end

    vehicle:Drive()
end

function CheckWayPoint()
    local distance_to_next_wp = VecLength(VecSub(vehicle.t.pos, waypoints[next_wp]))
    if distance_to_next_wp < 15.0 then
        prev_wp = next_wp
        next_wp = next_wp % #waypoints + 1
    end
end

function FindTarget()
    end_line_pos = waypoints[next_wp]
    start_line_pos = waypoints[prev_wp]
    target = end_line_pos
end

function PrevWP()
    return next_wp == 1 and #waypoints or next_wp - 1
end

function FindWaypoints()
    local waypoint_tag = "combine_way" .. pNum_of_combine
	local waypoints_loc = {}
	local waypoints_locations = FindLocations(waypoint_tag, true)
	for i = 1, #waypoints_locations do
		local handle = waypoints_locations[i]
		local number = tonumber(GetTagValue(handle, waypoint_tag))
		local transform = GetLocationTransform(handle)

		waypoints_loc[number] = transform.pos
	end

	return waypoints_loc
end
