// public static int counter=0;
        public List <string> Plan()
        {
            bool bAllAgentsEqual = CheckAgentEquality();

            int i = 200;

            int[] statesNum = new int[agents.Count];
            for (int k = 0; k < statesNum.Length; k++)
            {
                statesNum[k] = 0;
            }


            VertexHsp rootVertexHsp = new VertexHsp(statesNum, new List <Action>(), 0, countOfLandmarks, 0, 0, null);

            rootVertexHsp.h = rootVertexHsp.ComputeFF_h(new bool[countOfLandmarks]);
            rootVertexHsp.ComputePreferActions();


            int       count        = 0;
            VertexHsp curentVertex = null;
            DateTime  dtStart      = DateTime.Now;

            DateTime         begin = DateTime.Now;
            int              queue1PriorityCounter = 0;
            int              queue2PriorityCounter = 0;
            List <VertexHsp> queue  = new List <VertexHsp>();
            List <VertexHsp> queue2 = new List <VertexHsp>();
            //   List<VertexHsp> queue2 = new List<VertexHsp>();
            bool                first     = true;
            bool                first2    = true;
            HashSet <int[]>     lVisited  = new HashSet <int[]>(new ComparerArray());
            HashSet <VertexHsp> lVisited2 = new HashSet <VertexHsp>();

            queue.Add(rootVertexHsp);


            // lVisited.Add(rootVertexHsp.statesNubmber);
            // lVisited2.Add(rootVertexHsp);

            int           temp               = 0;
            List <string> lplan              = null;
            bool          flag               = true;
            double        minLandmark_h      = countOfLandmarks;
            double        minHsp_h           = rootVertexHsp.h;
            int           c                  = 0;
            int           cResets            = 0;
            double        minh               = 1000;
            int           blindCounter       = 0;
            TimeSpan      tsDeadendDetection = new TimeSpan();

            try
            {
                while (queue.Count > 0 || queue2.Count > 0)
                {
                    c++;
                    if (c % 30 == 0 && curentVertex != null)
                    {
                        Console.Write("\rExpanded: " + c + ", open: " + queue.Count +
                                      ", h: " + curentVertex.h + ", h2: " + curentVertex.h2 + ", T: " + (int)(DateTime.Now - dtStart).TotalSeconds
                                      + ", deadend = " + (int)tsDeadendDetection.TotalSeconds);
                        if (queue.Count > 2000000)
                        {
                            return(null);
                        }
                    }
                    flag = true;

                    temp++;
                    if (queue.Count > 0 && (queue1PriorityCounter > queue2PriorityCounter || queue2.Count == 0))
                    {
                        if (first)
                        {
                            first        = false;
                            curentVertex = FindMinLandmarks(queue);
                        }
                        else
                        {
                            first        = true;
                            curentVertex = FindMinHsp(queue);
                            queue1PriorityCounter--;
                        }
                        curentVertex = curentVertex.Calculate_h();
                    }
                    else
                    {
                        if (first2)
                        {
                            first2       = false;
                            curentVertex = FindMinLandmarks(queue2);
                        }
                        else
                        {
                            first2       = true;
                            curentVertex = FindMinHsp(queue2);
                            queue2PriorityCounter--;
                        }
                        curentVertex = curentVertex.Calculate_h();

                        if (curentVertex != null && (curentVertex.h2 < minLandmark_h || curentVertex.h < minHsp_h))
                        {
                            queue2PriorityCounter += 1000;
                        }
                    }
                    if (curentVertex == null)
                    {
                        continue;
                    }
                    //  counter++;
                    //  Console.WriteLine(counter);

                    bool bVisited = false;
                    if (bAllAgentsEqual)
                    {
                        bVisited = lVisited2.Contains(curentVertex);
                    }
                    else
                    {
                        bVisited = lVisited.Contains(curentVertex.statesNubmber);
                    }


                    if (bVisited)
                    {
                        continue;
                    }
                    if (bAllAgentsEqual)
                    {
                        lVisited2.Add(curentVertex);
                    }
                    else
                    {
                        lVisited.Add(curentVertex.statesNubmber);
                    }



                    if (curentVertex.h2 < minLandmark_h)
                    {
                        minLandmark_h = curentVertex.h2;
                    }
                    if (curentVertex.h < minHsp_h)
                    {
                        minHsp_h = curentVertex.h;
                    }
                    // DateTime dtBefore = DateTime.Now;

                    //tsDeadendDetection += (DateTime.Now - dtBefore);

                    count++;

                    blindCounter++;

                    int f = 0;
                    List <VertexHsp>          needUpDate     = new List <VertexHsp>();
                    HashSet <CompoundFormula> levelPotential = new HashSet <CompoundFormula>();
                    int  index = 1;
                    bool con   = false;
                    Program.StateExpendCounter++;
                    List <VertexHsp> lExpanded = Expand(curentVertex, levelPotential, needUpDate);

                    /*if (lExpanded.Count == 0)
                     * {
                     *      curentVertex.onlyPrefered = false;
                     *      lExpanded = Expand(curentVertex, levelPotential, needUpDate);
                     * }*/

                    foreach (VertexHsp newVertexHsp in lExpanded)
                    {
                        f++;

                        newVertexHsp.g += (double)index / 1000000;

                        //bool bVisited = false;

                        /*  if(bAllAgentsEqual)
                         *    bVisited = lVisited2.Contains(newVertexHsp);
                         * else
                         *    bVisited = lVisited.Contains(newVertexHsp.statesNubmber);
                         */

                        // if (!bVisited)
                        {
                            if (newVertexHsp.isPrefered)
                            {
                                queue2.Add(newVertexHsp);
                            }
                            else
                            {
                                queue.Add(newVertexHsp);
                            }
                            // queue2.Add(newVertexHsp);
                            // lVisited.Add(newVertexHsp.statesNubmber);
                            //lVisited2.Add(newVertexHsp);
                        }
                        index++;

                        /*if (minh > newVertexHsp.h)
                         * {
                         *  minh = newVertexHsp.h;
                         *
                         * }*/

                        /*if (VertexHsp.Comparer(newVertexHsp, curentVertex) == -1 && !con)
                         * {
                         *  blindCounter = 0;
                         * }*/
                    }

                    curentVertex.canGetInParallel = levelPotential;

                    if (curentVertex.h == 0 || curentVertex.h2 == 0)
                    {
                        curentVertex.publicPredicateImplications = new List <HashSet <CompoundFormula> >();
                        VertexHsp tmp = curentVertex;
                        while (tmp != null)
                        {
                            curentVertex.publicPredicateImplications.Insert(0, tmp.canGetInParallel);
                            tmp = tmp.Parent;
                        }
                        string isGoal = curentVertex.IsGoal(out lplan);
                        if (isGoal.Equals("true"))
                        {
                            double time = ((double)((DateTime.Now.Subtract(begin)).Minutes)) * 60.0;
                            time += ((double)((DateTime.Now.Subtract(begin)).Seconds));
                            time += ((double)((DateTime.Now.Subtract(begin)).Milliseconds) / 1000);

                            Program.times.Add(time);
                            Program.countActions.Add(lplan.Count);
                            Program.timeSum   += time;
                            Program.actionSum += lplan.Count;



                            return(lplan);
                        }
                        if (!isGoal.Equals("false"))
                        {
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.ToString());
                return(null);
            }


            return(null);
        }
        public List <string> Plan()
        {
            bool bAllAgentsEqual = CheckAgentEquality();

            int i = 200;

            int[] statesNum = new int[agents.Count];
            for (int k = 0; k < statesNum.Length; k++)
            {
                statesNum[k] = 0;
            }


            VertexHsp rootVertexHsp = new VertexHsp(statesNum, new List <Action>(), 0, countOfLandmarks, 0, 0, null);

            rootVertexHsp.h = rootVertexHsp.ComputeFF_h(new bool[countOfLandmarks]);
            rootVertexHsp.ComputePreferActions();
            int       count           = 0;
            VertexHsp curentVertexHsp = null;
            DateTime  dtStart         = DateTime.Now;

            DateTime            begin     = DateTime.Now;
            List <VertexHsp>    queue     = new List <VertexHsp>();
            HashSet <int[]>     lVisited  = new HashSet <int[]>(new ComparerArray());
            HashSet <VertexHsp> lVisited2 = new HashSet <VertexHsp>();

            queue.Add(rootVertexHsp);
            lVisited.Add(rootVertexHsp.statesNubmber);
            lVisited2.Add(rootVertexHsp);

            int           temp               = 0;
            List <string> lplan              = null;
            bool          flag               = true;
            int           c                  = 0;
            int           cResets            = 0;
            double        minh               = 1000;
            int           blindCounter       = 0;
            TimeSpan      tsDeadendDetection = new TimeSpan();

            while (queue.Count > 0)
            {
                c++;
                if (c % 30 == 0)
                {
                    Console.Write("\rExpanded: " + c + ", open: " + queue.Count +
                                  ", h: " + curentVertexHsp.h + ", h2: " + curentVertexHsp.h2 + ", T: " + (int)(DateTime.Now - dtStart).TotalSeconds
                                  + ", deadend = " + (int)tsDeadendDetection.TotalSeconds);
                    if (queue.Count > 200000)
                    {
                        return(null);
                    }
                }
                flag = true;

                temp++;
                curentVertexHsp = FindMin(queue);

                DateTime dtBefore = DateTime.Now;

                tsDeadendDetection += (DateTime.Now - dtBefore);

                count++;

                blindCounter++;

                int f = 0;
                List <VertexHsp>          needUpDate     = new List <VertexHsp>();
                HashSet <CompoundFormula> levelPotential = new HashSet <CompoundFormula>();
                int              index     = 1;
                bool             con       = false;
                List <VertexHsp> lExpanded = Expand(curentVertexHsp, levelPotential, needUpDate);
                foreach (VertexHsp newVertexHsp in lExpanded)
                {
                    f++;

                    newVertexHsp.g += (double)index / 1000000;

                    bool bVisited = false;
                    if (bAllAgentsEqual)
                    {
                        bVisited = lVisited2.Contains(newVertexHsp);
                    }
                    else
                    {
                        bVisited = lVisited.Contains(newVertexHsp.statesNubmber);
                    }

                    if (!bVisited)
                    {
                        queue.Add(newVertexHsp);
                        lVisited.Add(newVertexHsp.statesNubmber);
                        lVisited2.Add(newVertexHsp);
                    }
                    index++;
                    if (minh > newVertexHsp.h)
                    {
                        minh = newVertexHsp.h;
                    }

                    /*if (VertexHsp.Comparer(newVertexHsp, curentVertexHsp) == -1 && !con)
                     * {
                     *  blindCounter = 0;
                     * }*/
                }

                curentVertexHsp.canGetInParallel = levelPotential;

                if (curentVertexHsp.h == 0)
                {
                    curentVertexHsp.publicPredicateImplications = new List <HashSet <CompoundFormula> >();
                    VertexHsp tmp = curentVertexHsp;
                    while (tmp != null)
                    {
                        curentVertexHsp.publicPredicateImplications.Insert(0, tmp.canGetInParallel);
                        tmp = tmp.Parent;
                    }
                    string isGoal = curentVertexHsp.IsGoal(out lplan);
                    if (isGoal.Equals("true"))
                    {
                        double time = ((double)((DateTime.Now.Subtract(begin)).Minutes)) * 60.0;
                        time += ((double)((DateTime.Now.Subtract(begin)).Seconds));
                        time += ((double)((DateTime.Now.Subtract(begin)).Milliseconds) / 1000);

                        Program.times.Add(time);
                        Program.countActions.Add(lplan.Count);
                        Program.timeSum   += time;
                        Program.actionSum += lplan.Count;



                        return(lplan);
                    }
                    if (!isGoal.Equals("false"))
                    {
                    }
                }
            }


            return(null);
        }