-- v0.9

controllingMainframe = 0  --the mainframe with the target prioritization card
restrictSlot = 1          --ai will only control these weapons, and will orient the craft along those
attackFromDistance = 750  -- distance at which do the attack run
abortAttackDistance = 75 -- distance at which we disengage
abortRunDistance = 500    -- distance at which we reengage

Kp = 10   --amount of control proportional to deviation
Ki = 0.01  --amount of accumulation to contrast deviation (keep small!)
Kd = 1   --amount of dampening proportional to oscillation

minAlt = 50
maxAlt = 300


useOptimizer = 1

function SimplexOptimizerFactory()
    local this = {}
    function this.CreateSimplexOptimizer(this,dims)
        local simple = {}
        simple.Dim=dims
        simple.Simplex = {}

        for a,b in pairs(this) do
            simple[a] = b
        end
        simple.CreateSimplexOptimizer = nil
        simple.Update = simple.Initial

        return simple
    end
    function this.SortFirst(a,b)
        if a[1]<b[1] then
            return true
        else
            return false
        end
    end
    function this.AddHint(this,hint)
        if not this.Hint then
            this.Hint = {}
        end
        table.insert(this.Hint,hint)
    end
    function this.Initial(this,result,sample) -- Initial phase, populate samples
        if result then
            local merge = {result,sample}
            table.insert(this.Simplex,merge)
        end
        if this.Hint then
            local thishint = table.remove(this.Hint)
            if #this.Hint == 0 then
                this.Hint = nil
            end
            return thishint
        end
        if #this.Simplex > this.Dim then
            table.sort(this.Simplex,this.SortFirst)
            while #this.Simplex > this.Dim+1 do -- Drop worst samples to create a simplex
                table.remove(this.Simplex)
            end
            this.Update=this.SReflection
            return this.Point(this,1) -- 1 = reflection
        end
        local ret = {}
        for i=1,this.Dim do
            table.insert(ret,0.5-math.random())
        end
        return ret
    end
    function this.Centroid(this)
        local centroid = {}
        local ndim = this.Dim
        for i=1,ndim do
            table.insert(centroid,0)
        end
        for i=1,ndim do
            for a,b in pairs(this.Simplex[i][2]) do
                centroid[a] = centroid[a] + b
            end
        end
        for a,b in pairs(centroid) do
            centroid[a] = (b / ndim)
        end
        return centroid
    end
    function this.Point(this,pow)
        local point = this.Centroid(this)
        local ndim = this.Dim
        for a,b in pairs(point) do
            point[a] = b + pow*(b - this.Simplex[ndim+1][2][a])
        end
        return point
    end
    function this.SReflection(this,result,sample) -- Reflection stage
        if result > this.Simplex[1][1] then -- If it is the best one go to expansion
            this.Update = this.SExpansion
            this.Reflected={result,sample}
            return this.Point(this,2)
        end
        if result > this.Simplex[this.Dim][1] then -- if better than second worse
            local ns = {result,sample}
            table.insert(this.Simplex)
            table.sort(this.Simplex,this.SortFirst)
            table.remove(this.Simplex)
            return this.Reflection(this)
        end
        --- here we do contraction
        this.Update = this.SContraction
        return this.Point(this,-0.5)
    end
    function this.SExpansion(this,result,sample)
        if result > this.Reflected[1] then -- if the epanded point is better than the reflected point
            table.insert(this.Simplex,{result,sample})
        else
            table.insert(this.Simplex,this.Reflected)
        end
        table.sort(this.Simplex,this.SortFirst)
        table.remove(this.Simplex)
        this.Update = this.SReflection
        return this.Point(this,1)
    end
    function this.SContraction(this,result,sample)
        if result > this.Simplex[this.Dim+1][1] then -- If the contracted point is better than the worst point
            table.insert(this.Simplex,{result,sample})
            table.sort(this.Simplex,this.SortFirst)
            table.remove(this.Simplex)
        else -- do reduction
            for i=2,this.Dim+1 do
                local arr = this.Simplex[i][2]
                for a,b in pairs(arr) do
                    local x1 = this.Simplex[1][2][a]
                    arr[a] = x1 + 0.5*(b-x1)
                end
                this.AddHint(this,arr)
            end
            while #this.Simplex > 1 do
                table.remove(this.Simplex)
            end
            this.Update=this.Initial
            return this.Initial(this)
        end
    end
    return this
end

