Tested and fixed givens rotation