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

using MyImageProcessingClasses;

namespace World
{
    /// <summary>
    /// Car parameters.
    /// </summary>
    public static class Car
    {
        /// <summary>
        /// Car length [m].
        /// </summary>
        public const float CAR_LENGTH = 3.5f;

        /// <summary>
        /// Distance from driver seat to rear end [cm].
        /// </summary>
        public const Int32 DRIVER_TO_REAR_DISTANCE_CM = 250;

        /// <summary>
        /// Platform length [m].
        /// </summary>
        public const float PLATFORM_LENGTH = 1.0f;

        /// <summary>
        /// Color used to draw car.
        /// </summary>
        public static readonly Color CAR_COLOR = Color.FromArgb(128, Color.Red);

        /// <summary>
        /// Color used to draw platform.
        /// </summary>
        public static readonly Color PLATFORM_COLOR = Color.FromArgb(128, Color.LightBlue);

        /// <summary>
        /// Rectangle specifiing shape of the car.
        /// </summary>
        public static PointF[] CarRectangle = new PointF[4];

        /// <summary>
        /// Rectangle specifiing shape of the platform.
        /// </summary>
        public static PointF[] PlatformRectangle = new PointF[4];
    }

    /// <summary>
    /// Trajectory point.
    /// </summary>
    public class TrackPoint
    {
        /// <summary>
        /// Track point coordinates.
        /// </summary>
        private PointF coordinates = new PointF();

        /// <summary>
        /// Coordinates getter / setter.
        /// </summary>
        public PointF Coordinates
        {
            get
            {
                return coordinates;
            }

            set
            {
                this.coordinates = value;
            }
        }

        /// <summary>
        /// Default constructor.
        /// </summary>
        public TrackPoint()
        { }

        /// <summary>
        /// Cosntructor.
        /// </summary>
        /// <param name="coordinatesIn">Track point coordinates.</param>
        public TrackPoint(PointF coordinatesIn)
        {
            coordinates = coordinatesIn;
        }
    }

    /// <summary>
    /// Detected mark class.
    /// </summary>
    public class Mark
    {
        /// <summary>
        /// Mark world coordinates.
        /// </summary>
        private PointF coordinates = new PointF();

        /// <summary>
        /// Mark position.
        /// </summary>
        private WorldMap.MarkPosition position;

        /// <summary>
        /// Selected color.
        /// </summary>
        private Color markColor = new Color();

        /// <summary>
        /// Mark position getter / setter.
        /// </summary>
        public WorldMap.MarkPosition Position
        {
            get
            {
                return position;
            }

            set
            
            {
                position = value;
            }
        }

        /// <summary>
        /// Coordinates getter / setter.
        /// </summary>
        public PointF Coordinates
        {
            get
            {
                return coordinates;
            }

            set
            {
                this.coordinates = value;
            }
        }

        /// <summary>
        /// Mark color getter / setter.
        /// </summary>
        public Color MarkColor
        {
            get
            {
                return markColor;
            }

            set
            {
                this.markColor = value;
            }
        }

        /// <summary>
        /// Default constructor.
        /// </summary>
        public Mark()
        { }

        /// <summary>
        /// Constructor.
        /// </summary>
        /// <param name="coordinatesIn">Mark world coordinates.</param>
        /// <param name="colorIn">Mark selected color.</param>
        /// <param name="positionIn">Mark position.</param>
        public Mark(PointF coordinatesIn, Color colorIn, WorldMap.MarkPosition positionIn)
        {
            coordinates = coordinatesIn;
            markColor = colorIn;
            position = positionIn;
        }
    }

    /// <summary>
    /// Wheelchair model.
    /// </summary>
    /// <remarks>
    /// System.Runtime.Remoting.Contexts.Synchronization attribute used, inherited form ContexBoundObject.
    /// Mutual exclusion provided automatically by system.
    /// </remarks>
    [System.Runtime.Remoting.Contexts.Synchronization]
    public class WheelchairModel : ContextBoundObject
    {
        #region Constants
        /// <summary>
        /// Colors used to draw wheelchair into world map.
        /// </summary>
        public static readonly List<Brush> WHEELCHAIR_COLOR = new List<Brush>() { new SolidBrush(Color.Black), new SolidBrush(Color.Red) };

        /// <summary>
        /// Timeout for user response when mark confirmation via WiFi is demanded [ms].
        /// </summary>
        public const Int32 USER_RESPONSE_TIMEOUT_MS = 30000; //30 seconds

        /// <summary>
        /// Backward speed of wall-following [cm/s].
        /// </summary>
        public const Int16 VDES = -40;

