astar.z

const int ARR_EMPTY = -1;
const int ARR_INFINITY = -2;

// used in the algorithm.
const int ARR_W = 16;
const int ARR_H = 11;

const int DEBUG_COMBO_START = 900;
const int DEBUG_COMBO_FINISH = 901;
const int DEBUG_COMBO_RESULT = 914;
// 4 combos each, coming from up/down/left/right.
const int DEBUG_COMBO_OPEN = 902;
const int DEBUG_COMBO_CLOSED = 906;
const int DEBUG_COMBO_CURRENT = 910;

// Ex1 for step-by-step, ex2 for fast.
void WaitForUser()
{
        while (!Link->PressEx1 && !Link->InputEx2) Waitframe();
}

ffc script Astar
{
    void run()
    {
        ClearTrace();

        WaitForUser();

        int START = 72;
        int GOAL = 148;

        Screen->ComboD[START] = DEBUG_COMBO_START;
        Screen->ComboD[GOAL] = DEBUG_COMBO_FINISH;

        int CURRENT = START;

        int ClosedSet[176]; // Unsorted array
        ClearArrToVal(ClosedSet, ARR_EMPTY);

        for(int i = 0; i < SizeOfArray(ClosedSet); i += 1)
        {
            if(Screen->ComboS[i] != 0 && i != GOAL)
            {
                Put(ClosedSet, i);
            }
        }

        int OpenSet[176]; // Unsorted array
        ClearArrToVal(OpenSet, ARR_EMPTY);
        Put(OpenSet, START);

        int CameFrom[176]; // Sorted array
        ClearArrToVal(CameFrom, ARR_EMPTY);

        int GScore[176]; // Sorted array
        ClearArrToVal(GScore, ARR_INFINITY);
        GScore[START] = 0;

        int FScore[176]; // Sorted array
        ClearArrToVal(FScore, 9999);
        FScore[START] = heuristic(START, GOAL);

        int NEIGHBOR = 0;
        bool VALID_NEIGHBOR = false;

        int Tentative_GScore = 0;

        int Iterations = 0; // Custom stuff

        int Space[] = " "; // Debug stuff

        int smallest = 0;

        while(ArrEmpty(OpenSet, ARR_EMPTY) == false)
        {
            Iterations += 1;

            int choosing[] = "Choosing next node:"; TraceS(choosing); TraceNL();

            smallest = 0;
            for(int i = 0; i < SizeOfArray(OpenSet); i += 1)
            {
                if(OpenSet[i] > 0)
                {
                    int s1[] = "  X "; TraceS(s1); Trace(OpenSet[i] % 16);
                    int s4[] = "  Y "; TraceS(s4); Trace(OpenSet[i] >> 4);
                    int s2[] = "  Score "; TraceS(s2); Trace(FScore[OpenSet[i]]);

                    if(FScore[OpenSet[i]] < FScore[OpenSet[smallest]] && FScore[OpenSet[i]] >= 0)
                    {
                        smallest = i;
                        int s3[] = "  New smallest!"; TraceS(s3); TraceNL();
                    }
                }
            }

            if(smallest < 0)
            {
                continue;
            }

            CURRENT = OpenSet[smallest];
            if(CURRENT != START && CURRENT != GOAL)
            {
                Screen->ComboD[CURRENT] += DEBUG_COMBO_CURRENT - DEBUG_COMBO_OPEN;
            }

            WaitForUser();

            int currentval[] = "Current node: "; TraceS(currentval); Trace(CURRENT); TraceNL();

            if(CURRENT == GOAL)
            {
                int SUCCESS[] = "SUCCESS! The script works!";
                TraceS(SUCCESS);
                TraceNL();

                int cur = GOAL;
                Trace(CameFrom[GOAL]);
                Trace(GOAL);

                while(CameFrom[cur] != START)
                {
                    cur = CameFrom[cur];
                    Screen->ComboD[cur] = DEBUG_COMBO_RESULT;
                    Trace(cur);
                    Waitframe();
                    WaitForUser();
                }

                TraceNL();
                Trace(Iterations);

                break;
            }

            if(CURRENT < 0)
            {
                Waitframe();
                continue;
            }

            int msg4[] = "IndiceOfVal(OpenSet, Current) = "; TraceS(msg4); Trace(IndiceOfVal(OpenSet, CURRENT)); TraceNL();
            int msg5[] = "OpenSet(IndiceOfVal...) = "; TraceS(msg5); Trace(OpenSet[IndiceOfVal(OpenSet, CURRENT)]); TraceNL();
            OpenSet[IndiceOfVal(OpenSet, CURRENT)] = ARR_EMPTY;
            // ShiftArr(OpenSet, ARR_EMPTY, ARR_EMPTY);

            // Trace(8888);

            Put(ClosedSet, CURRENT);
            if(CURRENT != START) // Promote to closed combo.
            {
                Screen->ComboD[CURRENT] += DEBUG_COMBO_CLOSED - DEBUG_COMBO_CURRENT;
            }

            for(int i = 0; i < 4; i += 1) // Going through each neighbor of current
            {
                VALID_NEIGHBOR = false;

                if(i == 0) // Up
                {
                    NEIGHBOR = (CURRENT - ARR_W);

                    if(NEIGHBOR >= 0 && ArrContainsVal(ClosedSet, NEIGHBOR) == false)
                    {
                        VALID_NEIGHBOR = true;
                    }
                }

                if(i == 1) // Down
                {
                    NEIGHBOR = (CURRENT + ARR_W);

                    if(NEIGHBOR < SizeOfArray(OpenSet) && ArrContainsVal(ClosedSet, NEIGHBOR) == false)
                    {
                        VALID_NEIGHBOR = true;
                    }
                }

                //
                // NOTE: LOOK INTO LEFT DIRECTION NODE
                // It seems like there's an error with FloorToMultiple, which allows it to return a combo on the far right of the screen as being
                // On the same row as one combo down and on the far left.


                if(i == 2) // Left
                {
                    NEIGHBOR = (CURRENT - 1);

                    if(NEIGHBOR >= FloorToMultiple(CURRENT, ARR_W) && ArrContainsVal(ClosedSet, NEIGHBOR) == false)
                    {
                        VALID_NEIGHBOR = true;
                    }
                }

                if(i == 3) // Right
                {
                    NEIGHBOR = (CURRENT + 1);

                    if(NEIGHBOR < FloorToMultiple(CURRENT + ARR_W, ARR_W) && ArrContainsVal(ClosedSet, NEIGHBOR) == false)
                    {
                        VALID_NEIGHBOR = true;
                    }
                }

                if(VALID_NEIGHBOR == true)
                {
                    Tentative_GScore = GScore[CURRENT] + heuristic(CURRENT, NEIGHBOR);

                    int msg2[] = "Tentative GScore: "; TraceS(msg2); Trace(Tentative_GScore); TraceNL();

                    int msg6[] = "Neighbor node: "; TraceS(msg6); Trace(NEIGHBOR); TraceNL();

                    if(ArrContainsVal(OpenSet, NEIGHBOR) == false)
                    {
                        Put(OpenSet, NEIGHBOR);
                        if(NEIGHBOR != GOAL)
                        {
                            Screen->ComboD[NEIGHBOR] = DEBUG_COMBO_OPEN + i;
                        }
                    }
                    else if(Tentative_GScore >= GScore[NEIGHBOR])
                    {
                        continue; // Worse path
                    }

                    CameFrom[NEIGHBOR] = CURRENT;
                    GScore[NEIGHBOR] = Tentative_GScore;
                    FScore[NEIGHBOR] = GScore[NEIGHBOR] + heuristic(NEIGHBOR, GOAL);

                    int msg3[] = "End of subloop #"; TraceS(msg3); Trace(i); TraceNL();
                }
            }

            int msg[] = "End of algorithm loop ~~~~~~~~~~~~~~~~~~~~ Iteration #";
            TraceS(msg); Trace(Iterations); TraceNL();

            Waitframe();
        }

        while(true)
        {
            Waitframe();
        }
    }
}

