95% Completion

Working algorithm, needs final tweaks