using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;
using System.Drawing;

namespace Mobile_Robot
{
    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 void rotateAndTranslate(Position p, double dTheta, double dx, double dy)
        {
            p.x += (dx * Math.Cos(theta) - dy * Math.Sin(theta));
            p.y += (dx * Math.Sin(theta) + dy * Math.Cos(theta));
            p.theta += dTheta;
        }

        public Position toGlobal(Position refPos)
        {
            return new Position(x * Math.Cos(refPos.theta) - y * Math.Sin(refPos.theta) + refPos.x,
                                x * Math.Sin(refPos.theta) + y * Math.Cos(refPos.theta) + refPos.y, 
                                0); 
        }
    }

    public abstract class Sensor
    {
        private ArrayList shape;            // List of polygon vertices with respect to local coordinate system

        private Robot robot;                // Reference to encapsulating robot object

        private Position sensorPosition;    // Device position on the robot with respect to the local coordinate system 

        private String sensorName;

        
        public Sensor(String n, Robot r, Position localPos)
        {
            sensorName = n;
            shape = new ArrayList();
            sensorPosition = localPos;
            robot = r;
            
            // Create polygon vertices
        }

        public void paint(Graphics g)
        {
        }

    }

    public class Robot
    {
        private String name;

        private Position globalPosition;          // Position in global coordinate frame

        private Point[] platformPolygon;            // Defines robot chassis shape

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

            globalPosition = pos;

            // Create platform vertices
            platformPolygon = new Point[6];
            platformPolygon[0] = new Point(20, 20);
            platformPolygon[1] = new Point(30, 10);
            platformPolygon[2] = new Point(30, -10);
            platformPolygon[3] = new Point(20, -20);
            platformPolygon[4] = new Point(-20, -20);
            platformPolygon[5] = new Point(-20, 20);

        }

        public Point[] PlatformPolygon
        {
            get { return platformPolygon; }
        }

        public Position GlobalPosition
        {
            get { return globalPosition; }

            set { globalPosition = value; }
        }

        public void paint(SolidBrush b, Graphics g)
        {
            // Paint the platform and all on board sensors

            // Convert to global coordinates
            Position pos;
            Point[] polygon = new Point[6];
            for (int i = 0; i < platformPolygon.Length; i++)
            {
                pos = new Position(platformPolygon[i].X, platformPolygon[i].Y, 0);
                pos = pos.toGlobal(globalPosition);
                polygon[i].X = (int)pos.X;
                polygon[i].Y = (int)pos.Y;
            }

            g.FillPolygon(b, polygon);
        }

    }

    class MobileRobotMain
    {
        static void Main(string[] args)
        {
            Robot r=new Robot("ROBOT", new Position(0,0,0));
            foreach (Point p in r.PlatformPolygon)
            {
                Console.WriteLine(p.X + "  " + p.Y);
            }
        }
    }
}