Added some optimizations. Removed math.h dependancy