Колышки и колечки - решение

Как и обещал ранее предлагаю свое решение к задаче из предыдущего поста. Правда не было времени прокомментировать - сделаю позже.

import java.io.ByteArrayInputStream;
import java.io.InputStream;
import java.util.LinkedList;
import java.util.Queue;
import java.util.Scanner;

public class Solution {
 private static final int DEPTH_LIMIT = 6;
 private static final int MOVE_BIT_SIZE = 3;
 private int discsCount;
 private int pegsCount;
 private long startPegs;
 private long finalPegs;
 private int mask;
 
 public void init(InputStream is) {
  Scanner in = new Scanner(is);
  String str = in.nextLine();
  String[] tokens = str.split(" ");
  discsCount = Integer.parseInt(tokens[0]);
  pegsCount = Integer.parseInt(tokens[1]);
  startPegs = readConfig(in.nextLine());
  finalPegs = readConfig(in.nextLine());
  mask = (1 << discsCount) - 1;
 }

 private long readConfig(String str){
  String[] tokens = str.split(" ");
  int k = 0;
  int[] startCfg = new int[pegsCount];
  for (int i = 0; i < discsCount; i++) {
   k = Integer.parseInt(tokens[i]) - 1;
   startCfg[k] = startCfg[k] | (1 << i);
  }
  k = 0;
  long config = 0l;
  for (long peg : startCfg) {
   config |= peg << (discsCount * k++);
  }
  return config;
 }
 
 public void find() {
  Queue<Long> q = new LinkedList<Long>();//pegs configurations
  Queue<Integer> p = new LinkedList<Integer>();//moves path
  q.add(startPegs);//start from initial configuration
  int parentPath = 0;//parent path
  p.add(parentPath);
  int path = 0;//child path
  long curCfg = 0;//current configuration
  int movesCnt = 0;//count of moves
  int fromPeg = 0; int toPeg = 0;
  long fromPegCfg = 0; long toPegCfg = 0;
  int fromLeastBit = 0; int toLeastBit = 0;
  int[] levelSize = new int[DEPTH_LIMIT + 1];
  levelSize[0] = 1;
  while (!q.isEmpty() && movesCnt < DEPTH_LIMIT) {
   curCfg = q.poll();
   parentPath = p.poll();
   for (int i = 0; i < pegsCount; i++) {
    fromPegCfg = (curCfg >>> (discsCount * i)) & mask;
    fromLeastBit = discsCount;
    if (fromPegCfg > 0){
     fromLeastBit = 0;
     while (((fromPegCfg >>> fromLeastBit) & 1) == 0) {
      fromLeastBit++;
     }
    }
    for (int j = i + 1; j < pegsCount; j++) {
     fromPeg = i;
     toPeg = j;
     toPegCfg = (curCfg >>> (discsCount * j)) & mask;
     if (fromPegCfg == toPegCfg){
      //exclude empty pegs
      continue;
     }
     toLeastBit = discsCount;
     if (toPegCfg > 0){
      toLeastBit = 0;
      while (((toPegCfg >>> toLeastBit) & 1) == 0) {
       toLeastBit++;
      }
     }
     if (fromLeastBit > toLeastBit){
      fromPeg = j;
      toPeg = i;
     } else {
      toLeastBit = fromLeastBit;
     }
     toPegCfg = curCfg;
     toPegCfg |= (1 << ((discsCount * toPeg) + toLeastBit));
     toPegCfg ^= (1 << ((discsCount * fromPeg) + toLeastBit));
     assert checkDiscs(toPegCfg) : 
      i + ":" + j + " " + 
      Long.toBinaryString(toPegCfg) + "->" + Long.toBinaryString(toPegCfg);
     if (toPegCfg == finalPegs) {
      long movesPath = parentPath;
      movesPath |= (fromPeg << (movesCnt * 2 * MOVE_BIT_SIZE));
      movesPath |= (toPeg << ((movesCnt * 2 + 1) * MOVE_BIT_SIZE));
      print(movesCnt + 1, movesPath);
      return;
     } else if (movesCnt < DEPTH_LIMIT) {
      q.add(toPegCfg);
      path = parentPath | (fromPeg << (movesCnt * 2 * MOVE_BIT_SIZE));
      path |= (toPeg << ((movesCnt * 2 + 1) * MOVE_BIT_SIZE));
      p.add(path);
     }
     levelSize[movesCnt + 1]++;
    }
   }
   if (--levelSize[movesCnt] == 0) {
    movesCnt++;
   }
  }
 }
 
 private boolean checkDiscs(long config){
  long checked = 0;
  for(int i=0; i < pegsCount; i++){
   checked += (config >>> (i * discsCount)) & mask;
  }
  return checked == mask;
 }
 
 public void print(int movesCount, long movesPath){
  System.out.println(movesCount);
  int moveMask = (1 << MOVE_BIT_SIZE) - 1;
  int k = 0;
  int pegFrom = 0;
  int pegTo = 0;
  while(k < movesCount){
   pegFrom = (int)(movesPath >>> (k * 2 * MOVE_BIT_SIZE)) & moveMask;
   pegTo = (int)(movesPath >>> ((k * 2 + 1) * MOVE_BIT_SIZE)) & moveMask;
   System.out.println((pegFrom + 1) + " " + (pegTo + 1));
   k++;
  }
 }

 /**
  * @param args
  */
 public static void main(String[] args) {
  Solution solution = new Solution();
  //String input = "6 4\n" + "4 2 4 3 1 1\n" + "4 4 4 3 2 1\n";
  //ByteArrayInputStream bais = new ByteArrayInputStream(input.getBytes());
  //solution.init(bais);
  solution.init(System.in);
  solution.find();
 }
}