-- 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 cachedObjectWeights = {}
local trackingTimer = 0
local cacheVerificationTimer = 0
local additionalWeight = 0

--helpers
local function fvec3(data)
  return vec3(data.x, data.z, data.y)
end

local function sign (p1, p2, p3)
    return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y)
end

local function findHeightInTriangle(pt, p1, p2, p3)
    local x, y = pt.x, pt.y

    local det = (p2.y - p3.y) * (p1.x - p3.x) + (p3.x - p2.x) * (p1.y - p3.y)

    local l1 = ((p2.y - p3.y) * (x - p3.x) + (p3.x - p2.x) * (y - p3.y)) / det
    local l2 = ((p3.y - p1.y) * (x - p3.x) + (p1.x - p3.x) * (y - p3.z)) / det
    local l3 = 1.0 - l1 - l2;

    return l1 * p1.z + l2 * p2.z + l3 * p3.z
end
        
local function isPointInTriangle (pt, v1, v2, v3)
  local b1, b2, b3

  b1 = sign(pt, v1, v2) < 0.0
  b2 = sign(pt, v2, v3) < 0.0
  b3 = sign(pt, v3, v1) < 0.0

  return ((b1 == b2) and (b2 == b3))
end

local function isPointWithinVolume (pt, volume)
  --construct our triangles
  --["name",       "bottomTopLeft:", "bottomTopRight:", "bottomBottomLeft:", "bottomBottomRight:", "topTopLeft:", "topTopRight:", "topBottomLeft:", "topBottomRight:"],
  local bottomTris = {{vec3(obj:getNodePosition(volume.bottomBottomLeft)), vec3(obj:getNodePosition(volume.bottomBottomRight)), vec3(obj:getNodePosition(volume.bottomTopLeft))},
                      {vec3(obj:getNodePosition(volume.bottomBottomRight)), vec3(obj:getNodePosition(volume.bottomTopRight)), vec3(obj:getNodePosition(volume.bottomTopLeft))}}
                      
  local topTris = {{vec3(obj:getNodePosition(volume.topBottomLeft)), vec3(obj:getNodePosition(volume.topBottomRight)), vec3(obj:getNodePosition(volume.topTopLeft))},
                   {vec3(obj:getNodePosition(volume.topBottomRight)), vec3(obj:getNodePosition(volume.topTopRight)), vec3(obj:getNodePosition(volume.topTopLeft))}}
  
  local preferredTopTriangle = nil
  local preferredBottomTriangle = nil
  
  local topHeight = 0
  local bottomHeight = 0
  
  --find preferred triangles
  for i=1,2,1 do
    local withinBottomTriangle = isPointInTriangle(pt, bottomTris[i][1], bottomTris[i][2], bottomTris[i][3])
    local withinTopTriangle = isPointInTriangle(pt, topTris[i][1], topTris[i][2], topTris[i][3])
    
    if withinTopTriangle then preferredTopTriangle = topTris[i] end
    if withinBottomTriangle then preferredBottomTriangle = bottomTris[i] end
  end
  
  --if we only have one preferred triangle, use the closest point as a position instead
  if preferredTopTriangle == nil and preferredBottomTriangle ~= nil then
    local pt2d = pt:z0()
    local distance = 9999
    
    --compute distance
    for i=1,2,1 do
      for j=1,3,1 do
        local triPoint = topTris[i][j]
        local vec2d = triPoint:z0()
        
        --
        local dist = vec2d:distance(pt2d)
        if dist < distance then
          distance = dist
          topHeight = triPoint.z
        end
      end
    end
  elseif preferredBottomTriangle == nil and preferredTopTriangle ~= nil then
    local pt2d = pt:z0()
    local distance = 9999
    
    --compute distance
    for i=1,2,1 do
      for j=1,3,1 do
        local triPoint = bottomTris[i][j]
        local vec2d = triPoint:z0()
        
        --
        local dist = vec2d:distance(pt2d)
        if dist < distance then
          distance = dist
          bottomHeight = triPoint.z
        end
      end
    end
  end
  
  --compute triangle heights
  if preferredBottomTriangle ~= nil then
    bottomHeight = findHeightInTriangle(pt, preferredBottomTriangle[1], preferredBottomTriangle[2], preferredBottomTriangle[3])
  end
  
  if preferredTopTriangle ~= nil then
    topHeight = findHeightInTriangle(pt, preferredTopTriangle[1], preferredTopTriangle[2], preferredTopTriangle[3])
  end
  
  --check if the point is within top/bottom heights
  --if so, the vehicle is within this volume
  if (pt.z >= bottomHeight and pt.z <= topHeight) and (preferredBottomTriangle ~= nil or preferredTopTriangle ~= nil) then
    return true
  end
  
  --print("DEBUG IF FALSE")
  --print("pt.z ............. " .. tostring(pt.z))
  --print("bottomHeight ..... " .. tostring(bottomHeight))
  --print("topHeight ........ " .. tostring(bottomHeight))
  --print("topTriPreferred... " .. tostring(preferredTopTriangle))
  --print("btmTriPreferred... " .. tostring(preferredBottomTriangle))
  return false
