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;
            }
        }