Partially modified for FPGA, not tested