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

namespace Mobile_Robot
{
    using Environment_Model;

    public  enum RotationDirection { CLOCKWISE, ANTICLOCKWISE };

    public delegate void positionChangeDelegate(Position  p);   // Called  when  robot changes position

    public delegate void directionChangeDelegate();     // Called when robot platform direction changes on continuous rotate

    public delegate void laserAngleChangeDelegate(Double angle);

    public delegate void sensorStateChangeDelegate();

    public delegate void obstacleWithinCriticalRangeDelegate(Position pos);

    public delegate void motionRequestCompletedDelegate();

    public delegate void rotationCompletedDelegate();

    public delegate void debugMessageDelegate(String message);

    public class Position
    {
        private Double x, y, theta;     // position and orientation

        public Position(Double _x, Double _y, Double _theta)
        {
            x = _x; y = _y; theta = _theta;
        }

        public Double X
        {
            get { return x; }

            set { x = value; }
        }

        public Double Y
        {
            get { return y; }

            set { y = value; }
        }

        public Double Theta
        {
            get { return theta; }

            set { theta = value; }
        }

        public static Position getLineSegmentIntersection(Position p1, Position p2, Position p3, Position p4)
        {
            double dx1 = p1.X - p2.X;
            double dx2 = p4.X - p3.X;
            double dx3 = p4.X - p2.X;
            double dy1 = p1.Y - p2.Y;
            double dy2 = p4.Y - p3.Y;
            double dy3 = p4.Y - p2.Y;

            double lambda1 = dy2 * dx3 - dx2 * dy3;
            double lambda2 = dx1 * dy3 - dy1 * dx3;
            double det = dx1 * dy2 - dx2 * dy1;
            double absDet = Math.Abs(det);

            if ((Math.Abs(lambda1) > absDet) || (Math.Abs(lambda2) > absDet))
                return null;
            else
            {
                lambda1 /= det;
                lambda2 /= det;

                if ((lambda1 >= 0) && (lambda1 <= 1) && (lambda2 >= 0) && (lambda2 <= 1))
                    return (new Position(lambda1 * p1.X + (1 - lambda1) * p2.X, lambda1 * p1.Y + (1 - lambda1) * p2.Y, 0.0));
                else
                    return null;
            }
        }


        public override string ToString()
        {
            return x + "  " + y + "  " + theta;
        }
    }

    public class MotionRequest
    {
        private Position targetPosition;

        private Boolean angleOK;

        private Boolean xyOK;

        private Boolean motionPreempt;

        private RotationDirection rotationDirection;

        public MotionRequest(Position p, Boolean preempt)
        {
            targetPosition = p;
            angleOK = false;
            xyOK = false;
            rotationDirection = RotationDirection.CLOCKWISE;
            motionPreempt = preempt;
        }

        public Position TargetPosition
        {
            get { return targetPosition; }

            set { targetPosition = value; }
        }

        public Boolean AngleOK
        {
            get { return angleOK; }

            set { angleOK = value; }
        }

        public Boolean XYOK
        {
            get { return xyOK; }

            set { xyOK = value; }
        }

        public Boolean MotionPremempt
        {
            get { return motionPreempt; }

            set { motionPreempt = value; }
        }

        public RotationDirection RotationDirection
        {
            get { return rotationDirection; }

            set { rotationDirection = value; }
        }
    }

    public class Sensor
    {
        private Boolean on;

        private String name;

        private Position sensorOrigin;            // Position with respect to robot platform origin

        protected event sensorStateChangeDelegate sensorStateChangeEvent;

        protected Environment environment;

        public Sensor(String n, Position p, Environment env)
        {
            name = n;

            sensorOrigin = p;

            on = false;

            environment = env;
        }

        public Environment Environment
        {
            get
            {
                return environment;
            }

            set
            {
                environment = value;
            }
        }


        
        public Boolean On
        {
            get { return on; }

            set
            {
                if (on != value)
                {
                    on = value;

                    System.Windows.Application.Current.Dispatcher.BeginInvoke(new ThreadStart(() => sensorStateChangeEvent()), null);

                }
            }
        }

