MddMatchAndPrune.cs

﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;

namespace mapf
{
    /// <summary>
    /// Used by CostTreeNodeSolverKSimpleMatching (but actually commented out)
    /// </summary>
    class MddMatchAndPrune
    {
        MDD[] allMDDs;
        Queue<MddMatchAndPruneState> openList;
        Dictionary<MddMatchAndPruneState, MddMatchAndPruneState> closedList;
        int solutionDepth; // The depth is the cost + 1 because the root also counts as a level
        MddMatchAndPruneState goal; // This will contain the goal node if such was found
        bool legal; //indicates if all single MDDs are legal
        public bool[] conflicted; //indicates if the matching process found any illegal nodes/edges and pruned any of the MDDs
        Run runner;
        CostTreeNodeSolver nodeSolver;

        /// <summary>
        /// constructor
        /// </summary>
        public MddMatchAndPrune(Run runner, CostTreeNodeSolver nodeSolver)
        {
            this.openList = new Queue<MddMatchAndPruneState>();
            this.closedList = new Dictionary<MddMatchAndPruneState, MddMatchAndPruneState>();
            conflicted = new bool[4];
            this.runner = runner;
            this.nodeSolver = nodeSolver;
        }

        public void initialize(MDD[] allMDDs)
        {
            this.allMDDs = allMDDs;
            this.openList.Clear();
            this.closedList.Clear();
            MDDNode[] rootPositions = new MDDNode[allMDDs.Length];
            for (int i = 0; i < allMDDs.Length; i++)
            {
                if (allMDDs[i].levels == null)
                {
                    legal = false;
                    return;
                }
                rootPositions[i] = allMDDs[i].levels[0].First.Value;
                this.conflicted[i] = false;
            }
            MddMatchAndPruneState root = new MddMatchAndPruneState(rootPositions);
            this.solutionDepth = root.allPositions[0].mdd.levels.Length;
            openList.Enqueue(root);
            legal = true;
        }

        /// <summary>
        /// Build the generalized MDD
        /// </summary>
        /// <returns>Whether it was successful</returns>
        private bool buildGeneralMDD()
        {
            MddMatchAndPruneState current = openList.Dequeue();
            successorIterator allChildren = new successorIterator(allMDDs.Length);
            int currentLevel = current.stateLevel;

            while (current.stateLevel + 1 != this.solutionDepth) // while not goal
            {
                Expand(current, allChildren);
                if (openList.Count == 0)
                    return false;
                current = openList.Dequeue();
                if (current.stateLevel != currentLevel)
                {
                    closedList.Clear();
                    currentLevel++;
                }
                if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                    return false;
            }
            return true;
        }

        /// <summary>
        /// expands a given node
        /// </summary>
        /// <param name="toExpand"></param>
        /// <param name="allChildren"></param>
        private void Expand(MddMatchAndPruneState toExpand, successorIterator allChildren)
        {
            allChildren.initialize(toExpand);
            MddMatchAndPruneState successor;

            while (allChildren.hasNext)
            {
                successor = allChildren.getNext();
                if (closedList.ContainsKey(successor))
                {
                    closedList[successor].addParent(toExpand);
                }
                else
                {
                    this.nodeSolver.matchCounter++;
                    successor.addParent(toExpand);
                    closedList.Add(successor, successor);
                    openList.Enqueue(successor);
                    if (successor.stateLevel + 1 == this.solutionDepth)
                        goal = successor;
                }
            }
        }

        private void reverseExpand(MddMatchAndPruneState toExpand)
        {
            foreach (MddMatchAndPruneState parent in toExpand.parents)
            {
                for (int i = 0; i < toExpand.allPositions.Length; i++)
                {
                    ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrix[i,
                                                       parent.allPositions[i].getVertexIndex(),
                                                       (int)Move.Direction.Wait] =
                        ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrixCounter + 1;
                }
                if (closedList.ContainsKey(parent) == false)
                {
                    openList.Enqueue(parent);
                    closedList.Add(parent, parent);
                }
            }
        }


        /// <summary>
        /// Prunes a given level according to the edgesMatrix
        /// </summary>
        /// <param name="level"></param>
        private void pruneLevel(int level)
        {
            int parentI;

            for (int i = 0; i < allMDDs.Length; i++)
            {
                if (allMDDs[i].levels == null)
                    continue;

                foreach (MDDNode node in allMDDs[i].levels[level])
                {
                    MDDNode[] parentsToDelete = new MDDNode[5];

                    parentI = 0;
                    foreach (MDDNode parent in node.parents)
                    {
                        //if not legal
                        int edge = ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrix[i,
                                                                      parent.getVertexIndex(),
                                                                      (int)node.move.direction /*or parent.move.direction, I'm not sure*/];
                        if (edge != ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrixCounter + 1)
                        {
                            parentsToDelete[parentI] = parent;
                            this.conflicted[i] = true;
                        }
                        parentI++;
                    }
                    foreach (MDDNode deleteParent in parentsToDelete)
                        if (deleteParent != null)
                            node.removeParent(deleteParent);
                }
            }
        }

        /// <summary>
        /// Prunes the given MDDs according to each other
        /// </summary>
        /// <returns>Whether it was successful, by how much to increment edgesMatrixCounter, and by how much to increment matchCounter</returns>
        public bool pruneMDDs()
        {
            if (legal == false)
                return false;

            bool success = buildGeneralMDD();
            if (success == false)
                return false;

            Trace.Assert(openList.Count == 0);

            MddMatchAndPruneState current = goal;
            int currentLevel = goal.stateLevel;
            closedList.Clear();

            while (current.stateLevel > 0) // while not root
            {
                if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                    return false;

                if (current.stateLevel < currentLevel)
                {
                    pruneLevel(currentLevel);
                    currentLevel--;
                    ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrixCounter++;
                    closedList.Clear();
                }
                reverseExpand(current);
                current = openList.Dequeue();
            }
            pruneLevel(currentLevel); //prune level 1
            return true;
        }

    }

