using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Threading.Tasks;
using Interop;
using OpenTK;

namespace Fuseren
{
    public class PlayerState
    {
        public Vector2d Direction;
        public Vector2d Position;
        public Stack<TilePos> Path;
    }

    class Navigation
    {
        readonly Task<Heatmap> _heatmap;
        readonly GameInfo _gameInfo;
        readonly MapInfo _mapInfo;
        bool _useHeatmap = false;
        Bitmap _heatmapCache;
        int _lastTarget = -1;
        int _nextCheckpoint = 0;
        PlayerState _stateCopy = new PlayerState();

        public Bitmap CurrentBitmap 
        {
            get { return _heatmapCache; } 
        }

        public PlayerState OldState
        {
            get { return _stateCopy; }
        }

        public Navigation(Task<Heatmap> heatmap, GameInfo gameInfo)
        {
            _heatmap = heatmap;
            _gameInfo = gameInfo;
            _mapInfo = gameInfo.MapInfo;            

            _heatmap.ContinueWith(
                t =>
                {
                    _useHeatmap = true;
                    _heatmapCache = t.Result.ToBitmap();
                });
        }

        public int Steer(double dt, GameState gameState)
        {            
            if (_useHeatmap)
            {
                return SteerWithHeatmap(dt, _heatmap.Result, gameState);
            }
            else
            {
	            return SteerWithoutHeatmap(dt, gameState);
            }
        }

        public Stack<TilePos> CurrentPath; 

        int SteerWithHeatmap(double dt, Heatmap heatmap, GameState gameState)
        {
            var myself = gameState.Cars[_gameInfo.PlayerId];
            var pos = myself.Pos.Convert();
            var vel = myself.Velocity.Convert();
            var dir = myself.Direction.Convert();
            var curTile = GetTileIdFrom(pos);           
            var expectedPos = pos + vel * dt;

            TilePos nextTile;
            if (CurrentPath == null || CurrentPath.Count == 0 || curTile == CurrentPath.Last())
            {
                nextTile = GetNextCheckpoint(curTile);
                CurrentPath = CalcPath(curTile, nextTile);
            }
            else
            {
                nextTile = CurrentPath.Peek();
            }

            if (curTile == nextTile && CurrentPath.Count > 0)
            {
                CurrentPath.Pop();
                if(CurrentPath.Count > 0)
                    nextTile = CurrentPath.Peek();
            }
            
	        var wantedDir = Vector2d.Normalize(nextTile.GetAsVector2(_mapInfo.TileWidth, _mapInfo.TileHeight) - curTile.GetAsVector2(_mapInfo.TileWidth, _mapInfo.TileHeight));
			wantedDir += DirectionBasedOnPressure(heatmap, ItemsOnMap(gameState), (_mapInfo.TileWidth + _mapInfo.TileHeight) / 2, wantedDir, new Point((int)expectedPos.X, (int)expectedPos.Y)); ;

            if(double.IsNaN(wantedDir.X) || double.IsNaN(wantedDir.Y))
                throw new Exception("What, Nan!");

            var newDir = wantedDir;//new Vector2d(nextNode.X, nextNode.Y) - (pos + vel * dt);
            newDir.Normalize();

            var numDegreesToRotate = dir.AngleBetween(ref newDir);
            var steerKeys = 0;

            if (numDegreesToRotate < -2)
            {
                steerKeys |= 1 << 2;
            }

            if (numDegreesToRotate > 2)
            {
                steerKeys |= 1 << 3;
            }

			if (Math.Abs(numDegreesToRotate) > 10 && Math.Abs(numDegreesToRotate) < 30)
			{
				steerKeys |= 1 << 0;
				steerKeys |= 1 << 4;
			}

            if (Math.Abs(numDegreesToRotate) < 5)
				steerKeys |= 1 << 0;			

            if (double.IsNaN(newDir.X) || double.IsNaN(newDir.Y))
            {
                newDir = new Vector2d(0, 0);
            }

			_stateCopy = new PlayerState() { Direction = wantedDir*vel.Length, Position = pos, Path = CurrentPath };

            return steerKeys;
        }

