/*
 * 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 <r2.hxx>
#include <metric.hxx>

void R2::write(ostream& f) {
  f.setf(ios::scientific,ios::floatfield);
  f << '[' <<setw(13)<< x << ',' <<setw(13)<< y << ']' <<flush ;
  f.setf(0,ios::floatfield);}
void R2xR2::write(ostream& f)
    { f << x << endl << y  <<flush ;}
void writep(R2xR2* m,ostream &f) {
  R2 vec1[3],vec2[3];
  Scalar vp1[3],vp2[3],max=0;
  int i;

  f.setf(ios::scientific,ios::floatfield);
  for (i=0; i<3; i++) {
    m[i].vecpro(vp1[i],vec1[i],vp2[i],vec2[i]);
  }
  for (i=0; i<3; i++) {
    max=MAX(max,MAX(1.0/sqrt(fabs(vp1[i])),1.0/sqrt(fabs(vp2[i]))));
  }
  f<<"set xrange ["<<-max<<":"<<max<<"]"<<endl;
  f<<"set yrange ["<<-max<<":"<<max<<"]"<<endl;
  f<<"set noautoscale"<<endl;
  f<<"plot ";
  for (i=0; i<3; i++) {
      f<<"("<<setw(13)<<1/sqrt(vp1[i])*vec1[i].x<<")*cos(t)+";
      f<<"("<<setw(13)<<1/sqrt(vp2[i])*vec1[i].y<<")*sin(t) , ";
      f<<"("<<setw(13)<<1/sqrt(vp1[i])*vec2[i].x<<")*cos(t)+";
      f<<"("<<setw(13)<<1/sqrt(vp2[i])*vec2[i].y<<")*sin(t)";
      if (i<2) f<<", ";
  }
  f<<endl;
}
ostream& operator<<(ostream& f, R2& c) {c.write(f); return f;}
ostream& operator<<(ostream& f, R2xR2& c) {c.write(f); return f;}

istream& operator>>(istream& s, R2& r)
/*  input format for a R2; "f" indicates a Scalar:
      f
      f f
*/
{
  Scalar a=0, b=0;
  s>>a>>b;
    if (s) r=R2(a,b);
    return s;
}
R2xR2 R2xR2::operator*(R2xR2 m0) {
   R2 v1,v2;
   R2 b1,b2;
   b1.set(m0.x.x,m0.y.x);
   b2.set(m0.x.y,m0.y.y);
   v1=(*this)*b1;
   v2=(*this)*b2;
   return R2xR2(v1.x,v2.x,v1.y,v2.y);
}
/*
                SUBRRUTINA VECPRO

    Calculo de los vectores propios de una matriz 2x2.
    
    Entrada:
    -------       Matriz apuntada por this.
   
    Salida:
    ------      val1:  1. Valor propio.
                vec1:  1. Vector propio.
                val2:  2. Valor propio.
                vec2:  2. Vector propio.
*/
void R2xR2::vecpro(Scalar& val1,R2& vec1,Scalar& val2, R2& vec2) {
   R2xR2 m0;
   R2 avec;
   Scalar disc;
   Scalar norm1,norm2,max1,max2;

   disc=(x.x-y.y)*(x.x-y.y)+4*x.y*y.x;
   if (fabs(disc)<1e-16) disc=0;
   if (disc<0) {
      cerr<<"Atencion. Valores propios no reales:"<<disc<<endl;
      cerr<<"Error: R2xR2::vecpro."<<endl;
      cerr<<*this<<endl;
   }
   if (disc<=0) {
     val1=x.x;
     vec1.set(1,0);
     val2=y.y;
     vec2.set(0,1);
   }
   else {
     val1=(x.x+y.y - sqrt(disc))/2.0;
     val2=(x.x+y.y + sqrt(disc))/2.0;
     
     vec1.set(x.y,val1-x.x);
     norm1=vec1.norma();
     avec.set(val1-y.y,y.x);
     norm2=avec.norma();
     max1=MAX(norm1,norm2);
     if (max1<1e-16) {
       cerr<<"Atencion. Autovector 0:"<<vec1;
       cerr<<"Error: R2xR2::vecpro."<<endl;
       norm1=1e-16;	  
     }
     if (norm2>norm1) {vec1=avec;norm1=norm2;}
     vec1=vec1/norm1;
     vec2.set(x.y,val2-x.x);
     norm1=vec2.norma();
     avec.set(val2-y.y,y.x);
     norm2=avec.norma();
     max2=MAX(norm1,norm2);
     if (max2<1e-16){
       cerr<<"Atencion. Autovector 0:"<<vec1;
       cerr<<"Error: R2xR2::vecpro."<<endl;
       norm1=1e-16;  
     }
     if (norm2>norm1) {vec2=avec; norm1=norm2;}
     vec2=vec2/norm1;
     m0.set(vec1,vec2);
     if (fabs(m0.det())<1e-25) {
       if (max1>max2) {
	 vec2.set(-vec1.y,vec1.x);
       }
       else {
	 vec1.set(-vec2.y,vec2.x);
       }
     }
   }   
 }
