Mostly working.  Need to clean up code and double check algos