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

namespace AtariRobot.Pong
{
    public class PongLogic : IGameLogic
    {
        public PongLogic(string comPort)
        {
            _paddle = new NxtPaddleController(comPort);
        }

        protected float GetSlope(CvPoint2D32f pt1, CvPoint2D32f pt2)
        {
            if (pt1.X == pt2.X)
            {
                return float.PositiveInfinity;
            }

            return (float)(pt2.Y - pt1.Y) / (float)(pt2.X - pt1.X);
        }

        protected List<CvPoint2D32f> CalculateForwardIntersections(PongScreen screen, float slope, BallDirection direction)
        {
            List<CvPoint2D32f> intersections = new List<CvPoint2D32f>();
            CvPoint2D32f lastPoint = screen.Ball.Center;
            intersections.Add(lastPoint);

            int paddlePlaneX = direction == BallDirection.Left ? (screen.LeftPaddle.Box.X + screen.LeftPaddle.Box.Width) : screen.RightPaddle.Box.X;
            if (slope == 0 || double.IsInfinity(slope) || double.IsNaN(slope))
            {
                intersections.Add(new CvPoint2D32f(paddlePlaneX, lastPoint.Y));
            }
            else
            {
                bool foundPaddle = false;

                while (!foundPaddle)
                {
                    if (intersections.Count > 30) { break; }

                    int boundaryPlaneY = 0;
                    if (slope >= 0)
                    {
                        if (direction == BallDirection.Left)
                        {
                            boundaryPlaneY = (screen.TopBorder.Box.Y + screen.TopBorder.Box.Height);
                        }
                        else
                        {
                            boundaryPlaneY = screen.BottomBorder.Box.Y;
                        }
                    }
                    else
                    {
                        if (direction == BallDirection.Left)
                        {
                            boundaryPlaneY = screen.BottomBorder.Box.Y;
                        }
                        else
                        {
                            boundaryPlaneY = (screen.TopBorder.Box.Y + screen.TopBorder.Box.Height);
                        }
                    }

                    CvPoint2D32f boundaryIntersection = FindBoundaryIntersection(lastPoint, slope, boundaryPlaneY);

                    if (direction == BallDirection.Left)
                    {
                        if (boundaryIntersection.X <= paddlePlaneX)
                        {
                            CvPoint2D32f paddlePlaneIntersection = FindPaddleIntersection(lastPoint, slope, paddlePlaneX);
                            intersections.Add(paddlePlaneIntersection);
                            foundPaddle = true;
                            lastPoint = paddlePlaneIntersection;
                        }
                        else
                        {
                            intersections.Add(boundaryIntersection);
                            slope = -slope;
                            lastPoint = boundaryIntersection;
                        }
                    }
                    else
                    {
                        if (boundaryIntersection.X >= paddlePlaneX)
                        {
                            CvPoint2D32f paddlePlaneIntersection = FindPaddleIntersection(lastPoint, slope, paddlePlaneX);
                            intersections.Add(paddlePlaneIntersection);
                            foundPaddle = true;
                            lastPoint = paddlePlaneIntersection;
                        }
                        else
                        {
                            intersections.Add(boundaryIntersection);
                            slope = -slope;
                            lastPoint = boundaryIntersection;
                        }
                    }
                }
            }

            return intersections;
        }

        protected CvPoint2D32f FindBoundaryIntersection(CvPoint2D32f pt, float slope, int horizontal)
        {
            if (double.IsInfinity(slope))
            {
                return new CvPoint2D32f(pt.X, horizontal);
            }

            if (slope == 0)
            {
                return new CvPoint2D32f(0, 0);
            }

            float b = pt.Y - (slope * pt.X);

            float x = (horizontal - b) / slope;

            return new CvPoint2D32f(x, horizontal);
        }

