load "Element_P4"
load "lapack"
load "qf11to25"

include "CC.idp"

int[int] ne1=[1,2,0];
int[int] ne2=[2,0,1];
// the fonctions bubble 
int[int] k6=[4,5,  9, 11, 15,16 ]; 
//  the edges function 
int[int] Be=[1,3,2, 8,10,6, 12,17,13];

func NN=[N.x,N.y];

// the ref triangle 
int[int] ll=[2,0,0,1];
mesh Th=square(1,1,flags=2,label=ll);

Th = trunc(Th,x<0.5,label=0);
//  the 2 base vector [Cx,Cy] for interior DoF
real[int] Dx= [Th(2).x-Th(0).x, Th(0).x-Th(1).x];// 1: 20 et 2: 01 
real[int] Dy= [Th(2).y-Th(0).y, Th(0).y-Th(1).y];
real dJ1 =1/Th.area/2;
real[int] Cx=  Dy*-dJ1;
real[int] Cy=  Dx*dJ1;


fespace Rh(Th,RT0);
fespace P1h(Th,P1); 
fespace Ch(Th,[P4,P4]);// To store momoe function
Rh[int] [phi1,phi2](3);
P1h[int] l(3);

macro pp(i,j)  (l[i]*l[j])// P2 monome
	real err =0;

// Build phi and l basic functions 
real[int] sgf=[1,-1,1];
for(int i=0;i<3;++i)
{
l[i][]=0;     l[i][][i]=1;
phi1[i][]=0;  phi1[i][][i]=sgf[i];
}
Ch[int]  [b1,b2](6*3); //  build all momone functions
int [int,int] bii(18,3);

{   
	int k=0;   
	for(int i=0; i<3;++i)
	  for(int e=0; e<2;++e)//  
	    for(int j=0; j<3;++j)//  
	    {	
		int i1 = j, i2=j; //  sommet
		if(e) {i1 = ne1[j]; i2 =ne2[j];} // vertex of edge j
		bii(k,0) =i;
		bii(k,1) =i1;
		bii(k,2) =i2;
		[b1[k],b2[k]]= [phi1[i],phi2[i]]*pp(i1,i2);
		k++;
		}
}


real[int,int] Cb(15,6); //  coef of monone too be with mass mod 0
Ch[int] [Fb1,Fb2](15);  // the mono funct for verif.
{
	real[int,int] A(18,6);
	for(int i=0;i<18; ++i)
	  for(int j=0;j<3; ++j)
	   for(int k=0;k<2; ++k)
	     A(i,2*j+k) = int2d(Th)( [b1[i],b2[i]]'*[Cx[k],Cy[k]]*l[j]);

	real[int,int]  C(6,6),C1(6,6) ; 

	for( int j=0; j<6; ++j)// ligne 
	 for( int l=0; l<6; ++l)
	  C(j,l)= A(k6(l),j);
	C1=C^-1;


	for( int i=0; i<9; ++i)
	{
		int ki = Be[i];
		real[int] a6(6),b6(6);//
		for( int j=0; j<6; ++j)// ligne
		   b6(j)= -A(ki,j) ;
		a6 = C1*b6;
		Cb(i,:)=a6;
	}
	for( int i=0; i<6; ++i)
	 Cb(9+i,:)=C1(:,i);

	// les fonction bases

	for(int i=0; i< 15; ++i)
	{
		Ch [F1,F2];
		F1[]=0.; 
		if( i<9) 
			F1[] = b1[Be[i]][]; 
		for(int k=0; k< 6; ++k) // 
		  F1[] +=  Cb(i,k)*b1[k6[k]][];
		Fb1[i][]=F1[]; 
	}
}
//  Verif DOF 
// Les flux 
real[int][int] sigma(15);// To store all Dof linear form 
real[int,int] C1(3,3);// the DOf on edge 
{
	real[int,int] CC=[[ 24 , 6 , 4],[6,4,6],[4 , 6  ,24]];//   
	CC /= 120.;
	C1 = CC^-1; 
	int dof =0;
	// Edge dof 
	for (int i=0; i<3; ++i)
	{  	
		int i1 = ne1[i], i2 =ne2[i];
		for(int k=0;k<3;++k)
		{
			int kk = i*3+k;
			func FF=[Fb1[kk],Fb2[kk]];
			
			func fl = C1(0,k)*pp(i1,i1) 
			        + C1(1,k)*pp(i1,i2) 
					+ C1(2,k)*pp(i2,i2);
	
		    varf vdof([uu,vv],[u1,u2]) = int1d(Th,i,qforder=10)( fl*([u1,u2]'*[N.x,N.y]));
			sigma[dof].resize(Ch.ndof);
			sigma[dof++] = vdof(0,Ch);
	    }		
	}
	// Internal Dof 
	for (int j=0; j<6; ++j)
	   {
		   int i=j/2;
		   int k=j%2; 
		   varf vdof([uu,vv],[u1,u2]) = int2d(Th)( l[i]*[u1,u2]'*[Cx[k],Cy[k]]);
		   sigma[dof].resize(Ch.ndof);
		   sigma[dof++] =vdof(0,Ch);
	   }
	   assert(dof==15);
	   //  Check the if DoF and B.F are OK

	   for(int i=0; i<15; ++i)
	   {
	     for(int j=0; j<15; ++j)
		 {
			 real dij = Fb1[i][]'*sigma[j];
			 err = err+ abs( dij-(i==j));
			 cout << c00(dij) << " " ; 
		 }
		 cout << " err=" << err << endl;
       }
	   
}
// data Genaration for  FF++
CCt("cf",Cb); 
CCt("Bii",bii); 
CC("fe",Be);
CC("k6",k6);

CC("c1",C1);
cout << endl;
assert(err< 1e-10); 
