Updates to comments and readability. Some matrices preallocated for speed.