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

/*
 * wmmpol.cpp
 * procedures to set up lsq-error and normalization matrices
 */

#include<math.h>
#include<stdio.h>
#include<stdlib.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"wmmdisc.h"
#include"wmmglob.h"
#include"wmmpol.h"

/* error encountered */
void wmmpolerror(const char* msg)
{
	fprintf(stderr, "WMM: wmmpol: %s\n", msg);
	exit(1);
}

/* polarization depending factors */
/* for QTE/QTM calculations */
Dmatrix Pffh;
Dmatrix Pffv;
Dmatrix Pfdx;
Dmatrix Pfdy;
/* for vectorial calculations */
Dmatrix VPfO;
Dmatrix VPfP;
Dmatrix VPfQ;
Dmatrix VPfR;
Dmatrix VPfS;
Dmatrix VPfT;
Dmatrix VPfU;
Dmatrix VPfV;
Dmatrix VPfW;
Dmatrix VPfX;
Dmatrix VPfY;
Dmatrix VPfZ;

/* setup a list of field mismatch contributions */
int MaxNumdbac; 
typedef struct
{
	Derlev fod0;       /* field / derivative */
	int cp0;           /* x / y - component */
	int ri0;           /* rectangle - index, 1. factor */
	Derlev fod1;       /* field / derivative */
	int cp1;           /* x / y - component */
	int ri1;           /* rectangle - index, 2. factor */
	double wf;         /* weight */
} efcont;
Ivector Numdbac;
efcont** Dbac;

/* add a single new entry */
void adddbac(int vk, Derlev fod0, char cp0, int ri0,
		     Derlev fod1, char cp1, int ri1, double wf)
{
	efcont *d;
	if(Numdbac(vk) >= MaxNumdbac)
		wmmpolerror("wmmpol: Numdbac >= MaxNumdbac !");
	d = &Dbac[vk][Numdbac(vk)];
	(*d).fod0 = fod0;
	if(Pol == VEC)
	{
		switch(Par.vform)
		{
		case HXHY:
			switch(cp0)
			{
			case 'x': (*d).cp0 = 0; break;
			case 'y': (*d).cp0 = 1; break;
			}
			break;
		case EXEY:
			switch(cp0)
			{
			case 'x': (*d).cp0 = 1; break;
			case 'y': (*d).cp0 = 0; break;
			}
			break;
		case EZHZ:
			switch(cp0)
			{
			case 'a': (*d).cp0 = 1; break;
			case 'b': (*d).cp0 = 0; break;
			}
			break;
		}
	}
	else  (*d).cp0 = 0; 
	(*d).ri0 = ri0;
	(*d).fod1 = fod1;
	if(Pol == VEC)
	{
		switch(Par.vform)
		{
		case HXHY:
			switch(cp1)
			{
			case 'x': (*d).cp1 = 0; break;
			case 'y': (*d).cp1 = 1; break;
			}
			break;
		case EXEY:
			switch(cp1)
			{
			case 'x': (*d).cp1 = 1; break;
			case 'y': (*d).cp1 = 0; break;
			}
			break;
		case EZHZ:
			switch(cp1)
			{
			case 'a': (*d).cp1 = 1; break;
			case 'b': (*d).cp1 = 0; break;
			}
			break;
		}
	}
	else (*d).cp1 = 0; 
	(*d).ri1 = ri1;
	(*d).wf = wf;
	++Numdbac(vk);
	return;
}

/* add the square of up to three contributions, equal factors */
void sqrefcont(int vk, int nr, double vpf0, double vpf1, 
	       double f1, Derlev fod1, char cp1,
	       double f2, Derlev fod2, char cp2,
	       double f3, Derlev fod3, char cp3)
{
	switch(nr)
	{
	case 1: 
		adddbac(vk, fod1, cp1, 0, fod1, cp1, 0,      f1*f1*vpf0*vpf0);
		adddbac(vk, fod1, cp1, 0, fod1, cp1, 1, -2.0*f1*f1*vpf0*vpf1);
		adddbac(vk, fod1, cp1, 1, fod1, cp1, 1,      f1*f1*vpf1*vpf1);
		return;
	case 2: 
		adddbac(vk, fod1, cp1, 0, fod1, cp1, 0,      f1*f1*vpf0*vpf0);
		adddbac(vk, fod1, cp1, 0, fod2, cp2, 0,  2.0*f1*f2*vpf0*vpf0);
		adddbac(vk, fod2, cp2, 0, fod2, cp2, 0,      f2*f2*vpf0*vpf0);

		adddbac(vk, fod1, cp1, 0, fod1, cp1, 1, -2.0*f1*f1*vpf0*vpf1);
		adddbac(vk, fod1, cp1, 0, fod2, cp2, 1, -2.0*f1*f2*vpf0*vpf1);
		adddbac(vk, fod2, cp2, 0, fod1, cp1, 1, -2.0*f2*f1*vpf0*vpf1);
		adddbac(vk, fod2, cp2, 0, fod2, cp2, 1, -2.0*f2*f2*vpf0*vpf1);

		adddbac(vk, fod1, cp1, 1, fod1, cp1, 1,      f1*f1*vpf1*vpf1);
		adddbac(vk, fod1, cp1, 1, fod2, cp2, 1,  2.0*f1*f2*vpf1*vpf1);
		adddbac(vk, fod2, cp2, 1, fod2, cp2, 1,      f2*f2*vpf1*vpf1);
		return;
	case 3: 
		adddbac(vk, fod1, cp1, 0, fod1, cp1, 0,      f1*f1*vpf0*vpf0);
		adddbac(vk, fod2, cp2, 0, fod2, cp2, 0,      f2*f2*vpf0*vpf0);
		adddbac(vk, fod3, cp3, 0, fod3, cp3, 0,      f3*f3*vpf0*vpf0);
		adddbac(vk, fod1, cp1, 0, fod2, cp2, 0,  2.0*f1*f2*vpf0*vpf0);
		adddbac(vk, fod1, cp1, 0, fod3, cp3, 0,  2.0*f1*f3*vpf0*vpf0);
		adddbac(vk, fod2, cp2, 0, fod3, cp3, 0,  2.0*f2*f3*vpf0*vpf0);

		adddbac(vk, fod1, cp1, 0, fod1, cp1, 1, -2.0*f1*f1*vpf0*vpf1);
		adddbac(vk, fod1, cp1, 0, fod2, cp2, 1, -2.0*f1*f2*vpf0*vpf1);
		adddbac(vk, fod1, cp1, 0, fod3, cp3, 1, -2.0*f1*f3*vpf0*vpf1);
		adddbac(vk, fod2, cp2, 0, fod1, cp1, 1, -2.0*f2*f1*vpf0*vpf1);
		adddbac(vk, fod2, cp2, 0, fod2, cp2, 1, -2.0*f2*f2*vpf0*vpf1);
		adddbac(vk, fod2, cp2, 0, fod3, cp3, 1, -2.0*f2*f3*vpf0*vpf1);
		adddbac(vk, fod3, cp3, 0, fod1, cp1, 1, -2.0*f3*f1*vpf0*vpf1);
		adddbac(vk, fod3, cp3, 0, fod2, cp2, 1, -2.0*f3*f2*vpf0*vpf1);
		adddbac(vk, fod3, cp3, 0, fod3, cp3, 1, -2.0*f3*f3*vpf0*vpf1);

		adddbac(vk, fod1, cp1, 1, fod1, cp1, 1,      f1*f1*vpf1*vpf1);
		adddbac(vk, fod2, cp2, 1, fod2, cp2, 1,      f2*f2*vpf1*vpf1);
		adddbac(vk, fod3, cp3, 1, fod3, cp3, 1,      f3*f3*vpf1*vpf1);
		adddbac(vk, fod1, cp1, 1, fod2, cp2, 1,  2.0*f1*f2*vpf1*vpf1);
		adddbac(vk, fod1, cp1, 1, fod3, cp3, 1,  2.0*f1*f3*vpf1*vpf1);
		adddbac(vk, fod2, cp2, 1, fod3, cp3, 1,  2.0*f2*f3*vpf1*vpf1);
		return;
	}
	return;
}

