It's a bit difficult to read but I think it should work.  Can you make a unit test for it?