/****************************************************/
/* Nom du fichier : fonction.h						*/
/*													*/
/* Role du fichier : Regroupe les declarations et	*/
/*					 les definitions des fonctions.	*/
/****************************************************/

#define NRANSI
#include "e:\guillemot\code_sources\nrutil.h"
#define MAXM 50
#include <math.h>
#define RADIX 2.0
#define SIGN2(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
#define pi 3.141592653

/* Definition du type complexe. */
typedef struct{
	double R, I;
} complexe;


/*	Pour commencer, il faut savoir qu'une matrice complexe utilisee par Matlab 
	est transferee au programme C sous la forme de deux vecteurs reels 
	representants la partie reelle et la partie imaginaire de la matrice.
	Ces vecteur sont realises par concatenation des colonnes de la matrice.
	Pour simplifier le programme, on representera la matrice complexe sous
	la forme d'un vecteur complexe. */



/*	Fonction qui, a partir de 2 vecteurs reels representants la partie
	reelle et la partie imaginaire d'une, cree le vecteur complexe
	representant la matrice complexe. */
complexe *reunion(double *xr, double *xi, int m, int n){
	int k;
	complexe *aux;
	aux=mxCalloc((m*n),sizeof(complexe));
	for	(k=0; k<m*n; k++){
		aux[k].R=xr[k];
		aux[k].I=xi[k];
	}
	return aux;
}


/*	Fonction qui, a partir d'un vecteur complexe representant une matrice
	va donner les vecteurs reel et imaginaire. */ 
void separation (double *r, double *i, complexe *u, int m, int n){
	int k;
	for (k=0; k<m*n; k++){
		r[k]=u[k].R;
		i[k]=u[k].I;
	}
}


/*	Fonction qui calcule le moment d'ordre 2. */
complexe moment2 (complexe *vec1, complexe *vec2, int dim){
	complexe aux;
	int k;
	aux.R=0;
	aux.I=0;
	for (k=0; k<dim; k++){
		aux.R=aux.R+(vec1[k].R*vec2[k].R)-(vec1[k].I*vec2[k].I);
		aux.I=aux.I+(vec1[k].R*vec2[k].I)+(vec1[k].I*vec2[k].R);
	}
	aux.R=aux.R/dim;
	aux.I=aux.I/dim;
	return aux;
}


/*	Fonction qui calcule le moment d'ordre 4. */
complexe moment4 (complexe *vec1, complexe *vec2, complexe *vec3, complexe *vec4, int dim){
	complexe aux;
	int k;
	aux.R=0;
	aux.I=0;
	for (k=0; k<dim; k++){
		aux.R=aux.R+(vec1[k].R*vec2[k].R-vec1[k].I*vec2[k].I)*(vec3[k].R*vec4[k].R-vec3[k].I*vec4[k].I);
		aux.R=aux.R-(vec1[k].R*vec2[k].I+vec1[k].I*vec2[k].R)*(vec3[k].R*vec4[k].I+vec3[k].I*vec4[k].R);
		aux.I=aux.I+(vec1[k].R*vec2[k].R-vec1[k].I*vec2[k].I)*(vec3[k].R*vec4[k].I+vec3[k].I*vec4[k].R);
		aux.I=aux.I+(vec1[k].R*vec2[k].I+vec1[k].I*vec2[k].R)*(vec3[k].R*vec4[k].R-vec3[k].I*vec4[k].I);
	}
	aux.R=aux.R/dim;
	aux.I=aux.I/dim;
	return aux;
}


/*	Fonction qui retourne la somme de 2 nombre complexes. */
complexe add (complexe nbr1, complexe nbr2){
	complexe aux;
	aux.R=nbr1.R+nbr2.R;
	aux.I=nbr1.I+nbr2.I;
	return aux;
}


/*	Fonction qui retourne la difference de 2 nombre complexes: nbr1-nbr2 */
complexe sub (complexe nbr1, complexe nbr2){
	complexe aux;
	aux.R=nbr1.R-nbr2.R;
	aux.I=nbr1.I-nbr2.I;
	return aux;
}


