using System;
using System.IO;
using System.IO.Pipes;
using System.Collections;
using System.Windows;
using System.Linq;
using System.Text;
using Mobile_Robot;
using System.Windows.Threading;
using System.Threading;

namespace Mobile_Robot_Controller
{
    public enum OccupancyMapStatus { UNOCCUPIED, VISITED, OCCUPIED };

    public delegate void occupancyMapCellChangeDelegate(int x, int y, int newValue);    // Called when occupancy map is updated

    public delegate void obstacleDetectedDelegate(Position scannedObstaclePosition);    // Called when an obstacle is detected by  the laser scanning

    public class Controller
    {
        private Robot robot;

        private Laser laser;

        private OccupancyMap occupancyMap;

        private Random randomNumberGenerator;       // For non-deterministic path searching

        private Comms commsReader;

        private Comms commsWriter;

        public Controller(Robot r, int occupancyMapCellWidth, int occupancyMapCellHeight, int regionWidth, int  regionHeight)
        {
            robot = r;

            laser = (Laser)robot.Sensors[0];

            occupancyMap = new OccupancyMap(occupancyMapCellWidth, occupancyMapCellHeight, regionWidth, regionHeight);

            // Initialize the within critical range event handle 
            laser.ObstacleWithinCriticalRangeEvent += new obstacleWithinCriticalRangeDelegate(obstacleWithinCriticalRangeHandler);

            // Initialize the motion request complete event handler
            robot.MotionRequestCompletedEvent += new motionRequestCompletedDelegate(motionRequestCompletedHandler);

            // Initialize the direction change event handler
            robot.DirectionChangeEvent += new directionChangeDelegate(directionChangeHandler);

            // Initialize the position change event handler
            robot.PositionChangeEvent += new positionChangeDelegate(positionChangeHandler);

            // Initialize the rotation complete event handler
            robot.RotationCompletedEvent += new rotationCompletedDelegate(rotationCompletedHandler);

            // Initialize the random number  generator
            randomNumberGenerator = new Random();

            // Initialize the comms object for reading the pipe
            commsReader = new Comms("Controller Reader", 'r');

            // Initialize obstacle detected event
            commsReader.ObstacleDetectedEvent += new obstacleDetectedDelegate(obstacleDetectedHandler);

            // Initialize the comms object for writing the pipe
            commsWriter = new Comms("Controller Writer", 'w');

          
        }

        public OccupancyMap OccupancyMap
        {
            get
            {
                return occupancyMap;
            }
        }

        private void obstacleDetectedHandler(Position scannedObstaclePosition)
        {
            updateOccupancyMap(scannedObstaclePosition);
        }
        

        private void positionChangeHandler(Position pos)
        {
            if (laser.ScanOn)
            {
                Position tipPos = laser.ScanTipPosition;

                Double x = tipPos.X;
                Double y = tipPos.Y;

                int mapX = (int)(x / occupancyMap.CellWidth);
                int mapY = (int)(y / occupancyMap.CellHeight);

                int mapVal=occupancyMap.getMap(mapX,mapY);

                if (mapVal != (int) OccupancyMapStatus.OCCUPIED)
                    occupancyMap.setMap(mapX, mapY, (int)OccupancyMapStatus.VISITED);
            }
        }

        private void rotationCompletedHandler()
        {
            // Clear visited  cells
            occupancyMap.refreshMap();
        }

        private void obstacleWithinCriticalRangeHandler(Position pos)
        {
           // Turn off critical distance detection and stop robot
            robot.CriticalDistanceDetection = false;
            robot.stop();
            robot.Stopped = true;
        }

        private void motionRequestCompletedHandler()
        {
            if (robot.Stopped)
            {
                // Toss a coin to determine the rotation direction
                double rand = randomNumberGenerator.NextDouble();
                if (rand > 0.5)
                    robot.startRotate(RotationDirection.CLOCKWISE);            // Perform search for clear route
                else
                    robot.startRotate(RotationDirection.ANTICLOCKWISE);

                robot.Stopped = false;
            }
            else
            {
                robot.CriticalDistanceDetection = true;
                laser.CurrentScanAngle = 0.0;
                laser.On = true;
                laser.ScanOn = true;
                robot.startMoveForward();  
            }
        }

        private void directionChangeHandler()
        {
            for (int i = -1; i <= 1; i++)
            {
                laser.CurrentScanAngle = 10 * i; 

                Position basePos = laser.ScanBasePosition;

                Position tipPos = laser.ScanTipPosition;

                if (i == 0)
                {
                    // Check for a direction which has not already been visited

                    Double x = tipPos.X;
                    Double y = tipPos.Y;

                    int mapX = (int)(x / occupancyMap.CellWidth);
                    int mapY = (int)(y / occupancyMap.CellHeight);

                    int mapVal = occupancyMap.getMap(mapX, mapY);

                    if (mapVal == (int) OccupancyMapStatus.VISITED)
                    {
                        return;
                    }
                }

                if (laser.getNearestObstacleIntersection() != null)
                    return;
            } 


            laser.CurrentScanAngle = 0;

            laser.On = true;

            laser.ScanOn = true;

            robot.RotationAngle = 0.0;

            robot.CriticalDistanceDetection = true;
          
            robot.startMoveForward(); 
           
        }

        public Comms CommsReader
        {
            get
            {
                return commsReader;
            }
        }

