Night Stalker

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.IO;
using System.Windows.Forms;
using System.Runtime.Serialization.Formatters.Binary;

namespace Night_Stalker
{
    public struct udtShortestPathToWayPoint
    {
        public classWayPoint wp;
        public int index;
        public string strPath;
    }

    public class classWayPointPaths
    {
        public struct udtWP_BFSPixQ
        {
            public classWP_PixBFSElement head;
            public classWP_PixBFSElement tail;
        }

        const int intMaxRangeSearchNeaghbour = 85;
        public Bitmap bmpWPs = Night_Stalker.Properties.Resources.Map_WPs;

        int intCounter = 0;
        public bool bolWPDebug = false;

        const string strWPsFilename = "WayPointsArray.bfs";
        const string strWPsTable = "WayPointsTable.bfs";
        const string strNearestWPsTable = "NearestWayPointsTable.bfs";
        int intNearestWayPointGridSize = 25;
        Size szNearestWayPointsTableSize;

        classHelperFunctions cLibHelper;
        int intWPNameFieldLength = -1;

        // way points are locations on the map between which travel paths are known before hand
        // AI controlled characters travel from way-point to way-point as if they were on rails
        // by following these paths
        public classWayPoint[] WPs; // array containing all way-points 

        /// <summary>
        /// the map is divided into 25x25 squares which each contain a pointer to the way-point on the map
        /// closest to their location.  
        /// this table is used whenever an AI-Character needs to reach Jessica because Jessica is rarely ever
        /// directly on a way-point and the way-points are listed in a single array which does not allow 
        /// easy identification of the way-ponit nearest to her.
        /// </summary>
        public classWayPoint[,] nearestWPTable;

        /// <summary>
        /// when an AI-character needs to travel from one way-point to a distant way-point it cross checks this 2D array
        /// if it is at WPstart and needs to get to WPend then it looks at table entry WPTable[WPStart.index, WPEnd.index] 
        /// to know which way-point to travel to next.
        /// the .index field of the way-point corresponds to that way-point's location in the WPs[] array and
        /// since each WP has a list (of variable size) of all neaghbouring way-points which the AI-character can travel to
        /// the 'next-way-point' along its path from WPstart to WPend is in this list and when the AI-character
        /// reaches the next way-point then it begins the same algorithm again using the WP at its current location
        /// as the 'WPstart' and continues until it reaches the end.
        /// see Line 67 below
        /// </summary>
        public int[,] nextWPTable;

        // uses the way-point long integer name = pt.X.toString("D4") + pt.Y.toString("D4")
        public classWP_BinTreeElement WPBinTree; 
        classWP_BFSElement Q_WP_BFS = null;
        
        bool[,] bolSeen; 
        Point ptBolSeenReference;
        
        udtWP_BFSPixQ udrPixQ;

        Color clrWP;
        Color clrCD;

        public classWayPointPaths(ref classHelperFunctions Helper)
        {
            cLibHelper = Helper;
            clrWP = bmpWPs.GetPixel(0, 0);
            clrCD = bmpWPs.GetPixel(1, 0);

            string strWPNameFieldSample = bmpWPs.Width > bmpWPs.Height ? bmpWPs.Width.ToString() : bmpWPs.Height.ToString();
            intWPNameFieldLength = strWPNameFieldSample.Length;
            loadWPsAndTable();            
        }

        #region "rebuild Way Points"

        public void writeWPsAndTable()
        {
            // write way-points to memory
            BinaryFormatter formatter = new BinaryFormatter();
            FileStream fs = new FileStream(strWPsFilename, FileMode.Create);
            formatter.Serialize(fs, WPs.Length);

            for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
            {
                formatter.Serialize(fs, WPs[intWPCounter].pt);
                formatter.Serialize(fs, WPs[intWPCounter].neaghbours.Length);
                for (int intNeaghbourCounter = 0; intNeaghbourCounter < WPs[intWPCounter].neaghbours.Length; intNeaghbourCounter++)
                {
                    formatter.Serialize(fs, WPs[intWPCounter].neaghbours[intNeaghbourCounter].index);
                    formatter.Serialize(fs, WPs[intWPCounter].neaghbours[intNeaghbourCounter].strPath);
                }
            }

            fs.Close();

            // write table to memory
            formatter = new BinaryFormatter();
            fs = new FileStream(strWPsTable, FileMode.Create);

            for (int intX = 0; intX < WPs.Length; intX++)
                for (int intY = 0; intY < WPs.Length; intY++)
                {
                    if (intX != intY)
                        formatter.Serialize(fs, nextWPTable[intX, intY]);
                    else
                        formatter.Serialize(fs, intX);
                }

            fs.Close();
        }

