using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace Serial
{
    public partial class SerialMap : Form
    {
        int robotx, roboty, robotDir, goalx, goaly, travelDir, solved;

        //------------------LOCKING/UNLOCKING A BITMAP TO MEMORY FOR FASTER HANDLING------------------
        //----                 Credit to Vano Maisuradze for Publishing this code                  ----
        //----     http://www.codeproject.com/Tips/240428/Work-with-bitmap-faster-with-Csharp      ----
        //---------------------------------------------------------------------------------------------
        public class LockBitmap
        {
            Bitmap source = null;
            IntPtr Iptr = IntPtr.Zero;
            BitmapData bitmapData = null;

            public byte[] Pixels { get; set; }
            public int Depth { get; private set; }
            public int Width { get; private set; }
            public int Height { get; private set; }

            public LockBitmap(Bitmap source)
            {
                this.source = source;
            }

            // LOCK BITMAP DATA
            public void LockBits()
            {
                try
                {
                    // Get width and height of bitmap
                    Width = source.Width;
                    Height = source.Height;
                    
                    // get total locked pixels count
                    int PixelCount = Width * Height;

                    // Create rectangle to lock
                    Rectangle rect = new Rectangle(0, 0, Width, Height);

                    // get source bitmap pixel format size
                    Depth = System.Drawing.Bitmap.GetPixelFormatSize(source.PixelFormat);

                    // Check if bpp (Bits Per Pixel) is 8, 24, or 32
                    if (Depth != 8 && Depth != 24 && Depth != 32)
                    {
                        throw new ArgumentException("Only 8, 24 and 32 bpp images are supported.");
                    }

                    // Lock bitmap and return bitmap data
                    bitmapData = source.LockBits(rect, ImageLockMode.ReadWrite,
                                                 source.PixelFormat);

                    // create byte array to copy pixel values
                    int step = Depth / 8;
                    Pixels = new byte[PixelCount * step];
                    Iptr = bitmapData.Scan0;

                    // Copy data from pointer to array
                    System.Runtime.InteropServices.Marshal.Copy(Iptr, Pixels, 0, Pixels.Length);
                }
                catch (Exception ex)
                {
                    throw ex;
                }
            }

            // UNLOCK BITMAP DATA
            public void UnlockBits()
            {
                try
                {
                    // Copy data from byte array to pointer
                    System.Runtime.InteropServices.Marshal.Copy(Pixels, 0, Iptr, Pixels.Length);

                    // Unlock bitmap data
                    source.UnlockBits(bitmapData);
                }
                catch (Exception ex)
                {
                    throw ex;
                }
            }

            // GET PIXEL COLOUR @ X, Y
            public Color GetPixel(int x, int y)
            {
                Color clr = Color.Empty;

                // Get color components count
                int cCount = Depth / 8;

                // Get start index of the specified pixel
                int i = ((y * Width) + x) * cCount;

                if (i > Pixels.Length - cCount)
                    throw new IndexOutOfRangeException();

                if (Depth == 32) // For 32 bpp get Red, Green, Blue and Alpha
                {
                    byte b = Pixels[i];
                    byte g = Pixels[i + 1];
                    byte r = Pixels[i + 2];
                    byte a = Pixels[i + 3]; // a
                    clr = Color.FromArgb(a, r, g, b);
                }
                if (Depth == 24) // For 24 bpp get Red, Green and Blue
                {
                    byte b = Pixels[i];
                    byte g = Pixels[i + 1];
                    byte r = Pixels[i + 2];
                    clr = Color.FromArgb(r, g, b);
                }
                if (Depth == 8)
                // For 8 bpp get color value (Red, Green and Blue values are the same)
                {
                    byte c = Pixels[i];
                    clr = Color.FromArgb(c, c, c);
                }
                return clr;
            }

            /// SET PIXEL COLOUR @ X, Y
            public void SetPixel(int x, int y, Color color)
            {
                // Get color components count
                int cCount = Depth / 8;

                // Get start index of the specified pixel
                int i = ((y * Width) + x) * cCount;

                if (Depth == 32) // For 32 bpp set Red, Green, Blue and Alpha
                {
                    Pixels[i] = color.B;
                    Pixels[i + 1] = color.G;
                    Pixels[i + 2] = color.R;
                    Pixels[i + 3] = color.A;
                }
                if (Depth == 24) // For 24 bpp set Red, Green and Blue
                {
                    Pixels[i] = color.B;
                    Pixels[i + 1] = color.G;
                    Pixels[i + 2] = color.R;
                }
                if (Depth == 8)
                // For 8 bpp set color value (Red, Green and Blue values are the same)
                {
                    Pixels[i] = color.B;
                }
            }
        }
        //--------------------------END OF LOCK/UNLOCK--------------------------

  
        public SerialMap()
        {
            InitializeComponent();
            pictureBox1.Image = new Bitmap(210, 210); //, PixelFormat.Format24bppRgb  <<LIMIT TO 24BIT IF TIME SAVING NEEDED
        }
 

        //--------------------MAPPING THE VALUES FROM A SENSOR--------------------------
        //--             Uses the servo angle and the distance reading                --
        //--          the Math... Sin0 * Hypotenuse length = Opposite Length          --
        //--         Calculated on both angles to produce a X, Y Co-ordinate          --
        //--              The X, Y is then added to the robot Location                --
        //------------------------------------------------------------------------------
        private void mapBtn_Click(object sender, EventArgs e)
        {
            Double xAngleDeg = Convert.ToDouble(angleText.Text);
            Double xAngleRad = Math.PI / 180;  //Works out RADIANS as C# Uses them
            int distance = Convert.ToInt32(distText.Text);
            if (xAngleDeg == 90)  //If Angle is Straight ahead only uses the Distance
            {
                switch (robotDir)   //Adds Object(black pixel) based on Robots Direction and Location
                {
                    case 1:  //NORTH
                        draw(robotx, roboty - distance);
                        break;
                    case 2:  //EAST
                        draw(robotx + distance, roboty);
                        break;
                    case 3:  //SOUTH
                        draw(robotx, roboty + distance);
                        break;
                    case 4:  //WEST
                        draw(robotx - distance, roboty);
                        break;
                }  //no default as it'll always be one of these values
            }
            else
            {          
                if (xAngleDeg < 90)  //If Angle is under 90degress it subtracts the Sin0 result from the robots Location
                {                                //(to the Left of whatever direction the robot is facing)
                    xAngleDeg = 90 - xAngleDeg;
                    switch (robotDir)
                    {
                        case 1:  //NORTH
                            draw(robotx - Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance), roboty - Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance));
                            break;
                        case 2:  //EAST
                            draw(robotx + Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance), roboty - Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance));
                           break;
                        case 3:  //SOUTH
                            draw(robotx + Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance), roboty + Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance));
                            break;
                        case 4:  //WEST
                            draw(robotx - Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance), roboty + Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance));
                            break;
                    }  //no default as it'll always be one of these values
                }
                else  //If Angle is over 90degress it Adds the Sin0 result to the robots location
                {                    //(to the Right of whatever direction the robot is facing)
                    xAngleDeg = xAngleDeg - 90;
                    switch (robotDir)
                    {
                        case 1:  //NORTH
                            draw(robotx + Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance), roboty - Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance));
                            break;
                        case 2:  //EAST
                            draw(robotx + Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance), roboty + Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance));
                             break;
                        case 3:  //SOUTH
                            draw(robotx - Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance), roboty + Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance));
                            break;
                        case 4:  //WEST
                            draw(robotx - Convert.ToInt32(Math.Sin((90 - xAngleDeg) * xAngleRad) * distance), roboty - Convert.ToInt32(Math.Sin(xAngleDeg * xAngleRad) * distance));
                            break;
                    }  //no default as it'll always be one of these values
                }
            }
        }
        //--------------------------------END OF MAPPING--------------------------------


        //-----------------------------DRAWING ON THE MAP-----------------------------------
        //DRAWS A BLACK PIXEL @ xPos,yPos INDICATING AN OBJECT/WALL
        void draw(int xPos, int yPos)
        {   //Not Locking bitmap to memory as data from Robot will take longer than this function.
            ((Bitmap)pictureBox1.Image).SetPixel(xPos, yPos, System.Drawing.Color.Black);
            pictureBox1.Refresh();
        }
        //ADDS A (Red) GOAL TO THE MAP USING X AND Y
        private void addGoalBtn_Click(object sender, EventArgs e)
        {   //Not Locking bitmap to memory as data from Robot will take longer than this function.
            if (goalPosGrp.Visible == false)   //Check if there is already a goal
            {
                ((Bitmap)pictureBox1.Image).SetPixel(Convert.ToInt32(angleText.Text), Convert.ToInt32(distText.Text), System.Drawing.Color.Red);
                pictureBox1.Refresh();
                goalPosGrp.Visible = true;
                goalx = Convert.ToInt32(angleText.Text);   //Update Goals X,Y
                goaly = Convert.ToInt32(distText.Text);
                xPosLbl.Text = angleText.Text;
                yPosLbl.Text = distText.Text;
            }
        }
        //ADDS A (Blue) ROBOT TO THE MAP USING X AND Y
        private void addBotBtn_Click(object sender, EventArgs e)
        {   //Not Locking bitmap to memory as data from Robot will take longer than this function.
            if (botConGrp.Visible == false)    //Checks to see if there is already a Robot
            {
                ((Bitmap)pictureBox1.Image).SetPixel(Convert.ToInt32(angleText.Text), Convert.ToInt32(distText.Text), System.Drawing.Color.Blue);
                pictureBox1.Refresh();
                botXLbl.Text = angleText.Text;   //Update Robot X,Y,Dir
                botYLbl.Text = distText.Text;
                robotx = Convert.ToInt32(angleText.Text);
                roboty = Convert.ToInt32(distText.Text);
                robotDir = 1;
                botConGrp.Visible = true;        //Shows Robots Manual Controls
            }
        }
        //------------------------------END OF DRAWING-----------------------------


        //---------------------------FILL IN THE WAVEFRONT MOVING OUTWARD FROM THE GOAL---------------------------------
        //----             This code is derived from the Wavefront Tutorial on Society of Robots                    ----
        //----                   http://www.societyofrobots.com/programming_wavefront.shtml                         ----
        //--------------------------------------------------------------------------------------------------------------
        //----                   I decrease the Red Value of every pixel out from the goal                          ----
        //--------------------------------------------------------------------------------------------------------------
        private void waveBtn_Click(object sender, EventArgs e)
        {
            int minR_Val;
            int reset_minR_Val = 2;  //Lowest red value to go to. 0 = Black(used for Objects)
            //LOCK BITMAP INTO MEMORY
            Bitmap bmp = (Bitmap)pictureBox1.Image;
            LockBitmap lockBitmap = new LockBitmap(bmp);
            lockBitmap.LockBits();
            //SEARCH, MODIFY BITMAP IN MEMORY BELOW
            for (int i = 0; i < 100; i++)   //Scroll through find solution "i" times, IF IT LEAVES WHITE WITHOUT SOLVING INCREASE THIS
            {
                for (int y = 0; y < lockBitmap.Height; y++)   //Scroll down the bitmap
                {
                    for (int x = 0; x < lockBitmap.Width; x++)   //Scroll across each line in the bitmap
                    {

                        //if Location is not a wall (Black) or the Goal (Red)
                        if (lockBitmap.GetPixel(x, y).Name != "ff000000" && lockBitmap.GetPixel(x, y).Name != "ffff0000")        
                        {

                            minR_Val = reset_minR_Val;  //Clear data from last round
                            //Right   ***************************************************************
                            if (x < lockBitmap.Width - 1)//not out of boundary
                                if (lockBitmap.GetPixel(x + 1, y).R > minR_Val && lockBitmap.GetPixel(x + 1, y).Name != "ff000000")
                                {   //IF PIXEL TO THE RIGHTS RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                                    minR_Val = lockBitmap.GetPixel(x + 1, y).R;
                                }
                            //Left   ****************************************************************
                            if (x > 0)
                                if (lockBitmap.GetPixel(x - 1, y).R > minR_Val && lockBitmap.GetPixel(x - 1, y).Name != "ff000000")
                                {   //IF PIXEL TO THE LEFTS RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                                    minR_Val = lockBitmap.GetPixel(x - 1, y).R;
                                }
                            //Down, REMEMBER PIXELS COUNT FROM TOP LEFT   ***************************
                            if (y < lockBitmap.Height - 1)
                                if (lockBitmap.GetPixel(x, y + 1).R > minR_Val && lockBitmap.GetPixel(x, y + 1).Name != "ff000000")
                                {   //IF PIXEL TO BELOWS RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                                    minR_Val = lockBitmap.GetPixel(x, y + 1).R;
                                }
                            //Up  *******************************************************************
                            if (y > 0)
                                if (lockBitmap.GetPixel(x, y - 1).R > minR_Val && lockBitmap.GetPixel(x, y - 1).Name != "ff000000")
                                {   //IF PIXEL ABOVES RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                                    minR_Val = lockBitmap.GetPixel(x, y - 1).R;
                                }

                            //If Red Value > reset_minR_Val && Location is a Robot. SOLVED & EXIT
                            if (minR_Val > reset_minR_Val && lockBitmap.GetPixel(x, y).Name == "ff0000ff")
                            {
                                //L10.Text = Convert.ToString(min_node_location);
                                lockBitmap.UnlockBits();
                                pictureBox1.Refresh();
                                return;
                            }
                            else if (minR_Val != reset_minR_Val)  //If Red Value doesn't equal the reset value paint current pixel with the min red value found - 1
                            {

                                lockBitmap.SetPixel(x, y, Color.FromArgb(255, (minR_Val - 1), 0, 0)); //Paint using ARGB
                            }
                        }
                    }
                }
            }
            //ff000000 = Black, ffff0000 = Red, ff008000 = Green, ff0000ff = Blue
            //UNLOCK BITMAP
            lockBitmap.UnlockBits();
            pictureBox1.Refresh();
        }
        //---------------------------------------END OF WAVERFRONT FILL--------------------------------------------


        //------------------------SOLVE USING SHORTEST ROUTE TO GOAL, ONE STEP AT A TIME------------------------------
        private void solveBtn_Click(object sender, EventArgs e)
        {
            
            //LOCK BITMAP INTO MEMORY
            Bitmap bmp = (Bitmap)pictureBox1.Image;
            LockBitmap lockBitmap = new LockBitmap(bmp);
            lockBitmap.LockBits();
            //SEARCH, MODIFY BITMAP IN MEMORY BELOW

            //1. LOOK FOR HIGHEST RED VALUE AROUND ROBOT (Highest Red Value on map is GOAL)
            travelDir = 0;  //Which Direction the Lowest Value is. North = 1, EAST = 2, SOUTH = 3, WEST = 4, SOLVED = 0

            if (solved != 1)  //Check to see if Robot has already made it to the highest Red Value pixel
            {
                int minR_Val = 2;
                //Up  *******************************************************************
                if (roboty > 0)   //As long as Robot is not on the edge of the map
                {
                    if (lockBitmap.GetPixel(robotx, roboty - 1).R > minR_Val && lockBitmap.GetPixel(robotx, roboty - 1).Name != "ff000000")
                    {   //IF PIXEL ABOVES RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                        minR_Val = lockBitmap.GetPixel(robotx, roboty - 1).R;
                        travelDir = 1;
                    }
                }
                //LOOK RIGHT     ********************************************************
                if (robotx < lockBitmap.Width - 1)  //As long as Robot is not on the edge of the map
                {
                    if (lockBitmap.GetPixel(robotx + 1, roboty).R > minR_Val && lockBitmap.GetPixel(robotx + 1, roboty).Name != "ff000000")//find the lowest number node, and exclude empty nodes (0's)
                    {   //IF PIXEL TO THE RIGHTS RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                        minR_Val = lockBitmap.GetPixel(robotx + 1, roboty).R;
                        travelDir = 2;
                    }
                }
                //Down, REMEMBER PIXELS COUNT FROM TOP LEFT   ***************************
                if (roboty < lockBitmap.Height - 1)   //As long as Robot is not on the edge of the map
                {
                    if (lockBitmap.GetPixel(robotx, roboty + 1).R > minR_Val && lockBitmap.GetPixel(robotx, roboty + 1).Name != "ff000000")
                    {   //IF PIXEL BELOWS RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                        minR_Val = lockBitmap.GetPixel(robotx, roboty + 1).R;
                        travelDir = 3;
                    }
                }
                //Left   ****************************************************************
                if (robotx > 0)   //As long as Robot is not on the edge of the map
                {
                    if (lockBitmap.GetPixel(robotx - 1, roboty).R > minR_Val && lockBitmap.GetPixel(robotx - 1, roboty).Name != "ff000000")
                    {   //IF PIXEL TO THE LEFTS RED VALUE IS GREATER THAN minR_Val & IS NOT BLACK
                        minR_Val = lockBitmap.GetPixel(robotx - 1, roboty).R;
                        travelDir = 4;
                    }
                }
                if (minR_Val == 255)   //If the next Move is the Highest Red Value, It's Solved (after this move)
                {
                    solved = 1;
                }
            }
            //UNLOCK BITMAP
            lockBitmap.UnlockBits();


            //2. TRAVEL THAT DIRECTION
                //see what direction robot is facing compared to travel direction
                //turn robot the right way
                //drive forward, update robot location on map
            switch (travelDir)
            {
                case 1:  //NORTH
                    moveNorth();
                    botDirLbl.Text = "N";
                    robotDir = 1;
                    break;
                case 2:   //EAST
                    moveEast();
                    botDirLbl.Text = "E";
                    robotDir = 2;
                    break;
                case 3:   //SOUTH
                    moveSouth();
                    botDirLbl.Text = "S";
                    robotDir = 3;
                    break;
                case 4:   //WEST
                    moveWest();
                    botDirLbl.Text = "W";
                    robotDir = 4;
                    break;
                default:
                    L11.Text = "SOLVED";
                    break;
            }
        }
        //-----------------------------------END OF SOLVE-------------------------------------


        //-------------------------------ROBOT ROTATION-----------------------------
        private void turnBotRBtn_Click(object sender, EventArgs e)
        {   //TURN ROBOT RIGHT
            switch (robotDir)
            {
                case 1:  //NORTH
                    botDirLbl.Text = "E";
                    robotDir = 2;
                    break;
                case 2:  //EAST
                    botDirLbl.Text = "S";
                    robotDir = 3;
                    break;
                case 3:  //SOUTH
                    botDirLbl.Text = "W";
                    robotDir = 4;
                    break;
                case 4:  //WEST
                    botDirLbl.Text = "N";
                    robotDir = 1;
                    break;
            }  //no default as it'll always be one of these values
        }
        private void turnBotLBtn_Click(object sender, EventArgs e)
        {   //TURN ROBOT LEFT
            switch (robotDir)
            {
                case 1:
                    botDirLbl.Text = "W";
                    robotDir = 4;
                    break;
                case 2:
                    botDirLbl.Text = "N";
                    robotDir = 1;
                    break;
                case 3:
                    botDirLbl.Text = "E";
                    robotDir = 2;
                    break;
                case 4:
                    botDirLbl.Text = "S";
                    robotDir = 3;
                    break;
            }
        }
        //-----------------------------END OF ROBOT ROTATION--------------------------------


        //--------------------------------------ROBOT MOVEMENTS-------------------------------------------
        void moveNorth()
        {
            ((Bitmap)pictureBox1.Image).SetPixel(robotx, roboty - 1, System.Drawing.Color.Blue);  //draw new robot pos (robot colour)
            ((Bitmap)pictureBox1.Image).SetPixel(robotx, roboty, System.Drawing.Color.FromArgb(0, 0, 0, 0));  //delete old robot pos (white)
            roboty = roboty - 1;      //update robot location
            pictureBox1.Refresh();
            botYLbl.Text = Convert.ToString(roboty);     //update Label
        }
        void moveEast()
        {
            ((Bitmap)pictureBox1.Image).SetPixel(robotx + 1, roboty, System.Drawing.Color.Blue);
            ((Bitmap)pictureBox1.Image).SetPixel(robotx, roboty, System.Drawing.Color.FromArgb(0, 0, 0, 0));
            robotx = robotx + 1;
            pictureBox1.Refresh();
            botXLbl.Text = Convert.ToString(robotx);
        }
        void moveSouth()
        {
            ((Bitmap)pictureBox1.Image).SetPixel(robotx, roboty + 1, System.Drawing.Color.Blue);
            ((Bitmap)pictureBox1.Image).SetPixel(robotx, roboty, System.Drawing.Color.FromArgb(0, 0, 0, 0));
            roboty = roboty + 1;
            pictureBox1.Refresh();
            botYLbl.Text = Convert.ToString(roboty);
        }
        void moveWest()
        {
            ((Bitmap)pictureBox1.Image).SetPixel(robotx - 1, roboty, System.Drawing.Color.Blue);
            ((Bitmap)pictureBox1.Image).SetPixel(robotx, roboty, System.Drawing.Color.FromArgb(0, 0, 0, 0));
            robotx = robotx - 1;
            pictureBox1.Refresh();
            botXLbl.Text = Convert.ToString(robotx);
        }
        //-------------------------------------END OF MOVEMENTS-------------------------------------------


        //-----------------------DRIVING ROBOT MANUALLY----------------------------
        private void moveBotFBtn_Click(object sender, EventArgs e)
        {
            switch (robotDir) //Get robot direction and drive 1 block forwards
            {
                case 1:  //NORTH
                    if (roboty > 0)
                    {
                        moveNorth();
                    }
                    break;
                case 2:   //EAST
                    if (robotx < pictureBox1.Width - 1)    //Same as lockBitmap.Width, but thats not a global var
                    {
                        moveEast();
                    }
                    break;
                case 3:   //SOUTH
                    if (roboty < pictureBox1.Height - 1)    //Same as lockBitmap.Height, but thats not a global var
                    {
                        moveSouth();
                    }
                    break;
                case 4:   //WEST
                    if (robotx > 0)
                    {
                        moveWest();
                    }
                    break;
                //no default as it'll always be one of these values
            }
        }
        //------------------END OF DRIVING ROBOT MANUALLY-----------------------


        //---------------------------SAVE & LOAD MAPS------------------------------
        private void saveMapBtn_Click(object sender, EventArgs e)
        {
            //Displays a SaveFileDialog so the user can save the Map Image
            SaveFileDialog saveFileDialog1 = new SaveFileDialog();
            saveFileDialog1.Filter = "Png Image|*.Png";
            saveFileDialog1.Title = "Save a Map Image File";
            saveFileDialog1.ShowDialog();
            // If the file name is not an empty string open it for saving.
            if (saveFileDialog1.FileName != "")
            {
                // Saves the Image via a FileStream created by the OpenFile method.
                System.IO.FileStream fs = (System.IO.FileStream)saveFileDialog1.OpenFile();
                // Saves the Image as a Png ImageFormat
                this.pictureBox1.Image.Save(fs, System.Drawing.Imaging.ImageFormat.Png);            
                fs.Close();
            }
        }
        private void openMapBtn_Click(object sender, EventArgs e)
        {
            //Displays a OpenFileDialog so the user can open a Map Image
            using (OpenFileDialog openFileDialog1 = new OpenFileDialog())
            {
                openFileDialog1.Title = "Open a Map Image";
                openFileDialog1.Filter = "Png files (*.Png)|*.Png";
                if (openFileDialog1.ShowDialog() == DialogResult.OK)
                {
                    pictureBox1.Image = new Bitmap(openFileDialog1.FileName);
                }
            }

            //LOCK BITMAP INTO MEMORY
            Bitmap bmp = (Bitmap)pictureBox1.Image;
            LockBitmap lockBitmap = new LockBitmap(bmp);
            lockBitmap.LockBits();
            //SEARCH, MODIFY BITMAP IN MEMORY BELOW
            for (int y = 0; y < lockBitmap.Height; y++)
            {
                for (int x = 0; x < lockBitmap.Width; x++)
                {

                    if (lockBitmap.GetPixel(x, y).B == 255)  //LOOK FOR ROBOT
                    {
                        botXLbl.Text = Convert.ToString(x);   //Update Robot X,Y,Dir
                        botYLbl.Text = Convert.ToString(y);
                        robotx = x;
                        roboty = y;
                        robotDir = 1;
                        botDirLbl.Text = "N";
                        botConGrp.Visible = true;        //Shows Robots Manual Controls
                    }
                    if (lockBitmap.GetPixel(x, y).R == 255)  //LOOK FOR GOAL
                    {
                        goalPosGrp.Visible = true;
                        xPosLbl.Text = Convert.ToString(x);
                        yPosLbl.Text = Convert.ToString(y);
                    }
                }
            }
            //UNLOCK BITMAP
            lockBitmap.UnlockBits();     
        }
        //-------------------------END OF SAVE & LOAD MAPS----------------------------
    }
}