        TilePos GetNextCheckpoint(TilePos curTile)
        {
            if (_mapInfo.Path[_nextCheckpoint].Convert() == curTile)
            {
                ++_nextCheckpoint;
                if (_nextCheckpoint >= _mapInfo.Path.Count)
                    _nextCheckpoint = 0;
            }

            return _mapInfo.Path[_nextCheckpoint].Convert();
        }

        Vector2d DirectionBasedOnPressure(Heatmap heatmap, List<Rectangle> items, int radius, Vector2d dir, Point pos)
        {
            var rect = new Rectangle(pos.X - radius, pos.Y - radius, radius * 2, radius * 2);
            var sumForce = Vector2d.Zero; 
            for (var x = rect.Left; x < rect.Right;x += 4)
            {
                for (var y = rect.Top; y < rect.Bottom;y += 4)
                {
	                var p = new Point(x, y);

					if (p == pos)
						continue;

	                if (_mapInfo.IsInvalidPoint(p))
	                {
		                var toTargetOut = new Vector2d(x, y) - new Vector2d(pos.X, pos.Y);
						sumForce -= Vector2d.Normalize(toTargetOut) / toTargetOut.Length;  
  						continue;
	                }

	                var target = new Vector2d(x, y);
					var toTarget = target - new Vector2d(pos.X, pos.Y);
	                var dot = Vector2d.Dot(dir, toTarget);
					if(dot < 0)
						continue;

					var forceFromItems = ForceFromItems(items, target);
	                var desireability = heatmap.GetDesirability(x, y);
	                desireability += forceFromItems;

					var force = ((desireability - 0.5) * 2 *
						toTarget.Normalized()) / toTarget.Length;
					sumForce += force * 4;
                }
            }

            return Vector2d.Normalize(sumForce);
        }

	    double ForceFromItems(List<Rectangle> items, Vector2d p)
	    {
		    double power = 0;
		    foreach (var item in items)
		    {
				if(!item.Contains((int)p.X, (int)p.Y))
					continue;

			    power -= 0.3;
		    }

		    return power;
	    }

	    static List<Rectangle> ItemsOnMap(GameState gameState)
	    {
		    var items = new List<Rectangle>();

			foreach (var item in gameState.Items)
			{
				items.Add(new Rectangle(item.X-1, item.Y-1, item.Width+1, item.Height+1));
			}

			return items;
	    }

        int SteerWithoutHeatmap(double dt, GameState gameState)
        {
            var myself = gameState.Cars[_gameInfo.PlayerId];
            var pos = myself.Pos.Convert();
            var vel = myself.Velocity.Convert();
            var dir = myself.Direction.Convert();

            var curTile = GetTileIdFrom(pos);
			
			TilePos nextTile;

			if (CurrentPath == null || CurrentPath.Count == 0 || curTile == CurrentPath.Last())
			{
				nextTile = GetNextCheckpoint(curTile);
				CurrentPath = CalcPath(curTile, nextTile);
			}
			else
			{
				nextTile = CurrentPath.Peek();
			}

			if (curTile == nextTile && CurrentPath.Count > 0)
			{
				CurrentPath.Pop();
				if (CurrentPath.Count > 0)
					nextTile = CurrentPath.Peek();
			}

            var newDir = nextTile.GetAsVector2(_mapInfo.TileWidth, _mapInfo.TileHeight) - (pos + vel * dt);
            newDir.Normalize();

            var numDegreesToRotate = dir.AngleBetween(ref newDir);
            var steerKeys = 0;

            if (Math.Abs(numDegreesToRotate) > 10)
            {
                if (numDegreesToRotate < -3)
                {
                    steerKeys |= 1 << 2;
                }

                if (numDegreesToRotate > 3)
                {
                    steerKeys |= 1 << 3;
                }
            }
            else
                steerKeys |= 1 << 0;

            return steerKeys;
        }

        TilePos GetTileIdFrom(Vector2d position)
        {
            return new TilePos((int)(position.X / _mapInfo.TileWidth),
                               (int)(position.Y / _mapInfo.TileHeight));
        }

