Cleaned the code up A LOT. Added more maths.