        public void loadWPsAndTable()
        {
            /// first load the Way-Points from source file
            BinaryFormatter formatter = new BinaryFormatter();
            FileStream fs = new FileStream(cLibHelper.getResourceDirectoryString() + strWPsFilename, FileMode.Open);
            int intArraySize = (int)formatter.Deserialize(fs);
            WPs = new classWayPoint[intArraySize];

            for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
            {
                WPs[intWPCounter] = new classWayPoint();

                WPs[intWPCounter].index = intWPCounter;

                WPs[intWPCounter].pt = (Point)formatter.Deserialize(fs);
                WPs[intWPCounter].strName = getWPName(WPs[intWPCounter].pt);
                WPs[intWPCounter].lngName = Convert.ToInt64(WPs[intWPCounter].strName);

                int intNumNeaghbours = (int)formatter.Deserialize(fs);
                WPs[intWPCounter].neaghbours = new udtShortestPathToWayPoint[intNumNeaghbours];

                for (int intNeaghbourCounter = 0; intNeaghbourCounter < WPs[intWPCounter].neaghbours.Length; intNeaghbourCounter++)
                {
                    WPs[intWPCounter].neaghbours[intNeaghbourCounter].index = (int)formatter.Deserialize(fs);
                    WPs[intWPCounter].neaghbours[intNeaghbourCounter].strPath = (string)formatter.Deserialize(fs);
                }
            }

            /// reconnect pointers in neaghbours lists given indices AND complete array of WPs
            for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
                for (int intNeaghbourCounter = 0; intNeaghbourCounter < WPs[intWPCounter].neaghbours.Length; intNeaghbourCounter++)
                    WPs[intWPCounter].neaghbours[intNeaghbourCounter].wp = WPs[WPs[intWPCounter].neaghbours[intNeaghbourCounter].index];

            fs.Close();
            /// load Way-Points travel table
      
            formatter = new BinaryFormatter();
            fs = new FileStream(cLibHelper.getResourceDirectoryString() + strWPsTable, FileMode.Open);

            nextWPTable = new int[WPs.Length, WPs.Length];
            for (int intX = 0; intX < WPs.Length; intX++)
                for (int intY = 0; intY < WPs.Length; intY++)
                    nextWPTable[intX, intY] = (int)formatter.Deserialize(fs);
    
            fs.Close();

            buildBinTree();
  
            // load nearest WPs table
            formatter = new BinaryFormatter();
            fs = new FileStream(cLibHelper.getResourceDirectoryString() + strNearestWPsTable, FileMode.Open);

            int intWidth = (int)formatter.Deserialize(fs);
            int intHeight = (int)formatter.Deserialize(fs);

            szNearestWayPointsTableSize = new Size(intWidth, intHeight);
            nearestWPTable = new classWayPoint[szNearestWayPointsTableSize.Width, szNearestWayPointsTableSize.Height];

            for (int intX = 0; intX < szNearestWayPointsTableSize.Width; intX++)
                for (int intY = 0; intY < szNearestWayPointsTableSize.Height; intY++)
                {
                    long lngName = (long)formatter.Deserialize(fs);
                    if (lngName > 0)
                        nearestWPTable[intX, intY] = BinTree_Search(lngName);
                }
            
            fs.Close();
        }

        public void rebuildNearestWayPointsTable()
        {
            szNearestWayPointsTableSize = new Size(bmpWPs.Width / intNearestWayPointGridSize , bmpWPs.Height / intNearestWayPointGridSize);
            nearestWPTable = new classWayPoint[szNearestWayPointsTableSize.Width, szNearestWayPointsTableSize.Height];

            for (int intX = 0; intX < szNearestWayPointsTableSize.Width; intX++)
                for (int intY = 0; intY < szNearestWayPointsTableSize.Height; intY++)
                {
                    cLibHelper.lblFeedback.Text = "finding : (" + intX.ToString() + "/" + szNearestWayPointsTableSize.Width.ToString()+ "," + intY.ToString() + "/" + szNearestWayPointsTableSize.Height.ToString() +")";
                    cLibHelper.lblFeedback.Refresh();
                    Point pt = new Point(intX * intNearestWayPointGridSize + intNearestWayPointGridSize / 2,
                                         intY * intNearestWayPointGridSize + intNearestWayPointGridSize / 2);
                    nearestWPTable[intX, intY] = (findNearestWayPoint(pt)).wp;
                }

            BinaryFormatter formatter = new BinaryFormatter();
            FileStream fs = new FileStream(cLibHelper.getResourceDirectoryString() +  strNearestWPsTable, FileMode.Create);
            int intWidth = szNearestWayPointsTableSize.Width;
            int intHeight = szNearestWayPointsTableSize.Height;

            formatter.Serialize(fs, intWidth );
            formatter.Serialize(fs, intHeight);

            for (int intX = 0; intX < szNearestWayPointsTableSize.Width; intX++)
                for (int intY = 0; intY < szNearestWayPointsTableSize.Height; intY++)
                {
                    cLibHelper.lblFeedback.Text = "writing : (" + intX.ToString() + ", " + intY.ToString() + ")";
                    cLibHelper.lblFeedback.Refresh();
                    if (nearestWPTable[intX, intY] == null)
                        formatter.Serialize(fs, (long)-1);
                    else
                        formatter.Serialize(fs, nearestWPTable[intX, intY].lngName);
                }
            fs.Close();
        }

        string getWPString(classWayPoint thisWP)
        {
            return "(" + thisWP.pt.X.ToString() + "," + thisWP.pt.Y.ToString() + ")"
                   + thisWP.strName.ToString() + "," + thisWP.lngName.ToString()
                   + (thisWP.neaghbours == null ? "neaghbours are null" :
                                                   thisWP.neaghbours.Length.ToString());
        }