/*	Fonction qui cree la copie du vecteur complexe place en parametre. */
complexe *copieC(complexe* orig, int k){
	int i;
	complexe *aux;
	aux=mxCalloc(k,sizeof(complexe));
	for (i=0; i<k; i++){
		aux[i].R=orig[i].R;
		aux[i].I=orig[i].I;
	}
	return aux;
}


/*	Fonction qui retourne la multiplication d'un nombre complexe par un reel. */
complexe mul (complexe nbr, double n){
	complexe aux;
	aux.R=nbr.R*n;
	aux.I=nbr.I*n;
	return aux;
}


/*	Fonction qui retourne la multiplication de 2 nombres complexes. */
complexe prod (complexe nbr1,complexe nbr2){
	complexe aux;
	aux.R=nbr1.R*nbr2.R-nbr1.I*nbr2.I;
	aux.I=nbr1.R*nbr2.I+nbr2.R*nbr1.I;
	return aux;
}


/*	Fonction qui retourne la multiplicationc de 2 matrices representees sous forme 
	de vecteurs. Le resultat est un vecteur. */
/* x*y=a */
complexe *mulu1 (complexe *x, complexe *y, int mx, int nx, int ny){
	int i,j,k,p;
	int my=nx;
	complexe *aux;
	aux=mxCalloc((mx*ny),sizeof(complexe));
	for (i=0; i<mx; i++){
		for (j=0; j<ny; j++){
			k=i+j*mx;
			aux[k].R=0;
			aux[k].I=0;
			for (p=0; p<nx; p++){
				aux[k].R=aux[k].R+x[i+p*mx].R*y[p+j*my].R-x[i+p*mx].I*y[p+j*my].I;
				aux[k].I=aux[k].I+x[i+p*mx].R*y[p+j*my].I+y[p+j*my].R*x[i+p*mx].I;
			}
		}
	}
	return aux;
}

/* x*y=y */
complexe *mulu2 (complexe *x, complexe *y, int mx, int nx, int ny){
	int i,j,k,p;
	int my=nx;
	complexe *aux, *aux2;
	aux=mxCalloc((mx*ny),sizeof(complexe));
	aux2=copieC(y,(nx*ny));
	mxFree(y);
	for (i=0; i<mx; i++){
		for (j=0; j<ny; j++){
			k=i+j*mx;
			aux[k].R=0;
			aux[k].I=0;
			for (p=0; p<nx; p++){
				aux[k].R=aux[k].R+x[i+p*mx].R*aux2[p+j*my].R-x[i+p*mx].I*aux2[p+j*my].I;
				aux[k].I=aux[k].I+x[i+p*mx].R*aux2[p+j*my].I+aux2[p+j*my].R*x[i+p*mx].I;
			}
		}
	}
	mxFree(aux2);
	return aux;
}

/* x*y=x */
complexe *mulu3 (complexe *x, complexe *y, int mx, int nx, int ny){
	int i,j,k,p;
	int my=nx;
	complexe *aux, *aux2;
	aux=mxCalloc((mx*ny),sizeof(complexe));
	aux2=copieC(x,(mx*nx));
	mxFree(x);
	for (i=0; i<mx; i++){
		for (j=0; j<ny; j++){
			k=i+j*mx;
			aux[k].R=0;
			aux[k].I=0;
			for (p=0; p<nx; p++){
				aux[k].R=aux[k].R+aux2[i+p*mx].R*y[p+j*my].R-aux2[i+p*mx].I*y[p+j*my].I;
				aux[k].I=aux[k].I+aux2[i+p*mx].R*y[p+j*my].I+y[p+j*my].R*aux2[i+p*mx].I;
			}
		}
	}
	mxFree(aux2);
	return aux;
}



/*	Fonction qui retourne un vecteur de longueur n contenant les valeurs de la 
	ligne "ligne" de la matrice complexe m*n representee sous forme d'un vecteur 
	complexe "vec". */
complexe *lig_vec (complexe *vec, int ligne, int m, int n){
	int j;
	complexe *aux;
	aux=mxCalloc(n,sizeof(complexe));
	for (j=0; j<n; j++){
		aux[j]=vec[ligne+j*m];
	}
	return aux;
}


/*	Fonction qui retourne le vecteur complexe conjuguee du vecteur complexe 
	passe en parametre. */