void R2xR2::vecpro1(Scalar& val1,R2& vec1,Scalar& val2, R2& vec2) {
   Scalar disc;
   Scalar tmp,tmp1,tmp2;
   disc=(x.x-y.y)*(x.x-y.y)+4*x.y*y.x;
   if (fabs(disc)<1e-16) disc=0;
   if (disc<0) {
      cerr<<"Atencion. Valores propios no reales:"<<disc<<endl;
      cerr<<"Error: R2xR2::vecpro1."<<endl;
      cerr<<*this<<endl;
      val1=0;
      vec1.set(0,0);
      val2=0;
      vec2.set(0,0);
   }
   else {
     if (fabs(x.y)<1e-12 && fabs(y.x)<1e-12) { 
       val1=x.x;
       vec1.set(1,0);
       val2=y.y;
       vec2.set(0,1);
     }
     else {
       if (fabs(x.x-y.y)<1e-12 && (fabs(x.y)<1e-12 || fabs(y.x)<1e-12)) {
	 cerr<<"Atencion. NO se puede diagonalizar"<<endl;
	 cerr<<*this<<endl;
	 val1=x.x;
	 val2=y.y;
	 if (fabs(x.y)<1e-12) {
	   vec1.set(0,1);
	   vec2.set(0,1);
	 }
	 else {
	   vec1.set(1,0);
	   vec2.set(1,0);
	 }
       }
       else {
	 if (fabs(x.y)>1e-12) {
	   if (fabs(y.x)<1e-12) {
	     val1=x.x;
	     val2=y.y;
	     vec1.set(1,0);
	     tmp=sqrt(x.y*x.y+(x.x-y.y)*(x.x-y.y));
	     vec2.set(x.y/tmp,(y.y-x.x)/tmp);
	   }
	   else {		       
	     val1=(x.x+y.y - sqrt(disc))/2.0;
	     val2=(x.x+y.y + sqrt(disc))/2.0;

	     tmp1=x.y*x.y+(val1-x.x)*(val1-x.x);	     
	     tmp2=y.x*y.x+(val1-y.y)*(val1-y.y);	     
	     if (tmp1>tmp2) {
	       tmp=sqrt(tmp1);
	       vec1.set(x.y/tmp,(val1-x.x)/tmp);
	     }
	     else {
	       tmp=sqrt(tmp2);
	       vec1.set((val1-y.y)/tmp,y.x/tmp);
	     }
	     tmp1=y.x*y.x+(val2-y.y)*(val2-y.y);
	     tmp2=x.y*x.y+(val2-x.x)*(val2-x.x);
	     if (tmp1>tmp2) {
	       tmp=sqrt(tmp1);
	       vec2.set((val2-y.y)/tmp,y.x/tmp);
	     }
	     else {
	       tmp=sqrt(tmp2);
	       vec2.set(x.y/tmp,(val2-x.x)/tmp);
	     }
	   }
	 }
	 else {
	   val1=x.x;
	   val2=y.y;
	   tmp=sqrt((x.x-y.y)*(x.x-y.y)+y.x*y.x);
	   vec1.set((x.x-y.y)/tmp,y.x/tmp);
	   vec2.set(0,1);
	 }
       }
     }
   }
}	   
     

/*
                      SUBRRUTINA TRUNCADA

      Trunca los autovalores de una matriz simetrica 2x2.
    Los autovalores seran < 1/lmax^2 y > 1/lmin^2.

      Entrada:
      -------    Matriz apuntada por this.
                 lmax: longitud maxima.
                 lmin: longitud minima.
                 
      Salida:
      ------     m0: Matriz con los mismo autovectores que this pero
                     con los autovalores ya truncados.
*/

