Will add some comments, but I think this is solved.
Added comments.