Czoko_Smieciarka/Trunk/MonoGameView/Algorithms/BestFirstSearch.cs

using CzokoŚmieciarka.MonoGameView.DataModels.Enums;
using CzokoŚmieciarka.MonoGameView.DataModels.Interfaces;
using CzokoŚmieciarka.MonoGameView.DataModels.Interfaces.GarbageCollector;
using CzokoŚmieciarka.MonoGameView.DataModels.Interfaces.TrashCans;
using CzokoŚmieciarka.MonoGameView.DataModels.Models;
using CzokoŚmieciarka.MonoGameView.DataModels.Models.Steps;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Xna.Framework.Content;
using System.Threading;
using CzokoŚmieciarka.MonoGameView.DataModels.GeneralModels.Models;
using Microsoft.Xna.Framework.Graphics;
using MonoGameView.DataModels.Models;

namespace MonoGameView.Algorithms
{
    public class BestFirstSearch
    {
        public GarbageCollector Collector { get; set; }
        public ICloneable[,] Grid { get; set; }
        public BestFirstSearch(GarbageCollector collector, ICloneable[,] grid)
        {
            this.Collector = collector;
            this.Grid = grid;
        }
        int count;
        public List<Coords> Houses { get; set; }
        public List<Coords> Dumps { get; set; }
        public Tuple<List<IStep>,int,List<ICloneable[,]>> BestPath(ContentManager content, GarbageCollector collector, ICloneable[,] grid)
        {
            
            Houses = new List<Coords>();
            Dumps = new List<Coords>();
            for (int x = 0; x < grid.GetLength(0); x++)
            {
                for (int y = 0; y < grid.GetLength(1); y++)
                {
                    if (grid[x, y] is House)
                    {
                        Houses.Add(new Coords(x, y));

                    } else if (grid[x,y] is Dump)
                    {
                        Dumps.Add(new Coords(x, y));
                    }
                }
            }



            var r = SearchBestFirst(content, collector, grid, 0);


            var dataToLog = Jazda(r.Item1, CopyGrid(grid), (GarbageCollector) collector.Clone()).ToList();

            /*Oznaczenia
             *0 - trawa, nie do poruszania
             *10 - droga po której można się poruszać
             *11 - droga po której nie można się poruszać
             *20 - dom z którego można wziąć śmieci
             *21 - dom z którego nie można wziąć śmieci w tym momencie
             *22 - dom pusty, bez śmieci
             *30 - wysypisko szkła
             *31 - wysypisko plastików i metali
             *32 - wysypisko odpadów organicznych
             *33 - wysypisko papieru
             *40 - ruch w górę
             *41 - ruch w prawo
             *42 - ruch w dół
             *43 - ruch w lewo
             *50 - zbierz śmieci
             *51 - wyrzuć śmieci
             */

            Console.WriteLine($"Counts : {count}");
            using (StreamWriter writer = new StreamWriter("chuj4.txt"))
            {
                foreach (var item in dataToLog)
                {

                    string line = "";
                    if (item.Key is MoveStep)
                    {
                        MoveStep move = (item.Key as MoveStep);
                        String[,] mygrid = item.Value;
                        switch (move.GetDirection().ToString())
                        {
                            case "Up":
                                line += "40";
                                break;
                            case "Right":
                                line += "41";
                                break;
                            case "Down":
                                line += "42";
                                break;
                            case "Left":
                                line += "43";
                                break;
                        }

                        foreach (string field in mygrid)
                        {
                            switch (field)
                            {
                                case "grass":
                                    line += "," + "0";
                                    break;
                                case "road1":
                                    line += "," + "10";
                                    break;
                                case "road2":
                                    line += "," + "11";
                                    break;
                                case "house":
                                    line += "," + "20";
                                    break;
                                case "emptyHouse":
                                    line += "," + "22";
                                    break;
                                case "Glass":
                                    line += "," + "30";
                                    break;
                                case "PlasticMetal":
                                    line += "," + "31";
                                    break;
                                case "Organic":
                                    line += "," + "32";
                                    break;
                                case "Paper":
                                    line += "," + "33";
                                    break;
                            }
                        }
                    }
                    else if (item.Key is CollectStep)
                    {
                        CollectStep collect = (item.Key as CollectStep);
                        String[,] mygrid = item.Value;
                        line += "50";
                        foreach (string field in mygrid)
                        {
                            switch (field)
                            {
                                case "grass":
                                    line += "," + "0";
                                    break;
                                case "road1":
                                    line += "," + "10";
                                    break;
                                case "road2":
                                    line += "," + "11";
                                    break;
                                case "house":
                                    line += "," + "20";
                                    break;
                                case "emptyHouse":
                                    line += "," + "22";
                                    break;
                                case "Glass":
                                    line += "," + "30";
                                    break;
                                case "PlasticMetal":
                                    line += "," + "31";
                                    break;
                                case "Organic":
                                    line += "," + "32";
                                    break;
                                case "Paper":
                                    line += "," + "33";
                                    break;
                            }
                        }
                    }
                    else
                    {
                        SpillStep spill = (item.Key as SpillStep);
                        String[,] mygrid = item.Value;
                        line += "51";
                        foreach (string field in mygrid)
                        {
                            switch (field)
                            {
                                case "grass":
                                    line += "," + "0";
                                    break;
                                case "road1":
                                    line += "," + "10";
                                    break;
                                case "road2":
                                    line += "," + "11";
                                    break;
                                case "house":
                                    line += "," + "20";
                                    break;
                                case "emptyHouse":
                                    line += "," + "22";
                                    break;
                                case "Glass":
                                    line += "," + "30";
                                    break;
                                case "PlasticMetal":
                                    line += "," + "31";
                                    break;
                                case "Organic":
                                    line += "," + "32";
                                    break;
                                case "Paper":
                                    line += "," + "33";
                                    break;
                            }
                        }

                    }
                    writer.WriteLine(line);
                }
            }
            if (r == null)
                return new Tuple<List<IStep>, int, List<ICloneable[,]>>(new List<IStep>(), 0,
                    new List<ICloneable[,]>());
            return r;
        }