complexe *conj (complexe *vec, int m, int n){
	int i;
	complexe *aux;
	aux=mxCalloc((m*n),sizeof(complexe));
	for (i=0; i<m*n; i++){
		aux[i].R=vec[i].R;
		aux[i].I=-vec[i].I;
	}
	return aux;
}


/*	Fonction qui retourne le conjugue du nombre complexe passe en parametre. */
complexe conj2 (complexe nbr){
	complexe aux;
	aux.R=nbr.R;
	aux.I=-nbr.I;
	return aux;
}


/*	Fonction qui retourne le vecteur complexe representant la matrice transposee 
	et conjuguee de la matrice representee par le vecteur complexe passe en parametre. */
complexe *conj3 (complexe *vec, int k){
	int i,j;
	complexe *aux, *aux2;
	aux2=copieC(vec,k*k);
	aux=mxCalloc((k*k),sizeof(complexe));
	for (j=0; j<k; j++){
		for (i=0; i<k; i++){
			aux[i+j*k].R=vec[j+i*k].R;
			aux[i+j*k].I=-vec[j+i*k].I;
		}
	}
	mxFree(aux2);
	return aux;
}



/*	Fonction qui retourne le module d'un nombre. */
complexe norm2 (complexe nbr){
	complexe aux;
	aux.R=sqrt(pow(nbr.R,2)+pow(nbr.I,2));
	aux.I=0;
	return aux;
}



/*	Fonction qui retourne l'argument du nombre complexe passe en parametre. */
double angle2 (complexe nbr){
	return atan2(nbr.I,nbr.R);
}


/*	Fonction qui retourne la partie reelle d'un nombre complexe passe en parametre. */
double reel (complexe nbr){
	return nbr.R;
}
		

/*	Fonction qui retourne un vecteur reel contenant les parties reelles des nombres 
	du vecteur complexe passe en parametre. */
double *reel2(complexe *vec, int n){
	double *aux;
	int i;
	aux=mxCalloc(n,sizeof(double));
	for (i=0; i<n; i++){
		aux[i]=vec[i].R;
	}
	return aux;
}


/*	Fonction qui retourne le max et l'indice du max du vecteur reel "vec*w". */
void max2 (double *Umax, int *iopt, double *vec, int n, double w){
	int i;
	Umax[0]=w*vec[0];
	iopt[0]=0;
	for (i=1; i<n; i++){
		if (w*vec[i]>Umax[0]){
			Umax[0]=w*vec[i];
			iopt[0]=i;
		}
	}
}


/*	Fonction qui copie le vecteur reel passe en parametre. */
double *copie(double* orig, int k){
	int i;
	double *aux;
	aux=mxCalloc(k,sizeof(double));
	for (i=0; i<k; i++){
		aux[i]=orig[i];
	}
	return aux;
}


/*	Fonction qui retourne un vecteur contenant les elements du vecteur 
	complexe "vec" de longueur d, ayant une partie imaginaire negative ou nulle. 
	H[0] represente la longueur du vecteur resultat. */
complexe *find1(complexe *vec, int d, int *K){
	complexe *aux;
	int i;
	aux=mxCalloc(d,sizeof(complexe));
	K[0]=0;
	for (i=0; i<d; i++){
		if (vec[i].I<=0){
			aux[K[0]++]=vec[i];
		}
	}
	return aux;
}


/*	Fonction qui retourne un vecteur contenant les indices pour
	lesquels le module des elements du vecteur "sin(beta+Phi)" est inferieur
	a 0.01.
	L[0] represente la longueur du vecteur resultat. */
int *find2(double *Phi, double beta, int k, int *L){
	int i,*aux;
	aux=mxCalloc(k,sizeof(int));
	L[0]=0;
	for (i=0; i<k; i++){
		if (fabs(sin(beta+Phi[i]))<0.01){
			aux[L[0]++]=i;
		}
	}
	return aux;
}


/*	Fonction qui retourne un vecteur d'entier rempli par les indices pour
	lesquels la partie imaginaire du vecteur passe en parametre est positive.
	M[0] represente la longueur de vecteur resultat. */
int *find3(complexe *vec, int k, int *M){
	int i,*aux;
	aux=mxCalloc(k,sizeof(int));
	M[0]=0;
	for (i=0; i<k; i++){
		if (vec[i].I>0){
			aux[M[0]++]=i;
		}
	}
	return aux;
}


