writing algorithm in c for speed