Add more real working test and fix reference out of bounds checking