Algorithm Should be working.  Just need to clean up, test, and make it look nice.