end

--API
local function registerObject(id, weight)
  cachedObjectWeights[id] = weight
end

local function unregisterObject(id)
  cachedObjectWeights[id] = weight
end

--extension
local function onInit()
  mapmgr.requestMap() --If there is a place you got to get, I can get you there I bet, I'm the Map!
	cachedObjectWeights = {}
  additionalWeight = 0
  
  --get static weight
  for _, node in pairs(v.data.nodes) do
    local static = node.staticSensorLoad or false
    if static then 
      additionalWeight = additionalWeight + node.nodeWeight
    end
  end
end

local function onReset()
	onInit()
end

local function updateGFX(dt)
  --don't update at all if we're flying! lock the sensor in it's current state!
  local speedMS = obj:getVelocity():length()
  if speedMS >= 15 then return end
  
  --get map objects
  local mapObjects = mapmgr.objects or {}
  
  --get vehicle weights
  for _,mapObject in pairs(mapObjects) do
    --not us?
    if mapObject.id == obj:getID() then goto continue end
    
    local withinAnyVolume = false
    --check if this object is within our weight volumes
    for _, volume in pairs(v.data.sensorVolumes or {}) do
      local withinVolume = isPointWithinVolume(vec3(mapObject.pos) - vec3(obj:getPosition()), volume)
      if withinVolume then withinAnyVolume = true end
    end
    
    --any volume contained this?
    if withinAnyVolume and cachedObjectWeights[mapObject.id] == nil then 
        --print("Registering " .. tostring(mapObject.id))
        obj:queueObjectLuaCommand(mapObject.id, "obj:queueObjectLuaCommand("..obj:getID()..", 'weightsensor.registerObject('..obj:getID()..','..obj:calcBeamStats().total_weight..')')")
    elseif withinAnyVolume == false and cachedObjectWeights[mapObject.id] ~= nil then
        --print("Unregistering " .. tostring(mapObject.id))
        unregisterObject(mapObject.id)
    end 

    ::continue::
  end
  
  --verify tables
  cacheVerificationTimer = cacheVerificationTimer + dt
  if cacheVerificationTimer > 0.25 then
    for k,v in pairs(cachedObjectWeights) do
      local found = false
      for k2,v2 in pairs(mapObjects) do
        if v2.id == k then found = true end
      end
      
      --do we have this?
      if not found then
        --print("Object " .. tostring(k) .. " biffed")
        unregisterObject(k)
      end
    end
    
    cacheVerificationTimer = 0
  end
  
  --build the electric
  local totalWeight = 0
  for k,v in pairs(cachedObjectWeights) do
    totalWeight = totalWeight + v
  end
  electrics.values.weightsensor = totalWeight + additionalWeight

  --track all vehicles (YAY HACKS :3)
  trackingTimer = trackingTimer - dt
  if trackingTimer <= 0 then
    obj:queueGameEngineLua("be:queueAllObjectLua('mapmgr.enableTracking()')")
    trackingTimer = 5
  end
end

-- public interface
M.registerObject = registerObject
M.onInit      = onInit
M.onReset     = onReset
M.updateGFX = updateGFX

return M