        public Position SensorOrigin
        {
            get { return sensorOrigin; }
        }

        public event sensorStateChangeDelegate SensorStateChangeEvent
        {
            add
            {
                sensorStateChangeEvent += value;
            }
            remove
            {
                sensorStateChangeEvent -= value;
            }
        }
    }

    public class Laser : Sensor
    {
        private DispatcherTimer scanTimer = new DispatcherTimer();

        private Robot robot;

        private double currentScanAngle;                    // Scan angle from horizontal

        private int range;                                  // Beam range

        private int beamWidth;                              // Beam width; 

        private double scanAngleStep;                       // Laser angle step in degrees

        private double criticalRange;                       // Critical range for obstacle avoidance

        private Position scanBasePosition;                  // Global position of base of beam

        private Position localScanBasePosition;             // Local position of base of beam

        private Position scanTipPosition;                   // Global position of beam tip

        private Position nearestScannedObstaclePosition;    // Nearest scanned obstacle position 

        private double maxSweepAngle;                       // Maximum clcokwise/anticlockwise rotation   

        private RotationDirection scanDirection;

        private event laserAngleChangeDelegate laserAngleChangeEvent;

        private event obstacleWithinCriticalRangeDelegate obstacleWithinCriticalRangeEvent;

        public event debugMessageDelegate debugMessageEvent;       // Signals to display a debug message

        private Boolean scanOn;

        public Laser(Robot rb, Environment env, String n, Position p, int r) : base(n, p, env)
        {
            robot = rb;
            range = r;
            beamWidth = 1;
            scanAngleStep = 1.0;
            criticalRange = 5000;
            maxSweepAngle = 10;
            localScanBasePosition = p;
            scanBasePosition = new Position(0, 0, 0);
            scanTipPosition = new Position(0, 0, 0);

            currentScanAngle = p.Theta;

            scanOn = false;

            scanDirection = RotationDirection.CLOCKWISE;            // Initial scan direction

            // Timer for scanning the sensor            
            scanTimer.Interval = new TimeSpan(0, 0, 0, 0, 20);
            scanTimer.Tick += new EventHandler(scanTimerEventHandler);
        }

        private void scanTimerEventHandler(object sender, EventArgs eArgs)
        {
            double angle;

            if (sender == scanTimer)
            {
                if (scanDirection == RotationDirection.ANTICLOCKWISE)
                {
                    // Scan angle decreases
                    angle = currentScanAngle - scanAngleStep;

                    if (angle < (-maxSweepAngle))
                        scanDirection = RotationDirection.CLOCKWISE;
                }
                else
                {
                    // Scan angle increases
                    angle = currentScanAngle + scanAngleStep;

                    if (angle > (maxSweepAngle))
                        scanDirection = RotationDirection.ANTICLOCKWISE;
                }

                CurrentScanAngle = angle;

            }
        }

        public Boolean ScanOn
        {
            get { return scanOn; }

            set
            {
                if (scanOn != value)
                {
                    scanOn = value;

                    if (scanOn)
                        startScan();
                    else
                        stopScan();
                }
            }
        }

        public void startScan()
        {
            scanTimer.Start();
        }

        public void stopScan()
        {
            scanTimer.Stop();
        }

        public int BeamWidth
        {
            get { return beamWidth; }
        }

        public int Range
        {
            get { return range; }
        }