        public Comms CommsWriter
        {
            get
            {
                return commsWriter;
            }
        }

        public void updateOccupancyMap(Position scannedObstaclePosition)
        {
            if (scannedObstaclePosition != null)
            {

                Double x = scannedObstaclePosition.X;
                Double y = scannedObstaclePosition.Y;

                int mapX = (int)(x / occupancyMap.CellWidth);
                int mapY = (int)(y / occupancyMap.CellHeight);

                occupancyMap.setMap(mapX, mapY, (int)OccupancyMapStatus.OCCUPIED);
            }
        }


        public void startController()
        {
            Laser laser = (Laser)robot.Sensors[0];        
           
            // Random rotation before forward motion
            double rand = randomNumberGenerator.NextDouble();
            robot.startRotateBy(10 + rand * 10);
         
        }

        public void startCommsThread()
        {
            commsWriter.connect();
            commsReader.connect();
            commsReader.read();
        }

        
    }



    public class OccupancyMap
    {
        private int cellWidth, cellHeight;

        private int mapWidth, mapHeight;

        private int[,] map;

        private event occupancyMapCellChangeDelegate occupancyMapCellChangeEvent;

        public OccupancyMap(int cw, int ch, int rw, int rh)
        {
            cellHeight = ch;
            cellWidth = cw;

            mapWidth=1+rw/cellWidth;

            mapHeight=1+rh/cellHeight;

            map=new int[mapWidth, mapHeight];

            // Initialize to unvisited status (0.5)
            for (int x=0; x < mapWidth; x++)
                for (int y = 0; y < mapHeight; y++)
                {
                    map[x, y] = (int)OccupancyMapStatus.UNOCCUPIED;
                }

        }

        public void setMap(int x, int y, int value)
        {
            if ((x>=0) && (x<mapWidth))
                if ((y>=0) && (y<mapHeight))
                    if (map[x, y] != value)
                    {
                        // Fire occupancy map change event
                        System.Windows.Application.Current.Dispatcher.BeginInvoke(new ThreadStart(() => occupancyMapCellChangeEvent(x, y, value)), null);
                      //  occupancyMapCellChangeEvent(x, y, value);
                        map[x, y] = value;
                    }
        }

        public int getMap(int x, int y)
        {
            if ((x >= 0) && (x < mapWidth))
                if ((y >= 0) && (y < mapHeight))
                    return map[x, y];

            return -1;
        }

        public void refreshMap()
        {
            // Reset map visited cells
            for (int x = 0; x < mapWidth; x++)
                for (int y = 0; y < mapHeight; y++)
                    if (map[x, y] == (int)OccupancyMapStatus.VISITED)
                    {
                        // Fire occupancy map change event
                        System.Windows.Application.Current.Dispatcher.BeginInvoke(new ThreadStart(() => occupancyMapCellChangeEvent(x, y, 3)), null);
                       // occupancyMapCellChangeEvent(x, y, (int)OccupancyMapStatus.UNOCCUPIED);
                        map[x, y] = (int)OccupancyMapStatus.UNOCCUPIED;                
                    }
        }

        public int CellWidth
        {
            get
            {
                return cellWidth;
            }
        }

        public int CellHeight
        {
            get
            {
                return cellHeight;
            }
        }

        public event occupancyMapCellChangeDelegate OccupancyMapCellChangeEvent
        {
            add
            {
                occupancyMapCellChangeEvent += value;
            }
            remove
            {
                occupancyMapCellChangeEvent -= value;
            }
        }
    }


    public class Comms
    {
        // Simulates comms between the controller and the devices

        private NamedPipeServerStream pipe;
        private StreamWriter sw;
        private StreamReader sr;
        private event obstacleDetectedDelegate obstacleDetectedEvent;

        private Position getPositionFromString(String s)
        {
            String[] words = s.Split(' ');


            double x = Convert.ToDouble(words[1]);
            double y = Convert.ToDouble(words[3]);

            return (new Position(x, y, 0));

        }

        public Comms(String pipeName, char readWrite)
        {
            pipe = new NamedPipeServerStream(pipeName, PipeDirection.InOut);
        }


        public void connect()
        {
            Console.WriteLine("Waiting for connection");
            if (pipe != null)
                pipe.WaitForConnection();

            sw = new StreamWriter(pipe);

            sr = new StreamReader(pipe);

            sw.AutoFlush = true;

        }


        public void read()
        {
            String s;
            while (true)
            {
                try
                {
                    if (sr != null)
                    {
                        s = sr.ReadLine();

                        if (s[0] == 'P')    // Position of obstacle sent
                        {
                            Position obstaclePosition = getPositionFromString(s);

                            obstacleDetectedEvent(obstaclePosition);
                        }
                    }
                }
                catch (IOException e)
                { }
            }
        }

        public StreamWriter Writer
        {
            get
            {
                return sw;
            }
        }

        public StreamReader Reader
        {
            get
            {
                return sr;
            }
        }

        public event obstacleDetectedDelegate ObstacleDetectedEvent
        {
            add
            {
                obstacleDetectedEvent += value;
            }
            remove
            {
                obstacleDetectedEvent -= value;
            }
        }
    }

    class ControllerTest
    {
        static void Main(string[] args)
        {
            Comms comms = new Comms("Communication Link",'r');

            comms.connect();
        }
    }

}