local class = require("core.class")
local thunk = require("core.thunk")
local locomotion = require("creature.locomotion")
local WEAK = {__mode = "k"}
local activeRequests = setmetatable({}, WEAK)
local OnAIUpdate = function(ai)
  local updated = false
  for r in pairs(activeRequests) do
    assert(not updated, "Performing more than one FollowPointCloudUpdate in a single frame!")
    r.result = nil
    local result = game.AIUtil.FollowPointCloudUpdate(ai, r.params)
    if result then
      r:post_process_result(result)
    end
    updated = true
  end
end
thunk.Install("OnAIUpdate", OnAIUpdate)
local FollowPointRequester = class.Class("FollowPointRequester")
function FollowPointRequester:init(ai, tweakName)
  assert(tweakName, "No tweak name given")
  self.params = {TweakName = tweakName, DebugDraw = false}
  self.ai = ai
  self.intent_state = {}
end
local MAX_DRIFT_DISTANCE = 1
local MAX_DRIFT_TIME = 0.2
local check_intent_valid = function(dt, intent_state, result)
  local valid = false
  if intent_state.pos then
    valid = true
    local distance = game.AIUtil.Distance(intent_state.pos, result.Position)
    if distance > MAX_DRIFT_DISTANCE then
      intent_state.driftTimer = (intent_state.driftTimer or 0) + dt
      if intent_state.driftTimer > MAX_DRIFT_TIME then
        valid = false
      end
    else
      intent_state.driftTimer = 0
    end
  end
  return valid
end
local update_intent = function(dt, intent_state, result)
  local reset = not check_intent_valid(dt, intent_state, result)
  if reset then
    intent_state.driftTimer = 0
    intent_state.pos = result.Position
    intent_state.dir = result.Direction
    intent_state.vel = result.Velocity
    intent_state.original_vel = result.Velocity
  elseif intent_state.pos then
    intent_state.pos = intent_state.pos + intent_state.vel * dt
  end
end
local reduce_intent_velocity = function(intent_state, result)
  local intent_vel = intent_state.vel
  local ideal_vel = result.Velocity
  local original_vel = intent_state.original_vel
  local intent_speed = intent_vel:Length()
  local ideal_speed = ideal_vel:Length()
  local original_speed = original_vel:Length()
  local intent_norm = intent_vel:Normalized()
  local dot = math.max(ideal_vel:Dot(intent_vel), 0)
  local original_speed_adjusted = original_speed * dot
  local speed = math.min(original_speed_adjusted, ideal_speed, intent_speed)
  local output_vel = intent_norm * speed
  intent_state.vel = output_vel
end
function FollowPointRequester:post_process_result(result)
  local intent_state = self.intent_state
  local dt = self.ai:GetFrameTime()
  update_intent(dt, intent_state, result)
  reduce_intent_velocity(intent_state, result)
  self.result = {
    Position = intent_state.pos,
    Direction = intent_state.dir,
    Velocity = intent_state.vel,
    SelectedIndex = result.SelectedIndex,
    AreAllPointsValid = result.AreAllPointsValid,
    AreAllPointsBad = result.AreAllPointsBad,
    AreAllPointsUnusable = result.AreAllPointsUnusable
  }
end
function FollowPointRequester:SetTarget(target)
  self.params.FollowTarget = target
end
function FollowPointRequester:SetTweak(tweakName)
  self.params.TweakName = tweakName
end
function FollowPointRequester:Start()
  if activeRequests[self] then
    activeRequests[self] = activeRequests[self] + 1
  else
    activeRequests[self] = 1
  end
end
function FollowPointRequester:Stop()
  assert(activeRequests[self] ~= nil, "Calling Stop on FollowPointCloud without first calling Start.")
  activeRequests[self] = activeRequests[self] - 1
  if activeRequests[self] == 0 then
    activeRequests[self] = nil
    self.result = nil
  end
end
function FollowPointRequester:GetResult()
  return self.result
end
local Follow = function(ai, followPoints, params)
  local speed = params.Speed and params.Speed or 4
  local strafe = params.Strafe and params.Strafe or false
  local facing = params.Facing and params.Facing or ai:GetWorldForward()
  local followResult = followPoints:GetResult()
  if followResult and followResult.Position then
    _G.constants.followPointPosition = followResult.Position
    locomotion.SetActuator(ai, {
      Destination = followResult.Position,
      Facing = facing,
      Speed = speed,
      Strafe = strafe,
      StopDistance = ai:GetVelocity():Length() > 4.5 and 1 or 0.5,
      StartDistance = 1.5
    })
  end
end
return {FollowPointRequester = FollowPointRequester, Follow = Follow}