        /// <summary>
        /// finds all the WPs on bitmap bmpWPs (Night_Stalker.Properties.Resources.Map_WPs)
        /// </summary>
        public void findWPs()
        {
            WPs = new classWayPoint[0];
            //cLibHelper.clearDebug();
            for (int intX = 0; intX < bmpWPs.Width; intX++)
                for (int intY = 0; intY < bmpWPs.Height; intY ++ )
                {
                    if (!(intX == 0 && intY == 0))
                    {
                        if (bmpWPs.GetPixel(intX, intY) == clrWP)
                            addWP(intX, intY);
                    }
                }
            buildBinTree();
        }

        /// <summary>
        /// creates a new element of classWayPoint and resizes the WPs array to insert this new element
        /// </summary>
        /// <param name="intX">WP's x location</param>
        /// <param name="intY">WP's y location</param>
        void addWP(int intX, int intY)
        {
            Array.Resize<classWayPoint>(ref WPs, WPs.Length + 1);
            classWayPoint WPNew = new classWayPoint();
            WPNew.pt = new Point(intX, intY);
            WPNew.strName= getWPName(WPNew.pt);
            WPNew.lngName = Convert.ToInt64(WPNew.strName);
            WPNew.neaghbours = new udtShortestPathToWayPoint[0];
            WPNew.index = WPs.Length - 1;

            WPs[WPs.Length - 1] = WPNew;
        }

        /// <summary>
        /// converts a point into a unique string of standard 'intWPNameFieldLength' size for this way-point 
        /// </summary>
        /// <param name="WPpt">way-point's map location</param>
        /// <returns>string of standard size unique for this way-point</returns>
        string getWPName(Point WPpt) { return (WPpt.X.ToString().PadLeft(intWPNameFieldLength, '0') + WPpt.Y.ToString().PadLeft(intWPNameFieldLength, '0')); }

        /// <summary>
        /// converts a unique Way-Point's name string into its original Point format
        /// </summary>
        /// <param name="WPName">unique way-point name string to be converted back into point format</param>
        /// <returns>way-point's game map location</returns>
        Point getWPFromName(string WPName)
        {
            try
            {
                return new Point(Convert.ToInt16(WPName.Substring(0, intWPNameFieldLength)),
                                 Convert.ToInt16(WPName.Substring(intWPNameFieldLength)));
            }
            catch (Exception) { return new Point(0, 0); }
        }

        /// <summary>
        /// given an array of way-points search for all neaghbouring WPs and store the shortest paths to each neaghbour
        /// </summary>
        public void rebuildNeaghboursArray()
        {
            for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
            {
                cLibHelper.lblFeedback.Text = "rebuilding neaghboursarray (" + intWPCounter.ToString() + "/" + WPs.Length.ToString() + ")";
                cLibHelper.lblFeedback.Top = 100;
                cLibHelper.lblFeedback.Left = Screen.PrimaryScreen.WorkingArea.Width - cLibHelper.lblFeedback.Width;
                cLibHelper.lblFeedback.Visible = true;
                cLibHelper.lblFeedback.BringToFront();
                cLibHelper.lblFeedback.Refresh();
                findNeaghbours(ref WPs[intWPCounter]);
            }
            cLibHelper.lblFeedback.Visible = false;
       }

