local DL = require("design.DesignerLibrary")
local locomotion = require("creature.locomotion")
local color = require("core.color")
local curveRadius = 2
local maxLoopDistance = math.sqrt(2 * curveRadius * curveRadius)
local progressionHack = 10
local slowDownToleranceDistance = 3.5
local GetCreatureRadius = function(crt)
  local bb = crt:GetBlackboard()
  if bb and bb:Exists("CreatureRadius") then
    return bb:GetNumber("CreatureRadius")
  end
  return 1.75
end
local IsWaiting = function(ai, waiting)
  if not ai.GetMaxSpeedOverride then
    return waiting
  end
  local maxSpeedOverride = ai:GetMaxSpeedOverride()
  return maxSpeedOverride ~= nil and maxSpeedOverride == 0
end
local Loop = {
  off = 0,
  on = 1,
  oneWay = 2
}
local StringToLoop = {
  Off = Loop.off,
  On = Loop.on,
  ["One Way"] = Loop.oneWay
}
local Direction = function(progression, targetProgression, loop, curveDir)
  local difference = targetProgression - progression
  if difference == 0 then
    return 0
  end
  if loop == Loop.off then
    return curveDir * (difference / math.abs(difference))
  end
  if loop == Loop.on and (0 < difference and difference < 0.5 or -1 <= difference and difference < -0.5) then
    return 1
  end
  if loop == Loop.oneWay and (0 < difference or -1 <= difference and difference < -0.5) then
    return 1
  end
  return -1
