Cleaned up heuristic and added test - it works!!