        public IEnumerable<KeyValuePair<IStep, String[,]>> Jazda(List<IStep> steps, ICloneable[,] grid, GarbageCollector collector)
        {
            
            
            for (int i =0;i<steps.Count();i++)
            {

                var minGrid = new String[5, 5];

                for (int x = collector.Coords.X-2; x <= collector.Coords.X+2;x++)
                {
                    for (int y = collector.Coords.Y-2;y <= collector.Coords.Y+2;y++)
                    {
                        var xoffset = x - (collector.Coords.X - 2);
                        var yoffset = y - (collector.Coords.Y - 2);
                        if (x >= 0 && y >= 0 && x < grid.GetLength(0) && y < grid.GetLength(1))
                        {
                            

                            var cell = grid[x, y];

                            if (cell is Grass) minGrid[xoffset, yoffset] = "grass";
                            if (cell is Road1) minGrid[xoffset, yoffset] = "road1";
                            if (cell is Road2) minGrid[xoffset, yoffset] = "road2";
                            if (cell is Dump) minGrid[xoffset, yoffset] = (cell as Dump).TypeOfGarbage.GarbageType.ToString();
                            if (cell is House) minGrid[xoffset, yoffset] = "house";
                            if (cell is EmptyHouse) minGrid[xoffset, yoffset] = "emptyHouse";
                        } else
                        {
                            minGrid[xoffset, yoffset] = "grass";
                        }
                    }
                }
                yield return new KeyValuePair<IStep, String[,]>(steps[i], minGrid);



                steps[i].Invoke(collector, grid);


            }



        }




