Solved my problem and commented to explain algorithm