Pathfinding A* OpenCL Kernel

float heuristic(int2 start, int2 end)
{
    float result = 0;

    int x, y;

    x = (int)(start.x - end.x);
    y = (int)(start.y - end.y);

    if (x < 0)
    {
        x = -x;
    }

    if (y < 0)
    {
        y = -y;
    }

    result = sqrt((x * x) + (y * y));
    return result;
}

__kernel
void findPath(__global char * output,
              __global bool * input,
              const    int    width,
              const    int    height,
              const    int    agentsNum,
             __global int2 * startIndexes,
             __global int2 * destIndexes)
{
    uint globalIdx = get_global_id(0);

    /* wait until the whole block is filled */
    barrier(CLK_LOCAL_MEM_FENCE);

    int2 closedList[1024];
    int2 openedList[1024];

    int openedListLength = 0;
    int closedListLength = 0;

    int2 start = (int2)startIndexes[globalIdx];
    int2 end = (int2)destIndexes[globalIdx];

    if (start.x != end.x && start.y != end.y)
    {
        openedList[0] = (int2)start;
        openedListLength++;
    }

    int2 currentPoint, tempPoint;

    float currentH = 0;
    float tempH = 0;

    int i = 0;
    int x = 0;
    int y = 0;

    bool found = false;
    bool inClosedList;
    bool inOpenedList;
    bool betterPath;

    int stX, stY, enX, enY;
    int index;
    float hScore[1024];
    float fScore[1024];
    float gScore[1024];
    int cameFrom[1024];

    gScore[start.x + (start.y * width)] = (float)0.0f;

    tempH = (float)heuristic(start, end);

    index = (int)(start.x + (start.y * width));

    hScore[index] = (float)tempH;
    fScore[index] = (float)hScore[index];

    cameFrom[start.x + (start.y * width)] = (int)-1;

    output[index + ((width * height) * globalIdx)] = 'O';
    output[(int)(end.x + (end.y * width)) + ((width * height) * globalIdx)] = 'X';

    while ((openedListLength > 0) && (!found))
    {
        currentH = -1.0f;
        currentPoint = (int2)(-1,-1);

        for (i=0; i<openedListLength; i++)
        {
            tempPoint = (int2)openedList[i];
            tempH = (float)heuristic(tempPoint, end);//(float)fScore[(int)(tempPoint.x + (width * tempPoint.y))]

            if ((currentH < 0.0f) || (currentH > tempH))
            {
                currentH = tempH;
                currentPoint = tempPoint;
            }
        }

        openedListLength--;

        if (currentPoint.x == -1)
        {
            openedListLength = 0;
            break;
        }

        if ((end.x == currentPoint.x) && (end.y == currentPoint.y))
        {
            found = true;
            break;
        }

        closedList[closedListLength++] = (int2)currentPoint;

        stX = currentPoint.x - 1;
        enX = currentPoint.x + 1;

        if (stX < 0)
        {
            stX = 0;
        }
        if(enX > (width - 1))
        {
            enX = width - 1;
        }

        stY = currentPoint.y - 1;
        enY = currentPoint.y + 1;

        if (stY < 0)
        {
            stY = 0;
        }
        if(enY > (height - 1))
        {
            enY = height - 1;
        }

        for (x = stX; x <= enX; x++)
        {
            for (y = stY; y <= enY; y++)
            {
                index = (int)(x + (width * y));

                if (((x == currentPoint.x) && (y == currentPoint.y)) || (input[index]) == true)
                {
                    continue;
                }

                inClosedList = false;
                for (i=0; i<closedListLength; i++)
                {
                    tempPoint = (int2)closedList[i];

                    if ((tempPoint.x == x) && (tempPoint.y == y))
                    {
                        inClosedList = true;
                        break;
                    }
                }

                if (inClosedList)
                {
                    continue;
                }

                inOpenedList = false;

                for (i=0; i<openedListLength; i++)
                {
                    tempPoint = (int2)openedList[i];

                    if ((tempPoint.x == x) && (tempPoint.y == y))
                    {
                        inOpenedList = true;
                        break;
                    }
                }

                tempH = (float)(gScore[(int)(currentPoint.x + (currentPoint.y * width))] + heuristic(currentPoint, (int2)(x, y)));

                if (!inOpenedList)
                {
                    openedList[openedListLength++] = (int2)(x, y);
                    betterPath = true;
                }
                else if (tempH < (float)gScore[index])
                {
                    betterPath = true;
                }
                else
                {
                    betterPath = false;
                }

                if (betterPath)
                {
                    cameFrom[index] = (int)(currentPoint.x + (currentPoint.y * width));

                    gScore[index] = (float)tempH;
                    tempH = (float)heuristic((int2)(x, y), end);

                    hScore[index] = (float)tempH;
                    fScore[index] = (float)gScore[index] + (float)hScore[index];
                }
            }
        }
    }

    if (found)
    {
        int currentNode = cameFrom[end.x + (end.y * width)];

        while (currentNode != -1)
        {
            if ((currentNode != (int)(start.x + (width * start.y))) && (currentNode != (int)(end.x + (width * end.y))))
            {
                output[currentNode + ((width * height) * globalIdx)] = '*';
            }

            currentNode = (int)cameFrom[currentNode];
        }
    }
}