        List<IStep> PossibleSteps(AGarbageCollector collector, ICloneable[,] grid)
        {


            var result = new List<IStep>();
            var itemdupa = grid[collector.Coords.X, collector.Coords.Y];

            if (grid[collector.Coords.X, collector.Coords.Y] is House)
            {
                var collectSteps = new List<IStep>()
                {
                    new CollectStep(GarbageType.Glass),
                    new CollectStep(GarbageType.Organic),
                    new CollectStep(GarbageType.Paper),
                    new CollectStep(GarbageType.PlasticMetal)
                };
                foreach (var item in collectSteps)
                {
                    var copyCollector = (AGarbageCollector)collector.Clone();
                    var copyGrid = CopyGrid(grid);
                    if (item.Invoke(copyCollector, copyGrid))
                        result.Add(item);
                }
            }
            if (grid[collector.Coords.X, collector.Coords.Y] is ADump)
            {
                var collectSteps = new List<IStep>()
                {
                    new SpillStep(GarbageType.Glass),
                    new SpillStep(GarbageType.Organic),
                    new SpillStep(GarbageType.Paper),
                    new SpillStep(GarbageType.PlasticMetal)
                };
                foreach (var item in collectSteps)
                {
                    var copyCollector = (AGarbageCollector)collector.Clone();
                    var copyGrid = CopyGrid(grid);
                    if (item.Invoke(copyCollector, copyGrid))
                        result.Add(item);


                }
            }
            if (!(grid[collector.Coords.X, collector.Coords.Y] is Dump && collector.TrashContainers.Any(x => x.FillPercent > 0 && (grid[collector.Coords.X, collector.Coords.Y] as Dump).TypeOfGarbage.GarbageType == x.TypeOfGarbage.GarbageType))
                && !(grid[collector.Coords.X, collector.Coords.Y] is House
                    && (grid[collector.Coords.X, collector.Coords.Y] as House).TrashCans.Any(j => j.FillPercent > 0 && collector.TrashContainers.Any(i => i.FillPercent < 1 && i.Garbage.TypeOfGarbage.GarbageType == j.TypeOfGarbage.GarbageType))))
            {
                var moveSteps = new List<IStep>()
                {
                    new MoveStep(Direction.Up),
                    new MoveStep(Direction.Down),
                    new MoveStep(Direction.Left),
                    new MoveStep(Direction.Right)
                };
                var filteredMoveSteps = new List<IStep>();
                foreach (var item in moveSteps)
                {
                    var copyCollector = (AGarbageCollector)collector.Clone();
                    if (item.Invoke(copyCollector, grid))
                    {
                        var gcx = copyCollector.Coords.X;
                        var gcy = copyCollector.Coords.Y;
                        if (grid[gcx, gcy] is Road1 || grid[gcx, gcy] is House || (grid[gcx, gcy] is ADump && copyCollector.TrashContainers.Any(x => x.FillPercent > 0)))
                        {
                            result.Add(item);
                        }
                    }


                }


            }
            return result;
        }




        int Priority (Tuple<KeyValuePair<List<IStep>,List<ICloneable[,]>>,GarbageCollector,ICloneable[,]> t)
        {
            var inHouses = Houses
                .Aggregate(0.0, (a, b) => a + (t.Item3[b.X, b.Y] as IGarbageLocalization)
                    .TrashCans
                    .Aggregate(0.0, (i, j) => i + j.Garbage.Weight));
            var inDumps = Dumps
                            .Aggregate(0.0, (a, b) => a + (t.Item3[b.X, b.Y] as ATrashCan).Garbage.Weight);
            var inCollector = t.Item2.TrashContainers.Aggregate(0.0, (a, b) => a + b.Garbage.Weight);
            double p1 = inCollector + 2*inHouses - 3*inDumps;


            var houses2 = Houses.Select(x => t.Item3[x.X, x.Y]).Where(x => x is House).Select(x => x as House).ToList();

            var notFullCans = t.Item2.TrashContainers.Where(x => x.FillPercent != 1 
                && houses2.Any(i=> i.TrashCans.Any(j => j.TypeOfGarbage.GarbageType == x.TypeOfGarbage.GarbageType
                        && j.FillPercent > 0)) );
            if (!notFullCans.Any())
            {
                var fullCans = t.Item2.TrashContainers.Where(x => x.FillPercent > 0);
                if (!fullCans.Any()) return 0;
                //var fullCan=fullCans.First();
                var closestDump = Dumps
                .Select(x => t.Item3[x.X, x.Y])
                .Select(i => i as Dump)
                .Where(i => fullCans.Any(j=>i.TypeOfGarbage.GarbageType == j.TypeOfGarbage.GarbageType))
                .Min(b => Math.Pow(t.Item2.Coords.X - b.Coords.X, 2) + Math.Pow(t.Item2.Coords.Y - b.Coords.Y, 2));
                return (int) (p1 * 1000 + closestDump);
            }
            else
            {


                //var notFullCan = notFullCans.First();
                var closestHouses = Houses
                    .Select(x => t.Item3[x.X, x.Y])
                    .Where(i => i is House)
                    .Select(i => i as House)
                    .Where(i => i.TrashCans.Any(x => notFullCans.Any(j=>x.TypeOfGarbage.GarbageType ==j.TypeOfGarbage.GarbageType && x.FillPercent > 0)));
                var closestHouse = (closestHouses.Any()) ? closestHouses.Min(b => Math.Pow(t.Item2.Coords.X - b.Coords.X, 2) + Math.Pow(t.Item2.Coords.Y - b.Coords.Y, 2)): 0.0;

                var closestDumps = Dumps
                    .Select(x => t.Item3[x.X, x.Y])
                    .Select(i => i as Dump)
                    .Where(i => notFullCans.Any(j=>i.TypeOfGarbage.GarbageType == j.TypeOfGarbage.GarbageType));
                var closestDump = (closestDumps.Any()) ? closestDumps.Min(b => Math.Pow(t.Item2.Coords.X - b.Coords.X, 2) + Math.Pow(t.Item2.Coords.Y - b.Coords.Y, 2)) : 0.0;


                return (int) (p1 * 1000 + 100*closestHouse + closestDump);
            }
        }