        public Stack<TilePos> CalcPath(TilePos pos, TilePos target)
        {
            var size = _mapInfo.GetMapSize();

            var shortestPathTree = new Edge[size.Width, size.Height];
            var searchFrontier = new Edge[size.Width, size.Height];
            var nodes = new Node[size.Width, size.Height];

            for (var x = 0; x < nodes.GetLength(0); ++x)
            {
                for (var y = 0; y < nodes.GetLength(1); ++y)
                {
                    nodes[x, y] = new Node(new TilePos(x, y));
                }
            }

            var pq = new HeapPriorityQueue<Node>(size.Width * size.Height);
			if(pos.X < 0 || pos.Y < 0 || pos.X >= size.Width || pos.Y >= size.Height)
				return new Stack<TilePos>();

            pq.Enqueue(nodes[pos.X, pos.Y], GetCostBasedOnType(_mapInfo.GetTileType(pos)));

            var foundTarget = false;

            while (pq.Count != 0)
            {
                var nextNode = pq.Dequeue();

                shortestPathTree[nextNode.Position.X, nextNode.Position.Y] =
                    searchFrontier[nextNode.Position.X, nextNode.Position.Y];

                if (nextNode.Position == target)
                {
                    foundTarget = true;
                    break;
                }

	            var curType = _mapInfo.GetTileType(nextNode.Position);
	            for (var i = 0; i < 4; ++i)
                {
                    var xOffset = (i % 2) == 0 ? (i - 1) : 0;
                    var yOffset = (i % 2) == 0 ? 0 : (i - 2);

                    var p = new TilePos(nextNode.Position.X + xOffset, nextNode.Position.Y + yOffset);
                    if (p == nextNode.Position || p.X < 0 || p.Y < 0 || p.X >= size.Width || p.Y >= size.Height)
                        continue;

                    var cost = nextNode.Priority + GetCostBasedOnType(_mapInfo.GetTileType(p)) + GetCostToTarget(nextNode.Position, curType, p) + 1;
                    if (searchFrontier[p.X, p.Y] == null)
                    {
                        nodes[p.X, p.Y].Priority = cost;
                        pq.Enqueue(nodes[p.X, p.Y], cost);
                        searchFrontier[p.X, p.Y] = new Edge(nextNode.Position, p);
                    }
                    else if (cost < nodes[p.X, p.Y].Priority && shortestPathTree[p.X, p.Y] == null)
                    {
                        nodes[p.X, p.Y].Priority = cost;
                        pq.UpdatePriority(nodes[p.X, p.Y], cost);
                        searchFrontier[p.X, p.Y] = new Edge(nextNode.Position, p);
                    }
                }
            }

            var path = new Stack<TilePos>();
            if (!foundTarget)
                return path;

            var nd = target;
            path.Push(nd);

            while (nd != pos && shortestPathTree[nd.X, nd.Y] != null)
            {
                nd = shortestPathTree[nd.X, nd.Y].Start;
                path.Push(nd);
            }

            return path;
        }

        public static double GetCostBasedOnType(TileType tileType)
        {
            switch (tileType)
            {
                case TileType.Empty:
                    return 10;
                default:
                    return 0;
            }
        }

	    public static double GetCostToTarget(TilePos source, TileType sourceType, TilePos target)
	    {
		    if (sourceType == TileType.VerticalStraight)
		    {
			    if (Math.Abs(source.Y - target.Y) == 0)
				    return 1;
		    }
			else if (sourceType == TileType.HorizontalStraight)
			{
				if (Math.Abs(source.X - target.X) == 0)
					return 1;				
			}
			else if (sourceType == TileType.UpCorner)
			{
				if (source.X > target.X || source.Y < target.Y)
					return 1;
			}
			else if (sourceType == TileType.DownCorner)
			{
				if (source.X < target.X || source.Y > target.Y)
					return 1;
			}
			else if (sourceType == TileType.RightCorner)
			{
				if (source.X > target.X || source.Y > target.Y)
					return 1;
			}
			else if (sourceType == TileType.LeftCorner)
			{
				if (source.X < target.X || source.Y < target.Y)
					return 1;
			}

		    return 0;
	    }

        class Edge
        {
            public TilePos Start;
            public TilePos End;

            public Edge(TilePos start, TilePos end)
            {
                Start = start;
                End = end;             
            }
        }

        class Node : PriorityQueueNode
        {
            public TilePos Position;

            public Node(TilePos pos)
            {
                Position = pos;
            }
        }
    }
}