        protected CvPoint2D32f FindPaddleIntersection(CvPoint2D32f pt, float slope, int paddlePlane)
        {
            if (double.IsInfinity(slope))
            {
                return new CvPoint2D32f(0, 0);
            }

            if (slope == 0)
            {
                return new CvPoint2D32f(paddlePlane, pt.Y);
            }

            float b = pt.Y - (slope * pt.X);
            float y = slope * paddlePlane + b;

            return new CvPoint2D32f(paddlePlane, y);
        }

        protected CvPoint2D32f CalculateAveragePoint(List<CvPoint2D32f> points)
        {
            double sumX = 0;
            double sumY = 0;

            foreach (CvPoint2D32f point in points)
            {
                sumX += point.X;
                sumY += point.Y;
            }

            return new CvPoint2D32f((float)(sumX / points.Count), (float)(sumY / points.Count));
        }

        protected double Distance(CvPoint2D32f pt1, CvPoint2D32f pt2)
        {
            double yDelta = pt1.Y - pt2.Y;
            double xDelta = pt1.X - pt2.X;
            double distance = Math.Sqrt(yDelta * yDelta + xDelta * xDelta);

            return distance;
        }

        protected PongScreen[] ExcludeOutliers(PongScreen[] screens)
        {
            //Maybe there's something to some of that Linq stuff after all...
            List<CvPoint2D32f> ballPoints = screens.Select(x => x.Ball.Center).ToList();
            return screens.Where(x => ballPoints.Contains(x.Ball.Center)).ToArray();
        }

        protected List<CvPoint2D32f> ExcludeOutliers(List<CvPoint2D32f> points)
        {
            //If you don't have enough points for anything useful, don't try to remove outliers.
            if (points.Count < 3)
            {
                return new List<CvPoint2D32f>(points);
            }

            //Hmmm...  I don't think this is actually mathematically sound.
            //By using the distance, I'm no longer dealing with a normal distribution,
            //so the whole standard deviation thing goes out the window.  Oh well.
            CvPoint2D32f averagePt = CalculateAveragePoint(points);

            List<PointDistance> pointDistances = new List<PointDistance>();
            foreach (CvPoint2D32f point in points)
            {
                PointDistance distance = new PointDistance();
                distance.Point = point;
                distance.Distance = Distance(point, averagePt);
                pointDistances.Add(distance);
            }

            double[] distances = pointDistances.Select(x => x.Distance).ToArray();
            double stdDev = Math2.StandardDeviation(distances);

            List<CvPoint2D32f> nonOutliers = new List<CvPoint2D32f>();
            foreach (PointDistance point in pointDistances)
            {
                if (point.Distance < stdDev * 1.96)
                {
                    nonOutliers.Add(point.Point);
                }
            }

            if (nonOutliers.Count > 0)
            {
                return nonOutliers;
            }
            else
            {
                return new List<CvPoint2D32f>(points);
            }
        }

        protected BallDirection GetHistoricalDirection(List<BallDirection> directions, int samples)
        {
            Dictionary<BallDirection, int> directionCount = new Dictionary<BallDirection, int>();
            foreach(BallDirection direction in directions)
            {
                if(!directionCount.ContainsKey(direction))
                {
                    directionCount[direction] = 0;
                }

                directionCount[direction]++;
            }

            int winnerCount = 0;
            BallDirection winnerDirection = BallDirection.Left;
            foreach (KeyValuePair<BallDirection, int> pair in directionCount)
            {
                if (pair.Value > winnerCount)
                {
                    winnerCount = pair.Value;
                    winnerDirection = pair.Key;
                }
            }

            return winnerDirection;            
        }

        protected Recognizer recognizer = new Recognizer();
        protected List<PongScreen> screenHistory = new List<PongScreen>();
        protected List<CvPoint2D32f> targetPointHistory = new List<CvPoint2D32f>();
        protected List<BallDirection> directionHistory = new List<BallDirection>();
        protected BallDirection lastDirection = BallDirection.Left;
        protected int uncalculatedFrameCount = 0;

        protected IPaddle _paddle;
        protected bool _paddleCalibrated;
        protected bool _calibrationMode;
        protected float _calibrationInitialY;
        protected int _calibrationAngle = 10;

