An attempt to rewrite algorithm in c#