        public Position ScanBasePosition
        {
            get
            {
                // Update laser position
                double thetaRad = Math.PI * robot.CurrentPosition.Theta / 180.0;

                double localX = localScanBasePosition.X;
                double localY = localScanBasePosition.Y;

                double scanX = localX * Math.Cos(thetaRad) + localY * Math.Sin(thetaRad);
                double scanY = localX * Math.Sin(thetaRad) - localY * Math.Cos(thetaRad); 

                scanBasePosition.X = robot.CurrentPosition.X + scanX;
                scanBasePosition.Y = robot.CurrentPosition.Y + scanY;


                return scanBasePosition;
            }

            set
            {
                scanBasePosition = value;
            }
        }

    
        public Position ScanTipPosition
        {
            get 
            {
                // Update laser position
                double thetaRad = Math.PI * robot.CurrentPosition.Theta / 180.0;

                double localX = localScanBasePosition.X;
                double localY = localScanBasePosition.Y;

                double scanAngleRad = Math.PI * currentScanAngle / 180.0;

                if ((thetaRad >= Math.PI / 2) && (thetaRad < 3 * Math.PI / 2))
                {
                    localX += (range * Math.Cos(scanAngleRad));
                    localY += (range * Math.Sin(scanAngleRad));
                }
                else
                {
                    localX += (range * Math.Cos(scanAngleRad));
                    localY += (range * Math.Sin(scanAngleRad));
                }

                double scanX = localX * Math.Cos(thetaRad) + localY * Math.Sin(thetaRad);
                double scanY = localX * Math.Sin(thetaRad) - localY * Math.Cos(thetaRad);

                scanTipPosition.X = robot.CurrentPosition.X + scanX;
                scanTipPosition.Y = robot.CurrentPosition.Y + scanY;
                
                
                return scanTipPosition; 
            }

            set
            {
                scanTipPosition = value;
            }
        }

        public Position getNearestObstacleIntersection()
        {
            // Returns the nearest obstactle intersection position
            Position basePos = ScanBasePosition;

            Position tipPos = ScanTipPosition;

            Double nearestDistanceToObstacle = 1.0e10;

            nearestScannedObstaclePosition = null;

            foreach (Obstacle obs in environment.Obstacles)
            {
                ArrayList vertices = obs.Vertices;
                int j1;
                for (int j = 0; j < vertices.Count; j++)
                {
                    Point p0 = (Point)vertices[j];
                    Position pos0 = new Position(p0.X, p0.Y, 0);
                    if (j == (vertices.Count - 1))
                        j1 = 0;
                    else
                        j1 = j + 1;
                    Point p1 = (Point)(vertices[j1]);
                    Position pos1 = new Position(p1.X, p1.Y, 0);

                    Position pos = Position.getLineSegmentIntersection(basePos, tipPos, pos0, pos1);

                    if (pos != null)
                    {
                        Double d = (basePos.X - pos.X) * (basePos.X - pos.X) + (basePos.Y - pos.Y) * (basePos.Y - pos.Y);
                        if (d < nearestDistanceToObstacle)
                        {
                            nearestScannedObstaclePosition = pos;
                            nearestDistanceToObstacle = d;
                        }

                    }
                }

            }

            return nearestScannedObstaclePosition;
        }

        public Position NearestScannedObstaclePosition
        {
            get
            {
                // Returns the nearest obstactle intersection position
                ArrayList obstacles = environment.Obstacles;

                Position basePos = ScanBasePosition;

                Position tipPos = ScanTipPosition;

                Double nearestDistanceToObstacle = 1.0e10;

                nearestScannedObstaclePosition = null;

                foreach (Obstacle obs in obstacles)
                {
                    ArrayList vertices = obs.Vertices;
                    int j1;
                    for (int j = 0; j < vertices.Count; j++)
                    {
                        Point p0 = (Point)vertices[j];
                        Position pos0 = new Position(p0.X, p0.Y, 0);
                        if (j == (vertices.Count - 1))
                            j1 = 0;
                        else
                            j1 = j + 1;
                        Point p1 = (Point)(vertices[j1]);
                        Position pos1 = new Position(p1.X, p1.Y, 0);

                        Position pos = Position.getLineSegmentIntersection(basePos, tipPos, pos0, pos1); 

                        if (pos != null)
                        {
                            Double d = (basePos.X - pos.X) * (basePos.X - pos.X) + (basePos.Y - pos.Y) * (basePos.Y - pos.Y);
                            if (d < nearestDistanceToObstacle)
                            {
                                nearestScannedObstaclePosition = pos;
                                nearestDistanceToObstacle = d;
                            }

                        }
                    } 

                } 

          
                if ((robot.CriticalDistanceDetection) && (nearestDistanceToObstacle < criticalRange))      // Fire critical range event
                {
                    obstacleWithinCriticalRangeEvent(nearestScannedObstaclePosition);
                }

                return nearestScannedObstaclePosition;
            }
        }

