Modified for FPGA, tested on Xeon CPU