local assertArgs = include "assertArgs"

local util = include "util"

local Boid = class "boids.Boid"
local SelfControlledBoid = lib "class" ("SelfControlledBoid", Boid)

local COHESION_FACTOR = 0.5
local COHESION_RANGE = 10

local AVOIDANCE_FACTOR = 9--10 -- not inverted
local AVOIDANCE_DISTANCE = 4
local MIN_AVOID = 0.5 

local ALIGNMENT_FACTOR = 0.1 -- inverted
local ALIGNMENT_RANGE = 10

local GOAL_FACTOR = 0.3 -- inverted
local GOAL_LIMIT = 0.1

local BOUNDS_FACTOR = 2 -- speed that should be applied
local MAX_SPEED = 1
local AVOID_PREDATOR_FACTOR = 1
local AVOID_PREDATOR_LIMIT = 10
local AVOID_PREDATOR_MIN_DISTANCE = 2

function SelfControlledBoid:initialize(args)
    Boid.initialize(self, args)
    return self
end

function SelfControlledBoid:update(dt)

    -- boid behaviour using three rules
    local cohesion = self:cohesion(dt)
    local avoidance = self:avoidance(dt)
    local alignment = self:alignment(dt)

    -- additional tweaks
    -- local constrainToBounds = self:constrainToBounds(dt)
    local goal = self:goal(dt)
    -- local limitSpeed = self:limitSpeed(dt)
    -- local avoidPredator = self:avoidPredator(dt)

    -- apply force
    local force = lovr.math.vec3(0, 0, 0)
    force:add(cohesion)
    force:add(avoidance)
    force:add(alignment)
    -- force:add(constrainToBounds)
    force:add(goal)
    -- force:add(limitSpeed)
    -- force:add(avoidPredator)
    self:setFeedback(force)
    local fx,fy,fz=force:unpack()
    self:getCollider():setLinearVelocity(fx,0,fz)
    -- self:getCollider():applyForce(1,0,0)
    -- 
    -- set target orientation
    local quat = lovr.math.quat(lovr.math.vec3(self:getCollider():getLinearVelocity()))
    self:setTargetQuat(quat)
    
    
    Boid.update(self, dt)

end

--[[ BOID BEHAVIOUR ]]-- 

function SelfControlledBoid:cohesion(dt)
    local dir = lovr.math.vec3(0, 0, 0)
    local n_attract = 0 
    local selfPosition = lovr.math.vec3( self:getCollider():getPosition() )
    local n = self:getSwarm():forAllNeighbours(self, function (boid)
        local boidPosition = lovr.math.vec3( boid:getCollider():getPosition() )
        local distance = selfPosition:distance(boidPosition)
        
        if distance < COHESION_RANGE then 
            n_attract = n_attract + 1 
            dir:add(boidPosition:sub(selfPosition))
        end
    end)
    dir:div(n_attract) -- center of mass
    -- dir:sub(lovr.math.vec3(self:getCollider():getPosition()))
    dir:mul(COHESION_FACTOR)
    
    return dir
end

function SelfControlledBoid:avoidance(dt)
    local dir = lovr.math.vec3(0, 0, 0)
    local selfPosition = lovr.math.vec3( self:getCollider():getPosition() )
    local n = 0
    self:getSwarm():forAllNeighbours(self, function (boid)
        local boidPosition = lovr.math.vec3( boid:getCollider():getPosition() )
        local distance = selfPosition:distance(boidPosition)
        if distance < AVOIDANCE_DISTANCE then
            n=n+1
            local diff = ((AVOIDANCE_DISTANCE- distance)/AVOIDANCE_DISTANCE)
            if distance < MIN_AVOID then distance = MIN_AVOID end
            dir:add((boidPosition:sub(selfPosition)):mul(diff*diff))
        end
    end)
    dir:div(n)
    dir:mul(-1*AVOIDANCE_FACTOR)
    -- print(dir)
    return dir
end

function SelfControlledBoid:alignment(dt)
    local dir = lovr.math.vec3(0, 0, 0)
    local n = self:getSwarm():forAllNeighbours(self, function (boid)
        d=lovr.math.vec3(boid:getCollider():getPosition())
        if d:length() < COHESION_RANGE then 
            dir = dir:add( boid:getCollider():getLinearVelocity() ) 
        end 
        
    end)
    dir:div(n) -- average velocity
    dir:sub(lovr.math.vec3(self:getCollider():getLinearVelocity()) )
    dir:mul(ALIGNMENT_FACTOR)
    return dir
end

function SelfControlledBoid:constrainToBounds(dt)
    local vx, vy, vz = 0, 0, 0
    local x, y, z = self:getCollider():getPosition()
    local minX, maxX, minY, maxY, minZ, maxZ = self:getSwarm():getBounds():unpack()
    if x < minX then vx = BOUNDS_FACTOR elseif x > maxX then vx = -BOUNDS_FACTOR end
    if y < minY then vy = BOUNDS_FACTOR elseif y > maxY then vy = -BOUNDS_FACTOR end
    if z < minZ then vz = BOUNDS_FACTOR elseif z > maxZ then vz = -BOUNDS_FACTOR end
    return lovr.math.vec3(vx, vy, vz)
end

function SelfControlledBoid:goal(dt)
    local targetPosition = self:getSwarm():getTargetPosition()
    if not targetPosition then return lovr.math.vec3(0, 0, 0) end
    local x, y, z = targetPosition[1], targetPosition[2], targetPosition[3]
    local dir = lovr.math.vec3(x, y, z)
    dir:sub(self:getSwarm():getCenter())    
    -- dir:sub(lovr.math.vec3(self:getCollider():getPosition()))
    --dir:normalize()
    if dir:length() < GOAL_LIMIT then 
        dir = lovr.math.vec3(0,0,0)
    end

    dir:mul(GOAL_FACTOR)
    
    -- print(dir)
    -- dir:div(GOAL_FACTOR)
    -- if dir:length() > GOAL_LIMIT then
    --     dir:normalize()
    --     dir:mul(GOAL_LIMIT)
    -- end

    return dir
end

function SelfControlledBoid:avoidPredator(dt)
    local avoidPosition = self:getSwarm():getAvoidPosition()
    if not avoidPosition then return lovr.math.vec3(0, 0, 0) end
    local x, y, z = avoidPosition[1], avoidPosition[2], avoidPosition[3]
    local dir = lovr.math.vec3(x, y, z)
    dir:sub(lovr.math.vec3(self:getCollider():getPosition()))
    dir:mul(-1)
    if dir:length() < AVOID_PREDATOR_MIN_DISTANCE then
        return lovr.math.vec3(0, 0, 0)
    end
    dir:div(AVOID_PREDATOR_FACTOR)
    if dir:length() > AVOID_PREDATOR_LIMIT then
        dir:normalize()
        dir:mul(AVOID_PREDATOR_LIMIT)
    end
    return dir
end

function SelfControlledBoid:limitSpeed(dt)
    local velocity = lovr.math.vec3(self:getCollider():getLinearVelocity())
    local speed = velocity:length()
    if speed > MAX_SPEED then
        local remove = MAX_SPEED - speed
        velocity:normalize()
        velocity:mul(remove)
        return velocity
    else
        return lovr.math.vec3(0, 0, 0)
    end
end

return SelfControlledBoid