Optimizations

Doubles instead of floats.
Single time variable instead of calling millis() in several functions.
Removed passing some globals as a parameter.