Priority Ordering not working

library UnitAI initializer Init

    //    Units are faced with a certain set of conditions at any time. They must do whatever action best
    //responds to these conditions; at any time, there is a best action for a unit to do, which must be done
    //by it. Unit artificial intelligence works to simulate this function of apt evaluation and execution.
    //    Units may be impeded by factors of error, that they may commit errors in the evaluation and execution
    //of actions to-be-done.

globals
    private constant integer MAX_UNIT_INSTANCES = 8190
    private constant integer MAX_ACTIONS_PER_INSTANCE = 100
    private constant real UPDATE_INTERVAL_DURATION = 1.00
endglobals

interface UnitAction

    public integer intOrderId

    public method Evaluate takes nothing returns integer

    public method Execute takes nothing returns nothing

    public method Destroy takes nothing returns nothing

endinterface

struct UnitMind[MAX_UNIT_INSTANCES]

    private unit u
    private integer intErrorFactor = 0
    private UnitAction array action[MAX_ACTIONS_PER_INSTANCE]
    private integer array intActionPriority[MAX_ACTIONS_PER_INSTANCE]
    private integer intCountAction = 0
    private integer intQueueEndIndex = -1
    private UnitAction actionBest = 0

    public static method Create takes unit u returns UnitMind
        if u != null and UnitMind.intCountInst < MAX_UNIT_INSTANCES and UnitMind.Get(u) == 0 then
            set UnitMind.Inst[UnitMind.intCountInst] = UnitMind.allocate()
            set UnitMind.Inst[UnitMind.intCountInst].u = u

            set UnitMind.Inst[UnitMind.intCountInst].intInstIndex = UnitMind.intCountInst
            set UnitMind.intCountInst = UnitMind.intCountInst + 1

            return UnitMind.Inst[UnitMind.intCountInst - 1]
        endif

        return 0
    endmethod

    public method SetErrorFactor takes integer errorFactor returns boolean
        if errorFactor >= 0 and errorFactor <= 100 then
            set .intErrorFactor = errorFactor

            return true
        endif

        return false
    endmethod

    public method Destroy takes nothing returns nothing
        set .u = null
        loop
            exitwhen .intCountAction == 0
            set .intCountAction = .intCountAction - 1
            call .action[.intCountAction].Destroy()
        endloop
        call UnitMind.ManageTimer(-(.intQueueEndIndex + 1))

        set UnitMind.intCountInst = UnitMind.intCountInst - 1
        set UnitMind.Inst[UnitMind.intCountInst].intInstIndex = .intInstIndex
        set UnitMind.Inst[.intInstIndex] = UnitMind.Inst[UnitMind.intCountInst]
        set UnitMind.Inst[UnitMind.intCountInst] = 0
    endmethod

    public method AddAction takes UnitAction action returns boolean
        if action != 0 and .intCountAction < MAX_ACTIONS_PER_INSTANCE and .GetActionIndex(action) == -1 then
            set .action[.intCountAction] = action
            set .intCountAction = .intCountAction + 1

            return true
        endif

        return false
    endmethod

    public method RemoveAction takes UnitAction action returns boolean
        return false
    endmethod

    public method PromptAction takes UnitAction action returns boolean
         local integer INT_Index = .GetActionIndex(action)
         local UnitAction ACTION_Temp

         if INT_Index != -1 then
            if INT_Index > .intQueueEndIndex then
                set .intQueueEndIndex = .intQueueEndIndex + 1
                set ACTION_Temp = .action[.intQueueEndIndex]
                set .action[.intQueueEndIndex] = .action[INT_Index]
                set .action[INT_Index] = ACTION_Temp
            endif
            call .Update()
            if .actionBest == action then
                return true
            endif
         endif

         return false
    endmethod

    //NOTE: Error factor not yet implemented.
    public method Update takes nothing returns nothing
        call .EvaluateQueue()
        if .intQueueEndIndex != -1 then
            call .SortQueue()
            if .action[0] != .actionBest then
                set .actionBest = .action[0]
                call .actionBest.Execute.execute()
            elseif GetUnitCurrentOrder(.u) != .actionBest.intOrderId then
                call .UnpromptAction(0)
                if .intQueueEndIndex != -1 then
                    call .SortQueue()
                    set .actionBest = .action[0]
                    call .actionBest.Execute.execute()
                else
                    set .actionBest = 0
                endif
            endif
        endif
    endmethod

    private method UnpromptAction takes integer index returns nothing
        local UnitAction ACTION_Temp

        set ACTION_Temp = .action[.intQueueEndIndex]
        set .action[.intQueueEndIndex] = .action[index]
        set .action[index] = ACTION_Temp
        set .intQueueEndIndex = .intQueueEndIndex - 1
        //NOTE: Unprompted UnitAction always ends up in the index: .intQueueIndex + 1.
        call UnitMind.ManageTimer(-1)
    endmethod

    private method EvaluateQueue takes nothing returns nothing
        local integer INT_Index = 0

        loop
            exitwhen INT_Index == .intQueueEndIndex
            set .intActionPriority[INT_Index] = .action[INT_Index].Evaluate()
            if .intActionPriority[INT_Index] == -1 then
                call .UnpromptAction(INT_Index)
                set INT_Index = INT_Index - 1
            endif
            set INT_Index = INT_Index + 1
        endloop
    endmethod

    private method SortQueue takes nothing returns nothing
        local integer INT_Index = 1
        local integer INT_Index2
        local integer INT_Index2Sub
        local UnitAction ACTION_Temp
        local integer INT_Temp

        loop
            exitwhen INT_Index >= .intQueueEndIndex
            set INT_Index2 = INT_Index
            loop
                set INT_Index2Sub = INT_Index2 - 1
                exitwhen INT_Index2 == 0 or .intActionPriority[INT_Index2] <= .intActionPriority[INT_Index2Sub]
                set ACTION_Temp = .action[INT_Index2Sub]
                set INT_Temp = .intActionPriority[INT_Index2Sub]
                set .action[INT_Index2Sub] = .action[INT_Index2]
                set .intActionPriority[INT_Index2Sub] = .intActionPriority[INT_Index2]
                set .action[INT_Index2] = ACTION_Temp
                set .intActionPriority[INT_Index2] = INT_Temp
                set INT_Index2 = INT_Index2 - 1
            endloop
            set INT_Index = INT_Index + 1
        endloop
    endmethod

    private integer intInstIndex
    private static UnitMind array Inst[MAX_UNIT_INSTANCES]
    private static integer intCountInst = 0

    public static method Get takes unit u returns UnitMind
        local integer INT_Index = 0

        if u != null and UnitMind.intCountInst > 0 then
            loop
                exitwhen INT_Index == UnitMind.intCountInst
                if UnitMind.Inst[INT_Index].u == u then
                    return UnitMind.Inst[INT_Index]
                endif
                set INT_Index = INT_Index + 1
            endloop
        endif

        return 0
    endmethod

    private method GetActionIndex takes UnitAction action returns integer
        local integer INT_Index = 0

        loop
            exitwhen INT_Index == .intCountAction
            if .action[INT_Index] == action then
                return INT_Index
            endif
            set INT_Index = INT_Index + 1
        endloop

        return -1
    endmethod

    private static method UpdateAllQueued takes nothing returns nothing
        local integer INT_Index = 0

        loop
            exitwhen INT_Index == UnitMind.intCountInst
            if UnitMind.Inst[INT_Index].intQueueEndIndex != -1 then
                call UnitMind.Inst[INT_Index].Update()
            endif
            set INT_Index = INT_Index + 1
        endloop
    endmethod

    private static timer tim = CreateTimer()
    private static integer intCountQueued = 0

    private static method ManageTimer takes integer count returns nothing
        set UnitMind.intCountQueued = UnitMind.intCountQueued + count

        if UnitMind.intCountQueued == 0 then
            call PauseTimer(UnitMind.tim)
        elseif count == 1 and UnitMind.intCountQueued == 1 then
            call TimerStart(UnitMind.tim, UPDATE_INTERVAL_DURATION, true, function UnitMind.UpdateAllQueued)
        endif
    endmethod

endstruct

    private function DestroyDefunctInstance takes nothing returns boolean
        local UnitMind INST = UnitMind.Get(GetFilterUnit())
        
        if INST != 0 then
            call INST.Destroy()
        endif
        
        return false
    endfunction

    private function Init takes nothing returns nothing
        local trigger TRIG = CreateTrigger()
        local region REG = CreateRegion()
        
        call TriggerRegisterLeaveRegion(TRIG, REG, Filter(function DestroyDefunctInstance))
        
        set TRIG = null
        set REG = null
    endfunction

endlibrary