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

int mshopt (Triangulo_T1* tr, const int& a) {
  int err=0,cont=0,err0;
  Pila_trar* pila;
  Pila_trar_link* aux,*cab;
  Triangulo_T1 t0;
  Triangulo_T1* t1,*t2;
  Triangulo_T1* t1v2,*t1v3,*t2v2,*t2v3;
  Arista_T1* a1,*a2,*a3,*b2,*b3;
  Vertice_T1* so1,*so2,*so3,*so4;
  Metrica mm1,mm2,m1i,m2i,m3i,m4i;
  R2 c1,c2,c3,c4;
  Scalar area1,area2;
  Scalar crit1,crit2,critp,crit;
  int ip1[3];
  int a11,a12,a13,a21,a22,a23,aa;
  int reft;
  ip1[0]=1;ip1[1]=2;ip1[2]=0;
  
  pila =new Pila_trar;
  aux=new Pila_trar_link;
  if (pila==NIL || aux==NIL) ERROR();
  aux->set(tr,a);
  pila->in(aux);
  cab=pila->cabeza();
  while (cab!=NIL && cont<128) {
//   Saco un elemento de la pila y lo destruyo.
    cont++;
    aux=pila->out();
    t1=aux->t;    
    a11=aux->a;
    if (a11<0 || a11>2){
      cout<<"a11:"<<a11<<endl;
      cerr<<"Error in mshopt."<<endl;
      exit(1);
    }
    delete aux;
    a1=t1->a[a11];
    t2=t1->t[a11]; // Saco el triangulo vecino de t1 por la
    // la arista numero a.
    
    if (t2) {
//    Busco cual es la arista a11 en t2 y lo meto en a21.
//        cout<<"mshopt 1. arista"<<endl;
      a21=t2->arista(t1->a[a11]);
      if (a21<0 || a21>2){
        cerr<<"Error in mshopt."<<endl;
        cout<<"a21:"<<a21<<endl;
        cout<<"a11:"<<a11<<endl;
        cout<<"cont:"<<cont<<endl;
        cout<<"t1:"<<endl<<*t1<<endl;
        cout<<*(t1->s[0])<<endl<<*(t1->s[1])<<endl<<*(t1->s[2])<<endl;
        cout<<*(t1->a[0])<<endl<<*(t1->a[1])<<endl<<*(t1->a[2])<<endl;
        cout<<*(t1->t[0])<<endl<<*(t1->t[1])<<endl<<*(t1->t[2])<<endl;
        cout<<"t2:"<<endl<<*t2<<endl;
        cout<<*(t2->s[0])<<endl<<*(t2->s[1])<<endl<<*(t2->s[2])<<endl;
        cout<<*(t2->a[0])<<endl<<*(t2->a[1])<<endl<<*(t2->a[2])<<endl;
        cout<<*(t2->t[0])<<endl<<*(t2->t[1])<<endl<<*(t2->t[2])<<endl;
        exit (1);
      }
      if (a1->interseccion()==FALSE) {
        a12=ip1[a11];
        a13=ip1[a12];
        a22=ip1[a21];
        a23=ip1[a22];
        so1=t1->s[a13];
        so2=t1->s[a11];
        so3=t1->s[a12];
        so4=t2->s[a23];
/*   Criterio de optim. del cuadrilatero
     
                    x so1                          x so1
                   / \                            /|\
                  /   \                          / | \
                 /     \                        /  |  \
         t1v3 a3/       \  a2 t1v2             /   |   \
               /         \             t1v3 a3/    |    \
              /    t1     \                  /     |     \
             /             \                /      | a1   \ a2 t1v2
            /               \              /       |       \
       so2 x-----------------x so3    so2 x   t1   |   t2   x so3            
            \      a1       /              \       |       /
             \             /                \      |      /
              \     t2    /                  \     |     /
        t2v2 b2\         / b3 t2v3     t2v2 b2\    |    /  b3 t2v3
                \       /                      \   |   /
                 \     /                        \  |  /
                  \   /                          \ | /
                   \ /                            \|/
                    x                              x
                    so4                            so4
*/
        c1=so1->c;
        c2=so2->c;
        c3=so3->c;
        c4=so4->c;
        t0.set(so2,so4,so1,NIL,NIL,NIL,NIL,NIL,NIL,0);
        area1=t0.area2D();
        t0.set(so3,so1,so4,NIL,NIL,NIL,NIL,NIL,NIL,0);
        area2=t0.area2D();
        
        if (area1>0 && area2>0) { 
          m1i=so1->mtr.inv(err0);
          m2i=so2->mtr.inv(err0);
          m3i=so3->mtr.inv(err0);
          m4i=so4->mtr.inv(err0);
	    
          //mm1=(so1->mtr+so2->mtr+so3->mtr)/3.0;
          //mm2=(so2->mtr+so3->mtr+so4->mtr)/3.0;
          mm1=((m1i+m2i+m3i)/3.0).inv(err0);
          mm2=((m2i+m3i+m4i)/3.0).inv(err0);
          if (t1->krit==-999.0) t1->krit=criter(c1,c2,c3,mm1);
          if (t2->krit==-999.0) t2->krit=criter(c2,c3,c4,mm2);
          crit=MIN(t1->krit,t2->krit);
/*
           if (crit<=0) {
             cout<<"Error. criterio negativo o cero"<<endl;
             cout<<"t1:"<<t1->krit<<endl<<"t2:"<<t2->krit<<endl;
             cout<<*t1<<endl<<normal1<<endl<<*t2<<endl<<normal2<<endl;
             //exit (1);
           }
*/
          //mm1=(so1->mtr+so4->mtr+so3->mtr)/3.0;
          //mm2=(so1->mtr+so4->mtr+so2->mtr)/3.0;
          mm1=((m1i+m4i+m3i)/3.0).inv(err0);
          mm2=((m1i+m4i+m2i)/3.0).inv(err0);
          crit1=criter(c1,c4,c3,mm1);
          crit2=criter(c1,c4,c2,mm2);
          critp=MIN(crit1,crit2);
          if (critp<0) {
            cout<<"Error. Criterio negativo o cero"<<endl; 
            cout<<"crit1:"<<crit1<<endl<<"crit2:"<<crit2<<endl;
            exit (1);
          }
          if (crit<critp) {
            t1->krit=crit1;
            t2->krit=crit2;
            a1=t1->a[a11];
            a2=t1->a[a12];
            a3=t1->a[a13];
            b2=t2->a[a22];
            b3=t2->a[a23];
            t1v2=t1->t[a12];
            t1v3=t1->t[a13];
            t2v2=t2->t[a22];
            t2v3=t2->t[a23];
            so2->t=t1;
            so3->t=t2;
            a1->s[0]=so1;
            a1->s[1]=so4;
            a2->tr=t2;
            b2->tr=t1;
            reft=t1->ref;
            t1->set(so2,so4,so1,t2v2,t2,t1v3,b2,a1,a3,reft);
            reft=t2->ref;
            t2->set(so3,so1,so4,t1v2,t1,t2v3,a2,a1,b3,reft);
            if (t1v2) {
              aa=t1v2->arista(a2);
              t1v2->t[aa]=t2;
            }
            if (t2v2) {
              aa=t2v2->arista(b2);
              t2v2->t[aa]=t1;
            }      
            aux=new Pila_trar_link;
            if (aux==NIL) ERROR();
            aux->set(t1,0);
            pila->in(aux);           
            aux=new Pila_trar_link;
            if (aux==NIL) ERROR();
            aux->set(t2,0);
            pila->in(aux);     
            aux=new Pila_trar_link;
            if (aux==NIL) ERROR();
            aux->set(t1,2);
            pila->in(aux);
            aux=new Pila_trar_link;
            if (aux==NIL) ERROR();
            aux->set(t2,2);
            pila->in(aux);
          }     
        }
      }
    }
    cab=pila->cabeza();
  }               
  delete pila;
  return err;
}
