float calcH(Tile* current, Tile* target, Tile* start)
{
    //float H = 10*(abs(current->tileX-target->tileX) + abs(current->tileY-target->tileY));
    //float deltaX = current->tileX-target->tileX;
    //float deltaY = current->tileY-target->tileY;
    //float H = sqrtf((deltaX*deltaX)+(deltaY*deltaY));

    /*float dx1 = current->tileX - target->tileX;
    float dy1 = current->tileY - target->tileY;
    float dx2 = start->tileX - target->tileX;
    float dy2 = start->tileY - target->tileY;
    float cross = abs(dx1*dy2 - dx2*dy1);
    H += cross*0.026545;*/

    const int DiagCost = 14;
    const int NormalCost = 10;
    // distance code
    int dx = abs(current->tileX - target->tileX);
    int dy = abs(current->tileY - target->tileY);
    int diag = min(dx, dy);
    int straight = dx + dy - (diag << 1);
    int H = DiagCost * diag + NormalCost * straight;

    return H;
}

bool find(std::vector<Tile*>& closed, Tile* tile)
{
    for(int i = 0;i<closed.size();i++)
    {
        if(closed[i] == tile)
            return true;
    }
    return false;
}

void PathFinder::Init()
{
}

Tile* PathFinder::FindPath(int objectID, Tile* current, Tile* target, std::vector<Tile*>& closed)
{
    return current;
}

bool sort_function (Tile* i, Tile* j) { return (i->F<j->F); }

std::vector<Tile*> PathFinder::FindPath2(Tile* current, Tile* target)
{
    std::vector<Tile*> open;
    std::vector<Tile*> closed;

    open.push_back(current);

    Tile* curr = 0;
    bool done = false;

    //Phase 2
    int checks = 0;
    while(curr != target && !done && open.size() > 0)
    {
        if(open.size() == 1)
        {
            curr = open[0];
            open.pop_back();
            closed.push_back(curr);
        }else{
            std::sort(open.begin(), open.end(), sort_function);
            Tile* temp = open.front();
            open.erase(open.begin());
            closed.push_back(temp);

            curr = temp;
        }
        checks++;

        Tile* tile1 = root->map->GetTile1(curr->tileX-1, curr->tileY-1);
        Tile* tile2 = root->map->GetTile1(curr->tileX,   curr->tileY-1);
        Tile* tile3 = root->map->GetTile1(curr->tileX+1, curr->tileY-1);
        Tile* tile4 = root->map->GetTile1(curr->tileX+1, curr->tileY);
        Tile* tile5 = root->map->GetTile1(curr->tileX+1, curr->tileY+1);
        Tile* tile6 = root->map->GetTile1(curr->tileX,   curr->tileY+1);
        Tile* tile7 = root->map->GetTile1(curr->tileX-1, curr->tileY+1);
        Tile* tile8 = root->map->GetTile1(curr->tileX-1, curr->tileY);

        if((!tile1 || tile1->isTaken) && (!tile2 || tile2->isTaken) && (!tile3 || tile3->isTaken) && (!tile4 || tile4->isTaken) && (!tile5 || tile5->isTaken) && (!tile6 || tile6->isTaken) && (!tile7 || tile7->isTaken) && (!tile8 || tile8->isTaken))
        {
            //done = true;
            continue;
        }

        std::vector<Tile*> tempList;

        if(tile1 && !tile1->isTaken)
            tempList.push_back(tile1);

        if(tile2 && !tile2->isTaken)
            tempList.push_back(tile2);

        if(tile3 && !tile3->isTaken)
            tempList.push_back(tile3);

        if(tile4 && !tile4->isTaken)
            tempList.push_back(tile4);

        if(tile5 && !tile5->isTaken)
            tempList.push_back(tile5);

        if(tile6 && !tile6->isTaken)
            tempList.push_back(tile6);

        if(tile7 && !tile7->isTaken)
            tempList.push_back(tile7);

        if(tile8 && !tile8->isTaken)
            tempList.push_back(tile8);

        for(int i = 0;i<tempList.size();i++)
        {
            if(find(closed, tempList[i]))
            {
            }else if(find(open, tempList[i]))
            {
                Tile* parent = tempList[i]->parent;
                bool diag2 = (curr->tileX == tempList[i]->tileX-1 && curr->tileY == tempList[i]->tileY-1) || (curr->tileX == tempList[i]->tileX+1 && curr->tileY == tempList[i]->tileY-1) || (curr->tileX == tempList[i]->tileX+1 && curr->tileY == tempList[i]->tileY+1) || (curr->tileX == tempList[i]->tileX-1 && curr->tileY == tempList[i]->tileY+1);

                int tempG;
                tempG = (diag2)?14:10;
                if(curr->G + tempG < tempList[i]->G)
                {
                    tempList[i]->G = tempG;
                    tempList[i]->F = tempList[i]->G+tempList[i]->H;
                    tempList[i]->parent = curr;
                }
            }else{
                /*if(tempList[i] == target)
                {
                    tempList[i]->parent = curr;
                    curr = tempList[i];
                    break;
                }*/

                bool diag = (curr->tileX == tempList[i]->tileX-1 && curr->tileY == tempList[i]->tileY-1) || (curr->tileX == tempList[i]->tileX+1 && curr->tileY == tempList[i]->tileY-1) || (curr->tileX == tempList[i]->tileX+1 && curr->tileY == tempList[i]->tileY+1) || (curr->tileX == tempList[i]->tileX-1 && curr->tileY == tempList[i]->tileY+1);

                if(diag)
                    tempList[i]->G = curr->G + 14;//1.414;
                else
                    tempList[i]->G = curr->G + 10;

                tempList[i]->H = calcH(tempList[i], target, current);
                tempList[i]->F = tempList[i]->G+tempList[i]->H;
                tempList[i]->parent = curr;

                if(open.size() > 0)
                {
                    open.push_back(tempList[i]);
                }else{
                    open.push_back(tempList[i]);
                }
            }
        }
    }

    std::vector<Tile*> path;
    closed.push_back(curr);

    if(curr)
    {
        while(curr != current)
        {
            path.push_back(curr);
            curr = curr->parent;
        }
    }

    root->debugRenderer->setDebugPathPoints(closed);
    std::cout << checks << ", " << path.size() << std::endl;
    return path;
}