local function PIDFactory()
    local this = {}
    function this.pidupd(this,err)
        local P = this.P
        local I = this.I
        local D = this.D
        local Integrator = this.Integrator + I*err
        local max = this.SatMax
        local min = this.SatMin
        local last = this.Last
        this.Last = err
        local delta = err-last
        local out = P*err+D*delta+Integrator
        if out > max then -- Integrator windup prevention
            Integrator = this.Integrator
            out = max
        end
        if out < min then -- Integrator windup prevention
            Integrator = this.Integrator
            out = min
        end
        this.Integrator = Integrator
        return out
    end
    function this.pidconfig(this,p,i,d)
        this.P = p
        this.I = i
        this.D = d
    end
    function this.pidlimits(this,max,min)
        this.SatMax = max
        this.SatMin = min
    end
    function this.CreatePID(this)
        local pid = {}
        pid.Update = this.pidupd
        pid.Configure = this.pidconfig
        pid.SetLimits = this.pidlimits
        pid.P = 1 -- Just make it a simple passthrough for now
        pid.I = 0
        pid.D = 0
        pid.Integrator=0
        pid.Last=0
        pid.SatMax = 1
        pid.SatMin = -1
        return pid
    end
    return this
end

function killThatThing(I, weaponInfo, targetInfo, weaponIndex, turretSpinnerIndex)

    P = targetInfo.AimPointPosition
    V = targetInfo.Velocity
    WS = weaponInfo.Speed
    G = I:GetGravityForAltitude(P.y).magnitude


    T = (P - weaponInfo.GlobalPosition).magnitude / (WS*0.85) -- FIRST ESTIMATE OF FUTURE TARGET POSITION


    FP = P + V*T  - I:GetVelocityVector()*T
 -- position the target will be

    AD = FP - weaponInfo.GlobalPosition
  -- direction direct to the target



    PJ = Mathf.Sqrt(AD.x*AD.x + AD.z*AD.z)
 -- horizontal distance to the target


    S2 = WS*WS
  -- speed^2
    S4 = S2*WS*WS
  -- speed^4 (need these many times)
    DELTA = S4 - G*(G*PJ*PJ + 2*AD.y * S2)
 -- is there a solution to the parable?


    if ( DELTA > 0 ) then
        --ok we can reach it and now we have a better estimate

        AG = Mathf.Atan2(S2 - Mathf.Sqrt(DELTA),G*PJ)
 --calculate angle

        --now we can calculate a better time to target using the horizontal speed
        --as obtained from the firing angle

T = (P - weaponInfo.GlobalPosition).magnitude / (WS * Mathf.Cos(AG))


        FP = P + V*T - I:GetVelocityVector()*T
  --position target will be

        AD = FP - weaponInfo.GlobalPosition
 -- line direct to the target position



        PJ = Mathf.Sqrt(AD.x*AD.x + AD.z*AD.z)
  -- horizontal distance to the target



        DELTA = S4 - G*(G*PJ*PJ + 2*AD.y * S2)
 -- check the parable solution


        if ( DELTA > 0 ) then
        --ok we can reach it and now we have a better estimate
        PY = (S2 - Mathf.Sqrt(DELTA))/(G)
 -- no need to calculate angle or tangent, just the elev



        AD.y = PY --assign new elevation to the firing direction


        if(turretSpinnerIndex < 0) then
           I:AimWeaponInDirection(weaponIndex, AD.x, AD.y, AD.z, weaponInfo.WeaponSlot)
           I:FireWeapon(weaponIndex, weaponInfo.WeaponSlot)
        else
           I:AimWeaponInDirectionOnTurretOrSpinner(
                 turretSpinnerIndex,weaponIndex, AD.x, AD.y, AD.z, weaponInfo.WeaponSlot)


           I:FireWeaponOnTurretOrSpinner(
                 turretSpinnerIndex,weaponIndex,weaponInfo.WeaponSlot)

        end

      end

    end
   return AD
end

factory = PIDFactory()
yawPID = factory.CreatePID(factory)
pitchPID = factory.CreatePID(factory)
rollPID = factory.CreatePID(factory)

yawPID.SetLimits(yawPID,1,-1)
pitchPID.SetLimits(pitchPID,1,-1)
rollPID.SetLimits(rollPID,1,-1)

if (useOptimizer == 1) then
SimpFact = SimplexOptimizerFactory()

yawOpti = SimpFact.CreateSimplexOptimizer(SimpFact,3)
yawOpti.AddHint(yawOpti,{Kp,Ki,Kd})
yawSample = yawOpti.Update(yawOpti,nil,nil)
yawPID.Configure(yawPID,unpack(yawSample))

rollOpti = SimpFact.CreateSimplexOptimizer(SimpFact,3)
rollOpti.AddHint(rollOpti,{Kp,Ki,Kd})
rollSample = rollOpti.Update(rollOpti,nil,nil)
rollPID.Configure(rollPID,unpack(rollSample))