/* add the square of up to two contributions, separate factors */
void sqrefcontzf(int vk, int nr,  
	       double pf01, double pf11, Derlev fod1, char cp1,
	       double pf02, double pf12, Derlev fod2, char cp2)
{
	switch(nr)
	{
	case 1: 
		adddbac(vk, fod1, cp1, 0, fod1, cp1, 0,      pf01*pf01);
		adddbac(vk, fod1, cp1, 0, fod1, cp1, 1, -2.0*pf01*pf11);
		adddbac(vk, fod1, cp1, 1, fod1, cp1, 1,      pf11*pf11);
		return;
	case 2: 
		adddbac(vk, fod1, cp1, 0, fod1, cp1, 0,      pf01*pf01);
		adddbac(vk, fod1, cp1, 0, fod2, cp2, 0,  2.0*pf01*pf02);
		adddbac(vk, fod2, cp2, 0, fod2, cp2, 0,      pf02*pf02);

		adddbac(vk, fod1, cp1, 0, fod1, cp1, 1, -2.0*pf01*pf11);
		adddbac(vk, fod1, cp1, 0, fod2, cp2, 1, -2.0*pf01*pf12);
		adddbac(vk, fod2, cp2, 0, fod1, cp1, 1, -2.0*pf02*pf11);
		adddbac(vk, fod2, cp2, 0, fod2, cp2, 1, -2.0*pf02*pf12);

		adddbac(vk, fod1, cp1, 1, fod1, cp1, 1,      pf11*pf11);
		adddbac(vk, fod1, cp1, 1, fod2, cp2, 1,  2.0*pf11*pf12);
		adddbac(vk, fod2, cp2, 1, fod2, cp2, 1,      pf12*pf12);
		return;
	}
	return;
}