        /// <summary>
        /// find the WPs nearest this Way-Point
        /// </summary>
        /// <param name="thisWP">reference way-point who's neaghbours are to be found</param>
        void findNeaghbours(ref classWayPoint thisWP)
        {
            udrPixQ.head = null;
            udrPixQ.tail = null;
            classWP_PixBFSElement thisEle = createPixBFS_Element(thisWP.pt,0, enuDir.N_);

            thisWP.neaghbours = new udtShortestPathToWayPoint[0];

            enQPixBFS(ref udrPixQ, ref thisEle);

            Size szArray = new Size(intMaxRangeSearchNeaghbour * 2 + 1, intMaxRangeSearchNeaghbour * 2 + 1);

            // since map size may be much greater than search limit
            // create a bolSeen array that covers only the search max limit and use ptBolSeenReference to find correct indices
            bolSeen = new bool[szArray.Width, szArray.Height];
            classWP_PixBFSElement[,] BFSArray = new classWP_PixBFSElement[szArray.Width, szArray.Height];

            // ptBolSeenReference is used to calculate correct indices and translate from pixel location on map
            // to indices of 2d array bolSeen
            ptBolSeenReference = new Point(thisEle.pt.X, thisEle.pt.Y);

            while (udrPixQ.head != null)
            {
                thisEle = deQPixBFS(ref udrPixQ);

                if (thisEle.pt != thisWP.pt
                    && !(thisEle.pt.X == 0 && thisEle.pt.Y == 0)
                    && bmpWPs.GetPixel(thisEle.pt.X, thisEle.pt.Y) == clrWP)
                {
                    Array.Resize<udtShortestPathToWayPoint>(ref thisWP.neaghbours, thisWP.neaghbours.Length + 1);                    
                    thisWP.neaghbours[thisWP.neaghbours.Length - 1].wp = BinTree_Search(thisEle.pt);
                    thisWP.neaghbours[thisWP.neaghbours.Length - 1].index = thisWP.neaghbours[thisWP.neaghbours.Length - 1].wp.index;
                }

                // for each direction insert create a new BFS element and insert into array
                if (thisEle.cost < intMaxRangeSearchNeaghbour)
                    for (enuDir dirCounter = 0; dirCounter < enuDir._numDir; dirCounter += 2)
                    {
                        Point pt = cLibHelper.movePixel(thisEle.pt, dirCounter);
                        Point ptArr = getBolSeenPoint(pt);

                        if (cLibHelper.isValidLocOnWPMap(pt) && !seen(ptArr))
                        {
                            setSeen(ptArr);
                            classWP_PixBFSElement eleNew = createPixBFS_Element(pt, thisEle.cost + 1, cLibHelper.getOppositeDirection(dirCounter));
                            BFSArray[ptArr.X, ptArr.Y] = eleNew;
                            enQPixBFS(ref udrPixQ, ref eleNew);
                        }
                    }
            }

            // now write the shortest paths to the way-points found and added to neaghbours array
            for (int intNeaghbourCounter = 0; intNeaghbourCounter < thisWP.neaghbours.Length; intNeaghbourCounter++)
            {
                Point pt = thisWP.neaghbours[intNeaghbourCounter].wp.pt;
                while (pt != thisWP.pt)
                {
                    Point ptArr = getBolSeenPoint(pt);
                    //if (bmpWPs.GetPixel(pt.X, pt.Y) != clrWP)
                    //    bmpWPs.SetPixel(pt.X, pt.Y, Color.Green);
                    enuDir dirBack = BFSArray[ptArr.X, ptArr.Y].dir;
                    enuDir dirForward = cLibHelper.getOppositeDirection(dirBack);
                    thisWP.neaghbours[intNeaghbourCounter].strPath = dirForward.ToString().Substring(0,1) + thisWP.neaghbours[intNeaghbourCounter].strPath;
                    pt = cLibHelper.movePixel(pt, dirBack);
                }
            }
        }

        string getEleString(classWP_PixBFSElement thisEle)
        {
            return "(" + thisEle.pt.X.ToString() + "," + thisEle.pt.Y.ToString() + ") "
                       + thisEle.dir.ToString() + ","
                       + thisEle.cost.ToString();
        }

        /// <summary>
        /// returns the Way-Point nearest to a specified location on the map
        /// </summary>
        /// <param name="ptStart">location on map where the nearest way-point is to be determined</param>
        /// <returns>returns a pointer to the way-point nearest to ptStart location</returns>
        public classWayPoint NearestWayPoint(Point ptStart)
        {
            if (nearestWPTable != null)
            {
                Point pt = new Point((int)Math.Floor((double)ptStart.X / intNearestWayPointGridSize),
                                      (int)Math.Floor((double)ptStart.Y / intNearestWayPointGridSize));
                if (nearestWPTable[pt.X, pt.Y] != null)
                    return nearestWPTable[pt.X, pt.Y];
                else
                {
                    int intBestDistance =(int) cLibHelper.cLibMath.distanceBetweenTwoPoints(ptStart, WPs[0].pt);
                    int intBestIndex = 0;
                    for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
                    {
                        int intDistance = (int)cLibHelper.cLibMath.distanceBetweenTwoPoints(ptStart, WPs[intWPCounter].pt);
                        if (intDistance < intBestDistance)
                        {
                            intBestDistance = intDistance;
                            intBestIndex = intWPCounter;
                        }
                    }
                    nearestWPTable[pt.X, pt.Y] = WPs[intBestIndex];
                    return nearestWPTable[pt.X, pt.Y];
                }
            }

            return null;
        }

        void storeSPWPEleInArray(ref classWP_PixBFSElement[] thisArray, ref classWP_PixBFSElement ele)
        {
            Array.Resize<classWP_PixBFSElement>(ref thisArray, thisArray.Length + 1);
            thisArray[thisArray.Length-1] = ele;
        }