        Tuple<List<IStep>, int, List<ICloneable[,]>> SearchBestFirst(ContentManager content, GarbageCollector collector, ICloneable[,] grid, int length)
        {

            //Thread.Sleep(1);
            count = 0;

            var nodes = new PriorityQueue<Tuple<KeyValuePair<List<IStep>, List<ICloneable[,]>>, GarbageCollector, ICloneable[,]>>();

            var f = new Tuple<KeyValuePair<List<IStep>, List<ICloneable[,]>>, GarbageCollector, ICloneable[,]>(new KeyValuePair<List<IStep>, List<ICloneable[,]>>(new List<IStep>(),new List<ICloneable[,]>() { grid }), collector, grid);

            nodes.Enqueue(f,Priority(f));
                while (true)
                {
                count++;
                    var p = nodes.Dequeue();
                    var item = p.Key;
                    var priority = p.Value;

                    /*Thread.Sleep(10);
                    this.Collector.Coords = item.Item2.Coords;
                    this.Collector.TrashContainers = item.Item2.TrashContainers;
                    for (int x = 0; x < item.Item3.GetLength(0); x++)
                    {
                        for (int y = 0; y < item.Item3.GetLength(1); y++)
                        {
                            this.Grid[x, y] = item.Item3[x, y];
                        }
                    }*/

                    if (Houses.All(c => (item.Item3[c.X, c.Y] as IGarbageLocalization).TrashCans.All(j => j.FillPercent == 0.0))
                         &&
                        item.Item2.TrashContainers.All(i => i.FillPercent == 0.0)
                        )
                    {
                        return new Tuple<List<IStep>, int, List<ICloneable[,]>>(item.Item1.Key, length, item.Item1.Value);
                    }
                    if (true)//item.Item2.Counter <= 12)
                    {


                    foreach (var step in PossibleSteps(item.Item2, item.Item3))
                    {
                        if (step is SpillStep)
                        {
                            Console.WriteLine();
                        }
                        var collectorClone = (GarbageCollector)item.Item2.Clone();
                        var gridClone = CopyGrid(item.Item3);
                        step.Invoke(collectorClone, gridClone);


                        var steps = new List<IStep>();
                        steps.AddRange(item.Item1.Key);
                        steps.Add(step);
                        var f2 = new Tuple<KeyValuePair<List<IStep>, List<ICloneable[,]>>, GarbageCollector, ICloneable[,]>(new KeyValuePair<List<IStep>, List<ICloneable[,]>>(steps, item.Item1.Value.Concat(new List<ICloneable[,]>() {gridClone }).ToList()), collectorClone, gridClone);
                            var f2P = Priority(f2);

                            nodes.Enqueue(f2,Priority(f2));
                        }
                    }


            }
        }






















        private ICloneable[,] CopyGrid(ICloneable[,] grid)
        {
            ICloneable[,] result = new ICloneable[grid.GetLength(0), grid.GetLength(1)];
            for (int x = 0; x < grid.GetLength(0); x++)
            {
                for (int y = 0; y < grid.GetLength(1); y++)
                {
                    result[x, y] = (ICloneable)grid[x, y].Clone();
                }
            }
            return result;
        }
    }
}