        protected float _calibrationFactor;

        protected CvFont _font = new CvFont(FontFace.HersheyComplexSmall, .5, .5);


        public void Close()
        {
            //_paddle.Close();
        }

        public void HandleFrame(IplImage frame)
        {
            PongScreen screen = recognizer.Recognize(frame);
            screenHistory.Insert(0, screen);
            while (screenHistory.Count > 20) { screenHistory.RemoveAt(screenHistory.Count - 1); }

            while (directionHistory.Count > 20) { directionHistory.RemoveAt(directionHistory.Count - 1); }
            if (uncalculatedFrameCount > 3)
            {
                directionHistory.Clear();
                targetPointHistory.Clear();
            }
            uncalculatedFrameCount++;

            IplImage recognizedFrame = new IplImage(frame.Width, frame.Height, frame.Depth, frame.ElemChannels);
            recognizedFrame.Zero();
            frame.Copy(recognizedFrame);

            if (screen.TopBorder != null) { Helper.DrawRectangle(recognizedFrame, screen.TopBorder.Box, CvColor.White); }
            if (screen.BottomBorder != null) { Helper.DrawRectangle(recognizedFrame, screen.BottomBorder.Box, CvColor.White); }
            if (screen.LeftPaddle != null) { Helper.DrawRectangle(recognizedFrame, screen.LeftPaddle.Box, CvColor.Brown); }
            if (screen.RightPaddle != null) { Helper.DrawRectangle(recognizedFrame, screen.RightPaddle.Box, CvColor.Green); }
            if (screen.Ball != null) { Helper.DrawRectangle(recognizedFrame, screen.Ball.Box, CvColor.Cyan); }

            PongScreen[] screensWithBall = screenHistory.Where(x => x.Ball != null).ToArray();
            screensWithBall = ExcludeOutliers(screensWithBall);

            if (screensWithBall.Length > 2)
            {
                int samplePoints = 3;

                PongScreen firstFrame = screensWithBall[0];
                PongScreen secondFrame = screensWithBall[1];
                Helper.DrawRectangle(recognizedFrame, firstFrame.Ball.Box, CvColor.Orange);
                Helper.DrawRectangle(recognizedFrame, secondFrame.Ball.Box, CvColor.Pink);

                BallDirection direction = firstFrame.Ball.Center.X < secondFrame.Ball.Center.X ? BallDirection.Left : BallDirection.Right;
                directionHistory.Insert(0, direction);

                if (direction != GetHistoricalDirection(directionHistory, samplePoints))
                {
                    return;
                }

                float slope = GetSlope(firstFrame.Ball.Center, secondFrame.Ball.Center);

                //Skip the first two frames.
                for (int frameIndex = 2; frameIndex < samplePoints && frameIndex < screensWithBall.Length; frameIndex++)
                {
                    PongScreen currentFrame = screensWithBall[frameIndex];
                    Helper.DrawRectangle(recognizedFrame, currentFrame.Ball.Box, CvColor.Gray);
                    slope += GetSlope(firstFrame.Ball.Center, currentFrame.Ball.Center);
                    slope = slope / 2.0f;
                }

                List<CvPoint2D32f> forwardIntersectionPoints = CalculateForwardIntersections(firstFrame, slope, direction);
                for (int intersectionIndex = 0; intersectionIndex < forwardIntersectionPoints.Count - 1; intersectionIndex++)
                {
                    recognizedFrame.Line(forwardIntersectionPoints[intersectionIndex], forwardIntersectionPoints[intersectionIndex + 1], Helper.KnownColors[intersectionIndex % Helper.KnownColors.Length]);
                }

                if (direction != lastDirection)
                {
                    lastDirection = direction;
                    targetPointHistory.Clear();
                }

                targetPointHistory.Add(forwardIntersectionPoints.Last());

                foreach (CvPoint2D32f historicalPoint in targetPointHistory)
                {
                    recognizedFrame.Circle(historicalPoint, 1, CvColor.Gray);
                }

                CvPoint2D32f targetPoint = CalculateAveragePoint(ExcludeOutliers(targetPointHistory));
                recognizedFrame.Circle(targetPoint, (int)(20.0 / (targetPointHistory.Count * .25)), CvColor.Red);

                int limitedHistoryLength = Math.Min(targetPointHistory.Count, 20);
                List<CvPoint2D32f> limitedHistory = targetPointHistory.GetRange(targetPointHistory.Count - limitedHistoryLength, limitedHistoryLength);
                CvPoint2D32f limitedTargetPoint = CalculateAveragePoint(ExcludeOutliers(limitedHistory));
                recognizedFrame.Circle(limitedTargetPoint, (int)(20.0 / (limitedHistory.Count)), CvColor.Orange);

                targetPoint = limitedTargetPoint;

                if (!_paddleCalibrated)
                {
                    if (!_calibrationMode)
                    {
                        _calibrationMode = true;

                        float playfieldCenter = Math.Abs(firstFrame.TopBorder.Center.Y - firstFrame.BottomBorder.Center.Y) / 2.0f;
                        int calibrationDirection = (firstFrame.RightPaddle.Center.Y < playfieldCenter ? -1 : 1);
                        _calibrationInitialY = firstFrame.RightPaddle.Center.Y;
                        _paddle.Rotate(NxcMotorPort.OUT_A, (sbyte)(50 * calibrationDirection), (short)_calibrationAngle);
                    }
                    else
                    {
                        if (!_paddle.Busy)
                        {
                            float delta = Math.Abs(_calibrationInitialY - firstFrame.RightPaddle.Center.Y);

                            _calibrationFactor = delta / (float)_calibrationAngle;

                            Console.WriteLine("D: {0}  A: {1} F: {2}", delta, _calibrationAngle, _calibrationFactor);

                            _calibrationMode = false;
                            _paddleCalibrated = true;
                        }
                    }
                }

                if (direction == BallDirection.Right && targetPointHistory.Count > 5)
                {
                    float centerDifference = Math.Abs(targetPoint.Y - firstFrame.RightPaddle.Center.Y);
                    float playfieldHeight = Math.Abs(firstFrame.TopBorder.Center.Y - firstFrame.BottomBorder.Center.Y);

                    int degrees = (int)(centerDifference * _calibrationFactor);

                    RotationDirection rotationDirection = RotationDirection.None;
                    if (targetPoint.Y < firstFrame.RightPaddle.Box.Y)
                    {
                        rotationDirection = RotationDirection.Clockwise;
                    }
                    else if (targetPoint.Y > firstFrame.RightPaddle.Box.Y + firstFrame.RightPaddle.Box.Height)
                    {
                        rotationDirection = RotationDirection.CounterClockwise;
                    }

                    string rotationText = string.Format("d: {0} th: {1} p: {2} d: {3}", centerDifference, degrees, _paddle.Busy, rotationDirection);
                    Cv.PutText(recognizedFrame, rotationText, new CvPoint(0, 30), _font, CvColor.Green);

                    if(rotationDirection != RotationDirection.None)
                    {
                        int directionMult = rotationDirection == RotationDirection.CounterClockwise ? 1 : -1;
                        _paddle.Rotate(NxcMotorPort.OUT_A, (sbyte)(directionMult * 50), (short)degrees);
                    }

                }

                uncalculatedFrameCount = 0;
            }
            else
            {
                if (uncalculatedFrameCount > 20 && uncalculatedFrameCount < 60)
                {
                    _paddle.Rotate(NxcMotorPort.OUT_A, (sbyte)(-50), (short)_calibrationAngle);
                }
            }

            Cv.ShowImage("Recognized", recognizedFrame);
            recognizedFrame.Dispose();
        }
    }

    public enum BallDirection { Left, Right }

    public class PointDistance
    {
        public CvPoint2D32f Point { get; set; }
        public double Distance { get; set; }
    }
}