new void GetNearestWaypoint()
 {
     WPCurrent = WPTarget;
     if (graph != null)
     {
         WPTarget = graph.GetNearestWaypoint(pos);
     }
 }
        protected void ResetAll()
        {
            WPCurrent = null;
            WPTarget  = null;
            conn      = null;

            ConnectionHistory  = new List <WPConnection>();;
            mind.DecisionQueue = new List <string>();

            DecisionTimeoutTimer.Abort();
            snoringTimer.Abort();
            wakeUpTimer.Abort();
            surpriseTimer.Abort();
            StuckRunning.Abort();
            PrepareJumpTimer.Abort();
            LandTimeout.Abort();
            LandedTimer.Abort();
            DrillSubmergeTimer.Abort();
            DrillEmergeTimer.Abort();
            DrillWindDownTimer.Abort();
        }
        void GetNextTargetWaypoint()
        {
            TrackLastWaypoints();
            WPCurrent = WPTarget;

            if (WPCurrent != null)
            {
                //let's build a small collection of potential connections we could form, assign them scores based on how attractive they are, and choose one from
                //  the most attractive ones at random
                int attempt = 10;
                List <PotentialConnection> PCs   = new List <PotentialConnection>();
                List <PotentialConnection> valid = new List <PotentialConnection>();

                while (attempt > 0)
                {
                    bool useful = true;
                    //Create new connection with max score
                    PotentialConnection PC = new PotentialConnection(WPCurrent.GetRandomNextConnection(), 1000);
                    foreach (var pc in PCs)
                    {
                        if (pc.connection == PC.connection)
                        {
                            useful = false;  //don't add the same connection to our list twice
                            break;
                        }
                    }

                    if (useful)
                    {
                        foreach (var c in ConnectionHistory)
                        {
                            if (c != null)
                            {
                                if (c == PC.connection)
                                {
                                    PC.score -= 400;                             //heavy score penalty for exactly the same path we just came from
                                }
                                //look through our own prev list
                                foreach (var p in WPCurrent.prev)
                                {
                                    //ask whether one of the Waypoints here owns the Waypoint described by the Connection History
                                    if (p.wp.ConnectionInNext(c))
                                    {
                                        //Are you the one we're trying to path towards?
                                        if (p.wp == PC.connection.wp)
                                        {
                                            //then subtract score
                                            PC.score -= 200; //we don't super like going back to a Waypoint we've visited in general,
                                        }                    //   but it's not as bad as using the exact same path

                                        break;               //If no, that's gucci; and also we can stop asking around, because there's no way someone else owns the same connection.
                                    }                        //  Either way, we're breaking out of here
                                }
                            }
                        }

                        PCs.Add(PC);
                    }

                    attempt--;
                }

                //sort the list by highest scorers (highest at the end)
                PCs.Sort((PC1, PC2) => PC1.score.CompareTo(PC2.score));

                int threshold = 100;
                int highScore = PCs[PCs.Count - 1].score;

                valid.Add(PCs[PCs.Count - 1]);

                //starting at one before the end (which has already been added to valid), search through each PC and only add entries to valid that
                //  are within threshold of the highest scores
                for (var i = PCs.Count - 2; i >= 0; i--)
                {
                    if (PCs[i].score + threshold >= highScore)
                    {
                        valid.Add(PCs[i]);
                    }
                }

                //finally, go through all these valid candidates, and select one at random!
                conn     = valid[Random.Range(0, valid.Count)].connection;
                WPTarget = conn.wp;
            }
        }