First working iteration of code. Needs callibration