/*
    C++ Mesh Generation Library
    Copyright (c) 2000echet <eric at bechet dot ca>

    This file is part of the C++ Mesh Generation Library.

    See the NOTICE & LICENSE files for conditions.
*/
#include <cmath>
#include <iostream>
#include <FM3D.h>
#include <linear_algebra.h>

using namespace std;

#define LARGE_VAL (1e99)
#define SMALL_VAL (0)
#define PI (3.141592653589793238462643)
//#define DBUG



// sign function
int sign ( double x )
{
  return ( x == 0 ? 0 : ( x < 0 ? -1 : 1 ) );
}

// returns a / b (0 / 0 = 0, overflow = LARGE_VAL with correct sign)
double fdiv_ ( double a, double b )
{
  if ( b == 0 )
  {
    if ( a == 0 )
    {
      return 0;
    }
    else
    {
#ifdef DBUG
      cout << "overflow" << endl;
#endif
      return LARGE_VAL * sign ( a );
    }
  }
  else
  {
    if ( a == 0 )
    {
      return 0;
    }
    else
    {
      if ( ( a + b ) == a )
      {
#ifdef DBUG
        cout << "overflow" << endl;
#endif
        return LARGE_VAL * sign ( a ) * sign ( b );
      }
      else
      {
        return a / b;
      }
    }
  }
}


double dt1d ( double xd,double yd,double zd,double v )
{
  double fx=xd;
  double fy=yd;
  double fz=zd;
  return ( fdiv ( sqrt ( fx*fx+fy*fy+fz*fz ),v ) );
}

double dt2d ( double xi,double xb,double yb,double zb,double xd,double yd,double zd,double tb,double v )
{
  double fx=xi*xb-xd;
  double fy=xi*yb-yd;
  double fz=xi*zb-zd;
  double ft=xi*tb;
  return ( ft+fdiv_ ( sqrt ( fx*fx+fy*fy+fz*fz ),v ) );
}

double dt3d ( double xi,double eta,double xb,double yb,double zb,double xc,double yc,double zc,double xd,double yd,double zd,double tb,double tc,double v )
{
  double fx=xi*xb+eta*xc-xd;
  double fy=xi*yb+eta*yc-yd;
  double fz=xi*zb+eta*zc-zd;
  double ft=xi*tb+eta*tc;
  return ( ft+fdiv_ ( sqrt ( fx*fx+fy*fy+fz*fz ),v ) );
}


int Solve_dt1d ( double ax,double ay,double az,double dx,double dy,double dz,double at,double v,double &dt )
{
  double xd=dx-ax;
  double yd=dy-ay;
  double zd=dz-az;   // reference point a
  double td;   // ref temps point a aussi

  td=dt1d ( xd,yd,zd,v );
  dt=td+at;
  return 0;
}



int Solve_dt2d ( double ax,double ay,double az,double bx,double by,double bz,double dx,double dy,double dz,double at,double bt,double v,double &dt,double &alpha )
{
  double xb=bx-ax;
  double yb=by-ay;
  double zb=bz-az;   // reference point a
  double xd=dx-ax;
  double yd=dy-ay;
  double zd=dz-az;

  double tb=bt-at;

  double td=0.0;   // ref temps point a aussi

  double bb=xb*xb+yb*yb+zb*zb;
  double sbb=sqrt ( bb );
  double bd=xb*xd+yb*yd+zb*zd;

  double xi0=fdiv_ ( bd,bb );
  double x0= ( xi0*xb-xd );
  double y0= ( xi0*yb-yd );
  double z0= ( xi0*zb-zd );
  double rho0=sqrt ( x0*x0+y0*y0+z0*z0 );
  double calpha=fdiv_ ( v*tb,sbb ); //sign is important, overflow here is a bug

  if ( calpha>1.0 )
  {
    calpha=1.0;
  }
  else if ( calpha<-1.0 )
  {
    calpha=-1.0;
  }

//  double alpha=acos(calpha);
//  double beta=PI/2-alpha;
//  double tbeta=tan(beta);
//  double tbeta=fdiv_(calpha,sqrt(1-calpha*calpha));//overflow ok but sign is important !!

  double dxi=fdiv_ ( calpha*rho0,sbb*sqrt ( 1-calpha*calpha ) ); //overflow ok but sign is important !!
  double xi=xi0-dxi;
  int res=0;

  if ( xi> ( 1.+SMALL_VAL ) )
  {
    xi=1.0;
    res=1;
  }
  else if ( xi<-SMALL_VAL )
  {
    xi=0.0;
    res=1;
  }

  td= dt2d ( xi,xb,yb,zb,xd,yd,zd,tb,v );
  alpha=xi;
  dt=td+at;
  return ( res );
}



