Algorithm in C