Algorithm write in Java