--[[
in:
  RadiusTrajectoryLeft
  RadiusTrajectoryRight
  XCoordinatesCenterOfCircleTrajectory
  YCoordinatesCenterOfCircleTrajectory
  Trajectory_CConst
  VehicleWidthHalf
  
  out
  ADeviation
  InnerRadius
  RadiusDelta
--]]
function calculateTrajectoryParameters(RadiusLeft, RadiusRight, X, Y, C, VehicleWidthHalf)

  local R = nil
  local M = 1
  local RadiusDelta = math.abs(RadiusLeft - RadiusRight)

  if Y > 0 then
    R = RadiusRight
    M = -1
  else
    R = RadiusLeft
    M = 1
  end
  
  local A = M * (R - math.sqrt(R*R + C*C))
  
  -- Y inside vehicle or radius greater than 1000m
  if (math.abs(Y) < VehicleWidthHalf) or (math.max(RadiusLeft, RadiusRight) > 100000) or math.abs(Y) > 100000 then
    A = 0
    RadiusDelta = VehicleWidthHalf * 2
  end
  
  --print(string.format("RadiusLeft=%f RadiusRight=%f X=%f Y=%f C=%f w=%f ==> A=%f, R=%f Delta=%f", RadiusLeft, RadiusRight, X, Y, C, VehicleWidthHalf, A, R, RadiusDelta))

  return A, R, RadiusDelta
  
end

function largest(t)
  local maxvalue = t[1]
  for _, value in pairs(t) do
    maxvalue = math.max(maxvalue, value)
  end
  return maxvalue
end

function convertDistance(distance)

  local threshold_low = 20
  local threshold_high = 60
  -- 0 ... low == 1
  -- low ... high == 1 ... 0
  -- > high == 0
  local result = 0
  if distance <= threshold_low then  
    result = 1
  elseif distance <= threshold_high then
    result = (threshold_high - distance) / (threshold_high - threshold_low)
  end

  return result
  
end

function calculatePDCFrontRear( 
  Sector1LeftDistance,
  Sector2LeftDistance,
  Sector3LeftDistance,
  Sector4LeftDistance,
  Sector1LeftDisabled,
  Sector2LeftDisabled,
  Sector3LeftDisabled,
  Sector4LeftDisabled,
  Sector1LeftOpacity, 
  Sector2LeftOpacity, 
  Sector3LeftOpacity, 
  Sector4LeftOpacity, 
    
  Sector1RightDistance,
  Sector2RightDistance,
  Sector3RightDistance,
  Sector4RightDistance,
  Sector1RightDisabled,
  Sector2RightDisabled,
  Sector3RightDisabled,
  Sector4RightDisabled,
  Sector1RightOpacity,
  Sector2RightOpacity,
  Sector3RightOpacity,
  Sector4RightOpacity )

  local Left = 0
  local Middle = 0
  local Right = 0
  
  Sector1Left = convertDistance(Sector1LeftDistance) * (1.0 - Sector1LeftDisabled) * Sector1LeftOpacity
  Sector2Left = convertDistance(Sector2LeftDistance) * (1.0 - Sector2LeftDisabled) * Sector2LeftOpacity
  Sector3Left = convertDistance(Sector3LeftDistance) * (1.0 - Sector3LeftDisabled) * Sector3LeftOpacity
  Sector4Left = convertDistance(Sector4LeftDistance) * (1.0 - Sector4LeftDisabled) * Sector4LeftOpacity
  
  Sector1Right = convertDistance(Sector1RightDistance) * (1.0 - Sector1RightDisabled) * Sector1RightOpacity
  Sector2Right = convertDistance(Sector2RightDistance) * (1.0 - Sector2RightDisabled) * Sector2RightOpacity
  Sector3Right = convertDistance(Sector3RightDistance) * (1.0 - Sector3RightDisabled) * Sector3RightOpacity
  Sector4Right = convertDistance(Sector4RightDistance) * (1.0 - Sector4RightDisabled) * Sector4RightOpacity
  
  Left    = math.min(1.0, largest({Sector1Left, Sector2Left, Sector3Left}))
  Middle  = math.min(1.0, largest({Sector3Left, Sector4Left, Sector3Right, Sector4Right}))
  Right   = math.min(1.0, largest({Sector1Right, Sector2Right, Sector3Right}))
  
  return Left, Middle, Right
  
