I think this is working. Now to do this in C...