Works abit, need to change constants a bit