/*
 * ADAPT2D : a software for automatic mesh adaptation in 2D
 *
 * AUTHOR : Manuel J. Castro Diaz(e-mail:castro@gamba.cie.uma.es)
 * ADAPTED FOR FREEFEM : Prud'homme Christophe (e-mail:prudhomm@ann.jussieu.fr)
 *
 * this code is public domain
 * 
 * You may copy freely these files and use it for    
 * teaching or research. These or part of these may   
 * not be sold or used for a commercial purpose without
 * our consent
 * 
 * Any problems should be reported to the AUTHOR
 * at the following address : castro@gamba.cie.uma.es
 */

#include <header.hxx>
#include <v_t1_dl.hxx>
#include <t_t1_dl.hxx>
#include <m_t0.hxx>
#include <integ.hxx>

extern int renum(Triangulo_T1_dlist*,Vertice_T1_dlist*,int);

Scalar fa(R3 pt) {
  return sqrt(2.0);
}
Scalar fb(R3 pt) {
  R2 e1(1,0),e2(0,1);
  R2 pt0;
  pt0=e1*pt.x[0]+e2*pt.x[1];
  return (6.0*pt0.x-2.0);
}
Scalar fc(R3 pt) {
  R2 e1(1,0),e2(0,1);
  R2 pt0;
  Scalar sq=sqrt(3.0);
  pt0=e1*pt.x[0]+e2*pt.x[1];
  return (4*sq*pt0.y+2*sq*pt0.x-2*sq);
} 
Scalar eval(Triangulo_T0* t0, R2 cb, Scalar* sol, int nsoli, int nsol,Vertice_T0* s0_ini) {

  Scalar c[3];
  Scalar ev=0.0;
  int pos,i;
  c[0]=cb.x;
  c[1]=cb.y;
  c[2]=1-cb.x-cb.y;
  for (i=0; i<3; i++) {
    pos=t0->s[i]-s0_ini;
    ev+=c[i]*sol[pos*nsol+nsoli];
  }
  return ev;
}

Scalar gaussband (Scalar *a, Scalar *x, int n, int bdthl,Scalar eps,int nsol)
/*
 * Factorise (first=1) and/or solve 	Ay = x  with result in x 
 * LU is stored in A ; returns the value of the smallest pivot  
 * all pivots less than eps are put to eps 
 * a[i][j] is stored in a[i-j+bdthl][j]=a[n*(i-j+bdthl)+j) 
 * where -bdwth <= i-j <= bdthl
 */
{
  int i,j,k;	 
  Scalar s, s1, smin = 1e+30;
  
 			/* factorization */
  for (i=0;i<n;i++) 
    {
      for(j=MAX(i-bdthl,0);j<=i;j++)
        {
          s=0;	
          for (k=MAX(i-bdthl,0); k<j;k++) 
            s += a[n*(i-k+bdthl)+k]*a[n*(k-j+bdthl)+j] ;
          a[n*(i-j+bdthl)+j] -= s ;
        }
      for(j=i+1;j<=MIN(n-1,i+bdthl);j++)
        {
          s=0;	
          for (k=MAX(j-bdthl,0);k<i;k++) 
            s += a[n*(i-k+bdthl)+k]*a[n*(k-j+bdthl)+j] ;
          s1 = a[n*bdthl+i];
          if(fabs(s1) < smin) smin=fabs(s1);
          if(fabs(s1) < eps) s1 = eps;
          a[n*(i-j+bdthl)+j] = ( a[n*(i-j+bdthl)+j] - s)/s1;
        }
    }
  for (j=0; j<nsol; j++) {
    for (i=0;i<n;i++)   	/*  resolution */
      {	
        s=0;	
        for (k=MAX(i-bdthl,0);k<i;k++)
          s += a[n*(i-k+bdthl)+k] * x[j*n+k];
        x[j*n+i] = (x[j*n+i] - s ) / a[n*bdthl+i] ;
      }
    for (i=n-1;i>=0;i--)
      {
        s=0;	
        for (k=i+1; k<=MIN(n-1,i+bdthl);k++)
          s += a[n*(i-k+bdthl)+k] * x[j*n+k];
        x[j*n+i] -= s ;
      }
  }
  return smin;	
}
int matrix_wdth(Triangulo_T1_dlist* Ltriangle){
  Triangulo_T1_dlink* lk;
  int sig[3]={1,2,0};
  int i,length=0, aux=0,pos0,pos1;
  lk=Ltriangle->principio();
  while (lk) {
    for (i=0; i<3; i++) {
      pos0=lk->t->s[i]->lk->pos-1;
      pos1=lk->t->s[sig[i]]->lk->pos-1;
      aux=abs(pos0-pos1);
      length=MAX(aux,length);
    }
    lk=lk->sig();
  }
  return length;
}
void matrix_construction(Scalar *a, Triangulo_T1_dlist* Ltriangle,int nbs,int bdth) {
/* 
 *
 *------ Builds the matrix A ----------- 
 *
*/
  Triangulo_T1_dlink* lk;
  Scalar factor,area;
  int i,j,pos0,pos1;

  for(i=0;i<=2*bdth;i++)
    for(j=0;j<nbs;j++)
      a[nbs*i+j] = 0 ;
  lk=Ltriangle->principio();
  while (lk) {
    area=lk->t->area();
    for(i=0;i<3;i++) {
      pos0=lk->t->s[i]->lk->pos-1;
      for(j=0;j<3;j++) {
        pos1=lk->t->s[j]->lk->pos-1;
        factor = i==j ? 1.0/6.0 : 1.0 / 12.0 ;
        a[nbs*(pos0-pos1+bdth)+pos1]  +=area*factor;
      }
    }
    lk=lk->sig();
  }
}

