using System;
using System.Collections;
using System.Threading;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Forms;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Media.Animation;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Windows.Threading;
using System.Diagnostics;
using System.Timers;
using System.Xml;
using Mobile_Robot;
using Mobile_Robot_Controller;


namespace Mobile_Robot_Interface
{
    using Environment_Model;

    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class mainwindow : Window
    {
        private ArrayList obstacles;

        private Environment environment;

        private TranslateTransform platformTt;

        private RotateTransform platformRt;

        private RotateTransform sensorRt;

        private TranslateTransform sensorTt;

        private Robot robot;

        private Controller robotController;

        private Laser laser;

        private Position currentPosition;       // Robot position

        private double currentAngle;            // Laser angle (w.r.t. local coordinate system)

        private Polygon platformShape;          // Robot chassis rendering

        private Rectangle sensorShape;          // Laser beam graphical rendering

        private Rectangle dotShape;             // Tracks robot position on occupancy map

        private Rectangle[,] occupancyMapDisplay;

        private int occupancyMapCellWidth = 10;

        private int occupancyMapCellHeight = 10;

        private Boolean simulationRunning;

        public mainwindow()
        {
            InitializeComponent();

            // List of obstacles
            obstacles = new ArrayList();

            // Create robot and sensors
            robot = new Robot("ROBOT", new Position(0, 0, 0));

            currentPosition = robot.CurrentPosition;

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

            currentAngle = laser.CurrentScanAngle;


            // Initialize geometric transformations for the robot platform
            platformShape = new Polygon();
            platformShape.Fill = Brushes.Chocolate;

            platformTt = new TranslateTransform();
            platformRt = new RotateTransform();
            platformRt.CenterX = 0;
            platformRt.CenterY = 0;

            TransformGroup myTransformGroup = new TransformGroup();        

            myTransformGroup.Children.Add(platformRt);
            myTransformGroup.Children.Add(platformTt);

            platformShape.RenderTransform = myTransformGroup;

            // Initialize geometric transformations for the sensor
            sensorShape = new Rectangle();

             if (!laser.On)
                sensorShape.Fill = Brushes.Transparent;
             else
                sensorShape.Fill = Brushes.Yellow;

            sensorTt = new TranslateTransform();
            sensorRt = new RotateTransform();
            
            sensorRt.CenterX = 0;
            sensorRt.CenterY = 0;

            TransformGroup mySensorTransformGroup = new TransformGroup();

            mySensorTransformGroup.Children.Add(sensorRt);
            mySensorTransformGroup.Children.Add(sensorTt);

            sensorShape.RenderTransform = mySensorTransformGroup;

            // Render robot and sensor to canvas
            PointCollection pc = new PointCollection();

            System.Drawing.Point[] points = robot.PlatformPolygon;

            foreach (System.Drawing.Point p in points)
                pc.Add(new Point(p.X,p.Y));

            platformShape.Points = pc;

            canvas1.Children.Add(platformShape);

            sensorShape.Width = laser.Range;

            sensorShape.Height = laser.BeamWidth;

            canvas1.Children.Add(sensorShape);


            // Adjust local position of sensor
            sensorTt.X += laser.SensorOrigin.X;

            sensorTt.Y += laser.SensorOrigin.Y;

            sensorRt.Angle += laser.SensorOrigin.Theta;

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

            // Initialize sensor state change handler
            laser.SensorStateChangeEvent += new sensorStateChangeDelegate(sensorStateChangeHandler);

            // Initialize laser angle change event handler
            laser.LaserAngleChangeEvent += new laserAngleChangeDelegate(laserAngleChangeHandler);

            // Event for message passing
            robot.DebugMessageEvent += new debugMessageDelegate(debugMessageHandler);

            // Event for message passing
            laser.DebugMessageEvent += new debugMessageDelegate(debugMessageHandler);
            
            // Initialize the occupancy grid cells
            canvas2.Height = canvas1.Height;
            canvas2.Width = canvas1.Width;
        }

        public void positionChangeHandler(Position newPosition)
        {
            double angle = newPosition.Theta - currentPosition.Theta;

            double thetaRad = Math.PI * currentPosition.Theta / 180.0;

            double newThetaRad = Math.PI * newPosition.Theta / 180.0;

            sensorTt.X -= (laser.SensorOrigin.X * Math.Cos(thetaRad) + laser.SensorOrigin.Y * Math.Sin(thetaRad));
            sensorTt.Y -= (laser.SensorOrigin.X * Math.Sin(thetaRad) - laser.SensorOrigin.Y * Math.Cos(thetaRad));


            // Apply shift/rotate to sensor
            sensorRt.Angle += angle;
            sensorTt.X += (newPosition.X - currentPosition.X);
            sensorTt.Y += (newPosition.Y - currentPosition.Y);


            sensorTt.X += (laser.SensorOrigin.X * Math.Cos(newThetaRad) + laser.SensorOrigin.Y * Math.Sin(newThetaRad));
            sensorTt.Y += (laser.SensorOrigin.X * Math.Sin(newThetaRad) - laser.SensorOrigin.Y * Math.Cos(newThetaRad));


            // Apply shift/rotate to robot platform  
            platformRt.Angle += angle;         
            platformTt.X += (newPosition.X - currentPosition.X);
            platformTt.Y += (newPosition.Y - currentPosition.Y);

            // Update tracker dot position
            Canvas.SetLeft(dotShape, newPosition.X - 1);
            Canvas.SetTop(dotShape, newPosition.Y - 1);
            canvas2.Children.Remove(dotShape);
            canvas2.Children.Add(dotShape);
        }

        public void sensorStateChangeHandler()
        {
            if (!laser.On)
                sensorShape.Fill = Brushes.Transparent;
            else
                sensorShape.Fill = Brushes.Yellow;
        }

        public void laserAngleChangeHandler(Double newAngle)
        {
            double angle = laser.CurrentScanAngle - newAngle;

            // Apply shift/rotate to sensor
            sensorRt.Angle += angle;
        }

        public void occupancyMapChangeHandler(int x, int y, int value)
        {
            if (value == (int)OccupancyMapStatus.UNOCCUPIED)           // Empty cell
            {
                occupancyMapDisplay[x, y].Fill = Brushes.LightGray;
            }
            else if (value == (int)OccupancyMapStatus.OCCUPIED)      // Occupied cell
            {
                occupancyMapDisplay[x, y].Fill = Brushes.DarkGray;
            }
        }


        public void debugMessageHandler(String message)
        {
            System.Windows.Forms.MessageBox.Show(message);

        }

        public bool parseXML(String xmlFilename)
        {
            XmlTextReader reader = new XmlTextReader(xmlFilename);

            Obstacle obs = null;

            ArrayList vertices = null;

            while (reader.Read())
            {
                switch (reader.NodeType)
                {
       
                    case XmlNodeType.Element: // The node is an element.
                        if (reader.Name.Equals("OBSTACLE"))
                        {
                            obs = new Obstacle();
                            vertices = new ArrayList();
                            while (reader.MoveToNextAttribute()) // Read the attributes.
                            {
                                if (reader.Name.Equals("NAME")) 
                                    obs.Name = reader.Value;
                                if (reader.Name.Equals("OPAQUE"))
                                {
                                    if (reader.Value.Equals("true")) 
                                        obs.Opaque = true;
                                    else
                                        obs.Opaque = false;
                                }
                            }
                        }
                        else if (reader.Name.Equals("POINT"))
                        {
                            Point p = new Point();
                            while (reader.MoveToNextAttribute()) // Read the attributes.
                            {
                                if (reader.Name.Equals("X"))
                                {
                                    p.X = Convert.ToInt32(reader.Value);
                                }
                                else if (reader.Name.Equals("Y"))
                                {
                                    p.Y = Convert.ToInt32(reader.Value);
                                }
                            }

                            vertices.Add(p);
                        }
                        break;
                    
                    case XmlNodeType.EndElement: //Display the end of the element.
                        if (reader.Name.Equals("OBSTACLE"))
                        {
                            obs.Vertices = vertices;

                            obstacles.Add(obs);
                        }

                        break;

                }
            }

            return true;
        }

        private void paintEnvironment(Canvas c)
        {

            if (environment != null)
                foreach (Obstacle o in environment.Obstacles)
                    paintObstacle(o,c);
        }

        public void paintObstacle(Obstacle o, Canvas c)
        {
            Point[] vertexArray = o.Vertices.ToArray(typeof(Point)) as Point[];

            Polygon polygon = new Polygon();

            if (o.Opaque)
            {
                polygon.Stroke = Brushes.Black;

                polygon.StrokeThickness = 2;
            }

            polygon.Fill = Brushes.Gray;
            
                        
            PointCollection pc = new PointCollection();

            if (vertexArray.Length > 1)
            {
                foreach (Point p in vertexArray)
                    pc.Add(p);
            }

            polygon.Points = pc;

            c.Children.Add(polygon);
        }

        private void canvas1_MouseLeftButtonDown(object sender, MouseButtonEventArgs e)
        {
            int x = (int)(e.GetPosition(canvas1).X);
            int y = (int)(e.GetPosition(canvas1).Y);
            System.Windows.Forms.MessageBox.Show(x + "  " + y + "  " + laser.ScanTipPosition.X + "  " + laser.ScanTipPosition.Y);
            
        }

        private void canvas1_MouseRightButtonDown(object sender, MouseButtonEventArgs e)
        {
            int x = (int)(e.GetPosition(canvas1).X);
            int y = (int)(e.GetPosition(canvas1).Y);
            double dx = x - currentPosition.X;
            double dy = y - currentPosition.Y;
            double t = 180 * Math.Atan2(dy, dx) / Math.PI;

            if (robot.CurrentVelocity<0.1)
                robot.MotionRequest = new MotionRequest(new Position(x, y, t), false);     // Non premptive motion
            else
                robot.MotionRequest = new MotionRequest(new Position(x, y, t), true); 

        }

    
        private void loadEnvironmentButton_Click(object sender, RoutedEventArgs e)
        {
            OpenFileDialog openFileDialog1 = new OpenFileDialog();

            openFileDialog1.InitialDirectory = "Map Files";

            openFileDialog1.Filter = "xml files (*.xml) |" + "*.xml|"
                                     + "All files (*.*) |" + "*.*";

            if (openFileDialog1.ShowDialog() == System.Windows.Forms.DialogResult.OK)
            {
                String fileName = openFileDialog1.FileName;
                if (fileName.Length != 0)
                {
                    try
                    {
                        parseXML(fileName);

                        // Create the environment model
                        environment = new Environment((int)canvas1.Width, (int)canvas1.Height);

                        laser.Environment = environment;

                        robot.RegionWidth = laser.Environment.RegionWidth;

                        robot.RegionHeight = laser.Environment.RegionHeight;

                        environment.Obstacles = obstacles;

                        paintEnvironment(canvas1);

                        // Add the enclosing region rectangle to the obstacles list
                        ArrayList regionCorners = new ArrayList();
                        Point p1 = new Point(0, 0);
                        Point p2 = new Point(environment.RegionWidth - 1, 0);
                        Point p3 = new Point(environment.RegionWidth - 1, environment.RegionHeight - 1);
                        Point p4 = new Point(0, environment.RegionHeight - 1);
                        regionCorners.Add(p1);
                        regionCorners.Add(p2);
                        regionCorners.Add(p3);
                        regionCorners.Add(p4);

                        Obstacle enclosingRegion = new Obstacle("Region", true, regionCorners);
                        environment.Obstacles.Add(enclosingRegion);

                    }
                    catch (ArgumentException ex)
                    { }
                }
            }

        }

        private void controllerInitializeButton_Click(object sender, RoutedEventArgs e)
        {
            if (environment != null)
            {
                robotController = new Controller(robot, occupancyMapCellWidth, occupancyMapCellHeight, environment.RegionWidth, environment.RegionHeight);

                // Set up display for occupancy map
                int occupancyMapWidth=1+environment.RegionWidth/occupancyMapCellWidth;
                int occupancyMapHeight=1+environment.RegionHeight/occupancyMapCellHeight;
                occupancyMapDisplay = new Rectangle[occupancyMapWidth, occupancyMapHeight];

                for (int x = 0; x < occupancyMapWidth; x++)
                    for (int y = 0; y < occupancyMapHeight; y++)
                    {
                        occupancyMapDisplay[x, y] = new Rectangle();

                        occupancyMapDisplay[x, y].Fill = Brushes.LightGray;

                        occupancyMapDisplay[x, y].Width = occupancyMapCellWidth;
                        occupancyMapDisplay[x, y].Height = occupancyMapCellHeight;

                        Canvas.SetLeft(occupancyMapDisplay[x, y], x * occupancyMapCellWidth);
                        Canvas.SetTop(occupancyMapDisplay[x, y], y * occupancyMapCellHeight);

                        canvas2.Children.Add(occupancyMapDisplay[x, y]);
                    }

                dotShape = new Rectangle();
                dotShape.Width = 3;
                dotShape.Height = 3;
                dotShape.Fill = Brushes.Black;
                Canvas.SetLeft(dotShape, -1);
                Canvas.SetTop(dotShape, -1);
                canvas2.Children.Add(dotShape);

                // Initialize occupancy map change event handler
                robotController.OccupancyMap.OccupancyMapCellChangeEvent += new occupancyMapCellChangeDelegate(occupancyMapChangeHandler);

 
                // Start the robot controller
                robotController.startController();

                simulationRunning = true;

                

            }
            else
                System.Windows.Forms.MessageBox.Show("Environment not loaded");


        }


        private void pauseSimulationButton_Click(object sender, RoutedEventArgs e)
        {
            if (simulationRunning)
            {
                laser.ScanTimer.Stop();

                robot.MotionTimer.Stop();

                simulationRunning = false;

                pauseSimulationButton.Content = "Start";
            }
            else
            {
                laser.ScanTimer.Start();

                robot.MotionTimer.Start();

                simulationRunning = true;

                pauseSimulationButton.Content = "Pause";

            }
        }

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
          
        }

        private void Window_Closed(object sender, EventArgs e)
        {
          
        }

    }
 
}