Clean up code
Matrix should be able to store energy now