Another good thing I've found is counting millis between functions so you can see speed and change things to compare, this helps optimize the code.
Create some variables like below
unsigned long startMilli;
unsigned long stopMilli;
unsigned long loadMilli;
Then add this right before the function starts
startMilli = millis();
And add this right after the function ends
stopMilli = millis();
loadMilli = stopMilli - startMilli;
Serial.print(F("Load Millis: "));
Serial.println(loadMilli);
Whatever "load millis" shows in the serial monitor is how long in milliseconds it takes to run.
Also remember Serial.prints dramatically slow down your code so comment them out before you do a speed test and when you are certain everything will be ok. 