#define DEBUG
using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using SokobanResultSearch.BusinessEntities;

namespace SokobanResultSearch
{

    public class Astar
    {
        #region Private class fields

        /// <summary>
        /// Representation of the step directions in integer array.
        /// </summary>
        private static readonly int[] _directions = new int[] { 1, -1, 1 << 8, -1 << 8 };

        /// <summary>
        /// Representation of the step directions inverse in integer array.
        /// </summary>
        private static readonly int[] _directionsInverse = new int[] { -1, 1, -1 << 8, 1 << 8 };

        /// <summary>
        /// Representation of the directions in string.
        /// </summary>
        private static readonly string[] _tourDirections = new string[] { "R", "L", "D", "U" };

        #endregion

        #region Private fields

        /// <summary>
        /// Table in current state with the player and the boxes.
        /// </summary>
        private int[]                       _currentTable;

        /// <summary>
        /// Table in current state with the player and the boxes.
        /// </summary>
        private int[]                       _tourTable;

        /// <summary>
        /// Table without the boxes and the player.
        /// </summary>
        private int[]                       _emptyTable;

        /// <summary>
        /// Representation of the goal positions in array.
        /// </summary>
        private int[]                       _goalPositions;

        /// <summary>
        /// List of the wrong places.
        /// </summary>
        private int[]                       _wrongPlaces;

        /// <summary>
        /// Map of the distance without obstacles for each goal state.
        /// </summary>
        private int[, ,]                    _distancesMap;

        /// <summary>
        /// Count of  empty places.
        /// </summary>
        private int                         _emptyPositionsCount;

        /// <summary>
        /// Representation of the current box positions in string.
        /// This string is sorted.
        /// </summary>
        private string                      _currentBoxesPositions;

        /// <summary>
        /// Representation of the goal state in string.
        /// This string is sorted.
        /// </summary>
        private string                      _goalState;

        /// <summary>
        /// List of  close states.
        /// </summary>
        private Dictionary<string, int[]>   _history;

        /// <summary>
        /// List of  open states.
        /// </summary>
        private BinaryHeap                  _openStates;

        #endregion

        #region Constructors

        public Astar()
        {
            Initialize(
               new byte[,] {
               {3,3,3,3,3,3,3,3,3},
               {3,0,1,3,4,3,3,4,3},
               {3,0,0,3,0,3,3,0,3},
               {3,0,2,0,0,0,2,0,3},
               {3,0,0,3,2,3,0,0,3},
               {3,3,0,3,0,3,3,3,3},
               {3,3,0,0,0,0,0,4,3},
               {3,3,3,3,0,3,3,3,3},
               {3,3,3,3,3,3,3,3,3}
               });

        }

        public Astar(byte[,] table)
        {
            Initialize(table);
        }

        #endregion

        #region Private members

        /// <summary>
        /// Search the solution.
        /// </summary>
        public virtual State Search()
        {
            return Expand();
        }

        #endregion

        #region Private methods

        /// <summary>
        /// Initializes this class.
        /// </summary>
        private void Initialize(byte[,] table)
        {
            _history                = new Dictionary<string, int[]>();
            _openStates             = new BinaryHeap();

            State firstState        = InitializationFactory.InitializationProvider.GetStartState(table);
            AddToHistroy(firstState.PlayerPosition, firstState.BoxesPositions);
            _openStates.Push(firstState);

            _emptyTable             = InitializationFactory.InitializationProvider.GetEmptyTable(table, firstState.PlayerPosition);
            _emptyPositionsCount    = InitializationFactory.InitializationProvider.GetEmptyPositionsCount(_emptyTable);
            _goalPositions          = InitializationFactory.InitializationProvider.GetGoalPositions(table);
            _goalState              = InitializationFactory.InitializationProvider.ConvertGoalPositonsToString(_goalPositions);
            _wrongPlaces            = InitializationFactory.InitializationProvider.GetWrongPlaces(_emptyTable, _goalPositions);
            _distancesMap           = InitializationFactory.InitializationProvider.GetNoObstacleDistance(_emptyTable, _goalState, _emptyPositionsCount);

            #if DEBUG 
            Console.WriteLine(string.Format("{0} {1}", Properties.Resources.str_Empty, _emptyPositionsCount));
            Console.WriteLine(string.Format("{0} x: {1} y: {2}", new object[] {
                    Properties.Resources.str_Player, firstState.PlayerPosition >> 8, (byte)firstState.PlayerPosition}));
            Console.WriteLine();

            Console.WriteLine(Properties.Resources.str_GoalStates);
            PrintTable(_goalPositions);
            Console.WriteLine(Properties.Resources.str_EmptyTable);
            PrintTable(_emptyTable);
            Console.WriteLine(Properties.Resources.str_WrongPlaces);
            PrintTable(_wrongPlaces);
            PrintDistances(_distancesMap);
            #endif
        }

