Got the algorithm working -- needs cleanup work