        /// <summary>
        /// Wall-following y0 [cm].
        /// </summary>
        public const Byte Y0DES = 70;

        /// <summary>
        /// Maximal allowed distance for target to be considered reached.
        /// </summary>
        public const Byte TARGET_COORD_ALLOWED_ERROR_CM = 10;

        /// <summary>
        /// Angle to rotate wheelchair when marks are not detected.
        /// </summary>
        public const UInt16 MARK_DETECT_ROTATION_ANGLE_MRAD = 523; //523 mrad = 30°

        /// <summary>
        /// Enum to determine user response (when mark confirmation via WiFi is demanded).
        /// </summary>
        public enum UserResponseCode : byte
        { 
            /// <summary>
            /// User demanded script end.
            /// </summary>
            END = 0,

            /// <summary>
            /// Wrong detection, look further.
            /// </summary>
            LOOK_FURTHER,

            /// <summary>
            /// Marks detected, go on.
            /// </summary>
            MARKS_FOUND
        }

        /// <summary>
        /// Determines current phase of auto parking algorithm.
        /// </summary>
        public enum AutoParkingPhase : byte
        {
            /// <summary>
            /// Idle phase - waiting for start. 
            /// </summary>
            WAITING_FOR_START = 0,

            /// <summary>
            /// Wall-following phase.
            /// </summary>
            WALL_FOLLOWING,

            /// <summary>
            /// Backward drive.
            /// </summary>
            GOING_BACKWARDS,

            /// <summary>
            /// Trying to find marks.
            /// </summary>
            MARKS_TRACKING,

            /// <summary>
            /// Driving to next waypoint.
            /// </summary>
            PATH_FOLLOWING,

            /// <summary>
            /// Algorithm ended.
            /// </summary>
            END
        }

        /// <summary>
        /// Determines whether auto parking phase is in error state.
        /// </summary>
        public enum PhaseError : byte
        {
            /// <summary>
            /// Error occured.
            /// </summary>
            ERROR = 0,

            /// <summary>
            /// No error occured.
            /// </summary>
            NO_ERROR
        }
        
        /// <summary>
        /// Wheelchair size in mm.
        /// </summary>
        private const Int32 WHEELCHAIR_SIDE_SIZE_MM = 600;

        #endregion

        #region Private variables

        /// <summary>
        /// User response when mark confirmation via WiFi is demanded.
        /// </summary>
        private UserResponseCode userResponse = UserResponseCode.END;

        /// <summary>
        /// True if any of the wheelchair's regulators is active.
        /// </summary>
        private Boolean regulationInProgress = false;

        /// <summary>
        /// Stores current phase of the auto praking algorithm.
        /// </summary>
        private AutoParkingPhase currentPhase = AutoParkingPhase.WAITING_FOR_START;

        /// <summary>
        /// Wheelchair X coordinate in mm.
        /// </summary>
        private Int32 xCoordinate = 0;

        /// <summary>
        /// Wheelchair Y coordinate in mm.
        /// </summary>
        private Int32 yCoordinate = 0;

        /// <summary>
        /// Wheelchair phi in mrad.
        /// </summary>
        private Int32 phi = 0;

        #endregion

        #region Public variables and properties

        /// <summary>
        /// X coordinate property getter.
        /// </summary>
        public Int32 X
        { get { return xCoordinate; } }

        /// <summary>
        /// Y coordinate property getter.
        /// </summary>
        public Int32 Y
        { get { return yCoordinate; } }

        /// <summary>
        /// Phi property getter.
        /// </summary>
        public Int32 Phi
        { get { return phi; } }

        /// <summary>
        /// Current phase property getter / setter.
        /// </summary>
        public AutoParkingPhase CurrentPhase
        { 
            get { return currentPhase; }
            set { currentPhase = value; }
        }

        /// <summary>
        /// True if any of the wheelchair's regulators is active (property getter / setter).
        /// </summary>
        public Boolean IsRegulationActive
        {
            get { return regulationInProgress; }
            set { regulationInProgress = value; }
        }

        /// <summary>
        /// User response property getter / setter.
        /// </summary>
        /// <seealso cref="UserResponseCode"/>
        public UserResponseCode UserResponse
        {
            get { return userResponse; }
            set { userResponse = value; }
        }

        #endregion

        /// <summary>
        /// Constructor.
        /// </summary>
        public WheelchairModel()
        { /*Default constructor, private variables are initialized in declaration*/}