        /// <summary>
        /// Extends the start state and searches the solution.
        /// </summary>
        private State Expand()
        {
            int counter = 0;
            while (_openStates.Count > 0)
            {
                State currentState = _openStates.Pop();
                ++counter;
                if (GoalState(currentState.BoxesPositions))
                    return currentState;

                _currentBoxesPositions = currentState.BoxesPositions;
                _currentTable          = new int[_emptyTable.Length];
                Array.Copy(_emptyTable, _currentTable, _emptyTable.Length);
               
                for (int i = 0; i < _currentBoxesPositions.Length; i++)
                {
                    _currentTable[_currentBoxesPositions[i] >> 8] |= 1 << (byte)_currentBoxesPositions[i];
                }

               // PrintTable(_currentTable);

                string[,] directions = PlayerWalks(currentState.PlayerPosition);
                for (int i = 0; i < _currentBoxesPositions.Length; i++)
                {
                    for (int j = 0; j < _directions.Length; j++)
                    {
                        short newPosition       = (short)(_currentBoxesPositions[i] + _directions[j]);
                        short inversePosition   = (short)(_currentBoxesPositions[i] + _directionsInverse[j]);

                        if (ValidStep(newPosition, inversePosition) && !WrongPlace(newPosition))
                        {
                            _currentTable[_currentBoxesPositions[i] >> 8] ^= (1 << (byte)_currentBoxesPositions[i]);
                            _currentTable[newPosition >> 8] |= (1 << (byte)newPosition);

                            if (!DeadLock(newPosition, (short)(_directions[j] >> 8), (byte)_directions[j]))
                            {
                                _currentTable[newPosition >> 8] ^= (1 << (byte)newPosition);
                                _currentTable[_currentBoxesPositions[i] >> 8] |= (1 << (byte)_currentBoxesPositions[i]);

                                if ((_tourTable[inversePosition >> 8] & (1 << (byte)inversePosition)) != 0)
                                {
                                    string boxesPositions = InserPosition(newPosition, (short)_currentBoxesPositions[i]);
                                    if (!AddToHistroy((short)_currentBoxesPositions[i], boxesPositions))
                                    {
                                        HeuristicsInsert(new State(boxesPositions, (short)_currentBoxesPositions[i],
                                            currentState.Directions + directions[inversePosition >> 8, (byte)inversePosition] + ","
                                             + _tourDirections[j]));
                                    }
                                }
                            }
                            else
                            {
                                _currentTable[newPosition >> 8] ^= (1 << (byte)newPosition);
                                _currentTable[_currentBoxesPositions[i] >> 8] |= (1 << (byte)_currentBoxesPositions[i]);
                            }
                        }
                    }
                }
               
            }
            return null;
        }

        /// <summary>
        /// Returns true if the current state is the goal state.
        /// </summary>
        private bool GoalState(string boxesPositions)
        {
            return _goalState.Equals(boxesPositions);
        }

        /// <summary>
        /// Returns true if any goal positions cannot be accessed from the new box position.
        /// </summary>
        private bool WrongPlace(short newBoxPosition)
        {
            return (_wrongPlaces[newBoxPosition >> 8] & (1 << ((byte)newBoxPosition))) != 0;
        }

