Finished implementation. All test pass.
Included testbench output and some pics from waveforms.