        public udtShortestPathToWayPoint findNearestWayPoint(Point ptStart)
        {
            udrPixQ.head = null;
            udrPixQ.tail = null;

            classWP_PixBFSElement[] eleArray = new classWP_PixBFSElement[0];
            classWP_PixBFSElement thisEle = createPixBFS_Element(ptStart, 0, enuDir.N_);

            storeSPWPEleInArray(ref eleArray, ref thisEle);

            udtShortestPathToWayPoint retval = new udtShortestPathToWayPoint();

            enQPixBFS(ref udrPixQ, ref thisEle);
            Size szArray = new Size(intMaxRangeSearchNeaghbour * 2 + 1, intMaxRangeSearchNeaghbour * 2 + 1);

            // since map size may be much greater than search limit
            // create a bolSeen array that covers only the search max limit and use ptBolSeenReference to find correct indices
            bolSeen = new bool[szArray.Width, szArray.Height];
            classWP_PixBFSElement[,] BFSArray = new classWP_PixBFSElement[szArray.Width, szArray.Height];

            // ptBolSeenReference is used to calculate correct indices and translate from pixel location on map
            // to indices of 2d array bolSeen
            ptBolSeenReference = new Point(thisEle.pt.X, thisEle.pt.Y);

            while (udrPixQ.head != null)
            {
                thisEle = deQPixBFS(ref udrPixQ);

                if (bmpWPs.GetPixel(thisEle.pt.X, thisEle.pt.Y) == clrWP
                    && !(thisEle.pt.X == 0 && thisEle.pt.Y == 0))
                {
                    retval.wp = BinTree_Search(thisEle.pt);

                    retval.index = retval.wp.index;
                    udrPixQ.head = udrPixQ.tail = null;  // exit while loop
                }
                else
                    // for each direction insert create a new BFS element and insert into array
                    if (thisEle.cost < intMaxRangeSearchNeaghbour * 12)
                        for (enuDir dirCounter = 0; dirCounter < enuDir._numDir; dirCounter += 2)
                        {
                            Point pt = cLibHelper.movePixel(thisEle.pt, dirCounter);
                            Point ptArr = getBolSeenPoint(pt);

                            if (cLibHelper.isValidLocOnWPMap(pt)
                                && !seen(ptArr)
                                && cLibHelper.isValidLocOnWPMap(pt))
                            {
                                setSeen(ptArr);
                                classWP_PixBFSElement eleNew = createPixBFS_Element(pt, thisEle.cost + 1, cLibHelper.getOppositeDirection(dirCounter));
                                storeSPWPEleInArray(ref eleArray, ref eleNew);
                                BFSArray[ptArr.X, ptArr.Y] = eleNew;
                                enQPixBFS(ref udrPixQ, ref eleNew);
                            }
                        }
            }

            // now write the shortest paths to the way-points found and added to neaghbours array
            if (retval.wp == null) { return retval; }

            Point ptTemp = retval.wp.pt;
            while (ptTemp != ptStart)
            {
                Point ptArr = getBolSeenPoint(ptTemp);
                enuDir dirBack = BFSArray[ptArr.X, ptArr.Y].dir;
                enuDir dirForward = cLibHelper.getOppositeDirection(dirBack);
                retval.strPath = dirForward.ToString().Substring(0, 1) + retval.strPath;
                ptTemp = cLibHelper.movePixel(ptTemp, dirBack);
            }

            return retval;
        }

        /// <summary>
        /// randomly picks any way-point except the 0th one which is the robot's den entry
        /// </summary>
        /// <returns>a random way point</returns>
        public classWayPoint getRndWP()
        {
            int intRnd = 0;
            intRnd = cLibHelper.getRnd(WPs.Length - 1) + 1; // any WP except WP[0] because that is the back of the den
            return WPs[intRnd];
        }

        /// <summary>
        /// translates the map point to the bolseen 2d array indice equivalent
        /// </summary>
        /// <param name="pt">map game location</param>
        /// <returns>bolSeen indice that correspond to game-map location</returns>
        Point getBolSeenPoint(Point pt)
        {
            return new Point(intMaxRangeSearchNeaghbour - (ptBolSeenReference.X - pt.X),
                                       intMaxRangeSearchNeaghbour - (ptBolSeenReference.Y - pt.Y));
        }

        /// <summary>
        /// sets the bolSeen square array for the point on game map
        /// </summary>
        /// <param name="pt">point on game map PREVIOUSLY translated by getBolSeenPoint before call</param>
        void setSeen(Point ptTest)
        {
            if (ptTest.X >= 0 && ptTest.X < intMaxRangeSearchNeaghbour * 2 + 1
                && ptTest.Y >= 0 && ptTest.Y < intMaxRangeSearchNeaghbour * 2 + 1)
                bolSeen[ptTest.X, ptTest.Y] = true;
        }

        /// <summary>
        /// tests the bolSeen square array for the point on game map
        /// </summary>
        /// <param name="pt">point on game map PREVIOUSLY translated by getBolSeenPoint before call</param>
        /// <returns>true if that point has been seen, false if it hasn't</returns>
        bool seen(Point ptTest)
        {
            if (ptTest.X >= 0 && ptTest.X < intMaxRangeSearchNeaghbour * 2 + 1
                && ptTest.Y >= 0 && ptTest.Y < intMaxRangeSearchNeaghbour * 2 + 1)
                return bolSeen[ptTest.X, ptTest.Y];
            else
                return true;
        }
        
        /// <summary>
        /// creates a new Breadth-first-search pixel element from the point and direction inputs
        /// </summary>
        /// <param name="pt">point which this BFS element describes</param>
        /// <param name="dir">direction to travel back to source of search</param>
        /// <returns>returns a pixBFS element</returns>
        classWP_PixBFSElement createPixBFS_Element(Point pt, int cost, enuDir dir)
        {
            classWP_PixBFSElement retVal = new classWP_PixBFSElement();
            retVal.dir = dir;
            retVal.pt = pt;
            retVal.cost = cost;

            retVal.next = null;
            retVal.prev = null;

            return retVal;
        }

        /// <summary>
        /// enQ's a breadth-first-search pixel element into a referenced LIFO Q
        /// </summary>
        /// <param name="udrPixQ">reference to Q into which element is to be inserted</param>
        /// <param name="pixEle">reference to element which is to be inserted </param>
        void enQPixBFS(ref udtWP_BFSPixQ  udrPixQ, ref classWP_PixBFSElement pixEle)
        {
            if (udrPixQ.tail == null)
            {// first element
                udrPixQ.tail = udrPixQ.head = pixEle;
                pixEle.next = pixEle.prev = null;
                return;
            }
            else
            {
                udrPixQ.tail.next = pixEle;
                pixEle.prev = udrPixQ.tail;
                udrPixQ.tail = pixEle;
                pixEle.next = null;
            }
        }

