Some fixes (not completely tested).

Note: There are printfs to remove from ames later.
Crub implementation that seems to be working.