    class MddMatchAndPruneState
    {
        public MDDNode[] allPositions;
        public int stateLevel; //starts at 0
        public LinkedList<MddMatchAndPruneState> parents;
        LinkedList<MddMatchAndPruneState> childrens;

        public MddMatchAndPruneState(MDDNode[] allPositions)
        {
            this.allPositions = allPositions;
            this.stateLevel = allPositions[0].move.time;
            this.parents = new LinkedList<MddMatchAndPruneState>();
            this.childrens = new LinkedList<MddMatchAndPruneState>();
        }

        public MddMatchAndPruneState(LinkedListNode<MDDNode>[] allSuccessors)
        {
            allPositions = new MDDNode[allSuccessors.Length];
            for (int i = 0; i < allPositions.Length; i++)
            {
                allPositions[i] = allSuccessors[i].Value;
            }
            this.stateLevel = allPositions[0].move.time;
            this.parents = new LinkedList<MddMatchAndPruneState>();
            this.childrens = new LinkedList<MddMatchAndPruneState>();
        }

        public void margeParents(LinkedList<MddMatchAndPruneState> otherParents)
        {
            parents.Union(otherParents);
        }

        public void addParent(MddMatchAndPruneState parent)
        {
            this.parents.AddLast(parent);
            parent.childrens.AddLast(this);
        }

        public override int GetHashCode()
        {
            unchecked
            {
                int ans = 0;
                for (int i = 0; i < allPositions.Length; i = i + 2)
                {
                    ans += allPositions[i].move.GetHashCode();
                }
                return ans;
            }
        }

        public override bool Equals(object obj)
        {
            if (obj == null)
                return false;
            MddMatchAndPruneState comp = (MddMatchAndPruneState)obj;
            return this.allPositions.SequenceEqual<MDDNode>(comp.allPositions);
        }
    }

    class successorIterator
    {
        LinkedListNode<MDDNode>[] nodesFromChildrenList;
        MddMatchAndPruneState prevStep;
        public bool hasNext;

        public successorIterator(int size)
        {
            this.nodesFromChildrenList = new LinkedListNode<MDDNode>[size];
        }

        public void initialize(MddMatchAndPruneState prevStep)
        {
            this.hasNext = true;
            this.prevStep = prevStep;
            for (int i = 0; i < nodesFromChildrenList.Length; i++)
            {
                nodesFromChildrenList[i] = prevStep.allPositions[i].children.First; //if crashes check there if there can be a node with no children
                if (nodesFromChildrenList[i] == null)
                    this.hasNext = false;
            }
            while (hasNext == true && isLegal(0) == false)
            {
                getNext();
            }
        }

        /// <summary>
        /// Return the next generated child.
        /// </summary>
        /// <returns></returns>
        public MddMatchAndPruneState getNext()
        {
            //first return than iterate
            MddMatchAndPruneState ans = new MddMatchAndPruneState(nodesFromChildrenList);
            if (nextChild(0) == false)
            {
                hasNext = false;
            }
            return ans;
        }

        /// <summary>
        /// Sets the next child in line for a given agent, if it's the last child returns the first in order (closed loop). If reset returns false.
        /// </summary>
        /// <param name="agent"></param>
        private bool nextSuccessor(int agent)
        {
            if (nodesFromChildrenList[agent].Next == null)
            {
                nodesFromChildrenList[agent] = nodesFromChildrenList[agent].List.First;
                return false;
            }

            nodesFromChildrenList[agent] = nodesFromChildrenList[agent].Next;
            return true;
        }

        /// <summary>
        /// Recursive function, try to proceed the first agent if it is at the end of its children resets him and proceeds the second agent and so on
        /// </summary>
        /// <param name="agent"></param>
        private bool nextChild(int agent)
        {
            if (agent == nodesFromChildrenList.Length)
                return false;
            if (nextSuccessor(agent) == false)
                return nextChild(agent + 1);
            while (isLegal(agent) == false)
            {
                if (nextSuccessor(agent) == false)
                    return nextChild(agent + 1);
            }

            if (agent > 0 && isLegal(0) == false)
                return nextChild(0);
            return true;
        }

        /// <summary>
        /// If we move the i'th agent we check whether it is legal with all agents that moved before,
        /// i.e. from that point forward
        /// </summary>
        /// <param name="checkFrom"></param>
        /// <returns></returns>
        private bool isLegal(int checkFrom)
        {
            // check if all moves are legal from the i agent forward (collisons+head on collisions)
            for (int i = checkFrom; i < nodesFromChildrenList.Length - 1; i++)
            {
                var node = nodesFromChildrenList[i].Value;
                for (int j = i + 1; j < nodesFromChildrenList.Length; j++)
                {
                    var other = nodesFromChildrenList[j].Value;
                    // TODO: Replace the following duplicated logic is node.move.isColliding(other.move)
                    //       after making sure results stay correct
                    if (node.Equals(other))
                        return false;
                    if (Constants.ALLOW_HEAD_ON_COLLISION == false &&
                        node.getVertexIndex() == prevStep.allPositions[j].getVertexIndex() &&
                        node.getVertexIndex() == prevStep.allPositions[i].getVertexIndex())
                        return false;
                }
            }
            return true;
        }


    }
}