/*!\file Orthx
 * \brief orthogonalize searching directions for inverse control methods
 */

#include "./Orthx.h"

void	Orthx( Vector<IssmDouble>** pnewgradj, Vector<IssmDouble>* gradj, Vector<IssmDouble>* oldgradj){

	/*output: */
	Vector<IssmDouble>* newgradj=NULL;

	/*intermediary:*/
	IssmDouble norm_old,dot_product;;

	/*Initialize output*/
	newgradj=gradj->Duplicate();
	gradj->Copy(newgradj);

	/*rough orthagonalization
	gradient=gradient-(gradient'*oldgradient)*oldgradient /norm(oldgradient)^2; */
	if(oldgradj){
		norm_old=oldgradj->Norm(NORM_TWO);
		_assert_(norm_old!=0); 
		dot_product=newgradj->Dot(oldgradj);
		_assert_(dot_product!=NAN); 
		newgradj->AXPY(oldgradj,-dot_product/pow(norm_old,2));
	}

	/*Assign correct pointer*/
	*pnewgradj=newgradj;
}
