Algorithm - done...
(improved, connected to camera + debug information)