/*
 * WMM - Wave-matching method for mode analysis of dielectric waveguides
 * https://wmm.computational-photonics.eu/
 */

/*
 * wmmcmt.cpp
 * WMM Coupled Mode Theory 
 */

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include"inout.h"
#include"complex.h"
#include"matrix.h"
#include"waveg.h"
#include"maxweq.h"
#include"wmmmat.h"
#include"wmmtrif.h"
#include"mode.h"
#include"wmmfld.h"
#include"wmmarr.h"
#include"wmm.h"
#include"wmmglob.h"
#include"wmmpol.h"
#include"wmmdisc.h"
#include"wmmcmt.h"

/* error message */
void cmterror(const char *msg)
{
	fprintf(stderr, "\nWMM-CMT Error: %s !\n", msg);
	exit(1);
}

/* minimum distance between boundaries to be considered separated  */
#define HDIST 1.0e-8

/* subtract the permittivity profile in waveguide wp from that in wu  
   the PERMITTIVITY difference is returned in the Dmatrix n of wd     
   --- don't access wd.eps ! --- */
#define MAXNUMBD 100
Waveguide permdifference(Waveguide wu, Waveguide wp)
{
	Waveguide wd;

	double x, y;
	double p, u;
	int i, ip, iu, id;
	Dvector th(MAXNUMBD);
	Rect r;
	int l, m;

	iu = 0; ip = 0; id = 0;
	while((iu < wu.nx+1 || ip < wp.nx+1) && id < MAXNUMBD-1) 
	{
		u = wu.rectbounds(iu,1).x1;
		p = wp.rectbounds(ip,1).x1;
		if(fabs(u-p) < HDIST) { x = u; ++ip; ++iu; }
		else
		{
			if(u < p) { x = u; ++iu; }
			else      { x = p; ++ip; }
		}
		th(id) = x; 
		++id;
	}
	if(id>=MAXNUMBD-1) cmterror("MAXNUMBD exceeded");
	--id; 
	if(id<=-1) cmterror("empty structures (x)");
	wd.nx = id; 
	wd.hx = Dvector(id+1);
	for(i=0; i<=id; ++i) wd.hx(i) = th(i);

	iu = 0; ip = 0; id = 0;
	while((iu < wu.ny+1 || ip < wp.ny+1) && id < MAXNUMBD-1) 
	{
		u = wu.rectbounds(1,iu).y1;
		p = wp.rectbounds(1,ip).y1;
		if(fabs(u-p) < HDIST) { y = u; ++ip; ++iu; }
		else
		{
			if(u < p) { y = u; ++iu; }
			else      { y = p; ++ip; }
		}
		th(id) = y; 
		++id;
	}
	if(id>=MAXNUMBD-1) cmterror("MAXNUMBD exceeded");
	--id; 
	if(id<=-1) cmterror("empty structures (y)");
	wd.ny = id; 
	wd.hy = Dvector(id+1);
	for(i=0; i<=id; ++i) wd.hy(i) = th(i);

	wd.n = Dmatrix(wd.nx+2, wd.ny+2);
	for(l=0; l<=wd.nx+1; ++l)
	{
		for(m=0; m<=wd.ny+1; ++m)
		{
			r = wd.rectbounds(l, m); 
			x = 0.5*(r.x0+r.x1);
			y = 0.5*(r.y0+r.y1);
			u = wu.eps(x, y);
			p = wp.eps(x, y);
			wd.n(l, m) = u-p; 	
		}
	}

	wd.lambda = wu.lambda;
	return wd;
}

/* convert a Dmatrix to wmmmat-format */ 
double *dmattowmmmat(const Dmatrix a)
{
	int i, j;
	double *wmat;
	wmat = new double[a.r*a.c]; 
	for(j=0; j<=a.c-1; ++j)
	{
		for(i=0; i<=a.r-1; ++i) wmat[i+j*a.r] = a(i, j);	
	}
	return wmat;
}