/*	Fonction qui retourne un vecteur contenant les valeurs de "phi00" pour 
	les indices ou "vec" a une partie imagianire negative ou nulle. */
	double *raz1(double *phi00, complexe *vec, int k, int o){
	int i,j=0;
	double *aux;
	aux=mxCalloc((k-o),sizeof(double));
	for	(i=0; i<k; i++){
		if (vec[i].I<=0){
			aux[j++]=phi00[i];}
	}
	return aux;
}


/*	Fonction qui retourne un vecteur contenant la partie reelle des valeurs 
	de "vec" pour les indices ou "vec" a une partie imaginaire negative ou nulle. */
double *raz2(complexe *vec, int k, int o){
	int i,p=0;
	double *aux;
	aux=mxCalloc((k-o),sizeof(double));
	for	(i=0; i<k; i++){
		if (vec[i].I<=0){
			aux[p++]=vec[i].R;}
	}
	return aux;
}


/*	Fonction qui retourne un vecteur contenant les valeurs de "Phi2" pour 
	les indices ou le module des donnees de "sin(Phi2+beta)" est superieur
	ou egale a 0.01. */
double *raz3(double *Phi2, double beta, int lp, int l){
	double *aux;
	int i;
	int j=0;
	aux=mxCalloc((lp-l),sizeof(double));
	for (i=0; i<lp; i++){
		if (fabs(sin(beta+Phi2[i]))>=0.01){
			aux[j++]=Phi2[i];
		}
	}
	return aux;
}


/*	Fonction qui retourne une matrice identite de dimension r*r. */
complexe *id(int r){
	int i,j;
	complexe *aux;
	aux = mxCalloc((r*r),sizeof(complexe));
	for (j=0; j<r; j++){
		for (i=0; i<r; i++){
			aux[i+j*r].R=0;
			aux[i+j*r].I=0;
			if (i==j){
				aux[i+j*r].R=1;
				aux[i+j*r].I=0;
			}
		}
	}
	return aux;
}





/*	Fonctions tiree des recipies necesaire pour la resolution d'un polynome d'ordre n. */
void balanc(double **a, int n)
{
	int last,j,i;
	double s,r,g,f,c,sqrdx;

	sqrdx=RADIX*RADIX;
	last=0;
	while (last == 0) {
		last=1;
		for (i=1;i<=n;i++) {
			r=c=0.0;
			for (j=1;j<=n;j++)
				if (j != i) {
					c += fabs(a[j][i]);
					r += fabs(a[i][j]);
				}
			if (c && r) {
				g=r/RADIX;
				f=1.0;
				s=c+r;
				while (c<g) {
					f *= RADIX;
					c *= sqrdx;
				}
				g=r*RADIX;
				while (c>g) {
					f /= RADIX;
					c /= sqrdx;
				}
				if ((c+r)/f < 0.95*s) {
					last=0;
					g=1.0/f;
					for (j=1;j<=n;j++) a[i][j] *= g;
					for (j=1;j<=n;j++) a[j][i] *= f;
				}
			}
		}
	}
}