R2xR2 R2xR2::truncada(Scalar lmax, Scalar lmin) {
      R2xR2 m0=(*this);
      R2xR2 m1,m2;
      R2 vec1,vec2;
      Scalar val1,val2;
      Scalar vpmin=1.0/(lmax*lmax);
      Scalar vpmax=1.0/(lmin*lmin);

      this->vecpro(val1,vec1,val2,vec2);

      if (val1<vpmin || val2< vpmin || val1>vpmax || val2>vpmax) {
         // DESCOMPOSICION EN UNA MATRIZ m0=(m2^-1)*m1*m2
         val1=MAX(val1,vpmin);
         val2=MAX(val2,vpmin);
         m1.set(MIN(val1,vpmax),0,0,MIN(val2,vpmax));
         m2.set(vec1,vec2);
         m0=(m2.inv())*m1;
         m0=m0*m2;
      }
      return m0;
    }
/*
                      SUBRRUTINA METRIZ

    Transforma una matriz simetrica 2x2 en una matriz definida positiva.

    Entrada:
    -------   Matriz apuntada por this.
              aniso: (0-matriz isotropa - 1=anisotropa)

    Salida:
    ------    m0: matriz 2x2 con los mismos autovectores que la matriz 
                  apuntada por this pero con autovalores abs(val1),abs(val2).
                  val1 y val2 autovalores de la matriz apuntada por this.
*/

R2xR2  R2xR2::metriz(Boolean aniso) {
   R2xR2 m0;
   R2 vec1,vec2;
   Scalar val1,val2;
   Scalar aux;
   this->vecpro(val1,vec1,val2,vec2);
   if (MAX(fabs(val1),fabs(val2))<1e-16) {
     val2=val1=1e-12;
     m0.set(val1,0,0,val2);
   }
   else {
     if (aniso==FALSE) {
       aux=MAX(fabs(val1),fabs(val2));
       m0.set(aux,0,0,aux);
     }
     else {
       if (val1>0 && val2>0) {
	 m0=(*this);
       }
       else {
	 if (val2<0 && val1<0) {
	   m0=(*this);
	   m0=m0*(-1);
	 }
	 else {
	   // DESCOMPOSICION DE LA MATRIZ m0=(m2^-1)*m1*m2
	   m0=matriz(fabs(val1),vec1,fabs(val2),vec2);
	 }
       }
     }   
   }
   return m0;
 }

R2xR2 matriz(Scalar l1, R2 v1, Scalar l2, R2 v2) {
   R2xR2 m0,m1,m2;
   m1.set(l1,0,0,l2);
   m2.set(v1,v2);
   m0=(m2.inv())*m1;
   m0=m0*m2;
   return m0;
 }



R2xR2 inter (R2xR2 m0, R2xR2 m1) {
   R2xR2 inters,ma0,ma1,mmin0,mmax0;
   R2 vv1,vv2,wv1,wv2;
   Scalar v1,v2,w1,w2; 
   Scalar amin,amax;
   Scalar a0,a1,aa0,aa1;

   m0.vecpro(v1,vv1,v2,vv2);
   m1.vecpro(w1,wv1,w2,wv2);
   a0=1.0/(v1*v2);
   a1=1.0/(w1*w2);
   if (a0<a1) {
       mmin0=m0;
       amin=a0;
   }
   else {
       mmin0=m1;
       amin=a1;
   }   
   v1=MAX(v1,vv1*(m1*vv1));
   v2=MAX(v2,vv2*(m1*vv2));
   w1=MAX(w1,wv1*(m0*wv1));
   w2=MAX(w2,wv2*(m0*wv2));

   ma0=matriz(v1,vv1,v2,vv2);
   ma1=matriz(w1,wv1,w2,wv2);
   aa0=1.0/(v1*v2);
   aa1=1.0/(w1*w2);
   if (aa0>aa1) {
       mmax0=ma0;
       amax=aa0;
   }
   else {
       mmax0=ma1;
       amax=aa1;
   }

   if (fabs(amax-amin)<1e-12) {
      inters=mmax0;
   }
   else {
      inters=(ma1+ma0)/2.0;
   }
   return inters; 
}

