Use a matrix class.  Works now on FPGA.