Code cleanup, added some distance functions/angle