        /// <summary>
        /// Returns true if the new state is a deadlock.
        /// </summary>
        private bool DeadLock(short newBoxPosition, short directionX, short directionY)
        {
            //initialize return value
            bool deadlock = false;

            if (!GoalPosition(newBoxPosition))
            {
                //decompose position to x and y cordinates
                byte x = (byte)(newBoxPosition >> 8);
                byte y = (byte)newBoxPosition;

                if (directionX > 0)
                {
                    //check dead lokcks if step down
                    deadlock = ((_currentTable[x] & (1 << y + 1)) > 0) && ((_currentTable[x + 1] & (1 << y + 1)) > 0) &&
                                ((_currentTable[x + 1] & (1 << y)) > 0);
                    deadlock |= ((_currentTable[x] & (1 << y - 1)) > 0) && ((_currentTable[x + 1] & (1 << y - 1)) > 0) &&
                                ((_currentTable[x + 1] & (1 << y)) > 0);

                    deadlock |= ((_emptyTable[x] & (1 << y - 1)) > 0) && ((_emptyTable[x + 1] & (1 << y)) > 0);
                    deadlock |= ((_emptyTable[x] & (1 << y + 1)) > 0) && ((_emptyTable[x + 1] & (1 << y)) > 0);

                    deadlock |= ((_currentTable[x + 1] & (1 << y)) > 0) && ((_emptyTable[x] & (1 << y + 1)) > 0) &&
                                ((_emptyTable[x + 1] & (1 << y - 1)) > 0);
                    deadlock |= ((_currentTable[x + 1] & (1 << y)) > 0) && ((_emptyTable[x] & (1 << y - 1)) > 0) &&
                                ((_emptyTable[x + 1] & (1 << y + 1)) > 0);
                }
                else
                if (directionX < 0)
                {
                    //check dead lokcks if step up
                    deadlock = ((_currentTable[x] & (1 << y + 1)) > 0) && ((_currentTable[x - 1] & (1 << y + 1)) > 0) &&
                                ((_currentTable[x - 1] & (1 << y)) > 0);
                    deadlock |= ((_currentTable[x] & (1 << y - 1)) > 0) && ((_currentTable[x - 1] & (1 << y - 1)) > 0) &&
                                ((_currentTable[x - 1] & (1 << y)) > 0);

                    deadlock |= ((_emptyTable[x] & (1 << y + 1)) > 0) && ((_emptyTable[x - 1] & (1 << y)) > 0);
                    deadlock |= ((_emptyTable[x] & (1 << y - 1)) > 0) && ((_emptyTable[x - 1] & (1 << y)) > 0);


                    deadlock |= ((_currentTable[x - 1] & (1 << y)) > 0) && ((_emptyTable[x] & (1 << y + 1)) > 0) &&
                                ((_emptyTable[x - 1] & (1 << y - 1)) > 0);
                    deadlock |= ((_currentTable[x - 1] & (1 << y)) > 0) && ((_emptyTable[x] & (1 << y - 1)) > 0) &&
                                ((_emptyTable[x - 1] & (1 << y + 1)) > 0);

                }
                else
                if (directionY > 0)
                {
                    //check dead lokcks if step right
                    deadlock = ((_currentTable[x] & (1 << y + 1)) > 0) && ((_currentTable[x - 1] & (1 << y + 1)) > 0) &&
                                ((_currentTable[x - 1] & (1 << y)) > 0);
                    deadlock |= ((_currentTable[x] & (1 << y + 1)) > 0) && ((_currentTable[x + 1] & (1 << y + 1)) > 0) &&
                                ((_currentTable[x + 1] & (1 << y)) > 0);

                    deadlock |= ((_emptyTable[x] & (1 << y + 1)) > 0) && ((_emptyTable[x - 1] & (1 << y)) > 0);
                    deadlock |= ((_emptyTable[x] & (1 << y + 1)) > 0) && ((_emptyTable[x + 1] & (1 << y)) > 0);

                    deadlock |= ((_emptyTable[x - 1] & (1 << y)) > 0) && ((_currentTable[x] & (1 << y + 1)) > 0) &&
                                ((_emptyTable[x + 1] & (1 << y + 1)) > 0);
                    deadlock |= ((_emptyTable[x + 1] & (1 << y)) > 0) && ((_currentTable[x] & (1 << y + 1)) > 0) &&
                                ((_emptyTable[x - 1] & (1 << y + 1)) > 0);

                }
                else
                if (directionY < 0)
                {
                    //check dead lokcks if step left
                    deadlock = ((_currentTable[x] & (1 << y - 1)) > 0) && ((_currentTable[x - 1] & (1 << y - 1)) > 0) &&
                                ((_currentTable[x - 1] & (1 << y)) > 0);
                    deadlock |= ((_currentTable[x] & (1 << y - 1)) > 0) && ((_currentTable[x + 1] & (1 << y - 1)) > 0) &&
                                ((_currentTable[x + 1] & (1 << y)) > 0);

                    deadlock |= ((_emptyTable[x] & (1 << y - 1)) > 0) && ((_emptyTable[x - 1] & (1 << y)) > 0);
                    deadlock |= ((_emptyTable[x] & (1 << y - 1)) > 0) && ((_emptyTable[x + 1] & (1 << y)) > 0);

                    deadlock |= ((_emptyTable[x - 1] & (1 << y)) > 0) && ((_currentTable[x] & (1 << y - 1)) > 0) &&
                                ((_emptyTable[x + 1] & (1 << y - 1)) > 0);
                    deadlock |= ((_emptyTable[x + 1] & (1 << y)) > 0) && ((_currentTable[x] & (1 << y - 1)) > 0) &&
                                ((_emptyTable[x - 1] & (1 << y - 1)) > 0);
                }
            }

            return deadlock;
        }

        /// <summary>
        /// Adds new state to history.
        /// </summary>
        /// <returns>Returns true if history already contains the new state.</returns>
        private bool AddToHistroy(short playerPosition, string state)
        {
            bool result = _history.ContainsKey(state);
            if (result)
            {
                if (!(result &= (_history[state][playerPosition >> 8] & (1 << ((byte)playerPosition))) != 0))
                    _history[state][playerPosition >> 8] |= (1 << ((byte)playerPosition));
            }
            else
            {
                _history[state] = new int[32];
                _history[state][playerPosition >> 8] |= (1 << ((byte)playerPosition));
            }

            return result;
        }

