/************************************************************/
/* Nom du fichier  : tetan_nv.c								*/
/*															*/
/* Role du fichier : Calcul la meilleur matrice separante	*/
/*					 a partir de deux signaux observes.		*/
/************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include <math.h>
#include "e:\matlab53\extern\include\mex.h"
#include "e:\guillemot\code_sources\fonctions.h"




/*******************************************/
/************* Programme en C **************/
/*******************************************/


complexe *tetan (complexe **adr, complexe *z, double w, int m, int n){

/*************** Declarations *****************/


	double tol=2.22*pow(10,-12)/n;
	complexe *z1, *z1c, *z2, *z2c, *u, *theta;
	complexe m11, m11n, m22, m22n, m12, m12n;
	complexe m1111, m1112, m2112, m1122, m2122, m2222;
	complexe c1111, c1112, c2112, c1122, c2122, c2222;
	complexe b1;
	double a,beta,b,b2,delta,d,d2,f,mu,cm2,tempo;
	complexe r1, *r;
	double *Ups, *Umax;
	int *iopt;
	double *Phi,*cpsi,*sigma,*R,*r2,*phi,*Ups0,*Ups1,*Phi2;
	complexe *Theta, *theta0;
	double *T1, *R0, *R02, *R2;
	complexe *T, *Rj, *R00, *T2;
	int i=0,j=0,k=0,l=0,o=0,p=0,lp=0;
	int *K, *L, *J, *O, *JJ;
	double phij, cpb, *Phi0, *Phi00, *Phi1, *ro, ro0, rr, D, cc;
	double *h, *rtr, *rti, *coef;



/******************************************************/


	z1=lig_vec(z,0,m,n);
	z1c=conj(z1,1,n);
	z2=lig_vec(z,1,m,n);
	z2c=conj(z2,1,n);


	/* Calcul des moments d'ordre 2. */
	m11  = moment2(z1,z1c,n);
	m11n = moment2(z1,z1,n);
	m22  = moment2(z2,z2c,n);
	m22n = moment2(z2,z2,n);
	m12  = moment2(z1c,z2,n);
	m12n = moment2(z1,z2,n);



	/* Calcul des moments d'ordre 4 (le contraste ne fait intervenir que les cumulants ayant
	   autant de puissances complexes que de puissances complexes conjuguees). */
	m1111=moment4(z1,z1c,z1c,z1,n);
	m1112=moment4(z1,z1c,z1c,z2,n);
	m2112=moment4(z2,z1c,z1c,z2,n);
	m1122=moment4(z1,z1c,z2c,z2,n);
	m2122=moment4(z2,z1c,z2c,z2,n);
	m2222=moment4(z2,z2c,z2c,z2,n);


	mxFree(z1); mxFree(z1c); mxFree(z2); mxFree(z2c); 

	
	/* Calcul des cumulants à l'ordre 4. */
	c1111=sub(sub(m1111,mul(prod(m11,m11),2)),prod(norm2(m11n),norm2(m11n)));
	c1112=sub(sub(m1112,mul(prod(m11,m12),2)),prod(m12n,conj2(m11n)));
	c2112=sub(sub(m2112,mul(prod(m12,m12),2)),prod(m22n,conj2(m11n)));
	c1122=sub(sub(sub(m1122,prod(m11,m22)),prod(m12,conj2(m12))),prod(m12n,conj2(m12n)));
	c2122=sub(sub(m2122,mul(prod(m12,m22),2)),prod(m22n,conj2(m12n)));
	c2222=sub(sub(m2222,mul(prod(m22,m22),2)),prod(norm2(m22n),norm2(m22n)));

	c1122.I=0;
	c1111.I=0;
	c2222.I=0;

	/* Variables auxiliaires. */
	a=c1111.R+c2222.R;
	b1=sub(c2122,c1112);
	beta=angle2(b1); b=reel(norm2(b1)); b2=pow(b,2);
	delta=angle2(c2112); d=reel(norm2(c2112)); d2=pow(d,2);
	f=4*reel(c1122)+reel(c1111)+reel(c2222);
	mu=delta-2*beta; cm2=pow(cos(mu),2);
	tempo=pow(d,2)*sin(mu)*cos(mu);

	if (b<tol){
		r1.R=cos(-delta/2); r1.I=sin(-delta/2);
		r=mxCalloc(3,sizeof(complexe));
		r[0]=r1;
		r[1].R=-r1.I; r[1].I=r1.R;
		r[2].R=0; r[2].I=0;
		Ups=mxCalloc(3,sizeof(double));
		Ups[0]=f/2+d; Ups[1]=f/2-d; Ups[2]=a;
		Umax=mxCalloc(1,sizeof(double));
		iopt=mxCalloc(1,sizeof(int));
		max2(Umax,iopt,Ups,3,w);
		mxFree(Ups);
		theta=malloc(1*sizeof(complexe));
		theta[0]=r[iopt[0]];
		mxFree(r); mxFree(Umax); mxFree(iopt);
		*adr=theta;
		u=mxCalloc(4,sizeof(complexe));
		u[0].R=1; u[0].I=0;
		u[1].R=-theta[0].R; u[1].I=theta[0].I;
		u[2].R=theta[0].R; u[2].I=theta[0].I;
		u[3].R=1; u[3].I=0;
		for (k=0; k<4; k++){
			u[k].R=u[k].R/(sqrt(2));
			u[k].I=u[k].I/(sqrt(2));
		}
		return u;
	}
		
	/* Recherche de la phase phi du sinus de la rotation. */
	/*--------------------------------------------------  */
	/* Recherche de la tangente de (phi+beta)/2.          */
	else{
		if (fabs(cm2-1)<tol){
			Phi=mxCalloc(2,sizeof(double));
			Phi[0]=-delta/2; Phi[1]=-delta/2+pi/2;
			Theta=mxCalloc(5,sizeof(complexe));
			Theta[0].R=cos(Phi[0]); Theta[0].I=sin(Phi[0]);
			Theta[1].R=cos(Phi[1]); Theta[1].I=sin(Phi[1]);
			Ups=mxCalloc(5,sizeof(double));
			Ups[0]=f/2+d; Ups[1]=f/2-d;
			cpsi=mxCalloc(2,sizeof(double));
			cpsi[0]=-1; cpsi[1]=1;
			sigma=mxCalloc(2,sizeof(double));
			sigma[0]=(f-2*a+2*d*cos(mu))/(b*cpsi[0]);
			sigma[1]=(f-2*a+2*d*cos(mu))/(b*cpsi[1]);
			R=mxCalloc(2,sizeof(double));
			R[0]=(-sigma[0]+sqrt(16+pow(sigma[0],2)))/2;
			R[1]=(-sigma[1]+sqrt(16+pow(sigma[1],2)))/2;
			mxFree(sigma);
			Phi[0]=pi-beta; Phi[1]=-beta;
			r2=mxCalloc(2,sizeof(double));
			r2[0]=(-R[0]+sqrt(4+pow(R[0],2)))/2;
			r2[1]=(-R[1]+sqrt(4+pow(R[1],2)))/2;
			phi=mxCalloc(2,sizeof(double));
			phi[0]=Phi[0]+pi; phi[1]=Phi[1]+pi;
			mxFree(Phi);
			theta0=mxCalloc(2,sizeof(complexe));
			theta0[0].R=r2[0]*cos(phi[0]); theta0[0].I=r2[0]*sin(phi[0]);
			theta0[1].R=r2[1]*cos(phi[1]); theta0[1].I=r2[1]*sin(phi[1]);
			mxFree(r2);
			mxFree(phi);
			Ups0=mxCalloc(2,sizeof(double));
			Ups0[0]=(a*pow(R[0],2)+4*b*R[0]*cpsi[0]+4*d*cos(mu)+2*f)/(pow(R[0],2)+4);
			Ups0[1]=(a*pow(R[1],2)+4*b*R[1]*cpsi[1]+4*d*cos(mu)+2*f)/(pow(R[1],2)+4);
			mxFree(cpsi);
			mxFree(R);
			Theta[2].R=theta0[0].R; Theta[2].I=theta0[0].I;
			Theta[3].R=theta0[1].R; Theta[3].I=theta0[1].I;
			mxFree(theta0);
			Theta[4].R=0; Theta[4].I=0;
			Ups[2]=Ups0[0];
			Ups[3]=Ups0[1];
			mxFree(Ups0);
			Ups[4]=a;
			Umax=mxCalloc(1,sizeof(double));
			iopt=mxCalloc(1,sizeof(int));
			max2(Umax,iopt,Ups,5,w);
			mxFree(Ups);
			theta=malloc(1*sizeof(complexe));
			theta[0]=Theta[iopt[0]];
			mxFree(Umax); mxFree(iopt); mxFree(Theta);
			*adr=theta;
			u=mxCalloc(4,sizeof(complexe));
			u[0].R=1; u[0].I=0;
			u[1].R=-theta[0].R; u[1].I=theta[0].I;
			u[2].R=theta[0].R; u[2].I=theta[0].I;
			u[3].R=1; u[3].I=0;
			for (k=0; k<4; k++){
				u[k].R=u[k].R/(sqrt(1+pow(theta[0].R,2)+pow(theta[0].I,2)));
				u[k].I=u[k].I/(sqrt(1+pow(theta[0].R,2)+pow(theta[0].I,2)));
			}
			return u;
		}
		else{
			h=mxCalloc(9,sizeof(double));
			h[8]=(-1+cm2)*d2;
			h[7]=(-f+2*a)*sin(mu)*d+6*tempo;
			h[6]=(6-14*cm2)*d2+(-8*a+4*f)*cos(mu)*d+4*b2;
			h[5]=(-10*a+5*f)*sin(mu)*d-14*tempo;
			h[4]=0;
			h[3]=h[5];
			h[2]=-h[6];
			h[1]=h[7];
			h[0]=-h[8];

			rtr=mxCalloc(8,sizeof(double));
			rti=mxCalloc(8,sizeof(double));
			zrhqr(h, 8, rtr, rti);
			mxFree(h);
			T=mxCalloc(8,sizeof(complexe));
			for (i=0; i<8; i++){
				T[i].R=rtr[i+1];
				T[i].I=rti[i+1];
			}
			mxFree(rtr); mxFree(rti);
			K=mxCalloc(1,sizeof(int));
			T2=find1(T,8,K);
			mxFree(T);
			k=K[0];
			mxFree(K);
			T1=reel2(T2,k);
			mxFree(T2);

			/* Phase du sinus des rotations admissibles. */
			Phi=mxCalloc(k,sizeof(double));
			for (i=0; i<k; i++){
				Phi[i]=2*atan(T1[i])-beta;
			}
			mxFree(T1);
			Phi2=copie(Phi,k);
			lp=k;											// Phi de longueur lp=k.


			/* Recherche du module R de t-1/conj(t)
			   Cas ou Phi+beta=0 est solution.      */
			L=mxCalloc(1,sizeof(int));
			J=find2(Phi2,beta,lp,L);
			l=L[0];											// J de longueur l.
			mxFree(L);
			if (l>0){
				R00=mxCalloc((2*l),sizeof(complexe));
				Phi00=mxCalloc((2*l),sizeof(double));
				for (i=0; i<l; i++){
					j=J[i];
					phij=Phi2[j];
					cpb=cos(phij+beta);						// peut valoir 1 ou -1.
					coef=mxCalloc(3,sizeof(double));
					coef[2]=b*cpb; coef[1]=2*d*cos(delta+2*phij)+f-2*a; coef[0]=-4*b*cpb;
					rtr=mxCalloc(2,sizeof(double));
					rti=mxCalloc(2,sizeof(double));
					zrhqr(coef, 2, rtr, rti);
					mxFree(coef);
					Rj=mxCalloc(2,sizeof(complexe));
					for (j=0; j<2; j++){
						Rj[j].R=rtr[j+1];
						Rj[j].I=rti[j+1];
					}
					mxFree(rtr); mxFree(rti);
					R00[i*2]=Rj[0]; R00[i*2+1]=Rj[1];		// R00 de longueur 2l.
					mxFree(Rj);
					Phi00[i*2]=phij; Phi00[i*2+1]=phij;		// Phi00 de longueur 2l.
				}
				O=mxCalloc(1,sizeof(int));
				JJ=find3(R00,2*l,O);
				mxFree(JJ);
				o=O[0];
				mxFree(O);
				p=2*l-o;
				Phi0=raz1(Phi00,R00,2*l,o);	mxFree(Phi00);	// R0, Phi0 et JJ de longueur p=2l-o;
				R0=raz2(R00,2*l,o); mxFree(R00);
				Phi=raz3(Phi2,beta,lp,l);
				mxFree(Phi2);
				lp=lp-l;									// nouvelle longueur de Phi.
			}

			if (p>0){
				R02=mxCalloc(p,sizeof(double));
				Ups0=mxCalloc(p,sizeof(double));
				for (i=0; i<p; i++){
					R02[i]=pow(R0[i],2);
					Ups0[i]=(a*R02[i]+4*b*R0[i]+(4*d*cos(mu)+2*f))/(4+R02[i]);
															// Ups0 de longueur p.
				}
				mxFree(R02);
			}

			R=mxCalloc(lp,sizeof(double));
			R2=mxCalloc(lp,sizeof(double));
			for (i=0; i<lp; i++){
				R[i]=-2*d/b*sin(delta+2*Phi[i])/sin(beta+Phi[i]);
				R2[i]=pow(R[i],2);}							// R et R2 de longueur lp.
			Ups1=mxCalloc(lp,sizeof(double));
			for (i=0; i<lp; i++){
				Ups1[i]=(a*R2[i]+4*b*cos(beta+Phi[i])*R[i]+4*d*cos(delta+2*Phi[i])+2*f)/(4+R2[i]);}
			mxFree(R2);
															// Ups1 de longueur lp.


			/* Selection du meilleur angle (donnant contraste maxi). */
			Ups=mxCalloc((p+lp),sizeof(double));				// Ups de longueur p+lp.
			for (i=0; i<p; i++){
				Ups[i]=Ups0[i];}
			mxFree(Ups0);
			for (i=0; i<lp; i++){
				Ups[i+p]=Ups1[i];}
			mxFree(Ups1);
			Phi1=copie(Phi,lp);
			Phi=mxCalloc((p+lp),sizeof(double));				// Phi de longueur p+lp.
			for (i=0; i<p; i++){
				Phi[i]=Phi0[i];}
			mxFree(Phi0);
			for (i=0; i<lp; i++){
				Phi[i+2*l]=Phi1[i];}
			mxFree(Phi1);
			ro=mxCalloc((p+lp),sizeof(double));				// ro de longueur p+lp.
			for (i=0; i<p; i++){
				ro[i]=R0[i];}
			mxFree(R0);
			for (i=0; i<lp; i++){
				ro[i+p]=R[i];}
			mxFree(R);

			Umax=mxCalloc(1,sizeof(double));
			iopt=mxCalloc(1,sizeof(int));
			max2(Umax,iopt,Ups,p+lp,w);
			mxFree(Ups);

			/* Reconstitution de la rotation optimale. */
			ro0=ro[iopt[0]];
			mxFree(ro); mxFree(Umax); mxFree(iopt);
			rr=(ro0+sqrt(4+pow(ro0,2)))/2;
			theta=malloc(1*sizeof(complexe));
			theta[0].R=rr*cos(Phi[iopt[0]]); theta[0].I=rr*sin(Phi[iopt[0]]);
			mxFree(Phi);
			*adr=theta;
			if (Umax[0]<(w*a)){
				theta[0].R=0; theta[0].I=0;
				*adr=theta;
				rr=0;
				Umax[0]=w*a;
			}

			/******************Ligne rajoutee ******************/
			/* Choix de la solution theta dans le disque unite */
			/***************************************************/
			if (rr>1){
				D=(pow(theta[0].R,2)+pow(theta[0].I,2));
				theta[0].R=-theta[0].R/D; theta[0].I=-theta[0].I/D;
				*adr=theta;
				rr=reel(norm2(theta[0]));
			}
			cc=1/sqrt(1+pow(rr,2));
			u=mxCalloc(4,sizeof(complexe));
			u[0].R=cc;		u[0].I=0;
			u[3].R=u[0].R;	u[3].I=u[0].I;
			u[2]=mul(theta[0],cc);
			u[1]=mul(conj2(u[2]),-1);
			return u;
		}
	}
}




