Initial implementation of full pivoting
Fix full pivoting