end
local LeadTheWay = function(ai, curve, params)
  engine.PushProfileMarker("LeadTheWay()")
  local outputSpeed = params.Speed
  local leaderDistance_Wait = params.LeaderDistance_Wait
  local leaderDistance_Continue = params.LeaderDistance_Continue
  local leaderDistance_Backtrack = params.LeaderDistance_Backtrack
  local leaderDistance_BacktrackDone = params.LeaderDistance_BacktrackDone
  local followerDistance_CaughtUp = params.FollowerDistance_CaughtUp
  local followerDistance_Continue = params.FollowerDistance_Continue
  local role = params.Role
  local targetCreature = params.TargetCreature
  local leashingEnabled = params.EnableLeashing
  local targetCreatureDirBufferRad = params.TargetCreatureDirBufferRad
  local avoidDistance_Start = params.AvoidDistance_Start
  local avoidDistance_Wait = params.AvoidDistance_Wait
  local loop = StringToLoop[params.Loop]
  if loop ~= Loop.off then
    local p0 = curve:GetPosition(0)
    local p1 = curve:GetPosition(1)
    local offset = p1 - p0
    local d = math.sqrt(offset.x * offset.x + offset.y * offset.y)
    if d >= maxLoopDistance then
      engine.Warning(string.format("%s is supposed to loop, but the end points are too far apart. Distance on xz plane should be less than 2.5m.", curve:GetName()))
    end
    if role == "follower" then
      engine.Warning(string.format("%s is supposed to loop, but looping with 'Stay Behind' behavior has not been implemented.", curve:GetName()))
    end
  end
  local aiPosition = ai:GetWorldPosition()
  local aiProgression = curve:GetProgression(aiPosition)
  local aiPositionOnCurve = curve:GetPosition(aiProgression)
  local strafing = false
  local waiting = IsWaiting(ai, params.Waiting)
  if engine.IsDebug() then
    curve:DebugDraw(color.white, 45)
    engine.DrawSphere(aiPositionOnCurve, 0.2, color.white)
  end
  local curveDir = 1
  if role == "leader_reverse" then
    curveDir = -1
  end
  local debugStr = ""
  if role == "leader" or role == "leader_reverse" then
    local aiBB = ai:GetBlackboard()
    local aiInWaitGate = aiBB ~= nil and aiBB:Exists("inWaitGate") and aiBB:IsBoolean("inWaitGate") and aiBB:GetBoolean("inWaitGate") == true
    local targetCreaturePos = targetCreature:GetWorldPosition()
    local targetProgressionOnMyCurve = curve:GetProgression(targetCreaturePos)
    local targetPositionOnCurve = curve:GetPosition(targetProgressionOnMyCurve)
    local distBtwnMeAndTarget = (targetPositionOnCurve - aiPosition):Length()
    local distBtwnMeAndMySpline = (aiPositionOnCurve - aiPosition):Length()
    if params.TargetCreatureDirBufferPos == nil then
      params.TargetCreatureDirBufferPos = targetCreaturePos
    end
    local targetCreatureDistFromBufferPos = targetCreaturePos:Distance(params.TargetCreatureDirBufferPos)
    if targetCreatureDirBufferRad < targetCreatureDistFromBufferPos then
      local bufferPosProg = curve:GetProgression(params.TargetCreatureDirBufferPos)
      local direction = Direction(bufferPosProg, targetProgressionOnMyCurve, loop, curveDir)
      if 0 < direction then
        params.TargetCreatureDir = "forward"
        leaderDistance_Wait = params.LeaderDistance_Wait
        leaderDistance_Continue = params.LeaderDistance_Continue
        leaderDistance_Backtrack = 10000
        leaderDistance_BacktrackDone = 10000
      elseif direction < 0 then
        params.TargetCreatureDir = "backwards"
        leaderDistance_Wait = 0
        leaderDistance_Continue = 0
        leaderDistance_Backtrack = params.LeaderDistance_Backtrack
        leaderDistance_BacktrackDone = params.LeaderDistance_BacktrackDone
      end
      params.TargetCreatureDirBufferPos = targetCreaturePos
    elseif params.TargetCreatureDir == "forward" then
      leaderDistance_Wait = params.LeaderDistance_Wait
      leaderDistance_Continue = params.LeaderDistance_Continue
      leaderDistance_Backtrack = 10000
      leaderDistance_BacktrackDone = 10000
    elseif params.TargetCreatureDir == "backwards" then
      leaderDistance_Wait = 0
      leaderDistance_Continue = 0
      leaderDistance_Backtrack = params.LeaderDistance_Backtrack
      leaderDistance_BacktrackDone = params.LeaderDistance_BacktrackDone
    end
    if engine.IsDebug() then
      debugStr = debugStr .. "spline nav (outputSpeed = " .. tostring(outputSpeed) .. " loop = " .. params.Loop .. ", intended dir = " .. tostring(curveDir) .. ") - leading " .. targetCreature:GetName()
      if waiting == true then
        debugStr = debugStr .. [[

stopped (waiting for ]] .. targetCreature:GetName()
        if aiInWaitGate then
          debugStr = debugStr .. " in wait gate)"
        else
          debugStr = debugStr .. " due to normal leashing)"
        end
      elseif waiting == false then
        debugStr = debugStr .. [[

moving]]
        if params.Backtracking == true then
          debugStr = debugStr .. " (backtracking to " .. targetCreature:GetName() .. ")"
        end
        if leashingEnabled == false then
          debugStr = debugStr .. " (leashing is disabled)"
        end
      end
      engine.DrawCircle(aiPosition, leaderDistance_Continue, ai:GetWorldUp(), color.aqua)
      engine.DrawCircle(aiPosition, leaderDistance_Wait, ai:GetWorldUp(), color.lime)
      engine.DrawCircle(aiPosition, leaderDistance_BacktrackDone, ai:GetWorldUp(), color.green)
      engine.DrawCircle(aiPosition, leaderDistance_Backtrack, ai:GetWorldUp(), color.blue)
      engine.DrawCircle(aiPosition + engine.Vector.New(0, 0.5, 0), leaderDistance_Wait - slowDownToleranceDistance, ai:GetWorldUp(), color.red)
      engine.DrawCircle(aiPosition + engine.Vector.New(0, 0.5, 0), leaderDistance_BacktrackDone + slowDownToleranceDistance, ai:GetWorldUp(), color.red)
      engine.DrawTextInWorld(params.TargetCreatureDirBufferPos, [[
- Lead The Way Directionality -
Direction: ]] .. params.TargetCreatureDir .. [[

Direction change buffer radius: ]] .. tostring(targetCreatureDirBufferRad), color.yellow)
      engine.DrawCircle(params.TargetCreatureDirBufferPos, targetCreatureDirBufferRad, ai:GetWorldUp(), color.yellow)
    end
    if 0 < Direction(aiProgression, targetProgressionOnMyCurve, loop == Loop.off and Loop.off or Loop.on, curveDir) and aiInWaitGate == false then
      if engine.IsDebug() then
        debugStr = debugStr .. " (I'm behind " .. targetCreature:GetName() .. ")"
      end
      if waiting == true then
        waiting = false
      end
      if params.Backtracking == true then
        params.Backtracking = false
      end
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    elseif leashingEnabled and distBtwnMeAndTarget > leaderDistance_Backtrack and distBtwnMeAndMySpline < curveRadius then
      if waiting == true then
        waiting = false
      end
      if params.Backtracking == false then
        params.Backtracking = true
      end
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    elseif leashingEnabled and waiting == false and (params.Backtracking == false and distBtwnMeAndTarget > leaderDistance_Wait - slowDownToleranceDistance or params.Backtracking == true and distBtwnMeAndTarget < leaderDistance_BacktrackDone + slowDownToleranceDistance) or aiInWaitGate == true then
      if aiInWaitGate == true then
        waiting = true
        params.Backtracking = false
        ai:SetMaxSpeedOverride(0)
        outputSpeed = 0
      else
        outputSpeed = 1.3
        if (params.Backtracking == false and distBtwnMeAndTarget > leaderDistance_Wait or params.Backtracking == true and distBtwnMeAndTarget < leaderDistance_BacktrackDone) and distBtwnMeAndMySpline < curveRadius then
          waiting = true
          params.Backtracking = false
          ai:SetMaxSpeedOverride(0)
          outputSpeed = 0
        end
      end
      if engine.IsDebug() then
        debugStr = debugStr .. " (slowing down to stop)"
      end
    elseif leashingEnabled and distBtwnMeAndTarget < leaderDistance_Continue or leashingEnabled == false and aiInWaitGate == false and (waiting == true or params.Backtracking == true) then
      if waiting == true then
        waiting = false
      end
      if params.Backtracking == true then
        params.Backtracking = false
      end
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    end
    if distBtwnMeAndMySpline > curveRadius and not aiInWaitGate then
      waiting = false
      aiProgression = curve:AdvanceProgression(targetProgressionOnMyCurve, params.LeaderDistance_Continue * curveDir)
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    end
    if waiting then
      ai:ClearFocus()
    end
    local distanceToEnd = aiPosition:Distance(curve:GetPosition(1))
    if distanceToEnd < 2 then
      ai:ClearFocus()
    end
  elseif role == "follower" then
    if engine.IsDebug() then
      debugStr = debugStr .. "spline nav - following " .. targetCreature:GetName()
      if waiting == true then
        debugStr = debugStr .. [[

stopped (caught up to ]] .. targetCreature:GetName() .. ")"
      elseif waiting == false then
        debugStr = debugStr .. [[

moving]]
      end
      engine.DrawCircle(aiPosition, followerDistance_CaughtUp, ai:GetWorldUp(), color.blue)
      engine.DrawCircle(aiPosition, followerDistance_Continue, ai:GetWorldUp(), color.green)
    end
    local targetCreaturePos = targetCreature:GetWorldPosition()
    local leaderProgressionOnMyCurve = curve:GetProgression(targetCreaturePos)
    local distBtwnMeAndTarget = (targetCreaturePos - aiPosition):Length()
    local distBtwnMeAndMySpline = (aiPositionOnCurve - aiPosition):Length()
    params.Backtracking = false
    if waiting == false and (leaderProgressionOnMyCurve - aiProgression <= 0 or distBtwnMeAndTarget < followerDistance_CaughtUp + slowDownToleranceDistance) then
      outputSpeed = 1.3
      if followerDistance_CaughtUp > distBtwnMeAndTarget and distBtwnMeAndMySpline < curveRadius then
        ai:SetMaxSpeedOverride(0)
        outputSpeed = 0
        waiting = true
      end
      if engine.IsDebug() then
        debugStr = debugStr .. " slowing down to stop"
      end
    elseif waiting == true and followerDistance_Continue < distBtwnMeAndTarget and 0 < leaderProgressionOnMyCurve - aiProgression then
      waiting = false
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    end
    ai:SetFocus(targetCreature)
    if engine.IsDebug() then
      debugStr = debugStr .. [[

my focus is on ]] .. targetCreature:GetName()
    end
  elseif role == "relaxedFollower" then
    if engine.IsDebug() then
      debugStr = debugStr .. "spline nav (loop = " .. params.Loop .. ") - staying near " .. targetCreature:GetName()
      if waiting == true then
        debugStr = debugStr .. [[

stopped (caught up to ]] .. targetCreature:GetName() .. ")"
      elseif waiting == false then
        debugStr = debugStr .. [[

moving]]
      end
      engine.DrawCircle(aiPosition + engine.Vector.New(0, 0.5, 0), followerDistance_CaughtUp, ai:GetWorldUp(), color.blue)
      engine.DrawCircle(aiPosition + engine.Vector.New(0, 0.7, 0), followerDistance_CaughtUp + slowDownToleranceDistance, ai:GetWorldUp(), color.red)
      engine.DrawCircle(aiPosition + engine.Vector.New(0, 0.5, 0), followerDistance_Continue, ai:GetWorldUp(), color.green)
    end
    local targetCreaturePos = targetCreature:GetWorldPosition()
    local targetProgressionOnMyCurve = curve:GetProgression(targetCreaturePos)
    local targetPositionOnMyCurve = curve:GetPosition(targetProgressionOnMyCurve)
    local distBtwnMeAndTarget = (targetCreaturePos - aiPosition):Length()
    local distBtwnMeAndMySpline = (aiPositionOnCurve - aiPosition):Length()
    local distBtwnTargetAndMySpline = (targetPositionOnMyCurve - targetCreaturePos):Length()
    local distBtwnMeAndTargetsPosOnMySpline = (targetPositionOnMyCurve - aiPosition):Length()
    local targetReferencePos, distToTargetReferencePos
    if distBtwnTargetAndMySpline < followerDistance_CaughtUp - 0.5 and followerDistance_CaughtUp > distBtwnMeAndTarget then
      targetReferencePos = targetCreaturePos
      distToTargetReferencePos = distBtwnMeAndTarget
    else
      targetReferencePos = targetPositionOnMyCurve
      distToTargetReferencePos = distBtwnMeAndTargetsPosOnMySpline
    end
    if engine.IsDebug() then
      engine.DrawLine(aiPosition, targetReferencePos, color.lime)
      engine.DrawSphere(targetReferencePos, 0.2, color.lime)
    end
    if distBtwnMeAndMySpline > curveRadius then
      params.Backtracking = true
      waiting = false
      aiProgression = targetProgressionOnMyCurve
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    end
    local direction = Direction(aiProgression, targetProgressionOnMyCurve, loop, curveDir)
    if 0 < direction then
      params.Backtracking = false
    elseif direction < 0 then
      params.Backtracking = true
    end
    if engine.IsDebug() then
      if not params.Backtracking then
        debugStr = debugStr .. " (forward)"
      else
        debugStr = debugStr .. " (backward)"
      end
    end
    if waiting == false and distToTargetReferencePos < followerDistance_CaughtUp + slowDownToleranceDistance and distBtwnMeAndMySpline < curveRadius then
      outputSpeed = 1.3
      if followerDistance_CaughtUp > distToTargetReferencePos then
        ai:SetMaxSpeedOverride(0)
        outputSpeed = 0
        waiting = true
      end
      if engine.IsDebug() then
        debugStr = debugStr .. " (slowing down to stop)"
      end
    elseif waiting == true and followerDistance_Continue < distToTargetReferencePos then
      waiting = false
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    end
    local speedUpFollowerDistance_Continue = followerDistance_Continue * 2.5
    local targetCreatureSpeed = targetCreature:GetVelocity():Length()
    if distBtwnMeAndTarget > speedUpFollowerDistance_Continue or followerDistance_Continue < distToTargetReferencePos and 5 < targetCreatureSpeed then
      outputSpeed = 6.25
      if engine.IsDebug() then
        debugStr = debugStr .. " (sprinting)"
      end
    end
  elseif role == "avoid" then
    if engine.IsDebug() then
      debugStr = debugStr .. "spline nav (loop = " .. params.Loop .. ") - avoiding enemies"
      if waiting == true then
        debugStr = debugStr .. [[

stopped]]
      elseif waiting == false then
        debugStr = debugStr .. [[

moving]]
      end
    end
    local tangent = curve:GetTangent(aiProgression):Normalized()
    local avoid = false
    local enemies = DL.FindLivingEnemies(ai, avoidDistance_Wait + 10)
    local nearbyEnemies = {}
    for _, enemy in ipairs(enemies) do
      if avoidDistance_Wait > aiPosition:Distance(enemy.WorldPosition) - GetCreatureRadius(enemy) then
        table.insert(nearbyEnemies, enemy)
      end
    end
    if waiting == true then
      for _, enemy in ipairs(nearbyEnemies) do
        if not avoid then
          local enemyPosition = enemy.WorldPosition
          if avoidDistance_Start > aiPosition:Distance(enemyPosition) - GetCreatureRadius(enemy) then
            params.Backtracking = 0 < (enemyPosition - aiPosition):Dot(tangent)
            avoid = true
          end
        end
      end
    end
    if avoid or aiPosition:Distance(aiPositionOnCurve) > curveRadius then
      waiting = false
      ai:ClearMaxSpeedOverride()
      outputSpeed = params.Speed
    elseif #nearbyEnemies == 0 then
      waiting = true
      ai:SetMaxSpeedOverride(0)
      outputSpeed = 0
    end
    if engine.IsDebug() then
      local up = engine.Vector.New(0, 1, 0)
      local offset = engine.Vector.New(0, 0.05, 0)
      engine.DrawCircle(aiPosition + offset, avoidDistance_Start, up, avoid and color.red or color.green)
      engine.DrawCircle(aiPosition + offset, avoidDistance_Wait, up, #nearbyEnemies ~= 0 and color.red or color.yellow)
      for _, enemy in ipairs(enemies) do
        engine.DrawCircle(enemy.WorldPosition + offset, GetCreatureRadius(enemy), up, color.white)
      end
    end
  end
  if engine.IsDebug() then
    engine.DrawTextInWorld(aiPosition + engine.Vector.New(0, 2, 0), debugStr, color.white)
  end
  local aheadProgression, farAheadProgression
  local wrap = false
  if params.Backtracking == false then
    aheadProgression = curve:AdvanceProgression(aiProgression, 1 * curveDir)
    farAheadProgression = curve:AdvanceProgression(aiProgression, progressionHack * curveDir)
    wrap = loop ~= Loop.off and farAheadProgression == 1
  elseif params.Backtracking == true then
    aheadProgression = curve:AdvanceProgression(aiProgression, -1 * curveDir)
    farAheadProgression = curve:AdvanceProgression(aiProgression, -1 * progressionHack * curveDir)
    wrap = loop == Loop.on and farAheadProgression == 0
  end
  local destination = {}
  if not wrap then
    local pointCount = 4
    for i = 0, pointCount do
      local t = aheadProgression + i * (farAheadProgression - aheadProgression) / pointCount
      destination[i + 1] = curve:GetPosition(t)
    end
  elseif params.Backtracking == false then
    if aheadProgression ~= aiProgression then
      table.insert(destination, curve:GetPosition(aheadProgression))
      table.insert(destination, curve:GetPosition(1))
    end
    table.insert(destination, curve:GetPosition(0))
    table.insert(destination, curve:GetPosition(curve:AdvanceProgression(0, progressionHack * 0.5)))
    table.insert(destination, curve:GetPosition(curve:AdvanceProgression(0, progressionHack)))
  else
    if aheadProgression ~= aiProgression then
      table.insert(destination, curve:GetPosition(aheadProgression))
      table.insert(destination, curve:GetPosition(0))
    end
    table.insert(destination, curve:GetPosition(1))
    table.insert(destination, curve:GetPosition(curve:AdvanceProgression(1, -progressionHack * 0.5)))
    table.insert(destination, curve:GetPosition(curve:AdvanceProgression(1, -progressionHack)))
  end
  locomotion.SetActuator(ai, {
    Destination = destination,
    Facing = ai:GetWorldForward(),
    Speed = outputSpeed,
    Strafe = strafing
  })
  params.Waiting = waiting
  engine.PopProfileMarker()
end
return {LeadTheWay = LeadTheWay}