int Solve_dt3d ( double ax,double ay,double az,double bx,double by,double bz,double cx,double cy,double cz,double dx,double dy,double dz,double at,double bt,double ct,double v,double &dt,double &alpha,double &beta )
{
  double xb=bx-ax;
  double yb=by-ay;
  double zb=bz-az;   // reference point a
  double xc=cx-ax;
  double yc=cy-ay;
  double zc=cz-az;
  double xd=dx-ax;
  double yd=dy-ay;
  double zd=dz-az;

  double tb=bt-at;
  double tc=ct-at;
  double td;   // ref temps point a aussi

  double bc=xb*xc+yb*yc+zb*zc;
  double bb=xb*xb+yb*yb+zb*zb;
  double cc=xc*xc+yc*yc+zc*zc;
  double bd=xb*xd+yb*yd+zb*zd;
  double cd=xc*xd+yc*yd+zc*zd;
  double bpc=bb+cc;

  double xbvc= ( yb*zc-zb*yc );
  double ybvc= ( zb*xc-xb*zc );
  double zbvc= ( xb*yc-yb*xc );
  double nbvc=sqrt ( xbvc*xbvc+ybvc*ybvc+zbvc*zbvc );

  double xbvbvc=- ( yb*zbvc-zb*ybvc );
  double ybvbvc=- ( zb*xbvc-xb*zbvc );
  double zbvbvc=- ( xb*ybvc-yb*xbvc );
  double nbvbvc=sqrt ( xbvbvc*xbvbvc+ybvbvc*ybvbvc+zbvbvc*zbvbvc );


  double rho0=fabs ( ( xbvc*xd+ybvc*yd+zbvc*zd ) / ( nbvc ) );
//  cout << rho0 << endl;
  double i0=bd/sqrt ( bb );
//  cout << i0 << endl;
  double j0= ( xbvbvc*xd+ybvbvc*yd+zbvbvc*zd ) / ( nbvbvc );
//  cout << j0 << endl;
  double t=bc/ ( sqrt ( bb ) *sqrt ( cc ) );
  double tt=t/sqrt ( 1-t*t );
  double xi0=bd/bb- ( j0*tt ) /sqrt ( bb );
#ifdef DBUG
  cout << "xi=" << xi0 << " ";
#endif
  double eta0=j0*nbvbvc/ ( xbvbvc*xc+ybvbvc*yc+zbvbvc*zc );
#ifdef DBUG
  cout << "eta=" << eta0 << endl;
#endif
  double xproj=xi0*xb+eta0*xc;
  double yproj=xi0*yb+eta0*yc;
  double zproj=xi0*zb+eta0*zc;
#ifdef DBUG
  cout << xproj << " "<<  yproj << " " << zproj << endl;
#endif
//  double ps=(xproj-xd)*xb+(yproj-yd)*yb+(zproj-zd)*zb;
//  double ps2=(xproj-xd)*xc+(yproj-yd)*yc+(zproj-zd)*zc;
//  cout << ps << " " << ps2 << endl;


  Square_Matrix M ( 3 ),Minv ( 3 );
  Vector gradTxyz ( 3 ),gradTxieta ( 3 ),gradTxieta2 ( 3 );;
  M ( 0,0 ) =xb;
  M ( 0,1 ) =yb;
  M ( 0,2 ) =zb;
  M ( 1,0 ) =xc;
  M ( 1,1 ) =yc;
  M ( 1,2 ) =zc;
  M ( 2,0 ) =yb*zc-zb*yc;
  M ( 2,1 ) =zb*xc-xb*zc;
  M ( 2,2 ) =xb*yc-yb*xc;
  M.Invert ( Minv );

  double gxi=tb;
  double geta=tc;

  gradTxieta[0]=gxi;
  gradTxieta[1]=geta;
  gradTxieta[2]=0;
#ifdef DBUG
  gradTxieta.Display();
#endif
  Minv.Mult ( gradTxieta,gradTxyz );
#ifdef DBUG
  gradTxyz.Display()
#endif
  Minv.Transpose();
  Minv.Mult ( gradTxyz,gradTxieta2 );
#ifdef DBUG
  gradTxieta2.Display();
#endif
//  M.Mult(gradTxyz,gradTxieta);
//  gradTxieta.Display();
  double nxieta=gradTxyz.Norm();
#ifdef DBUG
  cout << "nxieta=" << nxieta << endl;
#endif


//  double gxigeta=bc*gxi*geta;
//  double nxieta=sqrt(gxi*gxi+geta*geta-2*(gxigeta));
//  double txi0eta0=xi0*tb+eta0*tc;
  double txieta=gradTxieta2[0]*tb+gradTxieta2[1]*tc;
#ifdef DBUG
  cout << "txieta=" << txieta << endl;
#endif
//  double



  double calpha=fdiv_ ( v*txieta,nxieta ); //sign is important, overflow here is a
#ifdef DBUG
  cout << "calpha=" << calpha << endl;
#endif

  if ( calpha>1.0 )
  {
    calpha=1.0;
  }
  else if ( calpha<-1.0 )
  {
    calpha=-1.0;
  }

  double ddd=fdiv_ ( calpha*rho0,sqrt ( 1-calpha*calpha ) ); //overflow ok but sign is important !!
#ifdef DBUG
  cout << "ddd=" << ddd << endl;
#endif
  gradTxyz.Normalize();
  gradTxyz*=-ddd;
  gradTxyz[0]+=xproj;
  gradTxyz[1]+=yproj;
  gradTxyz[2]+=zproj;
#ifdef DBUG
  gradTxyz.Display();
#endif
  Minv.Mult ( gradTxyz,gradTxieta2 );
#ifdef DBUG
  gradTxieta2.Display();
#endif

//  double dxi=fdiv_(ddd,gradTxieta2[0]);//overflow ok but sign is important !!
//  double deta=fdiv_(ddd,gradTxieta2[1]);//overflow ok but sign is important !!
//  cout << "dxi=" << dxi << " deta=" << deta<<  endl;

//  double xi=xi0+dxi;
//  double eta=eta0+deta;
  double xi=gradTxieta2[0];
  double eta=gradTxieta2[1];


#ifdef DBUG
  cout << "xi=" << xi << " eta=" << eta<<  endl;
#endif

  if ( ( xi+eta ) > ( 1.+SMALL_VAL ) )
  {
    double ttt;
    int t=Solve_dt2d ( bx,by,bz,cx,cy,cz,dx,dy,dz,bt,ct,v,dt,ttt ) +1;
    alpha=1-ttt;
    beta=ttt;
    return t;
  }

  if ( xi<-SMALL_VAL )
  {
    int t=Solve_dt2d ( ax,ay,az,cx,cy,cz,dx,dy,dz,at,ct,v,dt,beta ) +1;
    alpha=0;
    return t;
  }

  if ( eta<-SMALL_VAL )
  {
    int t=Solve_dt2d ( ax,ay,az,bx,by,bz,dx,dy,dz,at,bt,v,dt,alpha ) +1;
    beta=0;
    return t;
  }

  td= dt3d ( xi,eta,xb,yb,zb,xc,yc,zc,xd,yd,zd,tb,tc,v );
  alpha=xi;
  beta=eta;
  dt=at+td;
  return 0;
}





int val2d ( double ax,double ay,double az,double bx,double by,double bz,double dx,double dy,double dz,double at,double bt,double v,double& dt,double &alpha )
{
  return Solve_dt2d ( ax,ay,az,bx,by,bz,dx,dy,dz,at,bt,v,dt,alpha );
}



int val3d ( double ax,double ay,double az,double bx,double by,double bz,double cx,double cy,double cz,double dx,double dy,double dz,double at,double bt,double ct,double v,double& dt,double &alpha,double &beta )
{
  int res=Solve_dt3d ( ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz,at,bt,ct,v,dt,alpha,beta );
#ifdef DBUG

  if ( fabs ( dt-1.89287e-01 ) <1e-5 )
  {
//    cout << "rg";
    cout << setiosflags ( ios::scientific );
    cout << setprecision ( 15 );
    cout << endl << "a " << ax << " " << ay << " " << az;
    cout << endl << "b " << bx << " " << by << " " << bz;
    cout << endl << "c " << cx << " " << cy << " " << cz;
    cout << endl << "d " << dx << " " << dy << " " << dz;
    cout << endl << "tabcd " << at << " " << bt << " " << ct << " " <<  dt;
    cout << endl << "v= " << v << endl;
  }

#endif
  return res;
}