public void SinglePUpdate(TimeSpan elapsedGameTime)
        {
            //Plan
            if (planRRT && predictor != null && predictor.CharactersReady())
            {
                controlling = false;
                planSolution();
            }
            else if (getDebugInfo)
            {
                if (cutplan)
                {
                    debugInfo = RRT.getDebugInfo(T, pathPlan.cleanPlan(obstaclesInfo, Diamonds, area, circleInfo.Radius, true, true)).ToArray();
                }
                else
                {
                    debugInfo = RRT.getDebugInfo(T, pathPlan.debugCleanPlan()).ToArray();
                }
                //debugInfo = RRT.getDebugInfo(T, null).ToArray();

                getDebugInfo = false;
            }
            else if (!getDebugInfo)
            {
                debugInfo = null;
            }

            //Control - if there is a plan then execute it
            if (pathPlan.getPathPoints() != null && pathPlan.getPathPoints().Count != 0 && hasStarted)
            {
                controlling = true;
                planExecution();
            }
            else if (pathPlan.getTotalCollectibles() != 0 && controlling && pathPlan.getTotalCollectibles() == uncaughtCollectibles.Count)
            {
                //if currently the number of uncaught collectibles is the same as the one supposed to be at the end of the plan, stop the agent and replan
                replan(false);
            }
            //make sure the agent replans when missing the last action
            else if (pathPlan.getPathPoints().Count == 0 && controlling)// && lastAction)
            {
                lastActionReplan();
            }
            //if it none of the above work and the too much time has already passed, replan
            else if (controlling && (gameTime.ElapsedMilliseconds * 0.001f * gSpeed > pointTimeMargin * gSpeed))
            {
                replan(true);
            }
            else if (pathPlan.getPathPoints().Count == 0 && !controlling && !planRRT)
            {
                planRRT = true;
                newPlan = true;
            }
        }
        /*******************************************************/
        /*                  Planning - aux                     */
        /*******************************************************/

        //used when trying to recover a plan where the agent is not on a platform and state of the plan
        //join plans on utils

        //simple plan recovery - check if agent is on a platform that belongs to the plan and with the same state
        private void recoverPlan()
        {
            //TODO - update debug drawing
            currentAction          = Moves.NO_ACTION;
            lastMove               = Moves.NO_ACTION;
            controller.jumpReached = false;
            controller.rollReached = false;

            //see if there is a point in the original plan that is the same as the one the agent is on and that has the same number or less of diamonds caught
            bool pointFound = false;

            Platform     currentPlatform = utils.onPlatform(circleInfo.X, circleInfo.Y + circleInfo.Radius, 25, 10);
            List <Point> pathPoints      = pathPlan.getOriginalPoints();

            int i;

            //start from the end
            for (i = pathPoints.Count - 1; i >= 0; i--)
            {
                if (pathPoints[i].getUncaughtDiamonds().Count >= uncaughtCollectibles.Count)
                {
                    Platform pointPlatform = utils.onPlatform(pathPoints[i].getPosX(), pathPoints[i].getPosY() + circleInfo.Radius, 25, 10);

                    if (utils.samePlatform(currentPlatform, pointPlatform) && !utils.obstacleBetween(circleInfo.X, pathPoints[i].getPosX(), currentPlatform))
                    {
                        pointFound = true;
                        break;
                    }
                }
            }
            if (pointFound)
            {
                //create a new plan from the point we got previously
                pathPlan = new PathPlan(cutplan, remaining.Count, utils);
                pathPlan.setTotalCollectibles(originalPlan.getTotalCollectibles());
                pathPlan.setCurrentPoint(i);

                for (int j = i; j < pathPoints.Count; j++)
                {
                    pathPlan.addPointEnd(pathPoints[j]);
                }
                pathPlan.setOriginalPoints(pathPoints);
                firstAction = true;
                return;
            }
            //if no platform in common was found, then try to find a way to a platform in the plan and replan if this fails
            else if (pathPlan.getPathPoints().Count != 0)
            {
                List <DiamondInfo> remainingDiamonds = new List <DiamondInfo>();
                List <DiamondInfo> caughtDiamonds    = new List <DiamondInfo>();
                foreach (DiamondInfo diamond in Diamonds)
                {
                    if (!diamond.wasCaught())
                    {
                        remainingDiamonds.Add(diamond);
                    }
                    else
                    {
                        caughtDiamonds.Add(diamond);
                    }
                }

                //Simulator
                Simulator sim = new CircleSimulator(Platforms);
                sim.setSimulator(circleInfo.X, circleInfo.Y, circleInfo.VelocityX, circleInfo.VelocityY, remainingDiamonds);

                //TEST
                RRT.setDiamonds(Diamonds);

                State   initialState = new State(circleInfo.X, circleInfo.Y, circleInfo.VelocityX, circleInfo.VelocityY, circleInfo.Radius, circleInfo.Radius, caughtDiamonds, remainingDiamonds);
                float[] returnPos    = new float[2];
                returnPos[0] = pathPlan.getPathPoints()[0].getPosX();
                returnPos[1] = pathPlan.getPathPoints()[0].getPosY();
                RRT.setReturnPos(returnPos);
                Tree t = RRT.buildNewMPRRT(initialState, sim, GoalType.Return, iterationsS);

                if (t.getGoal() != null)
                {
                    PathPlan shortPlan = RRT.getPlan(t);
                    pathPlan = pathPlan.joinPlans(shortPlan, pathPlan);
                    pathPlan.cleanPlan(obstaclesInfo, remainingDiamonds, area, circleInfo.Radius, true, true);
                    getDebugInfo = true;
                    return;
                }
                else
                {
                    replan(false);
                }
            }
            replan(false);
        }
        /********************************************************************************************/
        /********************************************************************************************/
        /***                                                                                      ***/
        /***                                     PLANNING                                         ***/
        /***                                                                                      ***/
        /********************************************************************************************/
        /********************************************************************************************/

        private void planSolution()
        {
            //The agent must be still so it starts at the same position as the one in the first point of the plan
            //This is to guarantee that the agent stops before start planning, and keeps still
            if (circleInfo.VelocityY < correctVelYMargin && circleInfo.VelocityY > -correctVelYMargin &&
                circleInfo.VelocityX < correctVelXMargin && circleInfo.VelocityX > -correctVelXMargin &&
                utils.onPlatform(circleInfo.X, circleInfo.Y + circleInfo.Radius, 25, 10) != null)
            {
                //make sure there is nothing moving the agent when planning
                currentAction = Moves.NO_ACTION;

                //if the plan is new build a new tree
                if (newPlan)
                {
                    List <DiamondInfo> remainingDiamonds = new List <DiamondInfo>();
                    List <DiamondInfo> caughtDiamonds    = new List <DiamondInfo>();
                    foreach (DiamondInfo diamond in Diamonds)
                    {
                        if (!diamond.wasCaught())
                        {
                            remainingDiamonds.Add(diamond);
                        }
                        else
                        {
                            caughtDiamonds.Add(diamond);
                        }
                    }

                    //Simulator
                    Simulator sim = new CircleSimulator(Platforms);
                    sim.setSimulator(circleInfo.X, circleInfo.Y, circleInfo.VelocityX, circleInfo.VelocityY, remainingDiamonds);

                    //update the diamond list
                    RRT.setDiamonds(Diamonds);
                    //create initial state
                    State initialState = new State(circleInfo.X, circleInfo.Y, circleInfo.VelocityX, circleInfo.VelocityY, circleInfo.Radius, circleInfo.Radius, caughtDiamonds, remainingDiamonds);
                    //run algorithm
                    T = RRT.buildNewRRT(initialState, sim, iterationsS);
                }
                else //continue the previous tree
                {
                    T = RRT.RRT(T);
                }
                //draw the nodes of the tree
                if (debugTree)
                {
                    debugInfo = RRT.getDebugTreeInfo(T).ToArray();
                }

                newPlan = false;

                //if the argorithm reached a goal state or a semi plan then get the plan
                if (T.getGoal() != null)
                {
                    if (!written)
                    {
                        int exploredNodesOnce  = RRT.getExploredNodesOnce();
                        int exploredNodesTotal = RRT.getExploredNodesTotal();
                        int totalNodes         = T.getNodes().Count;
                        utils.writeTimeToFile(1, 0, searchTime, exploredNodesOnce, exploredNodesTotal, totalNodes, gSpeed);
                        written = true;
                    }

                    pathPlan = RRT.getPlan(T);

                    firstAction = true;
                    lastMove    = Moves.NO_ACTION;


                    //do not plan on the next iteration
                    planRRT      = false;
                    getDebugInfo = true;

                    //save a copy of the original plan
                    if (cutplan)
                    {
                        pathPlan.cleanPlan(obstaclesInfo, Diamonds, area, circleInfo.Radius, true, true);
                    }
                    originalPlan = pathPlan.clone();
                    pathPlan.saveOriginal();
                }
            }
            else
            {
                keepStill();
            }
        }