Sub2Ind indexing issue fix

Need to test these changes still