pitchOpti = SimpFact.CreateSimplexOptimizer(SimpFact,3)
pitchOpti.AddHint(pitchOpti,{Kp,Ki,Kd})
pitchSample = pitchOpti.Update(pitchOpti,nil,nil)
pitchPID.Configure(pitchPID,unpack(pitchSample))

else
yawPID.Configure(yawPID,Kp,Ki,Kd)
pitchPID.Configure(pitchPID,Kp,Ki,Kd)
rollPID.Configure(rollPID,Kp,Ki,Kd)
end


yawScore = 0
pitchScore = 0
rollScore = 0
accumulator = 0

function flyAlong(I, direction, normal)
    I:RequestThrustControl(0, 1)
    logstr = ""
     dt = 1
    lastYaw = eYaw
    lastPitch = ePitch
    lastRoll = eRoll
    --I:RequestControl(mode,type,drive)
    eYaw =  Vector3.Dot(
            Vector3.ProjectOnPlane(direction, I:GetConstructUpVector() ),
            I:GetConstructRightVector()
            )
    ePitch = Vector3.Dot(
            Vector3.ProjectOnPlane(direction, I:GetConstructRightVector() ),
            I:GetConstructUpVector()
            )
    eRoll = Vector3.Dot(
            Vector3.ProjectOnPlane(normal, I:GetConstructForwardVector() ),
            I:GetConstructRightVector()
            )

    accumulator = accumulator + 1
    yawScore = yawScore + math.abs(eYaw)
    pitchScore = pitchScore + math.abs(ePitch)
    rollScore = rollScore + math.abs(eRoll)

    if accumulator > 100 then
        if(useOptimizer == 1) then
            if (yawScore > 100) then
              yawSample = yawOpti.Update(yawOpti,yawScore/accumulator,yawSample)
              if(not yawSample == nil ) then yawPID.Configure(yawPID,unpack(yawSample)) end
            end
            if(pitchScore>100) then
              pitchSample = pitchOpti.Update(pitchOpti,pitchScore/accumulator,pitchSample)
              if(not pithSample == nil ) then pitchPID.Configure(pitchPID,unpack(pitchSample)) end
            end
            if(rollScore>100) then
               rollSample = rollOpti.Update(rollOpti,rollScore/accumulator,rollSample)
              if(not rollSample == nil ) then rollPID.Configure(rollPID,unpack(rollSample)) end
            end
        else
            yawPID.Configure(yawPID,Kp,Ki,Kd)
            pitchPID.Configure(pitchPID,Kp,Ki,Kd)
            rollPID.Configure(rollPID,Kp,Ki,Kd)
        end

        pitchScore=0
        rollScore=0
        yawScore = 0
        accumulator = 0
    end


    CYaw =  yawPID.Update(yawPID,eYaw)
    CPitch =  pitchPID.Update(pitchPID,ePitch)
    CRoll =  rollPID.Update(rollPID,eRoll)

    --CYaw = 0.63661977*Mathf.Atan(CYaw*10)
    --CPitch = 0.63661977*Mathf.Atan(CPitch*10)
    --CRoll = 0.63661977*Mathf.Atan(CRoll*10)

    if ( CYaw < 0 ) then
            --turn left
            I:RequestControl(2,0,-CYaw)
            logstr = logstr .. " left " .. -CYaw
    else
            --turn right
            I:RequestControl(2,1,CYaw)
            logstr = logstr .. " right " ..CYaw
    end
    if ( CPitch < 0 ) then
            --pitch down
            I:RequestControl(2,5,-CPitch)
            logstr = logstr .. " down " .. -CPitch
    else
            --pitch up
            I:RequestControl(2,4,CPitch)
            logstr = logstr .. " up " .. CPitch
    end

    if ( CRoll < 0 ) then
            -- roll left
            logstr = logstr .. " rccw " .. -CRoll
            I:RequestControl(2,2,-CRoll)
    else
            -- roll right
            I:RequestControl(2,3,CRoll)
            logstr = logstr .. " rcw " .. CRoll

    end

end


GAIN = 0 -- close in to target at max altitude
SPLIT = 1 -- match target direction rolling down/up according to start alt
ROLL = 2 -- if enemy vector are parallel, roll into tail
TARGET = 3 -- aim to a shoot solution
RUN = 4 -- too close and enemy not fast enough
LOOP = 5 --enemy is faster loop on it's tail

attackState = GAIN

