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

namespace AtariRobot.Kaboom
{
    public class BombTracker
    {
        public BombTracker()
        {
            Bombs = new List<BombList>();
        }
        public List<BombList> Bombs { get; set; }

        public void UpdateBombPostions(KaboomScreen screen)
        {
            int trackedBombIndex = 0;
            List<BombList> bombsToRemove = new List<BombList>();
            for (int screenBombIndex = 0; screenBombIndex < screen.Bombs.Count; screenBombIndex++, trackedBombIndex++)
            {
                if (trackedBombIndex >= Bombs.Count)
                {
                    Bombs.Add(new BombList());
                }
                TemplateMatch screenBomb = screen.Bombs[screenBombIndex];

                BombList candidateBomb = Bombs[trackedBombIndex];

                if (candidateBomb.BombPositions.Count < 3)
                {
                    if (candidateBomb.BombPositions.Count > 0)
                    {
                        if (Math.Abs(screenBomb.Location.X - candidateBomb.BombPositions[0].X) < _bombXEpsilon)
                        {
                            candidateBomb.AddBomb(screenBomb.Location);
                        }
                    }
                    else
                    {
                        candidateBomb.AddBomb(screenBomb.Location);
                    }
                }
                else
                {
                    CvPoint fwdPoint = candidateBomb.ProjectNextPosition();
                    if (candidateBomb.IsPositionConsistent(screenBomb.Location))
                    {
                        candidateBomb.AddBomb(screenBomb.Location);
                    }
                    else
                    {
                        screenBombIndex--;
                        if (BombLikelyCaught(screen.Buckets, fwdPoint))
                        {
                            bombsToRemove.Add(candidateBomb);
                        }
                        else
                        {
                            candidateBomb.AddBomb(fwdPoint);
                        }
                    }
                }
            }

            foreach (BombList remove in bombsToRemove)
            {
                Bombs.Remove(remove);
            }
        }

        protected int _bombXEpsilon = 3;
        protected int _bucketXEpsilon = 10;
        protected int _bucketYEpsilon = 5;
        public bool BombLikelyCaught(List<TemplateMatch> buckets, CvPoint position)
        {
            foreach (TemplateMatch bucket in buckets)
            {
                if (position.X < bucket.Location.X + _bucketXEpsilon && position.X > bucket.Location.X - _bucketXEpsilon
                    && position.Y < bucket.Location.Y + _bucketYEpsilon && position.Y > bucket.Location.Y - _bucketYEpsilon)
                {
                    return true;
                }
            }

            return false;
        }

        public void Clear()
        {
            Bombs.Clear();
        }

        public void Display(IplImage image)
        {
            for(int listIndex = 0; listIndex < Bombs.Count; listIndex++)
            {
                BombList list = Bombs[listIndex];
                for (int bombIndex = 0; bombIndex < list.BombPositions.Count; bombIndex++)
                {
                    CvPoint position = list.BombPositions[bombIndex];

                    image.Circle(position, 2, CvColor.Gray);
                    image.PutText(string.Format("{0}-{1}", listIndex, bombIndex), position, Helper.SmallFont, CvColor.Gray);
                }
            }
        }
    }

    public class BombList
    {
        public BombList()
        {
            BombPositions = new List<CvPoint>();
        }

        protected int _epsilon = 3;

        public List<CvPoint> BombPositions { get; set; }

        public CvPoint ProjectNextPosition()
        {
            CvPoint avgVelocity = GetAverageVelocity();
            return BombPositions[0] + avgVelocity;
        }

        public bool IsPositionConsistent(CvPoint position)
        {
            CvPoint next = ProjectNextPosition();
            if (Math2.Distance(position.X, position.Y, next.X, next.Y) < _epsilon)
            {
                return true;
            }

            return false;
        }

        public CvPoint GetAverageVelocity()
        {
            double differenceX = 0;
            double differenceY = 0;
            for (int bombIndex = 1; bombIndex < BombPositions.Count; bombIndex++)
            {
                CvPoint current = BombPositions[bombIndex];
                CvPoint last = BombPositions[bombIndex - 1];
                differenceX += last.X - current.X;
                differenceY += last.Y - current.Y;
            }

            return new CvPoint((int)(differenceX / (BombPositions.Count - 1)), (int)(differenceY / (BombPositions.Count - 1)));
        }


        public void AddBomb(CvPoint position)
        {
            BombPositions.Insert(0, position);
        }
    }
}