/* convert a wmm-matrix to Dmatrix-format */
Dmatrix wmmmattodmat(double *wmat, int n, int m)
{
	int i, j;
	Dmatrix a(n,m);
	for(j=0; j<=m-1; ++j)
	{
		for(i=0; i<=n-1; ++i) a(i,j) = wmat[i+j*a.r];	
	}
	return a;
}

/* Coupled mode theory: setup coupling matrices */
void CMTmats(WMM_ModeArray& umode, // modes to be coupled
	     Waveguide cpwg,       // the entire coupler structure
	     Dmatrix& cmtS,        // output: power coupling matrix
	     Dmatrix& cmtBK)       // output: coupling coefficients 
{
	int n;
	n = umode.num;
	Waveguide wd;
	Dmatrix s(n, n);
	Dmatrix k(n, n);
	Dmatrix tmp(n, n);
	Dvector p(n);
	Dvector b(n);
	Dmatrix a(n, n);
	int i, j, l, m;
	
	double wl;
	Polarization pl;
	Vecform vf;
	Trifunfield tff;
	WMM_Mode modei;
	WMM_Mode modej;

	Rect r;

	if(n<=1) cmterror("Grrr: n");
	modei = umode(0);
	pl = modei.pol;
	wl = modei.wg.lambda;
	vf = modei.vform;
	if(cpwg.bdmatch(modei.wg) != 0) cmterror("wg.hx, wg.hy");
	p(0) = modei.totpower();
	b(0) = modei.beta;
	for(i=1; i<=n-1; ++i)
	{
		modei = umode(i);
		if(modei.pol != pl) cmterror("pol");
		if(modei.wg.lambda != wl) cmterror("lambda");
		if(pl == VEC)
		{
			if(modei.vform != vf) cmterror("vform");
		}
		if(cpwg.bdmatch(modei.wg) != 0) cmterror("wg.hx, wg.hy");
		p(i) = modei.totpower(); 
		b(i) = modei.beta; 
	}

	for(i=0; i<=n-1; ++i)
	{
		modei = umode(i);
		for(j=0; j<=n-1; ++j)
		{
			if(i==j) tmp(i,j) = p(i);
			else     tmp(i,j) = scalprod(modei, umode(j));
		}
	}
	for(i=0; i<=n-1; ++i)
	{
		for(j=0; j<=n-1; ++j)
		{
			s(i,j) = 0.5*(tmp(i,j)+tmp(j,i))/sqrt(p(i)*p(j));
		}
	}
			
	for(i=0; i<=n-1; ++i)
	{
		modei = umode(i);
		wd = permdifference(cpwg, modei.wg);
		for(j=0; j<=n-1; ++j) 
		{
			modej = umode(j);
			tmp(i,j) = 0.0;
			for(l=0; l<=wd.nx+1; ++l)
			{
				for(m=0; m<=wd.ny+1; ++m)
				{
					if(fabs(wd.n(l,m)) > 1.0e-10)
					{
						r = wd.rectbounds(l,m);
						tmp(i,j) += wd.n(l,m)*(
				twomrecint(modei, EX, modej, EX, r)
			      + twomrecint(modei, EY, modej, EY, r)
			      + twomrecint(modei, EZ, modej, EZ, r));
					}
				}
			}
		}
	}
	for(i=0; i<=n-1; ++i)
	{
		for(j=0; j<=n-1; ++j) 
		{
			k(i,j) = 1.0/val_invomep0(wl)
			         /4.0/sqrt(p(i)*p(j))
				 *0.5*(tmp(i,j)+tmp(j,i));
			k(i,j) += s(i,j)
			          *0.5*(b(i) + b(j));
		}
	}

	cmtS  = s;
	cmtBK = k; 
	return;
}

/* Coupled mode theory:
   setup coupling matrices, calculate supermode propagation constants
   and normalized amplitude vectors */
