Updated some documentation, added DEBUG() macro to be able to disable/enable debug prints easier, started fixing test bench
Fixed test bench mostly to meet the latest implementation.