fixed matrix indexing error, but needs a faster solution