void CMTsetup(WMM_ModeArray& umode, // modes to be coupled
	      Waveguide cpwg,       // the entire coupler structure
	      Dmatrix& sigma,       // output: power coupling matrix
	      Dvector& smpc,        // output: supermode propagation constants
	      Dmatrix& smav)        // output: supermode amplitude vectors 
{
	int n;
	n = umode.num;
	Dmatrix s(n, n);
	Dmatrix k(n, n);
	Dmatrix a(n, n);
	Dvector b(n);
	
	fprintf(stderr, "* WMM-CMT setup (%d) - ", n);

	CMTmats(umode, cpwg, s, k); 

	solvegeneralEVproblem(k, s, b, a);

	sigma = s;	
	smpc = b; 
	smav = a;

	fprintf(stderr, "OK.\n");

	return; 
}


/* Coupled mode theory:
   setup coupling matrices, calculate supermode propagation constants
   and normalized amplitude vectors 
void CMTsetup(WMM_ModeArray& umode, // modes to be coupled
	      Waveguide cpwg,       // the entire coupler structure
	      Dmatrix& sigma,       // output: power coupling matrix
	      Dvector& smpc,        // output: supermode propagation constants
	      Dmatrix& smav)        // output: supermode amplitude vectors 
{
	int n;
	n = umode.num;
	Waveguide wd;
	Dmatrix s(n, n);
	Dmatrix k(n, n);
	Dmatrix tmp(n, n);
	Dvector p(n);
	Dvector b(n);
	Dmatrix a(n, n);
	int i, j, l, m;
	
	double wl;
	Polarization pl;
	Vecform vf;
	Trifunfield tff;
	WMM_Mode modei;
	WMM_Mode modej;

	Rect r;

	fprintf(stderr, "* WMM-CMT setup (%d) - ", n);

	if(n<=1) cmterror("Grrr: n");
	modei = umode(0);
	pl = modei.pol;
	wl = modei.wg.lambda;
	vf = modei.vform;
	if(cpwg.bdmatch(modei.wg) != 0) cmterror("wg.hx, wg.hy");
	p(0) = modei.totpower();
	b(0) = modei.beta;
	for(i=1; i<=n-1; ++i)
	{
		modei = umode(i);
		if(modei.pol != pl) cmterror("pol");
		if(modei.wg.lambda != wl) cmterror("lambda");
		if(pl == VEC)
		{
			if(modei.vform != vf) cmterror("vform");
		}
		if(cpwg.bdmatch(modei.wg) != 0) cmterror("wg.hx, wg.hy");
		p(i) = modei.totpower(); 
		b(i) = modei.beta; 
	}

	for(i=0; i<=n-1; ++i)
	{
		modei = umode(i);
		for(j=0; j<=n-1; ++j)
		{
			if(i==j) tmp(i,j) = p(i);
			else     tmp(i,j) = scalprod(modei, umode(j));
		}
	}
	for(i=0; i<=n-1; ++i)
	{
		for(j=0; j<=n-1; ++j)
		{
			s(i,j) = 0.5*(tmp(i,j)+tmp(j,i))/sqrt(p(i)*p(j));
		}
	}
			
	for(i=0; i<=n-1; ++i)
	{
		modei = umode(i);
		wd = permdifference(cpwg, modei.wg);
		for(j=0; j<=n-1; ++j) 
		{
			modej = umode(j);
			tmp(i,j) = 0.0;
			for(l=0; l<=wd.nx+1; ++l)
			{
				for(m=0; m<=wd.ny+1; ++m)
				{
					if(fabs(wd.n(l,m)) > 1.0e-10)
					{
						r = wd.rectbounds(l,m);
						tmp(i,j) += wd.n(l,m)*(
				twomrecint(modei, EX, modej, EX, r)
			      + twomrecint(modei, EY, modej, EY, r)
			      + twomrecint(modei, EZ, modej, EZ, r));
					}
				}
			}
		}
	}
	for(i=0; i<=n-1; ++i)
	{
		for(j=0; j<=n-1; ++j) 
		{
			k(i,j) = 1.0/val_invomep0(wl)
			         /4.0/sqrt(p(i)*p(j))
				 *0.5*(tmp(i,j)+tmp(j,i));
			k(i,j) += s(i,j)
			          *0.5*(b(i) + b(j));
		}
	}

	solvegeneralEVproblem(k, s, b, a);

	sigma = s;	
	smpc = b; 
	smav = a;

	fprintf(stderr, "OK.\n");

//	int um, sm;
//	double summe, si;
//	for(sm=0; sm<=n-1; ++sm)
//	{
//		summe = 0.0;
//		for(i=0; i<=n-1; ++i)
//		{	
//			si = 0.0;
//			for(j=0; j<=n-1; ++j)
//			{
//				si += sigma(i, j)*smav(j, sm);
//			}
//			summe += si*smav(i,sm);
//		}
//		fprintf(stderr, "supermode %d: power: %g\n", sm, summe);
//		
//	}

	return; 
}
*/