        /// <summary>
        /// returns the first element in the referenced Q
        /// </summary>
        /// <param name="udrPixQ">Q from which element is to be pulled</param>
        /// <returns>returns a pointer to the first element and removes that element from the Q</returns>
        classWP_PixBFSElement deQPixBFS(ref udtWP_BFSPixQ udrPixQ)
        {
            classWP_PixBFSElement retval = udrPixQ.head;

            if (udrPixQ.head == udrPixQ.tail)
                udrPixQ.head = udrPixQ.tail = null;
            else if (udrPixQ.head != null)
                udrPixQ.head = udrPixQ.head.next;

            return retval;
        }

        public void buildWPTable()
        {
            nextWPTable = new int[WPs.Length, WPs.Length];

            for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
                 findPathToAllWPs(ref WPs[intWPCounter]);
        }

        void findPathToAllWPs(ref classWayPoint thisWP)
        {
            Q_WP_BFS = null;
            int intInfinity =(int) Math.Pow(2,16);
            classWP_BFSElement[] WPEleArr = new classWP_BFSElement[WPs.Length];
            for (int intWPCounter =0; intWPCounter<WPs.Length; intWPCounter++)
            {
                WPs[intWPCounter].bolSeen = false;

                WPEleArr[intWPCounter] = new classWP_BFSElement();
                WPEleArr[intWPCounter].cost = intInfinity ;
                WPEleArr[intWPCounter].WPcurrent = WPs[intWPCounter];
                WPEleArr[intWPCounter].WPfrom = null;
                WPEleArr[intWPCounter].next = WPEleArr[intWPCounter].prev = null;
            }

            classWP_BFSElement thisEle = WPEleArr[thisWP.index];
            thisEle.cost = 0;
            thisEle.WPcurrent = thisWP;
            thisEle.WPfrom = null;
            thisEle.next = thisEle.prev = null;
            thisWP.bolSeen = true;

            enQWP_BFS(ref Q_WP_BFS, ref thisEle);
          
            while (Q_WP_BFS != null)
            {
                thisEle = popQWP_BFS();

                for (int intNeaghbourCounter = 0; intNeaghbourCounter < thisEle.WPcurrent.neaghbours.Length; intNeaghbourCounter++)
                {
                    ///the array element's cost is initialized to 'infinity' so when we test it here
                    ///if the cost of reaching the neaghbour (value currently int he 'cost' field) is greater than
                    ///the cost to travel from the current wp to its neaghbour (cost to get here plus number of steps to get to the neaghbour)
                    ///then reset the travel cost to reach this neaghbour to the cost to reach this wp + the number of steps to reach neaghbour
                    ///   point that neaghbour back to the current wp
                    ///   and insert this neaghbour's element into the Q.
                    if (WPEleArr[thisEle.WPcurrent.neaghbours[intNeaghbourCounter].index].cost >
                        WPEleArr[thisEle.WPcurrent.index].cost + thisEle.WPcurrent.neaghbours[intNeaghbourCounter].strPath.Length)
                    {
                        WPEleArr[thisEle.WPcurrent.neaghbours[intNeaghbourCounter].index].cost =
                                WPEleArr[thisEle.WPcurrent.index].cost + thisEle.WPcurrent.neaghbours[intNeaghbourCounter].strPath.Length;
                        WPEleArr[thisEle.WPcurrent.neaghbours[intNeaghbourCounter].index].WPfrom = thisEle.WPcurrent;

                        deQWP_BFS(ref WPEleArr[thisEle.WPcurrent.neaghbours[intNeaghbourCounter].index]);
                        enQWP_BFS(ref WPEleArr[thisEle.WPcurrent.neaghbours[intNeaghbourCounter].index]);
                    }
                }
            }

            // now write paths back to start into table
            /// table column index is the CURRENT WP from which robot wants to travel
            /// table row index is the DESTINATION WP to which robot wants to travel
            /// this function has set up an array of classWP_BFSElement type which travel back to thisWP
            /// therefore thisWP's index is the DESTINATION WP (row)
            ///       and the rest are the column indices
            ///       e.g. nextWPTable[WPIndex_current, 
            ///                        WPIndex_destination]
            ///       
            /// since the breadth-first search algorithm has written a path from every other WP to the source
            /// write these into the table using thisWP's index as the 'destination'
            for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
                nextWPTable[intWPCounter, thisWP.index] = findIndexOfWPInNeaghbourArray(ref WPEleArr[intWPCounter].WPcurrent.neaghbours,
                                                                                        ref WPEleArr[intWPCounter].WPfrom);
        }

        int findIndexOfWPInNeaghbourArray(ref udtShortestPathToWayPoint[] neaghbourArray, ref classWayPoint WP)
        {
            for (int intNeaghbourCounter = 0; intNeaghbourCounter < neaghbourArray.Length; intNeaghbourCounter++)
                if (neaghbourArray[intNeaghbourCounter].wp == WP)
                    return intNeaghbourCounter;
            return -1;
        }