R2xR2 intersec(R2xR2 m0, R2xR2 m1) {
   R2xR2 inters,mt,mtt,mt0,mt1;
   R2 vv1,vv2;
   Scalar v1,v2; 
   Scalar lambda1,lambda2;
   if (fabs(m0.det())<1e-20) {
     inters=inter(m0,m1);
   }
   else {
     mt=m0.inv()*m1;
     //   cout<<"m0^-1*m1"<<endl<<mt<<endl;
     mt.vecpro1(v1,vv1,v2,vv2);
     if (fabs(v1)<1e-16 && fabs(v2)<1e-16) {
       cerr<<"autovalores 0."<<endl;
       cerr<<"m0:"<<m0<<"   det:"<<m0.det()<<endl;
       cerr<<"m1:"<<m1<<"   det:"<<m1.det()<<endl;
     }
     mt.set(vv1.x,vv2.x,vv1.y,vv2.y);
     mtt=mt.transpose();
     mt0=(mtt*m0)*mt;
     mt1=(mtt*m1)*mt;
     //   cout<<"mt0:"<<endl<<mt0<<endl;
     //   cout<<"mt1:"<<endl<<mt1<<endl;
     lambda1=MAX(mt0.x.x,mt1.x.x);
     lambda2=MAX(mt0.y.y,mt1.y.y);
     mt0.set(lambda1,0,0,lambda2);
     if (fabs(mt.det())<1e-20) {
       inters=inter(m0,m1);
     }
     else {
       inters=((mt.inv()).transpose()*mt0)*mt.inv();
     }
   }     
   return inters; 
}
R2xR2  R2xR2::transwall(Scalar hwall,R2 tan1) {
   R2xR2 m0;
   R2 vec1,vec2;
   R2 normal;
   Scalar val1,val2;
   Scalar aux;
   Scalar ang1,ang2;

   normal.set(-tan1.y,tan1.x);
   this->vecpro(val1,vec1,val2,vec2);
//  METRICA ANISOTROPA.
   aux=tan1*vec1;
   if (fabs(aux)>1.0) aux=SGN(aux);
   ang1=acos(aux);
   aux=tan1*vec2;
   if (fabs(aux)>1.0) aux=SGN(aux);   
   ang2=acos(aux);
   if (ang1>ang2) {
     val1=val2;
   }      
   m0=matriz(val1,tan1,hwall,normal); 

   return m0;
 }   

R2  maximo(R2 rr1, R2 rr2) {
  R2 cc(MAX(rr1.x,rr2.x),MAX(rr1.y,rr2.y));
  return cc;
}
R2   minimo(R2 rr1, R2 rr2) {
  R2 cc(MIN(rr1.x,rr2.x),MIN(rr1.y,rr2.y));
  return cc;
}
Scalar maximo(R2 rr) {return MAX(rr.x,rr.y);} 
Scalar minimo(R2 rr) {return MIN(rr.x,rr.y);}
Scalar maximo (R2 rr, int& pos) {
  Scalar s;
  s=MAX(rr.x,rr.y);
  if (s==rr.x) 
    pos=0;
  else 
    pos=1; 
  return s;          
}
Scalar minimo (R2 rr, int& pos) {
  Scalar s;
  s=MIN(rr.x,rr.y); 
  if (s==rr.x)
    pos=0;
  else 
    pos=1;
  return s;        
}
Scalar distance(R2 rr1,R2 rr2) {
  R2 cc=rr1-rr2;
  return sqrt(cc*cc);} 
 

Scalar distance(R2 r1,Metrica m1,R2 r2,Metrica m2) {
  Scalar l0,l1,dist;
  R2 aux;
  aux=r1-r2;
  l0=norme(aux,m1);
  l1=norme(aux,m2);

  if (fabs(l0-l1)<1e-12)
    dist=l1;
  else
    dist=(l0*l1)/(l0-l1)*log(l0/l1);
  return dist;
}
Scalar norme (R2 r1,Metrica m1) {
  Scalar norma;
  norma=m1.factor*sqrt(m1.coef[0]*r1.x*r1.x+2*m1.coef[1]*r1.x*r1.y+ m1.coef[2]*r1.y*r1.y);
  return norma;
}

