local U = require "scriptbank\\utillib"
local Q  = require "scriptbank\\quatlib"
local rad   = math.rad
local deg   = math.deg 
local floor = math.floor
local atan  = math.atan2
local abs   = math.abs
local sqrt  = math.sqrt
local targetAng = {}

local maxSpeed = 40 --how fast the boid can move
local turnSpeed = 0.04 --how quickly the boid turns
local neighbourRange = 800 --how far to detect nearby boids 
local avoidRange = 500 --how close obstacles start to be detected - larger boids need larger range
local homeToStart = 1 --set to 1 if you want the boid to head back towards its starting location if it gets too far 
local maxRangeFromStart = 2500 --how far the boid can move away from its starting location before homing back again
--weights should be 0 ~ 1
local groupUpWeight = 0.3 --how much focus to apply to getting to center of group (of inGroup)
local avoidWeight = 1 --focus to apply on avoiding eachother
local alignWeight = 0.7 --focus to apply on moving in the same direction as inGroup 
local homeWeight = 1 --focus to apply on returning home 

local avoidRangeSqr = avoidRange^2
local neighbourRangeSqr = neighbourRange^2
local maxRangeFromStartSqr = maxRangeFromStart^2

local frameTime = 0
local velx,vely,velz = 0,0,0

local thisFlock = {}

local pressE = 0
local showPrompt = 0
local startx = {}
local starty = {}
local startz = {}
local x4 = {}
local y4 = {}
local z4 = {}

local upDateDelay = 50
local updateTime = {}

function flocking_init(e)
	thisFlock[e] = e
	CollisionOff(e)	
	startx[e] = g_Entity[e]['x']
	starty[e] = g_Entity[e]['y']
	startz[e] = g_Entity[e]['z']
	updateTime[e] = g_Time+math.random(upDateDelay*0.8,upDateDelay*1.2)
	x4[e] = 0
	y4[e] = 0
	z4[e] = 0
end 

function flocking_main(e)
	frameTime = GetTimeElapsed()	
	--loop through all entitys in this flock 
	local x1,y1,z1 = GetEntityPosAng(e)
	if g_Time > updateTime[e] then 
		updateTime[e] = g_Time+math.random(upDateDelay*0.8,upDateDelay*1.2)
		local x2,y2,z2 = 0,0,0
		local x3,y3,z3 = 0,0,0
		local isNeighbour = {}
		local inGroup = 0
		local isAvoiding = 0
		local headHome = 0
		--avoid crashing into eachother
		for a,_ in pairs (thisFlock) do 
			local x2,y2,z2 = GetEntityPosAng(a)
			local dx,dy,dz = x1-x2, y1-y2, z1-z2
			local dist1 = dx*dx + dy*dy + dz*dz		
			if a ~= e then --ignore self
				if dist1 < avoidRangeSqr then 
					inGroup = inGroup + 1
					x3,y3,z3 = x3+dx,y3+dy,z3+dz 
				end 
			end 
		end 
		if inGroup > 0 then 
			x3,y3,z3 = x3/inGroup,y3/inGroup,z3/inGroup
			x3 = x3 * maxSpeed * avoidWeight * frameTime
			y3 = y3 * maxSpeed * avoidWeight * frameTime
			z3 = z3 * maxSpeed * avoidWeight * frameTime
			x4[e],y4[e],z4[e] = x1+x3,y1+y3,z1+z3
			isAvoiding = 1
			updateTime[e] = 0
		else 
			--check if we should head home or not 
			headHome = 0
			if homeToStart == 1 then 
				local x2,y2,z2 = startx[e],starty[e],startz[e]
				local dx,dy,dz = x2-x1, y2-y1, z2-z1
				local dist1 = dx*dx + dy*dy + dz*dz		
				if dist1 > maxRangeFromStartSqr then 
					--[[
					if x1 > x2 then 
						x2 = -dx * maxSpeed * homeWeight * frameTime
					else 
						x2 = dx * maxSpeed * homeWeight * frameTime
					end 
					if y1 > y2 then 
						y2 = -dy * maxSpeed * homeWeight * frameTime
					else 
						y2 = dy * maxSpeed * homeWeight * frameTime
					end 
					if z1 > z2 then 
						z2 = -dz * maxSpeed * homeWeight * frameTime
					else 
						z2 = dz * maxSpeed * homeWeight * frameTime
					end 
					--]]
					x4[e],y4[e],z4[e] = x2,y2,z2
					headHome = 1
				end 
			end 
			if headHome == 0 then 
				--move to center of group
				inGroup = 0
				for a,_ in pairs (thisFlock) do 
					local x2,y2,z2 = GetEntityPosAng(a)
					local dx,dy,dz = x1-x2, y1-y2, z1-z2
					local dist1 = dx*dx + dy*dy + dz*dz		
					if dist1 < neighbourRangeSqr then 
						inGroup = inGroup + 1
						isNeighbour[inGroup] = a
					end 
				end 
				--group up 
				if inGroup > 1 then 			
					local x2,y2,z2 = 0,0,0
					for a = 1, inGroup do 				
						local x3,y3,z3 = GetEntityPosAng(isNeighbour[a])
						x2,y2,z2 = x2+x3,y2+y3,z2+z3
					end 
					x2,y2,z2 = x2/inGroup,y2/inGroup,z2/inGroup
					x2,y2,z2 = x2-x1,y2-y1,z2-z1		
					x2 = x2 * maxSpeed * groupUpWeight * frameTime
					y2 = y2 * maxSpeed * groupUpWeight * frameTime
					z2 = z2 * maxSpeed * groupUpWeight * frameTime
					x4[e],y4[e],z4[e] = x1+x2,y1+y2,z1+z2	
					local tx,ty,tz = x4[e],y4[e],z4[e]
					--now get the average group velo and head towards same direction
					x2,y2,z2 = 0,0,0
					for a = 1, inGroup do 
						local x3,y3,z3 = GetVelo(isNeighbour[a])				
						x2,y2,z2 = x2+x3,y2+y3,z2+z3
					end 
					x2,y2,z2 = x2/inGroup,y2/inGroup,z2/inGroup
					x2 = x2 * maxSpeed * alignWeight * frameTime
					y2 = y2 * maxSpeed * alignWeight * frameTime
					z2 = z2 * maxSpeed * alignWeight * frameTime
					x4[e],y4[e],z4[e] = tx+x2,ty+y2,tz+z2			
				else 
					--no1 else inGroup so just keep moving forwards - if we get a neighbour later it will automatically update
					local ax, ay, az = rad(GetEntityAngleX(e)), rad(GetEntityAngleY(e)), rad(GetEntityAngleZ(e))
					local vx, vy, vz = U.Rotate3D( 0, 0, maxSpeed, ax, ay, az )
					x4[e],y4[e],z4[e] = x1+vx,y1+vy,z1+vz
				end 
			end
			
		end 
	end
		
	--rotate to face forward direction 
	RotateToPointSlowly3D(e,x4[e],y4[e],z4[e],turnSpeed*frameTime)
	local ax, ay, az = rad(GetEntityAngleX(e)), rad(GetEntityAngleY(e)), rad(GetEntityAngleZ(e))
	x2, y2, z2 = U.Rotate3D( 0, 0, maxSpeed*frameTime, ax, ay, az )
	x4[e],y4[e],z4[e] = x1+x2,y1+y2,z1+z2
	SetPosition(e,x4[e],y4[e],z4[e])	
	--[[
	if g_KeyPressE == 1 then 
		if pressE == 0 then 
			pressE = 1 
			if showPrompt == 0 then 
				showPrompt = 1
			else 
				showPrompt = 0
			end 
		end 
	else 
		pressE = 0
	end 
	if showPrompt == 1 then 
		local tvx,tvy,tvz = GetVelo(e)
		PromptLocal(e,inGroup.." in group , isAvoiding "..isAvoiding.." , heading home "..headHome.." , current vel "..round(tvx,1).." , "..round(tvy,1).." , "..round(tvz,1))	
		--PromptLocal(e,startx[e].." , "..starty[e].." , "..startz[e])
	end 
	--]]
	
	
