Organized an algorithm a bit more