// 11/07/00

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

// Définition du type complexe.
typedef struct{
	double R, I;
} complexe;


/*	Pour commencer, il faut savoir q'une matrice complexe utilisée par Matlab 
	est transférée au programme C sous la forme de deux vecteurs réels 
	représentant la partie réelle et la partie imaginaire de la matrice.
	Ces vecteur sont réalisés par concaténation des colonnes de la matrice.
	Pour simplifier le programme, on représentera la matrice complexe sous
	la forme d'un vecteur complexe. */



/*	Fonction qui, à partir de 2 vecteurs réels représentant la partie
	réelle et la partie imaginaire d'une, crée le vecteur complexe
	représentant 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, à partir d'un vecteur complexe représentant une matrice
	va donner les vecteurs réel 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 différence 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 crée la copie du vecteur complexe placé en paramètre. */
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 réel. */
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 membre à membre de 2 vecteurs complexes. */
complexe *multi (complexe *vec1, complexe *vec2, int m, int n){
	int k;
	complexe *aux;
	aux=mxCalloc((m*n),sizeof(complexe));
	for (k=0;  k<m*n; k++){
		aux[k].R=vec1[k].R*vec2[k].R-vec1[k].I*vec2[k].I;
		aux[k].I=vec1[k].R*vec2[k].I+vec2[k].R*vec1[k].I;
	}
	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 représentées sous forme 
	de vecteurs. Le résultat est un vecteur. */
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;
}
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;
}
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 représentée 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 conjuguée du vecteur complexe 
	passé en paramètre. */
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 conjugué du nombre complexe passé en paramètre. */
complexe conj2 (complexe nbr){
	complexe aux;
	aux.R=nbr.R;
	aux.I=-nbr.I;
	return aux;
}


/*	Fonction qui retourne le vecteur complexe représentant la matrice transposée 
	et conjuguée de la matrice représentée par le vecteur complexe passé en paramètre. */
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 la moyenne d'un vecteur complexe de longueur n
	passé en paramètre. */
complexe mean2 (complexe *vec, int n){
	int j;
	complexe aux;
	aux.R=0;
	aux.I=0;
	for (j=0; j<n; j++){
		aux.R=aux.R+vec[j].R;
		aux.I=aux.I+vec[j].I;
	}
	aux.R=aux.R/n;
	aux.I=aux.I/n;
	mxFree(vec);
	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 passé en paramètre. */
double angle2 (complexe nbr){
	return atan2(nbr.I,nbr.R);
}


/*	Fonction qui retourne la partie réelle d'un nombre complexe passé en paramètre. */
double reel (complexe nbr){
	return nbr.R;
}
		

/*	Fonction qui retourne un vecteur réel contenant les parties réelles des nombres 
	du vecteur complexe passé en paramètre. */
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 réel "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 réel passé en paramètre. */
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 éléments du vecteur 
	complexe "vec" de longueur d, ayant une partie imaginaire négative ou nulle. 
	H[0] représente 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 éléments du vecteur "sin(beta+Phi)" est inférieur
	à 0.01.
	L[0] représente la longueur du vecteur résultat. */
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 passé en paramètre est positive.
	M[0] représente la longueur de vecteur résultat. */
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 où "vec" a une partie imagianire négative 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 réelle des valeurs 
	de "vec" pour les indices où "vec" a une partie imaginaire négative 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 où le module des données de "sin(Phi2+beta)" est supérieur
	ou égale à 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 identité 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 tirée des récipies nécésaire pour la résolution 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