void hqr(double **a, int n, double wr[], double wi[])
{
	int nn,m,l,k,j,its,i,mmin;
	double z,y,x,w,v,u,t,s,r,q,p,anorm;

	anorm=fabs(a[1][1]);
	for (i=2;i<=n;i++)
		for (j=(i-1);j<=n;j++)
			anorm += fabs(a[i][j]);
	nn=n;
	t=0.0;
	while (nn >= 1) {
		its=0;
		do {
			for (l=nn;l>=2;l--) {
				s=fabs(a[l-1][l-1])+fabs(a[l][l]);
				if (s == 0.0) s=anorm;
				if ((double)(fabs(a[l][l-1]) + s) == s) break;
			}
			x=a[nn][nn];
			if (l == nn) {
				wr[nn]=x+t;
				wi[nn--]=0.0;
			} else {
				y=a[nn-1][nn-1];
				w=a[nn][nn-1]*a[nn-1][nn];
				if (l == (nn-1)) {
					p=0.5*(y-x);
					q=p*p+w;
					z=sqrt(fabs(q));
					x += t;
					if (q >= 0.0) {
						z=p+SIGN2(z,p);
						wr[nn-1]=wr[nn]=x+z;
						if (z) wr[nn]=x-w/z;
						wi[nn-1]=wi[nn]=0.0;
					} else {
						wr[nn-1]=wr[nn]=x+p;
						wi[nn-1]= -(wi[nn]=z);
					}
					nn -= 2;
				} else {
					if (its == 30) nrerror("Too many iterations in hqr");
					if (its == 10 || its == 20) {
						t += x;
						for (i=1;i<=nn;i++) a[i][i] -= x;
						s=fabs(a[nn][nn-1])+fabs(a[nn-1][nn-2]);
						y=x=0.75*s;
						w = -0.4375*s*s;
					}
					++its;
					for (m=(nn-2);m>=l;m--) {
						z=a[m][m];
						r=x-z;
						s=y-z;
						p=(r*s-w)/a[m+1][m]+a[m][m+1];
						q=a[m+1][m+1]-z-r-s;
						r=a[m+2][m+1];
						s=fabs(p)+fabs(q)+fabs(r);
						p /= s;
						q /= s;
						r /= s;
						if (m == l) break;
						u=fabs(a[m][m-1])*(fabs(q)+fabs(r));
						v=fabs(p)*(fabs(a[m-1][m-1])+fabs(z)+fabs(a[m+1][m+1]));
						if ((double)(u+v) == v) break;
					}
					for (i=m+2;i<=nn;i++) {
						a[i][i-2]=0.0;
						if (i != (m+2)) a[i][i-3]=0.0;
					}
					for (k=m;k<=nn-1;k++) {
						if (k != m) {
							p=a[k][k-1];
							q=a[k+1][k-1];
							r=0.0;
							if (k != (nn-1)) r=a[k+2][k-1];
							if ((x=fabs(p)+fabs(q)+fabs(r)) != 0.0) {
								p /= x;
								q /= x;
								r /= x;
							}
						}
						if ((s=SIGN2(sqrt(p*p+q*q+r*r),p)) != 0.0) {
							if (k == m) {
								if (l != m)
								a[k][k-1] = -a[k][k-1];
							} else
								a[k][k-1] = -s*x;
							p += s;
							x=p/s;
							y=q/s;
							z=r/s;
							q /= p;
							r /= p;
							for (j=k;j<=nn;j++) {
								p=a[k][j]+q*a[k+1][j];
								if (k != (nn-1)) {
									p += r*a[k+2][j];
									a[k+2][j] -= p*z;
								}
								a[k+1][j] -= p*y;
								a[k][j] -= p*x;
							}
							mmin = nn<k+3 ? nn : k+3;
							for (i=l;i<=mmin;i++) {
								p=x*a[i][k]+y*a[i][k+1];
								if (k != (nn-1)) {
									p += z*a[i][k+2];
									a[i][k+2] -= p*r;
								}
								a[i][k+1] -= p*q;
								a[i][k] -= p;
							}
						}
					}
				}
			}
		} while (l < nn-1);
	}
}


void zrhqr(double a[], int m, double rtr[], double rti[])
{
	void balanc(double **a, int n);
	void hqr(double **a, int n, double wr[], double wi[]);
	int j,k;
	double **hess,xr,xi;

	hess=matrix(1,MAXM,1,MAXM);
	if (m > MAXM || a[m] == 0.0) nrerror("bad args in zrhqr");
	for (k=1;k<=m;k++) {
		hess[1][k] = -a[m-k]/a[m];
		for (j=2;j<=m;j++) hess[j][k]=0.0;
		if (k != m) hess[k+1][k]=1.0;
	}
	balanc(hess,m);
	hqr(hess,m,rtr,rti);
	for (j=2;j<=m;j++) {
		xr=rtr[j];
		xi=rti[j];
		for (k=j-1;k>=1;k--) {
			if (rtr[k] <= xr) break;
			rtr[k+1]=rtr[k];
			rti[k+1]=rti[k];
		}
		rtr[k+1]=xr;
		rti[k+1]=xi;
	}
	free_matrix(hess,1,MAXM,1,MAXM);
}
#undef MAXM
#undef NRANSI
#undef RADIX