Your algorithm should be modified in the following way:
1. Detect the code phase offset and carrier doppler shift using your
acquisition algorithm (by finding the peak signal power at a particular
Doppler offset).
2. Track the code offset using a delay-locked loop (DLL)
3. Track the carrier frequency using a FLL or the frequency and phase
using a PLL.
An good text on GPS receiver design using software receivers is
Fundamentals of Global Positioning Receivers: A Software Approach by
James Tsui, Wiley and Sons, 2000.
Regards,
Brent