/* HXHY - formulation for vectorial calculations */
void setvecerrfuncconthxhy()
{
	int vk;
	for(vk=0; vk<=NumVk-1; ++vk)
	{
		Numdbac(vk) = 0;
		switch(Vktyp(vk))
		{
		case HOR:
			if(Par.ccomp != CCHXYZEZ)
			{
				sqrefcont(vk, 3, VPfO(0,vk), VPfO(1,vk), 
			                Beta, FLD, 'y',
			           -1.0/Beta, DXY, 'x',
			           -1.0/Beta, DYY, 'y');
				sqrefcont(vk, 3, VPfP(0,vk), VPfP(1,vk), 
			               -Beta, FLD, 'x',
			            1.0/Beta, DXX, 'x',
			            1.0/Beta, DXY, 'y');
			}
			sqrefcont(vk, 2, VPfQ(0,vk), VPfQ(1,vk), 
			                 1.0, DYF, 'x',
			                -1.0, DXF, 'y',
			                 0.0, FLD, 'x');
			if(Par.ccomp != CCEXYZHZ)
			{
				sqrefcont(vk, 1, VPfR(0,vk), VPfR(1,vk), 
			                 1.0, FLD, 'x',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
				sqrefcont(vk, 1, VPfS(0,vk), VPfS(1,vk), 
			                 1.0, FLD, 'y',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
			}
			sqrefcont(vk, 2, VPfT(0,vk), VPfT(1,vk), 
			           -1.0/Beta, DXF, 'x',
			           -1.0/Beta, DYF, 'y',
			                 0.0, FLD, 'x');
			break;	
		case VER:
			if(Par.ccomp != CCHXYZEZ)
			{
				sqrefcont(vk, 3, VPfU(0,vk), VPfU(1,vk), 
			                Beta, FLD, 'y',
			           -1.0/Beta, DXY, 'x',
			           -1.0/Beta, DYY, 'y');
				sqrefcont(vk, 3, VPfV(0,vk), VPfV(1,vk), 
			               -Beta, FLD, 'x',
			            1.0/Beta, DXX, 'x',
			            1.0/Beta, DXY, 'y');
			}
			sqrefcont(vk, 2, VPfW(0,vk), VPfW(1,vk), 
			                 1.0, DYF, 'x',
			                -1.0, DXF, 'y',
			                 0.0, FLD, 'x');
			if(Par.ccomp != CCEXYZHZ)
			{
				sqrefcont(vk, 1, VPfX(0,vk), VPfX(1,vk), 
			                 1.0, FLD, 'x',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
				sqrefcont(vk, 1, VPfY(0,vk), VPfY(1,vk), 
			                 1.0, FLD, 'y',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
			}
			sqrefcont(vk, 2, VPfZ(0,vk), VPfZ(1,vk), 
			           -1.0/Beta, DXF, 'x',
			           -1.0/Beta, DYF, 'y',
			                 0.0, FLD, 'x');
			break;	
		}
	}
	return;
}

/* EXEY - formulation for vectorial calculations */
void setvecerrfunccontexey()
{
	int vk;
	for(vk=0; vk<=NumVk-1; ++vk)
	{
		Numdbac(vk) = 0;
		switch(Vktyp(vk))
		{
		case HOR:
			if(Par.ccomp != CCHXYZEZ)
			{
				sqrefcont(vk, 1, VPfO(0,vk), VPfO(1,vk), 
			                 1.0, FLD, 'x',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
				sqrefcont(vk, 1, VPfP(0,vk), VPfP(1,vk), 
			                 1.0, FLD, 'y',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
			}
			sqrefcont(vk, 2, VPfQ(0,vk), VPfQ(1,vk), 
			           -1.0/Beta, DXF, 'x',
			           -1.0/Beta, DYF, 'y',
			                 0.0, FLD, 'x');
			if(Par.ccomp != CCEXYZHZ)
			{
				sqrefcont(vk, 3, VPfR(0,vk), VPfR(1,vk), 
			               -Beta, FLD, 'y',
			            1.0/Beta, DXY, 'x',
			            1.0/Beta, DYY, 'y');
				sqrefcont(vk, 3, VPfS(0,vk), VPfS(1,vk), 
			                Beta, FLD, 'x',
			           -1.0/Beta, DXX, 'x',
			           -1.0/Beta, DXY, 'y');
			}
			sqrefcont(vk, 2, VPfT(0,vk), VPfT(1,vk), 
			                 1.0, DXF, 'y',
			                -1.0, DYF, 'x',
			                 0.0, FLD, 'x');
			break;	
		case VER:
			if(Par.ccomp != CCHXYZEZ)
			{
				sqrefcont(vk, 1, VPfU(0,vk), VPfU(1,vk), 
			                 1.0, FLD, 'x',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
				sqrefcont(vk, 1, VPfV(0,vk), VPfV(1,vk), 
			                 1.0, FLD, 'y',
			                 0.0, FLD, 'x',
			                 0.0, FLD, 'x');
			}
			sqrefcont(vk, 2, VPfW(0,vk), VPfW(1,vk), 
			           -1.0/Beta, DXF, 'x',
			           -1.0/Beta, DYF, 'y',
			                 0.0, FLD, 'x');
			if(Par.ccomp != CCEXYZHZ)
			{
				sqrefcont(vk, 3, VPfX(0,vk), VPfX(1,vk), 
			               -Beta, FLD, 'y',
			            1.0/Beta, DXY, 'x',
			            1.0/Beta, DYY, 'y');
				sqrefcont(vk, 3, VPfY(0,vk), VPfY(1,vk), 
			                Beta, FLD, 'x',
			           -1.0/Beta, DXX, 'x',
			           -1.0/Beta, DXY, 'y');
			}
			sqrefcont(vk, 2, VPfZ(0,vk), VPfZ(1,vk), 
			                 1.0, DXF, 'y',
			                -1.0, DYF, 'x',
			                 0.0, FLD, 'x');
			break;	
		}
	}
	return;
}

/* EZHZ - formulation for vectorial calculations */
void setvecerrfunccontezhz()
{
	int vk;
	int l1, m1, l2, m2;
	double eps1, eps2;
	for(vk=0; vk<=NumVk-1; ++vk)
	{
		l1 = Nrxi(0,vk);
		m1 = Nryi(0,vk);
		l2 = Nrxi(1,vk);
		m2 = Nryi(1,vk);
		eps1 = Wg.eps(l1,m1);
		eps2 = Wg.eps(l2,m2);
		Numdbac(vk) = 0;
		switch(Vktyp(vk))
		{
		case HOR:
			if(Par.ccomp != CCHXYZEZ)
			{
			   sqrefcontzf(vk, 2,  
	                       -K0*VPfO(0,vk),   -K0*VPfO(1,vk), DYF, 'b',
	                     -Beta*VPfO(0,vk), -Beta*VPfO(1,vk), DXF, 'a');
			   sqrefcontzf(vk, 2,  
	                        K0*VPfP(0,vk),    K0*VPfP(1,vk), DXF, 'b',
	                     -Beta*VPfP(0,vk), -Beta*VPfP(1,vk), DYF, 'a');
			}
			sqrefcontzf(vk, 1,  
	                           VPfQ(0,vk),       VPfQ(1,vk), FLD, 'a',
	                                   0.0,               0.0, FLD, 'a');
			if(Par.ccomp != CCEXYZHZ)
			{
			   sqrefcontzf(vk, 2,  
	                K0*eps1*VPfR(0,vk),  K0*eps2*VPfR(1,vk), DYF, 'a',
	                  -Beta*VPfR(0,vk),    -Beta*VPfR(1,vk), DXF, 'b');
			   sqrefcontzf(vk, 2,  
	               -K0*eps1*VPfS(0,vk), -K0*eps2*VPfS(1,vk), DXF, 'a',
	                  -Beta*VPfS(0,vk),    -Beta*VPfS(1,vk), DYF, 'b');
			}
			sqrefcontzf(vk, 1,  
	                           VPfT(0,vk),       VPfT(1,vk), FLD, 'b',
	                                   0.0,               0.0, FLD, 'b');
			break;	
		case VER:
			if(Par.ccomp != CCHXYZEZ)
			{
			   sqrefcontzf(vk, 2,  
	                       -K0*VPfU(0,vk),   -K0*VPfU(1,vk), DYF, 'b',
	                     -Beta*VPfU(0,vk), -Beta*VPfU(1,vk), DXF, 'a');
			   sqrefcontzf(vk, 2,  
	                        K0*VPfV(0,vk),    K0*VPfV(1,vk), DXF, 'b',
	                     -Beta*VPfV(0,vk), -Beta*VPfV(1,vk), DYF, 'a');
			}
			sqrefcontzf(vk, 1,  
	                           VPfW(0,vk),       VPfW(1,vk), FLD, 'a',
	                                   0.0,               0.0, FLD, 'a');
			if(Par.ccomp != CCEXYZHZ)
			{
			   sqrefcontzf(vk, 2,  
	                K0*eps1*VPfX(0,vk),  K0*eps2*VPfX(1,vk), DYF, 'a',
	                  -Beta*VPfX(0,vk),    -Beta*VPfX(1,vk), DXF, 'b');
			   sqrefcontzf(vk, 2,  
	               -K0*eps1*VPfY(0,vk), -K0*eps2*VPfY(1,vk), DXF, 'a',
	                  -Beta*VPfY(0,vk),    -Beta*VPfY(1,vk), DYF, 'b');
			}
			sqrefcontzf(vk, 1,  
	                           VPfZ(0,vk),       VPfZ(1,vk), FLD, 'b',
	                                   0.0,               0.0, FLD, 'b');
			break;	
		}
	}
	return;
}

/* formulation for semivectorial calculations */
void setqpolerrfunccont()
{
	int vk;
	for(vk=0; vk<=NumVk-1; ++vk)
	{
		Numdbac(vk) = 0;
		switch(Vktyp(vk))
		{
		case HOR:
			sqrefcont(vk, 1, Pffh(0,vk), Pffh(1,vk), 
			                 1.0, FLD, '-',
			                 0.0, FLD, '-',
			                 0.0, FLD, '-');
			sqrefcont(vk, 1, Pfdx(0,vk), Pfdx(1,vk), 
			                 1.0, DXF, '-',
			                 0.0, FLD, '-',
			                 0.0, FLD, '-');
			break;	
		case VER:
			sqrefcont(vk, 1, Pffv(0,vk), Pffv(1,vk), 
			                 1.0, FLD, '-',
			                 0.0, FLD, '-',
			                 0.0, FLD, '-');
			sqrefcont(vk, 1, Pfdy(0,vk), Pfdy(1,vk), 
			                 1.0, DYF, '-',
			                 0.0, FLD, '-',
			                 0.0, FLD, '-');
			break;	
		}
	}
	return;
}

/* initialize polarization factors / mode problem formulation */
void setmodeproblemformulation()
{
	int vk;
	int l1, m1, l2, m2;
	double eps1, eps2;
	double w1, w2;
	for(vk=0; vk<=NumVk-1; ++vk)
	{
		l1 = Nrxi(0,vk);
		m1 = Nryi(0,vk);
		l2 = Nrxi(1,vk);
		m2 = Nryi(1,vk);
		eps1 = Wg.eps(l1,m1);
		eps2 = Wg.eps(l2,m2);
		switch(Pol)
		{
		case QTE: Pffh(0,vk) = 1.0;
			  Pfdx(0,vk) = 1.0/K0;
			  Pffv(0,vk) = 2.0*eps1/(eps1+eps2);
			  Pfdy(0,vk) = 1.0/K0;
			  Pffh(1,vk) = 1.0;
			  Pfdx(1,vk) = 1.0/K0;
			  Pffv(1,vk) = 2.0*eps2/(eps1+eps2);
			  Pfdy(1,vk) = 1.0/K0;
			  break;
		case QTM: Pffh(0,vk) = 1.0;
			  Pfdx(0,vk) = 1.0/K0/eps1;
			  Pffv(0,vk) = 1.0;
			  Pfdy(0,vk) = 1.0/K0*(1.0/eps1+1.0/eps2)/2.0;
			  Pffh(1,vk) = 1.0;
			  Pfdx(1,vk) = 1.0/K0/eps2;
			  Pffv(1,vk) = 1.0;
			  Pfdy(1,vk) = 1.0/K0*(1.0/eps1+1.0/eps2)/2.0;
			  break;
		case VEC: 
		    switch(Par.vform) 
		    {
		    case HXHY: 
			  w1 = 1.0/sqrt(K0);
			  w2 = sqrt(K0);
			  VPfO(0,vk) = w1;
			  VPfP(0,vk) = w1/eps1;
			  VPfQ(0,vk) = w1/eps1;
			  VPfR(0,vk) = w2;
			  VPfS(0,vk) = w2;
			  VPfT(0,vk) = w2;
			  VPfU(0,vk) = w1/eps1;
			  VPfV(0,vk) = w1;
			  VPfW(0,vk) = w1/eps1;
			  VPfX(0,vk) = w2;
			  VPfY(0,vk) = w2;
			  VPfZ(0,vk) = w2;
			  VPfO(1,vk) = w1;
			  VPfP(1,vk) = w1/eps2;
			  VPfQ(1,vk) = w1/eps2;
			  VPfR(1,vk) = w2;
			  VPfS(1,vk) = w2;
			  VPfT(1,vk) = w2;
			  VPfU(1,vk) = w1/eps2;
			  VPfV(1,vk) = w1;
			  VPfW(1,vk) = w1/eps2;
			  VPfX(1,vk) = w2;
			  VPfY(1,vk) = w2;
			  VPfZ(1,vk) = w2;
			  break;
		    case EXEY: 
			  w1 = sqrt(K0);
			  w2 = 1.0/sqrt(K0);
			  VPfO(0,vk) = w1*eps1;
			  VPfP(0,vk) = w1;
			  VPfQ(0,vk) = w1;
			  VPfR(0,vk) = w2;
			  VPfS(0,vk) = w2;
			  VPfT(0,vk) = w2;
			  VPfU(0,vk) = w1;
			  VPfV(0,vk) = w1*eps1;
			  VPfW(0,vk) = w1;
			  VPfX(0,vk) = w2;
			  VPfY(0,vk) = w2;
			  VPfZ(0,vk) = w2;
			  VPfO(1,vk) = w1*eps2;
			  VPfP(1,vk) = w1;
			  VPfQ(1,vk) = w1;
			  VPfR(1,vk) = w2;
			  VPfS(1,vk) = w2;
			  VPfT(1,vk) = w2;
			  VPfU(1,vk) = w1;
			  VPfV(1,vk) = w1*eps2;
			  VPfW(1,vk) = w1;
			  VPfX(1,vk) = w2;
			  VPfY(1,vk) = w2;
			  VPfZ(1,vk) = w2;
			  break;
		    case EZHZ: 
			  w1 = 1.0/(Beta*Beta-K0*K0*eps1);
			  w2 = 1.0/(Beta*Beta-K0*K0*eps2);
			  VPfO(0,vk) = w1*eps1;
			  VPfP(0,vk) = w1;
			  VPfQ(0,vk) = 1.0;
			  VPfR(0,vk) = w1;
			  VPfS(0,vk) = w1;
			  VPfT(0,vk) = 1.0;
			  VPfU(0,vk) = w1;
			  VPfV(0,vk) = w1*eps1;
			  VPfW(0,vk) = 1.0;
			  VPfX(0,vk) = w1;
			  VPfY(0,vk) = w1;
			  VPfZ(0,vk) = 1.0;
			  VPfO(1,vk) = w2*eps2;
			  VPfP(1,vk) = w2;
			  VPfQ(1,vk) = 1.0;
			  VPfR(1,vk) = w2;
			  VPfS(1,vk) = w2;
			  VPfT(1,vk) = 1.0;
			  VPfU(1,vk) = w2;
			  VPfV(1,vk) = w2*eps2;
			  VPfW(1,vk) = 1.0;
			  VPfX(1,vk) = w2;
			  VPfY(1,vk) = w2;
			  VPfZ(1,vk) = 1.0;
			  break;
		    }
		    break;
		}
	}
	if(Pol == VEC)
	{
	    	switch(Par.vform) 
	    	{
	    	case HXHY: setvecerrfuncconthxhy(); break;
	    	case EXEY: setvecerrfunccontexey(); break;
	    	case EZHZ: setvecerrfunccontezhz(); break;
	    	}
	}
	else setqpolerrfunccont();
	return;
}	

/* normalization matrix for rectangle l, m: QTE/QTM - polarization */
int fillqtetmsnmat(int l, int m, double *nmat, int pen) 
{ 
	int j, k;
	int jr, dim, offs;
	int a, b;
	double v, pf;
	double mel;
	Rect r;
	pf = 1.0;
	if(Pol == QTE) pf = Swf(l,m);
	if(Pol == QTM) pf = Swf(l,m)/Wg.eps(l,m);
	if(pen == 1)
	{
		if(Pol == QTE) pf *= Par.penfacTE(l,m);
		if(Pol == QTM) pf *= Par.penfacTM(l,m);
	}
	jr = Phi.ntf(0,l,m);
	r = Wg.rectbounds(l,m);
	dim = -1;
	offs = (*Phi(0,l,m,0)).Amsi;
	for(j=0; j<=jr-1; ++j)
	{
		a = (*Phi(0,l,m,j)).Amsi-offs;
		if(a>dim) dim = a;
	}
	++dim;
	for(j=0; j<=jr-1; ++j)
	{
		a = (*Phi(0,l,m,j)).Amsi-offs;
		for(k=j; k<=jr-1; ++k)
		{
			b = (*Phi(0,l,m,k)).Amsi-offs;
			mel = tfnrm(*Phi(0,l,m,j), FLD, *Phi(0,l,m,k), FLD, r);
			mel *= pf;
			if(k == j) mel *= 0.5;
			nmat[a+b*dim] += mel;
			nmat[b+a*dim] += mel;
		}
	}
	if(Par.mshift == 0.0) return dim; 
	for(a=0; a<=dim-1; ++a)
	{
		v = nmat[a+a*dim];
		nmat[a+a*dim] += Par.mshift*v;
	}
	return dim;
}

/* component identifier -> Trifunfield index */
int cpidchtoidx(char c)
{
	switch(Par.vform)
	{
	case HXHY:
		if(c == 'x') return 0;
		if(c == 'y') return 1;
		wmmpolerror("H cpidchtoidx: wrong c !");
	case EXEY:
		if(c == 'y') return 0;
		if(c == 'x') return 1;
		wmmpolerror("E cpidchtoidx: wrong c !");
	case EZHZ:
		if(c == 'b') return 0;
		if(c == 'a') return 1;
		wmmpolerror("Z cpidchtoidx: wrong c !");
	}
	return 0;
}

/* normalization matrix for rectangle l, m: vectorial calculation */
int fillvecsnmat(int l, int m, double *nmat, int pen)
{
	int j, k;
	int jrj, jrk, dim, offs;
	int a, b;
	double v, v1, eps;
	double mel;
	int cpj, cpk;
	Derlev fodj, fodk;
	double pf;
	char   bcp[2][8];
	Derlev  bfod[2][8];
	double bpf[8];
	int t, mcp;
	Rect r;
	mcp = 0;
	switch(Par.vnorm)
	{
	case NRMMS:
		switch(Par.vform)
		{
		case HXHY: 
		      mcp = 2;
		      v = Beta*Swf(l,m)/Wg.eps(l,m);  
		      pf = 1.0; if(pen == 1) pf = Par.penfacTE(l,m);
		      bfod[0][0] = FLD; bcp[0][0] = 'x'; 
		      bfod[1][0] = FLD; bcp[1][0] = 'x'; bpf[0] = v*pf; 
		      pf = 1.0; if(pen == 1) pf = Par.penfacTM(l,m);
		      bfod[0][1] = FLD; bcp[0][1] = 'y'; 
		      bfod[1][1] = FLD; bcp[1][1] = 'y'; bpf[1] = v*pf; 
		      break;
		case EXEY: 
		      mcp = 2;
		      v = Beta*Swf(l,m); 
		      pf = 1.0; if(pen == 1) pf = Par.penfacTM(l,m);
		      bfod[0][0] = FLD; bcp[0][0] = 'x'; 
		      bfod[1][0] = FLD; bcp[1][0] = 'x'; bpf[0] = v*pf; 
		      pf = 1.0; if(pen == 1) pf = Par.penfacTE(l,m);
		      bfod[0][1] = FLD; bcp[0][1] = 'y'; 
		      bfod[1][1] = FLD; bcp[1][1] = 'y'; bpf[1] = v*pf; 
		      break;
		case EZHZ: 
		      if(pen == 1 &&
			 (Par.penfacTE(l,m) != 1.0 || Par.penfacTM(l,m) != 1.0))
			 wmmpolerror("EZHZ, NRMMS: PenFac not implemented !");
		      eps = Wg.eps(l,m);
		      v = 1.0/(Beta*Beta-K0*K0*eps);
		      v = Swf(l,m)*Beta*K0*v*v;
		      mcp = 4;
		      bfod[0][0] = DXF; bcp[0][0] = 'a'; 
		      bfod[1][0] = DXF; bcp[1][0] = 'a'; bpf[0] = v*eps; 
		      bfod[0][1] = DYF; bcp[0][1] = 'a'; 
		      bfod[1][1] = DYF; bcp[1][1] = 'a'; bpf[1] = v*eps; 
		      bfod[0][2] = DXF; bcp[0][2] = 'b'; 
		      bfod[1][2] = DXF; bcp[1][2] = 'b'; bpf[2] = v; 
		      bfod[0][3] = DYF; bcp[0][3] = 'b'; 
		      bfod[1][3] = DYF; bcp[1][3] = 'b'; bpf[3] = v; 
		      break;
		}
		break;
	case NRMMH:
		switch(Par.vform)
		{
		case HXHY: 
		      mcp = 5;
		      v = Swf(l,m); 
		      pf = 1.0; if(pen == 1) pf = Par.penfacTE(l,m);
		      bfod[0][0] = FLD; bcp[0][0] = 'x'; 
		      bfod[1][0] = FLD; bcp[1][0] = 'x'; bpf[0] = v*pf; 
		      pf = 1.0; if(pen == 1) pf = Par.penfacTM(l,m);
		      bfod[0][1] = FLD; bcp[0][1] = 'y'; 
		      bfod[1][1] = FLD; bcp[1][1] = 'y'; bpf[1] = v*pf; 
		      v = v/Beta/Beta;
		      bfod[0][2] = DXF; bcp[0][2] = 'x'; 
		      bfod[1][2] = DXF; bcp[1][2] = 'x'; bpf[2] = v; 
		      bfod[0][3] = DXF; bcp[0][3] = 'x'; 
		      bfod[1][3] = DYF; bcp[1][3] = 'y'; bpf[3] = 2.0*v; 
		      bfod[0][4] = DYF; bcp[0][4] = 'y'; 
		      bfod[1][4] = DYF; bcp[1][4] = 'y'; bpf[4] = v; 
		      break;
		case EXEY: 
		      wmmpolerror("EXEY: NRMMH not implemented !");
		      break;
		case EZHZ: 
		      if(pen == 1 &&
			 (Par.penfacTE(l,m) != 1.0 || Par.penfacTM(l,m) != 1.0))
			 wmmpolerror("EZHZ, NRMMH: PenFac not implemented !");
		      mcp = 7;
		      eps = Wg.eps(l,m);
		      v = Swf(l,m); 
		      v1 = v/((Beta*Beta-K0*K0*eps)*(Beta*Beta-K0*K0*eps));
		      bfod[0][0] = DYF; bcp[0][0] = 'a'; 
		      bfod[1][0] = DYF; bcp[1][0] = 'a'; 
		      	  bpf[0] = v1*K0*K0*eps*eps; 
		      bfod[0][1] = DYF; bcp[0][1] = 'a'; 
		      bfod[1][1] = DXF; bcp[1][1] = 'b'; 
		      	  bpf[1] = -v1*2.0*K0*eps*Beta; 
		      bfod[0][2] = DXF; bcp[0][2] = 'b'; 
		      bfod[1][2] = DXF; bcp[1][2] = 'b'; 
		      	  bpf[2] = v1*Beta*Beta; 
		      bfod[0][3] = DXF; bcp[0][3] = 'a'; 
		      bfod[1][3] = DXF; bcp[1][3] = 'a'; 
		      	  bpf[3] = v1*K0*K0*eps*eps; 
		      bfod[0][4] = DXF; bcp[0][4] = 'a'; 
		      bfod[1][4] = DYF; bcp[1][4] = 'b'; 
		      	  bpf[4] = v1*2.0*K0*eps*Beta; 
		      bfod[0][5] = DYF; bcp[0][5] = 'b'; 
		      bfod[1][5] = DYF; bcp[1][5] = 'b'; 
		      	  bpf[5] = v1*Beta*Beta; 
		      bfod[0][6] = FLD; bcp[0][6] = 'b'; 
		      bfod[1][6] = FLD; bcp[1][6] = 'b'; 
		      	  bpf[6] = v; 
		      break;
		}
		break;
	case NRMME:
		switch(Par.vform)
		{
		case HXHY: 
		      wmmpolerror("HXHY: NRMME not implemented !");
		      break;
		case EXEY: 
		      mcp = 5;
		      v = Swf(l,m); 
		      pf = 1.0; if(pen == 1) pf = Par.penfacTM(l,m);
		      bfod[0][0] = FLD; bcp[0][0] = 'x'; 
		      bfod[1][0] = FLD; bcp[1][0] = 'x'; bpf[0] = v*pf; 
		      pf = 1.0; if(pen == 1) pf = Par.penfacTE(l,m);
		      bfod[0][1] = FLD; bcp[0][1] = 'y'; 
		      bfod[1][1] = FLD; bcp[1][1] = 'y'; bpf[1] = v*pf; 
		      v = v/Beta/Beta;
		      bfod[0][2] = DXF; bcp[0][2] = 'x'; 
		      bfod[1][2] = DXF; bcp[1][2] = 'x'; bpf[2] = v; 
		      bfod[0][3] = DXF; bcp[0][3] = 'x'; 
		      bfod[1][3] = DYF; bcp[1][3] = 'y'; bpf[3] = 2.0*v; 
		      bfod[0][4] = DYF; bcp[0][4] = 'y'; 
		      bfod[1][4] = DYF; bcp[1][4] = 'y'; bpf[4] = v; 
		      break;
		case EZHZ: 
		      if(pen == 1 &&
			 (Par.penfacTE(l,m) != 1.0 || Par.penfacTM(l,m) != 1.0))
			 wmmpolerror("EZHZ, NRMMH: PenFac not implemented !");
		      mcp = 7;
		      eps = Wg.eps(l,m);
		      v = Swf(l,m); 
		      v1 = v/((Beta*Beta-K0*K0*eps)*(Beta*Beta-K0*K0*eps));
		      bfod[0][0] = DYF; bcp[0][0] = 'b'; 
		      bfod[1][0] = DYF; bcp[1][0] = 'b'; 
		      	  bpf[0] = v1*K0*K0; 
		      bfod[0][1] = DYF; bcp[0][1] = 'b'; 
		      bfod[1][1] = DXF; bcp[1][1] = 'a'; 
		      	  bpf[1] = v1*2.0*K0*Beta; 
		      bfod[0][2] = DXF; bcp[0][2] = 'a'; 
		      bfod[1][2] = DXF; bcp[1][2] = 'a'; 
		      	  bpf[2] = v1*Beta*Beta; 
		      bfod[0][3] = DXF; bcp[0][3] = 'b'; 
		      bfod[1][3] = DXF; bcp[1][3] = 'b'; 
		      	  bpf[3] = v1*K0*K0; 
		      bfod[0][4] = DXF; bcp[0][4] = 'b'; 
		      bfod[1][4] = DYF; bcp[1][4] = 'a'; 
		      	  bpf[4] = -v1*2.0*K0*Beta; 
		      bfod[0][5] = DYF; bcp[0][5] = 'a'; 
		      bfod[1][5] = DYF; bcp[1][5] = 'a'; 
		      	  bpf[5] = v1*Beta*Beta; 
		      bfod[0][6] = FLD; bcp[0][6] = 'a'; 
		      bfod[1][6] = FLD; bcp[1][6] = 'a'; 
		      	  bpf[6] = v; 
		      break;
		}
		break;
	case NRMMZ:
		if(pen == 1 &&
		   (Par.penfacTE(l,m) != 1.0 || Par.penfacTM(l,m) != 1.0))
		   wmmpolerror("NRMMZ: PenFac not implemented !");
		switch(Par.vform)
		{
		case HXHY: 
		      wmmpolerror("HXHY: NRMMZ not implemented !");
		      break;
		case EXEY: 
		      wmmpolerror("EXEY: NRMMZ not implemented !");
		      break;
		case EZHZ: 
		      mcp = 2;
		      v = Swf(l,m); 
		      bfod[0][0] = FLD; bcp[0][0] = 'a'; 
		      bfod[1][0] = FLD; bcp[1][0] = 'a'; 
		      	  bpf[0] = v; 
		      bfod[0][1] = FLD; bcp[0][1] = 'b'; 
		      bfod[1][1] = FLD; bcp[1][1] = 'b'; 
		      	  bpf[1] = v; 
		      break;
		}
		break;
	}	
	dim = -1;
	offs = (*Phi(0,l,m,0)).Amsi;
	for(cpj=0; cpj<=1; ++cpj)
	{
		jrj = Phi.ntf(cpj,l,m);
		for(j=0; j<=jrj-1; ++j)
		{
			a = (*Phi(cpj,l,m,j)).Amsi-offs;
			if(a>dim) dim = a;
		}
	}
	++dim;
	for(t=0; t<=mcp-1; ++t)
	{
		cpj = cpidchtoidx(bcp[0][t]);
		cpk = cpidchtoidx(bcp[1][t]);
		fodj = bfod[0][t];
		fodk = bfod[1][t];
		pf = bpf[t];
		jrj = Phi.ntf(cpj,l,m);
		jrk = Phi.ntf(cpk,l,m);
		r = Wg.rectbounds(l,m);
		for(j=0; j<=jrj-1; ++j)
		{
			a = (*Phi(cpj,l,m,j)).Amsi-offs;
			for(k=0; k<=jrk-1; ++k)
			{
				b = (*Phi(cpk,l,m,k)).Amsi-offs;
				mel = tfnrm(*Phi(cpj,l,m,j), fodj, 
					    *Phi(cpk,l,m,k), fodk, r);
				mel *= pf;
				nmat[a+b*dim] += mel;
				nmat[b+a*dim] += mel;
			}
		}
	}
	if(Par.mshift == 0.0) return dim; 
	for(a=0; a<=dim-1; ++a)
	{
		v = nmat[a+a*dim];
		nmat[a+a*dim] += Par.mshift*v;
	}
	return dim;
}

/* normalization matrix for rectangle l, m */
int fillsnmat(int l, int m, double *nmat, int pen)
{
	switch(Pol)
	{
	case QTE: case QTM: return fillqtetmsnmat(l, m, nmat, pen);
	case VEC: return fillvecsnmat(l, m, nmat, pen);
	}
	return 0;
}

/* fill amplitude matrix */
void fillqtetmvecampmat()
{
	int vk;
	int a, b;
	int j, k;
	int rxi0, ryi0; 
	int rxi1, ryi1; 
	int jr0, jr1;
	double beg, end, cor, mel;
	double cr;
	double v;
	double swf;

	int ri0, ri1, cp0, cp1;
	Derlev fod0, fod1;
	double wf;
	int dc;

	for(vk=0; vk<=NumVk-1; ++vk)
	{
		swf = Vkswf(vk);
		if(swf != 0.0)
		{
			for(dc=0; dc<=Numdbac(vk)-1; ++dc)
			{
				fod0 = (Dbac[vk][dc]).fod0;
				fod1 = (Dbac[vk][dc]).fod1;
				ri0  = (Dbac[vk][dc]).ri0;
				ri1  = (Dbac[vk][dc]).ri1;
				cp0  = (Dbac[vk][dc]).cp0;
				cp1  = (Dbac[vk][dc]).cp1;
				wf   = (Dbac[vk][dc]).wf;

				rxi0 = Nrxi(ri0,vk); 
				ryi0 = Nryi(ri0,vk); 
				jr0 = Phi.ntf(cp0,rxi0,ryi0);

				rxi1 = Nrxi(ri1,vk); 
				ryi1 = Nryi(ri1,vk); 
				jr1 = Phi.ntf(cp1,rxi1,ryi1);

				beg = Vkbeg(vk);
				end = Vkend(vk);
				cor = Vkcor(vk);

				switch(Vktyp(vk))
				{
				case HOR: 
					cr = wf*swf;
					for(j=0; j<=jr0-1; ++j)
					{
					   a = (*Phi(cp0,rxi0,ryi0,j)).Amsi;
					   for(k=0; k<=jr1-1; ++k)
					   {
					      b = (*Phi(cp1,rxi1,ryi1,k)).Amsi;
					      mel = 
					      tfinty(*Phi(cp0,rxi0,ryi0,j), 
						     fod0,
					             *Phi(cp1,rxi1,ryi1,k), 
						     fod1,
					             cor, beg, end);
					      mel *= cr;
					      Amat[a+b*AmDim] += mel;
					      Amat[b+a*AmDim] += mel;
					   }
					}
					break;

				case VER: 
					cr = wf*swf;
					for(j=0; j<=jr0-1; ++j)
					{
					   a = (*Phi(cp0,rxi0,ryi0,j)).Amsi;
					   for(k=0; k<=jr1-1; ++k)
					   {
					      b = (*Phi(cp1,rxi1,ryi1,k)).Amsi;
					      mel = 
					      tfintx(*Phi(cp0,rxi0,ryi0,j), 
						     fod0,
					             *Phi(cp1,rxi1,ryi1,k), 
						     fod1,
					             cor, beg, end);
					      mel *= cr;
					      Amat[a+b*AmDim] += mel;
					      Amat[b+a*AmDim] += mel;
					   }
					}
					break;
				}
			}
		}
	}
	if(Par.mshift == 0.0) return;
	for(a=0; a<=AmDim-1; ++a)
	{
		v = Amat[a+a*AmDim];
		Amat[a+a*AmDim] += Par.mshift*v;
	}
	return;
}

/* fill least squares error matrix */
void fillampmat()
{
	int vk;
	settfpars();
	Amat = new double[AmDim*AmDim];
	matto0(Amat, AmDim, AmDim);
	if(Pol != VEC)
	{
		Pffh = Dmatrix(2, NumVk);
		Pffv = Dmatrix(2, NumVk);
		Pfdx = Dmatrix(2, NumVk);
		Pfdy = Dmatrix(2, NumVk);
	}
	else
	{
		VPfO = Dmatrix(2, NumVk);
		VPfP = Dmatrix(2, NumVk);
		VPfQ = Dmatrix(2, NumVk);
		VPfR = Dmatrix(2, NumVk);
		VPfS = Dmatrix(2, NumVk);
		VPfT = Dmatrix(2, NumVk);
		VPfU = Dmatrix(2, NumVk);
		VPfV = Dmatrix(2, NumVk);
		VPfW = Dmatrix(2, NumVk);
		VPfX = Dmatrix(2, NumVk);
		VPfY = Dmatrix(2, NumVk);
		VPfZ = Dmatrix(2, NumVk);
	}
	Numdbac = Ivector(NumVk);
	Dbac = new efcont*[NumVk];
	if(Pol == VEC)
	{
		MaxNumdbac = 68;
		for(vk=0; vk<=NumVk-1; ++vk) Dbac[vk] = new efcont[MaxNumdbac];
	}
	else
	{
		MaxNumdbac = 6;
		for(vk=0; vk<=NumVk-1; ++vk) Dbac[vk] = new efcont[MaxNumdbac];
	}
	setmodeproblemformulation();
	fillqtetmvecampmat();

	return;
}

/* free least squares error matrix */
void closeampmat()
{
	int vk;
	delete[] Amat;
	if(Pol != VEC)
	{
		Pffh.strip();
		Pffv.strip();
		Pfdx.strip();
		Pfdy.strip();
	}
	else
	{
		VPfO.strip();
		VPfP.strip();
		VPfQ.strip();
		VPfR.strip();
		VPfS.strip();
		VPfT.strip();
		VPfU.strip();
		VPfV.strip();
		VPfW.strip();
		VPfX.strip();
		VPfY.strip();
		VPfZ.strip();
	}
	Numdbac.strip();
	for(vk=0; vk<=NumVk-1; ++vk) delete[] Dbac[vk];
	delete[] Dbac;
	return;
}