/* compose CMT supermodes */
void CMTsupermodes(WMM_ModeArray& umode,  // modes to be coupled
		   Waveguide cpwg,        // the entire coupler structure
				          // as output by CMTsetup:
	           Dvector smpc,          //   supermode propagation constants
	           Dmatrix smav,          //   supermode amplitude vectors 
		   WMM_ModeArray& smode)  // output: CMT supermodes 
{
	int n;
	n = umode.num;
	
	int i, j, m, l, cp, um, sm;
	double wl, k0, nu, nd;
	Polarization pl;
	Vecform vf;
	Trifunfield tff;
	Trifun* tf;
	int mcp;
	int nt, nntf;

	WMM_Mode tmpm;

	double x, y;
	int il, im;
	Rect r;

	fprintf(stderr, "* WMM-CMT supermode-composition (%d)\n", n);

	if(n<=1) cmterror("Grrr: n");
	pl = umode(0).pol;
	wl = umode(0).wg.lambda;
	vf = umode(0).vform;
	if(cpwg.bdmatch(umode(0).wg) != 0) cmterror("wg.hx, wg.hy");
	for(i=1; i<=n-1; ++i)
	{
		tmpm = umode(i);
		if(tmpm.pol != pl) cmterror("pol");
		if(tmpm.wg.lambda != wl) cmterror("lambda");
		if(pl == VEC)
		{
			if(tmpm.vform != vf) cmterror("vform");
		}
		if(cpwg.bdmatch(tmpm.wg) != 0) cmterror("wg.hx, wg.hy");
	}

	smode.clear();

	for(sm=0; sm<=n-1; ++sm)
	{
		fprintf(stderr, "* -> sm(%d): beta: %g,  ", sm, smpc(sm));

		WMM_Mode mp;
		mp.wg = cpwg;
		mp.pol = pl;
		mp.sym = NOS; 
		mp.beta = smpc(sm);
		k0 = val_k0(wl);
		mp.k0 = k0;
		mp.neff = smpc(sm)/k0;
		fprintf(stderr, "neff: %g,  ", smpc(sm)/k0);
		nu = cpwg.defaultneffmax();
		nd = cpwg.defaultneffmin();
		mp.npcB = (smpc(sm)*smpc(sm)/k0/k0-nd*nd)
				    /(nu*nu-nd*nd);
		fprintf(stderr, "npcB: %g\n", mp.npcB);
		mp.vform = vf;

		tff = Trifunfield(cpwg.nx, cpwg.ny); 
		mcp = 0; if(Pol == VEC) mcp = 1;
		for(cp=0; cp<=mcp; ++cp)
		{
			for(m=0; m<=cpwg.ny+1; ++m)
			{
				for(l=0; l<=cpwg.nx+1; ++l)
				{
					nt = 0;
					r = cpwg.rectbounds(l, m); 
					x = 0.5*(r.x0+r.x1);
					y = 0.5*(r.y0+r.y1);
					for(um=0; um<=n-1; ++um)
					{
					   tmpm = umode(um);
					   tmpm.wg.rectidx(x,y,il,im);
					   nt += tmpm.phi.ntf(cp,il,im);
					}
					tf = new Trifun[nt];
					nt = 0;
					for(um=0; um<=n-1; ++um)
					{
		tmpm = umode(um);
		tmpm.wg.rectidx(x,y,il,im);
		nntf = tmpm.phi.ntf(cp,il,im);
		for(j=0; j<=nntf-1; ++j)
		{
			tf[nt] = *(tmpm.phi(cp,il,im,j));
			tf[nt].Amplit *= tmpm.ampfac*smav(um, sm);
			++nt;
		}
					}
					tff.insert(tf, nt, cp, l, m); 
				}
			}
		}
		mp.phi = tff;
		mp.ampfac = 1.0;
// the supermodes should be normalized by construction ...
//		mp.normalize(1.0);
		mp.setfieldmax();
		smode.add(mp);
	}
	return;
}