        /// <summary>
        /// creates and returns a WP_el Breadth-first search element
        /// </summary>
        /// <param name="pt">point location</param>
        /// <param name="cost">cost to reach this point</param>
        /// <returns></returns>
        classWP_BFSElement createWP_BFS_Element(ref classWayPoint WPCurrent, ref classWayPoint WPFrom, int cost)
        {
            classWP_BFSElement eleRetVal = new classWP_BFSElement();
            eleRetVal.cost = cost;
            eleRetVal.WPcurrent = WPCurrent;
            eleRetVal.WPfrom = WPFrom;
            return eleRetVal;
        }

        /// <summary>
        /// insert a new element into breadth-first search Q according to cost, least cost first, most cost last
        /// </summary>
        /// <param name="WP_BFS_ele">reference to element to be inserted</param>
        void enQWP_BFS(ref classWP_BFSElement WP_BFS_ele) { enQWP_BFS(ref Q_WP_BFS, ref WP_BFS_ele); }
        /// <summary>
        /// insert a new element into breadth-first search Q according to cost, least cost first, most cost last
        /// </summary>
        /// <param name="WP_BFS_Q">reference to Q into which element is to be inserted</param>
        /// <param name="WP_BFS_ele">reference to element to be inserted</param>
        void enQWP_BFS(ref classWP_BFSElement thisQ, ref classWP_BFSElement eleNew)
        {
            if (thisQ == eleNew)
                return;
            if (thisQ == null)
            {
                thisQ = eleNew;
                eleNew.next = null;
                eleNew.prev = null;
            }
            else
            {  // rank by movement cost -> cheapest at the front
                if (thisQ.cost > eleNew.cost)
                {
                    eleNew.next = thisQ;
                    thisQ.prev = eleNew;

                    eleNew.prev = null;
                    thisQ = eleNew;
                }
                else
                {
                    classWP_BFSElement thisElement = thisQ;

                    while (thisElement.next != null && thisElement.next.cost < eleNew.cost)
                        thisElement = thisElement.next;

                    eleNew.next = thisElement.next;
                    if (eleNew.next != null)
                        eleNew.next.prev = eleNew;

                    thisElement.next = eleNew;
                    eleNew.prev = thisElement;
                }
            }
        }

        /// <summary>
        /// removes a specific array element from Q_WP_BFS
        /// </summary>
        /// <param name="thisEle">element to be removed from Q</param>
        void deQWP_BFS(ref classWP_BFSElement thisEle) { deQWP_BFS(ref Q_WP_BFS, ref thisEle); }
        /// <summary>
        /// removes a specific array element from referenced Q
        /// </summary>
        /// <param name="thisQ">Q from which element is to be removed</param>
        /// <param name="thisEle">element to be removed from Q</param>
        void deQWP_BFS(ref classWP_BFSElement thisQ, ref classWP_BFSElement thisEle) 
        {
            if (thisQ == null)
                return;
            if (thisQ == thisEle)
            {
                thisQ = thisEle.next;
                if (thisQ != null)
                    thisQ.prev = null;
            }
            else
            {
                if (thisEle.prev != null)
                    thisEle.prev.next = thisEle.next;
                if (thisEle.next != null)
                    thisEle.next.prev = thisEle.prev;
            }

            thisEle.prev = thisEle.next = null;
        }

        /// <summary>
        /// deQ the first element off of the WP_el breadth-first search Q : Q_WP_BFS
        /// </summary>
        /// <returns>returns the first element off of Q_WP_BFS queue</returns>
        classWP_BFSElement popQWP_BFS() { return popQWP_BFS(ref Q_WP_BFS); }
        /// <summary>
        /// deQ the first element off of the WP_el breadth-first search Q : Q_WP_BFS
        /// <param name="thisQ">reference to the Q from which the returned element is deQ'd</param>
        /// <returns>returns the first element off of Q_WP_BFS queue</returns>
        classWP_BFSElement popQWP_BFS(ref classWP_BFSElement thisQ)
        {
            classWP_BFSElement eleRetVal = new classWP_BFSElement();
            eleRetVal = thisQ;
            thisQ = thisQ.next;
            if (thisQ != null)
                thisQ.prev = null;

            return eleRetVal;
        }

        #endregion

        /// <summary>
        /// builds a binary tree of all the way-points in the array using their lngName as search key
        /// </summary>
        public void buildBinTree()
        {
            WPBinTree = null;
            if (WPs.Length > 1)
            {
                // first create circular linked-list of elements in array
                for (int intWPCounter = 0; intWPCounter < WPs.Length; intWPCounter++)
                {
                    if (intWPCounter > 0 && intWPCounter < WPs.Length)
                    {
                        WPs[intWPCounter - 1].next = WPs[intWPCounter];
                        WPs[intWPCounter].prev = WPs[intWPCounter - 1];
                    }
                    else
                    {
                        WPs[WPs.Length - 1].next = WPs[0];
                        WPs[0].prev = WPs[WPs.Length - 1];
                    }
                }

                /// now cycle through all the elements entering them into
                /// binary tree and removing them from the circular linked list
                /// until there are no more elements remaining in circular linked list
                /// 
                classWayPoint WPinsert = WPs[0];
                int intNumSkip;
                int intMaxNumSkip = (int)(WPs.Length * .4) + 1;

                while (WPinsert != null)
                {
                    intNumSkip = cLibHelper.getRnd(intMaxNumSkip) +1;
                    for (int intSkipCounter = 0; intSkipCounter < intNumSkip; intSkipCounter++)
                        WPinsert = WPinsert.next;

                    BinTree_Insert(ref WPinsert);
                }
            }
            else
                WPBinTree = createBinTreeElement(ref WPs[0]);
        }

