I am developing a software for fundamental frequency tracking. For this purpose, I have designed a function which calculates autocorrelation over the signal and a second function which, based on autocorrelation result, finds the peak corresponding to the fundamental frequency.
However the code I have written is not very reliable. What I want to know is: which algorithms are reliable for fundamental frequency peak search?
EDIT
My autocorrelation plot looks as follows so far, when done in MATLAB:

however, I implemented it in C, because my software is in C, as follows:
91 for(;i < *length;i++) {
92 for(j = i;j < *length + i;j++) {
93 if(j > i) {
94 sum += 0;
95 } else {
96 sum += samples[i] * samples[j];
97 }
98 }
99
100 result[i] = sum;
101 sum = 0.0f;
102 }
with i starting in 0. I also believe this code to be OK.
Approximating the above plot in MATLAB, I see this, where the marked peak would be my fundamental frequency one (the note I'm testing with is an E2)

as we can see from this plot

the signal I'm dealing with is quasiperiodic and the difference between those peaks actually result in the fundamental frequency for E2 note: the distance between the two is 389 - 193 == 196 (values obtained from drag & drop of MATLAB plot), which, divided by my sampling rate of 16 kHz, results in 0.01225, that would be my wave period. Inverting this value I obtain 81.6326530612 Hz, which is very near to my expected 82.41 Hz for E2.
However I'm having a real bad time trying to locate these two peaks in code, since I'm not sure what to compare to be sure the peaks I found are actually these two peaks.