a*2

struct state
{
    state *father;
    unsigned short g;
    unsigned short h;
    unsigned short fields[16];
};

struct compareWeight
{
    bool operator()(state const *s1, state const *s2)
    {
        return s1->g + s1->h < s2->g + s2->h;
    }
};

std::multiset<state*, compareWeight> openStates;

std::vector<state*> closedStates;

int getManhattanValue(state *currentState)
{
    int result = 0;
    for (int j = 0; j < size; j++)
    {
        if (currentState->fields[j] != 0)
        {
            unsigned short value = (currentState->fields[j] + size - 1) % size;
            unsigned short xGoal = value / side;
            unsigned short yGoal = value % side;
            unsigned short xActual = j / side;
            unsigned short yActual = j % side;
            result += abs(xGoal - xActual) + abs(yGoal - yActual);
        }
    }
    return result;
}

getMisplacedTilesValue(state *currentState)
{
    int result = 0;
    for (int j = 0; j < size; j++)
    {
        if (currentState->fields[j] != 0 && currentState->fields[j] != (j + 1) % 16)
        {
            result += 1;
        }
    }
    return result;
}




void aStar(int maximumNumberOfMoves, bool isManhattan)
{
    state *initialState = new state;
    getInitialState(initialState, maximumNumberOfMoves);
    initialState->g = 0;
    initialState->h = getManhattanValue(initialState);
    initialState->father = NULL;

    openStates.insert(initialState);

    std::cout << "Stan poczatkowy:\n";
    for (int i = 0; i < side; i++)
    {
        for (int j = 0; j < side; j++)
        {
            std::cout << initialState->fields[i * side + j] << " ";
        }
        std::cout << "\n";
    }
    std::cout << "\n" << "h = " << initialState->h << "\n";

    while (!openStates.empty())
    {
        state* currentState = (state*) (*openStates.begin());

        openStates.erase(openStates.begin());
        closedStates.push_back(currentState);

        if (isFinalState(currentState))
        {
            std::cout << "\nPrzesuniecia:  ";
            getAndPrintRoute(currentState);
            std::cout << "\nLiczba przesuniec:  " << counter << "\n";
            cleanMemory();
            return;
        }

        unsigned short posZero = 0;

        for (int i = 0; i < size; i++)
        {
            if (currentState->fields[i] == 0)
            {
                posZero = i;
                break;
            }
        }

        for (int i = 0; i < 4; i++)
        {
            int newPosZero;
            if (i == 0 && posZero / side != 0)
            {
                newPosZero = posZero - side;
            }
            else if (i == 1 && posZero / side != side - 1)
            {
                newPosZero = posZero + side;
            }
            else if (i == 2 && posZero % side != 0)
            {
                newPosZero = posZero - 1;
            }
            else if (i == 3 && posZero % side != side - 1)
            {
                newPosZero = posZero + 1;
            }
            else
            {
                continue;
            }

            unsigned short newFields[size];
            for (int j = 0; j < size; j++)
            {
                newFields[j] = currentState->fields[j];
            }
            std::swap(newFields[posZero], newFields[newPosZero]);

            std::vector<state*>::iterator it = closedStates.begin();

            bool isInClosed = false;
            while (it != closedStates.end())
            {
                bool isEqual = true;
                for (int j = 0; j < size; j++)
                {
                    if ((*it)->fields[j] != newFields[j])
                    {
                        isEqual = false;
                        break;
                    }
                }

                if (isEqual)
                {
                    isInClosed = true;
                    break;
                }
                it++;
            }
            if (isInClosed)
            {
                continue;
            }

            state *newState;
            std::multiset<state*>::iterator it2 = openStates.begin();

            bool isInOpen = false;
            while (it2 != openStates.end())
            {
                bool isEqual = true;
                for (int j = 0; j < size; j++)
                {
                    if ((*it2)->fields[j] != newFields[j])
                    {
                        isEqual = false;
                        break;
                    }
                }

                if (isEqual)
                {
                    isInOpen = true;
                    newState = (state *)&(*it2);
                    break;
                }
                it2++;
            }
            if (!isInOpen)
            {
                newState = new state;
                for (int j = 0; j < size; j++)
                {
                    newState->fields[j] = newFields[j];
                }
                newState->g = INT16_MAX;
                newState->h = INT16_MAX;
            }

            int newG = currentState->g + 1;
            if (newG < newState->g)
            {
                newState->g = newG;
                if (isManhattan)
                {
                    newState->h = getManhattanValue(newState);
                }
                else
                {
                    newState->h = getMisplacedTilesValue(newState);
                }

                newState->father = currentState;
            }

            if (!isInOpen)
            {
                openStates.insert(newState);
            }
        }
    }
}