/* Coupled mode theory:
   relative output amplitude of mode o, excitement in mode i */ 
Complex CMTamp(int i,              // number of input mode
               int o,              // number of output mode
	       double len,         // device length
	   		           // as output from CMTsetup:
	       Dmatrix& sigma,     //   power coupling matrix
	       Dvector& smpc,      //   supermode propagation constants
	       Dmatrix& smav)      //   supermode amplitude vectors 
{
	double fi, fo, f, ph;
	Complex a;	
	double p;
	int k, j, n, r, s;
	n = sigma.r;
	Dvector pow(n);

	for(k=0; k<=n-1; ++k)
	{
		pow(k) = 0.0;	
		for(r=0; r<=n-1; ++r)
		{
			p = 0.0;
			for(s=0; s<=n-1; ++s) p += sigma(r, s)*smav(s, k);
			pow(k) += smav(r, k)*p;
		}
//		fprintf(stderr, "(%d): p=%g\n", k, pow(k));
	}

	a = CC0;
	for(k=0; k<=n-1; ++k)
	{
		fi = 0.0; for(j=0; j<=n-1; ++j) fi += smav(j,k)*sigma(j,i);
		fo = 0.0; for(j=0; j<=n-1; ++j) fo += smav(j,k)*sigma(j,o);
		f = fi*fo/pow(k);
		ph = -smpc(k)*len;
		a = a + expi(ph)*f;
	}
	return a;
}

/* Coupled mode theory:
   relative output amplitude of mode o, 
   two input modes i0, i1, with complex amplitudes c0, c1 */
double CMTpower2(int i0,            // number of first input mode
	         Complex c0,        //   its amplitude
                 int i1,            // number of second input mode
	         Complex c1,        //   its amplitude
                 int o,             // number of output mode
	         double len,        // device length
	    	 	            // as output from CMTsetup:
	         Dmatrix& sigma,    //   power coupling matrix
	         Dvector& smpc,     //   supermode propagation constants
	         Dmatrix& smav)     //   supermode amplitude vectors 
{
	double fi, fo, f, ph;
	Complex a0, a1;
	Complex ps;
	double p;
	int k, j, n, r, s;
	n = sigma.r;
	Dvector pow(n);

	for(k=0; k<=n-1; ++k)
	{
		pow(k) = 0.0;	
		for(r=0; r<=n-1; ++r)
		{
			p = 0.0;
			for(s=0; s<=n-1; ++s) p += sigma(r, s)*smav(s, k);
			pow(k) += smav(r, k)*p;
		}
//		fprintf(stderr, "(%d): p=%g\n", k, pow(k));
	}

	a0 = CC0;
	for(k=0; k<=n-1; ++k)
	{
		fi = 0.0; for(j=0; j<=n-1; ++j) fi += smav(j,k)*sigma(j,i0);
		fo = 0.0; for(j=0; j<=n-1; ++j) fo += smav(j,k)*sigma(j,o);
		f = fi*fo/pow(k);
		ph = -smpc(k)*len;
		a0 = a0 + expi(ph)*f;
	}
	a1 = CC0;
	for(k=0; k<=n-1; ++k)
	{
		fi = 0.0; for(j=0; j<=n-1; ++j) fi += smav(j,k)*sigma(j,i1);
		fo = 0.0; for(j=0; j<=n-1; ++j) fo += smav(j,k)*sigma(j,o);
		f = fi*fo/pow(k);
		ph = -smpc(k)*len;
		a1 = a1 + expi(ph)*f;
	}
	
	ps = c0*a0+c1*a1;
	return ps.sqabs();
}

