Cleaned up matrix code, still needs to be worked on