void ClearArrToVal(int arr, int val)
{
    for(int i = 0; i < SizeOfArray(arr); i += 1)
    {
        arr[i] = val;
    }
}

void Put(int arr, int val, int pos)
{
    arr[pos] = val;
}

void Put(int arr, int val)
{
    for(int i = 0; i < SizeOfArray(arr); i += 1)
    {
        if(arr[i] == ARR_EMPTY)
        {
            arr[i] = val;
            break;
        }
    }
}

bool ArrEmpty(int arr, int exception)
{
    bool empty = true;
    for(int i = 0; i < SizeOfArray(arr); i += 1)
    {
        if(arr[i] != exception)
        {
            empty = false;
            break;
        }
    }

    return empty;
}

int Get(int arr, int arrP, bool remove)
{
    int smallest = arrP[0];
    int node = 0;
    int indice = 0;

    for(int i = 1; i < SizeOfArray(arrP); i += 1)
    {
        if(smallest < arrP[i] && smallest > ARR_EMPTY)
        {
            smallest = arrP[i];
            node = arr[i];
            indice = i;
        }
    }

    if(remove == true)
    {
        arr[indice] = ARR_EMPTY;
        arrP[indice] = ARR_EMPTY;

        //ShiftArr(arr, ARR_EMPTY, indice);
        //ShiftArr(arrP, ARR_EMPTY, indice);
    }

    //Trace(indice);

    return node;
}

// startPos is the indice to shift into/away from
void ShiftArr(int arr, int emptyVal, int startPos)
{
    for(int i = startPos; i < SizeOfArray(arr); i += 1)
    {
        if(i + 1 >= SizeOfArray(arr))
        {
            // arr[i] = emptyVal;
            break;
        }
        else
        {
            arr[i] = arr[i + 1];
        }
    }
}

// Floors a value to a multiple.
int FloorToMultiple(int input, int multiple)
{
    if(multiple == 0) {return 0;}
    int mul = Abs(multiple);
    return Floor(input/mul)*mul;
}

bool ArrContainsVal(int arr, int val)
{
    bool contains = false;

    for(int i = 0; i < SizeOfArray(arr); i += 1)
    {
        if(arr[i] == val)
        {
            contains = true;
            break;
        }
    }

    return contains;
}

int heuristic(int nodeA, int nodeB)
{
    int a_x = nodeA % ARR_W;
    int a_y = Floor(nodeA / ARR_W);
    int b_x = nodeB % ARR_W;
    int b_y = Floor(nodeB / ARR_W);

    return Abs(a_x - b_x) + Abs(a_y - b_y);
}

int IndiceOfLowestVal(int arr)
{
    int smallest = arr[0];
    int indice = 0;

    for(int i = 0; i < SizeOfArray(arr); i += 1)
    {
        if(smallest < arr[i] && smallest != ARR_EMPTY && smallest != ARR_INFINITY && smallest >= 0)
        {
            smallest = arr[i];
            indice = i;
        }
    }

    return indice;
}

int IndiceOfVal(int arr, int val)
{
    int indice = -1;

    for(int i = 0; i < SizeOfArray(arr); i += 1)
    {
        if(arr[i] == val)
        {
            indice = i;
            break;
        }
    }

    return indice;
}