Milestone 2

Working algorithm, needs fine tuning and more granularity.