Milestone 3

Algorithm working, needs fine tuning, maybe granularity.