        /// <summary>
        /// Gets wheelchair-symbolizing rectangle in world coordinates. 
        /// </summary>
        /// <returns>Wheelchair-symbolizing rectangle.</returns>
        public PointF[] GetSurroundingRectangle()
        {
            List<PointF> tempList = new List<PointF>();
            Int32 wheelchairDiagonalHalf = (Int32)(WHEELCHAIR_SIDE_SIZE_MM * Math.Sqrt(2.0) * 0.5);
            Double piQuart = Math.PI / 4.0;

            //Add upper left point
            tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 - piQuart) * wheelchairDiagonalHalf),
                                    yCoordinate + (Int32)(Math.Cos(phi / 1000.0 - piQuart) * wheelchairDiagonalHalf)));

            //Add upper right point
            tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 + piQuart) * wheelchairDiagonalHalf),
                                    yCoordinate + (Int32)(Math.Cos(phi / 1000.0 + piQuart) * wheelchairDiagonalHalf)));

            //Add lower right point
            tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 + (3 * piQuart)) * wheelchairDiagonalHalf),
                                    yCoordinate + (Int32)(Math.Cos(phi / 1000.0 + ( 3 * piQuart)) * wheelchairDiagonalHalf)));

            //Add lower left point
            tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 + (5 * piQuart)) * wheelchairDiagonalHalf),
                                    yCoordinate + (Int32)(Math.Cos(phi / 1000.0 + (5 * piQuart)) * wheelchairDiagonalHalf)));

            return tempList.ToArray();
        }

        /// <summary>
        /// Gets wheelchair-symbolizing rectangle in world coordinates.
        /// Wheelchair coordinates are considered to be [0(x);0(y);0(phi)]
        /// </summary>
        /// <returns>Wheelchair-symbolizing rectangle.</returns>
        public PointF[] GetDefaultSurroundingRectangle()
        {
            List<PointF> tempList = new List<PointF>();
            Int32 wheelchairDiagonalHalf = (Int32)(WHEELCHAIR_SIDE_SIZE_MM * Math.Sqrt(2.0) * 0.5);
            Double piQuart = Math.PI / 4.0;

            //Add upper left point
            tempList.Add(new PointF((Int32)(Math.Sin(-piQuart) * wheelchairDiagonalHalf),
                                    (Int32)(Math.Cos(-piQuart) * wheelchairDiagonalHalf)));

            //Add upper right point
            tempList.Add(new PointF((Int32)(Math.Sin( piQuart) * wheelchairDiagonalHalf),
                                    (Int32)(Math.Cos(piQuart) * wheelchairDiagonalHalf)));

            //Add lower right point
            tempList.Add(new PointF((Int32)(Math.Sin((3 * piQuart)) * wheelchairDiagonalHalf),
                                    (Int32)(Math.Cos((3 * piQuart)) * wheelchairDiagonalHalf)));

            //Add lower left point
            tempList.Add(new PointF((Int32)(Math.Sin((5 * piQuart)) * wheelchairDiagonalHalf),
                                    (Int32)(Math.Cos((5 * piQuart)) * wheelchairDiagonalHalf)));

            return tempList.ToArray();
        }

        /// <summary>
        /// Actualize wheelchair coordinates.
        /// </summary>
        /// <param name="X">X coordinate [cm].</param>
        /// <param name="Y">Y coordinate [cm].</param>
        /// <param name="Phi">Phi [mrad].</param>
        public void SetCoordinates(Int32 X, Int32 Y, Int32 Phi)
        {
            xCoordinate = X;
            yCoordinate = Y;
            phi = Phi;
        }
    }

    /// <summary>
    /// World map.
    /// </summary>
    public class WorldMap
    {
        /// <summary>
        /// Enum to specify mark position.
        /// </summary>
        public enum MarkPosition : byte
        {
            /// <summary>
            /// Left mark.
            /// </summary>
            LEFT = 0,

            /// <summary>
            /// Right mark.
            /// </summary>
            RIGHT = 1,

            /// <summary>
            /// Marks count.
            /// </summary>
            MARKS_COUNT
        };

        /// <summary>
        /// List of detected marks.
        /// </summary>
        private List<Mark> marks;

        /// <summary>
        /// List of wheelchair's track points.
        /// </summary>
        private List<TrackPoint> trackPoints;

        /// <summary>
        /// Marks list getter.
        /// </summary>
        public List<Mark> Marks
        {
            get
            {
                return marks;
            }
        }

        /// <summary>
        /// Track points getter.
        /// </summary>
        public List<TrackPoint> TrackPoints
        {
            get
            {
                return trackPoints;
            }
        }

        /// <summary>
        /// Default constructor.
        /// </summary>
        public WorldMap()
        {
            //Marks
            marks = new List<Mark>((Int32)MarkPosition.MARKS_COUNT);

            for (Byte i = 0; i < (Byte)MarkPosition.MARKS_COUNT; i++)
                marks.Add(new Mark());

            //Track points
            trackPoints = new List<TrackPoint>();
        }

        /// <summary>
        /// Modify parameters of the specified mark.
        /// </summary>
        /// <param name="position">Mark position.</param>
        /// <param name="coordinates">Mark coordinates.</param>
        /// <param name="color">Mark color.</param>
        public void ModifyMark(MarkPosition position, PointF coordinates, Color color)
        {
            //Modify marks coordinates
            marks[(Int32)position].Coordinates = coordinates;
            marks[(Int32)position].MarkColor = color;
            
            //Recalculate environment coordinates
            RecalculateTrack();
            RecalculateCarRectangle();
            RecalculatePlatformRectangle();
        }

        /// <summary>
        /// Recalculate track points' coordinates.
        /// </summary>
        private void RecalculateTrack()
        {
            //Car angle, width and marks' centre
            Double angle = Math.Atan2((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y),
                (marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X));

            Double width = Math.Sqrt((Math.Pow((marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X), 2.0))
                                     + (Math.Pow((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y), 2.0)));

            PointF centre = new PointF((marks[(Int32)MarkPosition.LEFT].Coordinates.X + marks[(Int32)MarkPosition.RIGHT].Coordinates.X) / 2.0f,
                                       (marks[(Int32)MarkPosition.LEFT].Coordinates.Y + marks[(Int32)MarkPosition.RIGHT].Coordinates.Y) / 2.0f);

            trackPoints.Clear();
         
            trackPoints.Add(new TrackPoint(new PointF((float)(centre.X - Math.Sin(angle) * Car.PLATFORM_LENGTH / 2.0),
                                                       (float)(centre.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH / 2.0))));

            trackPoints.Add(new TrackPoint(new PointF((float)(centre.X - Math.Sin(angle) * Car.PLATFORM_LENGTH * 1.5),
                                                       (float)(centre.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH * 1.5))));

            trackPoints.Add(new TrackPoint(new PointF((float)(trackPoints[1].Coordinates.X + Math.Cos(angle) * width),
                                                       (float)(trackPoints[1].Coordinates.Y + Math.Sin(angle) * width))));
        }

        /// <summary>
        /// Recalculates the coordinates of car-defining rectangle
        /// </summary>
        private void RecalculateCarRectangle()
        {
            //Car angle
            Double angle = Math.Atan2((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y),
                (marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X));

            //Rectangle corners
            Car.CarRectangle[3] = new PointF(marks[(Int32)MarkPosition.RIGHT].Coordinates.X, marks[(Int32)MarkPosition.RIGHT].Coordinates.Y);
            Car.CarRectangle[2] = new PointF(marks[(Int32)MarkPosition.LEFT].Coordinates.X, marks[(Int32)MarkPosition.LEFT].Coordinates.Y);

            Car.CarRectangle[1] = new PointF((float)(marks[(Int32)MarkPosition.LEFT].Coordinates.X + Math.Sin(angle) * Car.CAR_LENGTH),
                                                    (float)(marks[(Int32)MarkPosition.LEFT].Coordinates.Y - Math.Cos(angle) * Car.CAR_LENGTH));

            Car.CarRectangle[0] = new PointF((float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.X + Math.Sin(angle) * Car.CAR_LENGTH),
                                                    (float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.Y - Math.Cos(angle) * Car.CAR_LENGTH));
        }

        /// <summary>
        /// Recalculates the coordinates of platform-defining rectangle
        /// </summary>
        private void RecalculatePlatformRectangle()
        {
            //Car angle
            Double angle = Math.Atan2((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y),
                (marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X));

            //Rectangle corners
            Car.PlatformRectangle[0] = new PointF(marks[(Int32)MarkPosition.RIGHT].Coordinates.X, marks[(Int32)MarkPosition.RIGHT].Coordinates.Y);
            Car.PlatformRectangle[1] = new PointF(marks[(Int32)MarkPosition.LEFT].Coordinates.X, marks[(Int32)MarkPosition.LEFT].Coordinates.Y);

            Car.PlatformRectangle[2] = new PointF((float)(marks[(Int32)MarkPosition.LEFT].Coordinates.X - Math.Sin(angle) * Car.PLATFORM_LENGTH),
                                                    (float)(marks[(Int32)MarkPosition.LEFT].Coordinates.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH));

            Car.PlatformRectangle[3] = new PointF((float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.X - Math.Sin(angle) * Car.PLATFORM_LENGTH),
                                                    (float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH));
        }
    }
}
