Got everything to work. ToDo: Code cleanup and put all the math stuff into seperate class