-- 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
require('mathlib')

local M = {}
--Mandatory controller parameters
M.type = "auxiliary"
M.relevantDevice = nil

--LOCAL--
local groups = {}

--Group structure:
--name = nil
--triangles = {}
--nodes = {}
--effectiveRange
--force
--refCornerNodeA = nil
--refCornerNodeB = nil
--lastRaycastResult
--lastRaycastDistance
--lastRaycastDistanceReal
--area
--minDistance

-------
local function findNode(nodeName)
  if nodeName == nil then return nil end
  for _,nd in pairs(v.data.nodes) do
    if nd.name == nodeName then
      return nd
    end
  end
  return nil
end

local function findNodeCid(nodeName)
  local node = findNode(nodeName)
  if node ~= nil then return node.cid end
  return nil
end

local function triangleArea(tri)
    local n1 = v.data.nodes[tri.id1]
    local n2 = v.data.nodes[tri.id2]
    local n3 = v.data.nodes[tri.id3]
    local pt1 = vec3(n1.pos.x, n1.pos.y, n1.pos.z)
    local pt2 = vec3(n2.pos.x, n2.pos.y, n2.pos.z)
    local pt3 = vec3(n3.pos.x, n3.pos.y, n3.pos.z)
    
    local a = pt1:distance(pt2);
    local b = pt2:distance(pt3);
    local c = pt3:distance(pt1);
    local s = (a + b + c) / 2;
    return math.sqrt(s * (s-a) * (s-b) * (s-c));
end

-------
local function init()
  for _,tri in pairs(v.data.triangles) do
    if tri.groundEffectGroup ~= nil and type(tri.groundEffectGroup) == "string" then
      local group = groups[tri.groundEffectGroup]
      
      if group == nil then
        --print("new group " .. tri.groundEffectGroup)
        
        -- init corner nodes
        local refCornerNodeA = nil
        local refCornerNodeB = nil
        if tri.groundEffectCorners ~= nil and type(tri.groundEffectCorners) == "table" then
          refCornerNodeA = findNode(tri.groundEffectCorners[1])
          refCornerNodeB = findNode(tri.groundEffectCorners[2])
        else
          print("groundEffectCorners not specified or incorrect type. Group won't work!!!")
        end
        
        if refCornerNodeA == nil or refCornerNodeB == nil then
          print("groundEffectCorners failure, can't find one or more nodes")
          if refCornerNodeA == nil then
            print("can't find first corner")
          end
          if refCornerNodeB == nil then
            print("can't find second corner")
          end
        end
      
        -- init group
        group = {}
        group.name = tri.groundEffectGroup
        group.area = 0
        group.triangles = {}
        group.nodes = {}
        group.force = tri.groundEffectForce or 0
        group.effectiveRange = tri.groundEffectEffectiveRange or 10
        group.refCornerNodeA = refCornerNodeA
        group.refCornerNodeB = refCornerNodeB
        group.lastRaycastDistance = 0
        group.minDistance = tri.groundEffectMinDistance or group.effectiveRange / 2
        group.lastRaycastResult = false
        groups[tri.groundEffectGroup] = group
      end
      
      --add triangle to table
      table.insert(group.triangles, tri)
      
      --add area
      group.area = group.area + triangleArea(tri)
    end
  end
  
  --scale forces
  for _,group in pairs(groups) do
    group.force = group.force * group.area
  end
  
  --build node map for each group
  local df = false
  for _,group in pairs(groups) do
    local nodeAddedMap = {} --keep track of already added nodes
    for _,tri in pairs(group.triangles) do
      local triNodeIds = {tri.id1, tri.id2, tri.id3}
      for _,triNodeId in pairs(triNodeIds) do
        --add triangle nodes to map
        if nodeAddedMap[triNodeId] == nil then 
          table.insert(group.nodes, v.data.nodes[triNodeId])
          nodeAddedMap[triNodeId] = true
        end
      end
    end
  end
end


local function updateGFX(dt)
  local upDir = obj:getDirectionVectorUp()
  local downDir = vec3(-upDir.x, -upDir.y, -upDir.z)
  
  --print("updir " .. tostring(upDir.x) .. "," .. tostring(upDir.y) .. "," .. tostring(upDir.z))
  --raycast 
  for _,group in pairs(groups) do
    local refCornerNodeA = group.refCornerNodeA
    local refCornerNodeB = group.refCornerNodeB
    if refCornerNodeA ~= nil and refCornerNodeB ~= nil then
      local refCornerAPos = obj:getNodePosition(refCornerNodeA.cid)
      local refCornerBPos = obj:getNodePosition(refCornerNodeB.cid)
      local refCenterPos = vec3((refCornerAPos.x + refCornerBPos.x) / 2, (refCornerAPos.y + refCornerBPos.y) / 2, (refCornerAPos.z + refCornerBPos.z) / 2) + obj:getPosition()
      local castPos = refCenterPos + (downDir * group.effectiveRange)
      obj:queueGameEngineLua("be:getObjectByID("..obj:getID().."):queueLuaCommand('controller.getController(" .. '"groundeffect"' .. ").raycastCallback(".. '"' .. group.name .. '"' .. ",'..be:castRay(vec3(" .. refCenterPos.x .. "," .. refCenterPos.y .. "," .. refCenterPos.z .. "):toPoint3F(), vec3(" .. castPos.x .. "," .. castPos.y .. "," .. castPos.z .. "):toPoint3F())..')')")
    end
  end
end

local function raycastCallback(groupName, dist)
  local group = groups[groupName]
  group.lastRaycastResult = dist ~= 0
  group.lastRaycastDistanceReal = dist
  group.lastRaycastDistance = math.max(group.minDistance, dist)

  
  if M.debugRaycast then print(groupName .. ":" .. tostring(dist)) end
end

local pFrameCount = 0

local function getVA(upDir)
  local rawVA = (0.5 - math.min(0.5, math.abs(upDir.y))) / 0.5
  return math.sqrt(1 - math.pow(rawVA - 1, 2));
end

local function update()
  --pFrameCount = pFrameCount + 1
  
  --add ground effect forces
  local upDir = obj:getDirectionVectorUp()
  local speed = obj:getVelocity():length()

  for _,group in pairs(groups) do
    if group.lastRaycastResult then
      local verticalAdjustment = getVA(upDir)
      local raycastFac = ((group.effectiveRange - group.lastRaycastDistance) / group.effectiveRange)
      local force = group.force * raycastFac * speed * verticalAdjustment
      
      --if pFrameCount == 1000 then
        --print("Adding force to group " .. group.name  .. " : " .. tostring(force) .. " with vA " .. tostring(verticalAdjustment) .. " with rF " .. tostring(raycastFac)) 
        --print("rD " .. tostring(group.lastRaycastDistance))
      --end
      
      for _,node in pairs(group.nodes) do
        local fVec = float3(force * upDir.x, force * upDir.y, force * upDir.z)
        obj:applyForceVector(node.cid, fVec)
      end
    end
  end
  
  --if pFrameCount == 1000 then pFrameCount = 0 end
end

local function reset()
  for _,group in pairs(groups) do
    --don't reset with huge forces
    group.lastRaycastResult = false 
  end
end

M.reset = reset
M.raycastCallback = raycastCallback
M.init = init
M.update = update
M.updateGFX = updateGFX

return M