void solve()
 {
     navigateSolution(new object[] { -1 });
     queue.Clear();
     visited.Clear();
     Solution.Clear();
     solutionFound = null;
     // Create intial Position and push it
     //Position.SlidingBlock[] bs = new Position.SlidingBlock[Blocks.Count];
     byte[] bs = new byte[Blocks.Count * 3];
     //for (int i = 0; i < Blocks.Count; i++)
         //bs[i] = new Position.SlidingBlock((byte)Blocks[i].CellX, (byte)Blocks[i].CellY, (byte)Blocks[i].BlockType.TypeID);
     for (int i = 0; i < Blocks.Count; i++)
     {
         bs[i * 3] = (byte)Blocks[i].CellX;
         bs[i * 3 + 1] = (byte)Blocks[i].CellY;
         bs[i * 3 + 2] = (byte)Blocks[i].BlockType.TypeID;
     }
     Position pos = new Position(bs, 0, 0, 0, null);
     queue.Enqueue(pos);
     visited.Add(pos);
     while (queue.Count > 0 && solutionFound == null)
     {
         pos = queue.Dequeue();
         for (int y = 0; y < CellsY; y++) for (int x = 0; x < CellsX; x++) occupied[x, y] = false;           // Clear the occupied grid
         for (int i = 0; i < pos.Blocks.Length; i+=3)
             putBlock(pos.Blocks[i], pos.Blocks[i+1], blockPalette[pos.Blocks[i+2]].SubBlocks, true);              // Fill the occupied grid
         for (int i=0; i<pos.Blocks.Length;i+=3)
         {
             SlidingBlockType block = blockPalette[pos.Blocks[i+2]];
             if (pos.Blocks[i] > 0) tryPushMove(i, -1, 0, pos);
             if (pos.Blocks[i+1] > 0) tryPushMove(i, 0, -1, pos);
             if (pos.Blocks[i] + block.SizeX < CellsX) tryPushMove(i, 1, 0, pos);
             if (pos.Blocks[i+1] + block.SizeY < CellsY) tryPushMove(i, 0, 1, pos);
         }
     }
     if (solutionFound != null)
     {
         Stack<Move> stack = new Stack<Move>();
         while (solutionFound.PreviousPosition != null)
         {
             stack.Push(new Move(solutionFound.BlockNr, solutionFound.Xdir, solutionFound.Ydir));
             solutionFound = solutionFound.PreviousPosition;
         }
         while (stack.Count > 0)
             Solution.Add(stack.Pop());          // Reverse order
     }
     for (int y = 0; y < CellsY; y++) for (int x = 0; x < CellsX; x++) occupied[x, y] = false;           // Clear the occupied grid
     foreach (SlidingBlock block in Blocks)
         putBlock(block.CellX, block.CellY, block.BlockType.SubBlocks, true);              // Fill the occupied grid
 }
 void tryPushMove(int blockPos, int xdir, int ydir, Position pos)
 {
     int x = pos.Blocks[blockPos];
     int y = pos.Blocks[blockPos+1];
     int blockType = pos.Blocks[blockPos+2];
     IEnumerable<Point> subBlocks = blockPalette[blockType].SubBlocks;
     putBlock(x, y, subBlocks, false);                       // Remove it from occupied
     if (subBlocks.All(t => !occupied[x + xdir + t.X, y + ydir + t.Y]))          // Can it move?
     {
         pos.Blocks[blockPos] += (byte)xdir;
         pos.Blocks[blockPos+1] += (byte)ydir;
         Position newPos = new Position(pos.Blocks, blockPos/3, xdir, ydir, pos);       // Create newPos with the moved block
         if (blockType == TargetBlocks[0].BlockType.TypeID && pos.Blocks[blockPos] == TargetBlocks[0].CellX && pos.Blocks[blockPos+1] == TargetBlocks[0].CellY)
             solutionFound = newPos;
         pos.Blocks[blockPos] -= (byte)xdir;
         pos.Blocks[blockPos+1] -= (byte)ydir;
         if (visited.Add(newPos))
             queue.Enqueue(newPos);                      // Push it
     }
     putBlock(x, y, subBlocks, true);                    // Fill occupied back in
 }
 public Position(byte[] blocks, int blockNr, int xdir, int ydir, Position previousPosition)
 {
     Blocks = new byte[blocks.Length];
     blocks.CopyTo(Blocks, 0);
     this.BlockNr = blockNr;
     this.Xdir = xdir;
     this.Ydir = ydir;
     PreviousPosition = previousPosition;
 }