        /// <summary>
        /// Returns true if the destination position is a goal position.
        /// </summary>
        private bool GoalPosition(short position)
        {
            return (_goalPositions[position >> 8] & (1 << ((byte)position))) != 0;
        }

        /// <summary>
        /// Returns true if the destination position is empty.
        /// </summary>
        private bool ValidStep(short to, short playerPosition)
        {
            return ((_currentTable[(to >> 8)] & (1 << (byte)to)) == 0 &&
                (_currentTable[(playerPosition >> 8)] & (1 << (byte)playerPosition)) == 0);
        }

        /// <summary>
        /// Inserts new state into the open states.
        /// </summary>
        private void HeuristicsInsert(State insertingState)
        {
            insertingState.HeuristicsValue += _distancesMap[insertingState.BoxesPositions[0] >> 8, (byte)insertingState.BoxesPositions[0], 0] + 1;
            for (int i = 1; i < _goalState.Length; i++)
            {
                insertingState.HeuristicsValue += _distancesMap[insertingState.BoxesPositions[i] >> 8, (byte)insertingState.BoxesPositions[i], i]
                    + _distancesMap[insertingState.BoxesPositions[i] >> 8, (byte)insertingState.BoxesPositions[i], i - 1];
            }
            insertingState.HeuristicsValue *= insertingState.Directions.Length;
            _openStates.Push(insertingState);
        }

        /// <summary>
        /// The player walks around the table in the current state.
        /// </summary>
        private string[,] PlayerWalks(short playerPosition)
        {
            short[]     openStates      = new short[_emptyPositionsCount];
            string[,]   tourDirections   = new string[_emptyTable.Length,32];
            int index = 1;

            _tourTable  = new int[_currentTable.Length];
            Array.Copy(_currentTable, _tourTable, _emptyTable.Length);

            openStates[0] = playerPosition;
            _tourTable[playerPosition >> 8] |= (1 << (byte)playerPosition);

            for (int i = 0; i < index; i++)
            {
                for (int j = 0; j < _directions.Length; j++)
                {
                    short position = (short)(openStates[i] + _directions[j]);
                    if ((_tourTable[position >> 8] & (1 << (byte)position)) == 0)
                    {
                        tourDirections[position >> 8, (byte)position] = tourDirections[openStates[i] >> 8, (byte)openStates[i]] + "," 
                            + _tourDirections[j];
                        _tourTable[position >> 8] |= (1 << (byte)position);
                        openStates[index] = position;
                        ++index;
                    }
                }
            }

            return tourDirections;
        }

        /// <summary>
        /// Replaces the old box position with the new box position and keeps the sorting of box positions.
        /// </summary>
        private string InserPosition(short newPosition, short oldPosition)
        {
            char[] c    = _currentBoxesPositions.ToCharArray();
           int index    = Array.BinarySearch<char>(c, (char)oldPosition);
           c[index]     = (char)newPosition;

           if (index < c.Length - 1 && _currentBoxesPositions[index + 1] < newPosition)
           {
               //if the new positin is higher that the latest...
               do
               {
                   c[index] = c[index + 1];
                   ++index;
               } while (index < c.Length - 1 && _currentBoxesPositions[index + 1] < newPosition);
           }
           else if (index > 0 && _currentBoxesPositions[index - 1] > newPosition)
           {
               //if the new positin is lower that the latest...
               do
               {
                   c[index] = c[index - 1];
                   --index;
               } while (index > 0 && _currentBoxesPositions[index - 1] > newPosition);
           }
           //insert new position
           c[index] = (char)newPosition;

           return new string(c);
        }

        /// <summary>
        /// Prints the auxiliary tables on the console at debug mode.
        /// </summary>
        private void PrintTable(int[] table)
        {
            for (int i = 0; i < table.Length; i++)
            {
                for (int j = 0; j < 32; j++)
                {
                    if((table[i] & (1 << j)) != 0)
                        Console.Write(1 + ",");
                    else
                        Console.Write(0 + ",");
                }
                Console.WriteLine();
            }
            Console.WriteLine();
        }

        /// <summary>
        /// Prints the auxiliary distances on the console at debug mode.
        /// </summary>
        private void PrintDistances(int[,,] table)
        {
            int dimX = table.GetLength(0);
            for (int i = 0; i < dimX; i++)
            {
                for (int j = 0; j < 32; j++)
                {
                    for (int k = 0; k < _goalState.Length; k++)
                    {
                        Console.Write(string.Format("{0},{1},{2} = {3}; ", new object[] { i, j, k, table[i, j, k] }));
                    }
                }
                Console.WriteLine();
            }
            Console.WriteLine();
        }

        #endregion
    }
}