/* Coupled mode theory:
   relative output power in mode o, excitement in mode i */ 
double CMTpower(int i,              // number of input mode
                int o,              // number of output mode
		double len,         // device length
				    // as output from CMTsetup:
	        Dmatrix& sigma,     //   power coupling matrix
	        Dvector& smpc,      //   supermode propagation constants
	        Dmatrix& smav)      //   supermode amplitude vectors 
{
	double fi, fo, f, ph;
	Complex a;
	double p;
	int k, j, n, r, s;
	n = sigma.r;
	Dvector pow(n);

	for(k=0; k<=n-1; ++k)
	{
		pow(k) = 0.0;	
		for(r=0; r<=n-1; ++r)
		{
			p = 0.0;
			for(s=0; s<=n-1; ++s) p += sigma(r, s)*smav(s, k);
			pow(k) += smav(r, k)*p;
		}
//		fprintf(stderr, "(%d): p=%g\n", k, pow(k));
	}

	a = CC0;
	for(k=0; k<=n-1; ++k)
	{
		fi = 0.0; for(j=0; j<=n-1; ++j) fi += smav(j,k)*sigma(j,i);
		fo = 0.0; for(j=0; j<=n-1; ++j) fo += smav(j,k)*sigma(j,o);
		f = fi*fo/pow(k);
		ph = -smpc(k)*len;
		a = a + expi(ph)*f;
	}
	return a.sqabs();
}

/* Coupled mode theory: evaluate field superposition, local intensity  
   umode's are assumed to be normalized !!! */
double CMTsz(double x,              // coordinates on the 
	     double y,              //  waveguide cross section
	     double z,              // propagation distance
	     WMM_ModeArray& umode,  // modes to be coupled
             int i,                 // number of input mode, power 1 at z=0
	  		            // as output from CMTsetup:
	     Dmatrix& sigma,        //   power coupling matrix
	     Dvector& smpc,         //   supermode propagation constants
	     Dmatrix& smav)         //   supermode amplitude vectors 
{
	int ncm;
	int m, r, s;
	ncm = umode.num;
	Dvector eux(ncm);
	Dvector euy(ncm);
	Dvector hux(ncm);
	Dvector huy(ncm);
	Dvector esx(ncm);
	Dvector esy(ncm);
	Dvector hsx(ncm);
	Dvector hsy(ncm);
	Dvector a(ncm);
	double sz;

	for(m=0; m<=ncm-1; ++m)
	{
		eux(m) = umode(m).field(EX, x, y); 
		euy(m) = umode(m).field(EY, x, y); 
		hux(m) = umode(m).field(HX, x, y); 
		huy(m) = umode(m).field(HY, x, y); 
	}
	for(r=0; r<=ncm-1; ++r)
	{
		esx(r) = 0.0;
		esy(r) = 0.0;
		hsx(r) = 0.0;
		hsy(r) = 0.0;
		a(r) = 0.0;
		for(m=0; m<=ncm-1; ++m)
		{
			esx(r) += smav(m, r)*eux(m);
			esy(r) += smav(m, r)*euy(m);
			hsx(r) += smav(m, r)*hux(m);
			hsy(r) += smav(m, r)*huy(m);
			a(r) += sigma(i,m)*smav(m, r);
		}	
	}
	sz = 0.0;
	for(r=0; r<=ncm-1; ++r)
	{
		for(s=0; s<=ncm-1; ++s)
		{
			sz += a(r)*a(s)
			      *(esx(r)*hsy(s)-esy(r)*hsx(s))
			      *cos((smpc(r)-smpc(s))*z);
		}
	}	
	return sz*0.5;
}

/* Coupled mode theory: evaluate field superposition, local intensity  
   on a mesh in the y-z-plane
   (npy+1)*(npz+1) points between y0<y<y1, 0<z<ldev, 
   s(yi, zi) = CMTsz(x, y0+(y1-y0)/npy*yi, z0+ldev/npz*zi),
   umode's are assumed to be normalized !!! */
