bool AutomaticDeplacement::pathFinding(irr::core::vector3df arrivee)
{
    m_arrivee = arrivee;
    m_depart = m_robot->getPosition();
    m_current = m_robot->getPosition();
    int xValue[8] = {-1,-1,0,+1,+1,+1,0,-1};
    int zValue[8] = {0,+1,+1,+1,0,-1,-1,-1};
    int factor[8] = {10,14,10,14,10,14,10,14};
    float xVector[8]= {100,60,0,-60,-100,-60,0,60};
    float zVector[8]= {0,-60,-100,-60,0,60,100,60};
    irr::core::vector3df a = irr::core::vector3df(1000000,1000000,1000000);
    Node depart(m_current, a, 0, 0, m_arrivee);
    
    listeFermee.push_back(depart);
    searching = true;
    pathFound = false;
    while(searching)
    {
         if(listeFermee.size()==0)
        {
            cout << "Progression = 0%" << endl;
        }
        else
        {
            int ancienPourcentage = 0;
            ancienPourcentage = pourcentage;
            pourcentage = 100-((listeFermee.back().getF() - listeFermee.back().getG())/100);
            if(ancienPourcentage!=pourcentage)
                cout << "Progression = " << pourcentage << "%" << endl;
        }

        Node parent = listeFermee.back();
        if(parent.getPosition().X == m_arrivee.X && parent.getPosition().Z == m_arrivee.Z)
        {
            searching = false;
            pathFound = true;
        }
        else
        {
            for(unsigned int i=0; i<8; i++)
            {
                irr::core::vector3df newPosition = m_robot->getPosition();
                newPosition.X = parent.getPosition().X + xValue[i];
                newPosition.Z = parent.getPosition().Z + zValue[i];
                Node currentNode(newPosition, parent.getPosition(), parent.getG(), factor[i], m_arrivee);
                if(!checkCollision(currentNode.getPosition(),xVector[i], zVector[i]) && !checkListeFermee(currentNode))
                {
                    if(!checkListeOuverte(currentNode, parent))
                        listeOuverte.push_back(currentNode);
                }


            }
            LowestF(parent);
        }
    }
    if(pathFound)
        {
            list<Node>::reverse_iterator it;
            for(it = listeFermee.rbegin(); it!=listeFermee.rend();++it)
            {
                Node currentNode = *it;
                finalPath.push_back(currentNode.getPosition());
            }
        }

    return true;
}

bool AutomaticDeplacement::checkCollision(irr::core::vector3df position, float vectorX, float vectorZ)
{
    ray.start = position;
    ray.end = ray.start + irr::core::vector3df(X,40,Z);
    coll = collisionManager->getSceneNodeAndCollisionPointFromRay(ray,outCollisionPoint,outTriangle);
    if(coll)
    {
        return true;
        cout << "Collision" << endl;
    }
    else
        return false;
}