/*
 * 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 <m_t0.hxx>

/*
                  CAL_METRICA SUBROUTINE

   Metric computation.

   Input:
   -----     malla    : Mesh.
             solution : Solution over the mesh.
             aniso    : (0=Isotropic adaptation - 1=anisotropic adaptation)
             err0     : Error tolerance.
             lmax     : Maximal edge length. 
             lmin     : Minimzal edge length.
             nsol     : dimension of solution field.
             refwall  : Wall layer reference (NS equation).
             hwall    : Wall prescribed h in the normal direction.


   Output:
   ------    mtr  : Computed metrics.
*/

extern void der2(Scalar*,Mallado_T0*,Scalar*,int);

void cal_metrica(Mallado_T0* malla, Scalar* solution, Metrica* mtr,
                 Boolean aniso,Scalar err0,Scalar lmax, Scalar lmin,
                 int nsol,int refwall,Scalar hwall,Scalar escala,int nbt_max) {
  Triangulo_T0_dlist* t_vec;
  Triangulo_T0_dlink* t_aux;
  Triangulo_T0* t0;
  Arista_T0* a0;
  Vertice_T0* s0, *s1;
  Vertice_T0* s_ini=malla->saca_sommet(0);
  Metrica mtr0;
  R2xR2* mm;
  R2xR2 m0,m1,m2,mi;
  R2xR2* msol;
  R2 tang,c0,c1,c2;
  R2 tan1,normal;
  Scalar lg,lg1,lg2,epsm=0.5;
  Scalar *hes, *sol, *s_min, *s_max;
  Scalar snbt,snbt0,snbt1;
  Scalar rest,d00,dmax,dmin,d0;
  Scalar errf=err0;
  int nbs=malla->nbss();
  int nbt=malla->nbtt();
  int nba=malla->nbaa();
  int sig[3]={1,2,0};
  int i,j,cont=0,err;
  int pos0,pos1,ps;
  int n_cont=0,it_max=5,violation;


  sol=new Scalar[nbs*nsol];
  hes=new Scalar[nbs*nsol*3];
  msol=new R2xR2[nsol];
  mm=new R2xR2[nbs];
  if (hes==NULL || msol==NULL || mm==NULL || sol==NULL) ERROR();
  
  
/*
  copy of a Normalizacion de la solucion into sol[]
*/
  
  s_min=new Scalar[nsol];
  s_max=new Scalar[nsol];
  if (s_min==NULL || s_max==NULL) ERROR();
  for (i=0; i<nsol; i++) { 
    s_max[i]=-1e+30;
    s_min[i]=1e+30;
  }    
  for (i=0; i<nbs; i++) {
    for (j=0; j<nsol; j++) {
      if (solution[nsol*i+j]>s_max[j]) s_max[j]=solution[nsol*i+j];
      if (solution[nsol*i+j]<s_min[j]) s_min[j]=solution[nsol*i+j];
    }
  }
  for (j=0; j<nsol; j++) {
    if (fabs(s_max[j]-s_min[j])>1e-20) {
      for (i=0; i<nbs; i++) {
	sol[nsol*i+j]=(solution[nsol*i+j]-s_min[j])/(s_max[j]-s_min[j]);
      }
    }
    else {
      for (i=0; i<nbs; i++) {
	sol[nsol*i+j]=1.0;
      }    
    }
  }
  
  if (s_min) delete[] s_min;
  if (s_max) delete[] s_max;



/*
     hes = Hessian matric of sol
  */

  der2(sol,malla,hes,nsol);
  if (sol) delete[] sol;
  
/*
          Metric computation.
  
        M=| H | 

*/

  do {
#ifdef DEBUG
    cout<<endl;
    cout<<"Metric adaptation step:"<<n_cont+1<<endl;
    cout<<"-----------------------"<<endl;
#endif /* DEBUG */
    for (i=0; i<nbs; i++) {
      for (j=0; j<nsol; j++) {
        m0.set(hes[j*nbs*3+i*3+0],hes[j*nbs*3+i*3+1],hes[j*nbs*3+i*3+1],hes[j*nbs*3+i*3+2]);
        m0=m0*(escala*escala/errf);
        m1=m0.metriz(aniso);
        m2=m1.truncada(lmax/escala,lmin/escala);
        msol[j].set(m2.x.x,m2.x.y,m2.x.y,m2.y.y);
        if (msol[j].det()<1e-12) {
          cout<<"Warning!! Metric no positive definite."<<endl<<msol[j]<<endl;
        }
      }
      if (nsol==1) {
        mtr[i].set(m2.x.x,m2.x.y,m2.y.y);
      }
      else {
        m1=msol[0];
        m2=msol[1];
        cont=1;
        while (cont<nsol) {
          mi=intersec(m1,m2);
          cont++;
          m1=mi;
          if (cont<nsol) m2=msol[cont];
        }
        mtr[i].set(mi.x.x,mi.x.y,mi.y.y);
      }
    }
//  if (hes) delete[] hes;
    
//  Boundary corrections.
    for (i=0; i<nbs; i++) {
      s0=malla->saca_sommet(i);
      cont=0;
      if (s0->front==TRUE && s0->ref!=refwall) { // not boundary wall
        m0.set(0,0,0,0);
        t_vec=new Triangulo_T0_dlist;
        if (t_vec==NULL) ERROR();
        t_vecinos(s0,*t_vec);
        t_aux=t_vec->principio();
        while(t_aux) {
          t0=t_aux->t;
          pos0=t0->sommet(s0);
          if (pos0<0) {
            cerr<<"Error. Vertex not in triagle."<<endl;
            cerr<<"Vertex:"<<*s0<<endl;
            cerr<<"Triangle:"<<*t0<<endl;
            cerr<<"Error in subroutine cal_metrica."<<endl;
            exit(1);
          }
          else {
            pos0=sig[pos0];
            s1=t0->s[pos0];
            pos1=s1-s_ini;
            if (s1->front==FALSE) {
              cont++;
              m1.set(mtr[pos1].coef[0],mtr[pos1].coef[1],\
                     mtr[pos1].coef[1],mtr[pos1].coef[2]);
              m0=m0+m1;
            }
          }
          t_aux=t_aux->sig();
        }
        if (cont) {
          m0=m0/Scalar(cont);
          mtr[i].set(m0.x.x,m0.x.y,m0.y.y);
        }
        delete t_vec;
      }
      else { 
        if (s0->front==TRUE && s0->ref==refwall && hwall!=0.0) {// wall edge
          m0.set(mtr[i].coef[0],mtr[i].coef[1],\
                 mtr[i].coef[1],mtr[i].coef[2]);
          t_vec=new Triangulo_T0_dlist;
          if (t_vec==NULL) ERROR();
          t_vecinos(s0,*t_vec);
          t_aux=t_vec->principio();
          t0=t_aux->t;
          pos0=t0->sommet(s0); 
          pos0=sig[pos0];
          s1=t0->s[pos0];
          if (s1->front==FALSE) {cerr<<"Error in cal_metrica."<<endl; exit(-1);}
          c1=s1->c;
          t_aux=t_vec->fin(); 
          t0=t_aux->t;
          pos0=t0->sommet(s0); 
          pos0=sig[sig[pos0]];
          s1=t0->s[pos0];
          if (s1->front==FALSE) {cerr<<"Error in cal_metrica."<<endl; exit(-1);}
          c2=s1->c;
          c0=s0->c;
          tang=(c1-c2)/2.0; 
          lg=tang.norme();
          tang=tang/lg;
          if (t_vec) {delete t_vec; t_vec=NULL;}
          tan1.set(tang.x,tang.y);           
          normal.set(-tang.y,tang.x);
          lg=MIN(hwall*100.0,lmax);
          m1=matriz((escala*escala)/(hwall*hwall),normal,(escala*escala)/(lg*lg),tan1);
          
          m0=intersec(m0,m1);
          
          mtr[i].set(m0.x.x,m0.x.y,m0.y.y);         
        }
      }
    }
    
    // Average boundary corrections on wall nodes.
    for (i=0; i<nbs; i++) {
      s0=malla->saca_sommet(i);
      if (s0->front==TRUE && s0->ref==refwall) {
        m0.set(mtr[i].coef[0],mtr[i].coef[1],\
               mtr[i].coef[1],mtr[i].coef[2]);
        t_vec=new Triangulo_T0_dlist;
        if (t_vec==NULL) ERROR();
        t_vecinos(s0,*t_vec);
        t_aux=t_vec->principio();
        t0=t_aux->t;
        pos0=t0->sommet(s0); 
        pos0=sig[pos0];
        s1=t0->s[pos0];
        if (s1->front==FALSE) {
          cerr<<"Error. Not a boundary vertex."<<endl;
          cerr<<"Error in subroutine cal_metrica.C"<<endl;
          exit(-1);
        }
        ps=s1-s_ini;
        m1.set(mtr[ps].coef[0],mtr[ps].coef[1],\
               mtr[ps].coef[1],mtr[ps].coef[2]);
        c1=s1->c;
        t_aux=t_vec->fin(); 
        t0=t_aux->t;
        pos0=t0->sommet(s0); 
        pos0=sig[sig[pos0]];
        s1=t0->s[pos0];
        if (s1->front==FALSE) {
          cerr<<"Error. Not a boundary vertex."<<endl;
          cerr<<"Error in subroutine cal_metrica.C"<<endl;
          exit(-1);
        }
        ps=s1-s_ini;
        m2.set(mtr[ps].coef[0],mtr[ps].coef[1],\
               mtr[ps].coef[1],mtr[ps].coef[2]);
        c2=s1->c;
        c0=s0->c;
        lg1=(c1-c0).norme();
        lg2=(c2-c0).norme();
        mm[i]=(m0.inv()*epsm+((m1.inv()*lg1+m2.inv()*lg2)/(lg1+lg2))*(1-epsm)).inv();
      }
    }  
    for (i=0; i<nbs; i++) {
      s0=malla->saca_sommet(i);
      if (s0->front==TRUE && s0->ref==refwall) {
        mtr[i].set(mm[i].x.x,mm[i].x.y,mm[i].y.y);
      }
    }
    // wall metric propagation via intersetion.
    for (i=0; i<nba; i++) {
      a0=malla->saca_arete(i);
      if (a0->front==FALSE && (a0->s[0]->ref==refwall || a0->s[1]->ref==refwall)) {
        if (a0->s[1]->front==FALSE) {
          pos0=a0->s[0]-s_ini;
          pos1=a0->s[1]-s_ini;
          m0.set(mtr[pos0].coef[0],mtr[pos0].coef[1],
                 mtr[pos0].coef[1],mtr[pos0].coef[2]);
          m1.set(mtr[pos1].coef[0],mtr[pos1].coef[1],
                 mtr[pos1].coef[1],mtr[pos1].coef[2]);
          m1=intersec(m0,m1);
          mtr[pos1].set(m1.x.x,m1.x.y,m1.y.y);
        }
        else {
          if (a0->s[0]->front==FALSE) {
            pos0=a0->s[0]-s_ini;
            pos1=a0->s[1]-s_ini;
            m0.set(mtr[pos0].coef[0],mtr[pos0].coef[1],
                   mtr[pos0].coef[1],mtr[pos0].coef[2]);
            m1.set(mtr[pos1].coef[0],mtr[pos1].coef[1],
                   mtr[pos1].coef[1],mtr[pos1].coef[2]);
            m1=intersec(m0,m1);
            mtr[pos0].set(m1.x.x,m1.x.y,m1.y.y);
          }
        }
      }
    }
    // Metric verification.
    for (i=0; i<nbs; i++) {
      if (mtr[i].determinante()<1e-20) {
        cerr<<"Error. Non positive definite metric."<<endl;
        cerr<<"det:"<<mtr[i].determinante()<<endl;
        cerr<<"Metrica n.:"<<i<<endl<<mtr[i]<<endl;
        cerr<<"Vertex:"<<*(malla->saca_sommet(i))<<endl;
        cerr<<"Error in subroutine cal_metrica.C"<<endl;
        exit(1);
      }
    }
    snbt=0.0;
    snbt0=0.0;
    snbt1=0.0;
    for (i=0; i<nbt; i++) {
      t0=malla->saca_triangle(i);
      mtr0.set(0,0,0);
      d0=0.0;
      dmax=0.0;
      dmin=1e+30;
      for (j=0; j<3; j++) {
        pos0=t0->s[j]-s_ini;
        mtr0=mtr0+mtr[pos0].inv(err)/3.0;
        pos1=t0->s[sig[j]]-s_ini;
        d00=distance(t0->s[j]->c,mtr[pos0],t0->s[sig[j]]->c,mtr[pos1])/escala;
        dmax=MAX(dmax,d00);
        dmin=MIN(dmin,d00);
        d0+=d00/3.0;
      }
      mtr0=mtr0.inv(err);
      snbt+=sqrt(mtr0.determinante())/(sqrt(t0->metrica().determinante())*escala*escala);
      rest=d0-int(d0/1.4);
      if (rest>=0.75 && rest<1.3) rest=1;
      d0=int(d0/1.4)+rest;
      snbt0+=d0*d0;
      rest=dmin-int(dmin/1.4);
      if (rest>=0.75 && rest<1.3) rest=1;
      dmin=int(dmin/1.4)+rest;
      rest=dmax-int(dmax/1.4);
      if (rest>=0.75 && rest<1.3) rest=1;
      dmax=int(dmax/1.4)+rest;
      snbt1+=dmin*dmax;
    }
    snbt=snbt+snbt/10.0;
#ifdef DEBUG
    cout<<"Approximated number of triangles to be generated-1:"<<int(snbt)<<endl;
    cout<<"Approximated number of triangles to be generated-2:"<<int(snbt0)<<endl;
    cout<<"Approximated number of triangles to be generated-3:"<<int(snbt1)<<endl;
    cout<<"--------------------------------------------------------------"<<endl;
#endif /* DEBUG */
    violation=0;
    if (int(snbt)>nbt_max) violation++;
    if (int(snbt0)>nbt_max) violation++;
    if (int(snbt1)>nbt_max) violation++;
    if (nbt_max!=0)
      errf=errf*sqrt(Scalar(MAX(snbt,MAX(snbt0,snbt1)))/Scalar(nbt_max));
    n_cont++;
    if (nbt_max==0) n_cont=it_max;
  }while (violation>1 && n_cont<it_max );
  for (i=0; i<nbs; i++) {
    mtr[i]=mtr[i]/(escala*escala);
  }
  if (mm) delete[] mm;
  if (msol) delete[] msol;
  if (hes) delete [] hes;
} 
