Debugging the core algorithm. It seems the cost graph tree is wrong.