Scalar distance0(R2 r1,Metrica m1,R2 r2,Metrica m2) {
  Scalar l0,l1,dist;
  R2 aux;
  aux=r1-r2;
  l0=norme0(aux,m1);
  l1=norme0(aux,m2);
  if (fabs(l0-l1)<1e-12)
    dist=l1;
  else
    dist=(l0*l1)/(l0-l1)*log(l0/l1);
  return dist;
}
Scalar norme0 (R2 r1,Metrica m1) {
  Scalar norma;
  norma=sqrt(m1.coef[0]*r1.x*r1.x+2*m1.coef[1]*r1.x*r1.y+m1.coef[2]*r1.y*r1.y);
  return norma;
}

Scalar angle(R2 rr1,R2 rr2) {
  Scalar ang=0;
  Scalar es1,n1,n2;
  n1=rr1.norme();
  n2=rr2.norme();
  if (fabs(n1)>1e-16 && fabs(n2)>1e-16) 
    es1=(rr1*rr2)/(n1*n2);
  else 
    es1=0;
  if (fabs(es1)>1) {
    if (es1>0) 
      es1=1.0;
    else 
      es1=-1.0;   
  }
  ang=180.0*acos(es1)/PI;
  return ang;
        
}    

/*     
                   COOR_BAR subroutine

            Computation of the barycentric coordinates of point pt, 
            with respect to the triangle with vertices s0,s1,s2.
            Sortie: (coor.x,coor.y) (attention !!! third component 
                      equals to 1-coor.x-coor.y

*/
R2 coor_b(R2 p1,R2 p2,R2 p3,R2 pt) {
  Scalar d1;
  R2xR2 r1,eq1,eq2;
  R2 coor;
  r1.set(p1.x-p3.x, p2.x-p3.x,p1.y-p3.y,p2.y-p3.y);
  d1=r1.det();
  if (fabs(d1)<EPS) {
    cerr<<"Sorry. Degenerated triangle."<<endl;
    cerr<<"Error in subroutine R2 coor_b"<<endl;
    coor.set(0,0);
  }
  else {
    eq1.set(pt.x-p3.x,p2.x-p3.x,pt.y-p3.y,p2.y-p3.y);
    eq2.set(p1.x-p3.x,pt.x-p3.x,p1.y-p3.y,pt.y-p3.y);
    coor.set(eq1.det()/d1,eq2.det()/d1);
  }
  return coor;
}



/*

                   PROYECT subroutine

     Projection of point pt over the edge s0-s1.
   

     Input:       s0, s1 vertices.
     -----        pt= point to project.
                  
   
     Output:      dist= distance
     ------       proyec= projected point. 
                  temp=postion of proyec related to vertex s0.
*/
Scalar proyec(R2 s0,R2 s1,R2 pt, R2& pr_pt,\
              Scalar& temp) {
  R2 v1,vector1,vector2;
  R2 proyec_pt;
  R2 aux1;
  int pos;
  Scalar dist;
  Scalar aux;
  Scalar d1,d2;
  Scalar escl1,escl2;

  v1=s1-s0;
  vector1=pt-s0;
  aux=v1*v1;
  // Projection over the straight line
  if (fabs(aux)<1e-20) {
    proyec_pt=s0;
    dist=distance(pt,proyec_pt);
    temp=0.0;
  }
  else {
    temp=(vector1*v1)/aux;
    proyec_pt=s0+v1*temp;
    vector1=proyec_pt-s0;
    vector2=proyec_pt-s1;
    escl1=vector1*v1;
    escl2=vector2*v1;
    if ((escl1*escl2)>=0) { //both are the  same sign. Projection out the edge, so the projection is one of the vertices of the egde.
      
      d1=distance(pt,s0);
      d2=distance(pt,s1);   
      aux1.set(d1,d2);
      dist=minimo(aux1,pos);
      if (pos==0) {
	temp=0.;
	pr_pt=s0;
      }
      else {
	if (pos==1) {
	  temp=1.;
	  pr_pt=s1;
	}
      }
    }
    else {
      // el punto proyectado cae en medio
      dist=distance(pt,proyec_pt);
      pr_pt=proyec_pt;
      if (temp<0 || temp>1) {
        cerr<<"temp not in [0,1]"<<endl;
        cerr<<"Error in proyec (R2.C)"<<endl;
      }
    }
  }
  return dist;
}