end

function calculatePDCMiddle(
  Sector1LeftDistance,
  Sector2LeftDistance,
  Sector3LeftDistance,
  Sector4LeftDistance,
  Sector5LeftDistance,
  Sector6LeftDistance,
  Sector1LeftDisabled,
  Sector2LeftDisabled,
  Sector3LeftDisabled,
  Sector4LeftDisabled,
  Sector5LeftDisabled,
  Sector6LeftDisabled,
  Sector1LeftOpacity,
  Sector2LeftOpacity,
  Sector3LeftOpacity,
  Sector4LeftOpacity,
  Sector5LeftOpacity,
  Sector6LeftOpacity,

  Sector1RightDistance,
  Sector2RightDistance,
  Sector3RightDistance,
  Sector4RightDistance,
  Sector5RightDistance,
  Sector6RightDistance,
  Sector1RightDisabled,
  Sector2RightDisabled,
  Sector3RightDisabled,
  Sector4RightDisabled,
  Sector5RightDisabled,
  Sector6RightDisabled,
  Sector1RightOpacity,
  Sector2RightOpacity,
  Sector3RightOpacity,
  Sector4RightOpacity,
  Sector5RightOpacity,
  Sector6RightOpacity )

  
  local Left  = 0.0
  local Right = 0.0
  
  Sector1Left = convertDistance(Sector1LeftDistance) * (1.0 - Sector1LeftDisabled) * Sector1LeftOpacity
  Sector2Left = convertDistance(Sector2LeftDistance) * (1.0 - Sector2LeftDisabled) * Sector2LeftOpacity
  Sector3Left = convertDistance(Sector3LeftDistance) * (1.0 - Sector3LeftDisabled) * Sector3LeftOpacity
  Sector4Left = convertDistance(Sector4LeftDistance) * (1.0 - Sector4LeftDisabled) * Sector4LeftOpacity
  Sector5Left = convertDistance(Sector5LeftDistance) * (1.0 - Sector5LeftDisabled) * Sector5LeftOpacity
  Sector6Left = convertDistance(Sector6LeftDistance) * (1.0 - Sector6LeftDisabled) * Sector6LeftOpacity
  
  Sector1Right = convertDistance(Sector1RightDistance) * (1.0 - Sector1RightDisabled) * Sector1RightOpacity
  Sector2Right = convertDistance(Sector2RightDistance) * (1.0 - Sector2RightDisabled) * Sector2RightOpacity
  Sector3Right = convertDistance(Sector3RightDistance) * (1.0 - Sector3RightDisabled) * Sector3RightOpacity
  Sector4Right = convertDistance(Sector4RightDistance) * (1.0 - Sector4RightDisabled) * Sector4RightOpacity
  Sector5Right = convertDistance(Sector5RightDistance) * (1.0 - Sector5RightDisabled) * Sector5RightOpacity
  Sector6Right = convertDistance(Sector6RightDistance) * (1.0 - Sector6RightDisabled) * Sector6RightOpacity
  
  Left  = math.min( 1.0, largest({Sector1Left, Sector2Left, Sector3Left, Sector4Left, Sector5Left, Sector6Left}))
  Right = math.min( 1.0, largest({Sector1Right, Sector2Right, Sector3Right, Sector4Right, Sector5Right, Sector6Right}))
  
  return Left, Right
  
end

function splitVec3(vector)

  local x = 0
  local y = 0
  local z = 0
  
  if vector ~= nil then
    x = vector.x
    y = vector.y
    z = vector.z
  end

  return x, y, z
  
end