Basic algo working

Right now the code is a fucking mess. After last part is working it'll
need to be reworked big time