end 

function flocking_exit(e)


end 

function GetVelo(e)
	local ax, ay, az = rad(GetEntityAngleX(e)), rad(GetEntityAngleY(e)), rad(GetEntityAngleZ(e))
	velx, vely, velz = U.Rotate3D( 0, 0, maxSpeed, ax, ay, az )
	return velx,vely,velz
end 

function round(num, idp)
  local mult = 10^(idp or 0)
  return math.floor(num * mult + 0.5) / mult
end

function RotateToPointSlowly3D( e, x, y, z, spd)
		if x == nil or y == nil or z == nil then return end
    spd = spd or 0.05  -- default to twenty steps for SLERP function
 
    -- work out the angles from us to them
    local epx, epy, epz, eax, eay, eaz = GetEntityPosAng( e ) 
 
    eax, eay, eaz = rad( eax ), rad( eay ), rad( eaz )    
    local tA = targetAng[ e ]
    local timeNow = g_Time
     
    if tA == nil or timeNow - tA.lastTime > 200 then
        targetAng[ e ] = { lastTime = timeNow }
        return
    end
     
    if tA.za == nil or
       abs( tA.xa - eax ) > 0.01 or
       abs( tA.ya - eay ) > 0.01 or
       abs( tA.za - eaz ) > 0.01 then
         
        local DX, DY, DZ = x - epx, y - epy, z - epz
 
        local xAng = -atan( DY, sqrt( DX*DX + DZ*DZ ) )
     
        local newQ = Q.FromEuler( 0, atan( DX, DZ ), 0 )
     
        newQ = Q.Mul( newQ, Q.FromEuler( xAng, 0, 0 ) ) 
     
        tA.lastTime = timeNow
        tA.xa, tA.ya, tA.za = Q.ToEuler( newQ )
         
        newQ = Q.SLerp( Q.FromEuler( eax, eay, eaz ), newQ, spd )
                         
        local xr, yr, zr = Q.ToEuler( newQ )
     
        SetRotation( e, deg( xr ), deg( yr ), deg( zr ) )
    end
end