modified, I think it is right, but testbench has some problem