/*********************************************************/
/**************Interface entre Matlab et C*****************/
/*********************************************************/

		
/* Gateway routine */


void mexFunction(int nlhs,       mxArray *plhs[],
                 int nrhs, const mxArray *prhs[]){


	unsigned int mz,nz;
	double *zr, *zi, *ur, *ui, *pw;
	double w, *thetaR, *thetaI;
	complexe *z, *u, *theta, **adr;
	
	
	/* Dimensions des matrices de depart */
	mz=mxGetM(prhs[0]);
	nz=mxGetN(prhs[0]);


	/* Creation des pointeurs de la partie reelle et de la partie imaginaire */
	zr = mxGetPr(prhs[0]);
	zi = mxGetPi(prhs[0]);
	pw = mxGetPr(prhs[1]);
	w  = pw[0];
	
	
	/* Nombre correct d'arguments */
	if (nrhs != 2) {
		mexErrMsgTxt("Deux arguments obligatoires.");
	} else if (nlhs !=2) {
		mexErrMsgTxt("Deux arguments de sortie");
	}

	
	/* Allocation de la memoire pour la sortie */
	ur = mxCalloc(4,sizeof(double));
	ui = mxCalloc(4,sizeof(double));
	theta=mxCalloc(1,sizeof(complexe));
	thetaR=mxCalloc(1,sizeof(double));
	thetaI=mxCalloc(1,sizeof(double));
	
	


	/* Appel des routines reunion, tetan et separation */
	z=reunion(zr,zi,mz,nz);
	adr=&theta;
	u=tetan(adr,z,w,mz,nz);
	theta=*adr;
	separation(ur,ui,u,2,2);
	thetaR[0]=theta[0].R;
	thetaI[0]=theta[0].I;

	
	/* Assignation des pointeurs vers les variables de sortie */
	plhs[0] = mxCreateDoubleMatrix(2,2,mxCOMPLEX);
	mxSetPr(plhs[0],ur);
	mxSetPi(plhs[0],ui);
	plhs[1] = mxCreateDoubleMatrix(1,1,mxCOMPLEX);
	mxSetPr(plhs[1],thetaR);
	mxSetPi(plhs[1],thetaI);
	
}