        public event laserAngleChangeDelegate LaserAngleChangeEvent
        {
            add
            {
                laserAngleChangeEvent += value;
            }
            remove
            {
                laserAngleChangeEvent -= value;
            }
        }

        public event obstacleWithinCriticalRangeDelegate ObstacleWithinCriticalRangeEvent
        {
            add
            {
                obstacleWithinCriticalRangeEvent += value;
            }
            remove
            {
                obstacleWithinCriticalRangeEvent -= value;
            }
        }

        public double CurrentScanAngle
        {
            get { return currentScanAngle; }

            set
            {
                if (Math.Abs(currentScanAngle - value) >= 0.5 * scanAngleStep)
                {
                    // Fire scan angle change event
                    laserAngleChangeEvent(value);

                    currentScanAngle = value;

                    if (ScanOn)
                    {
                        Position nearestScannedObstaclePosition = NearestScannedObstaclePosition;

                        // Output position to comms.

                        if (nearestScannedObstaclePosition != null)
                            robot.CommsWriter.write("P " + nearestScannedObstaclePosition.X + "  " + nearestScannedObstaclePosition.Y);

                    }
                }

            }
        }

        public event debugMessageDelegate DebugMessageEvent
        {
            add
            {
                debugMessageEvent += value;
            }

            remove
            {
                debugMessageEvent -= value;
            }
        }

        public DispatcherTimer ScanTimer
        {
            get
            {
                return scanTimer;
            }
        }


    }


    public class Robot
    {
        private String name;

        private Comms commsReader;

        private Comms commsWriter;

        private Position currentPosition;           // Position in global coordinate frame

        private System.Drawing.Point[] platformPolygon;            // Defines robot chassis shape

        private ArrayList sensors;                  // To allow multiple sensors

        private event positionChangeDelegate positionChangeEvent;   // Signals change in  position

        private event motionRequestCompletedDelegate motionRequestCompletedEvent;   // Signals when a motion request has been completed

        private event rotationCompletedDelegate rotationCompletedEvent;   // Signals when a a search rotation has done a complete revolution

        private event directionChangeDelegate directionChangeEvent;     // Signals a new direction during contnuous rotation

        public event debugMessageDelegate debugMessageEvent;       // Signals to display a debug message

        private DispatcherTimer motionTimer=new DispatcherTimer();

        private MotionRequest motionRequest = null;

        private double currentVelocity, acceleration;

        private double maxVelocity;

        private double platformAngleResolution;

        private double platformDistanceResolution;

        private double decelerationVelocity;            // Start decelerating at this velocity   
        
        private double decelerationDistance = 100.0;    // Start decelerating at this distance

        private double minimumVelocity;                 // Less than this and assumed  stationary

        private Laser laser;

        private int regionWidth;                        // Defines region boundary

        private int regionHeight;                       // Defines region boundary

        private Boolean stopped;

        private Boolean criticalDistanceDetection;      // Critical distance decision on/off

        private double rotationAngle = 0.0;             // Measures accumulated rotation angle for rotation search

