lua/vehicle/extensions/overrideAI.lua
-- This Source Code Form is subject to the terms of the bCDDL, v. 1.1.
-- If a copy of the bCDDL was not distributed with this
-- file, You can obtain one at http://beamng.com/bCDDL-1.1.txt
local M = {}
local E = setmetatable({}, {
  __newindex = function(t, key, val)
    log('E', 'ai.lua', 'Tried to insert new elements into token empty table')
  end
})

M.mode = 'disabled' -- this is the main mode
M.manualTargetName = nil
M.debugMode = 'off'
M.speedMode = nil
M.routeSpeed = nil
M.extAggression = 0.3
M.cutOffDrivability = 0
M.driveInLaneFlag = 'off'
M.extAvoidCars = 'auto'

-- [[ STORE FREQUENTLY USED FUNCTIONS IN UPVALUES ]] --
local buffer = require("string.buffer")
local max = math.max
local min = math.min
local sin = math.sin
local asin = math.asin
local pi = math.pi
local abs = math.abs
local sqrt = math.sqrt
local floor = math.floor
local tableInsert = table.insert
local tableRemove = table.remove
local tableConcat = table.concat
local strFormat = string.format

-- [[ HEAVY DEBUG MODE ]] --
local visDebug = {
  trajecRec = {
    last = 0
  },
  routeRec = {
    last = 0
  },
  labelRenderDistance = 10,
  debugSpots = {},
  candidatePaths = nil
}
-- local newPositionsDebug = {} -- for debug purposes

-- [[ Simulation time step]] --
local dt

-- [[ ENVIRONMENT VARIABLES ]] --
local g = obj:getGravity() -- standard gravity is negative
local gravityDir = vec3(0, 0, sign2(g))
g = max(1e-30, abs(g)) -- prevent divivion by 0 gravity

-- [[ PERFORMANCE RELATED ]] --
local aggression = 1

-- [[ AI DATA: POSITION, CONTROL, STATE ]] --
local ego = {
  pos = obj:getFrontPosition(),
  dirVec = obj:getDirectionVector(),
  prevDirVec = obj:getDirectionVector(),
  upVec = obj:getDirectionVectorUp(),
  rightVec = vec3(),
  width = nil,
  length = nil,
  wheelBase = nil,
  currentSegment = {},
  vel = vec3(obj:getSmoothRefVelocityXYZ()),
  speed = vec3(obj:getSmoothRefVelocityXYZ()):length(),
  staticFrictionCoef = 1,
  ghostR = nil,
  ghostL = nil
}

ego.race = {
  dT_min = 0.1,
  dT_max = 0.3,
  Td = 0.1,
  Tp = 0.01,
  d = 4, -- distance thredshold, unless my distance from vehicle ahead is greater than this, I'll want to go faster than him
  kp = 0.6
}

local targetSpeedDifSmoother = newTemporalSmoothingNonLinear(1e300, 4, 0)
local targetSpeedSmoother = newTemporalSmoothingNonLinear(10, 10, ego.speed)
local egoDeviationSmoother = newTemporalSmoothing(1)
local smoothTcs = newTemporalSmoothingNonLinear(0.1, 0.9)
local throttleSmoother = newTemporalSmoothing(1e30, 0.2)

local internalState = {
  changePlanTimer = 0,
  egoCannotMoveTime = 0,
  egoForceGoFrontTime = 0,
  road = 'onroad',
  crash = {
    time = 0,
    manoeuvre = 0,
    dir = nil,
    pos = nil
  },
  chaseData = {
    playerState = nil,
    playerStoppedTimer = 0,
    playerRoad = nil,
    driveAhead = false,
    targetSpeed = nil
  }
}

local twt = {
  state = 0,
  dirState = {0, 0}, -- direction, steer
  posTable = {vec3(), vec3(), vec3(), vec3()},
  dirTable = {vec3(), vec3(), vec3(), vec3()},
  minRay = "",
  rayMins = {math.huge, math.huge, math.huge, math.huge, math.huge, math.huge, math.huge, math.huge}, -- clockwise beginning from corner front left
  minRayCoefs = {0, 0, 0, 0, 0, 0, 0, 0}, -- clockwise as above
  blueNoiseCoefs = {0, 0, 0, 0, 0, 0, 0, 0},
  biasedCoefs = {0, 0, 0, 0, 0, 0, 0, 0},
  blueNoiseRR = 0,
  sampleCounter = 0, -- for starting scan, to give some time for precision
  targetSpeed = 0,
  OBBinRange = {},
  RRT = {1, 1, 1, 2, 1, 2, 2, 2, 2, 2, 2, 3, 2, 3, 3, 3, 3, 3, 3, 4, 3, 4, 4, 4, 4, 4, 4, 1, 4, 1, 1, 1},
  speedSmoother = newTemporalSmoothingNonLinear(1000, 0.25),
  steerSmoother = newTemporalSmoothingNonLinear(2)
}

twt.reset = function()
  twt.state = 0
  twt.dirState[1], twt.dirState[2] = 0, 0
  twt.minRay = nil
  for i = 1, #twt.rayMins do
    twt.rayMins[i] = math.huge
  end
  for i = 1, #twt.blueNoiseCoefs do
    twt.blueNoiseCoefs[i] = 0
  end
  twt.sampleCounter = 0
  twt.targetSpeed = 0
  twt.speedSmoother:set(0)
  twt.steerSmoother:set(0)
end

local forces = {}
local velocities = {}
local lastCommand = {
  steering = 0,
  throttle = 0,
  brake = 0,
  parkingbrake = 0
}

local opt = {
  driveInLaneFlag = false,
  racing = false,
  trajMethod = nil,
  aggressionMode = nil,
  avoidCars = 'on'
}

-- [[ TRAFFIC ]] --
local trafficStates = {}
local trafficPathState = {}

local traffic = {
  trafficTable = {},
  trafficFilterQue = {
    array = {},
    dict = {}
  },
  trafficFilterFromQue = {},
  frameQue = 0,
  rateQue = 0, -- 1 means adding a que vehicle every second
  filtered = true, -- TrafficFilter on or off (true/false)
  intersection = true, -- check for intersection between ego and other vehicle
  vdraw = false, -- draw debuf spheres to show traffic vehicles in trafficTable
  Rfl = 60, -- larger radius for traffic filter queue
  Rfl_fun = function(v)
    return clamp((v + 10) * 3, 60, 200)
  end, --
  Rfs = 60, -- smaller radius for traffic table
  Rfs_fun = function(v)
    return clamp(v * 3, 60, 100)
  end,
  distAhead = 40 -- minimum Ahead distance for searching traffic vehicle
}

-- [[ OPPONENT DATA ]] --
local player

-- [[ SETTINGS, PARAMETERS, AUXILIARY DATA ]] --
local mapData -- map data including node connections (edges and edge data), node positions and node radii
local signalsData -- traffic intersection and signals data
local currentRoute
local MIN_PLAN_COUNT = 3
local targetWPName
local edgeDict
local wpList
local manualPath
local validateInput = nop
local speedProfile
local loopPath, noOfLaps
local parameters
local restoreGearboxMode = false
local targetObjectSelectionMode
local scriptData
local scriptai = nil
local recover = {
  recoverOnCrash = false,
  recoverTimer = 0,
  _recoverOnCrash = false
}
------------------------------

-- For Debugging (useful function to print a numbered list of upvalues of a specific function)
local function inspect_upvalues(func)
  local i = 1
  while true do
    local name, value = debug.getupvalue(func, i)
    if not name then
      break
    end
    print(string.format("Upvalue %d: %s = %s", i, name, tostring(value)))
    i = i + 1
  end
end

local dataLogger = {
  logData = nop
}
local function logDataTocsv()
  if not dataLogger.csvFile then
    print('Started Logging Data')
    dataLogger.csvFile = require('csvlib').newCSV("time", "posX", "posY", "posZ", "speed", "targetSpeed", "ax", "ay",
      "az", "vx", "vy", "vz", "throttle", "brake", "steering", "staticFrictionCoef")
    dataLogger.time = 0
  else
    dataLogger.time = dataLogger.time + dt
  end
  dataLogger.csvFile:add(dataLogger.time, ego.pos.x, ego.pos.y, ego.pos.z, ego.speed,
    ((currentRoute or E).plan or E).targetSpeed, -sensors.gy, sensors.gx, sensors.gz, ego.vel:dot(ego.dirVec),
    -(ego.vel:dot(ego.rightVec)), ego.vel:dot(ego.upVec), lastCommand.throttle * 100, lastCommand.brake * 100,
    lastCommand.steering * 100, ego.staticFrictionCoef)
end

local function writeCsvFile(name)
  if dataLogger.csvFile then
    print('Writing Data to CSV.')
    dataLogger.csvFile:write(name)
    dataLogger.csvFile = nil
    dataLogger.time = nil
    print('Done')
  else
    print('No data to write')
  end
end

local function startStopDataLog(name)
  if dataLogger.logData == nop then
    print('Initialized Data Log')
    dataLogger.logData = logDataTocsv
    dataLogger.name = name
  else
    print('Stopped Logging Data')
    dataLogger.logData = nop
    writeCsvFile(dataLogger.name or name)
    dataLogger.name = nil
  end
end

local function getObjectBoundingBox(id)
  local x = obj:getObjectDirectionVector(id)
  local z = obj:getObjectDirectionVectorUp(id)
  local y = z:cross(x)
  y:setScaled(obj:getObjectInitialWidth(id) * 0.5 / max(y:length(), 1e-30))
  x:setScaled(obj:getObjectInitialLength(id) * 0.5)
  z:setScaled(obj:getObjectInitialHeight(id) * 0.5)
  return obj:getObjectCenterPosition(id), x, y, z
end

local function drawOBB(c, x, y, z, col)
  -- c: center point
  -- x: front half extent vec
  -- y: left half extent vec
  -- z: up half extent vec

  local debugDrawer = obj.debugDrawProxy
  col = col or color(255, 0, 0, 255)

  local p1 = c - x + y - z -- RLD
  local p2 = c - x - y - z -- RRD
  local p3 = c - x - y + z -- RRU
  local p4 = c - x + y + z -- RLU
  local p5 = c + x + y - z -- FLD
  local p6 = c + x - y - z -- FRD
  local p7 = c + x - y + z -- FRU
  local p8 = c + x + y + z -- FLU

  -- rear face
  debugDrawer:drawCylinder(p1, p2, 0.02, col)
  debugDrawer:drawCylinder(p2, p3, 0.02, col)
  debugDrawer:drawCylinder(p3, p4, 0.02, col)
  debugDrawer:drawCylinder(p4, p1, 0.02, col)

  -- front face
  debugDrawer:drawCylinder(p5, p6, 0.02, col)
  debugDrawer:drawCylinder(p6, p7, 0.02, col)
  debugDrawer:drawCylinder(p7, p8, 0.02, col)
  debugDrawer:drawCylinder(p8, p5, 0.02, col)

  -- left face
  debugDrawer:drawCylinder(p1, p5, 0.02, col)
  debugDrawer:drawCylinder(p4, p8, 0.02, col)

  -- right face
  debugDrawer:drawCylinder(p2, p6, 0.02, col)
  debugDrawer:drawCylinder(p3, p7, 0.02, col)
end

local function drawVehicleBoundingBox(vid, col)
  local c, vx, vy, vz = getObjectBoundingBox(vid)
  drawOBB(c, vx, vy, vz, col)
end

local function aistatus(status, category)
  guihooks.trigger("AIStatusChange", {
    status = status,
    category = category
  })
end

local function getState()
  return M
end

local function stateChanged()
  if playerInfo.anyPlayerSeated then
    guihooks.trigger("AIStateChange", getState())
  end
end

local function setSpeed(speed)
  if type(speed) ~= 'number' then
    M.routeSpeed = nil
  else
    M.routeSpeed = speed
  end
end

local function setSpeedMode(speedMode)
  if speedMode == 'set' or speedMode == 'limit' or speedMode == 'legal' or speedMode == 'off' then
    M.speedMode = speedMode
  else
    M.speedMode = nil
  end
end

local function resetSpeedModeAndValue()
  M.speedMode = nil -- maybe this should be 'off'
  M.routeSpeed = nil
end

local function setAggressionInternal(v)
  aggression = v or M.extAggression
end

local function setAggressionExternal(v)
  M.extAggression = v or M.extAggression
  setAggressionInternal()
  stateChanged()
end

local function setAggressionMode(aggrmode)
  if aggrmode == 'rubberBand' then
    opt.aggressionMode = aggrmode
  else
    opt.aggressionMode = nil
    setAggressionInternal()
  end
end

local function resetAggression()
  setAggressionInternal()
end

local function resetInternalStates()
  trafficStates.block = {
    timer = 0,
    coef = 0,
    timerLimit = 6,
    block = false
  }
  trafficStates.side = {
    timer = 0,
    cTimer = 0,
    side = 1,
    displacement = 0,
    timerLimit = 6
  }
  trafficStates.action = {
    hornTimer = -1,
    hornTimerLimit = 1,
    forcedStop = false
  }
  trafficStates.intersection = {
    timer = 0,
    turn = 0,
    block = false
  }

  internalState.chaseData.playerState = nil
  internalState.chaseData.playerStoppedTimer = 0
  internalState.chaseData.playerRoad = nil
  internalState.chaseData.driveAhead = false
  internalState.chaseData.targetSpeed = nil

  internalState.crash.time = 0
  internalState.crash.manoeuvre = 0
  internalState.crash.dir = nil
  internalState.crash.pos = nil

  if electrics.values.horn == 1 then
    electrics.horn(false)
  end
  if electrics.values.signal_left_input or electrics.values.signal_right_input then
    electrics.set_warn_signal(false)
  end

  trafficPathState = {}
end
resetInternalStates()

local function resetParameters()
  -- parameters are used for finer AI control
  parameters = {
    turnForceCoef = 2, -- coefficient for curve spring forces
    awarenessForceCoef = 0.25, -- coefficient for vehicle awareness displacement
    edgeDist = 0, -- minimum distance from the edge of the road
    trafficWaitTime = 2, -- traffic delay after stopping at intersection
    delayFactorWaitTime = {0.55 + (0.95 - 0.55) * math.random(), 0.05 + (0.45 - 0.05) * math.random()},
    enableElectrics = true, -- use electrics such as hazard lights (especially for traffic)
    driveStyle = 'default',
    staticFrictionCoefMult = 0.95,
    lookAheadKv = 0.6,
    applyWidthMarginOffset = true,
    planErrorSmoothing = true,
    springForceIntegratorDispLim = 0.1, -- node displacement force magnitude limit
    understeerThrottleControl = 'on', -- anything other than an 'off' value will keep this active
    oversteerThrottleControl = 'on', -- anything other than an 'off' value will keep this active
    throttleTcs = 'on', -- anything other than an 'off' value will keep this active
    abBrakeControl = 'on',
    underSteerBrakeControl = 'on',
    throttleKp = 0.5,
    targetSpeedSmootherRate = 10
  }
end
resetParameters()

local function setParameters(data)
  tableMerge(parameters, data)
end

local function dumpParameters()
  dump(parameters)
  dump('aggression = ', aggression)
end
M.dumpParameters = dumpParameters

local function setTargetObjectID(id)
  M.targetObjectID = M.targetObjectID ~= objectId and id or -1
  if M.targetObjectID ~= -1 then
    targetObjectSelectionMode = 'manual'
  end
end

local function calculateWheelBase()
  local avgWheelNodePos, numOfWheels = vec3(), 0
  for _, wheel in pairs(wheels.wheels) do
    -- obj:getNodePosition is the pos vector of query node (wheel.node1) relative to ref node in world coordinates
    avgWheelNodePos:setAdd(obj:getNodePosition(wheel.node1))
    numOfWheels = numOfWheels + 1
  end
  if numOfWheels == 0 then
    return 0
  end

  avgWheelNodePos:setScaled(1 / numOfWheels)

  local dirVec = obj:getDirectionVector()
  local avgFrontWheelPos, frontWheelCount = vec3(), 0
  local avgBackWheelPos, backWheelCount = vec3(), 0
  for _, wheel in pairs(wheels.wheels) do
    local wheelPos = obj:getNodePosition(wheel.node1)
    if wheelPos:dot(dirVec) > avgWheelNodePos:dot(dirVec) then
      avgFrontWheelPos:setAdd(wheelPos)
      frontWheelCount = frontWheelCount + 1
    else
      avgBackWheelPos:setAdd(wheelPos)
      backWheelCount = backWheelCount + 1
    end
  end

  avgFrontWheelPos:setScaled(1 / frontWheelCount)
  avgBackWheelPos:setScaled(1 / backWheelCount)

  return avgFrontWheelPos:distance(avgBackWheelPos)
end

local function updatePlayerData()
  mapmgr.getObjects()
  if not next(mapmgr.objects) then
    return
  end -- prevents changes if table is empty

  if mapmgr.objects[M.targetObjectID] and targetObjectSelectionMode == 'manual' then
    player = mapmgr.objects[M.targetObjectID]
  elseif tableSize(mapmgr.objects) == 2 then
    if player ~= nil then
      player = mapmgr.objects[player.id]
    else
      for k, v in pairs(mapmgr.objects) do
        if k ~= objectId then
          M.targetObjectID = k
          player = v
          break
        end
      end
      targetObjectSelectionMode = 'auto'
    end
  else
    if player ~= nil and player.active == true then
      player = mapmgr.objects[player.id]
    else
      for k, v in pairs(mapmgr.objects) do
        if k ~= objectId and v.active == true then
          M.targetObjectID = k
          player = v
          break
        end
      end
      targetObjectSelectionMode = 'targetActive'
    end
  end
end

local function driveCar(steering, throttle, brake, parkingbrake)
  input.event("steering", steering, "FILTER_AI", nil, nil, nil, "ai")
  input.event("throttle", throttle, "FILTER_AI", nil, nil, nil, "ai")
  input.event("brake", brake, "FILTER_AI", nil, nil, nil, "ai")
  input.event("parkingbrake", parkingbrake, "FILTER_AI", nil, nil, nil, "ai")

  lastCommand.steering = steering
  lastCommand.throttle = throttle
  lastCommand.brake = brake
  lastCommand.parkingbrake = parkingbrake
end

local function populateOBBinRange(range)
  range = range * range
  local i = 0
  for id in pairs(mapmgr.getObjects()) do
    if id ~= objectId then
      local pos = obj:getObjectCenterPosition(id)
      if pos:squaredDistance(ego.pos) < range then
        twt.OBBinRange[i + 1] = pos

        -- Get bounding box direction vectors
        twt.OBBinRange[i + 2] = obj:getObjectDirectionVector(id) -- x
        twt.OBBinRange[i + 4] = obj:getObjectDirectionVectorUp(id) -- z
        twt.OBBinRange[i + 3] = twt.OBBinRange[i + 3] or vec3()
        twt.OBBinRange[i + 3]:setCross(twt.OBBinRange[i + 4], twt.OBBinRange[i + 2]);
        twt.OBBinRange[i + 3]:normalize() -- y (left)

        -- Scale bounding box direction vectors to vehicle diamensions
        twt.OBBinRange[i + 3]:setScaled(obj:getObjectInitialWidth(id) * 0.5)
        twt.OBBinRange[i + 2]:setScaled(obj:getObjectInitialLength(id) * 0.5)
        twt.OBBinRange[i + 4]:setScaled(obj:getObjectInitialHeight(id) * 0.5)
        i = i + 4
      end
    end
  end
  twt.OBBinRange.n = i
end

local function castRay(rpos, rdir, rayDist)
  for i = 1, twt.OBBinRange.n, 4 do
    local minHit, maxHit = intersectsRay_OBB(rpos, rdir, twt.OBBinRange[i], twt.OBBinRange[i + 1],
      twt.OBBinRange[i + 2], twt.OBBinRange[i + 3])
    if maxHit > 0 then
      rayDist = min(rayDist, max(minHit, 0))
    end
  end
  return obj:castRayStatic(rpos, rdir, rayDist)
end

local function driveToTarget(targetPos, throttle, brake, targetSpeed)
  if not targetPos then
    return
  end

  local plan = currentRoute and currentRoute.plan
  targetSpeed = targetSpeed or plan and plan.targetSpeed
  if not targetSpeed then
    return
  end

  local targetVec = targetPos - ego.pos;
  targetVec:normalize()
  local dirAngle = asin(ego.rightVec:dot(targetVec))

  -- oversteer
  local throttleOverCoef = 1
  if ego.speed > 1 then
    local rightVel = ego.rightVec:dot(ego.vel)
    if rightVel * ego.rightVec:dot(targetPos - ego.pos) > 0 then
      local rotVel = min(1, (ego.prevDirVec:projectToOriginPlane(ego.upVec):normalized()):distance(ego.dirVec) * dt *
        10000)
      throttleOverCoef = max(0, 1 - abs(rightVel * ego.speed * 0.05) * min(1, dirAngle * dirAngle * ego.speed * 6) *
        rotVel)
    end
  end

  local dirVel = ego.vel:dot(ego.dirVec)
  local absegoSpeed = abs(dirVel)
  local throttleUnderCoef = 1
  local brakeUnderCoef = 1

  if plan and plan[3] and dirVel > 3 then
    local p1, p2 = plan[1].pos, plan[2].pos
    local p2p1DirVec = p2 - p1;
    p2p1DirVec:normalize()

    local tp2 = (plan.targetSeg or 0) > 1 and targetPos or plan[3].pos
    local targetSide = (tp2 - p2):dot(p2p1DirVec:cross(ego.upVec))

    local outDeviation = egoDeviationSmoother:value() - plan.egoDeviation * sign(targetSide)
    outDeviation = sign(outDeviation) * min(1, abs(outDeviation))
    egoDeviationSmoother:set(outDeviation)
    egoDeviationSmoother:getUncapped(0, dt)

    if outDeviation > 0 and absegoSpeed > 3 then
      local steerCoef = outDeviation * absegoSpeed * absegoSpeed * min(1, dirAngle * dirAngle * 4)
      local understeerCoef = max(0, steerCoef) * min(1, abs(ego.vel:dot(p2p1DirVec) * 3))
      local noUndersteerCoef = max(0, 1 - understeerCoef)
      throttleUnderCoef = noUndersteerCoef
      brakeUnderCoef = min(brakeUnderCoef, max(0, 1 - understeerCoef * understeerCoef))
    end
  else
    egoDeviationSmoother:set(0)
  end

  -- wheel speed
  local throttleTcsCoef = 1
  local brakeABSCoef = 1
  if absegoSpeed > 0.05 then
    if sensors.gz <= 0.1 then
      local totalSlip = 0
      local propSlip = 0
      local totalDownForce = 0
      local lwheels = wheels.wheels
      for i = 0, tableSizeC(lwheels) - 1 do
        local wd = lwheels[i]
        if not wd.isBroken then
          local lastSlip = wd.lastSlip
          local downForce = wd.downForceRaw
          totalSlip = totalSlip + lastSlip * downForce
          totalDownForce = totalDownForce + downForce
          if wd.isPropulsed then
            propSlip = max(propSlip, lastSlip)
          end
        end
      end

      absegoSpeed = max(absegoSpeed, 3)

      totalSlip = totalSlip / (totalDownForce + 1e-25)

      -- abs
      brakeABSCoef = min(1, 1.5 * square(square(square(max(0, absegoSpeed - totalSlip) / absegoSpeed))))

      -- tcs
      propSlip = propSlip * (parameters.driveStyle == 'offRoad' and 0.8 or 1)
      local tcsCoef = max(0, absegoSpeed - propSlip * propSlip) / absegoSpeed
      throttleTcsCoef = min(tcsCoef, smoothTcs:get(tcsCoef, dt))
    else
      brakeABSCoef = 0
      throttleTcsCoef = 0
    end
  end

  local brakeCoef = 1
  if parameters.underSteerBrakeControl ~= 'off' then
    brakeCoef = min(brakeCoef, brakeUnderCoef)
  end
  if parameters.abBrakeControl ~= 'off' then
    brakeCoef = min(brakeCoef, brakeABSCoef)
  end

  local throttleCoef = 1
  if parameters.oversteerThrottleControl ~= 'off' then
    throttleCoef = min(throttleCoef, throttleOverCoef)
  end
  if parameters.understeerThrottleControl ~= 'off' then
    throttleCoef = min(throttleCoef, throttleUnderCoef)
  end
  if parameters.throttleTcs ~= 'off' then
    throttleCoef = min(throttleCoef, throttleTcsCoef)
  end

  local dirTarget = ego.dirVec:dot(targetVec)
  local dirTargetAxis = ego.rightVec:dot(targetVec)

  if internalState.crash.manoeuvre == 1 and dirTarget < ego.dirVec:dot(internalState.crash.dir) then
    driveCar(-sign(dirAngle), brake * brakeCoef, throttle * throttleCoef, 0)
    return
  else
    internalState.crash.manoeuvre = 0
  end

  if parameters.driveStyle == 'offRoad' then
    brakeCoef = 1
    throttleCoef = sqrt(throttleCoef)
  end

  internalState.egoForceGoFrontTime = max(0, internalState.egoForceGoFrontTime - dt)
  if twt.state == 1 and internalState.egoCannotMoveTime > 1 and internalState.egoForceGoFrontTime == 0 then
    twt.state = 0
    internalState.egoCannotMoveTime = 0
    internalState.egoForceGoFrontTime = 2
  end

  if internalState.egoForceGoFrontTime > 0 and dirTarget < 0 then
    dirTarget = -dirTarget
    dirAngle = -dirAngle
  end

  if currentRoute and (dirTarget < 0 or (twt.state == 1 and dirTarget < 0.866)) then -- TODO: Improve entry condition when twt.state == 1
    local helperVec = 0.35 * ego.upVec -- auxiliary vector
    twt.posTable[2]:setAdd2(ego.pos, helperVec)
    helperVec:setScaled2(ego.dirVec, -0.05 * ego.length)
    twt.posTable[2]:setAdd(helperVec)
    helperVec:setScaled2(ego.rightVec, 0.5 * (0.7 * ego.width)) -- lateral translation
    twt.posTable[2]:setAdd(helperVec) -- front right corner pos
    helperVec:setScaled(-2)
    twt.posTable[1]:setAdd2(twt.posTable[2], helperVec) -- front left corner pos
    helperVec:setScaled2(ego.dirVec, -0.9 * ego.length) -- longitudinal translation
    twt.posTable[3]:setAdd2(twt.posTable[2], helperVec) -- back right corner pos
    twt.posTable[4]:setAdd2(twt.posTable[1], helperVec) -- back left corner pos

    twt.dirTable[1]:setScaled2(ego.rightVec, -1)
    twt.dirTable[2]:set(ego.dirVec)
    twt.dirTable[3]:set(ego.rightVec)
    twt.dirTable[4]:setScaled2(ego.dirVec, -1)

    local sizeRatio = ego.length / ego.width
    local blueNoiseRange = 6 + 2 * sizeRatio
    local rayDist = 4 * ego.wheelBase -- TODO: Optimize rayDist. Higher is better for more open spaces but w performance hit
    populateOBBinRange(rayDist)
    local tmpVec = vec3()
    local RRPicks = max(1, min(floor(150 * dt + 0.5), 8)) -- x * dt where x/fps is the numer of samples
    for k = 1, RRPicks do -- the ray cast loop: each iteration scans one area and casts on the minimum
      local i = 0
      local blueIndex = twt.blueNoiseRR * blueNoiseRange
      if blueIndex < 3 then
        i = floor(1 + blueIndex)
      elseif blueIndex < 6 then
        i = floor(1 + blueIndex) + 1
      elseif blueIndex < 6 + sizeRatio then
        i = 4
      else
        i = 8
      end

      local j = i * 4
      tmpVec:setLerp(twt.posTable[twt.RRT[j - 3]], twt.posTable[twt.RRT[j - 2]], twt.biasedCoefs[i])
      helperVec:setLerp(twt.dirTable[twt.RRT[j - 1]], twt.dirTable[twt.RRT[j]], twt.biasedCoefs[i]) -- LERP corner direction
      helperVec:normalize()
      local rayLen = castRay(tmpVec, helperVec, min(twt.rayMins[i], rayDist))
      if twt.rayMins[i] > rayLen or rayLen == rayDist then
        twt.minRayCoefs[i] = twt.biasedCoefs[i]
        twt.rayMins[i] = rayLen
      else
        tmpVec:setLerp(twt.posTable[twt.RRT[j - 3]], twt.posTable[twt.RRT[j - 2]], twt.minRayCoefs[i])
        helperVec:setLerp(twt.dirTable[twt.RRT[j - 1]], twt.dirTable[twt.RRT[j]], twt.minRayCoefs[i]) -- LERP corner direction
        helperVec:normalize()
        twt.rayMins[i] = castRay(tmpVec, helperVec, min(max(2 * twt.rayMins[i], 0.02), rayDist))
      end
      twt.blueNoiseRR = getBlueNoise1d(twt.blueNoiseRR)
      twt.blueNoiseCoefs[i] = getBlueNoise1d(twt.blueNoiseCoefs[i])
      local criticality = 2 * twt.rayMins[i] / (twt.rayMins[i] + rayDist) + 0.25
      twt.biasedCoefs[i] = biasGainFun(twt.blueNoiseCoefs[i], twt.minRayCoefs[i], criticality)
    end
    local minRayDist = min(twt.rayMins[1], twt.rayMins[2], twt.rayMins[3], twt.rayMins[4], twt.rayMins[5],
      twt.rayMins[6], twt.rayMins[7], twt.rayMins[8])

    twt.sampleCounter = twt.sampleCounter + 1
    if twt.state == 0 then
      if twt.sampleCounter * dt > max(0.5, min(50 * dt, 2.5)) and ego.speed <= 0.5 then -- TODO: Optimize sample count condition
        twt.state = 1
      else
        local speed = max(0, ego.speed - 0.25)
        local throttle = max(0, -sign(speed * dirVel)) * 0.3
        local brake = max(0, sign(speed * dirVel)) * 0.3
        driveCar(0, throttle, brake, 0)
      end
    end

    if twt.state == 1 then -- start driving
      if twt.targetSpeed < 0.66 or twt.minRay == "S" then -- TODO: Some branches may be consolidated
        if minRayDist == twt.rayMins[7] then
          twt.dirState[1] = 1
          twt.dirState[2] = sign2(dirTargetAxis)
          twt.minRay = "BL"
        elseif minRayDist == twt.rayMins[5] then
          twt.dirState[1] = 1
          twt.dirState[2] = sign2(dirTargetAxis)
          twt.minRay = "BR"
        elseif minRayDist == twt.rayMins[1] then
          twt.dirState[1] = -1
          twt.dirState[2] = -sign2(dirTargetAxis)
          twt.minRay = "FL"
          if twt.dirState[2] == 1 and twt.rayMins[8] < 0.4 and twt.targetSpeed < 0.6 then
            twt.dirState[2] = 0
          end
        elseif minRayDist == twt.rayMins[3] then
          twt.dirState[1] = -1
          twt.dirState[2] = -sign2(dirTargetAxis)
          if twt.dirState[2] == -1 and twt.rayMins[4] < 0.4 and twt.targetSpeed < 0.6 then
            twt.dirState[2] = 0
          end
          twt.minRay = "FR"
        elseif minRayDist == twt.rayMins[6] then
          twt.dirState[1] = 1
          twt.dirState[2] = sign2(dirTargetAxis)
          twt.minRay = "B"
        elseif minRayDist == min(twt.rayMins[4], twt.rayMins[8]) then
          local sideRuler = 0
          local minSide = minRayDist == twt.rayMins[8] and "L" or "R"
          if minSide == "L" then
            sideRuler = ego.length * (1 - twt.minRayCoefs[8])
          else
            sideRuler = ego.length * twt.minRayCoefs[4]
          end
          twt.dirState[2] = 0
          if sideRuler < twt.rayMins[6] and dirTarget < 0 then -- TODO : test this
            twt.dirState[1] = -1
          elseif (ego.length - sideRuler) < twt.rayMins[2] then
            twt.dirState[1] = 1
            if minSide == "L" then
              twt.dirState[2] = 1
            else
              twt.dirState[2] = -1
            end
          end
          twt.minRay = "S"
        else
          twt.dirState[1] = -1
          twt.dirState[2] = -sign2(dirTargetAxis)
          twt.minRay = "F"
        end
      end

      local threshold = min(ego.speed - 0.1, 0.66)
      local dirCoef, minDist = 1, nil -- TODO: what is car is trapped/sandwiched?
      if twt.dirState[1] == -1 then -- reverse
        dirCoef = min(1.2 - dirTarget, 1) -- dirTarget -> targetSpeed modulation
        if twt.dirState[2] == -1 then -- steer left
          minDist = min(twt.rayMins[4], twt.rayMins[6], twt.rayMins[7])
        elseif twt.dirState[2] == 1 then -- steer right
          minDist = min(twt.rayMins[5], twt.rayMins[6], twt.rayMins[8])
        else -- no steering
          minDist = twt.rayMins[6]
          if twt.minRay ~= "S" then
            threshold = twt.rayMins[2] -- possibly max(threshold, twt.rayMins[2])
          end
        end
      else -- forward
        if twt.dirState[2] == -1 then
          minDist = min(twt.rayMins[1], twt.rayMins[2], twt.rayMins[8])
        elseif twt.dirState[2] == 1 then
          minDist = min(twt.rayMins[2], twt.rayMins[3], twt.rayMins[4])
        else
          minDist = twt.rayMins[2]
        end
      end

      local targetSpeed = sqrt(2 * g * min(aggression, ego.staticFrictionCoef) * max(0, minDist - threshold) * dirCoef)
      twt.targetSpeed = max(min(twt.speedSmoother:get(targetSpeed, dt), min(6, aggression * 6)), 0.3)
      local speedDif = twt.targetSpeed - twt.dirState[1] * sign2(dirVel) * ego.speed
      local steering = twt.steerSmoother:get(twt.dirState[2], dt)
      local pbrake = 0 -- * clamp(sign2(0.83 + ego.upVec:dot(gravityDir)), 0, 1) -- >= 10 deg
      local throttle, brake = 0, 0
      if twt.dirState[1] == -1 then
        throttle = clamp(-speedDif * 0.4, 0, 0.4)
        brake = clamp(speedDif * 0.2, 0, 1)
      elseif twt.dirState[1] == 1 then
        throttle = clamp(speedDif * 0.2, 0, 1)
        brake = clamp(-speedDif * 0.4, 0, 0.4)
      end
      driveCar(steering, throttle, brake, pbrake)
    end
  else
    twt.reset()

    local pbrake
    if ego.vel:dot(ego.dirVec) < 0 and ego.speed > 0.1 then
      -- TODO: handbrake when stoped or moving backward
      -- apply brake (in arcade mode throttle input when moving backwards activates the brakes) and or parkingbrake
      -- when vehicle is moving backwards but the target speed is effectively zero
      if ego.speed < 0.15 and targetSpeed <= 1e-5 then
        pbrake = 1
      else
        pbrake = 0
      end
      throttle = 0.5 * throttleCoef
      brake = 0
    else
      if (ego.speed > 4 and ego.speed < 30 and abs(dirAngle) > 0.97 and brake == 0) or
        (ego.speed < 0.15 and targetSpeed <= 1e-5) then
        -- The first condition was supposed to help the vehicles turn in tight corners by applying handbrake. This is very old logic. It might
        -- no longer be needed. Revise.
        -- The second condition is supposed to apply handbrake when the vehicle is at a stop but it looks to be unstable
        -- TODO: handbrake when turning
        pbrake = 1
      else
        pbrake = 0
      end
      throttle = throttle * throttleCoef
      brake = brake * brakeCoef
    end

    local aggSq = square(aggression + max(0, -(ego.dirVec:dot(gravityDir))))
    local rate = max(throttleSmoother[throttleSmoother:value() < throttle], 10 * aggSq * aggSq)
    throttle = throttleSmoother:getWithRateUncapped(throttle, dt, rate)

    driveCar(dirAngle, throttle, brake, pbrake)
  end
end

local function posOnPlan(pos, plan, dist)
  if not plan then
    return
  end
  dist = dist or 4
  dist = dist * dist
  local bestSeg, bestXnorm
  for i = 1, #plan - 2 do
    local p0, p1 = plan[i].pos, plan[i + 1].pos
    local xnorm1 = pos:xnormOnLine(p0, p1)
    if xnorm1 > 0 then
      local p2 = plan[i + 2].pos
      local xnorm2 = pos:xnormOnLine(p1, p2)
      if xnorm1 < 1 then -- contained in segment i
        if xnorm2 > 0 then -- also partly contained in segment i+1
          local sqDistFromP1 = pos:squaredDistance(p1)
          if sqDistFromP1 <= dist then
            bestSeg = i
            bestXnorm = 1
            break -- break inside conditional
          end
        else
          local sqDistFromLine = pos:squaredDistance(p0 + (p1 - p0) * xnorm1)
          if sqDistFromLine <= dist then
            bestSeg = i
            bestXnorm = xnorm1
          end
          break -- break should be outside above conditional
        end
      elseif xnorm2 < 0 then
        local sqDistFromP1 = pos:squaredDistance(p1)
        if sqDistFromP1 <= dist then
          bestSeg = i
          bestXnorm = 1
        end
        break -- break outside conditional
      end
    else
      break
    end
  end

  return bestSeg, bestXnorm
end

local function egoPosOnPlan(plan)
  local planCount = plan.planCount
  local egoSeg = 1
  local egoXnormOnSeg = 0
  for i = 1, planCount - 1 do
    local p0Pos, p1Pos = plan[i].pos, plan[i + 1].pos
    local xnorm = ego.pos:xnormOnLine(p0Pos, p1Pos)
    if xnorm < 1 then
      if i < planCount - 2 then
        local nextXnorm = ego.pos:xnormOnLine(p1Pos, plan[i + 2].pos)
        if nextXnorm >= 0 then
          local p1Radius = plan[i + 1].radiusOrig
          if ego.pos:squaredDistance(linePointFromXnorm(p1Pos, plan[i + 2].pos, nextXnorm)) <
            square(ego.width + lerp(p1Radius, plan[i + 2].radiusOrig, min(1, nextXnorm))) then
            egoXnormOnSeg = nextXnorm
            egoSeg = i + 1
            break
          end
        end
      end
      egoXnormOnSeg = xnorm
      egoSeg = i
      break
    end
  end

  local disp = 0
  if egoSeg > 1 then
    local sumLen = 0
    disp = egoSeg - 1
    for i = 1, disp do
      sumLen = sumLen + plan[i].length
    end

    for i = 1, plan.planCount do
      plan[i] = plan[i + disp]
      if disp > 0 and i + disp > plan.planCount then
        velocities[i] = vec3(0, 0, 0)
      else
        velocities[i] = velocities[i + disp]
      end
    end

    plan.planCount = plan.planCount - disp
    plan.planLen = max(0, plan.planLen - sumLen)
    plan.stopSeg = max(1, plan.stopSeg - disp)
  end

  plan.egoXnormOnSeg = egoXnormOnSeg
  plan.egoSeg = egoSeg - disp
end

-- returns the node index
local function getLastNodeWithinDistance(plan, dist)
  dist = dist - plan[1].length * (1 - plan.egoXnormOnSeg)
  if dist < 0 then
    return 1
  end

  local planSeg = plan.planCount
  for i = 2, plan.planCount - 1 do
    dist = dist - plan[i].length
    if dist < 0 then
      planSeg = i
      break
    end
  end

  return planSeg
end

local function calculateTarget(plan)
  egoPosOnPlan(plan)
  local targetLength = max(ego.speed * parameters.lookAheadKv, 4.5)

  if plan.planCount >= 3 then
    local xnorm = clamp(plan.egoXnormOnSeg, 0, 1)
    targetLength = max(targetLength, plan[1].length * (1 - xnorm), plan[2].length * xnorm)
  end

  local remainder = targetLength

  local targetPos = vec3(plan[plan.planCount].pos)
  local targetSeg = max(1, plan.planCount - 1)
  local prevPos = linePointFromXnorm(plan[1].pos, plan[2].pos, plan.egoXnormOnSeg) -- ego.pos

  local segVec, segLen = vec3(), nil
  for i = 2, plan.planCount do
    local pos = plan[i].pos

    segVec:setSub2(pos, prevPos)
    segLen = segLen or segVec:length()

    if remainder <= segLen then
      targetSeg = i - 1
      targetPos:setScaled2(segVec, remainder / (segLen + 1e-30));
      targetPos:setAdd(prevPos)

      -- smooth target
      local xnorm = clamp(targetPos:xnormOnLine(prevPos, pos), 0, 1)
      local lp_n1n2 = linePointFromXnorm(prevPos, pos, xnorm * 0.5 + 0.25)
      if xnorm <= 0.5 then
        if i >= 3 then
          targetPos = linePointFromXnorm(linePointFromXnorm(plan[i - 2].pos, prevPos, xnorm * 0.5 + 0.75), lp_n1n2,
            xnorm + 0.5)
        end
      else
        if i <= plan.planCount - 2 then
          targetPos = linePointFromXnorm(lp_n1n2, linePointFromXnorm(pos, plan[i + 1].pos, xnorm * 0.5 - 0.25),
            xnorm - 0.5)
        end
      end
      break
    end

    prevPos = pos
    remainder = remainder - segLen
    segLen = plan[i].length
  end

  plan.targetPos = targetPos
  plan.targetSeg = targetSeg
end

local function targetsCompatible(baseRoute, newRoute)
  local baseTvec = baseRoute.plan.targetPos - ego.pos
  local newTvec = newRoute.plan.targetPos - ego.pos
  if ego.speed < 2 then
    return true
  end
  if newTvec:dot(ego.dirVec) * baseTvec:dot(ego.dirVec) <= 0 then
    return false
  end
  local baseTargetRight = baseTvec:cross(ego.upVec);
  baseTargetRight:normalize()
  return abs(newTvec:normalized():dot(baseTargetRight)) * ego.speed < 2
end

local function getMinPlanLen(limLow, speed, accelg)
  -- given current speed, distance required to come to a stop if I can decelerate at 0.2g
  limLow = limLow or 150
  speed = speed or ego.speed
  accelg = max(0.2, accelg or 0.2)
  return min(550, max(limLow, 0.5 * speed * speed / (accelg * g)))
end

local function pickAiWp(wp1, wp2, dirVec)
  dirVec = dirVec or ego.dirVec
  local vec1 = mapData.positions[wp1] - ego.pos
  local vec2 = mapData.positions[wp2] - ego.pos
  local dot1 = vec1:dot(dirVec)
  local dot2 = vec2:dot(dirVec)
  if (dot1 * dot2) <= 0 then
    if dot1 < 0 then
      return wp2, wp1
    end
  else
    if vec2:squaredLength() < vec1:squaredLength() then
      return wp2, wp1
    end
  end
  return wp1, wp2
end

local function pathExtend(path, newPath)
  if newPath == nil then
    return
  end
  local pathCount = #path
  if path[pathCount] ~= newPath[1] then
    return
  end
  pathCount = pathCount - 1
  for i = 2, #newPath do
    path[pathCount + i] = newPath[i]
  end
end

-- http://cnx.org/contents/--TzKjCB@8/Projectile-motion-on-an-inclin
local function projectileSqSpeedToRangeRatio(pos1, pos2, pos3)
  local sinTheta = (pos2.z - pos1.z) / pos1:distance(pos2)
  local sinAlpha = (pos3.z - pos2.z) / pos2:distance(pos3)
  local cosAlphaSquared = max(1 - sinAlpha * sinAlpha, 0)
  local cosTheta = sqrt(max(1 - sinTheta * sinTheta, 0)) -- in the interval theta = {-pi/2, pi/2} cosTheta is always positive
  return 0.5 * g * cosAlphaSquared / max(cosTheta * (sinTheta * sqrt(cosAlphaSquared) - cosTheta * sinAlpha), 0)
end

local function getPathLen(path, startIdx, stopIdx)
  if not path then
    return
  end
  startIdx = startIdx or 1
  stopIdx = stopIdx or #path
  local positions = mapData.positions
  local pathLen = 0
  for i = startIdx + 1, stopIdx do
    pathLen = pathLen + positions[path[i - 1]]:distance(positions[path[i]])
  end

  return pathLen
end

local function getPathBBox(path, startIdx, stopIdx)
  if not path then
    return
  end
  startIdx = startIdx or 1
  stopIdx = stopIdx or #path

  local positions = mapData.positions
  local p = positions[path[startIdx]]
  local v = positions[path[stopIdx]] + p;
  v:setScaled(0.5)
  local r = getPathLen(path, startIdx, stopIdx) * 0.5

  return {v.x - r, v.y - r, v.x + r, v.y + r}, v, r
end

local function waypointInPath(path, waypoint, startIdx, stopIdx)
  if not path or not waypoint then
    return
  end
  startIdx = startIdx or 1
  stopIdx = stopIdx or #path
  for i = startIdx, stopIdx do
    if path[i] == waypoint then
      return i
    end
  end
end

local function getPlanLen(plan, from, to)
  from = max(1, from or 1)
  to = min(plan.planCount - 1, to or math.huge)
  local planLen = 0
  for i = from, to do
    planLen = planLen + plan[i].length
  end

  return planLen
end

local function abortUpcommingLaneChange()
  if not currentRoute then
    return
  end
  if currentRoute.laneChanges[1] then
    currentRoute.laneChanges[1].side = 0
  end
  local lastPlanPidx = currentRoute.plan[currentRoute.plan.planCount].pathidx
  for _, lc in ipairs(currentRoute.laneChanges) do
    if lc.pathIdx <= lastPlanPidx then
      lc.side = 0
    end
  end
end

local function updatePlanLen(plan, j, k)
  -- profilerPushEvent("ai_update_planLen")
  -- bulk recalculation of plan edge lengths and length of entire plan
  -- j: index of earliest node position that has changed
  -- k: index of latest node position that has changed
  j = max((j or 1) - 1, 1)
  k = min(k or plan.planCount, plan.planCount - 1)

  local planLen = plan.planLen
  for i = j, k do
    local edgeLen = plan[i + 1].pos:distance(plan[i].pos)
    planLen = planLen - plan[i].length + edgeLen
    plan[i].length = edgeLen
  end
  plan.planLen = planLen
  -- profilerPopEvent("ai_update_planLen")
end

-- expand lane lateral limits of this plan node to the lane range limits
local function openLaneToLaneRange(planNode)
  local roadHalfWidth = planNode.halfWidth
  planNode.laneLimLeft = linearScale(planNode.rangeLeft, 0, 1, -roadHalfWidth, roadHalfWidth)
  planNode.laneLimRight = linearScale(planNode.rangeRight, 0, 1, -roadHalfWidth, roadHalfWidth)
  planNode.lanesOpen = true
end

-- expand lane lateral limits to the lane range for all nodes up to and including idx
local function openPlanLanesToLaneRange(plan, idx)
  plan = plan or (currentRoute and currentRoute.plan)
  if not plan then
    return
  end
  for i = 1, min(plan.planCount, idx) do
    openLaneToLaneRange(plan[i])
  end
end

local function laneChange(plan, dist, signedDisp)
  if not plan and currentRoute then
    plan = currentRoute.plan
  end
  if not plan then
    return
  end

  -- Apply node displacement
  local invDist = 1 / (dist + 1e-30)
  local curDist, normalDispVec = 0, vec3()
  for i = 2, plan.planCount do
    openLaneToLaneRange(plan[i])
    curDist = curDist + plan[i - 1].length
    plan[i].lateralXnorm = clamp(plan[i].lateralXnorm + signedDisp * min(curDist * invDist, 1), plan[i].laneLimLeft,
      plan[i].laneLimRight)
    normalDispVec:setScaled2(plan[i].normal, plan[i].lateralXnorm)
    plan[i].pos:setAdd2(plan[i].posOrig, normalDispVec)

    -- Recalculate vec and dirVec
    plan[i].vec:setSub2(plan[i - 1].pos, plan[i].pos);
    plan[i].vec.z = 0
    plan[i].dirVec:set(plan[i].vec)
    plan[i].dirVec:normalize()
  end

  updatePlanLen(plan)

  --[[ For debugging
  table.clear(newPositionsDebug)
  for i = 1, #newPositions do
    newPositionsDebug[i] = vec3(newPositions[i])
  end
  --]]
end

local function setStopPoint(plan, dist, args)
  if not plan and currentRoute then
    plan = currentRoute.plan
  end
  if not plan then
    return
  end

  if not dist then -- reset stop segment
    plan.stopSeg = math.huge
    return
  end

  if (args or E).avoidJunction and currentRoute.path then -- prevents stopping directly in junctions
    -- this code is temporary, and inefficient...
    local seg
    while true do
      seg = getLastNodeWithinDistance(plan, dist)
      local nid = currentRoute.path[plan[seg].pathidx]
      if seg < plan.planCount - 1 and tableSize(mapData.graph[nid]) > 2 and
        plan[seg].pos:squaredDistance(mapData.positions[nid]) <= square(mapData.radius[nid]) then
        dist = dist + 20
      else
        break
      end
    end
    plan.stopSeg = seg
  else
    plan.stopSeg = getLastNodeWithinDistance(plan, dist)
  end
end

local function numOfLanesFromRadius(rad1, rad2)
  return max(1, math.floor(min(rad1, rad2 or math.huge) * 2 / 3.61 + 0.5)) -- math.floor(min(rad1, rad2) / 2.7) + 1
end

local laneStringBuffer = buffer.new()
local function flipLanes(lanes)
  -- ex. '--+++' becomes '---++'
  for i = #lanes, 1, -1 do
    laneStringBuffer:put(lanes:byte(i) == 43 and '-' or lanes:byte(i) == 45 and '+' or '0')
  end
  return laneStringBuffer:get()
end

-- returns the number of lanes in a given direction
local function numOfLanesInDirection(lanes, dir)
  -- lanes: a lane string
  -- dir: '+' or '-'
  dir = dir == '+' and 43 or dir == '-' and 45
  local lanesN = 0
  for i = 1, #lanes, 1 do
    if lanes:byte(i) == dir then
      lanesN = lanesN + 1
    end
  end
  return lanesN
end

-- Returns the lane configuration of an edge as traversed in the fromNode -> toNode direction
-- if an edge does not have lane data they are deduced from the node radii
local function getEdgeLaneConfig(fromNode, toNode)
  local lanes
  local edge = mapData.graph[fromNode][toNode]
  if edge.lanes then
    lanes = edge.lanes
  else -- make up some lane data in case they don't exist
    if edge.oneWay then
      local numOfLanes = numOfLanesFromRadius(mapData.radius[fromNode], mapData.radius[toNode])
      lanes = string.rep("+", numOfLanes)
    else
      local numOfLanes =
        max(1, math.floor(numOfLanesFromRadius(mapData.radius[fromNode], mapData.radius[toNode]) * 0.5))
      if mapmgr.rules.rightHandDrive then
        lanes = string.rep("+", numOfLanes) .. string.rep("-", numOfLanes)
      else
        lanes = string.rep("-", numOfLanes) .. string.rep("+", numOfLanes)
      end
    end
  end

  return edge.inNode == fromNode and lanes or flipLanes(lanes) -- flip lanes string based on inNode data
end

-- Calculate the edge incident on wp2 which is most similar to the edge wp1->wp2
local function roadNaturalContinuation(wp1, wp2)
  local inLaneConfig = getEdgeLaneConfig(wp1, wp2)
  local inRadiuswp2, inRadiuswp1 = mapData:getEdgeRadii(wp2, wp1)
  local inEdgeDir = vec3();
  inEdgeDir:setSub2(mapData:getEdgePositions(wp2, wp1));
  inEdgeDir:normalize()
  local laneFlow = mapData.graph[wp1][wp2].drivability * 4 * min(inRadiuswp2, inRadiuswp1) / #inLaneConfig
  local inLaneCount = numOfLanesInDirection(inLaneConfig, '+')
  local inFwdFlow = inLaneCount * laneFlow
  local inLaneCountOpposite = (#inLaneConfig - inLaneCount)
  local inBackFlow = inLaneCountOpposite * laneFlow
  local outEdgeDir, maxOutflow, minNode = vec3(), 0, nil
  for k, v in pairs(mapData.graph[wp2]) do
    if k ~= wp1 then
      local outLaneConfig = getEdgeLaneConfig(wp2, k)
      local numOfOutLanes = numOfLanesInDirection(outLaneConfig, '+')
      outEdgeDir:setSub2(mapData:getEdgePositions(k, wp2));
      outEdgeDir:normalize()
      local dirCoef = 0.5 * max(0, 1 + outEdgeDir:dot(inEdgeDir))
      local outLaneCountOpposite = (#outLaneConfig - numOfOutLanes)
      local outRadiuswp2, outRadiusk = mapData:getEdgeRadii(wp2, k)
      if numOfOutLanes == inLaneCount and outLaneCountOpposite == inLaneCountOpposite then
        laneFlow = mapData.graph[wp2][k].drivability * 4 * 0.5 * (inRadiuswp2 + outRadiuswp2) / #outLaneConfig
      else
        laneFlow = mapData.graph[wp2][k].drivability * 4 * min(outRadiuswp2, outRadiusk) / #outLaneConfig
      end
      local outFwdFlow = min(inFwdFlow, numOfOutLanes * laneFlow)
      local outBackFlow = min(inBackFlow, outLaneCountOpposite * laneFlow)
      local outflow = outFwdFlow * (1 + outBackFlow) * dirCoef

      if outflow > maxOutflow then
        maxOutflow = outflow
        minNode = k
      end
    end
  end

  return minNode
end

-- returns the lane indices of the left most and right most lanes in the direction of travel
local function laneRangeIdx(laneConfig)
  local numOfLanes = #laneConfig
  local leftIdx, rightIdx = 1, numOfLanes
  for i = 1, numOfLanes do
    if laneConfig:byte(i) == 43 then -- "+"
      leftIdx = i
      break
    end
  end

  for i = numOfLanes, 1, -1 do
    if laneConfig:byte(i) == 43 then -- "+"
      rightIdx = i
      break
    end
  end

  return leftIdx, rightIdx, numOfLanes
end

-- returns the lane lateral xnorm limits (boundaries in [0, 1] range 0 being the left boundary)
-- of the left most and right most lanes in the direction of travel and the number of lanes in that range.
-- This range might include lanes in the opposite direction if they are interleaved with lanes in the direction of travel
-- ex. '--++-++' the left most lane is lane 3, the right most lane is lane 7. The range is from lane 3 to lane 7 inclusive.
local function laneRange(laneConfig)
  local leftLim, rightLim, numOfLanes = laneRangeIdx(laneConfig)
  return (leftLim - 1) / numOfLanes, rightLim / numOfLanes, rightLim - leftLim + 1
end

-- Splits the lane string into three parts 1:leftIdx-1, leftIdx:rightIdx, rightIdx+1:numOfLanes and returns the three strings
-- leftIdx is the indice of the left most lane in the direction of travel
-- rightIdx is the indice of the right most lane in the direction of travel
-- therefore leftIdx:rightIdx is the lane string from the left most to the right most lane in the direction of travel
local function splitLaneRange(laneConfig)
  local leftIdx, rightIdx, numOfLanes = laneRangeIdx(laneConfig)
  -- leftIdx = leftIdx
  -- rightIdx = rightIdx

  return laneConfig:sub(1, leftIdx - 1), laneConfig:sub(leftIdx, rightIdx), laneConfig:sub(rightIdx + 1, numOfLanes)
end

-- Lateral xnorm limits ([0, 1]) of the left most and right most lanes from "lane" in the direction of "lane"
local function getLaneRangeFromLane(laneConfig, lane)
  -- laneConfig: a string representing lane configuration ex. "-a-a+a+a"
  -- lane: the lane number (1 to n) counting from left to right in the direction of travel
  local numOfLanes = #laneConfig
  local leftIdx, rightIdx = lane, lane
  -- search left
  for i = lane - 1, 1, -1 do
    if laneConfig:byte(i) == 43 then -- "+"
      leftIdx = i
    else
      break
    end
  end

  -- search right
  for i = lane + 1, numOfLanes do
    if laneConfig:byte(i) == 43 then -- "+"
      rightIdx = i
    else
      break
    end
  end

  return (leftIdx - 1) / numOfLanes, rightIdx / numOfLanes, leftIdx, rightIdx
end

-- Calculates the composite lane configuration coming into newNode
-- and adjusted lateral position, lane limits and lane range of the last plan node.
local function processNodeIncommingLanes(newNode, planNode)
  local leftRange, centerRange, rightRange = splitLaneRange(newNode.inEdgeLanes)
  local latXnorm = planNode.lateralXnorm
  local laneLimLeft = planNode.laneLimLeft
  local laneLimRight = planNode.laneLimRight
  local halfWidth = planNode.halfWidth
  local numOfOutLanes = numOfLanesInDirection(newNode.outEdgeLanes, "+")
  local numOfInLanes = numOfLanesInDirection(newNode.inEdgeLanes, "+")
  local defaultLaneWidth = min(2 * halfWidth / (#newNode.inEdgeLanes), 3.45)
  local inEdgeDrivabilityInv = 1 / ((newNode.inEdgedrivability or 1) + 1e-30)
  if not planNode.rangeLeft then
    planNode.rangeLeft, planNode.rangeRight = laneRange(newNode.inEdgeLanes)
  end
  local rangeLeft = linearScale(planNode.rangeLeft or 0, 0, 1, -halfWidth, halfWidth)
  local rangeRight = linearScale(planNode.rangeRight or 1, 0, 1, -halfWidth, halfWidth)

  for nodeId, edgeData in pairs(mapData.graph[newNode.name]) do
    if nodeId ~= newNode.nextNodeInPath and nodeId ~= newNode.prevNodeInPath then -- and not mapmgr.signalsData.nodes[newNode.name]
      local lanes = getEdgeLaneConfig(nodeId, newNode.name)
      local thisEdgeInLanes = numOfLanesInDirection(lanes, '+')
      if thisEdgeInLanes > 0 then -- TODO: Should also possibly include traffic light data
        local nodePos = mapData.positions[nodeId]
        local dirRatio = max(0,
          (mapData.positions[newNode.nextNodeInPath or newNode.name] - newNode.posOrig):normalized()
            :dot((newNode.posOrig - nodePos):normalized()))
        local drivabilityRatio = min(1, edgeData.drivability * inEdgeDrivabilityInv)
        thisEdgeInLanes = math.floor(thisEdgeInLanes * dirRatio * drivabilityRatio + 0.5)

        if thisEdgeInLanes > 0 then
          if newNode.inEdgeNormal:dot(nodePos) < newNode.inEdgeNormal:dot(newNode.posOrig) then -- coming into newNode from the left
            numOfInLanes = numOfInLanes + thisEdgeInLanes
            centerRange = string.rep("+", thisEdgeInLanes) .. centerRange
            local radius = thisEdgeInLanes * defaultLaneWidth * 0.5
            latXnorm = latXnorm + radius
            laneLimLeft = laneLimLeft + radius
            laneLimRight = laneLimRight + radius
            halfWidth = halfWidth + radius
            rangeLeft = rangeLeft - radius
            rangeRight = rangeRight + radius -- TODO: is this correct?
          elseif numOfInLanes < numOfOutLanes then
            local capInLanes = min(numOfInLanes + thisEdgeInLanes, numOfOutLanes)
            thisEdgeInLanes = capInLanes - numOfInLanes
            numOfInLanes = capInLanes
            local radius = thisEdgeInLanes * defaultLaneWidth * 0.5
            centerRange = centerRange .. string.rep("+", thisEdgeInLanes)
            latXnorm = latXnorm - radius
            laneLimLeft = laneLimLeft - radius
            laneLimRight = laneLimRight - radius
            halfWidth = halfWidth + radius
            rangeLeft = rangeLeft - radius
            rangeRight = rangeRight + radius
          end
        end
      end
    end
  end

  return leftRange .. centerRange .. rightRange, latXnorm, laneLimLeft, laneLimRight, rangeLeft, rangeRight, halfWidth
end

-- Calculate the most appropriate lane of laneConfig
local function getBestLane(laneConfig, nodeLatPos, laneLeftLimLatPos, laneRightLimLatPos, plan, newNode)
  -- nodeLatPos, laneLeftLimLatPos, laneRightLimLatPos are in the [0, 1] interval
  local laneWidth = laneRightLimLatPos - laneLeftLimLatPos
  local numOfLanes = max(1, #laneConfig)
  local bestError = math.huge
  local bestLane, newLaneLimLeft, newLaneLimRight
  local planPos, dirVec
  if plan then
    planPos = plan[plan.planCount].pos
    dirVec = plan[plan.planCount].pos - plan[max(1, plan.planCount - 1)].pos;
    dirVec:normalize()
  else
    planPos = vec3()
    dirVec = planPos
    newNode = {}
    newNode.posOrig = planPos
    newNode.radiusOrig = 0
    newNode.halfWidth = 0
    newNode.normal = planPos
    newNode.outEdgeLanes = laneConfig
  end
  if #newNode.outEdgeLanes == #laneConfig then -- TODO: this might not be correct
    dirVec:set(0, 0, 0)
  end

  -- Get driving side parameter of the map for more exterior lane selection
  local isRightHandDrive = mapmgr.rules.rightHandDrive
  local newNodeWidthVec = newNode.halfWidth * newNode.normal

  for i = 1, numOfLanes do -- traverse lanes in laneConfig
    local thisLaneLimLeft, thisLaneLimRight = (i - 1) / numOfLanes, i / numOfLanes
    local thisLaneLatPos = (thisLaneLimRight + thisLaneLimLeft) * 0.5
    local laneColinearity = ((thisLaneLatPos * 2 - 1) * push3(newNodeWidthVec) + newNode.posOrig - planPos):normalized()
      :dot(dirVec)
    local colinearityError = 1 - max(0, laneColinearity)
    local distError = abs(thisLaneLatPos - nodeLatPos)
    local overlapError = laneWidth -
                           max(0, min(thisLaneLimRight, laneRightLimLatPos) - max(thisLaneLimLeft, laneLeftLimLatPos))
    local directionError = laneConfig:byte(i) == 43 and 0 or 1

    -- lane bias, as driving-side aware, for right-hand drive: prefer leftmost lanes, for left-hand drive: prefer rightmost lanes
    local laneBias = isRightHandDrive and i - 1 or numOfLanes - i
    local totalError = distError + overlapError * 10 + directionError * 100 + laneBias * 0.1 + colinearityError * 3
    if totalError < bestError then
      bestError = totalError
      bestLane = i
      newLaneLimLeft = thisLaneLimLeft
      newLaneLimRight = thisLaneLimRight
    end
  end

  return bestLane, newLaneLimLeft, newLaneLimRight -- lateral positions are in the [0, 1] interval
end

local function getPathNodePosition(route, i)
  local path = route.path
  local wp1 = path[i - 1] or route.plan[1].wp
  local wp2 = path[i]
  local wp3 = path[i + 1]
  -- dump('---- > in', i, path[i])
  if not wp1 and not wp3 then
    -- dump('!!!!!!!!!!!!!!', path, objectId)
    return mapData.positions[wp2]:copy()
  elseif not wp1 then
    local wp2Pos = mapData:getEdgePositions(wp2, wp3)
    return wp2Pos:copy()
  elseif not wp3 then
    local _, wp2Pos = mapData:getEdgePositions(wp1, wp2)
    return wp2Pos:copy()
  else
    local e1P1, e1P2 = mapData:getEdgePositions(wp1, wp2)
    local e2P1, e2P2 = mapData:getEdgePositions(wp2, wp3)
    -- dump(wp1, wp2, wp3, e1P2:squaredDistance(e2P1))
    if e1P2:squaredDistance(e2P1) < 0.005 then
      -- dump('b0')
      return (e1P2 + e2P1) * 0.5
    else
      local e1Xnorm, e2Xnorm = closestLinePoints(e1P1, e1P2, e2P1, e2P2)
      local e2Xnorm2 = closestLinePoints(e2P1, e2P2, e1P1, e1P2)
      local _, e1R2 = mapData:getEdgeRadii(wp1, wp2)
      local e2R1 = mapData:getEdgeRadii(wp2, wp3)

      if e1Xnorm == 0 and e2Xnorm2 == 0 then
        -- segments are parallel
        -- dump('b1')
        return (e1P2 + e2P1) * 0.5

      elseif e1Xnorm >= 0 and (e1Xnorm - 1) * e1P2:distance(e1P1) <= e1R2 and e2Xnorm * e2P1:distance(e2P2) >= -e2R1 and
        e2Xnorm <= 1 then
        -- intersection is within the (radius augmented at P2) e1 and e2 segments
        -- dump('b2', 'e1Xnorm = ', e1Xnorm, 'e2Xnorm = ', e2Xnorm)
        local p1 = linePointFromXnorm(e1P1, e1P2, e1Xnorm)
        local p2 = linePointFromXnorm(e2P1, e2P2, e2Xnorm)
        p1:setAdd(p2);
        p1:setScaled(0.5)
        return p1

      elseif e2Xnorm < 1 and e1Xnorm >= 0 and (e1Xnorm - 1) * e1P2:distance(e1P1) <= e1R2 then
        -- intersection is within (radius augmented at P2) e1 segment and not furnter than the end of e2 (enures that it is not over segment 2 end, if it is too short)
        -- dump('b3', 'e1Xnorm = ', e1Xnorm, 'e2Xnorm = ', e2Xnorm)
        local segLen = e1P2:distance(e1P1)
        return linePointFromXnorm(e1P1, e1P2, max(e1Xnorm, 1 - e1R2 / segLen))

      elseif e1Xnorm > 0 and e2Xnorm * e2P1:distance(e2P2) >= -e2R1 and e2Xnorm <= 1 then
        -- intersection is within (radius augmented at P2) e2 segment and infront of e1P1
        -- dump('b4', 'e1Xnorm = ', e1Xnorm, 'e2Xnorm = ', e2Xnorm)
        local segLen = e2P1:distance(e2P2)
        return linePointFromXnorm(e2P1, e2P2, min(e2Xnorm, e2R1 / segLen))

      elseif e2Xnorm >= 1 or e1Xnorm < 0 then
        -- intersection is over segment 2 end or before segment 1 start (particular case)
        -- dump('b5')
        return (e1P2 + e2P1) * 0.5

      else
        -- intersection is between segments 1 and 2 but outside of radii extensions
        -- dump('b6')
        local p1 = linePointFromXnorm(e1P1, e1P2, e1Xnorm)
        local p2 = linePointFromXnorm(e2P1, e2P2, e2Xnorm)
        p1:setAdd(p2);
        p1:setScaled(0.5)
        local avgPoint = (e1P2 + e2P1) * 0.5
        local avgPointToP1Line = p1 - avgPoint
        return avgPoint + max(0, min(1, (e1R2 + e2R1) * 0.5 / avgPointToP1Line:length())) * avgPointToP1Line
      end
    end
  end
end

local function getPathNodeRadius(path, i)
  if not path[i - 1] and not path[i + 1] then
    return mapData.radius[path[i]]
  elseif not path[i - 1] then
    local wp1Rad = mapData:getEdgeRadii(path[i], path[i + 1])
    return wp1Rad
  elseif not path[i + 1] then
    local _, wp2Rad = mapData:getEdgeRadii(path[i - 1], path[i])
    return wp2Rad
  else
    local wp1Pos, wp2Pos = mapData:getEdgePositions(path[i - 1], path[i])
    local wp3Pos, wp4Pos = mapData:getEdgePositions(path[i], path[i + 1])
    local e1Xnorm, e2Xnorm = closestLineSegmentPoints(wp1Pos, wp2Pos, wp3Pos, wp4Pos)
    local wp1Rad, wp2Rad = mapData:getEdgeRadii(path[i - 1], path[i])
    local wp3Rad, wp4Rad = mapData:getEdgeRadii(path[i], path[i + 1])
    local r1 = wp1Rad + (wp2Rad - wp1Rad) * e1Xnorm
    local r2 = wp3Rad + (wp4Rad - wp3Rad) * e2Xnorm
    return (r1 + r2) * 0.5
  end
end

local function buildNextRoute(route, currentplan)
  local plan, path = currentplan or route.plan, route.path
  local planCount = plan.planCount
  local nextPathIdx = (plan[planCount].pathidx or 0) + 1

  if loopPath == true and noOfLaps and noOfLaps > 1 and not path[nextPathIdx] then -- in case the path loops
    local loopPathId
    local pathCount = #path
    local lastWayPoint = path[pathCount]
    for i = 1, pathCount do
      if lastWayPoint == path[i] then
        loopPathId = i
        break
      end
    end
    nextPathIdx = 1 + loopPathId -- nextPathIdx % #path
    noOfLaps = not plan.copy and noOfLaps - 1 or noOfLaps -- avoid decreasing noOfLaps for alternative plans
  end

  local newNodeName = path[nextPathIdx]
  if not newNodeName then
    return
  end
  local graph = mapData.graph
  if not graph[newNodeName] then
    return
  end

  -- gather information about new node
  local tmpPos = getPathNodePosition(route, nextPathIdx)
  local newNode = {
    name = newNodeName,
    posOrig = tmpPos,
    pos = tmpPos:copy(),
    radiusOrig = getPathNodeRadius(path, nextPathIdx),
    biNormal = -mapmgr.surfaceNormalBelow(mapData.positions[newNodeName], mapData.radius[newNodeName] * 0.5),
    prevNodeInPath = path[nextPathIdx - 1],
    inEdgeDrivability = nil,
    inEdgeLanes = nil, -- lanes going into the node along the path
    inEdgeSpeedLimit = nil,
    nextNodeInPath = path[nextPathIdx + 1],
    outEdgeLanes = nil, -- lanes coming out of the node along the path
    outEdgeSpeedLimit = nil
  }

  if newNode.prevNodeInPath then
    newNode.prevNodePos = mapData.positions[newNode.prevNodeInPath]
    local link = graph[newNode.prevNodeInPath][newNode.name]
    if link then
      newNode.inEdgeLanes = getEdgeLaneConfig(newNode.prevNodeInPath, newNode.name)
      newNode.inEdgeSpeedLimit = link.speedLimit
      newNode.inEdgeDrivability = link.drivability
    end
  else -- if previous plan node is the ego.pos then
    newNode.prevNodePos = vec3(plan[1].posOrig)
    if planCount == 1 and plan[1].wp then -- check whether information is available
      -- newNode.inEdgeLanes = plan[1].lanes -- TODO: use plan[1].wp to get lane configuration instead of saving the lane configuration in plan[1].lanes?
      newNode.prevNodeInPath = plan[1].wp
      newNode.inEdgeLanes = getEdgeLaneConfig(newNode.prevNodeInPath, newNode.name)
      newNode.inEdgeDrivability = graph[newNode.prevNodeInPath][newNode.name].drivability
    end
  end

  -- if there is no nextNodeInPath can we deduce it by elimination?
  -- we need to see if there is a unique node towards which we can lawfully drive
  if not newNode.nextNodeInPath then
    local nextPosibleNode = nil
    for k, v in pairs(graph[newNode.name]) do
      if k ~= newNode.prevNodeInPath then
        if numOfLanesInDirection(getEdgeLaneConfig(newNode.name, k), '+') > 0 then
          if nextPosibleNode then
            nextPosibleNode = nil
            break
          else
            nextPosibleNode = k
          end
        end
      end
    end
    newNode.nextNodeInPath = nextPosibleNode
  end

  if newNode.nextNodeInPath then
    local link = graph[newNode.name][newNode.nextNodeInPath]
    if link then
      newNode.outEdgeLanes = getEdgeLaneConfig(newNode.name, newNode.nextNodeInPath)
      newNode.outEdgeSpeedLimit = link.speedLimit
    end
  else
    if M.mode ~= 'traffic' then
      newNode.outEdgeLanes = newNode.inEdgeLanes
      newNode.outEdgeSpeedLimit = newNode.inEdgeSpeedLimit
    else
      return
    end
  end

  if newNode.outEdgeLanes and not newNode.inEdgeLanes then
    newNode.inEdgeLanes = newNode.outEdgeLanes
  end

  -- Adjust last plan node normal given information about node to be inserted:
  -- The normal of the node that is currently the last in the plan may need to be updated
  -- either because the path has been extended (where previously it was the last node in the path)
  -- or because the path has changed from this node forwards
  if planCount > 1 then
    if plan[planCount].wayPointAhead ~= path[nextPathIdx] then
      local norm1 = plan[planCount].biNormal:cross(plan[planCount].posOrig - plan[planCount - 1].posOrig);
      norm1:normalize()
      local norm2 = plan[planCount].biNormal:cross(newNode.posOrig - plan[planCount].posOrig);
      norm2:normalize()
      plan[planCount].normal:setAdd2(norm1, norm2)
      local tmp = plan[planCount].normal:length()
      plan[planCount].normal:setScaled(1 / (tmp + 1e-30))
      local chordLength = min(2 / tmp, (1 - norm1:dot(norm2) * 0.5) * tmp) -- 2 / tmp == cos(x/2) : x is angle between norm1, norm2
      plan[planCount].halfWidth = plan[planCount].radiusOrig * chordLength
    end
  else
    plan[planCount].normal:setSub2(newNode.posOrig, plan[planCount].posOrig)
    plan[planCount].normal:setCross(plan[planCount].biNormal, plan[planCount].normal)
    plan[planCount].normal:normalize()
  end

  -- Calculate normal of node to be inserted into the plan
  -- This normal is calculated from the normals of the two path edges incident on it
  newNode.inEdgeNormal = vec3()
  if newNode.prevNodeInPath then -- TODO: why not check for newNode.prevNodePos?
    local wp1Pos, wp2Pos = mapData:getEdgePositions(newNode.prevNodeInPath, newNode.name)
    newNode.inEdgeNormal:setSub2(wp2Pos, wp1Pos)
    newNode.inEdgeNormal:setCross(newNode.biNormal, newNode.inEdgeNormal)
    newNode.inEdgeNormal:normalize()
  end

  newNode.outEdgeNormal = vec3()
  if newNode.nextNodeInPath then
    local wp1Pos, wp2Pos = mapData:getEdgePositions(newNode.name, newNode.nextNodeInPath)
    newNode.outEdgeNormal:setSub2(wp2Pos, wp1Pos)
    newNode.outEdgeNormal:setCross(newNode.biNormal, newNode.outEdgeNormal)
    newNode.outEdgeNormal:normalize()
  end

  newNode.normal = newNode.inEdgeNormal + newNode.outEdgeNormal
  local tmp = newNode.normal:length()
  newNode.normal:setScaled(1 / (tmp + 1e-30))
  newNode.chordLength = min(2 / tmp, (1 - newNode.inEdgeNormal:dot(newNode.outEdgeNormal) * 0.5) * tmp) -- new node road width multiplier
  newNode.halfWidth = newNode.radiusOrig * newNode.chordLength

  local newNodeLaneLimLeft = 0
  local newNodeLaneLimRight = 1
  local newNodeRangeLeft = 0
  local newNodeRangeRight = 1
  local newNodeRangeLaneCount = 1
  local rangeBestLane = 0
  local bestLane
  local newNodeLaneConfig
  local newNodeRangeLeftIdx -- index of left most lane in range
  local newNodeRangeRightIdx -- index of right most lane in range

  if opt.driveInLaneFlag and newNode.inEdgeLanes then
    -- Consider lanes comming into newNode, scale/translate planNode lateral position and lane limits and add them to the in lane configuration as appropriate
    local inLaneConfig, planNodeLatPos, planNodelaneLimLeft, planNodelaneLimRight, planNodeRangeLeft,
      planNodeRangeRight, planNodeHalfWidth = processNodeIncommingLanes(newNode, plan[planCount]) -- lateral coordinates in [-r, r]

    -- Calculate lateral xnorm ranges of lanes in the direction of travel (in [0, 1])
    local outRangeLeft, outRangeRight, outRangeLaneCount = laneRange(newNode.outEdgeLanes)
    local inRangeLeft, inRangeRight, inRangeLaneCount = laneRange(inLaneConfig)

    -- Decide on the lane configuration of newNode: --> Retain the narrowest range
    local newNodeLaneConfigOrig
    if (inRangeRight - inRangeLeft) * outRangeLaneCount < (outRangeRight - outRangeLeft) * inRangeLaneCount then
      newNodeLaneConfig, newNodeRangeLeft, newNodeRangeRight, newNodeRangeLaneCount = inLaneConfig, inRangeLeft,
        inRangeRight, inRangeLaneCount
      newNodeLaneConfigOrig = newNode.inEdgeLanes
    else
      newNodeLaneConfig, newNodeRangeLeft, newNodeRangeRight, newNodeRangeLaneCount = newNode.outEdgeLanes,
        outRangeLeft, outRangeRight, outRangeLaneCount
      newNodeLaneConfigOrig = newNode.outEdgeLanes
    end

    local planNodeLatPosNrmd = linearScale(planNodeLatPos, planNodeRangeLeft, planNodeRangeRight, newNodeRangeLeft,
      newNodeRangeRight) -- TODO: lateralXnorm might be outside [inRangeLeft, inRangeRight]
    local planNodeLaneLimLeftNrmd = linearScale(planNodelaneLimLeft, planNodeRangeLeft, planNodeRangeRight,
      newNodeRangeLeft, newNodeRangeRight)
    local planNodeLaneLimRightNrmd = linearScale(planNodelaneLimRight, planNodeRangeLeft, planNodeRangeRight,
      newNodeRangeLeft, newNodeRangeRight)
    -- Calculate the most appropriate lane out of newNodeLaneConfig. This will be where to position the new node lateraly along the normal of newNode.
    bestLane, newNodeLaneLimLeft, newNodeLaneLimRight = getBestLane(newNodeLaneConfig, planNodeLatPosNrmd,
      planNodeLaneLimLeftNrmd, planNodeLaneLimRightNrmd, plan, newNode) -- lateral coordinates in [0, 1]

    -- Calculate the lane range (i.e. lateral limits of left most and right most lanes traveling in the same direction as bestLane, starting from bestLane)
    local rangeLeft, rangeRight = newNodeRangeLeft, newNodeRangeRight
    newNodeRangeLeft, newNodeRangeRight, newNodeRangeLeftIdx, newNodeRangeRightIdx = getLaneRangeFromLane(
      newNodeLaneConfig, bestLane) -- lateral coordinates in [0, 1]

    local rangeLeftOrig, rangeRightOrig = laneRange(newNodeLaneConfigOrig) -- Will this work when the lane config is not monotonic?
    newNodeRangeLeft = linearScale(newNodeRangeLeft, rangeLeft, rangeRight, rangeLeftOrig, rangeRightOrig)
    newNodeRangeRight = linearScale(newNodeRangeRight, rangeLeft, rangeRight, rangeLeftOrig, rangeRightOrig)
    -- newNodeLaneLimLeft = linearScale(newNodeRangeLeft + (bestLane - newNodeRangeLeftIdx) * laneWidth, 0, 1, -roadHalfWidth, roadHalfWidth)
    -- newNodeLaneLimRight = linearScale(newNodeRangeLeft + (bestLane - newNodeRangeLeftIdx) + 1) * laneWidth, 0, 1, -roadHalfWidth, roadHalfWidth)
    newNodeLaneLimLeft = linearScale(newNodeLaneLimLeft, rangeLeft, rangeRight, rangeLeftOrig, rangeRightOrig)
    newNodeLaneLimRight = linearScale(newNodeLaneLimRight, rangeLeft, rangeRight, rangeLeftOrig, rangeRightOrig)
  end

  newNode.pos:setAdd(((newNodeLaneLimLeft + newNodeLaneLimRight) - 1) * newNode.halfWidth * newNode.normal)

  newNode.inEdgeNormal:setSub2(newNode.pos, plan[planCount].pos)
  newNode.inEdgeNormal:setCross(newNode.biNormal, newNode.inEdgeNormal)
  newNode.inEdgeNormal:normalize()
  newNode.normal = newNode.inEdgeNormal + newNode.outEdgeNormal

  local tmp = newNode.normal:length()
  newNode.normal:setScaled(1 / (tmp + 1e-30))
  newNode.chordLength = min(2 / tmp, (1 - newNode.inEdgeNormal:dot(newNode.outEdgeNormal) * 0.5) * tmp)
  newNode.halfWidth = newNode.radiusOrig * newNode.chordLength

  local newNodeLatPos
  if newNodeLaneConfig and #newNodeLaneConfig > 1 and newNode.nextNodeInPath and newNode.prevNodeInPath then -- TODO: error hits if the nextNode/prevNode conditions are not here
    local laneWidth = 2 * newNode.radiusOrig * (newNodeRangeRight - newNodeRangeLeft) / newNodeRangeLaneCount -- lane width not including chordLength
    local turnDir = (mapData.positions[newNode.nextNodeInPath] - newNode.posOrig) +
                      (mapData.positions[newNode.prevNodeInPath] - newNode.posOrig)
    -- keep inside lanes narrower
    if turnDir:dot(newNode.normal) < 0 then
      newNodeLaneLimLeft = linearScale(newNodeRangeLeft, 0, 1, -newNode.halfWidth, newNode.halfWidth) +
                             (bestLane - newNodeRangeLeftIdx) * laneWidth
      --[[ tighter outside left turns
      newNodeLaneLimLeft = -newNodeHalfWidth + (bestLane - 1) * laneWidth
      newNodeRangeLeft = min(newNodeRangeLeft, linearScale(-newNodeHalfWidth + (newNodeRangeLeftIdx - 1) * laneWidth, -newNodeHalfWidth, newNodeHalfWidth, 0, 1))
      --]]
      newNodeLaneLimRight = newNodeLaneLimLeft + laneWidth
      newNodeLatPos = (newNodeLaneLimLeft + newNodeLaneLimRight) * 0.5
      if bestLane == #newNodeLaneConfig then
        newNodeLaneLimRight = newNode.halfWidth
      end -- give remaining space on the right to the right most lane
    else
      newNodeLaneLimRight = linearScale(newNodeRangeRight, 0, 1, -newNode.halfWidth, newNode.halfWidth) -
                              (newNodeRangeRightIdx - bestLane) * laneWidth
      newNodeLaneLimLeft = newNodeLaneLimRight - laneWidth
      newNodeLatPos = (newNodeLaneLimLeft + newNodeLaneLimRight) * 0.5
      if bestLane == 1 then
        newNodeLaneLimLeft = -newNode.halfWidth
      end -- give remaining space on the left to left most lane
    end
  else
    -- Transform newNode lateral position and lane limits from [0, 1] to [-r, r] using the recalculated newNodeHalfWidth
    newNodeLaneLimLeft = linearScale(newNodeLaneLimLeft, 0, 1, -newNode.halfWidth, newNode.halfWidth)
    newNodeLaneLimRight = linearScale(newNodeLaneLimRight, 0, 1, -newNode.halfWidth, newNode.halfWidth)
    newNodeLatPos = (newNodeLaneLimLeft + newNodeLaneLimRight) * 0.5
  end
  newNodeLatPos = newNodeLatPos + (plan.offset or 0)
  -- plan converges gradually towards the center of the road
  if not opt.driveInLaneFlag then
    -- normalized lateral xnorm of the last node on the plan
    local normalizedPrevLatXnorm = plan[plan.planCount].lateralXnorm / (plan[plan.planCount].halfWidth)

    -- last plan node lateral xnorm scaled to the width of the new node
    local lateralXnorm = max(min(normalizedPrevLatXnorm * newNode.halfWidth, newNode.halfWidth), -newNode.halfWidth)

    -- calculate smoothing parameter
    local t = min(1, max(0, plan.planLen / 100)) -- currently converges to the center of the road at 100m

    -- calculate desired lateral xnorm for the new plan node
    newNodeLatPos = lateralXnorm * (1 - t) + newNodeLatPos * t
  end

  newNode.pos = newNode.posOrig + newNodeLatPos * newNode.normal

  local lastPlanPos = plan[planCount] and plan[planCount].pos or ego.pos
  local vec = lastPlanPos - newNode.pos;
  vec.z = 0

  return {
    posOrig = newNode.posOrig,
    pos = newNode.pos,
    vec = vec,
    dirVec = vec:normalized(),
    turnDir = vec3(0, 0, 0),
    biNormal = newNode.biNormal,
    normal = newNode.normal,
    radiusOrig = newNode.radiusOrig,
    manSpeed = speedProfile and speedProfile[newNode.name],
    roadSpeedLimit = newNode.prevNodeInPath and mapData.graph[newNode.prevNodeInPath][newNode.name].speedLimit,
    pathidx = nextPathIdx,
    halfWidth = newNode.halfWidth,
    widthMarginOffset = 0,
    wayPointAhead = newNode.nextNodeInPath,
    laneLimLeft = newNodeLaneLimLeft, -- lateral coordinate of current lane left limit [-hW, hW]
    laneLimRight = newNodeLaneLimRight, -- lateral coordinate of current lane right limit [-hW, hW]
    curvature = 0,
    curvatureZ = 0,
    lateralXnorm = newNodeLatPos, -- lateral coordinate of pos [-hW, hW]
    legalSpeed = nil,
    speed = nil,
    inLaneConfig = newNode.inEdgeLanes, -- lane configuration of incoming edge to node
    rangeLeft = newNodeRangeLeft, -- lateral coordinate [0, 1] of the left hand side limit of the left most lane in the direction of travel
    rangeRight = newNodeRangeRight, -- lateral coordinate [0, 1] of the right hand side limit of the right most lane in the direction of travel
    rangeLaneCount = newNodeRangeLaneCount, -- number of contiguous lanes in the direction of travel counting from the current lane
    rangeBestLane = bestLane and newNodeRangeLeftIdx and (bestLane - newNodeRangeLeftIdx) or rangeBestLane, -- 0 indexed
    trafficSqVel = math.huge,
    dispDir = 0
  }
end

local function mergePathPrefix(source, dest, srcStart)
  srcStart = srcStart or 1
  local sourceCount = #source
  local dict = table.new(0, sourceCount - (srcStart - 1))
  for i = srcStart, sourceCount do
    dict[source[i]] = i
  end

  local destCount = #dest
  for i = destCount, 1, -1 do
    local srci = dict[dest[i]]
    if srci ~= nil then
      local res = table.new(destCount, 0)
      local resi = 1
      for i1 = srcStart, srci - 1 do
        res[resi] = source[i1]
        resi = resi + 1
      end
      for i1 = i, destCount do
        res[resi] = dest[i1]
        resi = resi + 1
      end

      return res, srci
    end
  end

  return dest, 0
end

local function uniformPlanErrorDistribution(plan)
  if twt.state == 0 then
    local p1, p2 = plan[1].pos, plan[2].pos
    local dispVec = ego.pos - linePointFromXnorm(p1, p2, ego.pos:xnormOnLine(p1, p2))
    dispVec:setScaled(min(1, 4 * dt))
    local dispVecDir = dispVec:normalized()

    local tmpVec = p2 - p1;
    tmpVec:setCross(tmpVec, ego.upVec);
    tmpVec:normalize()
    -- egoDeviation = dispVec:dot(tmpVec)

    local j = 0
    local dTotal = 0
    for i = 1, plan.planCount - 1 do
      tmpVec:setSub2(plan[i + 1].pos, plan[i].pos)
      if math.abs(dispVecDir:dot(tmpVec)) > 0.5 * plan[i].length then
        break
      end
      j = i
      dTotal = dTotal + plan[i].length
    end
    local laneWidthMargin = ego.width * 0.6
    local sumLen = 0
    for i = j, 1, -1 do
      local n = plan[i]
      sumLen = sumLen + plan[i].length

      local lateralXnorm = n.lateralXnorm or 0
      local newLateralXnorm = clamp(lateralXnorm + ((dispVec):dot(n.normal) * sumLen / dTotal),
        (n.laneLimLeft + laneWidthMargin) or -math.huge, (n.laneLimRight - laneWidthMargin) or math.huge)
      tmpVec:setScaled2(n.normal, newLateralXnorm - lateralXnorm)
      n.pos:setAdd(tmpVec)
      n.lateralXnorm = newLateralXnorm

      plan[i + 1].vec:setSub2(plan[i].pos, plan[i + 1].pos);
      plan[i + 1].vec.z = 0
      plan[i + 1].dirVec:setScaled2(plan[i + 1].vec, 1 / plan[i + 1].vec:lengthGuarded())
    end

    updatePlanLen(plan, 1, j)
  end
end

local function createNewRoute(path)
  local newRoute = {
    path = path,
    plan = table.new(15, 10),
    laneChanges = {}, -- array: in the array each lane change is a dict with an idx key (path index at which lane change occurs) and a side key (direction of lane change)
    lastLaneChangeIdx = 1, -- path node up to which we have checked for a posible lane change
    pathLength = {0} -- distance from beggining of path to node at index i
  }
  newRoute.plan.stopSeg = math.huge

  return newRoute
end

local function isVehicleStopped(v)
  if v.isParked then
    return true
  elseif v.states.ignitionLevel == 0 or v.states.ignitionLevel == 1 then
    return true
  elseif v.states.hazard_enabled == 1 and v.vel:squaredLength() < 9 then
    return true
  end
  return false
end

local function inCurvature(v1, v2)
  --[[
    Given three points A, B, C (with AB being the vector from A to B), the curvature (= 1 / radius)
    of the circle going through them is:

    curvature = 2 * (AB x BC) / ( |AB| * |BC| * |CA| ) =>
              = 2 * |AB| * |BC| * Sin(th) / ( |AB| * |BC| * |CA| ) =>
              = 2 * (+/-) * sqrt ( 1 - Cos^2(th) ) / |CA| =>
              = 2 * (+/-) sqrt [ ( 1 - Cos^2(th) ) / |CA|^2 ) ] -- This is an sqrt optimization step

    In the calculation below the (+/-) which indicates the turning direction (direction of AB x BC) has been dropped
  --]]

  -- v1 and v2 vector components
  local v1x, v1y, v1z, v2x, v2y, v2z = v1.x, v1.y, v1.z, v2.x, v2.y, v2.z

  local v1Sqlen, v2Sqlen = v1x * v1x + v1y * v1y + v1z * v1z, v2x * v2x + v2y * v2y + v2z * v2z
  local dot12 = v1x * v2x + v1y * v2y + v1z * v2z
  local cosSq = min(1, dot12 * dot12 / max(1e-30, v1Sqlen * v2Sqlen))

  if dot12 < 0 then -- angle between the two segments is > 90 deg
    local minDsq = min(v1Sqlen, v2Sqlen)
    local maxDsq = minDsq / max(1e-30, cosSq)
    if max(v1Sqlen, v2Sqlen) > (minDsq + maxDsq) * 0.5 then
      if v1Sqlen > v2Sqlen then
        -- swap v1 and v2
        v1x, v1y, v1z, v2x, v2y, v2z = v2x, v2y, v2z, v1x, v1y, v1z
        v1Sqlen, v2Sqlen = v2Sqlen, v1Sqlen
      end
      local s = sqrt(0.5 * (minDsq + maxDsq) / max(1e-30, v2Sqlen))
      v2x, v2y, v2z = s * v2x, s * v2y, s * v2z
    end
  end

  return 2 * sqrt((1 - cosSq) / max(1e-30, square(v1x + v2x) + square(v1y + v2y) + square(v1z + v2z))) -- the denominator is v1:squaredLength(-v2)
end

-- ********* FUNCTIONS FOR SPEED PROFILE GENERATION ********* --

-- Computes the acceleration budget based on the vehicle speed (x) to simulate more realistic real world driving behaviour
-- https://arxiv.org/pdf/1907.01747
local function speedBasedAccelBudget(x, a)
  x = max(0, x)
  a = a or 0

  local fx
  if 0 <= x and x < 5 then
    fx = 0.3 * x + 4
  elseif 5 <= x and x < 10 then
    fx = 5.5
  elseif 10 <= x and x < 15 then
    fx = -0.1 * x + 6.5
  elseif 15 <= x and x < 20 then
    fx = -0.15 * x + 7.25
  elseif 20 <= x and x < 25 then
    fx = -0.15 * x + 7.25
  elseif 25 <= x and x < 30 then
    fx = -0.1 * x + 6
  else
    fx = 3
  end

  return max(0, min(fx + a, ego.staticFrictionCoef * g))
end

local speedProfileMode -- ('Back' for new backward method, 'ForwBack' for forward+Backward method)
local function setSpeedProfileMode(mode)
  if mode == 'Back' then
    speedProfileMode = 'Back'
  elseif mode == 'ForwBack' then
    speedProfileMode = 'ForwBack'
  else
    speedProfileMode = nil
  end
end

-- Compute maximum available longitudinal acceleration
local function acc_eval_1(speedSq, acc_max, curvature)
  local ax_max_tyre = acc_max -- has to be exctracted from ggv
  local ay_max_tyre = acc_max -- has to be exctracted from ggv
  local ay_used = speedSq * curvature
  return ax_max_tyre * max(0, 1 - ay_used / ay_max_tyre)
end

local function acc_eval_2(speedSq, acc_max, curvature)
  local ax_max_tyre = acc_max -- has to be exctracted from ggv
  local ay_max_tyre = acc_max -- has to be exctracted from ggv
  local ay_used = speedSq * curvature
  if ay_used < ay_max_tyre then
    return ax_max_tyre * sqrt(1 - square(ay_used / ay_max_tyre))
  else
    return 0
  end
end

local acc_eval = acc_eval_2 -- Default adherence constraint
-- set the index for adherence constraint, useful only for Back or ForwBack mode
local function setTractionModel(model_index)
  if model_index == 1 then
    acc_eval = acc_eval_1
  else
    acc_eval = acc_eval_2
  end
end

-- Compute forward pass (speed0 = starting velocity, model_index = index for adherence constraint)
local function solver_f_acc_profile(plan, speed0)
  -- Note: This calculation need the node speed values to be squared.
  -- This will already be the case if the previous calculation for the limiting turn speed avoided calculating square root of speeds.

  plan[1].speed = speed0 or plan[1].speed

  -- The last node speed value was not calculated from the limiting turn speed (due to absence of curvature value), so square it.
  plan[plan.planCount].speed = square(plan[plan.planCount].speed)

  for i = 1, plan.planCount - 1 do
    local n1, n2 = plan[i], plan[i + 1]

    local vx_possible_next
    if plan.stopSeg <= i + 1 then
      vx_possible_next = 0
    else
      if min(n2.speed, n2.trafficSqVel) < n1.speed then -- max velocity at i-1 is less than velocity at i (not a deceleration phase)
        vx_possible_next = min(n2.speed, n2.trafficSqVel)
      else
        local ax_final = acc_eval(n1.speed, n1.acc_max, abs(n1.curvature))
        vx_possible_next = n1.speed + 2 * ax_final * n1.length -- speed squared
        vx_possible_next = min(n2.speed, min(n2.trafficSqVel, vx_possible_next))
      end
    end

    n2.speed = vx_possible_next
  end

  -- Backwards aceel integrator expects the last node speed value to not be squared
  plan[plan.planCount].speed = sqrt(plan[plan.planCount].speed)
end

-- Compute backward pass (speed_end = final velocity, model_index = index for adherence constraint)
local function solver_b_acc_profile(plan)
  for i = plan.planCount, 2, -1 do
    local n1, n2 = plan[i - 1], plan[i]
    local vx_possible_next
    if plan.stopSeg <= i - 1 then
      vx_possible_next = 0
    else
      local n2SpeedSq = n2.speed * n2.speed
      if min(n1.speed, n1.trafficSqVel) < n2SpeedSq then -- max velocity at i-1 is less than velocity at i (not a deceleration phase)
        vx_possible_next = sqrt(min(n1.speed, n1.trafficSqVel))
      else
        -- available longitudinal acceleration at node i
        local ax_possible_current = acc_eval(n2SpeedSq, n2.acc_max, abs(n2.curvature))
        -- possible velocity at node i-1 given available longitudinal acceleration at node i
        vx_possible_next = n2SpeedSq + 2 * n1.length * ax_possible_current

        -- available longitudinal acceleration at node i-1 given velocity estimate at node i-1
        local ax_possible_next = acc_eval(vx_possible_next, n1.acc_max, abs(n1.curvature))
        -- possible velocity at i-1 given available longitudinal acceleration at node i-1
        local vx_tmp = n2SpeedSq + 2 * n1.length * ax_possible_next

        if vx_possible_next > vx_tmp then
          -- available longitudinal acceleration at node i-1 given new velocity estimate at node i-1
          ax_possible_next = acc_eval(vx_tmp, n1.acc_max, abs(n1.curvature))
          -- improve velocity estimate at i-1 given available longitudinal acceleration at node i-1
          vx_tmp = n2SpeedSq + 2 * n1.length * ax_possible_next
          -- keep the velocity that satisfies longitudinal acceleration constraints at node i-1 and node i
          vx_possible_next = min(vx_possible_next, vx_tmp)
        end

        vx_possible_next = sqrt(min(n1.speed, min(n1.trafficSqVel, vx_possible_next))) -- respect traffic speed
      end
    end

    n1.speed = n1.manSpeed or (M.speedMode == 'limit' and M.routeSpeed and min(M.routeSpeed, vx_possible_next)) or
                 (M.speedMode == 'set' and M.routeSpeed) or vx_possible_next

    if M.speedMode == 'legal' then
      n2.legalSpeed = n2.legalSpeed or n2.speed
      n1.legalSpeed = min(min(n1.speed, sqrt(n2.legalSpeed * n2.legalSpeed + (n1.acc_max + n2.acc_max) * n1.length)),
        (n1.roadSpeedLimit or math.huge))
    end

    n1.trafficSqVel = math.huge
  end
end

local function setRacing(v)
  if v == true then
    opt.racing = true
    traffic.filtered = false
  else
    opt.racing = false
  end
end
M.setRacing = setRacing

local function setTrafficFilter(v)
  if v == true then
    traffic.filtered = true
  else
    traffic.filtered = false
  end
end
M.setTrafficFilter = setTrafficFilter

local function setVdraw(v)
  if v == true then
    traffic.vdraw = true
  else
    traffic.vdraw = false
  end
end
M.setVdraw = setVdraw

local function trafficFilter(index, route, v, draw)
  local path = route.path
  local plan1 = route.plan[1]
  local plan2 = route.plan[2]
  -- local ego2vmiddle =  ego.pos:squaredDistance(v.posMiddle)

  -- Initialize n1 node data table
  local n1 = {
    name = nil,
    posOrig = plan1.posOrig,
    radiusOrig = plan1.radiusOrig,
    biNormal = plan1.biNormal,
    normal = vec3()
  }

  -- Initialize n2 node data table
  local n2 = {
    normal = vec3()
  }

  -- placeholder for segment direction vector
  local dirVec, dirVecNext = vec3(), vec3()

  -- cache player vehicle position and direction data
  local vPfront, vPrear, vPmiddle, vdirVec = v.posFront, v.posRear, v.posMiddle, v.dirVec

  -- will be set to true if any of the path nodes is an intersection
  local intersectionFound = false
  for i = max(index.start, plan2.pathidx), index.final do
    -- check if this node is an intersection
    intersectionFound = intersectionFound or tableSize(mapData.graph[route.path[i]]) > 2

    -- gather data on node n2
    n2.name = path[i]
    n2.posOrig = getPathNodePosition(route, i)
    n2.radiusOrig = getPathNodeRadius(path, i) -- mapData.radius[path[i]],
    n2.biNormal = -mapmgr.surfaceNormalBelow(mapData.positions[path[i]], mapData.radius[path[i]] * 0.5)

    dirVec:setSub2(n1.posOrig, n2.posOrig);
    dirVec.z = 0;
    dirVec:normalize()
    n1.normal:setCross(dirVec, n1.biNormal)
    n2.normal:setCross(dirVec, n2.biNormal)

    local roadHalfWidth1, roadHalfWidth2 = n1.radiusOrig * 1.05, n2.radiusOrig * 1.05

    -- n1 and n2 left boundaries
    local n1PosLeft, n2PosLeft = n1.posOrig - roadHalfWidth1 * n1.normal, n2.posOrig - roadHalfWidth2 * n2.normal

    -- n1 and n2 right boundaries
    local n1PosRight, n2PosRight = n1.posOrig + roadHalfWidth1 * n1.normal, n2.posOrig + roadHalfWidth2 * n2.normal

    -- projection of vehicle front position on line passing through right boundaries
    local xnormFrRight = vPfront:xnormOnLine(n1PosRight, n2PosRight)

    -- projection of vehicle front position on line passing through left boundaries
    local xnormFrLeft = vPfront:xnormOnLine(n1PosLeft, n2PosLeft)

    -- projection of vehicle rear position on line passing through right boundaries
    local xnormReRight = vPrear:xnormOnLine(n1PosRight, n2PosRight)

    -- projection of vehicle rear position on line passing through left boundaries
    local xnormReLeft = vPrear:xnormOnLine(n1PosLeft, n2PosLeft)

    -- taking the nearest projection of vehicle front and rear
    local minF = min(xnormFrRight, xnormFrLeft)
    local minR = min(xnormReRight, xnormReLeft)

    -- ego to vehicle direction vector
    local ego2PlVec = vPfront - ego.pos;
    ego2PlVec:normalize()

    local ego2v = vPmiddle:squaredDistance(ego.pos - 0.5 * ego.length * ego.dirVec)
    local xnormV = vPmiddle:xnormOnLine(plan1.posOrig - roadHalfWidth1 * plan1.normal,
      plan1.posOrig + roadHalfWidth1 * plan1.normal)
    local lanew = (plan1.rangeRight - plan1.rangeLeft) / plan1.rangeLaneCount
    if plan1.rangeLaneCount > 1 and ego.dirVec:dot(vdirVec) > 0.7 and ego2v <
      square((1.2 * ego.length + 1.2 * v.length)) and (xnormV > plan1.rangeLeft and xnormV < plan1.rangeRight) and
      (not (xnormV > (plan1.rangeLeft + lanew * plan1.rangeBestLane) and xnormV <
        (plan1.rangeLeft + lanew * (plan1.rangeBestLane + 1)))) then
      if draw then
        obj.debugDrawProxy:drawSphere(2, vPfront, color(0, 0, 0, 160))
      end
      if ego2PlVec:dot(ego.rightVec) > 0 then
        ego.ghostR = true
      else
        ego.ghostL = true
      end
      return true
    end

    -- *Check 1.1: check if v-vehicle is in the current path-segment projection
    if (minF > 0 and minF < 1) or (minR > 0 and minR < 1) then
      -- *Check 1.1.1: check if v-vehicle is coming in opposite direction
      if ego2PlVec:dot(vdirVec) < 0 then
        -- smooth transition of dirVec if mesh is not large enough
        if minF < 1 and minR > 1 and path[i + 1] then
          local n3posOrig = getPathNodePosition(route, i + 1)
          dirVecNext:setSub2(n2.posOrig, n3posOrig);
          dirVecNext.z = 0;
          dirVecNext:normalize()
          dirVec:setAdd2(max(0, minF) * dirVecNext, max(0, 1 - minF) * dirVec)
          -- dirVec:setAdd(dirVecNext)
          dirVec:normalize()
        end
        -- *Check 1.1.1.1: add it if it is not parallel to current path-segment
        if vdirVec:dot(dirVec) < 0.95 then
          if draw then
            obj.debugDrawProxy:drawSphere(2, vPfront, color(255, 0, 0, 160))
          end
          return true
          -- *Check 1.1.1.2: add it if it is parallel to current path-segment but there is an intersection
        else
          if traffic.intersection and intersectionFound then
            if draw then
              obj.debugDrawProxy:drawSphere(2, vPfront, color(255, 100, 0, 160))
            end
            return true
          end
        end
        -- *Check 1.1.2: add v-vehicle if it is in front of us
      else
        v.noproj = true
        if draw then
          obj.debugDrawProxy:drawSphere(2, vPfront, color(0, 255, 0, 160))
        end
        return true
      end
      -- *Check 1.1.3: check for v-vehicles that are coming in opposite direction and are in our lanes
      local xnormP = vPfront:xnormOnLine(n1PosLeft, n1PosRight)
      local rangeLeft, rangeRight = plan1.rangeLeft, plan1.rangeRight
      if n1.name then
        local lanes = getEdgeLaneConfig(n1.name, n2.name)
        rangeLeft, rangeRight = laneRange(lanes)
      end
      if xnormP > rangeLeft and xnormP < rangeRight then
        if draw then
          obj.debugDrawProxy:drawSphere(2, vPfront, color(255, 255, 255, 160))
        end
        return true
      else
        return false
      end
      -- *Check 1.2: add v-vehicles if it is behind us or in an dead corner (due to path projection)
    elseif xnormFrRight < 0 or xnormFrLeft < 0 then
      if vPfront:squaredDistance(n1.posOrig) < 100 * 100 then
        if draw then
          obj.debugDrawProxy:drawSphere(2, vPfront, color(0, 0, 255, 160))
        end
        return true
      end
    end

    -- update n1 data
    n1.name, n1.posOrig, n1.radiusOrig, n1.biNormal = n2.name, n2.posOrig, n2.radiusOrig, n2.biNormal
  end

  return false
end

local function trafficFilter_2(route, v)
  local path = route.path
  local curPlanIdx = route.plan[2].pathidx
  local pathCount = #path
  local frontChecks, rearChecks = nil, nil
  for i = 1, pathCount - 1 do

    if i >= curPlanIdx then
      if tableSize(mapData.graph[path[i]]) > 2 then
        return true
      end
    end

    local pos1, pos2 = mapData:getEdgePositions(path[i], path[i + 1])
    local rad1, rad2 = mapData:getEdgeRadii(path[i], path[i + 1])
    local biNormal = mapmgr.surfaceNormalBelow(pos1)
    local normal = (pos2 - pos1):cross(biNormal);
    normal:normalize()
    local laneConfig = getEdgeLaneConfig(path[i], path[i + 1])
    local laneRangeLimLeft, laneRangeLimRight = laneRange(laneConfig)

    if not frontChecks then
      local longXnormFront = v.posFront:xnormOnLine(pos1, pos2)
      local latXnormFront = v.posFront:xnormOnLine(pos1,
        pos1 + normal * (rad1 + (rad2 - rad1) * clamp(longXnormFront, 0, 1)))
      latXnormFront = (latXnormFront + 1) * 0.5 -- maps [-1, 1] interval to [0, 1] interval
      if longXnormFront >= 0 and longXnormFront <= 1 and latXnormFront >= 0 and latXnormFront <= laneRangeLimLeft then
        frontChecks = i
      end
    end

    if not rearChecks then
      local longXnormRear = v.posRear:xnormOnLine(pos1, pos2)
      local latXnormRear = v.posRear:xnormOnLine(pos1,
        pos1 + normal * (rad1 + (rad2 - rad1) * clamp(longXnormRear, 0, 1)))
      latXnormRear = (latXnormRear + 1) * 0.5 -- maps [-1, 1] interval to [0, 1] interval
      if longXnormRear >= 0 and longXnormRear <= 1 and latXnormRear >= 0 and latXnormRear <= laneRangeLimLeft then
        rearChecks = i
      end
    end

    if frontChecks and rearChecks then
      return false
    end

  end

  return true
end

local function updatePlanbestRange(plan)
  for i = 1, plan.planCount do
    local n = plan[i]
    local roadHalfWidth = n.halfWidth
    local laneWidth = 2 * roadHalfWidth * (n.rangeRight - n.rangeLeft) / n.rangeLaneCount
    plan[i].rangeBestLane = clamp((n.laneLimLeft - ((2 * n.rangeLeft - 1) * roadHalfWidth)) / laneWidth, 0,
      n.rangeLaneCount - 1)
    -- print(plan[i].rangeBestLane)
  end
end

local function calculateTrafficTargetSpeed(plan, trafficTable, trafficTableLen, pmode, route)
  plan.distances = math.huge
  plan.trafficTargetSpeed = math.huge
  if trafficTableLen > 0 and plan.targetSpeed > 0 then
    local distOnPlan = -plan[1].length * plan.egoXnormOnSeg -- 0
    local segDirVec, segVx, segC, segVz, segVy = vec3(), vec3(), vec3(), vec3(), vec3()
    for i = 1, plan.planCount - 1 do
      local n1, n2 = plan[i], plan[i + 1]
      distOnPlan = distOnPlan + n1.length -- distance from the ego vehicle front position to the end of the ith segment along the plan
      segDirVec:setSub2(n2.pos, n1.pos);
      segDirVec:setScaled(1 / (n1.length + 1e-30))

      -- Bounding box of segment [n1, n2]
      segVx:setScaled2(segDirVec, 0.5 * n1.length)
      segC:setAdd2(n1.pos, segVx)
      segVz = mapmgr.surfaceNormalBelow(segC, (n1.radiusOrig + n2.radiusOrig) * 0.25);
      segVz:setScaled(-1)
      segVy:setCross(segVz, segVx);
      segVy:setScaled(0.5 * ego.width / (segVy:length() + 1e-30)) -- 0.5 * (ego.width + 1)
      segVy:setScaled2(segVz:cross(segVx):normalized(), 0.5 * ego.width)

      for j = trafficTableLen, 1, -1 do
        local v = trafficTable[j]
        v.posMiddle, v.vx, v.vy, v.vz = getObjectBoundingBox(v.id)
        v.vx = v.vx * 1.1
        v.posFront = v.posMiddle + v.vx
        v.posRear = v.posMiddle - v.vx
        v.dirVec = v.vx:normalized()
        v.length = v.vx:length() * 2
        v.width = v.vy:length() * 2
        for kk = 1, 2 do
          if kk == 2 then
            local check_ahead = (v.posFront - (ego.pos - ego.length * ego.dirVec)):dot(ego.dirVec) > 0
            if check_ahead then
              local vProj = v.vel * 0.2
              v.posMiddle, v.posFront, v.posRear = v.posMiddle + vProj, v.posFront + vProj, v.posRear + vProj
              -- obj.debugDrawProxy:drawSphere(0.5, v.posFront, color(0,255,0,50))
            else
              break
            end
          end

          if overlapsOBB_OBB(v.posMiddle, v.vx, v.vy, v.vz, segC, segVx, segVy, segVz) then
            local vRearXnorm, vFrontXnorm = v.posRear:xnormOnLine(n1.pos, n2.pos),
              v.posFront:xnormOnLine(n1.pos, n2.pos)

            plan.distances = min(distOnPlan, plan.distances)
            -- if the vehicle overlaps the first segment check if its xnorm is greater than the xnorm of the ego vehicle
            if i > 1 or max(vRearXnorm, vFrontXnorm) > plan.egoXnormOnSeg then
              local vXnorm = clamp(min(vRearXnorm, vFrontXnorm), 0, 1)
              -- local ego2PlDist = max(0, distOnPlan - plan[1].length * plan.egoXnormOnSeg + clamp(vXnorm, 0, 1) * n1.length) -- if current segment length is added after the traffic table loop

              -- the ego.speed term creates a feedback loop
              -- lowering the coefficient of that term mitigates the effect of the feedback loop
              -- parts of the throttle/brake/handbrake control logic at the end of driveToTarget also seem to be creating issues when at low targetSpeed
              -- see function driveToTarget: TODO: handbrake when stoped or moving backward and TODO: handbrake when turning
              local ego2PlDist_0 = max(0, distOnPlan - (1 - vXnorm) * n1.length) -- max(2.5, opt.racing and ego.speed*0.5 or ego.speed * 1)
              plan.distances = min(ego2PlDist_0, plan.distances)

              local ego2PlDist = ego2PlDist_0 - max(2.5, ego.speed * 1)
              local velProjOnSeg = v.vel:dot(segDirVec) -- max(0, v.vel:dot(segDirVec))

              -- dump('dist and vel on seg', ego2PlDist, velProjOnSeg)

              local targetSpeed = max(0, abs(velProjOnSeg) * velProjOnSeg + 2 * g *
                min(aggression, ego.staticFrictionCoef) * ego2PlDist)

              -- might help improve the throttle/brake input stability caused by the feedback loop of the ego.speed term in ego2PlDist
              -- if plan.trafficTargetSpeed < 0.25 then plan.trafficTargetSpeed = 0 end
              if plan.distances == ego2PlDist_0 then
                plan.trafficTargetSpeed = targetSpeed
                plan.trafficMinProjSpeed = velProjOnSeg
              end

            end

            table.remove(trafficTable, j)
            trafficTableLen = trafficTableLen - 1
          end

          if plan.trafficTargetSpeed == 0 then
            break
          end
        end
      end

      -- distOnPlan = distOnPlan + n1.length -- distance to the end of the ith segment

      if trafficTableLen == 0 or plan.trafficTargetSpeed == 0 then -- or ego.speed * ego.speed < 2 * distOnPlan * (0.2 * ego.staticFrictionCoef * g)
        break
      end
    end
  end

  -- logic to overwrite trafficTargetSpeed in racing mode
  if opt.racing then
    -- speed difference between ego vehicle and vehicle ahead
    local dv = ego.speed - plan.trafficMinProjSpeed
    -- catching distance: distance from vehicle ahead up to which I can increase my trafficTargetSpeed in order to catch him
    ego.race.d = linearScale(dv, 0, 20, 0.5 * ego.length, 2 * ego.length)
    -- logic for emergency brake (it is not working do to false flag, need some improvements on TTC value)
    local TTC = plan.distances / max(dv, 1e-3)
    if TTC < 2 and abs(plan.trafficMinProjSpeed) < 1 then
      plan.trafficTargetSpeed = 0
    else
      -- controller for trafficTargetSpeed: it is defined by 3 terms:
      -- 1) speed of vehicle ahead on our current plan
      -- 2) boost factor in order to try to catch him. This factor increases trafficTargetSpeed up to ego.race.d distance and with an aggression defined by ego.race.Td
      -- 3) Proportional factor: useful to reduce trafficTargetSpeed when dv > 0. It acts strongly when distance is less than ego.race.d
      plan.trafficTargetSpeed = plan.trafficMinProjSpeed + max(0, (plan.distances - ego.race.d) / ego.race.Td) -
                                  ego.race.kp * max(0, dv)
    end
    plan.trafficTargetSpeed = max(plan.trafficTargetSpeed, 0)
  else
    plan.trafficTargetSpeed = sqrt(plan.trafficTargetSpeed)
  end
end

local function offsetPlan(plan, altPlan, side)
  local sideSign = side == 'right' and 1 or side == 'left' and -1 or 0
  altPlan.copy = true
  local latdisp = (1.2 * ego.width) or 3
  local margin = ego.width * 0.5
  altPlan.offset = latdisp * sideSign
  local dist = 0
  local tempVec = vec3()
  -- apply offset and update other geometrical quantities
  for i = 1, plan.planCount do
    local n = altPlan[i]
    dist = dist + (n.length or 0)
    local offset = clamp(altPlan.offset, -(plan[i].halfWidth - margin) - plan[i].lateralXnorm,
      (plan[i].halfWidth - margin) - plan[i].lateralXnorm)
    tempVec:setScaled2(plan[i].normal, offset)
    n.pos:setAdd(tempVec)
    n.lateralXnorm = n.lateralXnorm + offset

    if i > 1 then
      local n_1 = altPlan[i - 1]
      n.vec:setSub2(n_1.pos, n.pos);
      n.vec.z = 0
      n.dirVec:set(n.vec);
      n.dirVec:normalize()
      n_1.turnDir:setSub2(n_1.dirVec, n.dirVec);
      n_1.turnDir:normalize()
    end
  end

  updatePlanLen(altPlan, 2, altPlan.planCount)
end

local function minimalcopyPlan(originalPlan)
  local newPlan = {}

  -- Copy array elements
  for i = 1, #originalPlan do
    local original = originalPlan[i]
    newPlan[i] = {
      pos = vec3(original.pos),
      length = original.length,
      radiusOrig = original.radiusOrig,
      normal = vec3(original.normal),
      halfWidth = original.halfWidth,
      lateralXnorm = original.lateralXnorm,
      ---
      speed = original.speed,
      posOrig = vec3(original.posOrig),
      biNormal = vec3(original.biNormal),
      dirVec = vec3(original.dirVec),
      vec = vec3(original.vec),
      rangeLeft = original.rangeLeft,
      rangeRight = original.rangeRight,
      rangeLaneCount = original.rangeLaneCount,
      rangeBestLane = original.rangeBestLane, -- Avoid ?
      pathidx = original.pathidx,
      widthMarginOffset = original.widthMarginOffset, -- Avoid ?
      curvature = original.curvature,
      turnDir = original.turnDir, -- Avoid ?
      dispDir = original.dispDir, -- Avoid ?
      laneLimLeft = original.laneLimLeft,
      laneLimRight = original.laneLimRight,
      curvatureZ = 0,
      trafficSqVel = math.huge

    }
  end

  -- Copy plan metadata
  newPlan.planLen = originalPlan.planLen
  newPlan.planCount = originalPlan.planCount
  newPlan.egoXnormOnSeg = originalPlan.egoXnormOnSeg
  newPlan.egoSeg = originalPlan.egoSeg
  newPlan.stopSeg = math.huge
  newPlan.trafficMinProjSpeed = math.huge
  newPlan.targetSpeed = originalPlan.targetSpeed
  newPlan.trafficTargetSpeed = originalPlan.trafficTargetSpeed
  newPlan.targetSeg = originalPlan.targetSeg
  newPlan.targetPos = vec3(originalPlan.targetPos)
  newPlan.egoDeviation = originalPlan.egoDeviation
  newPlan.targetSpeedLegal = originalPlan.targetSpeedLegal -- Avoid ?

  return newPlan
end

local function side_avoidance(plan)
  plan.sideDisp = nil
  plan.dispLat = 0
  if #traffic.trafficTable > 0 then
    local dispLeft, dispRight = 0, 0
    local Lss, c_base, T_close, c_base_away, margin_static = ego.length, 0.3, 0.8, 0.3, 0.2
    local dP, dV, nvec = vec3(), vec3(), vec3()
    nvec:setScaled2(ego.rightVec, -1)

    for _, v in ipairs(traffic.trafficTable) do
      dP:setScaled2(ego.pos, -1)
      dV:setScaled2(ego.vel, -1)
      dP:setAdd(v.posFront)
      dV:setAdd(v.vel)
      local dS = dP:dot(ego.dirVec) -- longitudinal distance between ego vehicle and v-vehicle
      local dD = dP:dot(nvec) -- lateral distance between ego vehicle and v-vehicle
      local v_rel = dV:dot(nvec) -- lateral relative speed between ego vehicle and v-vehicle
      local a = math.exp(-square(dS / Lss)) -- Intensity factor
      -- a = a > 0.05 and a or 0
      a = dS < -1.2 * ego.length and 0 or a -- No action if other vehicle is far behind us
      local v_close = max(0, -sign(dD) * v_rel) -- Approaching speed, positive if approaching, zero otherwise
      local c_req = v_close > 0 and c_base + T_close * v_close or c_base_away -- compute desired free gap
      local half_sum = 0.5 * (ego.width + v.width) + margin_static
      local gap_free = max(0, abs(dD) - half_sum) -- compute current free gap
      local deficit = max(0, c_req - gap_free) -- compute missing free gap
      local contrib_mag = a * deficit -- scale missing free gap by intensity factor
      if contrib_mag > 0 then
        if dD >= 0 then
          dispLeft = contrib_mag > 0.05 and max(contrib_mag, dispLeft) or 0
        else
          dispRight = contrib_mag > 0.05 and max(contrib_mag, dispRight) or 0
        end
      end
    end
    plan.dispLat = dispLeft - dispRight -- store displacement. dispLat > 0 -> I want to move targetPos to the right, hence on tragetSeg.normal direction
  end
end

local function raceplanAhead(route, baseRoute, pmode)
  ----########## Intro Code #########-----
  if not route then
    return
  end

  if not route.path then
    route = createNewRoute(route)
  end
  -- load the plan that has to be updated ()
  local plan

  if pmode == 'left' then
    plan = route.planL
  elseif pmode == 'pp' then
    plan = route.planPP
  elseif pmode == 'right' then
    plan = route.planR
  else
    plan = route.plan
  end

  if baseRoute and not plan[1] then
    -- merge from base plan
    local bsrPlan = baseRoute.plan
    if bsrPlan[2] then
      local commonPathEnd
      route.path, commonPathEnd = mergePathPrefix(baseRoute.path, route.path, bsrPlan[2].pathidx)
      route.lastLaneChangeIdx = 2
      table.clear(route.laneChanges)
      route.pathLength = {0}
      if commonPathEnd >= 1 then
        local refpathidx = bsrPlan[2].pathidx - 1
        local planLen, planCount = 0, 0
        for i = 1, #bsrPlan do
          local n = bsrPlan[i]
          if n.pathidx > commonPathEnd then
            break
          end
          planLen = planLen + (n.length or 0)
          planCount = i

          plan[i] = {
            posOrig = vec3(n.posOrig),
            pos = vec3(n.pos),
            vec = vec3(n.vec),
            dirVec = vec3(n.dirVec),
            turnDir = vec3(n.turnDir),
            biNormal = vec3(n.biNormal),
            normal = vec3(n.normal),
            radiusOrig = n.radiusOrig,
            pathidx = max(1, n.pathidx - refpathidx),
            roadSpeedLimit = n.roadSpeedLimit,
            halfWidth = n.halfWidth,
            widthMarginOffset = n.widthMarginOffset,
            wayPointAhead = n.wayPointAhead,
            length = n.length,
            curvature = n.curvature,
            curvatureZ = n.curvatureZ,
            lateralXnorm = n.lateralXnorm,
            laneLimLeft = n.laneLimLeft,
            laneLimRight = n.laneLimRight,
            legalSpeed = nil,
            speed = nil,
            rangeLeft = n.rangeLeft,
            rangeRight = n.rangeRight,
            rangeLaneCount = n.rangeLaneCount,
            rangeBestLane = n.rangeBestLane,
            trafficSqVel = math.huge,
            dispDir = n.dispDir
          }
        end
        plan.planLen = planLen
        plan.planCount = planCount
        plan.egoDeviation = 0
        if plan[bsrPlan.targetSeg + 1] then
          plan.targetSeg = bsrPlan.targetSeg
          plan.targetPos = vec3(bsrPlan.targetPos)
          plan.egoSeg = bsrPlan.egoSeg
          plan.egoXnormOnSeg = bsrPlan.egoXnormOnSeg
        end
      end
    end
  end

  if not plan[1] then
    local posOrig = vec3(ego.pos)
    local radiusOrig = 2
    local normal = vec3(0, 0, 0)
    local latXnorm = 0
    local rangeLeft, rangeRight, rangeBestLane = 0, 1, 0
    local laneLimLeft, laneLimRight = -ego.width * 0.5, ego.width * 0.5
    local biNormal = mapmgr.surfaceNormalBelow(ego.pos, ego.width * 0.5);
    biNormal:setScaled(-1)
    local wp, lanes, roadSpeedLimit
    if ego.currentSegment[1] and ego.currentSegment[2] then
      local wp1 = route.path[1]
      local wp2
      if wp1 == ego.currentSegment[1] then
        wp2 = ego.currentSegment[2]
      elseif wp1 == ego.currentSegment[2] then
        wp2 = ego.currentSegment[1]
      end
      if wp2 and route.path[2] ~= wp2 then
        -- local pos1 = mapData.positions[wp1]
        -- local pos2 = mapData.positions[wp2]
        local pos1, pos2 = mapData:getEdgePositions(wp1, wp2)
        local xnorm = ego.pos:xnormOnLine(pos2, pos1)
        if xnorm >= 0 and xnorm <= 1 then
          local rad1, rad2 = mapData:getEdgeRadii(wp1, wp2)
          local rad = lerp(rad2, rad1, xnorm)
          posOrig = linePointFromXnorm(pos2, pos1, xnorm)
          normal:setCross(biNormal, pos1 - pos2);
          normal:normalize()
          radiusOrig = rad
          wp = wp2
          roadSpeedLimit = mapData.graph[wp1][wp2].speedLimit
          latXnorm = ego.pos:xnormOnLine(posOrig, posOrig + normal)
          laneLimLeft = latXnorm - ego.width * 0.5 -- TODO: rethink the limits here
          laneLimRight = latXnorm + ego.width * 0.5
          if opt.driveInLaneFlag then
            lanes = getEdgeLaneConfig(wp2, wp1)
            rangeLeft, rangeRight = laneRange(lanes)

            local normalizedLatXnorm = (latXnorm / radiusOrig + 1) * 0.5
            local normalizedLaneLimLeft = (laneLimLeft / radiusOrig + 1) * 0.5
            local normalizedLaneLimRight = (laneLimRight / radiusOrig + 1) * 0.5

            local bestLane
            bestLane, normalizedLaneLimLeft, normalizedLaneLimRight =
              getBestLane(lanes, normalizedLatXnorm, normalizedLaneLimLeft, normalizedLaneLimRight)

            local _, _, rangeLeftIdx, rangeRightIdx = getLaneRangeFromLane(lanes, bestLane)
            rangeBestLane = bestLane - rangeLeftIdx -- bestLane - rangeRightIdx

            laneLimLeft = (normalizedLaneLimLeft * 2 - 1) * radiusOrig
            laneLimRight = (normalizedLaneLimRight * 2 - 1) * radiusOrig
            latXnorm = (laneLimLeft + laneLimRight) * 0.5
          end
        end
      end
    end

    local rangeLaneCount = lanes and numOfLanesInDirection(lanes, "+") or 1 -- numOfLanesFromRadius(radiusOrig)
    local vec = vec3(-8 * ego.dirVec.x, -8 * ego.dirVec.y, 0)

    plan[1] = {
      posOrig = posOrig,
      pos = vec3(ego.pos),
      vec = vec,
      dirVec = vec:normalized(),
      turnDir = vec3(0, 0, 0),
      biNormal = biNormal,
      normal = normal,
      radiusOrig = radiusOrig,
      widthMarginOffset = 0,
      length = 0,
      curvature = 0,
      curvatureZ = 0,
      halfWidth = radiusOrig,
      lateralXnorm = latXnorm,
      laneLimLeft = laneLimLeft,
      laneLimRight = laneLimRight,
      rangeLeft = rangeLeft,
      rangeRight = rangeRight,
      rangeLaneCount = rangeLaneCount,
      rangeBestLane = rangeBestLane,
      pathidx = nil,
      roadSpeedLimit = roadSpeedLimit,
      legalSpeed = nil,
      speed = nil,
      wp = wp,
      trafficSqVel = math.huge,
      dispDir = 0
    }

    plan.planCount = 1
    plan.planLen = 0
    plan.egoXnormOnSeg = 0
    plan.egoDeviation = 0
  end

  -- estimate of the stopping distance assuming 0.2 g longitudinal acceleration
  local minPlanLen = clamp(0.5 * ego.speed * ego.speed / (0.2 * g), 40, 400)

  -- profilerPushEvent("ai_buildPlan")
  while not plan[MIN_PLAN_COUNT] or (plan.planLen - plan.egoXnormOnSeg * plan[1].length) < minPlanLen do -- TODO: (plan.planLen < minPlanLen and plan.stopSeg + 1 == plan.planCount)
    local n = buildNextRoute(route, plan)
    if not n then
      break
    end
    plan.planCount = plan.planCount + 1
    plan[plan.planCount] = n
    plan[plan.planCount - 1].length = n.pos:distance(plan[plan.planCount - 1].pos)
    plan.planLen = plan.planLen + plan[plan.planCount - 1].length
  end
  -- profilerPopEvent("ai_buildPlan")

  if not plan[2] then
    return
  end
  if not plan[1].pathidx then
    plan[1].pathidx = plan[2].pathidx
    plan[1].roadSpeedLimit = plan[2].roadSpeedLimit
  end

  -- Calculate the length of the path at each path node one segment per call of planAhead
  if not route.pathLength[#route.path] then
    local n = #route.pathLength
    route.pathLength[n + 1] = mapData.positions[route.path[n + 1]]:distance(mapData.positions[route.path[max(1, n)]]) +
                                route.pathLength[n]
  end

  -- subdivide plan segments that are longer than some (distance dependent) length.
  -- i.e. whether a segment will be subdivided depends on its distance (path length) from the start of the plan.
  -- at the minimum a segment will be subdivided if it is longer than 6m.
  -- Performs at most one subdivision per call of planAhead.
  local distOnPlan = 0
  -- profilerPushEvent("ai_trajectory_splitting")
  for i = 1, plan.planCount - 1 do
    local segLenSq = plan[i].posOrig:squaredDistance(plan[i + 1].posOrig)
    local xSq = square(distOnPlan)
    if min(segLenSq, square(plan[i].length)) > square(min(220, (25e-8 * xSq + 1e-5) * xSq + 6)) and distOnPlan < 550 then
      local n1, n2 = plan[i], plan[i + 1]

      local posOrig = n1.posOrig + n2.posOrig;
      posOrig:setScaled(0.5)
      local radiusOrig = (n1.radiusOrig + n2.radiusOrig) * 0.5 -- TODO: this might be inacurate since posOrig might not be halfway between n1.posOrig and n2.posOrig

      local biNormal = mapmgr.surfaceNormalBelow(posOrig, radiusOrig * 0.5);
      biNormal:setScaled(-1)

      -- Interpolated normals
      local normal = vec3()
      if segLenSq > square(2 * radiusOrig + n1.radiusOrig + n2.radiusOrig) then
        -- calculate normal from the direction vector of edge (i, i+1)
        normal:setSub2(plan[i + 1].pos, plan[i].pos)
        normal:setCross(biNormal, normal)
        normal:normalize()
      else
        -- calculate from adjacent normals
        local norm1 = plan[i].normal:cross(biNormal)
        norm1:setCross(biNormal, norm1);
        norm1:normalize()
        local norm2 = plan[i + 1].normal:cross(biNormal)
        norm2:setCross(biNormal, norm2);
        norm2:normalize()
        normal:setAdd2(norm1, norm2)
        normal:setScaled(1 / (normal:length() + 1e-30))
      end

      local pos = n1.pos + n2.pos;
      pos:setScaled(0.5)
      local vec = n1.pos - pos;
      vec.z = 0
      local dirVec = vec:normalized()

      local _, t2 = closestLinePoints(pos, pos + normal, n1.posOrig, n2.posOrig)
      posOrig = linePointFromXnorm(n1.posOrig, n2.posOrig, t2)
      local edgeNormal = biNormal:cross(n2.posOrig - n1.posOrig);
      edgeNormal:normalize()
      local _, t2 = closestLinePoints(posOrig, posOrig + normal, plan[i].posOrig + plan[i].radiusOrig * edgeNormal,
        plan[i + 1].posOrig + plan[i + 1].radiusOrig * edgeNormal)
      local limPos = linePointFromXnorm(plan[i].posOrig + plan[i].radiusOrig * edgeNormal,
        plan[i + 1].posOrig + plan[i + 1].radiusOrig * edgeNormal, max(0, min(1, t2)))
      local roadHalfWidth = posOrig:distance(limPos)
      local lateralXnorm = pos:xnormOnLine(posOrig, limPos) * roadHalfWidth -- [-r, r]

      n1.length = n1.length * 0.5

      n2.vec:set(vec)
      n2.dirVec:set(dirVec)

      -- local laneLimLeft = (n1.laneLimLeft / (n1.radiusOrig * n1.chordLength) + n2.laneLimLeft / (n2.radiusOrig * n2.chordLength)) * 0.5 * roadHalfWidth
      -- local laneLimRight = (n1.laneLimRight / (n1.radiusOrig * n1.chordLength) + n2.laneLimRight / (n2.radiusOrig * n2.chordLength)) * 0.5 * roadHalfWidth

      local rangeLeft = (n1.rangeLeft + n2.rangeLeft) * 0.5 -- lane range left boundary lateral coordinate in [0, 1]. 0 is left road boundary: always 0 when driveInLane is off
      local rangeRight = (n1.rangeRight + n2.rangeRight) * 0.5 -- lane range right boundary lateral coordinate in [0, 1]. 1 is right road boundary: always 1 when driveInLane is off
      local rangeLaneCount = (n1.rangeLaneCount + n2.rangeLaneCount) * 0.5 -- number of lanes in the range: always 1 when driveInLane is off

      local laneWidth = (rangeRight - rangeLeft) / rangeLaneCount -- self explanatory: entire width of the road when driveInLane is off i.e. 1
      local rangeBestLane = (n1.rangeBestLane + n2.rangeBestLane) * 0.5 -- best lane in the range: only one lane to pick from when driveInLane is off

      local laneLimLeft = linearScale(rangeLeft + rangeBestLane * laneWidth, 0, 1, -roadHalfWidth, roadHalfWidth) -- lateral coordinate of left boundary of lane rescaled to the road half width
      local laneLimRight = linearScale(rangeLeft + (rangeBestLane + 1) * laneWidth, 0, 1, -roadHalfWidth, roadHalfWidth) -- lateral coordinate of right boundary of lane rescaled to the road half width

      local roadSpeedLimit
      if n2.pathidx > 1 then
        roadSpeedLimit = mapData.graph[route.path[n2.pathidx]][route.path[n2.pathidx - 1]].speedLimit
      else
        roadSpeedLimit = n2.roadSpeedLimit
      end

      if plan.stopSeg >= i + 1 then
        plan.stopSeg = plan.stopSeg + 1
      end

      local manSpeed
      if n1.manSpeed and n2.manSpeed then
        manSpeed = (n1.manSpeed + n2.manSpeed) * 0.5
      end

      tableInsert(plan, i + 1, {
        posOrig = posOrig,
        pos = pos,
        vec = vec,
        dirVec = dirVec,
        turnDir = vec3(0, 0, 0),
        biNormal = biNormal,
        normal = normal,
        radiusOrig = radiusOrig,
        pathidx = n2.pathidx,
        roadSpeedLimit = roadSpeedLimit,
        halfWidth = roadHalfWidth,
        widthMarginOffset = (n1.widthMarginOffset + n2.widthMarginOffset) * 0.5,
        laneLimLeft = laneLimLeft,
        laneLimRight = laneLimRight,
        rangeLeft = rangeLeft,
        rangeRight = rangeRight,
        rangeLaneCount = rangeLaneCount,
        rangeBestLane = rangeBestLane, -- 0 Indexed
        length = n1.length,
        curvature = (n1.curvature + n2.curvature) * 0.5,
        curvatureZ = 0,
        lateralXnorm = lateralXnorm,
        legalSpeed = nil,
        manSpeed = manSpeed,
        speed = nil,
        trafficSqVel = math.huge,
        dispDir = 0
      })

      if n1.lanesOpen and n2.lanesOpen then
        openLaneToLaneRange(plan[i + 1])
      end

      plan.planCount = plan.planCount + 1
      break
    end
    distOnPlan = distOnPlan + sqrt(segLenSq)
  end
  -- profilerPopEvent("ai_trajectory_splitting")
  distOnPlan = nil

  if plan.targetSeg == nil then
    calculateTarget(plan)
  end

  ----#### Populate Traffic Table ####----
  table.clear(traffic.trafficTable)
  for plID, v in pairs(mapmgr.getObjects()) do
    if plID ~= objectId and (M.mode ~= 'chase' or plID ~= player.id or internalState.chaseData.playerState == 'stopped') then
      v.targetType = (player and plID == player.id) and M.mode
      if opt.avoidCars == 'on' or v.targetType == 'follow' then
        v.length = obj:getObjectInitialLength(plID) + 0.3
        v.width = obj:getObjectInitialWidth(plID)
        local posFront = obj:getObjectFrontPosition(plID)
        local dirVec = v.dirVec
        v.posFront = dirVec * 0.3 + posFront
        v.posRear = dirVec * (-v.length) + posFront
        v.posMiddle = (v.posFront + v.posRear) * 0.5
        local egoRear = ego.pos - ego.dirVec * ego.length
        local ego2v = v.posFront - egoRear
        table.insert(traffic.trafficTable, v)
      end
    end
  end
  plan.trafficMinProjSpeed = math.huge

  ----#### Side avoidance pt. 1####----
  plan.sideDisp = nil
  if pmode == 'pp' then
    side_avoidance(plan) -- compute displacement
    -- apply limited displacement by modifying the current plan (useful to avoid cars being stuck side by side when touching)
    if plan.dispLat ~= 0 then
      plan.sideDisp = true
      local sideDisp = plan.dispLat -- (sideDisp > 0) means v-vehicle is on our left side and ego should move right
      sideDisp = min(dt * parameters.awarenessForceCoef * 10, abs(sideDisp)) * sign2(sideDisp) -- limited displacement per frame
      local curDist = 0
      local lastPlanIdx = 2
      local targetDist = square(ego.speed) / (2 * g * aggression) + max(30, ego.speed * 3) -- longer adjustment at higher speeds
      local tmpVec = vec3()
      for i = 1, plan.planCount - 1 do
        openLaneToLaneRange(plan[i])
        plan[i].lateralXnorm = clamp(plan[i].lateralXnorm + sideDisp * (targetDist - curDist) / targetDist,
          plan[i].laneLimLeft - ego.width * 0.6, plan[i].laneLimRight + ego.width * 0.6)
        tmpVec:setScaled2(plan[i].normal, plan[i].lateralXnorm)
        plan[i].pos:setAdd2(plan[i].posOrig, tmpVec)
        curDist = curDist + (i > 1 and plan[i - 1].length or 0)
        lastPlanIdx = i

        if i > 1 then
          plan[i].vec:setSub2(plan[i - 1].pos, plan[i].pos);
          plan[i].vec.z = 0
        end
        plan[i].dirVec:set(plan[i].vec)
        plan[i].dirVec:normalize()

        if curDist > targetDist then
          break
        end
      end

      updatePlanLen(plan, 2, lastPlanIdx + 1)
    end
  end

  plan.hitLimit = false
  -- detect if plan is moved to close to road limit
  if pmode == 'pp' then
    plan.hitLimit = min(abs((abs(plan[2].lateralXnorm) - plan[2].halfWidth)),
      abs((abs(plan[1].lateralXnorm) - plan[1].halfWidth))) / (0.5 * ego.width) < 1
  end
  -- calculate spring forces
  ------######### Compute Forces ########---------
  -- profilerPushEvent("ai_calculate_smoothing")
  for i = 0, plan.planCount do
    if forces[i] then
      if pmode == 'pp' and route.plan_index and i > 0 and currentRoute.offset then
        forces[i] = plan[i].normal * currentRoute.offset
      else
        forces[i]:set(0, 0, 0)
      end
    else
      forces[i] = vec3()
    end
  end

  local nforce = vec3()

  for i = 1, plan.planCount - 1 do
    local n1 = plan[i]
    local v1 = n1.dirVec
    local v2 = plan[i + 1].dirVec

    n1.turnDir:setSub2(v1, v2);
    n1.turnDir:normalize()
    nforce:setScaled2(n1.turnDir, (1 - twt.state) * max(1 - v1:dot(v2), 0) * parameters.turnForceCoef)

    forces[i + 1]:setSub(nforce)
    forces[i - 1]:setSub(nforce)
    nforce:setScaled(2)
    forces[i]:setAdd(nforce)
  end

  -- profilerPopEvent("ai_calculate_smoothing")

  ------######### Smoothing ##########----------
  -- profilerPushEvent("ai_smoothness_integration")
  local tmpVec = vec3()
  local roadWidthMargin = ego.width * 0.6
  local laneWidthMargin = ego.width * 0.6
  local forceMagLim = parameters.springForceIntegratorDispLim
  local skipLast = 1
  for i = 2, plan.planCount - skipLast do
    local n = plan[i]
    local roadHalfWidth = n.halfWidth

    -- Calculate node displacement: avoids switching displacement direction (sign) in concecutive frames
    local displacement = max(min(n.normal:dot(forces[i]), forceMagLim), -forceMagLim)
    displacement = displacement * max(min(displacement * n.dispDir * math.huge, 1), 0) -- second term returns 0 if (displacement * n.dispDir) is negative, 1 otherwise.
    n.dispDir = sign(displacement)

    local roadLimRight = max(0, roadHalfWidth - roadWidthMargin) -- should be non-negative (zero or positive)
    local newLateralXnorm = clamp(n.lateralXnorm + displacement,
      max(n.laneLimLeft + laneWidthMargin, -roadLimRight) + max(0, plan.offset or 0),
      min(n.laneLimRight - laneWidthMargin, roadLimRight) + min(0, plan.offset or 0))

    tmpVec:setScaled2(n.normal, newLateralXnorm - n.lateralXnorm)
    n.pos:setAdd(tmpVec) -- remember that posOrig and pos are not alligned along the normal
    n.vec:setSub2(plan[i - 1].pos, n.pos);
    n.vec.z = 0
    n.dirVec:set(n.vec);
    n.dirVec:normalize()

    n.lateralXnorm = newLateralXnorm
  end
  -- profilerPopEvent("ai_smoothness_integration")

  updatePlanLen(plan, 2, plan.planCount)

  -------########## Error Distribution ##########---------
  -- profilerPopEvent("ai_error_smoother")
  if pmode == 'pp' then -- adjust plan error for current plan
    uniformPlanErrorDistribution(plan)
  end

  calculateTarget(plan)

  -- ######side avoidance pt. 2####### --
  if plan.sideDisp and not plan.hitLimit then
    -- move targetPos to ensure a side avoidance actions in terms of steering (pt. 1 is not moving it too much due to limited displacement per frame)
    local targetNode = plan[plan.targetSeg]
    local newdisp = clamp(targetNode.lateralXnorm + plan.dispLat, max(targetNode.laneLimLeft + laneWidthMargin,
      -max(0, targetNode.halfWidth - roadWidthMargin)), min(targetNode.laneLimRight - laneWidthMargin,
      max(0, targetNode.halfWidth - roadWidthMargin)))
    local dispLat = newdisp - targetNode.lateralXnorm
    plan.targetPos = plan.targetPos + plan[plan.targetSeg].normal * dispLat
  end
  -- profilerPopEvent("ai_calculate_target")

  -----###### calculate node horizontal curvature ######------
  -- profilerPushEvent("ai_calculate_curvature")
  local len, n3vec = 0, vec3()
  plan[1].curvature = plan[1].curvature or inCurvature(plan[1].vec, plan[2].vec)
  for i = 2, plan.planCount - 1 do
    local n1, n2 = plan[i], plan[i + 1]

    n3vec:setSub2(n1.pos, plan[min(plan.planCount, i + 2)].pos);
    n3vec.z = 0

    local c1 = inCurvature(n1.vec, n2.vec)
    local c2 = inCurvature(n1.vec, n3vec)

    local curvature
    if c1 <= c2 then
      curvature = sign2(n1.turnDir:dot(n1.normal)) * c1
    else
      curvature = sign2((n1.vec):dot(n1.normal) - n3vec:dot(n1.normal)) * c2
    end

    -- calculate curvature temporal smoothing parameter as a function of the trajectory length distance
    local curvatureRateDt = min(5 + 0.005 * len * len, 100) * dt
    n1.curvature = n1.curvature + (curvature - n1.curvature) * curvatureRateDt / (1 + curvatureRateDt)

    len = len + n1.length
  end
  -- profilerPopEvent("ai_calculate_curvature")

  ------######## Speed Planning ########-----
  local totalAccel = min(aggression, ego.staticFrictionCoef) * g

  local lastNode = plan[plan.planCount]
  if route.path[lastNode.pathidx + 1] or (loopPath and noOfLaps and noOfLaps > 1) then
    if plan.stopSeg <= plan.planCount then
      lastNode.speed = 0
    else
      lastNode.speed = lastNode.manSpeed or sqrt(2 * 550 * totalAccel) -- shouldn't this be calculated based on the path length remaining?
    end
  else
    lastNode.speed = lastNode.manSpeed or 0
  end
  lastNode.roadSpeedLimit = plan[plan.planCount - 1].roadSpeedLimit
  lastNode.legalSpeed = min(lastNode.roadSpeedLimit or math.huge, lastNode.speed)

  -- Use Backward or Forward + Backward algorithm
  -- profilerPushEvent('ai_speedProfile')
  local gT = vec3()
  for i = 1, plan.planCount - 1 do -- curvature is not defined on the last plan node
    local n1, n2 = plan[i], plan[i + 1]
    -- consider inclination
    gT:setSub2(n2.pos, n1.pos);
    gT:setScaled(gravityDir:dot(gT) / max(square(n1.length), 1e-30)) -- gravity vec parallel to road segment: positive when downhill
    local gN = gravityDir:distance(gT) -- gravity component normal to road segment
    n1.acc_max = totalAccel * gN
    n1.speed = min(n1.acc_max / max(abs(n1.curvature), 1e-30), gN * g / max(n1.curvatureZ, 1e-30)) -- available centripetal acceleration * radius
  end
  plan[plan.planCount].acc_max = plan[plan.planCount - 1].acc_max

  if speedProfileMode == 'ForwBack' then
    solver_f_acc_profile(plan)
  end

  solver_b_acc_profile(plan)

  -- profilerPopEvent('ai_speedProfile')

  plan.targetSpeed = plan[1].speed + max(0, plan.egoXnormOnSeg) * (plan[2].speed - plan[1].speed)
  plan.targetSpeed = targetSpeedSmoother:get(plan.targetSpeed, dt)
  if M.speedMode == 'legal' then
    plan.targetSpeedLegal = plan[1].legalSpeed + max(0, plan.egoXnormOnSeg) * (plan[2].legalSpeed - plan[1].legalSpeed)
  else
    plan.targetSpeedLegal = math.huge
  end

  calculateTrafficTargetSpeed(plan, traffic.trafficTable, #traffic.trafficTable, pmode, route)
  plan.originaltargetSpeed = plan.targetSpeed -- save target speed computed by geometry only
  plan.targetSpeed = min(plan.targetSpeed, plan.trafficTargetSpeed)

  ------######## Return #########--------
  return route
end

local function planAhead(route, baseRoute)
  if not route then
    return
  end

  if not route.path then
    route = createNewRoute(route)
  end

  local plan = route.plan

  if baseRoute and not plan[1] then
    -- merge from base plan
    local bsrPlan = baseRoute.plan
    if bsrPlan[2] then
      local commonPathEnd
      route.path, commonPathEnd = mergePathPrefix(baseRoute.path, route.path, bsrPlan[2].pathidx)
      route.lastLaneChangeIdx = 2
      table.clear(route.laneChanges)
      route.pathLength = {0}
      if commonPathEnd >= 1 then
        local refpathidx = bsrPlan[2].pathidx - 1
        local planLen, planCount = 0, 0
        for i = 1, #bsrPlan do
          local n = bsrPlan[i]
          if n.pathidx > commonPathEnd then
            break
          end
          planLen = planLen + (n.length or 0)
          planCount = i

          plan[i] = {
            posOrig = vec3(n.posOrig),
            pos = vec3(n.pos),
            vec = vec3(n.vec),
            dirVec = vec3(n.dirVec),
            turnDir = vec3(n.turnDir),
            biNormal = vec3(n.biNormal),
            normal = vec3(n.normal),
            radiusOrig = n.radiusOrig,
            pathidx = max(1, n.pathidx - refpathidx),
            roadSpeedLimit = n.roadSpeedLimit,
            halfWidth = n.halfWidth,
            widthMarginOffset = n.widthMarginOffset,
            wayPointAhead = n.wayPointAhead,
            length = n.length,
            curvature = n.curvature,
            curvatureZ = n.curvatureZ,
            lateralXnorm = n.lateralXnorm,
            laneLimLeft = n.laneLimLeft,
            laneLimRight = n.laneLimRight,
            legalSpeed = nil,
            speed = nil,
            rangeLeft = n.rangeLeft,
            rangeRight = n.rangeRight,
            rangeLaneCount = n.rangeLaneCount,
            rangeBestLane = n.rangeBestLane,
            trafficSqVel = math.huge,
            dispDir = n.dispDir
          }
        end
        plan.planLen = planLen
        plan.planCount = planCount
        plan.egoDeviation = 0
        if plan[bsrPlan.targetSeg + 1] then
          plan.targetSeg = bsrPlan.targetSeg
          plan.targetPos = vec3(bsrPlan.targetPos)
          plan.egoSeg = bsrPlan.egoSeg
          plan.egoXnormOnSeg = bsrPlan.egoXnormOnSeg
        end
      end
    end
  end

  if not plan[1] then
    local posOrig = vec3(ego.pos)
    local radiusOrig = 2
    local normal = vec3(0, 0, 0)
    local latXnorm = 0
    local rangeLeft, rangeRight, rangeBestLane = 0, 1, 0
    local laneLimLeft, laneLimRight = -ego.width * 0.5, ego.width * 0.5
    local biNormal = mapmgr.surfaceNormalBelow(ego.pos, ego.width * 0.5);
    biNormal:setScaled(-1)
    local wp, lanes, roadSpeedLimit
    if ego.currentSegment[1] and ego.currentSegment[2] then
      local wp1 = route.path[1]
      local wp2
      if wp1 == ego.currentSegment[1] then
        wp2 = ego.currentSegment[2]
      elseif wp1 == ego.currentSegment[2] then
        wp2 = ego.currentSegment[1]
      end
      if wp2 and route.path[2] ~= wp2 then
        -- local pos1 = mapData.positions[wp1]
        -- local pos2 = mapData.positions[wp2]
        local pos1, pos2 = mapData:getEdgePositions(wp1, wp2)
        local xnorm = ego.pos:xnormOnLine(pos2, pos1)
        if xnorm >= 0 and xnorm <= 1 then
          local rad1, rad2 = mapData:getEdgeRadii(wp1, wp2)
          local rad = lerp(rad2, rad1, xnorm)
          posOrig = linePointFromXnorm(pos2, pos1, xnorm)
          normal:setCross(biNormal, pos1 - pos2);
          normal:normalize()
          radiusOrig = rad
          wp = wp2
          roadSpeedLimit = mapData.graph[wp1][wp2].speedLimit
          latXnorm = ego.pos:xnormOnLine(posOrig, posOrig + normal)
          laneLimLeft = latXnorm - ego.width * 0.5 -- TODO: rethink the limits here
          laneLimRight = latXnorm + ego.width * 0.5
          if opt.driveInLaneFlag then
            lanes = getEdgeLaneConfig(wp2, wp1)
            rangeLeft, rangeRight = laneRange(lanes)

            local normalizedLatXnorm = (latXnorm / radiusOrig + 1) * 0.5
            local normalizedLaneLimLeft = (laneLimLeft / radiusOrig + 1) * 0.5
            local normalizedLaneLimRight = (laneLimRight / radiusOrig + 1) * 0.5

            local bestLane
            bestLane, normalizedLaneLimLeft, normalizedLaneLimRight =
              getBestLane(lanes, normalizedLatXnorm, normalizedLaneLimLeft, normalizedLaneLimRight)

            local _, _, rangeLeftIdx, rangeRightIdx = getLaneRangeFromLane(lanes, bestLane)
            rangeBestLane = bestLane - rangeLeftIdx -- bestLane - rangeRightIdx

            laneLimLeft = (normalizedLaneLimLeft * 2 - 1) * radiusOrig
            laneLimRight = (normalizedLaneLimRight * 2 - 1) * radiusOrig
            latXnorm = (laneLimLeft + laneLimRight) * 0.5
          end
        end
      end
    end

    local rangeLaneCount = lanes and numOfLanesInDirection(lanes, "+") or 1 -- numOfLanesFromRadius(radiusOrig)
    local vec = vec3(-8 * ego.dirVec.x, -8 * ego.dirVec.y, 0)

    plan[1] = {
      posOrig = posOrig,
      pos = vec3(ego.pos),
      vec = vec,
      dirVec = vec:normalized(),
      turnDir = vec3(0, 0, 0),
      biNormal = biNormal,
      normal = normal,
      radiusOrig = radiusOrig,
      widthMarginOffset = 0,
      length = 0,
      curvature = 0,
      curvatureZ = 0,
      halfWidth = radiusOrig,
      lateralXnorm = latXnorm,
      laneLimLeft = laneLimLeft,
      laneLimRight = laneLimRight,
      rangeLeft = rangeLeft,
      rangeRight = rangeRight,
      rangeLaneCount = rangeLaneCount,
      rangeBestLane = rangeBestLane,
      pathidx = nil,
      roadSpeedLimit = roadSpeedLimit,
      legalSpeed = nil,
      speed = nil,
      wp = wp,
      trafficSqVel = math.huge,
      dispDir = 0
    }

    plan.planCount = 1
    plan.planLen = 0
    plan.egoXnormOnSeg = 0
    plan.egoDeviation = 0
  end

  local minPlanLen
  if M.mode == 'traffic' then
    minPlanLen = getMinPlanLen(20, ego.speed, 0.2 * ego.staticFrictionCoef) -- 0.25 * min(aggression, ego.staticFrictionCoef)
  else
    minPlanLen = getMinPlanLen(40)
  end

  -- profilerPushEvent("ai_buildPlan")
  while not plan[MIN_PLAN_COUNT] or (plan.planLen - (plan.egoXnormOnSeg or 0) * plan[1].length) < minPlanLen do -- TODO: (plan.planLen < minPlanLen and plan.stopSeg + 1 == plan.planCount)
    local n = buildNextRoute(route)
    if not n then
      break
    end
    plan.planCount = plan.planCount + 1
    plan[plan.planCount] = n
    plan[plan.planCount - 1].length = n.pos:distance(plan[plan.planCount - 1].pos)
    plan.planLen = plan.planLen + plan[plan.planCount - 1].length
  end
  -- profilerPopEvent("ai_buildPlan")

  if not plan[2] then
    return
  end
  if not plan[1].pathidx then
    plan[1].pathidx = plan[2].pathidx
    plan[1].roadSpeedLimit = plan[2].roadSpeedLimit
  end

  -- Calculate the length of the path at each path node one segment per call of planAhead
  if not route.pathLength[#route.path] then
    local n = #route.pathLength
    table.insert(route.pathLength, mapData.positions[route.path[n + 1]]:distance(
      mapData.positions[route.path[max(1, n)]]) + route.pathLength[n])
  end

  -- check path node at lastLaneChangeIdx for a possible lane change
  -- profilerPushEvent("ai_find_LaneChanges")
  if route.lastLaneChangeIdx < #route.path then
    local wp1, wp2
    if route.lastLaneChangeIdx == 1 then
      wp2 = route.path[route.lastLaneChangeIdx]
      if plan[1].wp and (plan[1].wp ~= wp2) then
        wp1 = plan[1].wp
      else
        -- wp1 not found: safeguard
        route.lastLaneChangeIdx = 2
        wp1, wp2 = route.path[route.lastLaneChangeIdx - 1], route.path[route.lastLaneChangeIdx]
      end
    else
      wp1, wp2 = route.path[route.lastLaneChangeIdx - 1], route.path[route.lastLaneChangeIdx]
    end
    if route.lastLaneChangeIdx < #route.path then
      if numOfLanesInDirection(getEdgeLaneConfig(wp1, wp2), '+') > 1 then
        local minNode = roadNaturalContinuation(wp1, wp2)
        local wp3 = route.path[route.lastLaneChangeIdx + 1]
        if minNode and minNode ~= wp3 then -- road natural continuation at wp2 is not in our path
          local minNodePos, wp2Pos_2 = mapData:getEdgePositions(minNode, wp2)
          local minNodeEdgeVec = minNodePos - wp2Pos_2
          local wp1Pos, wp2Pos_1 = mapData:getEdgePositions(wp1, wp2)
          local wp1TOwp2EdgeVec = wp2Pos_1 - wp1Pos
          if minNodeEdgeVec:dot(wp1TOwp2EdgeVec) > 0 then -- road natural continuation is up to 90 deg
            local edgeNormal = gravityDir:cross(minNodePos - wp2Pos_2):normalized()
            local wp3Pos = mapData:getEdgePositions(wp3, wp2)
            local side = sign2(edgeNormal:dot(wp3Pos) - edgeNormal:dot(minNodePos))
            table.insert(route.laneChanges, {
              pathIdx = route.lastLaneChangeIdx,
              side = side,
              alternate = minNode
            })
          end
        end
      end
      route.lastLaneChangeIdx = route.lastLaneChangeIdx + 1
    end
  end
  -- profilerPopEvent("ai_find_LaneChanges")

  local distOnPlan = 0
  -- profilerPushEvent("ai_trajectory_splitting")
  for i = 1, plan.planCount - 1 do
    local segLenSq = plan[i].posOrig:squaredDistance(plan[i + 1].posOrig)
    local xSq = square(distOnPlan)
    if min(segLenSq, square(plan[i].length)) > square(min(220, (25e-8 * xSq + 1e-5) * xSq + 6)) and distOnPlan < 550 then
      local n1, n2 = plan[i], plan[i + 1]

      local posOrig = n1.posOrig + n2.posOrig;
      posOrig:setScaled(0.5)
      local radiusOrig = (n1.radiusOrig + n2.radiusOrig) * 0.5 -- TODO: this might be inacurate since posOrig might not be halfway between n1.posOrig and n2.posOrig

      local biNormal = mapmgr.surfaceNormalBelow(posOrig, radiusOrig * 0.5);
      biNormal:setScaled(-1)

      -- Interpolated normals
      local normal = vec3()
      if segLenSq > square(2 * radiusOrig + n1.radiusOrig + n2.radiusOrig) then
        -- calculate normal from the direction vector of edge (i, i+1)
        normal:setSub2(plan[i + 1].pos, plan[i].pos)
        normal:setCross(biNormal, normal)
        normal:normalize()
      else
        -- calculate from adjacent normals
        local norm1 = plan[i].normal:cross(biNormal)
        norm1:setCross(biNormal, norm1);
        norm1:normalize()
        local norm2 = plan[i + 1].normal:cross(biNormal)
        norm2:setCross(biNormal, norm2);
        norm2:normalize()
        normal:setAdd2(norm1, norm2)
        normal:setScaled(1 / (normal:length() + 1e-30))
      end

      local pos = n1.pos + n2.pos;
      pos:setScaled(0.5)
      local vec = n1.pos - pos;
      vec.z = 0
      local dirVec = vec:normalized()

      local _, t2 = closestLinePoints(pos, pos + normal, n1.posOrig, n2.posOrig)
      posOrig = linePointFromXnorm(n1.posOrig, n2.posOrig, t2)
      local edgeNormal = biNormal:cross(n2.posOrig - n1.posOrig);
      edgeNormal:normalize()
      local _, t2 = closestLinePoints(posOrig, posOrig + normal, plan[i].posOrig + plan[i].radiusOrig * edgeNormal,
        plan[i + 1].posOrig + plan[i + 1].radiusOrig * edgeNormal)
      local limPos = linePointFromXnorm(plan[i].posOrig + plan[i].radiusOrig * edgeNormal,
        plan[i + 1].posOrig + plan[i + 1].radiusOrig * edgeNormal, max(0, min(1, t2)))
      local roadHalfWidth = posOrig:distance(limPos)
      local lateralXnorm = pos:xnormOnLine(posOrig, limPos) * roadHalfWidth -- [-r, r]

      n1.length = n1.length * 0.5

      n2.vec:set(vec)
      n2.dirVec:set(dirVec)

      -- local laneLimLeft = (n1.laneLimLeft / (n1.radiusOrig * n1.chordLength) + n2.laneLimLeft / (n2.radiusOrig * n2.chordLength)) * 0.5 * roadHalfWidth
      -- local laneLimRight = (n1.laneLimRight / (n1.radiusOrig * n1.chordLength) + n2.laneLimRight / (n2.radiusOrig * n2.chordLength)) * 0.5 * roadHalfWidth

      local rangeLeft = (n1.rangeLeft + n2.rangeLeft) * 0.5 -- lane range left boundary lateral coordinate in [0, 1]. 0 is left road boundary: always 0 when driveInLane is off
      local rangeRight = (n1.rangeRight + n2.rangeRight) * 0.5 -- lane range right boundary lateral coordinate in [0, 1]. 1 is right road boundary: always 1 when driveInLane is off
      local rangeLaneCount = (n1.rangeLaneCount + n2.rangeLaneCount) * 0.5 -- number of lanes in the range: always 1 when driveInLane is off

      local laneWidth = (rangeRight - rangeLeft) / rangeLaneCount -- self explanatory: entire width of the road when driveInLane is off i.e. 1
      local rangeBestLane = (n1.rangeBestLane + n2.rangeBestLane) * 0.5 -- best lane in the range: only one lane to pick from when driveInLane is off

      local laneLimLeft = linearScale(rangeLeft + rangeBestLane * laneWidth, 0, 1, -roadHalfWidth, roadHalfWidth) -- lateral coordinate of left boundary of lane rescaled to the road half width
      local laneLimRight = linearScale(rangeLeft + (rangeBestLane + 1) * laneWidth, 0, 1, -roadHalfWidth, roadHalfWidth) -- lateral coordinate of right boundary of lane rescaled to the road half width

      local roadSpeedLimit
      if n2.pathidx > 1 then
        roadSpeedLimit = mapData.graph[route.path[n2.pathidx]][route.path[n2.pathidx - 1]].speedLimit
      else
        roadSpeedLimit = n2.roadSpeedLimit
      end

      if plan.stopSeg >= i + 1 then
        plan.stopSeg = plan.stopSeg + 1
      end

      local manSpeed
      if n1.manSpeed and n2.manSpeed then
        manSpeed = (n1.manSpeed + n2.manSpeed) * 0.5
      end

      tableInsert(plan, i + 1, {
        posOrig = posOrig,
        pos = pos,
        vec = vec,
        dirVec = dirVec,
        turnDir = vec3(0, 0, 0),
        biNormal = biNormal,
        normal = normal,
        radiusOrig = radiusOrig,
        pathidx = n2.pathidx,
        roadSpeedLimit = roadSpeedLimit,
        halfWidth = roadHalfWidth,
        widthMarginOffset = (n1.widthMarginOffset + n2.widthMarginOffset) * 0.5,
        laneLimLeft = laneLimLeft,
        laneLimRight = laneLimRight,
        rangeLeft = rangeLeft,
        rangeRight = rangeRight,
        rangeLaneCount = rangeLaneCount,
        rangeBestLane = rangeBestLane, -- 0 Indexed
        length = n1.length,
        curvature = (n1.curvature + n2.curvature) * 0.5,
        curvatureZ = 0,
        lateralXnorm = lateralXnorm,
        legalSpeed = nil,
        manSpeed = manSpeed,
        speed = nil,
        trafficSqVel = math.huge,
        dispDir = 0
      })

      if n1.lanesOpen and n2.lanesOpen then
        openLaneToLaneRange(plan[i + 1])
      end

      plan.planCount = plan.planCount + 1
      break
    end
    distOnPlan = distOnPlan + sqrt(segLenSq)
  end
  -- profilerPopEvent("ai_trajectory_splitting")
  distOnPlan = nil

  if plan.targetSeg == nil then
    calculateTarget(plan)
  end

  -- profilerPushEvent("ai_calculate_smoothing")
  -- calculate spring forces
  local nforce = vec3()
  if opt.trajMethod ~= 'springDampers' then
    for i = 0, plan.planCount do
      if forces[i] then
        forces[i]:set(0, 0, 0)
      else
        forces[i] = vec3()
      end
    end

    for i = 1, plan.planCount - 1 do
      local n1 = plan[i]
      local v1 = n1.dirVec
      local v2 = plan[i + 1].dirVec

      n1.turnDir:setSub2(v1, v2);
      n1.turnDir:normalize()
      nforce:setScaled2(n1.turnDir, (1 - twt.state) * max(1 - v1:dot(v2), 0) * parameters.turnForceCoef)

      forces[i + 1]:setSub(nforce)
      forces[i - 1]:setSub(nforce)
      nforce:setScaled(2)
      forces[i]:setAdd(nforce)
    end
  else
    for i = 0, plan.planCount do
      if forces[i] then
        forces[i]:set(0, 0, 0)
      else
        forces[i] = vec3()
        velocities[i] = vec3()
      end
    end

    local dforce = vec3()
    local stiff = 400
    local damper = 2
    for i = 1, plan.planCount - 1 do
      local n1 = plan[i]
      local v1 = n1.dirVec
      local v2 = plan[i + 1].dirVec

      n1.turnDir:setSub2(v1, v2);
      n1.turnDir:normalize()
      nforce:setScaled2(n1.turnDir, (1 - twt.state) * max(1 - v1:dot(v2), 0) * parameters.turnForceCoef)

      nforce:setScaled2(n1.turnDir, (1 - twt.state) * max(1 - v1:dot(v2), 0) * parameters.turnForceCoef * stiff)
      dforce:setScaled2((velocities[i + 1] - velocities[i]) - (velocities[i] - velocities[i - 1]),
        parameters.turnForceCoef * damper * 0.25)
      dforce:setScaled2(n1.turnDir, n1.turnDir:dot(dforce))
      nforce:setAdd(dforce)

      forces[i + 1]:setSub(nforce)
      forces[i - 1]:setSub(nforce)
      nforce:setScaled(2)
      forces[i]:setAdd(nforce)
    end

    for i = 1, plan.planCount - 1 do
      forces[i] = forces[i] - velocities[i] * parameters.turnForceCoef * damper
      velocities[i] = velocities[i] + forces[i] * dt
      forces[i] = velocities[i] * dt
    end
  end
  -- profilerPopEvent("ai_calculate_smoothing")

  -- other vehicle awareness --
  -- computing path indexes (start/final) for trafficFilter
  -- and computing filter radius
  -- profilerPushEvent("ai_awareness")
  local indexes = {
    start = plan[1].pathidx,
    final = plan[plan.planCount].pathidx
  }
  if traffic.filtered then
    traffic.Rfs = traffic.Rfs_fun(ego.speed)
    traffic.Rfl = traffic.Rfl_fun(ego.speed)
    traffic.distAhead = traffic.Rfs
    local dist = plan.planLen
    local final = indexes.final
    while (dist < traffic.distAhead or final < indexes.start + 1) and route.path[final + 1] do
      dist = dist + mapData.positions[route.path[final]]:distance(mapData.positions[route.path[final + 1]])
      final = final + 1
    end
    -- obj.debugDrawProxy:drawSphere(2, mapData.positions[route.path[final]], color(0,255,0,50))
    indexes.final = final
  end

  table.clear(traffic.trafficTable)
  ego.ghostR, ego.ghostL = nil, nil

  for plID, v in pairs(mapmgr.getObjects()) do
    if plID ~= objectId and (M.mode ~= 'chase' or plID ~= player.id or internalState.chaseData.playerState == 'stopped') then
      v.targetType = (player and plID == player.id) and M.mode
      if opt.avoidCars == 'on' or v.targetType == 'follow' then
        v.length = obj:getObjectInitialLength(plID) + 0.3
        v.width = obj:getObjectInitialWidth(plID)
        local posFront = obj:getObjectFrontPosition(plID)
        local dirVec = v.dirVec
        v.posFront = dirVec * 0.3 + posFront
        v.posRear = dirVec * (-v.length) + posFront
        v.posMiddle = (v.posFront + v.posRear) * 0.5
        -- obj.debugDrawProxy:drawSphere(traffic.Rfs, ego.pos, color(0,0,0,50))
        -- obj.debugDrawProxy:drawSphere(traffic.Rfl, ego.pos, color(255,0,0,50))

        if traffic.filtered then
          local dist = ego.pos:squaredDistance(v.posMiddle)
          if dist <= traffic.Rfs * traffic.Rfs then
            -- process with trafficFilter vehicles inside smaller radius
            if trafficFilter(indexes, route, v, traffic.vdraw) then
              table.insert(traffic.trafficTable, v)
            end
            -- be sure to not have this vehicle inside Que
            traffic.trafficFilterQue.dict[plID] = nil
            -- set FromQue to nil (means that this vehicle was not taken from Que)
            traffic.trafficFilterFromQue[plID] = nil
          elseif dist <= traffic.Rfl * traffic.Rfl then
            -- process with trafficFilter vehicles inside larger radius
            if traffic.trafficFilterFromQue[plID] then
              -- if vehicle (at the previous frame) was taken from Que, then process it again with trafficFilter
              if trafficFilter(indexes, route, v, traffic.vdraw) then
                table.insert(traffic.trafficTable, v)
              end
              -- be sure to not have this vehicle inside Que
              traffic.trafficFilterQue.dict[plID] = nil
            else
              -- if it is not has been processed from que, then add it to que if it is not already inside
              if not traffic.trafficFilterQue.dict[plID] then
                table.insert(traffic.trafficFilterQue.array, plID)
                traffic.trafficFilterQue.dict[plID] = true
                traffic.trafficFilterFromQue[plID] = nil
              end
            end
          else
            -- process vehicles outside of both radii
            traffic.trafficFilterQue.dict[plID] = nil
            traffic.trafficFilterFromQue[plID] = nil
          end
        else -- no filtered case (take all vehicles)
          table.insert(traffic.trafficTable, v)
        end
      end
    end
  end

  if traffic.filtered then
    local tFQarray, tFQdict = traffic.trafficFilterQue.array, traffic.trafficFilterQue.dict
    -- cleanup trafficFilterQue
    local j = 1
    for i = 1, #tFQarray do
      local vid = tFQarray[i]
      if tFQdict[vid] and mapmgr.objects[vid] and mapmgr.objects[vid].posFront then --
        if i ~= j then
          tFQarray[j] = tFQarray[i]
          tFQarray[i] = nil
        end
        j = j + 1
      else
        tFQarray[i], tFQdict[vid] = nil, nil
      end
    end

    -- pop que and check vehicle
    if tFQarray[1] and (traffic.frameQue * dt >= traffic.rateQue) then
      local vid = table.remove(tFQarray, 1)
      tFQdict[vid] = nil
      local v = mapmgr.objects[vid]
      -- process the first vehicle in Que
      if trafficFilter(indexes, route, v, traffic.vdraw) then
        -- set FromQue to true (means that this vehicle was taken from Que)
        traffic.trafficFilterFromQue[vid] = true
        table.insert(traffic.trafficTable, v)
      end
      traffic.frameQue = 0
    end
  end

  local trafficTableLen = #traffic.trafficTable
  plan.trafficMinProjSpeed = math.huge

  local newOvertake = false
  local openPlanLanesIdx = 0
  if trafficTableLen > 0 then
    local fl, rl, fr, rr
    local lenVec = ego.dirVec * ego.length
    local midPos = ego.pos - lenVec * 0.5
    -- local planPos = linePointFromXnorm(plan[1].pos, plan[2].pos, plan.egoXnormOnSeg or 0)
    -- midPos = linePointFromXnorm(planPos, planPos - lenVec, midPos:xnormOnLine(planPos, planPos - lenVec)) -- offset as next plan node
    local dispLeft, dispRight = 0, 0

    if opt.racing then
      for _, v in ipairs(traffic.trafficTable) do -- side avoidance loop
        local backAwereness = 0 -- 0.25 -- must be between 0 and 1
        local xnormF = v.posFront:xnormOnLine(midPos + lenVec, midPos - backAwereness * lenVec)
        local xnormR = v.posRear:xnormOnLine(midPos + lenVec, midPos - backAwereness * lenVec)
        local xnormL = xnormF > 0 and xnormF or xnormR
        if ego.speed > 1 and v.vel:dot(ego.dirVec) > 0 and xnormL > 0 and xnormL < 1 and abs(v.posFront.z - ego.pos.z) <
          4 then
          -- distance from ego.pos to v.posFront or v.posRear
          local distF = (v.posFront - ego.pos):dot(ego.rightVec)
          local distR = (v.posRear - ego.pos):dot(ego.rightVec)
          -- side of v w.r.t. ego (ego -> v = 1, v <- ego = -1)
          local side = sign(distR)

          local dist = min(abs(distF), abs(distR))
          -- Relative Velocity of ego w.r.t. v vehicle
          local vRel = ego.vel - v.vel
          -- relative lateral speed of v w.r.t. ego (rsLat > 0 means v is moving toward us)
          local rsLat = vRel:dot(ego.rightVec) * side
          -- time to lateral collision (> 0 means collision is imminent, < 0 means collision does not happen)
          local TTC = dist * sign2(rsLat) / (abs(rsLat) + 1e-30)
          -- relative longitudinal speed of v w.r.t. ego (rsLong > 0 means ego is moving faster than v)
          local rsLong = vRel:dot(ego.dirVec)

          local TTO
          if rsLong > 0 then -- time to overtake v
            TTO = (2 * ego.length * lerp(1, lerp(0.5, 0, backAwereness), xnormL) + (xnormF < 0 and v.length or 0)) /
                    (rsLong + 1e-30)
          else -- time to be overtaken by v
            TTO = (2 * ego.length * lerp(0.5, 1, backAwereness) * xnormL + (xnormF > 0 and v.length or 0)) /
                    (-rsLong + 1e-30)
          end

          -- if TTC is less than TTO or if is greater but vehicles are close to each other: ACTIVATE SIDE AVOIDANCE
          if v.dirVec:dot(ego.dirVec) > 0.3 then
            if TTC > 0 and TTC < min(3, TTO) then
              if side < 0 then -- v-vehicle is on our left side
                dispLeft = max(dispLeft, square(clamp(0.1 / TTC, 0, 1)))
              elseif side > 0 then -- v-vehicle is on our right side  --(and (xnormF < 0.5 or v.vel:dot(ego.dirVec) > ego.speed))
                dispRight = max(dispRight, square(clamp(0.1 / TTC, 0, 1)))
              end
              -- obj.debugDrawProxy:drawSphere(0.2, ego.pos + 0.5 * ego.dirVec * ego.length, color(255, 128, 0, 160))
              -- obj.debugDrawProxy:drawSphere(0.2, midPos, color(255, 128, 0, 160))
            elseif dist < 0.5 * (ego.width + v.width + 0.2) and TTC > -0.1 then
              if side < 0 then -- v-vehicle is on our left side
                dispLeft = max(dispLeft, 0.1)
              elseif side > 0 then -- v-vehicle is on our right side  --(and (xnormF < 0.5 or v.vel:dot(ego.dirVec) > ego.speed))
                dispRight = max(dispRight, 0.1)
              end
              -- obj.debugDrawProxy:drawSphere(0.2, ego.pos + 0.5 * ego.dirVec * ego.length, color(0, 0, 0, 160))
              -- obj.debugDrawProxy:drawSphere(0.2, midPos, color(0, 0, 0, 160))
            end
          end
        end
      end
    else
      for _, v in ipairs(traffic.trafficTable) do -- side avoidance loop
        local xnorm = v.posFront:xnormOnLine(midPos + lenVec, midPos - lenVec)
        if ego.speed > 1 and v.vel:dot(ego.dirVec) > 0 and xnorm > 0 and xnorm < 1 and abs(v.posFront.z - ego.pos.z) < 4 then
          fl = fl or midPos - ego.rightVec * ego.width * 0.5 + lenVec
          rl = rl or fl - lenVec * 2
          fr = fr or midPos + ego.rightVec * ego.width * 0.5 + lenVec
          rr = rr or fr - lenVec * 2

          local posF = v.posFront
          local rightVec = v.dirVec:cross(v.dirVecUp)
          local posL = posF - rightVec * (v.width * 0.5 + 0.1)
          local posR = posF + rightVec * (v.width * 0.5 + 0.1)

          local xnorm1, xnorm2 = closestLinePoints(posF, posR, rl, fl)
          local xnorm3, xnorm4 = closestLinePoints(posF, posL, rr, fr)

          if xnorm1 > 0 and xnorm1 < 1 and xnorm2 > 0 and xnorm2 < 1 then -- v-vehicle is on our left side
            dispLeft = max(dispLeft, linePointFromXnorm(posF, posR, xnorm1):squaredDistance(posR))
          elseif xnorm3 > 0 and xnorm3 < 1 and xnorm4 > 0 and xnorm4 < 1 then -- v-vehicle is on our right side
            dispRight = max(dispRight, linePointFromXnorm(posF, posL, xnorm3):squaredDistance(posL))
          end
        end
      end
    end

    -- if fl then
    -- local cornerColor = color(255, 128, 0, 160)
    -- obj.debugDrawProxy:drawSphere(0.2, fl, cornerColor)
    -- obj.debugDrawProxy:drawSphere(0.2, rl, cornerColor)
    -- obj.debugDrawProxy:drawSphere(0.2, fr, cornerColor)
    -- obj.debugDrawProxy:drawSphere(0.2, rr, cornerColor)
    -- local sideColor = color(clamp(sqrt(dispLeft) * 255, 0, 255), 0, clamp(sqrt(dispRight) * 255, 0, 255), 160)
    -- obj.debugDrawProxy:drawSphere(0.4, midPos + vec3(0,0,2), sideColor)
    -- end

    plan.sideDisp = nil
    if dispLeft > 0 or dispRight > 0 then
      local sideDisp = sqrt(dispLeft) - sqrt(dispRight) -- (sideDisp > 0) means v-vehicle is on our left side and ego should move right
      sideDisp = min(dt * parameters.awarenessForceCoef * 10, abs(sideDisp)) * sign2(sideDisp) -- limited displacement per frame
      plan.sideDisp = true
      -- maybe needs some smoother to prevent left / right "bouncing"
      local curDist = 0
      local lastPlanIdx = 2
      local targetDist = square(ego.speed) / (2 * g * aggression) + max(opt.racing and 100 or 30, ego.speed * 3) -- longer adjustment at higher speeds

      local tmpVec = vec3()
      for i = 2, plan.planCount - 1 do
        openLaneToLaneRange(plan[i])
        plan[i].lateralXnorm = clamp(plan[i].lateralXnorm + sideDisp * (targetDist - curDist) / targetDist,
          plan[i].laneLimLeft, plan[i].laneLimRight)
        tmpVec:setScaled2(plan[i].normal, plan[i].lateralXnorm)
        plan[i].pos:setAdd2(plan[i].posOrig, tmpVec)
        curDist = curDist + plan[i - 1].length
        lastPlanIdx = i

        plan[i].vec:setSub2(plan[i - 1].pos, plan[i].pos);
        plan[i].vec.z = 0
        plan[i].dirVec:set(plan[i].vec)
        plan[i].dirVec:normalize()

        if curDist > targetDist then
          break
        end
      end

      updatePlanLen(plan, 2, lastPlanIdx + 1)
    end

    local trafficMinSpeedSq = math.huge
    local distanceT = 0
    local minTrafficDir = 1
    local nDir, forceVec = vec3(), vec3()
    nDir:setSub2(plan[2].pos, plan[1].pos);
    nDir:setScaled(1 / (plan[1].length + 1e-30))
    local egoPathVel = ego.vel:dot(nDir)
    local egoPathVelInv = 1 / abs(egoPathVel + 1e-30)
    local inMultipleLanes = plan[2].laneLimRight - plan[2].laneLimLeft > 3.45 * 1.5 -- one and a half lanes
    for i = 2, plan.planCount - 1 do
      local arrivalT = distanceT * egoPathVelInv
      local n1, n2 = plan[i], plan[i + 1]
      local n1pos, n2pos = n1.pos, n2.pos
      nDir:setSub2(n2pos, n1pos);
      nDir:setScaled(1 / (n1.length + 1e-30))
      n1.trafficSqVel = math.huge

      for j = trafficTableLen, 1, -1 do
        local v = traffic.trafficTable[j]
        local plPosFront, plPosRear, plWidth = v.posFront, v.posRear, v.width
        local ego2PlVec = plPosFront - ego.pos
        local ego2PlDir = ego2PlVec:dot(ego.dirVec)

        if ego2PlDir > 0 then
          local velDisp = arrivalT * v.vel
          if v.vel:length() < ego.speed and (v.noproj or opt.racing) then
            velDisp = velDisp * min(1, square(square(square(max(0, arrivalT / 4)))))
          end
          plPosFront = plPosFront + velDisp
          plPosRear = plPosRear + velDisp
        end

        local extVec = 0.5 * max(ego.width, plWidth) * nDir
        local n1ext, n2ext = n1pos - extVec, n2pos + extVec
        local rnorm, vnorm = closestLinePoints(n1ext, n2ext, plPosFront, plPosRear)

        local minSqDist = math.huge
        if rnorm > 0 and rnorm < 1 and vnorm > 0 and vnorm < 1 then
          minSqDist = 0
        else
          local rlen = n1.length + plWidth
          local xnorm = plPosFront:xnormOnLine(n1ext, n2ext) * rlen
          local v1 = vec3()
          if xnorm > 0 and xnorm < rlen then
            v1:setScaled2(nDir, xnorm);
            v1:setAdd(n1ext)
            minSqDist = min(minSqDist, v1:squaredDistance(plPosFront))
          end

          xnorm = plPosRear:xnormOnLine(n1ext, n2ext) * rlen
          if xnorm > 0 and xnorm < rlen then
            v1:setScaled2(nDir, xnorm);
            v1:setAdd(n1ext)
            minSqDist = min(minSqDist, v1:squaredDistance(plPosRear))
          end

          rlen = v.length + ego.width
          v1:setSub2(n1ext, plPosRear)
          local v1dot = v1:dot(v.dirVec)
          if v1dot > 0 and v1dot < rlen then
            minSqDist = min(minSqDist, v1:squaredDistance(v1dot * v.dirVec))
          end

          v1:setSub2(n2ext, plPosRear)
          v1dot = v1:dot(v.dirVec)
          if v1dot > 0 and v1dot < rlen then
            minSqDist = min(minSqDist, v1:squaredDistance(v1dot * v.dirVec))
          end
        end

        local limWidth = v.targetType == 'follow' and 2 * max(n1.radiusOrig, n2.radiusOrig) or plWidth

        if minSqDist < square((ego.width + limWidth) * 0.8) then
          local velProjOnSeg = max(0, v.vel:dot(nDir))

          local vehicleIsStopped = isVehicleStopped(v)

          if vehicleIsStopped then
            local distToParked = 0
            for ii = 2, i do
              distToParked = distToParked + plan[ii].length
            end
            if distToParked < 25 then
              openPlanLanesIdx = max(openPlanLanesIdx, i)
            end
          end

          if not newOvertake then
            if plan.stopSeg == math.huge and v.targetType ~= 'follow' then -- apply side forces to avoid vehicles
              local side1 = sign(n1.normal:dot(v.posMiddle) - n1.normal:dot(n1.pos))
              local side2 = sign(n2.normal:dot(v.posMiddle) - n2.normal:dot(n2.pos))

              if not v.sideDir then
                v.sideDir = side1 -- save the avoidance direction once to compare it with all of the subsequent plan nodes
              end

              if v.sideDir == side1 and inMultipleLanes then -- calculate force coef only if the avoidance side matches the initial value

                local forceCoef = trafficStates.side.side * parameters.awarenessForceCoef *
                                    max(0, ego.speed - velProjOnSeg,
                    -sign(nDir:dot(v.dirVec)) * trafficStates.side.cTimer) /
                                    ((1 + minSqDist) *
                                      (1 + distanceT * min(0.1, 1 / (2 * max(0, egoPathVel - v.vel:dot(nDir)) + 1e-30))))

                if opt.racing then
                  forceCoef = forceCoef * 0.25
                end
                forceVec:setScaled2(n1.normal, side1 * forceCoef)
                forces[i]:setSub(forceVec)

                forceVec:setScaled2(n1.normal, side2 * forceCoef)
                forces[i + 1]:setSub(forceVec)
              end
            end
          else
            if plan.stopSeg == math.huge and v.targetType ~= 'follow' and not v.overtaken then
              v.overtaken = true
              if inMultipleLanes then
                local Deltax = distanceT
                local factor1 = clamp(ego.vel:dot(nDir) - velProjOnSeg, 0, 4)
                local factor2 = max(0, -sign(nDir:dot(v.dirVec)) * trafficStates.side.cTimer)
                for ii = 2, plan.planCount - 1 do
                  -- Logic for performing the overtake manouver
                  local n1i = plan[ii]
                  -- local n2i = plan[ii + 1]
                  local distY = n1i.normal:dot(v.posMiddle) - n1i.normal:dot(n1i.pos)
                  -- local distY = plan[1].normal:dot(v.posMiddle) - plan[1].normal:dot(ego.pos)
                  -- local Deltax = -n2i.dirVec:dot(v.posMiddle - n1i.pos) --Need to be improved
                  if ii > 2 then
                    Deltax = Deltax - plan[ii - 1].length
                  end
                  if not v.sideDir then
                    v.sideDir = sign(distY) -- save the avoidance direction once to compare it with all of the subsequent plan nodes
                  end
                  if v.sideDir == sign(distY) then
                    local forceCoef = sign(distY) *
                                        max(0, ((n1i.radiusOrig) - abs(distY)) * math.exp(-0.001 * square(Deltax))) *
                                        (1 * parameters.awarenessForceCoef * dt)
                    forceCoef = forceCoef * max(factor1, factor2) * trafficStates.side.side
                    forceVec:setScaled2(n1i.normal, forceCoef)
                    forces[ii]:setSub(forceVec)
                  end
                end
              end
            end
          end

          if M.mode ~= 'flee' and M.mode ~= 'random' and
            (M.mode ~= 'manual' or (n1.laneLimRight - n1.laneLimLeft) <= ego.width + plWidth) then
            -- sets a minimum speed due to other vehicle velocity projection on plan segment
            -- only sets it if ego mode is valid; or if mode is "manual" but there is not enough space to pass

            if minSqDist < square((ego.width + limWidth) * 0.51) then
              -- obj.debugDrawProxy:drawSphere(0.25, v.posFront, color(0,0,255,255))
              -- obj.debugDrawProxy:drawSphere(0.25, plPosFront, color(0,0,255,255))
              if not vehicleIsStopped then
                table.remove(traffic.trafficTable, j)
                trafficTableLen = trafficTableLen - 1
              end
              plan.trafficMinProjSpeed = min(plan.trafficMinProjSpeed, velProjOnSeg)

              n1.trafficSqVel = min(n1.trafficSqVel, velProjOnSeg * velProjOnSeg)
              trafficMinSpeedSq = min(trafficMinSpeedSq, v.vel:squaredLength())
              minTrafficDir = min(minTrafficDir, v.dirVec:dot(nDir))
            end

            if i == 2 and minSqDist < square((ego.width + limWidth) * 0.6) and ego2PlDir > 0 and v.vel:dot(ego.rightVec) *
              ego2PlVec:dot(ego.rightVec) < 0 then
              n1.trafficSqVel = max(0, n1.trafficSqVel - abs(1 - v.vel:dot(ego.dirVec)) * (v.vel:length()))
            end
          end
        end
      end

      distanceT = distanceT + n1.length

      if trafficTableLen < 1 then
        break
      end
    end

    -- this code was supposed to keep the vehicle stopped until the intersection was clear
    -- not working as intended, different solution needed
    -- if trafficStates.intersection.timer < parameters.trafficWaitTime and plan.trafficMinProjSpeed < 3 then
    -- trafficStates.intersection.timer = 0 -- reset the intersection waiting timer
    -- end

    trafficStates.block.block = max(trafficMinSpeedSq, ego.speed * ego.speed) < 1 and
                                  (minTrafficDir < -0.7 or trafficStates.intersection.block)
    if not opt.racing then
      plan[1].trafficSqVel = plan[2].trafficSqVel
    end

    if openPlanLanesIdx > 0 and plan[openPlanLanesIdx].length < ego.length * 1.5 then
      openPlanLanesIdx = openPlanLanesIdx + 1
    end

    openPlanLanesToLaneRange(plan, openPlanLanesIdx)
  end
  -- profilerPopEvent("ai_awareness")

  -- remove lane change if vehicle has gone past it
  -- profilerPushEvent("ai_remove_laneChange")
  while route.laneChanges[1] and plan[2].pathidx > route.laneChanges[1].pathIdx do
    electrics.stop_turn_signal()
    table.remove(route.laneChanges, 1)
  end
  -- profilerPopEvent("ai_remove_laneChange")

  local skipLast = 1
  local lastNodeLatXnorm, lastNodeLimLeft, lastNodeLimRight

  -- profilerPushEvent("ai_process_laneChange")
  if route.laneChanges[1] and math.floor(plan[2].rangeLaneCount + 0.5) > 1 and route.laneChanges[1].side ~= 0 then
    local exitNodeIdx = route.laneChanges[1].pathIdx
    local distToExit
    if plan[plan.planCount].pathidx > exitNodeIdx then
      distToExit = plan.planLen
      for i = plan.planCount, 2, -1 do
        if plan[i].pathidx == exitNodeIdx then
          break
        else
          distToExit = distToExit - plan[i - 1].length
        end
      end
    else
      distToExit = plan.planLen +
                     max(0, route.pathLength[route.laneChanges[1].pathIdx] -
          route.pathLength[plan[plan.planCount].pathidx])
    end

    if (distToExit < min(600, max(15, ego.speed * ego.speed * 0.7)) or route.laneChanges[1].commit) then -- and not plan.noExit
      local side = route.laneChanges[1].side
      -- check if the exit is blocked by a vehicle
      if (side < 0 and ego.ghostL) or (side > 0 and ego.ghostR) then
        plan.noExit = true
      else
        plan.noExit = nil
      end

      -- if the exit is not blocked, do a lane change
      if not plan.noExit or route.laneChanges[1].commit then
        route.laneChanges[1].commit = true
        -- print('lane change is going to happen')
        -- obj.debugDrawProxy:drawSphere(2, ego.pos, color(255,0,255,255))
        if side < 0 then
          if (electrics.values.turnsignal or -1) >= 0 then
            electrics.toggle_left_signal()
          end
        else
          if (electrics.values.turnsignal or 1) <= 0 then
            electrics.toggle_right_signal()
          end
        end
        skipLast = 0
        lastNodeLatXnorm = plan[plan.planCount].lateralXnorm
        lastNodeLimLeft = plan[plan.planCount].laneLimLeft
        lastNodeLimRight = plan[plan.planCount].laneLimRight
        local planDist = 0
        local sideForceCoeff = 2.5 * side * dt / distToExit
        local planExitIdx = plan.planCount
        if plan[plan.planCount].pathidx >= exitNodeIdx then -- if last plan node is at or past the path exit node
          planExitIdx = 1
          for i = plan.planCount, 3, -1 do
            if plan[i].pathidx == exitNodeIdx then -- if this plan node is at the exit node
              if i == plan.planCount then -- if it is also the last plan node
                lastNodeLatXnorm = nil
              end
              planExitIdx = i
              break
            end
          end
        else
          lastNodeLatXnorm = nil
        end

        -- Add lane change forces to all nodes up to the exit node
        for i = 3, planExitIdx do
          planDist = planDist + plan[i - 1].length
          local n = plan[i]
          local roadHalfWidth = n.halfWidth
          local laneHalfWidth = roadHalfWidth * (n.rangeRight - n.rangeLeft) / n.rangeLaneCount
          if side < 0 then
            n.laneLimLeft = (2 * n.rangeLeft - 1) * roadHalfWidth -- open left lane limit to left range limit
            n.laneLimRight = max(n.laneLimLeft + ego.width, min(n.laneLimRight, n.lateralXnorm + laneHalfWidth))
            forces[i]:setAdd(planDist * sideForceCoeff * square(min(1, 0.25 * abs(n.lateralXnorm - n.laneLimLeft))) *
                               n.normal)
          else
            n.laneLimRight = (2 * n.rangeRight - 1) * roadHalfWidth -- open right lane limit to right range limit
            n.laneLimLeft = min(n.laneLimRight - ego.width, max(n.laneLimLeft, n.lateralXnorm - laneHalfWidth))
            forces[i]:setAdd(planDist * sideForceCoeff * square(min(1, 0.25 * abs(n.laneLimRight - n.lateralXnorm))) *
                               n.normal)
          end
        end
        for i = planExitIdx + 1, plan.planCount do
          planDist = max(0, planDist - plan[i - 1].length)
          if planDist == 0 then
            break
          end
          local n = plan[i]
          if side < 0 then
            forces[i]:setAdd(planDist * sideForceCoeff * square(min(1, 0.25 * abs(n.lateralXnorm - n.laneLimLeft))) *
                               n.normal)
          else
            forces[i]:setAdd(planDist * sideForceCoeff * square(min(1, 0.25 * abs(n.laneLimRight - n.lateralXnorm))) *
                               n.normal)
          end
        end
        updatePlanbestRange(plan)
      else -- if the exit is blocked, avoid lane change by changing path & plan

        --------- Plan cleaning -----------
        local clearIdx = max(exitNodeIdx - 1, plan[2].pathidx)
        -- clear all plan nodes from clearIdx to plan.planCount
        local final_plan_count = plan.planCount
        for i = plan.planCount, 1, -1 do
          if plan[i].pathidx > clearIdx then
            plan.planLen = plan.planLen - plan[i - 1].length
            plan[i] = nil
            final_plan_count = final_plan_count - 1
          else
            break
          end
        end
        plan.planCount = final_plan_count

        ----------- Path cleaning -------------
        -- remove all path nodes after the exit node
        for i = #route.path, min(#route.path, exitNodeIdx) + 1, -1 do
          table.remove(route.path, i)
          table.remove(route.pathLength, i)
        end
        -- insert the alternate path at the exit node
        table.insert(route.path, route.laneChanges[1].alternate)
        -- clear route lane changes
        route.lastLaneChangeIdx = #route.path - 1
        table.clear(route.laneChanges)
        -- clear and update traffic path state for the new path
        if #trafficPathState > 0 then
          table.clear(trafficPathState)
          trafficPathState[1] = route.path[#route.path - 1]
          trafficPathState[2] = route.path[#route.path]
        end
      end
    end
  end
  -- profilerPopEvent("ai_process_laneChange")

  -- profilerPushEvent("ai_smoothness_integration")
  local tmpVec = vec3()
  local roadWidthMargin = ego.width * 0.5
  local laneWidthMargin = ego.width * 0.4
  local forceMagLim = parameters.springForceIntegratorDispLim
  for i = 2, plan.planCount - skipLast do
    local n = plan[i]
    local roadHalfWidth = n.halfWidth

    -- Apply a force towards the center of the lane when driving in lane
    local forceToLaneCenter = 0
    if opt.driveInLaneFlag and not n.lanesOpen then
      local rangeLeft = (2 * n.rangeLeft - 1) * roadHalfWidth
      local rangeRight = (2 * n.rangeRight - 1) * roadHalfWidth
      local b = 0.5 * (n.laneLimRight - n.laneLimLeft) + 1e-30
      local dispToLeftRange = min(1, max(0, b - (n.lateralXnorm - rangeLeft)) / b)
      local dispToRightRange = min(1, max(0, b - (rangeRight - n.lateralXnorm)) / b)
      forceToLaneCenter = 0.15 * (dispToLeftRange - dispToRightRange)
    end

    -- Calculate node displacement: avoids switching displacement direction (sign) in concecutive frames
    local displacement = forceToLaneCenter + max(min(n.normal:dot(forces[i]), forceMagLim), -forceMagLim)
    displacement = displacement * max(min((displacement * n.dispDir) * math.huge, 1), 0) -- second term returns 0 if (displacement * n.dispDir) is negative, 1 otherwise.
    n.dispDir = sign(displacement)

    local roadLimRight = max(0, roadHalfWidth - roadWidthMargin) -- should be non-negative (zero or positive)
    local limLow = max(n.laneLimLeft + laneWidthMargin, -roadLimRight)
    local limHigh = min(n.laneLimRight - laneWidthMargin, roadLimRight)
    local limAvg = (limLow + limHigh) * 0.5 -- guard against clamp limit crossing

    local newLateralXnorm = clamp(n.lateralXnorm + displacement, min(limLow, limAvg), max(limHigh, limAvg))

    tmpVec:setScaled2(n.normal, newLateralXnorm - n.lateralXnorm)
    n.pos:setAdd(tmpVec) -- remember that posOrig and pos are not alligned along the normal
    n.vec:setSub2(plan[i - 1].pos, n.pos);
    n.vec.z = 0
    n.dirVec:set(n.vec);
    n.dirVec:normalize()

    n.lateralXnorm = newLateralXnorm
  end
  -- profilerPopEvent("ai_smoothness_integration")

  if lastNodeLatXnorm then
    local roadHalfWidth = plan[plan.planCount].halfWidth
    local rangeLeft, rangeRight = (2 * plan[plan.planCount].rangeLeft - 1) * roadHalfWidth,
      (2 * plan[plan.planCount].rangeRight - 1) * roadHalfWidth
    local latXnorm = clamp(plan[plan.planCount].lateralXnorm, rangeLeft, rangeRight)
    local disp = latXnorm - lastNodeLatXnorm
    plan[plan.planCount].laneLimLeft = max(rangeLeft, min(latXnorm, lastNodeLimLeft + disp))
    plan[plan.planCount].laneLimRight = min(rangeRight, max(latXnorm, lastNodeLimRight + disp))
  end

  updatePlanLen(plan, 2, plan.planCount)

  -- smoothly distribute error from planline onto the front segments
  -- profilerPushEvent("ai_error_smoother")
  if parameters.planErrorSmoothing and plan.targetPos and plan.targetSeg and plan.planCount > plan.targetSeg and
    twt.state == 0 then
    local dTotal = 0
    local sumLen = table.new(plan.targetSeg - 1, 0)
    sumLen[1] = 0
    for i = 2, plan.targetSeg - 1 do
      sumLen[i] = dTotal
      dTotal = dTotal + plan[i].length
    end
    dTotal = max(1, dTotal + plan.targetPos:distance(plan[plan.targetSeg].pos))

    local p1, p2 = plan[1].pos, plan[2].pos
    local dispVec = ego.pos - linePointFromXnorm(p1, p2, ego.pos:xnormOnLine(p1, p2));
    dispVec:setScaled(0.5 * dt)

    tmpVec:setSub2(p2, p1);
    tmpVec:setCross(tmpVec, ego.upVec);
    tmpVec:normalize()
    plan.egoDeviation = dispVec:dot(tmpVec)

    local dispVecRatio = dispVec / dTotal
    for i = plan.targetSeg - 1, 1, -1 do
      local n = plan[i]

      dispVec:setScaled2(dispVecRatio, dTotal - sumLen[i])
      dispVec:setSub(dispVec:dot(n.biNormal) * n.biNormal)
      n.pos:setAdd(dispVec)

      local halfWidth = n.halfWidth
      tmpVec:setAdd2(n.posOrig, n.normal)
      n.lateralXnorm = clamp(n.pos:xnormOnLine(n.posOrig, tmpVec), -halfWidth, halfWidth)

      plan[i + 1].vec:setSub2(plan[i].pos, plan[i + 1].pos);
      plan[i + 1].vec.z = 0
      plan[i + 1].dirVec:setScaled2(plan[i + 1].vec, 1 / plan[i + 1].vec:lengthGuarded())
    end

    updatePlanLen(plan, 1, plan.targetSeg - 1)
  end
  -- profilerPopEvent("ai_error_smoother")

  -- TODO: This error smoother looks to behave better for race driving. It could replace the above.
  -- uniformPlanErrorDistribution(plan)

  -- profilerPushEvent("ai_calculate_target")
  calculateTarget(plan)
  -- profilerPopEvent("ai_calculate_target")

  -- calculate node horizontal curvature
  -- profilerPushEvent("ai_calculate_curvature")
  local len, n3vec = 0, vec3()
  plan[1].curvature = plan[1].curvature or inCurvature(plan[1].vec, plan[2].vec)
  for i = 2, plan.planCount - 1 do
    local n1, n2 = plan[i], plan[i + 1]

    n3vec:setSub2(n1.pos, plan[min(plan.planCount, i + 2)].pos);
    n3vec.z = 0

    local c1 = inCurvature(n1.vec, n2.vec)
    local c2 = inCurvature(n1.vec, n3vec)

    local curvature
    if c1 <= c2 then
      curvature = sign2(n1.turnDir:dot(n1.normal)) * c1
    else
      curvature = sign2((n1.vec):dot(n1.normal) - n3vec:dot(n1.normal)) * c2
    end

    -- calculate curvature temporal smoothing parameter as a function of the trajectory length distance
    local curvatureRateDt = min(5 + 0.005 * len * len, 100) * dt
    n1.curvature = n1.curvature + (curvature - n1.curvature) * curvatureRateDt / (1 + curvatureRateDt)

    len = len + n1.length
  end
  -- profilerPopEvent("ai_calculate_curvature")

  -- calculate node vertical curvature
  if not opt.racing then
    len = 0
    local v1z, v2z, v3z = vec3(), vec3(), vec3()
    for i = 2, plan.planCount - 1 do
      local n1, n2 = plan[i], plan[i + 1]

      v1z:set(plan[i - 1].length, 0, n1.posOrig.z - plan[i - 1].posOrig.z)
      v2z:set(n1.length, 0, n2.posOrig.z - n1.posOrig.z)
      v3z:set(n1.length + (n2.length or 0), 0, plan[min(plan.planCount, i + 2)].posOrig.z - n1.posOrig.z)

      local curvatureZ = min(inCurvature(v1z, v2z), inCurvature(v1z, v3z))
      -- dump(i,'curvature', curvature, 'Zcurv', curvatureZ)

      -- calculate curvature temporal smoothing parameter (fast reacting, time dependent)
      local curvatureRateDt = min(25 + 0.000045 * len * len * len * len, 1000) * dt
      n1.curvatureZ = curvatureZ + (curvatureRateDt / (1 + curvatureRateDt)) * (n1.curvatureZ - curvatureZ)

      len = len + n1.length
    end
  end

  -- Speed Planning --
  local totalAccel = min(aggression, ego.staticFrictionCoef) * g

  local lastNode = plan[plan.planCount]
  if route.path[lastNode.pathidx + 1] or (loopPath and noOfLaps and noOfLaps > 1) then
    if plan.stopSeg <= plan.planCount then
      lastNode.speed = 0
    else
      lastNode.speed = lastNode.manSpeed or sqrt(2 * 550 * totalAccel) -- shouldn't this be calculated based on the path length remaining?
    end
  else
    lastNode.speed = lastNode.manSpeed or 0
  end
  lastNode.roadSpeedLimit = plan[plan.planCount - 1].roadSpeedLimit
  lastNode.legalSpeed = min(lastNode.roadSpeedLimit or math.huge, lastNode.speed)

  -- Use Backward or Forward + Backward algorithm
  -- profilerPushEvent('ai_speedProfile')
  if speedProfileMode then
    local gT = vec3()
    for i = 1, plan.planCount - 1 do -- curvature is not defined on the last plan node
      local n1, n2 = plan[i], plan[i + 1]
      -- consider inclination
      gT:setSub2(n2.pos, n1.pos);
      gT:setScaled(gravityDir:dot(gT) / max(square(n1.length), 1e-30)) -- gravity vec parallel to road segment: positive when downhill
      local gN = gravityDir:distance(gT) -- gravity component normal to road segment
      n1.acc_max = totalAccel * gN
      n1.speed = min(n1.acc_max / max(abs(n1.curvature), 1e-30), gN * g / max(n1.curvatureZ, 1e-30)) -- available centripetal acceleration * radius
    end
    plan[plan.planCount].acc_max = plan[plan.planCount - 1].acc_max

    if speedProfileMode == 'ForwBack' then
      solver_f_acc_profile(plan)
    end

    solver_b_acc_profile(plan)
  else -- Use standard algotihm with curvature
    local gT = vec3()
    for i = plan.planCount - 1, 1, -1 do
      local n1, n2 = plan[i], plan[i + 1]

      -- consider inclination
      gT:setSub2(n2.pos, n1.pos);
      gT:setScaled(gravityDir:dot(gT) / max(square(n1.length), 1e-30)) -- gravity vec parallel to road segment: positive when downhill
      local gN = gravityDir:distance(gT) -- gravity component normal to road segment

      local curvature = max(abs(n1.curvature), 1e-5)
      local turnSpeedSq = totalAccel * gN / curvature -- available centripetal acceleration * radius

      local n1SpeedSq
      if plan.stopSeg <= i then
        n1SpeedSq = 0
      else -- speed limit imposed by other traffic vehicles and speed limit imposed by trajectory geometry (curvature and path length)
        -- https://physics.stackexchange.com/questions/312569/non-uniform-circular-motion-velocity-optimization
        n1SpeedSq = min(n1.trafficSqVel, turnSpeedSq *
          sin(min(asin(min(1, square(n2.speed) / turnSpeedSq)) + 2 * curvature * n1.length, pi * 0.5)))
      end

      n1.speed = n1.manSpeed or (M.speedMode == 'limit' and M.routeSpeed and min(M.routeSpeed, sqrt(n1SpeedSq))) or
                   (M.speedMode == 'set' and M.routeSpeed) or sqrt(n1SpeedSq)

      -- Speed envelope considering road speed limits
      if M.speedMode == 'legal' then
        n2.legalSpeed = n2.legalSpeed or n2.speed

        if plan.stopSeg <= i then
          n1.legalSpeed = 0
        else -- speed limit imposed by other traffic vehicles and speed limit imposed by trajectory geometry (curvature and path length)
          local n1LegalSpeedSq = min(n1.trafficSqVel, turnSpeedSq *
            sin(min(asin(min(1, square(n2.legalSpeed) / turnSpeedSq)) + 2 * curvature * n1.length, pi * 0.5)))
          if n1.roadSpeedLimit then
            n1.legalSpeed = min(sqrt(n1LegalSpeedSq), n1.roadSpeedLimit * (1 + aggression * 2 - 0.6))
          else
            n1.legalSpeed = sqrt(n1LegalSpeedSq)
          end
        end
      end

      n1.trafficSqVel = math.huge
    end
  end
  -- profilerPopEvent('ai_speedProfile')

  plan.targetSpeed = plan[1].speed + max(0, plan.egoXnormOnSeg) * (plan[2].speed - plan[1].speed)
  plan.targetSpeed = targetSpeedSmoother:get(plan.targetSpeed, dt)
  if M.speedMode == 'legal' then
    plan.targetSpeedLegal = plan[1].legalSpeed + max(0, plan.egoXnormOnSeg) * (plan[2].legalSpeed - plan[1].legalSpeed)
  else
    plan.targetSpeedLegal = math.huge
  end

  -- TODO: WIP
  -- calculateTrafficTargetSpeed(plan, traffic.trafficTable, trafficTableLen)
  -- plan.targetSpeed = min(plan.targetSpeed, plan.trafficTargetSpeed)

  return route
end

local function resetMapAndRoute()
  mapData = nil
  signalsData = nil
  currentRoute = nil
  loopPath = nil
  noOfLaps = nil
  internalState.road = 'onroad'
  internalState.changePlanTimer = 0
  resetAggression()
  resetInternalStates()
  resetParameters()
end

local function getMapEdges(cutOffDrivability, node)
  -- creates a table (edgeDict) with map edges with drivability > cutOffDrivability
  if mapData ~= nil then
    local allSCC = mapData:scc(node) -- An array of dicts containing all strongly connected components reachable from 'node'.
    local maxSccLen = 0
    local sccIdx
    for i, scc in ipairs(allSCC) do
      -- finds the scc with the most nodes
      local sccLen = scc[0] -- position at which the number of nodes in currentSCC is stored
      if sccLen > maxSccLen then
        sccIdx = i
        maxSccLen = sccLen
      end
      scc[0] = nil
    end
    local currentSCC = allSCC[sccIdx]
    local keySet = {}
    local keySetLen = 0

    edgeDict = {}
    for nid, n in pairs(mapData.graph) do
      if currentSCC[nid] or not opt.driveInLaneFlag then
        for lid, data in pairs(n) do
          if (currentSCC[lid] or not opt.driveInLaneFlag) and (data.drivability > cutOffDrivability) then
            local inNode = data.inNode or nid
            local outNode = inNode == nid and lid or nid
            keySetLen = keySetLen + 1
            keySet[keySetLen] = {inNode, outNode}
            edgeDict[inNode .. '\0' .. outNode] = 1
            if not data.inNode or not opt.driveInLaneFlag then
              edgeDict[outNode .. '\0' .. inNode] = 1
            end
          end
        end
      end
    end

    if keySetLen == 0 then
      return
    end
    local edge = keySet[math.random(keySetLen)]

    return edge[1], edge[2]
  end
end

local function newManualPath()
  local newRoute, n1, n2, dist
  local offRoad = false

  if manualPath then
    if currentRoute and currentRoute.path then
      pathExtend(currentRoute.path, manualPath)
    else
      newRoute = createNewRoute(manualPath)
      currentRoute = newRoute
    end
    manualPath = nil
  elseif wpList then
    if currentRoute and currentRoute.path then
      newRoute = {
        path = currentRoute.path,
        plan = currentRoute.plan,
        laneChanges = currentRoute.laneChanges,
        lastLaneChangeIdx = currentRoute.lastLaneChangeIdx,
        pathLength = currentRoute.pathLength
      }
    else
      n1, n2, dist = mapmgr.findClosestRoad(ego.pos)

      if n1 == nil or n2 == nil then
        guihooks.message("Could not find a road network, or closest road is too far", 5, "AI debug")
        log('D', "AI", "Could not find a road network, or closest road is too far")
        return
      end

      ego.currentSegment[1] = n1
      ego.currentSegment[2] = n2

      if dist > 2 * max(mapData.radius[n1], mapData.radius[n2]) then
        offRoad = true
        local vec1 = mapData.positions[n1] - ego.pos
        local vec2 = mapData.positions[n2] - ego.pos

        if ego.dirVec:dot(vec1) > 0 and ego.dirVec:dot(vec2) > 0 then
          if vec1:squaredLength() > vec2:squaredLength() then
            n1, n2 = n2, n1
          end
        elseif ego.dirVec:dot(mapData.positions[n2] - mapData.positions[n1]) > 0 then
          n1, n2 = n2, n1
        end
      elseif ego.dirVec:dot(mapData.positions[n2] - mapData.positions[n1]) > 0 then
        n1, n2 = n2, n1
      end

      newRoute = createNewRoute({n1})
    end

    for i = 0, #wpList - 1 do
      local wp1 = wpList[i] or newRoute.path[#newRoute.path]
      local wp2 = wpList[i + 1]
      local route = mapData:getPath(wp1, wp2, opt.driveInLaneFlag and 1e4 or 1)
      local routeLen = #route
      if routeLen == 0 or (routeLen == 1 and wp2 ~= wp1) then
        guihooks.message("Path between waypoints '" .. wp1 .. "' - '" .. wp2 .. "' Not Found", 7, "AI debug")
        log('D', "AI", "Path between waypoints '" .. wp1 .. "' - '" .. wp2 .. "' Not Found")
        return
      end

      for j = 2, routeLen do
        tableInsert(newRoute.path, route[j])
      end
    end

    wpList = nil

    if not offRoad and newRoute.path[3] and newRoute.path[2] == n2 then
      tableRemove(newRoute.path, 1)
    end

    currentRoute = newRoute
  end
end

local function setScriptedPath(arg)
  mapmgr.setCustomMap(arg.mapData)
  mapData = mapmgr.mapData

  setParameters({
    driveStyle = arg.driveStyle or 'default',
    staticFrictionCoefMult = max(0.95, arg.staticFrictionCoefMult or 0.95),
    lookAheadKv = max(0.1, arg.lookAheadKv or parameters.lookAheadKv),
    planErrorSmoothing = false,
    avoidCars = arg.avoidCars,
    understeerThrottleControl = arg.understeerThrottleControl,
    oversteerThrottleControl = arg.oversteerThrottleControl,
    throttleTcs = arg.throttleTcs,
    abBrakeControl = arg.abBrakeControl,
    underSteerBrakeControl = arg.underSteerBrakeControl,
    throttleKp = arg.throttleKp,
    springForceIntegratorDispLim = arg.springForceIntegratorDispLim,
    turnForceCoef = arg.turnForceCoef
  })

  opt.avoidCars = arg.avoidCars or 'off'
  noOfLaps = arg.noOfLaps
  loopPath = arg.loopPath
  setSpeed(arg.routeSpeed)
  setSpeedMode(arg.routeSpeedMode)
  setAggressionExternal(arg.aggression)

  if arg.speedProfile and next(arg.speedProfile) then
    speedProfile = arg.speedProfile
  end

  currentRoute = createNewRoute(arg.path) -- {path = arg.path, plan = {}}

  stateChanged()
end

local function validateUserInput(list)
  validateInput = nop
  list = list or wpList
  if not list then
    return
  end
  local isValid = list[1] and true or false
  for i = 1, #list do -- #wpList
    local nodeAlias = mapmgr.nodeAliases[list[i]]
    if nodeAlias then
      if mapData.graph[nodeAlias] then
        list[i] = nodeAlias
      else
        if isValid then
          guihooks.message(
            "One or more of the waypoints were not found on the map. Check the game console for more info.", 6,
            "AI debug")
          log('D', "AI", "The waypoints with the following names could not be found on the Map")
          isValid = false
        end
        -- print(list[i])
      end
    end
  end

  return isValid
end

local function fleePlan()
  if opt.aggressionMode == 'rubberBand' then
    setAggressionInternal(max(0.3, 0.95 - 0.0015 * player.pos:distance(ego.pos)))
  end

  -- extend the plan if possible and desirable
  if currentRoute and not currentRoute.plan.reRoute then
    local plan = currentRoute.plan
    if ego.pos:dot(ego.dirVec) >= player.pos:dot(ego.dirVec) and not targetWPName and internalState.road ~= 'offroad' and
      plan.trafficMinProjSpeed > 3 then
      local path = currentRoute.path
      local pathCount = #path
      if pathCount >= 3 and plan[2].pathidx > pathCount * 0.7 then
        local cr1 = path[pathCount - 1]
        local cr2 = path[pathCount]
        local dirVec = mapData.positions[cr2] - mapData.positions[cr1]
        dirVec:normalize()
        pathExtend(path, mapData:getFleePath(cr2, dirVec, player.pos, getMinPlanLen(), 0.01, 0.01))
        planAhead(currentRoute)
        return
      end
    end
  end

  if not currentRoute or internalState.changePlanTimer == 0 or currentRoute.plan.reRoute then
    local wp1, wp2 = mapmgr.findClosestRoad(ego.pos)
    if wp1 == nil or wp2 == nil then
      internalState.road = 'offroad'
      return
    else
      internalState.road = 'onroad'
    end

    ego.currentSegment[1] = wp1
    ego.currentSegment[2] = wp2

    local dirVec
    if currentRoute and currentRoute.plan.trafficMinProjSpeed < 3 then
      internalState.changePlanTimer = 5
      dirVec = -ego.dirVec
    else
      dirVec = ego.dirVec
    end

    local startnode = pickAiWp(wp1, wp2, dirVec)
    local path
    if not targetWPName then
      path = mapData:getFleePath(startnode, dirVec, player.pos, getMinPlanLen(), 0.01, 0.01)
    else -- flee to destination
      path = mapData:getPathAwayFrom(startnode, targetWPName, ego.pos, player.pos)
      if next(path) == nil then
        targetWPName = nil
      end
    end

    if not path[1] then
      internalState.road = 'offroad'
      return
    else
      internalState.road = 'onroad'
    end

    local route = planAhead(path, currentRoute)
    if route and route.plan then
      local tempPlan = route.plan
      if not currentRoute or internalState.changePlanTimer > 0 or tempPlan.targetSpeed >=
        min(ego.speed, currentRoute.plan.targetSpeed) and targetsCompatible(currentRoute, route) then
        currentRoute = route
        internalState.changePlanTimer = max(1, internalState.changePlanTimer)
        return
      elseif currentRoute.plan.reRoute then
        currentRoute = route
        internalState.changePlanTimer = max(1, internalState.changePlanTimer)
        return
      end
    end
  end

  planAhead(currentRoute)
end

local function chasePlan()
  local positions = mapData.positions
  local radii = mapData.radius

  internalState.chaseData.targetSpeed = nil

  local wp1, wp2, dist1 = mapmgr.findBestRoad(ego.pos, ego.dirVec)
  if wp1 == nil or wp2 == nil then
    internalState.road = 'offroad'
    return
  end

  local playerSpeed = player.vel:length()
  local playerVel = playerSpeed > 1 and player.vel or player.dirVec -- uses dirVec for very low speeds

  local plwp1, plwp2, dist2 = mapmgr.findBestRoad(player.pos, playerVel)
  if plwp1 == nil or plwp2 == nil then
    internalState.road = 'offroad'
    return
  end

  if positions[wp2]:dot(ego.dirVec) < positions[wp1]:dot(ego.dirVec) then
    wp1, wp2 = wp2, wp1
  end
  -- wp2 is next node for ego to drive to

  ego.currentSegment[1] = wp1
  ego.currentSegment[2] = wp2

  if (playerVel / (playerSpeed + 1e-30)):dot(positions[plwp2] - positions[plwp1]) < 0 then
    plwp1, plwp2 = plwp2, plwp1
  end
  -- plwp2 is next node that player is driving to

  if dist1 > max(radii[wp1], radii[wp2]) + ego.width and dist2 > max(radii[plwp1], radii[plwp2]) +
    obj:getObjectInitialWidth(player.id) then
    internalState.road = 'offroad'
    return
  end

  local playerNode = plwp2
  local egoPlDist = ego.pos:distance(player.pos) -- should this be a signed distance?
  local egoPosRear = ego.pos - ego.dirVec * ego.length
  local nearDist = max(ego.length + 8, internalState.chaseData.playerStoppedTimer) -- larger if player stopped for longer (anti softlock)
  local isAtPlayerSeg = (wp1 == playerNode or wp2 == playerNode)

  if opt.aggressionMode == 'rubberBand' then
    if M.mode == 'follow' then
      setAggressionInternal(min(0.75, 0.3 + 0.0025 * egoPlDist))
    else
      setAggressionInternal(min(0.95, 0.8 + 0.0015 * egoPlDist))
    end
  end

  -- consider calculating the aggression value but then passing it through a smoother so that transitions between chase mode and follow mode are smooth

  if playerSpeed < 1 then
    internalState.chaseData.playerStoppedTimer = internalState.chaseData.playerStoppedTimer + dt
  else
    internalState.chaseData.playerStoppedTimer = 0
  end

  if internalState.chaseData.playerStoppedTimer > 5 and egoPlDist <
    max(nearDist, square(ego.speed) / (2 * g * aggression)) then -- within braking distance to player
    internalState.chaseData.playerState = 'stopped'

    if ego.speed < 0.3 and egoPlDist < nearDist then
      -- do not plan new route if stopped near player
      currentRoute = nil
      internalState.road = 'onroad'
      return
    end
  else
    internalState.chaseData.playerState = nil
  end

  if internalState.chaseData.driveAhead and ego.speed >= 10 then -- unset this flag if the ego reached a minimum speed
    internalState.chaseData.driveAhead = false
  end

  if M.mode == 'follow' and ego.speed < 0.3 and isAtPlayerSeg and egoPlDist < nearDist then
    -- do not plan new route if ego reached player
    currentRoute = nil
    internalState.road = 'onroad'
    return
  end

  if currentRoute then
    local curPlan = currentRoute.plan
    local playerNodeInPath = waypointInPath(currentRoute.path, playerNode, curPlan[2].pathidx) or false

    local planVec = curPlan[2].pos - curPlan[1].pos
    local playerIncoming = playerSpeed >= 3 and playerNode == wp1 and egoPlDist < max(ego.speed, playerSpeed) and
                             playerVel:dot(planVec) < 0 -- player is driving towards or past ego on the segment
    local playerBehind = playerSpeed >= 3 and planVec:dot(playerVel) > 0 and ego.dirVec:dot(egoPosRear - player.pos) > 0 -- player got passed by ego
    local playerOtherWay = not playerNodeInPath and planVec:dot(positions[playerNode] - ego.pos) < 0 and
                             (playerSpeed < 3 or playerVel:dot(player.pos - ego.pos) > 0) -- player is driving other way from ego

    local route
    if not playerNodeInPath and not internalState.chaseData.driveAhead and
      (ego.speed < 3 or ego.dirVec:dot(player.pos - ego.pos) > 0) then -- prevents ego from cancelling its current route if it should slow down to turn around
      local path = mapData:getChasePath(wp1, wp2, plwp1, plwp2, ego.pos, ego.vel, player.pos, player.vel,
        opt.driveInLaneFlag and 1e4 or 1)

      route = planAhead(path, currentRoute) -- ignore current route if path should go other way
      if route and route.plan then -- and tempPlan.targetSpeed >= min(ego.speed, curPlan.targetSpeed) and (tempPlan.targetPos-curPlan.targetPos):dot(ego.dirVec) >= 0 then
        currentRoute = route
      end
    end

    local pathLen = getPathLen(currentRoute.path, playerNodeInPath or math.huge) -- curPlan[2].pathidx
    local playerMinPlanLen = getMinPlanLen(0, playerSpeed)
    if M.mode == 'chase' and pathLen < playerMinPlanLen then -- chase path should be extended
      local pathCount = #currentRoute.path
      local fleePath = mapData:getFleePath(currentRoute.path[pathCount], playerVel, player.pos, playerMinPlanLen, 0, 0)
      if fleePath[2] ~= wp1 and fleePath[2] ~= wp2 and fleePath[2] ~= currentRoute.path[pathCount - 1] then -- only extend the path if it does not do a u-turn
        pathExtend(currentRoute.path, fleePath)
      end
    end

    if not route then
      planAhead(currentRoute)
    end

    local targetSpeed

    if M.mode == 'chase' then
      local brakeDist = square(ego.speed) / (2 * g * aggression)
      local relSpeed = playerVel:dot(ego.dirVec)
      local crashSpeed = 10 -- minimum relative crash speed
      if egoPlDist < max(brakeDist, nearDist) and ego.dirVec:dot(egoPosRear - player.pos) < 0 then
        targetSpeed = max(crashSpeed, relSpeed + crashSpeed)
        internalState.chaseData.targetSpeed = targetSpeed
      end
    end

    if not internalState.chaseData.driveAhead then
      if playerIncoming or playerOtherWay then -- come to a stop, then plan to turn around
        targetSpeed = 0
      elseif playerBehind then -- match the player speed based on distance
        targetSpeed = clamp(playerSpeed * (1 - egoPlDist / 120), 5, max(5, curPlan[2].speed))
      end
    end
    curPlan.targetSpeed = targetSpeed or curPlan.targetSpeed

    if M.mode == 'chase' then
      internalState.road = 'onroad'

      if playerIncoming then -- player is head on versus ego
        internalState.road = 'tail'
      elseif egoPlDist < 25 and ego.dirVec:dot((ego.pos - ego.dirVec * ego.length) - player.pos) < 0 then -- player is near ego, but ego path does a u-turn
        local uTurn = false
        for i, p in ipairs(currentRoute.path) do -- detect path u-turn
          if p == playerNode then
            break
          end
          if i > 2 and ego.dirVec:dot(positions[p] - positions[currentRoute.path[i - 1]]) < 0 then
            uTurn = true
            break
          end
        end
        if uTurn then
          -- cast a ray to see if ego can directly attack player without hitting a barrier
          if obj:castRayStatic(ego.pos + ego.upVec * 0.5, (ego.pos - player.pos) / (egoPlDist + 1e-30), egoPlDist) >=
            egoPlDist then
            internalState.road = 'tail'
            if isAtPlayerSeg then -- important to reset the route here
              currentRoute = nil
              return
            end
          end
        end
      end
      if not playerIncoming and
        (plwp2 == currentRoute.path[curPlan[2].pathidx] or plwp2 == currentRoute.path[curPlan[2].pathidx + 1]) then -- player is matching ego target node
        local playerNodePos1 = positions[plwp2]
        local segDir = playerNodePos1 - positions[plwp1]
        local targetLineDir = vec3(-segDir.y, segDir.x, 0);
        targetLineDir:normalize()
        local xnorm1 = closestLinePoints(playerNodePos1, playerNodePos1 + targetLineDir, player.pos,
          player.pos + player.dirVec)
        local xnorm2 = closestLinePoints(playerNodePos1, playerNodePos1 + targetLineDir, ego.pos, ego.pos + ego.dirVec)
        -- player xnorm and ego xnorm get interpolated here
        local tarPos = playerNodePos1 + targetLineDir * clamp(lerp(xnorm1, xnorm2, 0.5), -radii[plwp2], radii[plwp2])

        local p2Target = tarPos - player.pos;
        p2Target:normalize()
        local plVel2Target = playerSpeed > 0.1 and player.vel:dot(p2Target) or 0
        local plTimeToTarget = tarPos:distance(player.pos) / (plVel2Target + 1e-30)

        local egoVel2Target = ego.speed > 0.1 and ego.vel:dot((tarPos - ego.pos):normalized()) or 0
        local egoTimeToTarget = tarPos:distance(ego.pos) / (egoVel2Target + 1e-30)

        if egoTimeToTarget < plTimeToTarget and not playerBehind then
          internalState.road = 'tail'
        end
      end
    end

    if internalState.chaseData.playerState == 'stopped' then
      currentRoute.plan.targetSpeed = 0
    end
  else
    local path
    if M.mode == 'chase' and ego.dirVec:dot(playerVel) > 0 and ego.dirVec:dot(egoPosRear - player.pos) > 0 then
      path = mapData:getFleePath(wp2, playerVel, player.pos, getMinPlanLen(100, ego.speed), 0, 0)
      internalState.chaseData.driveAhead = true
    else
      path = mapData:getChasePath(wp1, wp2, plwp1, plwp2, ego.pos, ego.vel, player.pos, player.vel,
        opt.driveInLaneFlag and 1e4 or 1)
      internalState.chaseData.driveAhead = false
    end

    local route = planAhead(path)
    if route and route.plan then
      currentRoute = route
    end
  end
end

M.pullOver = false
local function setPullOver(val)
  M.pullOver = val
end

local relativePosOtherVehicle = vec3()
local posRelativeToPolice = vec3()
local function trafficActions()
  if not currentRoute then
    return
  end
  local path, plan = currentRoute.path, currentRoute.plan

  -- horn
  if parameters.enableElectrics and trafficStates.action.hornTimer == 0 then
    electrics.horn(true)
    trafficStates.action.hornTimerLimit = max(0.1, math.random())
  end
  if trafficStates.action.hornTimer >= trafficStates.action.hornTimerLimit then
    electrics.horn(false)
    trafficStates.action.hornTimer = -1
  end

  if trafficStates.action.hornTimer >= 0 then
    trafficStates.action.hornTimer = trafficStates.action.hornTimer + dt
  end

  local pullOver = false
  pullOver = M.pullOver

  -- hazard lights
  if beamstate.damage >= 1000 then
    if recover.recoverOnCrash then
      recover._recoverOnCrash = true
    else
      if electrics.values.signal_left_input == 0 and electrics.values.signal_right_input == 0 then
        electrics.set_warn_signal(1)
      end
      pullOver = true
    end
  end

  -- pull over
  local minSirenSqDist = math.huge
  local nearestPoliceId

  for plID, v in pairs(mapmgr.getObjects()) do
    if plID ~= objectId and v.states then
      if v.states.lightbar == 2 or (v.states.lightbar == 1 and v.vel:squaredLength() >= 100) then
        local posFront = obj:getObjectFrontPosition(plID)
        minSirenSqDist = min(minSirenSqDist, posFront:squaredDistance(ego.pos))
        nearestPoliceId = plID
      end
    end
  end
  if minSirenSqDist <= 10000 then
    pullOver = true
    trafficStates.action.nearestPoliceId = nearestPoliceId
  end

  if trafficStates.action.nearestPoliceId then
    local police = mapmgr.objects[trafficStates.action.nearestPoliceId]
    if police and police.states and police.states.lightbar then
      posRelativeToPolice:setSub2(ego.pos, police.pos)
      posRelativeToPolice:normalize()
      if posRelativeToPolice:dot(police.dirVec) > 0.94 then
        if ego.speed < 10 and ego.pos:squaredDistance(police.pos) < 400 then
          pullOver = true -- vehicle stays pulled over in this case, and other traffic may keep driving
        end

        if ego.dirVec:dot(police.dirVec) > 0.5 then
          -- Check if there's a vehicle next to this one
          for otherID, v in pairs(mapmgr.getObjects()) do
            if otherID ~= objectId and v.pos and otherID ~= trafficStates.action.nearestPoliceId and v.dirVec and
              v.dirVec:dot(ego.dirVec) < 0 then
              local distance = ego.pos:squaredDistance(v.pos)

              -- Check if the other vehicle is within a reasonable distance to be considered "next to" us
              if distance < 225 then -- within 15 meters
                relativePosOtherVehicle:setSub2(v.pos, ego.pos)
                local forwardDist = relativePosOtherVehicle:dot(ego.dirVec)
                local lateralDist = relativePosOtherVehicle:dot(ego.rightVec)

                -- Check if the vehicle is alongside us
                if abs(forwardDist) < 10 and abs(lateralDist) < 6 then -- vehicle is alongside within 8m forward/back and 6m lateral
                  -- Only check for vehicles on the other side of the street
                  -- For right-hand drive: other side is to the left (positive cross product z-component)
                  -- For left-hand drive: other side is to the right (negative cross product z-component)
                  local isOnOtherSide = mapmgr.rules.rightHandDrive and lateralDist > 0 or
                                          not mapmgr.rules.rightHandDrive and lateralDist < 0

                  if isOnOtherSide then
                    pullOver = false
                    break
                  end
                end
              end
            end
          end
        end
      end
    end
  end

  if pullOver and not trafficStates.action.forcedStop and ego.speed >= 3 then
    local brakeDist = square(ego.speed) / (2 * g * aggression)
    local dist = max(10, brakeDist)
    local idx = getLastNodeWithinDistance(plan, dist)
    local n = plan[idx]
    local side = mapmgr.rules.rightHandDrive and -1 or 1
    local disp = n.pos:distance(n.posOrig + n.normal * side * (n.halfWidth - ego.width * 0.5))
    trafficStates.side.displacement = disp
    -- dist = dist + disp * 4 -- arbitrary extra distance?

    laneChange(plan, max(10, dist - 20), disp * side)
    setStopPoint(plan, dist, {
      avoidJunction = true
    })
    trafficStates.action.forcedStop = true
    trafficStates.side.side = mapmgr.rules.rightHandDrive and -1 or 1
  end

  if not pullOver and trafficStates.action.forcedStop then
    -- laneChange(plan, 40, -trafficStates.side.displacement) -- this is no longer needed?
    setStopPoint()
    trafficStates.action.forcedStop = false
    trafficStates.action.nearestPoliceId = nil
  end

  if trafficStates.action.forcedStop and ego.speed < min(plan.targetSpeed or 0, 1) then -- instant plan stop
    setStopPoint(plan, 0)
  end

  -- Search for controlled (traffic light or stop sign) or uncontrolled (right of way) intersections along the path
  local tSi = trafficStates.intersection
  if not tSi.node then
    tSi.block = false

    tSi.startIdx = tSi.startIdx or (plan[1].wp and plan[1].pathidx - 1 or plan[1].pathidx)
    for i = tSi.startIdx, #path - 1 do -- TODO: would searching up to min(#path-1, plan[plan.planCount].pathIdx) work to distribute the load over more frames work?
      local nid1, nid2 = path[i], path[i + 1]
      if not nid1 then
        nid1 = plan[1].wp -- just in case the ego is at the very start of the plan
      end

      if nid1 and nid2 then
        -- if trafficStates.intersection.prevNode == nid1 then break end -- vehicle is still within previous intersection

        local n1Pos, n2Pos = mapData.positions[nid1], mapData.positions[nid2]

        -- *****Controlled intersection (traffic light or stop sign)******
        if signalsData and signalsData[nid1] and signalsData[nid1][nid2] then -- nodes from current path match the signals dict
          -- TODO: check the array for the ideal signal to use
          -- lane check as well, if applicable
          local bestSignal = signalsData[nid1][nid2][1]

          local nDir = n2Pos - n1Pos
          nDir.z = 0
          nDir:normalize()
          table.clear(tSi)
          tSi.node = nid1
          tSi.nextNode = nid2
          tSi.nodeIdx = 1
          tSi.pos = bestSignal.pos
          tSi.dir = nDir
          tSi.action = bestSignal.action
          tSi.block = false
          tSi.controlled = true

          -- adding turnDir
          if path[i + 2] then
            local n3Pos = mapData.positions[path[i + 2]]
            if tableSize(mapData.graph[nid2]) > 2 then -- if next path node is not in the middle of the junction
              tSi.turnDir = n3Pos - n2Pos
              tSi.turnDir.z = 0
              tSi.turnDir:normalize()
              tSi.turnNode = nid2
            else
              if path[i + 3] then
                tSi.turnDir = mapData.positions[path[i + 3]] - n3Pos
                tSi.turnDir.z = 0
                tSi.turnDir:normalize()
                tSi.turnNode = path[i + 2]
              end
            end
          end

        end

        -- *****detect uncontrolled intersection or set the turn direction for an already detected controlled intersection******
        if tableSize(mapData.graph[nid1]) > 2 and not tSi.node and tSi.startIdx > 0 then
          -- we should try to get the effective curvature of the path after this point to determine turn signals

          -- Get Direction Vector of edge exiting nid1
          local linkDir = vec3();
          linkDir:setSub2(mapData:getEdgePositions(nid2, nid1))
          linkDir.z = 0
          linkDir:normalize()

          -- Get path Direction Vector to nid1
          local prevNode = path[i - 1] or plan[1].wp
          local nDir = vec3()
          local drivability = 1
          if mapData.graph[nid1][prevNode] then
            nDir:setSub2(mapData:getEdgePositions(nid1, prevNode))
            nDir.z = 0
            nDir:normalize()
            drivability = mapData.graph[nid1][prevNode].drivability
          else
            prevNode = nil
            nDir:set(ego.dirVec)
          end

          -- Give way if the direction change at nid1 is greater than 45 deg and less than 135 deg
          -- or if the current path edge leading to nid1 has drivability lower than other roads incident on nid1
          local giveWay = abs(nDir:dot(linkDir)) < 0.707

          if not giveWay and drivability < 1 then
            for _, edgeData in pairs(mapData.graph[nid1]) do
              if edgeData.drivability > drivability then
                giveWay = true
                break
              end
            end
          end

          if giveWay then
            local fourthNode -- the fourth node in a T-junction (the other three nodes being prevNode, nid1, nid2)
            -- the prevNode check is to make sure one of the three roads of the T-Junction is the one the vehicle is comming from
            if prevNode and tableSize(mapData.graph[nid1]) == 3 then
              for k, v in pairs(mapData.graph[nid1]) do
                if k ~= prevNode and k ~= nid2 then
                  fourthNode = k
                end
              end
            end

            -- Checks if the vehicle has right of way in this T-junction
            local side = mapmgr.rules.rightHandDrive and -1 or 1
            local nContinuation = fourthNode and roadNaturalContinuation(fourthNode, nid1)
            local dotDir = side * gravityDir:cross(nDir):dot(linkDir)
            if not (fourthNode and nContinuation ~= nid2 and dotDir > 0) then

              local pos = n1Pos - nDir * (max(3, mapData.radius[nid1]) + 2) -- needs at least + 3

              table.clear(tSi)
              tSi.node = nid1
              tSi.nextNode = nid2
              tSi.turnNode = nid1
              tSi.turnDir = linkDir
              tSi.pos = pos
              tSi.dir = nDir
              tSi.action = 3
              tSi.block = false
              -- tSi.fullGiveWay -> giveWay to both left + right
              -- tSi.oneGiveWay -> giveWay only on (right) side
              -- tSi.otherGiveWay -> giveWay only on (left) side
              if fourthNode then
                tSi.fullGiveWay = nContinuation == nid2
                if not tSi.fullGiveWay then
                  tSi.oneGiveWay = dotDir < 0
                else
                  if dotDir > 0 then
                    tSi.fullGiveWay = false
                    tSi.otherGiveWay = true
                  end
                end
              else
                tSi.oneGiveWay = true
              end
            end
          end
        end
      end
      tSi.startIdx = i + 1

      if tSi.node then

        tSi.turn = 0
        tSi.timer = 0
        if tSi.turnDir then
          if abs(tSi.dir:dot(tSi.turnDir)) < 0.707 then
            tSi.turn = -sign2(tSi.dir:cross(gravityDir):dot(tSi.turnDir))
          end
        end

        break
      end
    end
  end

  -- Manage stopping at found intersections
  if tSi.node and not trafficStates.action.forcedStop then
    local signalsRef = tSi.nodeIdx and signalsData[tSi.node][tSi.nextNode][tSi.nodeIdx]
    if signalsRef then
      tSi.action = signalsRef.action or 0 -- get action from referenced table
    else
      tSi.action = tSi.action or 0 -- default action ("go")
    end
    -- local sColor = tSi.action == 0 and color(0,255,0,160) or color(255,255,0,160)
    -- obj.debugDrawProxy:drawSphere(1, tSi.pos, sColor)
    -- obj.debugDrawProxy:drawText(tSi.pos + vec3(0, 0, 1), color(0,0,0,255), tostring(tSi.turn))

    local stopSeg = math.huge
    local brakeDist = square(ego.speed) / (2 * g * ego.staticFrictionCoef * min(1, aggression * 1.3))
    local distSq = ego.pos:squaredDistance(tSi.pos)

    if not tSi.proximity then -- checks if intersection was reached (needs improvement)
      tSi.proximity = distSq <= 400
    end

    if tSi.pos:dot(tSi.dir) + 4 * tSi.dir:dot(tSi.dir) >= ego.pos:dot(tSi.dir) then -- vehicle position is at the stop pos (with extra distance, to be safe)
      if tSi.action == 3 or tSi.action == 2 or
        (tSi.action == 1 and (square(brakeDist) < distSq or tSi.commitStopOnYellow)) then -- red light or other stop condition
        local bestDist = 100
        for i = 1, plan.planCount - 1 do -- get best plan node to set as a stopping point
          -- currently checks every frame due to plan segment updates
          -- positional check is used due to issues with using plan.pathidx or complex intersections
          -- it would be great to improve this in the future
          local dist = plan[i].pos:squaredDistance(tSi.pos)
          if dist < bestDist then
            bestDist = dist
            stopSeg = i
          end
        end
        if stopSeg < math.huge and tSi.action == 1 then
          tSi.commitStopOnYellow = true
        end
      end

      if tSi.action == 3 or tSi.action == 2 then
        if stopSeg <= 2 and ego.speed <= 1 then -- stopped at stopping point
          tSi.timer = tSi.timer + dt
          -- if on an uncontrolled intersection, check if there are any vehicles around. if there aren't then continue.
          if tSi.action == 3 then
            local vehicleInRange = false
            local vPosF = vec3()
            local egoCenterPos = getObjectBoundingBox(objectId)
            local distToJcenter = tSi.turnNode and ego.pos:distance(mapData.positions[tSi.turnNode]) or 1
            for vId, v in pairs(mapmgr.getObjects()) do
              if vId ~= objectId then
                local vC, vX = getObjectBoundingBox(vId)
                vPosF:setAdd2(vC, vX)

                -- checkDirRad: v is outside a slice of +-45 degrees and is under a thredshold distance
                local check = false
                local checkDirRad
                if not isVehicleStopped(v) and v.dirVec:dot(vPosF - (ego.pos + distToJcenter * ego.dirVec)) < 0 then
                  checkDirRad = v.dirVec:dot(tSi.dir) < 0.707 and egoCenterPos:squaredDistance(vPosF) <
                                  square(
                      max(min(60, max(40, v.vel:squaredLength() / (g * ego.staticFrictionCoef))), v.vel:length() * 4))
                end
                -- Different tests for each kind of stop (controlled or fullGiveway/oneGiveWay/otherGiveWay)
                if checkDirRad then
                  if tSi.oneGiveWay then
                    local vFxnorm = tSi.turnDir and
                                      vPosF:xnormOnLine(tSi.pos, tSi.pos + 1.5 *
                        mapData.radius[tSi.turnNode or tSi.node] * tSi.turnDir) or 0
                    if vFxnorm > 0 and vFxnorm < 1 and v.dirVec:dot(tSi.dir) < -0.707 then
                      check = true
                    end
                  elseif tSi.otherGiveWay or tSi.turn == (mapmgr.rules.rightHandDrive and -1 or 1) then -- other or stop/full but turning to right
                    if tSi.turnDir and v.dirVec:dot(tSi.turnDir) > 0.707 then
                      check = true
                    end
                  else
                    check = true
                  end
                end

                if check then
                  vehicleInRange = true
                  local kr = tSi.controlled and parameters.delayFactorWaitTime[1] or parameters.delayFactorWaitTime[2]
                  local ks = 0.9
                  tSi.timer = tSi.timer - (kr + min(v.vel:squaredLength(), 1) * (ks - kr) / 1) * dt
                  tSi.timer = max(tSi.timer, 0)
                  break
                end
              end
            end
            if not vehicleInRange then
              tSi.timer = parameters.trafficWaitTime
            end
          end
        end

        if tSi.timer >= parameters.trafficWaitTime then
          if tSi.action == 2 then
            -- Turn on red allowed (right turn for RHT (LHD) and left turn for LHT (RHD) allowed after stopping and intersection is clear)
            if mapmgr.rules.turnOnRed and tSi.turn == (mapmgr.rules.rightHandDrive and -1 or 1) then
              tSi.nodeIdx = nil
              tSi.action = 0
            end
          else
            tSi.nodeIdx = nil
            tSi.action = 0
          end
        end
      end
    else
      if tSi.proximity then
        tSi.nodeIdx = nil
        tSi.action = 0
        if distSq > 400 then -- assumes that vehicle has cleared the intersection (20 m away from the signal point)
          -- temp data until next intersection search
          -- resync startIdx if it has fallen behind by the time the intersection is cleared.
          local startIdx = tSi.startIdx
          if plan[1].pathidx < plan[2].pathidx then -- plan[1] node is on a different path index than the node ahead of it
            -- skip path node if it is behind vehicle
            startIdx = max(startIdx, plan[1].pathidx)
          else
            startIdx = max(startIdx, plan[1].pathidx - 1)
          end
          table.clear(tSi)
          tSi.timer = 0
          tSi.turn = 0
          tSi.block = false
          -- tSi.prevNode = tSi.node
          tSi.startIdx = startIdx
        end
      end
    end

    plan.stopSeg = stopSeg
    if parameters.enableElectrics and tSi.turnNode and ego.pos:squaredDistance(mapData.positions[tSi.turnNode]) <
      square(max(50, brakeDist * 1.2)) then -- approaching intersection
      if tSi.turn < 0 and electrics.values.turnsignal >= 0 then
        electrics.toggle_left_signal()
      elseif tSi.turn > 0 and electrics.values.turnsignal <= 0 then
        electrics.toggle_right_signal()
      end
    end
  end
end

local function trafficPlan()
  -- profilerPushEvent('ai_trafficPlan_pathfinding')
  if trafficStates.block.block then
    trafficStates.block.timer = trafficStates.block.timer + dt
  else
    trafficStates.block.timer = trafficStates.block.timer * 0.8
    trafficStates.block.hornFlag = false
  end

  if currentRoute and currentRoute.path[1] and not currentRoute.plan.reRoute and trafficStates.block.timer <=
    trafficStates.block.timerLimit then -- and not currentRoute.plan.noExit
    local plan = currentRoute.plan
    local path = currentRoute.path
    if (internalState.road ~= 'offroad' and plan.planLen + getPathLen(path, plan[plan.planCount].pathidx) <
      getMinPlanLen()) or not path[plan[plan.planCount].pathidx + 2] then
      local pathCount = #path

      local newPath
      newPath = mapData:getPathTWithState(path[pathCount], mapData.positions[path[pathCount]], getMinPlanLen(100),
        trafficPathState[1] and trafficPathState or ego.dirVec)
      table.clear(trafficPathState)
      for i, v in ipairs(newPath) do
        trafficPathState[i] = v
      end

      pathExtend(path, newPath)
    end
  else
    local wp1, wp2 = mapmgr.findBestRoad(ego.pos, ego.dirVec)

    if wp1 == nil or wp2 == nil then
      guihooks.message("Could not find a road network, or closest road is too far", 5, "AI debug")
      currentRoute = nil
      internalState.road = 'offroad'
      internalState.changePlanTimer = 0
      driveCar(0, 0, 0, 1)
      return
    end

    local radius = mapData.radius
    local position = mapData.positions
    local graph = mapData.graph

    local dirVec
    if trafficStates.block.timer > trafficStates.block.timerLimit and not graph[wp1][wp2].oneWay and
      (radius[wp1] + radius[wp2]) * 0.5 > ego.length then
      dirVec = -ego.dirVec -- tries to plan reverse direction
    else
      dirVec = ego.dirVec
    end

    wp1, wp2 = pickAiWp(wp1, wp2, dirVec)

    local path
    path = mapData:getPathTWithState(wp1, ego.pos, getMinPlanLen(80), ego.dirVec)
    table.clear(trafficPathState)
    for i, v in ipairs(path) do
      trafficPathState[i] = v
    end

    if path[2] == wp2 and path[3] then
      local xnorm = ego.pos:xnormOnLine(position[wp1], position[wp2])
      if xnorm >= 0 and xnorm <= 1 and (position[wp2] - position[wp1]):dot(ego.dirVec) < 0 then
        -- vehicle is within the first path segment but facing the wrong way
        table.remove(path, 1)
      end
    end
    ego.currentSegment[1] = wp1
    ego.currentSegment[2] = wp2

    if path and path[1] then
      local route = planAhead(path, currentRoute)

      if route and route.plan then
        trafficStates.block.timerLimit = max(1, parameters.trafficWaitTime * 2)

        table.clear(trafficStates.intersection)
        trafficStates.intersection.timer = 0
        trafficStates.intersection.turn = 0
        trafficStates.intersection.block = false

        if trafficStates.block.timer > trafficStates.block.timerLimit and trafficStates.action.hornTimer == -1 then
          trafficStates.block.timer = 0
          if not trafficStates.block.hornFlag then
            trafficStates.action.hornTimer = 0 -- activates horn
            trafficStates.block.hornFlag = true -- prevents horn from triggering again while stopped
          end

          currentRoute = route
          return
        elseif not currentRoute then
          currentRoute = route
          return
        elseif route.plan.targetSpeed >= min(currentRoute.plan.targetSpeed, ego.speed) and
          targetsCompatible(currentRoute, route) then
          currentRoute = route
          return
        end
      end
    end
  end
  -- profilerPopEvent('ai_trafficPlan_pathfinding')

  -- profilerPushEvent('ai_trafficActions')
  trafficActions()
  -- profilerPopEvent('ai_trafficActions')

  -- profilerPushEvent('ai_planAhead')
  planAhead(currentRoute)
  -- profilerPopEvent('ai_planAhead')
end

local function warningAIDisabled(message)
  guihooks.message(message, 5, "AI debug")
  M.mode = 'disabled'
  M.updateGFX = nop
  resetMapAndRoute()
  stateChanged()
end

local function targetFollowControl(targetSpeed, distLim) -- throttle and brake control for when driving directly to player
  -- distLim: Minimum distance allowed

  -- local targetPos, throttleTargetSpeed, brakeTargetSpeed <-- WIP
  if not targetSpeed then
    if not player or not player.pos then
      return 0, 0, 0
    end
    local plC, plX, plY, plZ = getObjectBoundingBox(player.id)
    local ego2PlDirVec = plC - ego.pos;
    ego2PlDirVec:normalize()
    local minHit = intersectsRay_OBB(ego.pos, ego2PlDirVec, plC, plX, plY, plZ)
    local plSpeedFromego = player.vel:dot(ego2PlDirVec)
    local ego2PlDist = minHit - (distLim or 3)
    targetSpeed = sqrt(max(0, abs(plSpeedFromego) * plSpeedFromego + 2 * g * min(aggression, ego.staticFrictionCoef) *
      ego2PlDist))

    -- WIP
    -- throttleTargetSpeed = sqrt(max(0, abs(plSpeedFromego) * plSpeedFromego + 2 * g * min(aggression, ego.staticFrictionCoef) * (minHit - (distLim or 3) - ego.speed * 1)))
    -- brakeTargetSpeed = sqrt(max(0, abs(plSpeedFromego) * plSpeedFromego + 2 * g * min(aggression * 1.5, ego.staticFrictionCoef) * (minHit - (distLim or 3))))
    -- targetPos = catmullRomChordal(ego.pos-ego.dirVec, ego.pos, plC-plX, plC-plX+plX:normalized(), min(1, max(ego.speed * parameters.lookAheadKv, 4.5) / ego.pos:distance(plC - plX)))
    -- obj.debugDrawProxy:drawSphere(0.25, targetPos, color(255, 0, 0, 255))
    -- obj.debugDrawProxy:drawSphere(0.25, plC-plX, color(0, 0, 255, 255))
    -- obj.debugDrawProxy:drawSphere(0.15, ego.pos + brakeTargetSpeed * ego.upVec, color(255, 0, 0, 255))
    -- obj.debugDrawProxy:drawSphere(0.15, ego.pos + throttleTargetSpeed * ego.upVec, color(0, 255, 0, 255))
    -- obj.debugDrawProxy:drawSphere(0.15, ego.pos + ego.speed * ego.upVec, color(0, 0, 255, 255))
  end

  local speedDif = targetSpeed - ego.speed
  return clamp(speedDif, 0, 1), clamp(-speedDif, 0, 1), targetSpeed

  -- WIP
  -- return clamp((throttleTargetSpeed - ego.speed) * 0.25, 0, 1), clamp(ego.speed - brakeTargetSpeed, 0, 1), brakeTargetSpeed, targetPos
end

local function drivabilityChangeReroute()
  -- Description: handle changes in edge drivabilities
  -- This function compares the current path for collisions with the drivability change set
  -- if there is an edge along the current path that had its drivability decreased
  -- a flag is raised (currentRoute.plan.reRoute) then handled by the appropriate planner

  if currentRoute ~= nil then
    -- changeSet format: {nodeA1, nodeB1, driv1, nodeA2, nodeB2, driv2, ...}
    local changeSet = mapmgr.changeSet
    local changeSetCount = #changeSet
    local changeSetDict = table.new(0, 2 * (changeSetCount / 3))

    -- populate the changeSetDict with the changeSet nodes
    for i = 1, changeSetCount, 3 do
      if changeSet[i + 2] < 0 then
        changeSetDict[changeSet[i]] = true
        changeSetDict[changeSet[i + 1]] = true
      end
    end

    local path = currentRoute.path
    local nodeCollisionIdx
    for i = currentRoute.plan[2].pathidx, #path do
      if changeSetDict[path[i]] then
        -- if there is a collision continue with a thorough check (edges against edges)
        nodeCollisionIdx = i
        break
      end
    end

    if nodeCollisionIdx then
      table.clear(changeSetDict)
      local edgeTab = {'', '\0', ''}
      -- populate the changeSetDict with changeSet edges
      for i = 1, changeSetCount, 3 do
        if changeSet[i + 2] < 0 then
          local nodeA, nodeB = changeSet[i], changeSet[i + 1]
          edgeTab[1] = nodeA < nodeB and nodeA or nodeB
          edgeTab[3] = nodeA == edgeTab[1] and nodeB or nodeA
          changeSetDict[tableConcat(edgeTab)] = true
        end
      end

      local edgeCollisionIdx
      -- compare path edges with changeSetDict edges starting with the earliest edge containing the initialy detected node collision
      for i = max(currentRoute.plan[2].pathidx, nodeCollisionIdx - 1), #path - 1 do
        local nodeA, nodeB = path[i], path[i + 1]
        edgeTab[1] = nodeA < nodeB and nodeA or nodeB
        edgeTab[3] = nodeA == edgeTab[1] and nodeB or nodeA
        if changeSetDict[tableConcat(edgeTab)] then
          edgeCollisionIdx = i
          currentRoute.plan.reRoute = edgeCollisionIdx
          break
        end
      end

      -- if edgeCollisionIdx then
      --   -- find closest possible diversion point from edgeCollisionIdx
      --   local graph = mapData.graph
      --   for i = edgeCollisionIdx, currentRoute.plan[2].pathidx, -1 do
      --     local node = path[i]
      --     if tableSize(graph[node]) > 2 then

      --     end
      --   end
      --   dump(objectId, edgeCollisionIdx, path[edgeCollisionIdx], path[edgeCollisionIdx+1])
      -- end
    end
  end
end

local function getSafeTeleportPosRot()
  if currentRoute and currentRoute.path and currentRoute.path[2] then
    local pathIdx = #currentRoute.path
    local node = currentRoute.path[pathIdx]
    local pos = mapData.positions[node]

    local node2 = currentRoute.path[pathIdx - 1]
    local pos2 = mapData.positions[node2]

    local dir = (pos - pos2):normalized()
    local up = mapmgr.surfaceNormalBelow(pos)
    local rot = quatFromDir(-dir:cross(up):cross(up), up) -- minus sign is due to convention used by safeTeleport

    if not mapData.graph[node][node2].oneway then
      -- if not a one way road shift the position to the right by half the road radius (ie. 1/4 of the width)
      pos = pos + dir:cross(up):normalized() * (mapData.radius[node] * 0.5)
    end

    return pos, rot
  end
end

local function modeAdjustments()
  ------------------ RANDOM MODE ----------------
  if M.mode == 'random' then
    local route
    if not currentRoute or currentRoute.plan.reRoute or currentRoute.plan.planLen +
      getPathLen(currentRoute.path, currentRoute.plan[currentRoute.plan.planCount].pathidx) < getMinPlanLen() then
      local wp1, wp2 = mapmgr.findClosestRoad(ego.pos)
      if wp1 == nil or wp2 == nil then
        warningAIDisabled("Could not find a road network, or closest road is too far")
        return true
      end
      ego.currentSegment[1] = wp1
      ego.currentSegment[2] = wp2

      if internalState.road == 'offroad' then
        local vec1 = mapData.positions[wp1] - ego.pos
        local vec2 = mapData.positions[wp2] - ego.pos
        if ego.dirVec:dot(vec1) > 0 and ego.dirVec:dot(vec2) > 0 then
          if vec1:squaredLength() > vec2:squaredLength() then
            wp1, wp2 = wp2, wp1
          end
        elseif ego.dirVec:dot(mapData.positions[wp2] - mapData.positions[wp1]) > 0 then
          wp1, wp2 = wp2, wp1
        end
      elseif ego.dirVec:dot(mapData.positions[wp2] - mapData.positions[wp1]) > 0 then
        wp1, wp2 = wp2, wp1
      end

      local path = mapData:getRandomPath(wp1, wp2, opt.driveInLaneFlag and 1e4 or 1)

      if path and path[1] then
        local route = planAhead(path, currentRoute)
        if route and route.plan then
          if not currentRoute then
            currentRoute = route
          else
            local curPlanIdx = currentRoute.plan[2].pathidx
            local curPathCount = #currentRoute.path
            if curPlanIdx >= curPathCount * 0.9 or
              (targetsCompatible(currentRoute, route) and route.plan.targetSpeed >= ego.speed) then
              currentRoute = route
            end
          end
        end
      end
    end

    if currentRoute ~= route then
      planAhead(currentRoute)
    end

    ------------------ TRAFFIC MODE ----------------
  elseif M.mode == 'traffic' then
    -- profilerPushEvent('ai_trafficPlan')
    trafficPlan()
    -- profilerPopEvent('ai_trafficPlan')

    ------------------ MANUAL MODE ----------------
  elseif M.mode == 'manual' then
    if validateInput(wpList or manualPath) then
      newManualPath()
    elseif scriptData then
      setScriptedPath(scriptData)
      scriptData = nil
    end

    if opt.aggressionMode == 'rubberBand' then
      updatePlayerData()
      if player ~= nil then
        if (ego.pos - player.pos):dot(ego.dirVec) > 0 then
          setAggressionInternal(max(min(0.1 + max((150 - player.pos:distance(ego.pos)) / 150, 0), M.extAggression), 0.5))
        else
          setAggressionInternal()
        end
      end
    end

    if opt.racing and false then

      -- timeprobe()

      if currentRoute then
        if not currentRoute.plan[1] then
          raceplanAhead(currentRoute)
        end
        if not currentRoute.altPlan then
          currentRoute.altPlan = {}
        else
          table.clear(currentRoute.altPlan)
        end

        -- create purePerformance plan
        if not currentRoute.planPP then
          currentRoute.planPP = minimalcopyPlan(currentRoute.plan)
        end
        raceplanAhead(currentRoute, nil, 'pp')
        currentRoute.plan = currentRoute.planPP
        -- table.insert(currentRoute.altPlan, currentRoute.planPP)

        -- create Left plan
        if not currentRoute.planL then
          currentRoute.planL = minimalcopyPlan(currentRoute.plan)
          offsetPlan(currentRoute.plan, currentRoute.planL, 'left')
        end
        raceplanAhead(currentRoute, nil, 'left')
        table.insert(currentRoute.altPlan, currentRoute.planL)

        -- create Right plan
        if not currentRoute.planR then
          currentRoute.planR = minimalcopyPlan(currentRoute.plan)
          offsetPlan(currentRoute.plan, currentRoute.planR, 'right')
        end
        raceplanAhead(currentRoute, nil, 'right')
        table.insert(currentRoute.altPlan, currentRoute.planR)

        -- Search the best plan
        local plan_index
        local current_cost = currentRoute.plan.targetSpeed
        local current_trafficSpeed = currentRoute.plan.trafficTargetSpeed
        local current_throttle = clamp((currentRoute.plan.targetSpeed - ego.speed) * parameters.throttleKp, 0, 1)
        local cost_time = currentRoute.plan.planLen / max(currentRoute.plan.targetSpeed, 0.1)
        local speed_margin = 2
        local n = vec3(-ego.dirVec.y, ego.dirVec.x, 0);
        n:normalize()
        for k = 1, #currentRoute.altPlan do
          local kplan = currentRoute.altPlan[k]
          local d = currentRoute.plan.targetPos + 0.1 * sign(k - 1.5) *
                      currentRoute.plan[currentRoute.plan.targetSeg].normal - (ego.pos - ego.length * ego.dirVec)
          local kcurvature = abs(2 * d:dot(n) / (d:dot(d)))
          local kspeed = kplan[kplan.targetSeg].speed
          local speed_check = kplan.targetSpeed > current_cost + speed_margin
          local ax = max(ego.speed * ego.speed - kspeed * kspeed, 0) / (2 * d:lengthGuarded())
          -- acc_check: check if overtake respects grip limit
          local acc_check = square(currentRoute.plan[1].acc_max) - square(ax) >
                              square(ego.speed * ego.speed * kcurvature)
          -- time_check: check if alternative plan is faster (minor time)
          local time_check = kplan.planLen / kplan.targetSpeed < cost_time - 1
          -- traffic_check: check if trafficTargetSpeed of alternative plan is greater than current one
          local traffic_check = kplan.trafficTargetSpeed > current_trafficSpeed + speed_margin
          -- throttle_check: check if alternative plan is rising the throttle value (only when I'm not braking)
          local delta_throttle = linearScale(currentRoute.plan.distances, ego.length, 3 * ego.length, ego.race.dT_min,
            ego.race.dT_max)
          local throttle_check = lastCommand.throttle > 0 and
                                   clamp((kplan.targetSpeed - ego.speed) * parameters.throttleKp, 0, 1) >=
                                   min(current_throttle + delta_throttle, 1) or false
          -- side_check: avoid overtake it is against side avoidance manouver
          local side_check = (currentRoute.plan.dispLat and currentRoute.plan.dispLat ~= 0) and
                               currentRoute.plan.dispLat * (k - 1.5) or 1
          if acc_check and traffic_check and throttle_check and speed_check and time_check and side_check > 0 then
            current_cost = kplan.targetSpeed
            plan_index = k
          end
        end

        -- Committ the best plan
        if plan_index then
          currentRoute.plan_index = plan_index
          currentRoute.offset = currentRoute.altPlan[plan_index].offset
          -- If distance to the vehicle ahead is less than catching distance, I'll push the overtake using targetSpeed from geometry only
          if currentRoute.plan.distances > ego.race.d then
            currentRoute.plan.targetSpeed = currentRoute.plan.originaltargetSpeed
          end
          -- Monitor distance between targetPos of current Plan and desidered plan, do a full switch if my targetPos is too close or goes beyond the desired one
          local targetError = currentRoute.plan.targetPos - currentRoute.altPlan[plan_index].targetPos
          local side = currentRoute.plan[currentRoute.plan.targetSeg].normal:dot(targetError)
          if targetError:length() < 0.9 or (plan_index == 1 and side < 0) or (plan_index == 2 and side > 0) then
            currentRoute.planL = nil
            currentRoute.planR = nil
          end
        else
          currentRoute.plan_index = nil
          currentRoute.offset = nil
        end
      end
    else
      planAhead(currentRoute)
    end

    ------------------ SPAN MODE ------------------
  elseif M.mode == 'span' then
    if currentRoute == nil then
      local positions = mapData.positions
      local wpAft, wpFore = mapmgr.findClosestRoad(ego.pos)
      if not (wpAft and wpFore) then
        warningAIDisabled("Could not find a road network, or closest road is too far")
        return true
      end
      if ego.dirVec:dot(positions[wpFore] - positions[wpAft]) < 0 then
        wpAft, wpFore = wpFore, wpAft
      end

      ego.currentSegment[1] = wpFore
      ego.currentSegment[2] = wpAft

      local target, targetLink

      if not (edgeDict and edgeDict[1]) then
        -- creates the edgeDict and return trues a random edge
        target, targetLink = getMapEdges(M.cutOffDrivability or 0, wpFore)
        if not target then
          warningAIDisabled("No available target with selected characteristics")
          return true
        end
      end

      local path = {}
      while true do
        if not target then
          local maxDist = -math.huge
          local lim = 1
          repeat
            -- get most distant non walked edge
            for k, v in pairs(edgeDict) do
              if v <= lim then
                if lim > 1 then
                  edgeDict[k] = 1
                end
                local i = string.find(k, '\0')
                local n1id = string.sub(k, 1, i - 1)
                local sqDist = positions[n1id]:squaredDistance(ego.pos)
                if sqDist > maxDist then
                  maxDist = sqDist
                  target = n1id
                  targetLink = string.sub(k, i + 1, #k)
                end
              end
            end
            lim = math.huge -- if the first iteration does not produce a target
          until target
        end

        local nodeDegree = 1
        for lid, _ in pairs(mapData.graph[target]) do
          -- we're looking for neighboring nodes other than the targetLink
          if lid ~= targetLink then
            nodeDegree = nodeDegree + 1
          end
        end
        if nodeDegree == 1 then
          local key = target .. '\0' .. targetLink
          edgeDict[key] = edgeDict[key] + 1
        end

        path = mapData:spanMap(wpFore, wpAft, target, edgeDict, opt.driveInLaneFlag and 1e7 or 1)

        if not path[2] and wpFore ~= target then
          -- remove edge from edgeDict list and get a new target (while loop will iterate again)
          edgeDict[target .. '\0' .. targetLink] = nil
          edgeDict[targetLink .. '\0' .. target] = nil
          target = nil
          if next(edgeDict) == nil then
            warningAIDisabled("Could not find a path to any of the possible targets")
            return true
          end
        elseif not path[1] then
          warningAIDisabled("No Route Found")
          return true
        else
          -- insert the second edge node in newRoute if it is not already contained
          local pathCount = #path
          if path[pathCount - 1] ~= targetLink then
            path[pathCount + 1] = targetLink
          end
          break
        end
      end

      local route = planAhead(path)
      if not route then
        return true
      end
      currentRoute = route
    else
      planAhead(currentRoute)
    end

    ------------------ FLEE MODE ------------------
  elseif M.mode == 'flee' then
    updatePlayerData()
    if player then
      if validateInput() then
        targetWPName = wpList[1]
        wpList = nil
      end

      fleePlan()

      if internalState.road == 'offroad' then
        local targetPos = ego.pos + (ego.pos - player.pos) * 100
        local targetSpeed = math.huge
        driveToTarget(targetPos, 1, 0, targetSpeed)
        return true
      end
    else
      -- guihooks.message("No vehicle to Flee from", 5, "AI debug") -- TODO: this freezes the up because it runs on the gfx step
      return true
    end

    ------------------ CHASE MODE ------------------
  elseif M.mode == 'chase' or M.mode == 'follow' then
    updatePlayerData()
    if player then
      chasePlan()

      if internalState.road == 'tail' then
        -- internalState.road = 'onroad'
        -- currentRoute = nil
        local plego = player.pos - ego.pos
        local relvel = ego.vel:dot(plego) - player.vel:dot(plego)

        local throttle, brake, targetSpeed = targetFollowControl(internalState.chaseData.targetSpeed or math.huge)
        if relvel > 0 then
          driveToTarget(player.pos + (plego:length() / (relvel + 1e-30)) * player.vel, throttle, brake, targetSpeed)
        else
          driveToTarget(player.pos, throttle, brake, targetSpeed)
        end
        return true
      elseif internalState.road == 'offroad' then
        local throttle, brake, targetSpeed = targetFollowControl(M.mode == 'chase' and math.huge)
        driveToTarget(player.pos, throttle, brake, targetSpeed)
        return true
      elseif currentRoute == nil then
        driveCar(0, 0, 0, 1)
        return true
      end

    else
      -- guihooks.message("No vehicle to Chase", 5, "AI debug")
      return true
    end

    ------------------ STOP MODE ------------------
  elseif M.mode == 'stop' then
    if currentRoute then
      planAhead(currentRoute)
      local targetSpeed =
        max(0, ego.speed - sqrt(max(0, square(ego.staticFrictionCoef * g) - square(sensors.gx2))) * dt) -- TODO: check this calculation, i don't think it does what you think it does
      currentRoute.plan.targetSpeed = min(currentRoute.plan.targetSpeed, targetSpeed)
    elseif ego.vel:dot(ego.dirVec) > 0 then
      driveCar(0, 0, 0.5, 0)
    else
      driveCar(0, 1, 0, 0)
    end
    if ego.speed < 0.08 then
      driveCar(0, 0, 0, 1)
      M.mode = 'disabled'
      M.manualTargetName = nil
      M.updateGFX = nop
      resetMapAndRoute()
      stateChanged()
      if controller.mainController and restoreGearboxMode then
        controller.mainController.setGearboxMode('realistic')
      end
      return true
    end
    ------------------ MANUAL TRAFFIC MODE ------------------
  elseif M.mode == 'manualTraffic' then
    if validateInput(wpList or manualPath) then
      newManualPath()

    elseif scriptData then
      setScriptedPath(scriptData)
      scriptData = nil
    end

    if aggressionMode == 'rubberBand' then
      updatePlayerData()
      if player ~= nil then
        if (ai.pos - player.pos):dot(ai.dirVec) > 0 then
          setAggressionInternal(max(min(0.1 + max((150 - player.pos:distance(ai.pos)) / 150, 0), M.extAggression), 0.5))
        else
          setAggressionInternal()
        end
      end
    end

    -- Manual traffic: use manual planning but add traffic rule enforcement
    planAhead(currentRoute)

    -- Initialize traffic states for stop lights and intersections
    if trafficStates.block.block then
      trafficStates.block.timer = trafficStates.block.timer + dt
    else
      trafficStates.block.timer = trafficStates.block.timer * 0.8
      trafficStates.block.hornFlag = false
    end

    -- Add traffic actions for stop lights, stop signs, and intersections
    if currentRoute then
      trafficActions()
    end
  end
  return false
end

M.updateGFX = nop
local function updateGFX(dtGFX)
  dt = dtGFX
  if traffic.rateQue > 0 then
    traffic.frameQue = traffic.frameQue + 1
  end

  if mapData ~= mapmgr.mapData then
    currentRoute = nil
  end

  if mapmgr.changeSet then
    drivabilityChangeReroute()
    mapmgr.changeSet = nil
  end

  mapData = mapmgr.mapData
  signalsData = mapmgr.signalsData

  if mapData == nil then
    return
  end

  ego.pos:set(obj:getFrontPosition())
  ego.pos.z = max(ego.pos.z - 1, obj:getSurfaceHeightBelow(ego.pos))
  ego.prevDirVec:set(ego.dirVec)
  ego.dirVec:set(obj:getDirectionVectorXYZ())
  ego.upVec:set(obj:getDirectionVectorUpXYZ())
  ego.rightVec:setCross(ego.dirVec, ego.upVec);
  ego.rightVec:normalize()
  ego.vel:set(obj:getSmoothRefVelocityXYZ())
  ego.speed = ego.vel:length()
  ego.width = ego.width or obj:getInitialWidth()
  ego.length = ego.length or obj:getInitialLength()
  ego.staticFrictionCoef = parameters.staticFrictionCoefMult * obj:getStaticFrictionCoef() -- depends on ground model, tire and tire load

  if lastCommand.throttle > 0.5 and ego.speed < 1 then
    internalState.egoCannotMoveTime = internalState.egoCannotMoveTime + dt
  else
    internalState.egoCannotMoveTime = 0
  end

  if ego.speed < 3 then
    trafficStates.side.cTimer = trafficStates.side.cTimer + dt
    trafficStates.side.timer = (trafficStates.side.timer + dt) % (2 * trafficStates.side.timerLimit)
    trafficStates.side.side = sign2(trafficStates.side.timerLimit - trafficStates.side.timer)
  else
    trafficStates.side.cTimer = max(0, trafficStates.side.cTimer - dt)
    trafficStates.side.timer = 0
    trafficStates.side.side = 1
  end

  if recover.recoverOnCrash then
    if ego.speed < 3 then
      recover.recoverTimer = recover.recoverTimer + dt
    else
      recover.recoverTimer = max(0, recover.recoverTimer - 5 * dt)
    end
    if recover._recoverOnCrash or recover.recoverTimer > 60 then
      recover._recoverOnCrash = false
      recover.recoverTimer = 0
      local pos, rot = getSafeTeleportPosRot()
      if pos then
        obj:queueGameEngineLua("map.safeTeleport(" .. obj:getId() .. ", " .. pos.x .. ", " .. pos.y .. ", " .. pos.z ..
                                 ", " .. rot.x .. ", " .. rot.y .. ", " .. rot.z .. ", " .. rot.w ..
                                 ", nil, nil, true, true, true)")
        return
      end
    end
  end

  internalState.changePlanTimer = max(0, internalState.changePlanTimer - dt)

  -- local wp1, wp2 = mapmgr.findClosestRoad(ego.pos)
  -- if (mapData.positions[wp2] - mapData.positions[wp1]):dot(ego.dirVec) > 0 then
  --   wp1, wp2 = wp2, wp1
  -- end
  -- ego.currentSegment = {wp1, wp2}
  ego.currentSegment[1] = nil
  ego.currentSegment[2] = nil

  if modeAdjustments() then
    return true
  end
  -----------------------------------------------

  if currentRoute then
    local plan = currentRoute.plan
    local targetPos = plan.targetPos
    local egoSeg = plan.egoSeg

    -- cleanup path if it has gotten too long
    if not opt.racing and not loopPath and plan[egoSeg].pathidx >= 10 and currentRoute.path[20] then
      local path = currentRoute.path
      local k = plan[egoSeg].pathidx - 2
      for i = 1, #path do
        path[i] = path[k + i]
      end
      for i = 1, plan.planCount do
        plan[i].pathidx = plan[i].pathidx - k
      end
      -- sync lane change indices
      currentRoute.lastLaneChangeIdx = currentRoute.lastLaneChangeIdx - k
      for _, v in ipairs(currentRoute.laneChanges) do
        v.pathIdx = v.pathIdx - k
      end
      local pathDistK = currentRoute.pathLength[k + 1]
      for i = 1, #currentRoute.pathLength do
        currentRoute.pathLength[i] = currentRoute.pathLength[k + i] and currentRoute.pathLength[k + i] - pathDistK or
                                       nil
      end
      -- sync trafficState intersection search index
      if trafficStates and trafficStates.intersection and trafficStates.intersection.startIdx then
        trafficStates.intersection.startIdx = trafficStates.intersection.startIdx - k
        -- if trafficStates.intersection.startIdx < 1 then
        --   trafficStates.intersection.startIdx = nil
        -- end
      end
    end

    local targetSpeed = plan.targetSpeed

    if ego.upVec:dot(gravityDir) >= -0.2588 then -- vehicle upside down
      driveCar(0, 0, 0, 0)
      return
    end

    local lowTargetSpeedVal = 0.24
    if not plan[egoSeg + 2] and
      ((targetSpeed < lowTargetSpeedVal and ego.speed < 0.15) or (targetPos - ego.pos):dot(ego.dirVec) < 0) then
      if M.mode == 'span' then
        local path = currentRoute.path
        for i = 1, #path - 1 do
          local key = path[i] .. '\0' .. path[i + 1]
          -- in case we have gone over an edge that is not in the edgeDict list
          edgeDict[key] = edgeDict[key] and (edgeDict[key] * 20)
        end
      end

      if ego.speed < 0.15 then
        driveCar(0, 0, 0, 1)
        aistatus('route done', 'route')
        if dataLogger.logData ~= nop then
          startStopDataLog()
        end
        guihooks.message("Route done", 5, "AI debug")
        currentRoute = nil
      else
        driveCar(0, 0, max(0.3, lastCommand.brake), 0)
      end
      return
    end

    -- come off controls when close to intermediate node with zero speed (ex. intersection), arcade autobrake takes over
    if (plan[egoSeg + 1].speed == 0 and plan[egoSeg + 2]) and ego.speed < 0.15 then
      driveCar(0, 0, 0, 0)
      return
    end

    if electrics.values.ignitionLevel == 3 then
      if ego.speed < 1.5 then
        driveCar(0, 0, 0, 1)
      end
      return
    end

    if ego.speed < 0.1 and targetSpeed > 0.5 and (lastCommand.throttle ~= 0 or lastCommand.brake ~= 0) and twt.state ==
      0 and not (controller.isFrozen or electrics.values['transbrake'] or
      (trafficStates.intersection.action == 3 or trafficStates.intersection.action == 2)) then

      if internalState.crash.time == 0 then
        internalState.crash.pos = ego.pos:copy()
      end
      internalState.crash.time = internalState.crash.time + dt
      if internalState.crash.time > 1 then
        local diff = ego.pos:squaredDistance(internalState.crash.pos)
        if diff < 0.1 * 0.1 then
          if recover.recoverOnCrash then
            recover._recoverOnCrash = true
            internalState.crash.time = 0
          else
            internalState.crash.dir = vec3(ego.dirVec)
            internalState.crash.manoeuvre = 1
          end
        else
          internalState.crash.time = 0
        end
      end
    end

    -- Throttle and Brake control
    local speedDif = targetSpeed - ego.speed
    local rate = targetSpeedDifSmoother[speedDif > 0 and targetSpeedDifSmoother.state >= 0 and speedDif >=
                   targetSpeedDifSmoother.state]
    speedDif = targetSpeedDifSmoother:getWithRate(speedDif, dt, rate)

    local legalSpeedDif = plan.targetSpeedLegal - ego.speed
    local lowSpeedDif = min(speedDif - clamp((ego.speed - 2) * 0.5, 0, 1), legalSpeedDif) * parameters.throttleKp
    local lowTargSpeedConstBrake = lowTargetSpeedVal - targetSpeed -- apply constant brake below some targetSpeed

    local throttle = clamp(lowSpeedDif, 0, 1) * sign(max(0, -lowTargSpeedConstBrake)) -- throttle not enganged for targetSpeed < 0.26

    local brakeLimLow = sign(max(0, lowTargSpeedConstBrake)) * 0.5

    local brake = sign(max(0, (electrics.values.smoothShiftLogicAV or 0) - 3))
    if plan.sideDisp and opt.racing then
      local speedDifH = targetSpeed / sqrt(min(1, aggression)) - ego.speed
      brake = clamp(-speedDifH, brakeLimLow, 1) * brake -- arcade autobrake comes in at |smoothShiftLogicAV| < 5
    else
      brake = clamp(-speedDif, brakeLimLow, 1) * brake -- arcade autobrake comes in at |smoothShiftLogicAV| < 5
    end

    if brake > 0 and abs(speedDif) < 0.5 and lastCommand.throttle == 0 and lastCommand.brake == 0 then
      -- check if deceleration without braking is larger or equal to the desired deceleration
      if sensors.gy2 >= (square(plan[1].speed) - square(plan[2].speed)) / (2 * plan[1].length) then
        brake = 0
      end
    end

    driveToTarget(targetPos, throttle, brake)
  end

  dataLogger.logData()
end

local function debugDraw(focusPos)
  local debugDrawer = obj.debugDrawProxy

  if M.mode == 'script' and scriptai ~= nil then
    scriptai.debugDraw()
  end

  if currentRoute then
    local plan = currentRoute.plan
    local targetPos = plan.targetPos
    local targetSpeed = plan.targetSpeed

    if targetPos then
      debugDrawer:drawSphere(0.25, targetPos, color(255, 0, 0, 255))

      local egoSeg = plan.egoSeg
      local shadowPos = currentRoute.plan[egoSeg].pos + plan.egoXnormOnSeg * (plan[egoSeg + 1].pos - plan[egoSeg].pos)
      local blue = color(0, 0, 255, 255)
      debugDrawer:drawSphere(0.25, shadowPos, blue)

      for vehId in pairs(mapmgr.getObjects()) do
        if vehId ~= objectId then
          debugDrawer:drawSphere(0.25, obj:getObjectFrontPosition(vehId), blue)
        end
      end

      if player then
        debugDrawer:drawSphere(0.3, player.pos, color(0, 255, 0, 255))
      end
    end

    if M.debugMode == 'target' then
      if mapData and mapData.graph and currentRoute.path then
        local p = mapData.positions[currentRoute.path[#currentRoute.path]]
        debugDrawer:drawSphere(4, p, color(255, 0, 0, 100))
        debugDrawer:drawText(p + vec3(0, 0, 4), color(0, 0, 0, 255), 'Destination')
      end

    elseif M.debugMode == 'route' then
      local maxCount = 700
      local last = visDebug.routeRec.last
      local count = min(#visDebug.routeRec, maxCount)
      if count == 0 or visDebug.routeRec[last]:squaredDistance(ego.pos) > 7 * 7 then
        last = 1 + last % maxCount
        visDebug.routeRec[last] = vec3(ego.pos)
        count = min(count + 1, maxCount)
        visDebug.routeRec.last = last
      end

      local tmpVec = vec3(0.7, ego.width, 0.7)
      local black = color(0, 0, 0, 128)
      for i = 1, count - 1 do
        debugDrawer:drawSquarePrism(visDebug.routeRec[1 + (last + i - 1) % count],
          visDebug.routeRec[1 + (last + i) % count], tmpVec, tmpVec, black)
      end

      if currentRoute.plan[1].pathidx then
        local positions = mapData.positions
        local path = currentRoute.path
        tmpVec:setAdd(vec3(0, ego.width, 0))
        local transparentRed = color(255, 0, 0, 120)
        for i = currentRoute.plan[1].pathidx, #path - 1 do
          debugDrawer:drawSquarePrism(positions[path[i]], positions[path[i + 1]], tmpVec, tmpVec, transparentRed)
        end
      end

      -- Draw candidate paths if available
      if visDebug.candidatePaths and visDebug.candidatePaths[1] then
        local winner = visDebug.candidatePaths.winner
        local source = visDebug.candidatePaths[1][1] -- all paths have the same source node
        debugDrawer:drawSphere(2, mapData.positions[source], color(0, 0, 0, 255))
        for i = 1, #visDebug.candidatePaths do
          local thisPath = visDebug.candidatePaths[i]
          local thisPathCount = #thisPath
          local thisScore = visDebug.candidatePaths[i].score
          for i = 1, thisPathCount - 1 do
            debugDrawer:drawCylinder(mapData.positions[thisPath[i]], mapData.positions[thisPath[i + 1]], 0.5,
              jetColor(thisScore, 200))
          end
          local thisPathLastNode = thisPath[thisPathCount]
          debugDrawer:drawSphere(4, mapData.positions[thisPathLastNode], jetColor(thisScore, 255))
          if thisPathLastNode == winner then
            debugDrawer:drawCylinder(mapData.positions[thisPathLastNode],
              mapData.positions[thisPathLastNode] + vec3(0, 0, 8), 2, color(0, 0, 0, 255))
          end
          local txt = thisPathLastNode .. " -> " .. strFormat("%0.4f", thisScore)
          debugDrawer:drawText(mapData.positions[thisPathLastNode] + vec3(0, 0, 2), color(0, 0, 0, 255), txt)
        end
      else
        -- Mark destination node in current path
        if currentRoute.path then
          local p = mapData.positions[currentRoute.path[#currentRoute.path]]
          debugDrawer:drawSphere(4, p, color(255, 0, 0, 100))
          debugDrawer:drawText(p + vec3(0, 0, 4), color(0, 0, 0, 255), 'Destination')
        end
      end

    elseif M.debugMode == 'speeds' then
      -- Debug graph
      local altPlanTab = currentRoute.altPlan and currentRoute.altPlan or {}
      for k = 1, #altPlanTab do
        -- Plot altPlan
        local altPlan = altPlanTab[k]
        if altPlan then
          debugDrawer:drawSphere(0.1, altPlan.targetPos + vec3(0, 0, 0.5), color(0, 255, 255, 255))
          for j = 1, altPlan.planCount do
            local point = altPlan[j]
            debugDrawer:drawSphere(0.1, point.pos + vec3(0, 0, 0.5), color(255, 255, 255, 255))
            debugDrawer:drawSphere(0.1, point.pos + vec3(0, 0, point.speed * 0.2), color(255, 255, 255, 255))
            debugDrawer:drawText(point.pos + vec3(0, 0, point.speed * 0.2), color(0, 0, 0, 255),
              strFormat("%2.0f", point.speed * 3.6) .. " kph")
            if j > 1 then
              debugDrawer:drawCylinder(altPlan[j - 1].pos + vec3(0, 0, 0.5), point.pos + vec3(0, 0, 0.5), 0.05,
                color(255, 255, 255, 100))
              debugDrawer:drawCylinder(altPlan[j - 1].pos + vec3(0, 0, altPlan[j - 1].speed * 0.2),
                point.pos + vec3(0, 0, point.speed * 0.2), 0.05,
                color(255 * clamp(k - 2, 0, 1), 255 * clamp(k - 1, 0, 1), 255, 100))
            end
          end
        end
      end
      if plan.traj_index then
        local altPlan = altPlanTab[plan.traj_index]
        if altPlan then
          for j = 1, altPlan.planCount do
            local point = altPlan[j]
            debugDrawer:drawSphere(0.1, point.pos + vec3(0, 0, 0.5), color(0, 0, 0, 255))
            if j > 1 then
              debugDrawer:drawCylinder(altPlan[j - 1].pos + vec3(0, 0, 0.5), point.pos + vec3(0, 0, 0.5), 0.1,
                color(100, 200, 100, 255))
            end
          end
        end
      end

      -- Debug Throttle brake application
      local maxCount = 175
      local count = min(#visDebug.trajecRec, maxCount)
      local last = visDebug.trajecRec.last
      if count == 0 or visDebug.trajecRec[last][1]:squaredDistance(ego.pos) > (0.2 * 0.2) then
        last = 1 + last % maxCount
        visDebug.trajecRec[last] = {vec3(ego.pos), ego.speed, targetSpeed, lastCommand.brake, lastCommand.throttle}
        count = min(count + 1, maxCount)
        visDebug.trajecRec.last = last
      end

      local tmpVec1 = vec3(0.7, ego.width, 0.7)
      for i = 1, count - 1 do
        local n = visDebug.trajecRec[1 + (last + i) % count]
        debugDrawer:drawSquarePrism(visDebug.trajecRec[1 + (last + i - 1) % count][1], n[1], tmpVec1, tmpVec1,
          color(255 * sqrt(abs(n[4])), 255 * sqrt(n[5]), 0, 100))
      end

      local prevEntry
      local zOffSet = vec3(0, 0, 0.4)
      local yellow, blue = color(255, 255, 0, 200), color(0, 0, 255, 200)
      local tmpVec2 = vec3()
      for i = 1, count - 1 do
        local v = visDebug.trajecRec[1 + (last + i - 1) % count]
        if prevEntry then
          -- actuall speed
          tmpVec1:set(0, 0, prevEntry[2] * 0.2)
          tmpVec2:set(0, 0, v[2] * 0.2)
          debugDrawer:drawCylinder(prevEntry[1] + tmpVec1, v[1] + tmpVec2, 0.02, yellow)

          -- target speed
          tmpVec1:set(0, 0, prevEntry[3] * 0.2)
          tmpVec2:set(0, 0, v[3] * 0.2)
          debugDrawer:drawCylinder(prevEntry[1] + tmpVec1, v[1] + tmpVec2, 0.02, blue)
        end

        tmpVec1:set(0, 0, v[3] * 0.2)
        debugDrawer:drawCylinder(v[1], v[1] + tmpVec1, 0.01, blue)

        if focusPos:squaredDistance(v[1]) < visDebug.labelRenderDistance * visDebug.labelRenderDistance then
          tmpVec1:set(0, 0, v[2] * 0.2)
          debugDrawer:drawText(v[1] + tmpVec1 + zOffSet, yellow, strFormat("%2.0f", v[2] * 3.6) .. " kph")

          tmpVec1:set(0, 0, v[3] * 0.2)
          debugDrawer:drawText(v[1] + tmpVec1 + zOffSet, blue, strFormat("%2.0f", v[3] * 3.6) .. " kph")
        end
        prevEntry = v
      end

      -- Planned speeds
      if plan[1] then
        local red = color(255, 0, 0, 200) -- getContrastColor(objectId)
        local black = color(0, 0, 0, 255)
        local green = color(0, 255, 0, 200)
        local prevSpeed = -1
        local prevLegalSpeed = -1
        local prevPoint = plan[1].pos
        local prevPoint_ = plan[1].pos
        local tmpVec = vec3()
        for i = 1, #plan do
          local n = plan[i]

          local speed = (n.speed >= 0 and n.speed) or prevSpeed
          tmpVec:set(0, 0, speed * 0.2)
          local p1 = n.pos + tmpVec
          debugDrawer:drawCylinder(n.pos, p1, 0.03, red)
          debugDrawer:drawCylinder(prevPoint, p1, 0.05, red)
          debugDrawer:drawText(p1, black, strFormat("%2.0f", speed * 3.6) .. " kph")
          prevSpeed = speed
          prevPoint = p1

          if M.speedMode == 'legal' then
            local legalSpeed = (n.legalSpeed >= 0 and n.legalSpeed) or prevLegalSpeed
            tmpVec:set(0, 0, legalSpeed * 0.2)
            local p1_ = n.pos + tmpVec
            debugDrawer:drawCylinder(n.pos, p1_, 0.03, green)
            debugDrawer:drawCylinder(prevPoint_, p1_, 0.05, green)
            debugDrawer:drawText(p1_, black, strFormat("%2.0f", legalSpeed * 3.6) .. " kph")
            prevLegalSpeed = legalSpeed
            prevPoint_ = p1_
          end

          --[[
          if traffic and traffic[i] then
            for _, data in ipairs(traffic[i]) do
              local plPosOnPlan = linePointFromXnorm(n.pos, plan[i+1].pos, data[2])
              debugDrawer:drawSphere(0.25, plPosOnPlan, color(0,255,0,100))
            end
          end
          --]]
        end

        ---[[ Debug road width and lane limits
        local prevPointOrig = plan[1].posOrig
        local tmpVec = vec3(1, 1, 1)
        local tmpVec1 = vec3(0.5, 0.5, 0.5)

        for i = 1, #plan do
          local n = plan[i]
          local p1Orig = n.posOrig - n.biNormal
          debugDrawer:drawCylinder(n.posOrig, p1Orig, 0.03, black)
          debugDrawer:drawCylinder(p1Orig, p1Orig + n.normal, 0.03, black)
          debugDrawer:drawCylinder(prevPointOrig, p1Orig, 0.03, black)
          local roadHalfWidth = n.halfWidth
          -- debugDrawer:drawCylinder(n.posOrig, p1Orig, roadHalfWidth, color(255, 0, 0, 40))
          if n.laneLimLeft and n.laneLimRight then -- You need to uncomment the appropriate code in planAhead force integrator loop for this to work
            debugDrawer:drawSquarePrism(n.pos - (n.lateralXnorm - n.laneLimLeft) * n.normal,
              n.pos + (n.laneLimRight - n.lateralXnorm) * n.normal, tmpVec, tmpVec, color(0, 0, 255, 120))
          end
          if n.rangeLeft and n.rangeRight then
            local rangeLeft = linearScale(n.rangeLeft, 0, 1, -roadHalfWidth, roadHalfWidth)
            local rangeRight = linearScale(n.rangeRight, 0, 1, -roadHalfWidth, roadHalfWidth)
            debugDrawer:drawSquarePrism(n.posOrig + rangeLeft * n.normal, n.posOrig + rangeRight * n.normal, tmpVec1,
              tmpVec1, color(255, 0, 0, 120))
          end
          prevPointOrig = p1Orig
        end
        -- ]]

        --[[ Debug lane change. You need to uncomment upvalue newPositionsDebug for this to work
        if newPositionsDebug[1] then
          local green = color(0,255,0,200)
          local prevPoint = newPositionsDebug[1]
          for i = 1, #newPositionsDebug do
            local pos = newPositionsDebug[i]
            local p1 = pos + vec3(0, 0, 2)
            debugDrawer:drawCylinder(pos, p1, 0.03, green)
            debugDrawer:drawCylinder(prevPoint, p1, 0.05, green)
            prevPoint = p1
          end
        end
        --]]

        for i = max(1, plan[1].pathidx - 2), #currentRoute.path - 2 do
          local wp1 = currentRoute.path[i]
          local wp2 = currentRoute.path[i + 1]
          if tableSize(mapData.graph[wp2]) > 2 then
            local minNode = roadNaturalContinuation(wp1, wp2)
            if minNode and minNode ~= currentRoute.path[i + 2] then
              debugDrawer:drawCylinder(mapData.positions[wp2], mapData.positions[minNode], 0.2, black)
            end
          end
        end
      end

      -- Player segment visual debug for chase / follow mode
      -- if internalState.chaseData.playerRoad then
      --   local col1, col2
      --   if internalState.road == 'tail' then
      --     col1 = color(0,0,0,200)
      --     col2 = color(0,0,0,200)
      --   else
      --     col1 = color(255,0,0,100)
      --     col2 = color(0,0,255,100)
      --   end
      --   local plwp1 = internalState.chaseData.playerRoad[1]
      --   debugDrawer:drawSphere(2, mapData.positions[plwp1], col1)
      --   local plwp2 = internalState.chaseData.playerRoad[2]
      --   debugDrawer:drawSphere(2, mapData.positions[plwp2], col2)
      -- end

    elseif M.debugMode == 'trajectory' then
      -- Debug Curvatures
      -- local plan = currentRoute.plan
      -- if plan ~= nil then
      --   local prevPoint = plan[1].pos
      --   for i = 1, #plan do
      --     local p = plan[i].pos
      --     local v = plan[i].curvature or 1e-10
      --     local scaledV = abs(1000 * v)
      --     debugDrawer:drawCylinder(p, p + vec3(0, 0, scaledV), 0.06, color(abs(min(sign(v),0))*255,max(sign(v),0)*255,0,200))
      --     debugDrawer:drawText(p + vec3(0, 0, scaledV), color(0,0,0,255), strFormat("%5.4e", v))
      --     debugDrawer:drawCylinder(prevPoint, p + vec3(0, 0, scaledV), 0.06, col)
      --     prevPoint = p + vec3(0, 0, scaledV)
      --   end
      -- end

      -- Debug Planned Speeds
      if plan[1] then
        local col = getContrastColor(objectId)
        local prevPoint = plan[1].pos
        local prevSpeed = -1
        local drawLen = 0
        for i = 1, #plan do
          local n = plan[i]
          local p = n.pos
          local v = (n.speed >= 0 and n.speed) or prevSpeed
          local p1 = p + vec3(0, 0, v * 0.2)
          -- debugDrawer:drawLine(p + vec3(0, 0, v*0.2), (n.pos + n.turnDir) + vec3(0, 0, v*0.2), col)
          debugDrawer:drawCylinder(p, p1, 0.03, col)
          debugDrawer:drawCylinder(prevPoint, p1, 0.05, col)
          debugDrawer:drawText(p1, color(0, 0, 0, 255), strFormat("%2.0f", v * 3.6) .. " kph")
          prevPoint = p1
          prevSpeed = v
          drawLen = drawLen + n.vec:length()
          if drawLen > 80 then
            break
          end
        end
      end

      -- Debug Throttle brake application
      local maxCount = 175
      local count = min(#visDebug.trajecRec, maxCount)
      local last = visDebug.trajecRec.last
      if count == 0 or visDebug.trajecRec[last][1]:squaredDistance(ego.pos) > 0.25 * 0.25 then
        last = 1 + last % maxCount
        visDebug.trajecRec[last] = {vec3(ego.pos), lastCommand.throttle, lastCommand.brake}
        count = min(count + 1, maxCount)
        visDebug.trajecRec.last = last
      end

      local tmpVec = vec3(0.7, ego.width, 0.7)
      for i = 1, count - 1 do
        local n = visDebug.trajecRec[1 + (last + i) % count]
        debugDrawer:drawSquarePrism(visDebug.trajecRec[1 + (last + i - 1) % count][1], n[1], tmpVec, tmpVec,
          color(255 * sqrt(abs(n[3])), 255 * sqrt(n[2]), 0, 100))
      end
    elseif false and M.debugMode == 'rays' then
      local egoScanLength = ego.length * 0.9 -- small adjustments for origins to be a bit inside the car
      local egoScanWidth = ego.width * 0.7
      local shiftHorizontalVec = (egoScanWidth * 0.5) * ego.rightVec -- creation of horizontal helper vector
      local shiftVerticalVec = -0.05 * ego.length * ego.dirVec -- creation of vertical helper vector
      local shiftPerpendicularVec = 0.35 * ego.upVec
      local egoPosElevatedR = ego.pos:copy() -- creation of FR corner vector
      egoPosElevatedR:setAdd(shiftHorizontalVec)
      egoPosElevatedR:setAdd(shiftVerticalVec)
      egoPosElevatedR:setAdd(shiftPerpendicularVec) -- elevation, this should work only on flat inclination for now
      local egoPosElevatedL = egoPosElevatedR:copy() -- creation of FL corner vector
      shiftHorizontalVec:setScaled(-2)
      egoPosElevatedL:setAdd(shiftHorizontalVec)
      local egoPosBackElevatedL = egoPosElevatedL:copy() -- creation of BR corner vector
      shiftVerticalVec:setScaled(egoScanLength * 20 / ego.length)
      egoPosBackElevatedL:setAdd(shiftVerticalVec)
      local egoPosBackElevatedR = egoPosElevatedR:copy() -- creation of BL corner vector
      egoPosBackElevatedR:setAdd(shiftVerticalVec)

      local rayDist = 4 * ego.wheelBase -- TODO: Optimize rayDist. Higher is better for more open spaces but w performance hit
      populateOBBinRange(rayDist)
      local tmpVec = vec3()
      local helperVec = vec3()
      local rounds = twt.idx - 1
      dump("rounds in debug", rounds)

      for i = twt.idx, rounds do -- the ray cast loop: each iteration scans one corner and one side
        i = i % 8 + 1
        local j = i * 4
        tmpVec:setLerp(twt.posTable[twt.RRT[j - 3]], twt.posTable[twt.RRT[j - 2]], twt.blueNoiseCoef)
        helperVec:setLerp(twt.dirTable[twt.RRT[j - 1]], twt.dirTable[twt.RRT[j]], twt.blueNoiseCoef) -- LERP corner direction
        helperVec:normalize()
        local rayLen = castRay(tmpVec, helperVec, min(twt.rayMins[i], rayDist))

        local shiftVec = helperVec * rayLen
        local rayHitPos = tmpVec + shiftVec
        debugDrawer:drawCylinder(tmpVec, rayHitPos, 0.02, color(255, 255, 255, 255))

        if twt.rayMins[i] > rayLen then -- odd index is corners
          twt.minRayCoefs[i] = twt.blueNoiseCoef
          twt.rayMins[i] = rayLen
        else
          tmpVec:setLerp(twt.posTable[twt.RRT[j - 3]], twt.posTable[twt.RRT[j - 2]], twt.minRayCoefs[i])
          helperVec:setLerp(twt.dirTable[twt.RRT[j - 1]], twt.dirTable[twt.RRT[j]], twt.minRayCoefs[i]) -- LERP corner direction
          helperVec:normalize()
          twt.rayMins[i] = castRay(tmpVec, helperVec, min(2 * twt.rayMins[i], rayDist))
        end
      end

      -- debugDrawer:drawSphere(0.3, ego.pos, color(255,255,0,255))
      -- dump(obj)
      -- local test = obj:getCornerPosition(0)
      -- local test2 = obj:getCornerPosition(1)
      -- local test3 = obj:getCornerPosition(2)
      -- local test4 = obj:getCornerPosition(3)
      -- local frontPos = obj:getFrontPosition()

      -- dump(test)
      -- debugDrawer:drawSphere(0.1, frontPos, color(255,255,0,255))
      -- debugDrawer:drawSphere(0.3, test2, color(255,0,0,255))
      -- debugDrawer:drawSphere(0.3, test3, color(255,0,0,255))
      -- debugDrawer:drawSphere(0.3, test4, color(255,0,0,255))
      -- debugDrawer:drawSphere(0.3, test, color(255,0,0,255))
      -- local test2 = obj:getFrontPositionRelative()
      -- dump(test2)
      -- debugDrawer:drawSphere(0.3, ego.wheelBase[2], color(255,255,0,255))
      -- ray origins
      -- debugDrawer:drawSphere(0.1, ego.pos, color(0,0,0,255))
      debugDrawer:drawSphere(0.1, twt.posTable[1], color(255, 255, 255, 255))
      debugDrawer:drawSphere(0.1, twt.posTable[2], color(255, 255, 255, 255))
      debugDrawer:drawSphere(0.1, twt.posTable[3], color(255, 255, 255, 255))
      debugDrawer:drawSphere(0.1, twt.posTable[4], color(255, 255, 255, 255))

      for _, d in pairs(visDebug.debugSpots) do
        debugDrawer:drawSphere(0.2, d[1], d[2])
      end
    end
  end

  if false then
    local c, x, y, z = getObjectBoundingBox(objectId) -- center, front vec, left vec, up vec
    local col = color(255, 0, 0, 255)
    drawOBB(c, x, y, z, col)

    if dt then
      for k, v in pairs(mapmgr.getObjects()) do
        if k ~= objectId then
          -- dump(v.pos)
          obj.debugDrawProxy:drawSphere(0.2, v.pos, color(0, 0, 255, 255))
          local vPos = vec3(obj:getObjectFrontPosition(k))
          obj.debugDrawProxy:drawSphere(0.2, vPos, color(0, 0, 255, 255))
          obj.debugDrawProxy:drawSphere(0.2, vPos + v.vel * dt, color(0, 255, 0, 255))
          -- dump(obj:getObjectDirectionVector(k))
          -- dump(obj:getObjectDirectionVectorUp(k))
        end
      end
    end
    obj.debugDrawProxy:drawSphere(0.15, obj:getFrontPosition(), color(255, 0, 0, 255))
  end

  --[[
  if true then
    -- Draw vehicle ref node, wheel hub positions and wheel contact points (estimates) with ground
    local refNodePos = obj:getPosition()
    debugDrawer:drawSphere(0.1, refNodePos, color(255,0,0,255))
    for _, wheel in pairs(wheels.wheels) do
      local wheelRadius = wheel.radius
      local wheelPosAbsolute = refNodePos + obj:getNodePosition(wheel.node1)
      debugDrawer:drawSphere(0.1, wheelPosAbsolute, color(255,0,0,255))
      local contactPointPos = wheelPosAbsolute - obj:getDirectionVectorUp() * wheelRadius
      debugDrawer:drawSphere(0.1, contactPointPos, color(255,0,0,255))
    end

    -- vehicle frontPos
    local vehFrontPos = obj:getFrontPosition()
    debugDrawer:drawSphere(0.1, obj:getFrontPosition(), color(255, 255, 255, 255))
    -- vehicle frontPos
    debugDrawer:drawSphere(0.1, vehFrontPos:z0(), color(0, 255, 255, 255))

    -- calculated spawn pos (from script front pos)
    debugDrawer:drawSphere(0.1, vec3(736.8858419,102.6078886,0.1169999319), color(0, 0, 255, 255))

    -- script first pos (ground truth)
    debugDrawer:drawSphere(0.1, vec3(734.9434413112983, 102.21897064457461, 1.0), color(255, 0, 255, 255))

    -- Draw world reference Frame
    debugDrawer:drawSphere(0.1, vec3(0, 0, 0), color(0, 255, 0, 255)) -- World 0
    debugDrawer:drawCylinder(vec3(0, 0, 0), 5 * vec3(1, 0, 0), 0.05, color(0, 255, 0, 255)) -- x (green)
    debugDrawer:drawCylinder(vec3(0, 0, 0), 5 * vec3(0, 1, 0), 0.05, color(255, 0, 0, 255)) -- y (red)
    debugDrawer:drawCylinder(vec3(0, 0, 0), 5 * vec3(0, 0, 1), 0.05, color(0, 0, 255, 255)) -- z (blue)
  end
  --]]
end

local function setAvoidCars(v)
  M.extAvoidCars = v
  if M.extAvoidCars == 'off' or M.extAvoidCars == 'on' then
    opt.avoidCars = M.extAvoidCars
  else
    opt.avoidCars = M.mode == 'manual' and 'off' or 'on'
  end
  stateChanged()
end

local function driveInLane(v)
  if v == 'on' then
    M.driveInLaneFlag = 'on'
    opt.driveInLaneFlag = true
  else
    M.driveInLaneFlag = 'off'
    opt.driveInLaneFlag = false
  end
  stateChanged()
end

local function setVehicleDebugMode(newMode)
  tableMerge(M, newMode)
  if M.debugMode ~= 'trajectory' then
    visDebug.trajecRec = {
      last = 0
    }
  end
  if M.debugMode ~= 'route' then
    visDebug.routeRec = {
      last = 0
    }
  end
  if M.debugMode ~= 'speeds' then
    visDebug.trajecRec = {
      last = 0
    }
  end
  if M.debugMode ~= 'rays' then
    visDebug.trajecRec = {
      last = 0
    }
  end
  if M.debugMode ~= 'off' then
    M.debugDraw = debugDraw
  else
    M.debugDraw = nop
  end
end

local function setMode(mode)
  if tableSizeC(wheels.wheels) == 0 then
    return
  end
  if mode ~= nil then
    if M.mode ~= mode then -- new AI mode is not the same as the old one
      obj:queueGameEngineLua('onAiModeChange(' .. objectId .. ', "' .. mode .. '")')
    end
    M.mode = mode
  end

  if M.extAvoidCars == 'off' or M.extAvoidCars == 'on' then
    opt.avoidCars = M.extAvoidCars
  else
    opt.avoidCars = (M.mode == 'manual' and 'off' or 'on')
  end

  if M.mode ~= 'script' then
    if M.mode ~= 'disabled' and M.mode ~= 'stop' then
      resetMapAndRoute()

      mapmgr.requestMap()
      M.updateGFX = updateGFX
      targetSpeedDifSmoother:set(0)
      targetSpeedSmoother:set(ego.speed)

      if controller.mainController then
        if electrics.values.gearboxMode == 'realistic' then
          restoreGearboxMode = true
        end
        controller.mainController.setGearboxMode('arcade')
      end

      ego.wheelBase = calculateWheelBase()

      if M.mode == 'flee' or M.mode == 'chase' or M.mode == 'follow' then
        setAggressionMode('rubberBand')
      end

      if M.mode == 'traffic' then
        setSpeedMode('legal')
        driveInLane('on')
        setTractionModel(1)
        setSpeedProfileMode('Back')
        obj:setSelfCollisionMode(2)
        obj:setAerodynamicsMode(2)
      else
        setTractionModel(2)
        obj:setSelfCollisionMode(1)
        obj:setAerodynamicsMode(1)
      end
    end

    if M.mode == 'disabled' then
      driveCar(0, 0, 0, 0)
      M.updateGFX = nop
      currentRoute = nil
      if controller.mainController and restoreGearboxMode then
        controller.mainController.setGearboxMode('realistic')
      end
    end

    stateChanged()
    sounds.updateObjType()
  end

  visDebug.trajecRec = {
    last = 0
  }
  visDebug.routeRec = {
    last = 0
  }
end

local function setRecoverOnCrash(val)
  recover.recoverOnCrash = val
end

local function toggleTrafficMode()
  if M.mode == "traffic" then
    setMode("disabled")
    setRecoverOnCrash(false)
  else
    setMode("traffic")
    setRecoverOnCrash(true)
  end
end

local function reset() -- called when the user pressed I
  M.manualTargetName = nil
  resetInternalStates()

  throttleSmoother:set(0)
  smoothTcs:set(1)

  if M.mode ~= 'disabled' then
    driveCar(0, 0, 0, 0)
    setMode() -- some scenarios don't work if this is changed to setMode('disabled')
  end
  stateChanged()

  if dataLogger.logData ~= nop then
    startStopDataLog()
  end
end

local function resetLearning()
end

local function setState(newState)
  if tableSizeC(wheels.wheels) == 0 then
    return
  end

  if newState.mode and newState.mode ~= M.mode then -- new AI mode is not the same as the old one
    obj:queueGameEngineLua('onAiModeChange(' .. objectId .. ', "' .. newState.mode .. '")')
  end

  local mode = M.mode
  tableMerge(M, newState)
  setAggressionExternal(M.extAggression)

  -- after a reload (cntr-R) vehicle should be left with handbrake engaged if mode is disabled
  -- preserve initial state of vehicle controls (handbrake engaged) if current mode and new mode are both 'disabled'
  if not (mode == 'disabled' and M.mode == 'disabled') then
    setMode()
  end

  setVehicleDebugMode(M)
  setTargetObjectID(M.targetObjectID)
  stateChanged()
end

local function setTarget(wp)
  M.manualTargetName = wp
  validateInput = validateUserInput
  wpList = {wp}
end

local function setPath(path)
  manualPath = path
  validateInput = validateUserInput
end

local function driveUsingPath(arg)
  --[[ At least one argument of either path or wpTargetList or script must be specified. All other arguments are optional.

  * path: A sequence of waypoint names that form a path by themselves to be followed in the order provided.
  * wpTargetList: A sequence of waypoint names to be used as succesive targets ex. wpTargetList = {'wp1', 'wp2'}.
                  Between any two consequitive waypoints a shortest path route will be followed.
  * script: A sequence of positions from a user-defined trajectory. For each node/position we have
            * mandatory properties:
              * x, y, z global coordinates of the corresponding node
            * optional properties:
              * r, the width of the path built using the scripted trajectory at the corresponding node
              * vl, roadSpeedLimit for the corresponding node. If this value is specified and routeSpeedMode is set to 'legal', vl will act as an upper bound for the speed profile
              * v, speed value for the corresponding node. If this value is specified the ai is forced to reach it at the corresponding node, skipping awareness and routeSpeedLimit (if they are enabled)

  -- Optional Arguments --
  * wpSpeeds: Type: (key/value pairs, key: "node_name", value: speed, number in m/s)
              Define target speeds for individual waypoints. The ai will try to meet this speed when at the given waypoint.
  * noOfLaps: Type: number. Default value: nil
              The number of laps if the path is a loop. If not defined, the ai will just follow the succesion of waypoints once.
  * routeSpeed: A speed in m/s. To be used in tandem with "routeSpeedMode".
                Type: number
  * routeSpeedMode: Values: 'limit': the ai will not go above the 'routeSpeed' defined by routeSpeed.
                            'set': the ai will try to always go at the speed defined by "routeSpeed".
  * driveInLane: Values: 'on' (anything else is considered off/inactive)
                 When 'on' the ai will keep on the correct side of the road on two way roads.
                 This also affects pathFinding in that when this option is active ai paths will traverse roads in the legal direction if posibble.
                 Default: inactive
  * aggression: Value: 0.3 - 1. The aggression value with which the ai will drive the route.
                At 1 the ai will drive at the limit of traction. A value of 0.3 would be considered normal every day driving, going shopping etc.
                Default: 0.3
  * avoidCars: Values: 'on' / 'off'.  When 'on' the ai will be aware of (avoid crashing into) other vehicles on the map. Default is 'off'
  * examples:
  ai.driveUsingPath{ wpTargetList = {'wp1', 'wp10'}, driveInLane = 'on', avoidCars = 'on', routeSpeed = 35, routeSpeedMode = 'limit', wpSpeeds = {wp1 = 10, wp2 = 40}, aggression = 0.3}
  In the above example the speeds set for wp1 and wp2 will take precedence over "routeSpeed" for the specified nodes.
  --]]

  if (arg.wpTargetList == nil and arg.path == nil and arg.script == nil) or
    (type(arg.wpTargetList) ~= 'table' and type(arg.path) ~= 'table' and type(arg.script) ~= 'table') or
    (arg.wpSpeeds ~= nil and type(arg.wpSpeeds) ~= 'table') or (arg.noOfLaps ~= nil and type(arg.noOfLaps) ~= 'number') or
    (arg.routeSpeed ~= nil and type(arg.routeSpeed) ~= 'number') or
    (arg.routeSpeedMode ~= nil and type(arg.routeSpeedMode) ~= 'string') or
    (arg.driveInLane ~= nil and type(arg.driveInLane) ~= 'string') or
    (arg.aggression ~= nil and type(arg.aggression) ~= 'number') then
    return
  end

  if arg.script then
    -- Set vehicle position and orientation at the start of the path
    -- Get initial position and orientation of vehicle at start of path (possibly time offset and/or time delayed)
    local script = arg.script
    local dir, up, pos
    if script[1].dir then
      -- vehicle initial orientation vectors exist

      dir = vec3(script[1].dir)
      up = vec3(script[1].up or mapmgr.surfaceNormalBelow(vec3(script[1])))

      local frontPosRelOrig = obj:getOriginalFrontPositionRelative() -- original relative front position in the vehicle coordinate system (left, back, up)
      local vx = dir * -frontPosRelOrig.y
      local vz = up * frontPosRelOrig.z
      local vy = dir:cross(up) * -frontPosRelOrig.x
      pos = vec3(script[1]) - vx - vz - vy
      local dH = require('scriptai').wheelToGroundDist(pos, dir, up)
      pos:setAdd(dH * up)
    else
      -- vehicle initial orientation vectors don't exist
      -- estimate vehicle orientation vectors from path and ground normal

      local p1 = vec3(script[1])
      local p1z0 = p1:z0()
      local scriptPosi = vec3()
      local k
      for i = 2, #script do
        scriptPosi:set(script[i].x, script[i].y, 0)
        if p1z0:squaredDistance(scriptPosi) > 0.2 * 0.2 then
          k = i
          break
        end
      end

      if k then
        local p2 = vec3(script[k])
        dir = p2 - p1;
        dir:normalize()
        up = mapmgr.surfaceNormalBelow(p1)

        local frontPosRelOrig = obj:getOriginalFrontPositionRelative() -- original relative front position in the vehicle coordinate system (left, back, up)
        local vx = dir * -frontPosRelOrig.y
        local vz = up * frontPosRelOrig.z
        local vy = dir:cross(up) * -frontPosRelOrig.x
        pos = p1 - vx - vz - vy
        local dH = require('scriptai').wheelToGroundDist(pos, dir, up)
        pos:setAdd(dH * up)
      end
    end

    if dir then
      local rot = quatFromDir(dir:cross(up):cross(up), up)
      obj:queueGameEngineLua(
        "getObjectByID(" .. objectId .. "):resetBrokenFlexMesh();" .. "vehicleSetPositionRotation(" .. objectId .. "," ..
          pos.x .. "," .. pos.y .. "," .. pos.z .. "," .. rot.x .. "," .. rot.y .. "," .. rot.z .. "," .. rot.w .. ")")

      mapmgr.setCustomMap() -- nils mapmgr.mapData
      M.mode = 'manual'
      stateChanged()
      noOfLaps = max(arg.noOfLaps or 1, 1)
      local pathMap = require('graphpath').newGraphpath() -- create a dummy graph

      local scrCount = #arg.script
      if noOfLaps > 1 and arg.script[scrCount].x == arg.script[1].x and arg.script[scrCount].y == arg.script[1].y and
        arg.script[scrCount].z == arg.script[1].z then
        loopPath = true
      end

      local path = table.new(scrCount, 0)
      local speedProfile = table.new(0, scrCount)
      local radius = obj:getInitialWidth()

      -- set the graph node positions and widths and create the path array and speed profile data
      for i = 1, scrCount - (loopPath and 1 or 0) do -- avoid adding the last point if the scripts loops
        local node = 'wp_' .. tostring(i)
        pathMap:setPointPositionRadius(node, vec3(arg.script[i].x, arg.script[i].y, arg.script[i].z),
          arg.script[i].r or radius)
        path[i] = node
        speedProfile[node] = arg.script[i].v -- TODO: This is a problem with a looping path for first/last node speed setting
      end

      if loopPath then
        table.insert(path, path[1])
      end

      -- add the nodes and edges to the dummy graph
      for i = 1, scrCount - 1 do
        local n1id, n2id = path[i], path[i + 1]
        pathMap:uniEdge(n1id, n2id, pathMap.positions[n1id]:distance(pathMap.positions[n2id]), 1,
          arg.script[i].vl or 100, nil, false)
      end

      scriptData = deepcopy(arg)
      scriptData.mapData = pathMap
      scriptData.path = path
      scriptData.speedProfile = speedProfile
      scriptData.noOfLaps = noOfLaps
      scriptData.loopPath = loopPath
    end
  else
    setState({
      mode = 'manual'
    })

    setParameters({ -- setParameters calls tableMerge so the nil guards are not really needed
      driveStyle = arg.driveStyle or 'default',
      staticFrictionCoefMult = arg.staticFrictionCoefMult,
      lookAheadKv = max(0.1, arg.lookAheadKv or parameters.lookAheadKv),
      understeerThrottleControl = arg.understeerThrottleControl,
      oversteerThrottleControl = arg.oversteerThrottleControl,
      throttleTcs = arg.throttleTcs,
      abBrakeControl = arg.abBrakeControl,
      underSteerBrakeControl = arg.underSteerBrakeControl,
      throttleKp = arg.throttleKp,
      targetSpeedSmootherRate = arg.targetSpeedSmootherRate,
      turnForceCoef = arg.turnForceCoef,
      springForceIntegratorDispLim = arg.springForceIntegratorDispLim
    })

    noOfLaps = max(arg.noOfLaps or 1, 1)
    wpList = arg.wpTargetList
    manualPath = arg.path
    validateInput = validateUserInput
    opt.avoidCars = arg.avoidCars or 'off'

    if noOfLaps > 1 and ((wpList and wpList[2] and wpList[1] == wpList[#wpList]) or
      (manualPath and manualPath[2] and manualPath[1] == manualPath[#manualPath])) then
      loopPath = true
    end

    speedProfile = arg.wpSpeeds or {}
    setSpeed(arg.routeSpeed)
    setSpeedMode(arg.routeSpeedMode)

    driveInLane(arg.driveInLane)

    setAggressionExternal(arg.aggression)
    stateChanged()
  end
end

local function driveUsingPathWithTraffic(arg)
  -- Helper function to easily enable manual traffic mode
  -- This combines destination-oriented driving with traffic rule following
  if not arg then
    return
  end

  -- Set up the parameters with traffic-friendly defaults
  local trafficArgs = {
    wpTargetList = arg.wpTargetList,
    path = arg.path,
    script = arg.script,
    wpSpeeds = arg.wpSpeeds,
    noOfLaps = arg.noOfLaps,
    routeSpeed = arg.routeSpeed,
    routeSpeedMode = arg.routeSpeedMode,
    driveInLane = arg.driveInLane or 'on', -- Default to staying in lanes
    aggression = arg.aggression or 0.3, -- Default to conservative driving
    avoidCars = arg.avoidCars or 'on', -- Default to avoiding cars
    driveStyle = arg.driveStyle,
    staticFrictionCoefMult = arg.staticFrictionCoefMult,
    lookAheadKv = arg.lookAheadKv,
    understeerThrottleControl = arg.understeerThrottleControl,
    oversteerThrottleControl = arg.oversteerThrottleControl,
    throttleTcs = arg.throttleTcs,
    abBrakeControl = arg.abBrakeControl,
    underSteerBrakeControl = arg.underSteerBrakeControl
  }

  -- Use the regular driveUsingPath function to set up the route
  driveUsingPath(trafficArgs)

  -- Then switch to manual traffic mode
  M.mode = 'manualTraffic'
  stateChanged()
end

local function spanMap(cutOffDrivability)
  M.cutOffDrivability = cutOffDrivability or 0
  setState({
    mode = 'span'
  })
  stateChanged()
end

local function setCutOffDrivability(drivability)
  M.cutOffDrivability = drivability or 0
  stateChanged()
end

local function onDeserialized(v)
  setState(v)
  stateChanged()
end

local function dumpCurrentRoute()
  dump(currentRoute)
end

local function getParameters()
  return parameters
end

local function startRecording(recordSpeed)
  M.mode = 'script'
  scriptai = require("scriptai")
  scriptai.startRecording(recordSpeed)
  M.updateGFX = scriptai.updateGFX
end

local function stopRecording()
  M.mode = 'disabled'
  scriptai = require("scriptai")
  local script = scriptai.stopRecording()
  M.updateGFX = scriptai.updateGFX
  return script
end

local function startFollowing(...)
  local script = ...
  if script.path then
    script = script.path
  end
  if script[1] and script[1].v then
    driveUsingPath({
      script = script
    })
  else
    M.mode = 'script'
    scriptai = require("scriptai")
    scriptai.startFollowing(...)
    M.updateGFX = scriptai.updateGFX
  end
end

local function scriptStop(...)
  M.mode = 'disabled'
  scriptai = require("scriptai")
  scriptai.scriptStop(...)
  M.updateGFX = scriptai.updateGFX
end

local function scriptState()
  scriptai = require("scriptai")
  return scriptai.scriptState()
end

local function setScriptDebugMode(mode)
  scriptai = require("scriptai")
  if mode == nil or mode == 'off' then
    M.debugMode = 'all'
    M.debugDraw = nop
    return
  end

  M.debugDraw = debugDraw
  scriptai.debugMode = mode
end

local function isDriving()
  return M.updateGFX == updateGFX or (scriptai ~= nil and scriptai.isDriving())
end

-- public interface
M.driveInLane = driveInLane
M.stateChanged = stateChanged
M.reset = reset
M.setMode = setMode
M.toggleTrafficMode = toggleTrafficMode
M.setAvoidCars = setAvoidCars
M.setTarget = setTarget
M.setPath = setPath
M.setSpeed = setSpeed
M.setSpeedMode = setSpeedMode
M.setParameters = setParameters
M.getParameters = getParameters
M.setVehicleDebugMode = setVehicleDebugMode
M.setState = setState
M.getState = getState
M.debugDraw = nop
M.driveUsingPath = driveUsingPath
M.setAggressionMode = setAggressionMode
M.setAggression = setAggressionExternal
M.onDeserialized = onDeserialized
M.setTargetObjectID = setTargetObjectID
M.laneChange = laneChange
M.setStopPoint = setStopPoint
M.dumpCurrentRoute = dumpCurrentRoute
M.spanMap = spanMap
M.setCutOffDrivability = setCutOffDrivability
M.resetLearning = resetLearning
M.isDriving = isDriving
M.startStopDataLog = startStopDataLog
M.setRecoverOnCrash = setRecoverOnCrash
M.getEdgeLaneConfig = getEdgeLaneConfig
M.setPullOver = setPullOver
M.roadNaturalContinuation = roadNaturalContinuation -- for debugging
M.driveUsingPathWithTraffic = driveUsingPathWithTraffic

-- scriptai
M.startRecording = startRecording
M.stopRecording = stopRecording
M.startFollowing = startFollowing
M.stopFollowing = scriptStop
M.scriptStop = scriptStop
M.scriptState = scriptState
M.setScriptDebugMode = setScriptDebugMode
M.setTractionModel = setTractionModel
M.setSpeedProfileMode = setSpeedProfileMode

return M