void second_construction(Scalar* u,Triangulo_T1_dlist* Ltriangle,Mallado_T0* malla,Scalar* solucion,int nsol,int nbs) {
  Integ itn;
  Triangulo_T1_dlink* lk;
  Triangulo_T1* t1;
  Triangulo_T0**tp;
  Triangulo_T0* t0,*t_fin;
  Vertice_T0* s0_ini=malla->saca_sommet(0);
  R2* cb;
  R2 pt,proyec;
  Scalar area0,ev,beta0,beta1,beta2,eps=malla->eps_malla()/3.0;
  int i,j,pos0,npint=16;
  itn.set(npint);
  tp=new Triangulo_T0*[npint];
  cb=new R2[npint];
  if (cb==NIL || tp==NIL) ERROR();
  for (i=0; i<nbs*nsol; i++) u[i]=0.0;
  lk=Ltriangle->principio();
  while (lk) {
    t1=lk->t;
    area0=t1->area();
    t0=t1->s[0]->t0;
    for (i=0; i<npint; i++) {
      pt.set(0,0);
      for (j=0; j<3; j++) {
        pt=pt+t1->s[j]->c*itn.puntos[i].x[j];
      }
      t_fin=malla->localiza(pt,proyec,t0,eps);
      cb[i]=t_fin->coor_bar(proyec);
      tp[i]=t_fin;
    }
    for (i=0; i<nsol; i++) {
      beta0=beta1=beta2=0;
      for (j=0; j<npint; j++) {
        ev=eval(tp[j],cb[j],solucion,i,nsol,s0_ini);
        beta0+=ev*itn.puntos[j].x[0]*itn.pesos[j];
        beta1+=ev*itn.puntos[j].x[1]*itn.pesos[j];
        beta2+=ev*itn.puntos[j].x[2]*itn.pesos[j];
      }
      pos0=(t1->s[0]->lk->pos-1);
      u[i*nbs+pos0]+=beta0*2*area0;
      pos0=(t1->s[1]->lk->pos-1);
      u[i*nbs+pos0]+=beta1*2*area0;
      pos0=(t1->s[2]->lk->pos-1);
      u[i*nbs+pos0]+=beta2*2*area0;
    }
    lk=lk->sig();
  }
  delete[] cb;
  delete[] tp; 
}
Scalar *interpolacion (Vertice_T1_dlist* Lsommet,Mallado_T0 * malla,\
                    int nsol, Scalar* solucion,Triangulo_T1_dlist* Ltriangle, int tinterp) {
  Integ itn;
  Triangulo_T1_dlink* lt;
  Triangulo_T1* t1;
  Triangulo_T0**tp;
  Triangulo_T0* t0,*t_fin;
  Vertice_T1_dlink* lk;
  Vertice_T1* s0;
  Vertice_T0* s0_ini=malla->saca_sommet(0);
  R3* puntos;
  R2* cb,ccb;
  R2 pp[3];
  R2 c0,pt,proyec,v1,v2;
  Scalar* solucion_interp;
  Scalar* area,*u,*mt;
  Scalar area0,beta0,beta1,beta2,ev,pivot;
  Scalar *eva;
  Scalar eps=malla->eps_malla()/3.0;
  Scalar arr[3][2];
  int nbs,i,j,k,l,npint=16,cont=0,pos0,length,npt=25;
  int err;
  int ptr[3][2][7];


#ifdef DEBUG
  cout<<endl<<endl;
  cout<<"------------------------------------------------"<<endl;
  cout<<"            SOLUTION INTERPOLATION."<<endl;
  cout<<"------------------------------------------------"<<endl;
  cout<<endl;
#endif /* DEBUG */
  
  nbs=Lsommet->num_elem();
  solucion_interp=new Scalar[nsol*nbs];
  if (solucion_interp==NIL) ERROR();
  switch (tinterp) {
  case 1: {
    Lsommet->numera();
#ifdef DEBUG
    length=matrix_wdth(Ltriangle);
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */
    err=renum(Ltriangle,Lsommet,nbs);
    if (err!=0) {
      cerr<<"Error in renum."<<endl;
      if (err==-2) cerr<<"Not enough memory."<<endl;
      exit(-1);
    }      
#ifdef DEBUG
    length=matrix_wdth(Ltriangle);
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */
    lk=Lsommet->principio();
    while (lk) {
      s0=lk->s;
      c0=s0->c;
      t0=s0->t0;
      cont=s0->lk->pos-1;
      malla->interpola_sol(c0,t0,solucion,solucion_interp,cont,nsol);
//      cont++;
      lk=lk->sig();
    }
    break;
  }
  case 2: {
    itn.set(npint);
    area=new Scalar[nbs];
    tp=new Triangulo_T0*[npint];
    cb=new R2[npint];
    if (cb==NIL || tp==NIL || area==NIL) ERROR();
    for (i=0; i<nbs*nsol; i++) solucion_interp[i]=0.0;
    for (i=0; i<nbs; i++) area[i]=0.0;
    Lsommet->numera();
#ifdef DEBUG
    length=matrix_wdth(Ltriangle);
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */
    err=renum(Ltriangle,Lsommet,nbs);
    if (err!=0) {
      cerr<<"Error in renum."<<endl;
      if (err==-2) cerr<<"Not enough memory."<<endl;
      exit(-1);
    }      
#ifdef DEBUG
    length=matrix_wdth(Ltriangle);
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */
    lt=Ltriangle->principio();
    while (lt) {
      t1=lt->t;
      area0=t1->area();
      for (i=0; i<3; i++) {
        area[t1->s[i]->lk->pos-1]+=area0;
      }
      t0=t1->s[0]->t0;
      for (i=0; i<npint; i++) {
        pt.set(0,0);
        for (j=0; j<3; j++) {
          pt=pt+t1->s[j]->c*itn.puntos[i].x[j];
        }
        t_fin=malla->localiza(pt,proyec,t0,eps);
        cb[i]=t_fin->coor_bar(proyec);
        tp[i]=t_fin;
      }
      for (i=0; i<nsol; i++) {
        beta0=beta1=beta2=0;
        for (j=0; j<npint; j++) {
          ev=eval(tp[j],cb[j],solucion,i,nsol,s0_ini);
          pt.set(0,0);
          for (k=0; k<3; k++) {
            pt=pt+t1->s[k]->c*itn.puntos[j].x[k];
          }
          beta0+=ev*fa(itn.puntos[j])*itn.pesos[j];
          beta1+=ev*fb(itn.puntos[j])*itn.pesos[j];
          beta2+=ev*fc(itn.puntos[j])*itn.pesos[j];
        }
        pos0=(t1->s[0]->lk->pos-1)*nsol+i;
        solucion_interp[pos0]+=(sqrt(2)*beta0+4*beta1)*area0;
        pos0=(t1->s[1]->lk->pos-1)*nsol+i;
        solucion_interp[pos0]+=(sqrt(2)*beta0-2*beta1+2*sqrt(3)*beta2)*area0;
        pos0=(t1->s[2]->lk->pos-1)*nsol+i;
        solucion_interp[pos0]+=(sqrt(2)*beta0-2*beta1-2*sqrt(3)*beta2)*area0;
      }
      lt=lt->sig();
    }
    for (i=0; i<nsol; i++) {
      for (j=0; j<nbs; j++) {
        solucion_interp[j*nsol+i]=solucion_interp[j*nsol+i]/area[j];
      }
    }
    delete[] area;
    delete[] cb;
    delete[] tp;
    break;
  }
  case 3: {
    delete[] solucion_interp;
    Lsommet->numera();
    length=matrix_wdth(Ltriangle);
#ifdef DEBUG
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */
    err=renum(Ltriangle,Lsommet,nbs);
    if (err!=0) {
      cerr<<"Error in renum."<<endl;
      if (err==-2) cerr<<"Not enough memory."<<endl;
      exit(-1);
    }      
    length=matrix_wdth(Ltriangle);
#ifdef DEBUG
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */
    mt=new Scalar [nbs*(2*length+1)];
    u=new Scalar[nbs*nsol];
    if (mt==NIL || u==NIL) ERROR();

    matrix_construction(mt,Ltriangle,nbs,length);

    second_construction(u,Ltriangle,malla,solucion,nsol,nbs);
#ifdef DEBUG
    cout<<"Computing gaussband."<<endl;
#endif /* DEBUG */
    pivot=gaussband(mt,u,nbs,length,1e-16,nsol);

    delete[] mt;
    solucion_interp=new Scalar[nsol*nbs];
    if (solucion_interp==NIL) ERROR();
    for (i=0; i<nsol; i++) {
      for (j=0; j<nbs; j++) {
        solucion_interp[j*nsol+i]=u[i*nbs+j];
      }
    }
    delete[] u;
    break;
  } 
  case 4: {
    npint=7;
    itn.set(npint);
    area=new Scalar[nbs];
    puntos=new R3[npt];
    eva=new Scalar[nsol*npt];
    if (puntos==NIL || eva==NIL || area==NIL) ERROR();
    for (i=0; i<nbs*nsol; i++) solucion_interp[i]=0.0;
    for (i=0; i<nbs; i++) area[i]=0.0;
    ptr[0][0][0]=0 ;ptr[0][0][1]=1 ; ptr[0][0][2]=24;  
    ptr[0][0][3]=6 ;ptr[0][0][4]=13; ptr[0][0][5]=12; ptr[0][0][6]= 18;
    ptr[0][1][0]=0 ;ptr[0][1][1]=24; ptr[0][1][2]=5;  
    ptr[0][1][3]=12;ptr[0][1][4]=17; ptr[0][1][5]=11; ptr[0][1][6]= 23;
    
    ptr[1][0][0]=1 ;ptr[1][0][1]=2 ; ptr[1][0][2]=24;  
    ptr[1][0][3]=7 ;ptr[1][0][4]=14; ptr[1][0][5]=13; ptr[1][0][6]= 19;
    ptr[1][1][0]=2 ;ptr[1][1][1]=3 ; ptr[1][1][2]=24;  
    ptr[1][1][3]=14;ptr[1][1][4]=8 ; ptr[1][1][5]=15; ptr[1][1][6]= 20;
    
    ptr[2][0][0]=24;ptr[2][0][1]=3 ; ptr[2][0][2]= 4;  
    ptr[2][0][3]=15;ptr[2][0][4]=9 ; ptr[2][0][5]=16; ptr[2][0][6]= 21;
    ptr[2][1][0]=4 ;ptr[2][1][1]=5 ; ptr[2][1][2]=24;  
    ptr[2][1][3]=10;ptr[2][1][4]=17; ptr[2][1][5]=16; ptr[2][1][6]= 22;
    
    puntos[0 ].x[0]=1.;         puntos[0].x[1]=0.;
    puntos[1 ].x[0]=.5;         puntos[1].x[1]=.5;
    puntos[2 ].x[0]=0.;         puntos[2].x[1]=1.;
    puntos[3 ].x[0]=0.;         puntos[3].x[1]=.5;
    puntos[4 ].x[0]=0.;         puntos[4].x[1]=0.;
    puntos[5 ].x[0]=.5;         puntos[5].x[1]=0.;
    puntos[6 ].x[0]=3./4.;      puntos[6].x[1]=1./4.;
    puntos[7 ].x[0]=1./4.;      puntos[7].x[1]=3./4.;
    puntos[8 ].x[0]=0.;         puntos[8].x[1]=3./4.;
    puntos[9 ].x[0]=0.;         puntos[9].x[1]=1./4.;
    puntos[10].x[0]=1./4.;      puntos[10].x[1]=0.;
    puntos[11].x[0]=3./4.;      puntos[11].x[1]=0.;
    puntos[12].x[0]=2./3.;      puntos[12].x[1]=1./6.;
    puntos[13].x[0]=5./12.;     puntos[13].x[1]=5./12.;
    puntos[14].x[0]=1./6.;      puntos[14].x[1]=2./3.;
    puntos[15].x[0]=1./6.;      puntos[15].x[1]=5./12.;
    puntos[16].x[0]=1./6.;      puntos[16].x[1]=1./6.;
    puntos[17].x[0]=5./12.;     puntos[17].x[1]=1./6.;
    puntos[18].x[0]=11./18.;    puntos[18].x[1]=5./18.;
    puntos[19].x[0]=5./18.;     puntos[19].x[1]=11./18.;
    puntos[20].x[0]=1./9.;      puntos[20].x[1]=11./18.;
    puntos[21].x[0]=1./9.;      puntos[21].x[1]=5./18.;
    puntos[22].x[0]=5./18.;     puntos[22].x[1]=1./9.;
    puntos[23].x[0]=11./18.;    puntos[23].x[1]=1./9.;
    puntos[24].x[0]=1./3.;      puntos[24].x[1]=1./3.;
    for (i=0; i<npt; i++) 
      puntos[i].x[2]=1-puntos[i].x[0]-puntos[i].x[1];
    Lsommet->numera();
#ifdef DEBUG
    length=matrix_wdth(Ltriangle);
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */
    err=renum(Ltriangle,Lsommet,nbs);
    if (err!=0) {
      cerr<<"Error in renum."<<endl;
      if (err==-2) cerr<<"Not enough memory."<<endl;
      exit(-1);
    }      
#ifdef DEBUG
    length=matrix_wdth(Ltriangle);
    cout<<"length:"<<length<<endl;
#endif /* DEBUG */  
    lt=Ltriangle->principio();
    while (lt) {
      t1=lt->t;
      for (i=0; i<3; i++) {
        area0=0;
        for (j=0; j<2; j++) {
          for (k=0; k<3; k++) {
            pp[k]=t1->s[0]->c*puntos[ptr[i][j][k]].x[0]+
                  t1->s[1]->c*puntos[ptr[i][j][k]].x[1]+
                  t1->s[2]->c*puntos[ptr[i][j][k]].x[2];
          }
          v1=pp[0]-pp[2];
          v2=pp[1]-pp[2];
          area0+=fabs((v1.x*v2.y-v1.y*v2.x)/2.0);
          arr[i][j]=fabs((v1.x*v2.y-v1.y*v2.x)/2.0);
        }        
        area[t1->s[i]->lk->pos-1]+=area0;
      }
      t0=t1->s[0]->t0;

      for (i=0; i<npt; i++) {
        pt.set(0,0);
        for (j=0; j<3; j++) {
          pt=pt+t1->s[j]->c*puntos[i].x[j];
        }
        t_fin=malla->localiza(pt,proyec,t0,eps);
        ccb=t_fin->coor_bar(proyec);
        for (k=0; k<nsol; k++) 
          eva[k*npt+i]=eval(t_fin,ccb,solucion,k,nsol,s0_ini);
      }
      
      for (i=0; i<nsol; i++) {
        for (j=0; j<3; j++) {
          pos0=(t1->s[j]->lk->pos-1)*nsol+i;
          for (k=0; k<2; k++) {
            beta0=0.0;
            for (l=0; l<npint; l++) {
              ev=eva[i*npt+ptr[j][k][l]];
              beta0+=ev*itn.pesos[l];
            }
            solucion_interp[pos0]+=beta0*2.0*arr[j][k];
          }        
        }
      }
      lt=lt->sig();
    }
    for (i=0; i<nsol; i++) {
      for (j=0; j<nbs; j++) {
        solucion_interp[j*nsol+i]=solucion_interp[j*nsol+i]/area[j];
      }
    }
    delete[] area;
    delete[] puntos;
    delete[] eva;
    break;
  } 
  default: {
    cerr<<"Sorry. Not implemented."<<endl;
    cerr<<"Error in interpolation.C"<<endl;
    exit(-1);
  }
  }
 

  return solucion_interp;

 
}