        public Robot(String n, Position pos)
        {
            name = n;

            currentPosition = pos;

            platformAngleResolution = 0.5;

            platformDistanceResolution = 0.5;

            minimumVelocity = 0.1;

            // Create sensors arraylist
            sensors = new ArrayList();

            // Add a laser sensor
            sensors.Add(new Laser(this, null, "LASER1", new Position(10, 0, 0), 150));

            laser = (Laser) sensors[0];

            // Create platform vertices
            platformPolygon = new System.Drawing.Point[6];

            platformPolygon[0] = new System.Drawing.Point(9, 7);
            platformPolygon[1] = new System.Drawing.Point(13, 3);
            platformPolygon[2] = new System.Drawing.Point(13, -3);
            platformPolygon[3] = new System.Drawing.Point(9, -7);
            platformPolygon[4] = new System.Drawing.Point(-9, -7);
            platformPolygon[5] = new System.Drawing.Point(-9, 7);

            // Timer for moving the robot            
            motionTimer.Interval = new TimeSpan(0,0,0,0,1);             
            motionTimer.Tick += new EventHandler(motionTimerEventHandler);
            motionTimer.Start();  
          
            // Set dynamical properties
            currentVelocity = 0.0;

            maxVelocity = 1000;

            acceleration = 20000;

            // Turn on critical distance detection
            criticalDistanceDetection = true;

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

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

        private void motionTimerEventHandler(object sender, EventArgs eArgs)
        {
            double dt = 0.001;
            double dx1, dy1, distanceToTarget, dd;

            if (sender == motionTimer)
            {
                if (motionRequest != null)
                {
                    if (motionRequest.MotionPremempt)
                    {
                        // Decelerate to stationary before dealing with request

                        double dx = Math.Cos(currentPosition.Theta * Math.PI / 180);
                        double dy = Math.Sin(currentPosition.Theta * Math.PI / 180);
                        moveTo(new Position(currentPosition.X + currentVelocity * dt * dx, currentPosition.Y + currentVelocity * dt * dy, currentPosition.Theta));

                        currentVelocity -= (acceleration * dt);

                        if (currentVelocity < minimumVelocity)
                        {
                            currentVelocity = 0;

                            if (motionRequest.TargetPosition == null)   // Enables stop command
                            {
                                motionRequest = null;

                                // Send motion request completed event
                                motionRequestCompletedEvent();

                                return;
                            }

                            motionRequest.MotionPremempt = false;

                            // Evaluate updated target angle
                            dx = motionRequest.TargetPosition.X - currentPosition.X;
                            dy = motionRequest.TargetPosition.Y - currentPosition.Y;

                            motionRequest.TargetPosition.Theta = 180 * Math.Atan2(dy, dx) / Math.PI;

                        }

                        return;
                    }
                }

                if ((motionRequest != null) && (!motionRequest.AngleOK))
                {
                    // Continuous roation if no target position
                    if (motionRequest.TargetPosition == null)
                    {
                        if (motionRequest.RotationDirection == RotationDirection.CLOCKWISE)
                            rotateBy(platformAngleResolution);
                        else
                            rotateBy(-platformAngleResolution);

                        rotationAngle += Math.Abs(platformAngleResolution);

                        // Fire direction change event to initiate searching for clear path
                        directionChangeEvent();

                        if (rotationAngle >= 360.0)
                        {
                            rotationCompletedEvent();

                            rotationAngle = 0.0;
                        }

                        return;
                    }

                    // Rotate to correct direction
                    double currentAngle = currentPosition.Theta;
                    double targetAngle = motionRequest.TargetPosition.Theta;

                    if (targetAngle > 360)
                        targetAngle -= 360;
                    else if (targetAngle < 0)
                        targetAngle += 360;


                    if (targetAngle < 180)
                    {
                        if ((currentAngle >= targetAngle) && (currentAngle <= (targetAngle + 180)))
                            rotateBy(-platformAngleResolution);
                        else
                            rotateBy(platformAngleResolution);
                    }
                    else
                    {
                        if (((currentAngle >= targetAngle) && (currentAngle <= 360)) || ((currentAngle >= 0) && (currentAngle <= (targetAngle - 180))))
                            rotateBy(-platformAngleResolution);
                        else
                            rotateBy(platformAngleResolution);

                    }

                    if (Math.Abs(currentAngle - targetAngle) <= platformAngleResolution)
                    {
                        motionRequest.AngleOK = true;
                        dx1 = motionRequest.TargetPosition.X - currentPosition.X;
                        dy1 = motionRequest.TargetPosition.Y - currentPosition.Y;
                        distanceToTarget = Math.Sqrt(dx1 * dx1 + dy1 * dy1);

                        if (distanceToTarget < platformDistanceResolution)
                        {
                            motionRequest = null;

                            // Send motion request completed event
                            motionRequestCompletedEvent();

                            return;
                        }

                        dd = Math.Max(platformDistanceResolution, distanceToTarget);

                        decelerationDistance = Math.Min(dd / 2, decelerationDistance);
                    }

                }
                else if ((motionRequest != null) && (!motionRequest.XYOK))
                {
                    // Continuous forward motion if no target position
                    if (motionRequest.TargetPosition == null)
                    {
                        double tRad = Math.PI * currentPosition.Theta / 180.0;

                        dx1 = Math.Max(regionWidth, regionHeight) * Math.Cos(tRad);
                        dy1 = Math.Max(regionWidth, regionHeight) * Math.Sin(tRad);
                        distanceToTarget = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
                        dd = distanceToTarget;
                    }
                    else
                    {
                        dx1 = motionRequest.TargetPosition.X - currentPosition.X;
                        dy1 = motionRequest.TargetPosition.Y - currentPosition.Y;
                        distanceToTarget = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
                        dd = Math.Max(platformDistanceResolution, distanceToTarget);
                    }

                    if (distanceToTarget > decelerationDistance)
                    {
                        // Perform constant acceleration up to the maximum velocity
                        if (currentVelocity < maxVelocity)
                            currentVelocity += (acceleration * dt);

                        moveTo(new Position(currentPosition.X + currentVelocity * dt * dx1 / dd, currentPosition.Y + currentVelocity * dt * dy1 / dd, currentPosition.Theta));

                        decelerationVelocity = currentVelocity;
                    }
                    else if (distanceToTarget > platformDistanceResolution)
                    {
                        currentVelocity = decelerationVelocity * distanceToTarget / decelerationDistance;
                        moveTo(new Position(currentPosition.X + currentVelocity * dt * dx1 / dd, currentPosition.Y + currentVelocity * dt * dy1 / dd, currentPosition.Theta));
                    }
                    else        // Motion request complete
                    {
                        motionRequest.XYOK = true;
                        motionRequest = null;
                        currentVelocity = 0;
                    }
                }

            }
        }


        public event positionChangeDelegate PositionChangeEvent
        {
            add
            {
                positionChangeEvent += value;
            }

            remove
            {
                positionChangeEvent -= value;
            }
        }

      
        public event directionChangeDelegate DirectionChangeEvent
        {
            add
            {
                directionChangeEvent += value;
            }

            remove
            {
                directionChangeEvent -= value;
            }
        }

        public event motionRequestCompletedDelegate MotionRequestCompletedEvent
        {
            add
            {
                motionRequestCompletedEvent += value;
            }

            remove
            {
                motionRequestCompletedEvent -= value;
            }
        }

        public event rotationCompletedDelegate RotationCompletedEvent
        {
            add
            {
                rotationCompletedEvent += value;
            }

            remove
            {
                rotationCompletedEvent -= value;
            }
        }

        
        public event debugMessageDelegate DebugMessageEvent
        {
            add
            {
                debugMessageEvent += value;
            }

            remove
            {
                debugMessageEvent -= value;
            }
        }

        public ArrayList Sensors
        {
            get { return sensors; }
        }


        public System.Drawing.Point[] PlatformPolygon
        {
            get { return platformPolygon; }
        }

        public MotionRequest MotionRequest
        {
            get { return motionRequest; }

            set
            {
                motionRequest = value;
            }
        }

        public Position CurrentPosition
        {
            get { return currentPosition; }

            set
            {
                if ((currentPosition.X != value.X) || (currentPosition.Y != value.Y) || (currentPosition.Theta != value.Theta))
                {
                    // Fire position change event
                    positionChangeEvent(value);

                    currentPosition.X = value.X;
                    currentPosition.Y = value.Y;
                    currentPosition.Theta = value.Theta;

                    if (currentPosition.Theta > 360)
                        currentPosition.Theta -= 360;
                    else if (currentPosition.Theta < 0)
                        currentPosition.Theta += 360;
                }

            }
        }

        public double CurrentVelocity
        {
            get
            {
                return currentVelocity;
            }

            set
            {
                currentVelocity = value;
            }
        }

        public int RegionWidth
        {
            get
            {
                return regionWidth;
            }

            set
            {
                regionWidth = value;
            }
        }

        public int RegionHeight
        {
            get
            {
                return regionHeight;
            }

            set
            {
                regionHeight = value;
            }
        }

        public Boolean Stopped
        {
            get
            {
                return stopped;
            }

            set
            {
                stopped = value;
            }
        }

        public Boolean CriticalDistanceDetection
        {
            get
            {
                return criticalDistanceDetection;
            }

            set
            {
                criticalDistanceDetection = value;
            }
        }

        public Double RotationAngle
        {
            get
            {
                return rotationAngle;
            }

            set
            {
                rotationAngle = value;
            }
        }

        public DispatcherTimer MotionTimer
        {
            get
            {
                return motionTimer;
            }
        }

        public Comms CommsReader
        {
            get
            {
                return commsReader;
            }
        }

        public Comms CommsWriter
        {
            get
            {
                return commsWriter;
            }
        }

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


        private  void moveTo(Position pos)
        {
            // Applies an incremental translation

            CurrentPosition = pos;
        }

        private  void rotateBy(Double angle)
        {
            // Applies an incremental rotation

            Position pos=new Position(currentPosition.X, currentPosition.Y,angle+currentPosition.Theta);

            CurrentPosition = pos;
        }

        public void startRotateBy(Double angle)
        {
            // Initiates a rotation  by specified amount 
            // angle>0 clockwise rotation
            // angle<0 anticlockwise rotation

            double x = CurrentPosition.X;
            double y = CurrentPosition.Y;
            double t = CurrentPosition.Theta + angle;

            MotionRequest = new MotionRequest(new Position(x, y, t), false);
        }

        public void startRotate(RotationDirection direction)
        {
            // Initiates a continuous rotation

            MotionRequest = new MotionRequest(null, false);
            MotionRequest.RotationDirection = direction;
        }

        public void startMoveForward()
        {
            // Initiates forward motion in curently pointing direction

            MotionRequest = new MotionRequest(null, false);                       // Non premptive motion
            MotionRequest.AngleOK = true;
        }

        public void stop()
        {
            // A preemptive stop request

            MotionRequest = new MotionRequest(null, true);
        }
    }

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

        private NamedPipeClientStream pipe;
        private StreamWriter sw;
        private StreamReader sr;


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

        public void connect()
        {
            if (pipe != null)
                pipe.Connect();

            sw = new StreamWriter(pipe);

            sr = new StreamReader(pipe);

            sw.AutoFlush = true;
        }

        public void write(String message)
        {
            sw.WriteLine(message);
        }

        public void read()
        {
            String s;

 
            while (true)
            {
                try
                {
                    if (sr != null)
                    {
                        s = sr.ReadLine();
                    }
                }
                catch (IOException e)
                { }
            }
        }

        public StreamWriter Writer
        {
            get
            {
                return sw;
            }
        }

        public StreamReader Reader
        {
            get
            {
                return sr;
            }
        }
    }

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

            String s="P " + 10.23 + "  " + 11.23;

            String[] words = s.Split(' ');

            if (words[0] == "P")
            {
                double x = Convert.ToDouble(words[1]);
                double y = Convert.ToDouble(words[3]);

                Console.WriteLine("x = " + x + " y= " + y);
            }

        }
    }
}