Added implementation of C3 linearization
Slight clean up
Clean up and generalize code