Cleaning up memory allocation and some potential dereferences of NULL pointers.
cleanup
Infinite woops fixed.
Replacing matrix pointer with matrix.