--Turret Aimer

----------
--Config--
----------

--Max position derivation degree to use for prediction
maxderive = 6
--Rate to smooth interpolated derivation degree information, exponential by degree
smoothrate = 1/2
--Gravity settings by weapon type, if not listed, gravity is 1.0
gravtable = {[6]=0}
--The angle at which a missile has passed its target that it will be detonated
missiledetang = 120

----------
--Config--
----------

--Maximum number of targets to store data for
maxbuffered = 50

--Position derivation info for all tracked targets
targposinf = {}
targposinfsz = 0

--Smoothed aim point offsets for all targets
targaimpoints = {}
targaimpointsz = 0
targaimsmoothing = 1/80

ticklen = 1

----------

--Calculates target position derivation values for given target 't'
function CalcTargetPosInf(I,t)
    if not targposinf[t.Id] then
        if targposinfsz >= maxbuffered then targposinf = {}; targposinfsz = 0 end
        targposinf[t.Id] = {}
        targposinfsz = targposinfsz+1
    end
    arr = {}
    arr[0] = t.Position
    arr[1] = t.Velocity
    smoothamt = 1
    for i=1,maxderive,1 do
        oldval = targposinf[t.Id][i-1]
        if oldval == nil then break end
        arr[i] = (arr[i-1]-oldval)*(1/ticklen)
        arr[i] = arr[i]*smoothamt + (targposinf[t.Id][i] or Vector3(0,0,0))*(1-smoothamt)

        smoothamt = smoothamt*smoothrate
    end
    targposinf[t.Id]=arr
    return targposinf[t.Id]
end

function SmoothTargetAimPoint(I,t)
    if not targaimpoints[t.Id] then
        if targaimpointsz >= maxbuffered then targaimpoints = {}; targaimpointsz = 0 end
        targaimpoints[t.Id] = Vector3(0,0,0)
    end
    targaimpoints[t.Id] = (t.AimPointPosition-t.Position)*targaimsmoothing + targaimpoints[t.Id]*(1-targaimsmoothing)
end

--Performs per-tick processing on all targets
function UpdateTargets(I)
    for i=0,I:GetNumberOfMainframes(),1 do
        for j=0,I:GetNumberOfTargets(i),1 do
            tmp = I:GetTargetInfo(i,j)
            if tmp.Valid then
                CalcTargetPosInf(I,tmp)
                SmoothTargetAimPoint(I,tmp)
            end
        end
    end
end

--Predicts TargetInf 't's predicted location in 'time' seconds
function PredictTargetVel(I,t,time)
    arr = targposinf[t.Id]
    res = Vector3(0,0,0)
    nom = 1
    denom = 1
    for i=1,maxderive,1 do
        if arr[i] == nil then break end
        denom = denom*(i)
        nom = nom*(time+ticklen*i)
        res = res + nom/denom*arr[i]
        tmp = nom/denom*arr[i]
    end
    return res/time
    end

--Calculates a predicted world position in which a weapon at position 'curpos' with projectile speed 'speed' would hit target 'targ' with gravity multiplier 'gravity'
function CalcAimPos(I,curpos,targ,speed,gravity)
    --TODO: Can recalculate Gravity for each iteration to improve accuracy more
    aimpos = targaimpoints[targ.Id]+targ.Position

    diff = aimpos-curpos
    tvel = targ.Velocity
    --More iterations improves accuracy
    for i=1,4,1 do
        veldif = diff.normalized*speed-tvel
        timetohit = diff.magnitude/math.max(Vector3.Dot(veldif,diff.normalized),0.1)
        tvel = PredictTargetVel(I,targ,timetohit)
        grav = I:GetGravityForAltitude(curpos.y+diff.y/2.0)*gravity
        gravloss = grav*timetohit*(timetohit)*0.5
        diff = aimpos-gravloss-curpos
    end
    timetohit = timetohit
    return diff+tvel*timetohit+curpos

end

--Aims weapon 'wep' at predicted location of target 'targ' and fires if aligned
function FireWeapon(I,wep,targ)
    if not targ.Valid then return end
    inf = I:GetWeaponInfo(wep)
    if not inf.Valid then return end

    --grav = gravtable[inf.WeaponType] or 1.0
    grav = 1.0 --TODO: gravtable is not working, weapon type is turret for each
    speed = inf.Speed
    res = CalcAimPos(I,inf.GlobalFirePoint,targ,speed,grav)-inf.GlobalFirePoint

    I:AimWeaponInDirection(wep,res.x,res.y,res.z,0)
    --TODO: Move next line to its own function that finds target painters based on weapon entity's sub-block
    --I:Component_SetFloatLogic(13,0,timetohit)
    --TODO: Only fire if inf.CurrentDirection is within acceptable limits
    I:FireWeapon(wep,0)
    end

----------

function Update(I)
    curtime = I:GetGameTime()
    ticklen = curtime - (lasttick or curtime-1)
    lasttick = curtime

    UpdateTargets(I)

    --Select a target to fire at
    --TODO: Implement a system to assign weapon system to certain mainframes, probably requires using block names
    targ = nil
    for i=0,I:GetNumberOfMainframes(),1 do
        for j=0,I:GetNumberOfTargets(i),1 do
            targ = I:GetTargetInfo(i,j)
            if targ.Valid then break end
        end
        if targ.Valid then break end
    end
    --Fire weapons
    if targ.Valid then
        for i=1,I:GetWeaponCount(),1 do
            FireWeapon(I,i,targ)
        end
    end
    if not targ.Valid then return end

    trans = I:GetLuaTransceiverCount()
    for i=0,trans,1 do
        missiles = I:GetLuaControlledMissileCount(i)
        for j=0,missiles,1 do
            inf = I:GetLuaControlledMissileInfo(i,j)
            aim = CalcAimPos(I,inf.Position,targ,381,1)
            if inf.TimeSinceLaunch >= 3 and Vector3.Angle(aim-inf.Position,inf.Velocity)>=missiledetang then
                I:DetonateLuaControlledMissile(i,j)
            end
            I:SetLuaControlledMissileAimPoint(i,j,aim.x,aim.y,aim.z)
        end
    end
end