void CMTprop(double x,              // x-level 
	     double y0,             // evaluate between 
	     double y1,             //   y0<y<y1, 
	     double ldev,           //   0<z<ldev 
	     int npy,               // number of
	     int npz,               //   mesh points
	     WMM_ModeArray& umode,  // modes to be coupled
             int i,                 // number of input mode, power 1 at z=0
	                            // as output from CMTsetup:
	     Dmatrix& sigma,        //   power coupling matrix
	     Dvector& smpc,         //   supermode propagation constants
	     Dmatrix& smav,         //   supermode amplitude vectors 
	     Dmatrix& p)            // output: power values
{
	int ncm;
	int m, r, s;
	ncm = umode.num;
	Dmatrix eux(ncm, npy+1);
	Dmatrix euy(ncm, npy+1);
	Dmatrix hux(ncm, npy+1);
	Dmatrix huy(ncm, npy+1);
	Dmatrix esx(ncm, npy+1);
	Dmatrix esy(ncm, npy+1);
	Dmatrix hsx(ncm, npy+1);
	Dmatrix hsy(ncm, npy+1);
	Dvector a(ncm);
	WMM_Mode mode;

	double y, z;
	int yi, zi;

	fprintf(stderr, ".");
	
	for(m=0; m<=ncm-1; ++m)
	{
		fprintf(stderr, ".");
		mode = umode(m);
		for(yi=0; yi<=npy; ++yi)
		{
			y = y0+(y1-y0)/npy*yi;
			eux(m,yi) = mode.field(EX, x, y); 
			euy(m,yi) = mode.field(EY, x, y); 
			hux(m,yi) = mode.field(HX, x, y); 
			huy(m,yi) = mode.field(HY, x, y); 
		}
	}
	fprintf(stderr, ".");
	for(yi=0; yi<=npy; ++yi)
	{
		y = y0+(y1-y0)/npy*yi;
		for(r=0; r<=ncm-1; ++r)
		{
			esx(r,yi) = 0.0;
			esy(r,yi) = 0.0;
			hsx(r,yi) = 0.0;
			hsy(r,yi) = 0.0;
			for(m=0; m<=ncm-1; ++m)
			{
				esx(r,yi) += smav(m, r)*eux(m,yi);
				esy(r,yi) += smav(m, r)*euy(m,yi);
				hsx(r,yi) += smav(m, r)*hux(m,yi);
				hsy(r,yi) += smav(m, r)*huy(m,yi);
			}	
		}
	}
	for(r=0; r<=ncm-1; ++r)
	{
		a(r) = 0.0;
		for(m=0; m<=ncm-1; ++m)
		{
			a(r) += sigma(i,m)*smav(m, r);
		}
	}
	fprintf(stderr, ".");
	p = Dmatrix(npy+1, npz+1);
	p.init(0.0);
	for(yi=0; yi<=npy; ++yi)
	{
		for(zi=0; zi<=npz; ++zi)
		{
			z = ldev/npz*zi;
			for(r=0; r<=ncm-1; ++r)
			{
				for(s=0; s<=ncm-1; ++s)
				{
					p(yi, zi) += a(r)*a(s)
					      *(esx(r, yi)*hsy(s, yi)
					       -esy(r, yi)*hsx(s, yi))
					      *cos((smpc(r)-smpc(s))*z);
				}
			}	
			p(yi, zi) *= 0.5;
		}
	}	
	fprintf(stderr, ".");
	return;
}