        /// <summary>
        /// searches the Way-Point binary tree for a Way Point given the location on the game map
        /// </summary>
        /// <param name="pt">way-point's location on the game map</param>
        /// <returns>reference to way point if found, null if not found</returns>
        classWayPoint BinTree_Search(Point pt)
        {
            string strWPName = getWPName(pt);
            return BinTree_Search(strWPName);
        }

        /// <summary>
        /// searches the Way-Point binary tree for a Way Point by unique string name
        /// </summary>
        /// <param name="strWPName">unique string name</param>
        /// <returns>reference to way point if found, null if not found</returns>
        classWayPoint BinTree_Search(string strWPName)
        {
            long lngName = Convert.ToInt64(strWPName);
            return BinTree_Search(lngName);
        }

        /// <summary>
        /// searches the Way-Point binary tree for a Way Point by unique long id name
        /// </summary>
        /// <param name="lngName">unique long id name</param>
        /// <returns>reference to way point if found, null if not found</returns>
        classWayPoint BinTree_Search(long lngName)
        {
            classWP_BinTreeElement eleThis = WPBinTree;
            while (true)
            {
                if (lngName > eleThis.WP.lngName)
                { // go to right 
                    if (eleThis.right == null)
                    {
                        return null;
                    }
                    else
                        eleThis = eleThis.right;
                }
                else if (lngName < eleThis.WP.lngName)
                { // go to left
                    if (eleThis.left == null)
                    {
                        return null;
                    }
                    else
                        eleThis = eleThis.left;
                }
                else
                {
                    return eleThis.WP;
                }
            }
        }

        void BinTree_Insert(ref classWayPoint thisWP)
        {
            if (WPBinTree == null)
            {
                WPBinTree = createBinTreeElement(ref thisWP);
                goto exit_WPBinTree_Insert;
            }
            else
            {
                classWP_BinTreeElement eleNew = createBinTreeElement(ref thisWP);
                classWP_BinTreeElement eleThis = WPBinTree;

                while (true)
                {
                    if (eleNew.WP.lngName > eleThis.WP.lngName)
                    { // go to right 
                        if (eleThis.right == null)
                        {
                            eleThis.right = eleNew;
                            goto exit_WPBinTree_Insert;
                        }
                        else
                            eleThis = eleThis.right;
                    }
                    else if (eleNew.WP.lngName < eleThis.WP.lngName)
                    { // go to left
                        if (eleThis.left == null)
                        {
                            eleThis.left = eleNew;
                            goto exit_WPBinTree_Insert;
                        }
                        else
                            eleThis = eleThis.left;
                    }
                    else
                    {// this should not happen
                        MessageBox.Show("this should not happen");
                    }
                }
            }

        exit_WPBinTree_Insert:
            // remove thisWP from linked list
            if (thisWP == thisWP.next)
            {
                thisWP.next = null;
                thisWP.prev = null;
                thisWP = null;
            }
            else
            {
                thisWP.next.prev = thisWP.prev;
                thisWP.prev.next = thisWP.next;

                classWayPoint WPNext = thisWP.next;
                thisWP.next = null;
                thisWP.prev = null;

                thisWP = WPNext;
            }
        }

        classWP_BinTreeElement createBinTreeElement(ref classWayPoint thisWP)
        {
            classWP_BinTreeElement binRetVal = new classWP_BinTreeElement();
            binRetVal.left = null;
            binRetVal.right = null;
            binRetVal.WP = thisWP;
            return binRetVal;
        }
    }

    public class classWP_BFSElement
    {
        public classWayPoint WPcurrent;
        public classWayPoint WPfrom;

        public int cost;

        public classWP_BFSElement next;
        public classWP_BFSElement prev;
    }

    /// <summary>
    /// elements used in a Q for pixel breadth-first search when finding nearest neaghbours to each way-point
    /// </summary>
    public class classWP_PixBFSElement
    {
        public Point pt;
        public enuDir dir;
        public int cost;
        public classWP_PixBFSElement next;
        public classWP_PixBFSElement prev;
    }

    /// <summary>
    /// location on map and an array of elements identifying neaghbouring way-points along with shortest-paths to reach them
    /// </summary>
    public class classWayPoint
    {
        public Point pt;
        public string strName;
        public long lngName;

        public classWayPoint next;
        public classWayPoint prev;

        public int index;
        public bool bolSeen;

        public udtShortestPathToWayPoint[] neaghbours;
    }

    public class classWP_BinTreeElement
    {
        public classWP_BinTreeElement left;
        public classWP_BinTreeElement right;

        public classWayPoint WP;
    }
}