function performManeuver(I,AD,targetInfo,attackState)
    P = targetInfo.AimPointPosition
    V = targetInfo.Velocity
    FP = P
    TD = FP - I:GetConstructCenterOfMass()
    com = I:GetConstructCenterOfMass()
    fcom = I:GetConstructCenterOfMass() + I:GetVelocityVector()
    normal =  -I:GetGravityForAltitude(I:GetConstructCenterOfMass().y)
    if(attackState == GAIN) then
        I:LogToHud("GAINING")
        y = I:GetConstructCenterOfMass().y
        if ( y < maxAlt - 50 ) then
             AD = AD.normalized
             AD.y = 1
        else
            AD.y = 0
        end
    end
    if(attackState == RUN) then
        V = targetInfo.Velocity

        I:LogToHud("RUNNING")
        if (V.magnitude > I:GetVelocityMagnitude()*0.80) then
            AD = V * -1
        else
            AD = TD * -1
        end
        y = I:GetConstructCenterOfMass().y
        if ( y < maxAlt - 50 ) then
             AD = AD.normalized
             AD.y = 4
        else
            if ( y > (maxAlt - minAlt / 2) ) then
                AD = AD.normalized
                AD.y = -4
             else
                AD.y=0
             end
        end
    end
    if(attackState == TARGET) then
        I:LogToHud("TARGETING")
        normal = TD.normalized
        normal.y = 0.1
    end
    if(attackState == LOOP) then
        I:LogToHud("LOOP")
        AD = V * -1
        normal = AD
    end
    -- emergency climb

    if (com.y < I:GetTerrainAltitudeForLocalPosition(com.x,0,com.z) + minAlt or fcom.y < I:GetTerrainAltitudeForLocalPosition(fcom.x,0,fcom.z) + minAlt) then
      AD = Vector3(0,1,0)
        I:LogToHud("EC")
      normal =  -I:GetGravityForAltitude(I:GetConstructCenterOfMass().y)
    end
    flyAlong(I, AD, normal)
end



function attackEnemy(I, AD, targetInfo)
    P = targetInfo.AimPointPosition
    V = targetInfo.Velocity
    FP = P
    TD = FP - I:GetConstructCenterOfMass()
    distance = TD.magnitude

    if (distance > attackFromDistance) then
        attackState = GAIN
    else
      if (distance < abortAttackDistance) then
          attackState = RUN
      else
          if (attackState == GAIN or distance > abortRunDistance) then
              attackState = TARGET
          end
          if (attackState == RUN and distance < abortRunDistance and Vector3.Dot(V.normalized,I:GetConstructForwardVector() )> 0.7) then
             attackState = LOOP
          end
          if (attackState == LOOP and distance > abortAttackDistance and Vector3.Dot(V,I:GetConstructForwardVector() ) < 0) then
             attackState = TARGET
          end
          if (attackState == LOOP and distance < abortAttackDistance and Vector3.Dot(V,I:GetConstructForwardVector() ) < 0) then
             attackState = RUN
          end
           if (attackState == TARGET and  math.abs(Vector3.Dot(TD.normalized,I:GetConstructForwardVector() )) < 0.2 and distance < abortRunDistance) then
             attackState = RUN
          end
      end
    end

    performManeuver(I,AD,targetInfo,attackState)

end

function Update(I)

    targetInfo = I:GetTargetInfo(controllingMainframe, 0)
    AttackDirection = nil
    if(targetInfo.Valid) then
        for weaponIndex=0,I:GetWeaponCount() do
          weaponInfo = I:GetWeaponInfo(weaponIndex)

          if ((weaponInfo.WeaponType == 4 or weaponInfo.WeaponType == 0 )
               and weaponInfo.Valid and weaponInfo.PlayerCurrentlyControllingIt == false
               and weaponInfo.WeaponSlot == restrictSlot) then
               AttackDirection = killThatThing(I, weaponInfo, targetInfo, weaponIndex, -1)
          end
        end

        for turretSpinnerIndex=0,I:GetTurretSpinnerCount() do
            for weaponIndex=0,I:GetWeaponCountOnTurretOrSpinner(turretSpinnerIndex) do
              weaponInfo = I:GetWeaponInfoOnTurretOrSpinner(turretSpinnerIndex, weaponIndex)
                if ((weaponInfo.WeaponType == 4 or weaponInfo.WeaponType == 0 )
                and weaponInfo.Valid and weaponInfo.PlayerCurrentlyControllingIt == false
                and weaponInfo.WeaponSlot == restrictSlot ) then
                AttackDirection = killThatThing(I, weaponInfo, targetInfo, weaponIndex, turretSpinnerIndex)
                end
            end
        end
        if (AttackDirection == nil) then
            P = targetInfo.AimPointPosition
            V = targetInfo.Velocity

            FP = P + V


              I:LogToHud("No weapon configured!!")
            AttackDirection = FP - I:GetConstructCenterOfMass()
        end
        attackEnemy(I, AttackDirection, targetInfo)
    else


       I:LogToHud("IDLING")

    end

end