/* Coupled mode theory: evaluate field superposition, local intensity  
   on a mesh in the y-z-plane
   (npy+1)*(npz+1) points between y0<y<y1, 0<z<ldev, 
   s(yi, zi) = CMTsz(x, y0+(y1-y0)/npy*yi, z0+ldev/npz*zi),
   two input modes i0, i1, with complex amplitudes c0, c1, 
   umode's are assumed to be normalized !!! 
*/
void CMTprop2(double x,              // x-level 
	      double y0,             // evaluate between 
	      double y1,             //   y0<y<y1, 
	      double ldev,           //   0<z<ldev 
	      int npy,               // number of
	      int npz,               //   mesh points
	      WMM_ModeArray& umode,  // modes to be coupled
              int i0,                // number of first input mode
	      Complex c0,            //   its amplitude
              int i1,                // number of second input mode
	      Complex c1,            //   its amplitude
	                             // as output from CMTsetup:
	      Dmatrix& sigma,        //   power coupling matrix
	      Dvector& smpc,         //   supermode propagation constants
	      Dmatrix& smav,         //   supermode amplitude vectors 
	      Dmatrix& p)            // output: power values
{
	int ncm;
	int m, r, s;
	ncm = umode.num;
	Dmatrix eux(ncm, npy+1);
	Dmatrix euy(ncm, npy+1);
	Dmatrix hux(ncm, npy+1);
	Dmatrix huy(ncm, npy+1);
	Dmatrix esx(ncm, npy+1);
	Dmatrix esy(ncm, npy+1);
	Dmatrix hsx(ncm, npy+1);
	Dmatrix hsy(ncm, npy+1);
	Dvector ar(ncm);
	Dvector ai(ncm);
	WMM_Mode mode;

	double y, z;
	int yi, zi;

	fprintf(stderr, ".");
	
	for(m=0; m<=ncm-1; ++m)
	{
		fprintf(stderr, ".");
		mode = umode(m);
		for(yi=0; yi<=npy; ++yi)
		{
			y = y0+(y1-y0)/npy*yi;
			eux(m,yi) = mode.field(EX, x, y); 
			euy(m,yi) = mode.field(EY, x, y); 
			hux(m,yi) = mode.field(HX, x, y); 
			huy(m,yi) = mode.field(HY, x, y); 
		}
	}
	fprintf(stderr, ".");
	for(yi=0; yi<=npy; ++yi)
	{
		y = y0+(y1-y0)/npy*yi;
		for(r=0; r<=ncm-1; ++r)
		{
			esx(r,yi) = 0.0;
			esy(r,yi) = 0.0;
			hsx(r,yi) = 0.0;
			hsy(r,yi) = 0.0;
			for(m=0; m<=ncm-1; ++m)
			{
				esx(r,yi) += smav(m, r)*eux(m,yi);
				esy(r,yi) += smav(m, r)*euy(m,yi);
				hsx(r,yi) += smav(m, r)*hux(m,yi);
				hsy(r,yi) += smav(m, r)*huy(m,yi);
			}	
		}
	}
	for(r=0; r<=ncm-1; ++r)
	{
		ar(r) = 0.0;
		ai(r) = 0.0;
		for(m=0; m<=ncm-1; ++m)
		{
			ar(r) += c0.re*sigma(i0,m)*smav(m, r);
			ai(r) += c0.im*sigma(i0,m)*smav(m, r);
			ar(r) += c1.re*sigma(i1,m)*smav(m, r);
			ai(r) += c1.im*sigma(i1,m)*smav(m, r);
		}
	}
	fprintf(stderr, ".");
	p = Dmatrix(npy+1, npz+1);
	p.init(0.0);
	for(yi=0; yi<=npy; ++yi)
	{
		for(zi=0; zi<=npz; ++zi)
		{
			z = ldev/npz*zi;
			for(r=0; r<=ncm-1; ++r)
			{
				for(s=0; s<=ncm-1; ++s)
				{
					p(yi, zi) += 
					       (esx(r, yi)*hsy(s, yi)
					       -esy(r, yi)*hsx(s, yi))
					     *( (ar(r)*ar(s)+ai(r)*ai(s))
					        *cos((smpc(s)-smpc(r))*z)
					       +(ar(r)*ai(s)-ai(r)*ar(s))
					        *sin((smpc(s)-smpc(r))*z) );
				}
			}	
			p(yi, zi) *= 0.5;
		}
	}	
	fprintf(stderr, ".");
	return;
}