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

#include "./Orthx.h"

#undef __FUNCT__ 
#define __FUNCT__ "Orthx"

void	Orthx( Vec* pnewgradj, Vec gradj, Vec oldgradj){

	/*output: */
	Vec newgradj=NULL;

	/*intermediary:*/
	Vec oldgradj2=NULL;
	double norm,dot_product;;


	if (!oldgradj){
		VecDuplicate(gradj,&oldgradj2);
		VecCopy(gradj,oldgradj2);
		/*scale to 1:*/
		VecNorm(oldgradj2,NORM_INFINITY,&norm);
		VecScale(oldgradj2,1.0/norm);
	}

	/*% normalize gradient to 1:*/
	VecDuplicate(gradj,&newgradj);
	VecCopy(gradj,newgradj);
	VecNorm(newgradj,NORM_INFINITY,&norm);
	VecScale(newgradj,1.0/norm);

	/*rough orthagonalization
	gradient=gradient-(gradient'*oldgradient)*oldgradient; 
	*/

	if(oldgradj){
		VecDot(newgradj,oldgradj,&dot_product);
		VecAXPY(newgradj, -dot_product,oldgradj);
	}
	else{
		VecDot(newgradj,oldgradj2,&dot_product);
		VecAXPY(newgradj, -dot_product,oldgradj2);
	}

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

	/*Free ressources and return*/
	VecFree(&oldgradj2);
}
