// C++ includes
#include "m3d.h"
#include <iostream>
/* m3d
 * 	These files may be found at:
 * 	http://software.seg.org/2017/0006
 *
 * 	Please first read and accept:
 * 	http://software.seg.org/disclaimer.txt
 */
/*
 Input: a tetrahedon and an observation site
 Output: ni  (outgoing normal vector of 4 surfaces)
         wi0 (heights from site to 4 surfaces
         Rij_minus and Rij_plus
         oi (projection point of site on 4 surfaces)
         Sij_minus and Sij_plus
         Rij0 (distance from site to edges of 4 surfaces)
         tij (tangential normal vector on edges)
         mij (outgoing normal vector of edges)
 */
M3D::M3D(Tet& tet_, Point r_):
  tet   (tet_),
  r     (r_),
  TOL   (1e-10),
  Cm    (1e-07)  
{
  // calculate variables for facets and edges
  for(int i=0; i<4; i++) {
    Point n;
    this->tet.get_facet_normal_vector(n, i);
    ni[i] = n;
    double w0; 
    for(int j=0; j<3; j++) {
      Node* a[2];
      this->tet.get_facet_edge_ends (a, i, j);
      Rij_minus[i][j] = (this->r-(*a[0])).size();
      Rij_plus[i][j]  = (this->r-(*a[1])).size();
      if(j==0) { w0 = (r-(*a[0]))*n;}
    }
    wi0[i]=w0;

    Point o = r - n*w0;
    oi[i] = o;
    for(int j=0; j<3; j++) {
      Point m, t; Node* a[2];
      this->tet.get_facet_edge_normal(m, i, j);
      this->tet.get_facet_edge_tangential(t, i, j);
      this->tet.get_facet_edge_ends (a, i, j);
      Sij_minus[i][j] = ((*a[0])-o)*t;
      Sij_plus[i][j]  = ((*a[1])-o)*t;
      mij0[i][j]= ((*a[0])-o)*m;
      Rij0[i][j] = std::sqrt(w0*w0+mij0[i][j]*mij0[i][j]);
      mij[i][j]  = m;
      tij[i][j]  = t;
    }
  }
 
}

/*
 Input: surface index i 
 Output: k0=\iint_{T_{i}} 1/R which is given in equations (24) and (25)
 */
void M3D::compute_1_R(unsigned int i, double &k0)
{
  double w0 = wi0[i];
  double temp_k=0.;
  
  for(int j=0; j<3; j++) {
    double part1=0, part2=0;
    if(std::abs(w0)>TOL) {
      double beta = std::atan((mij0[i][j]*Sij_plus[i][j])/(Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_plus[i][j]))
	- std::atan((mij0[i][j]*Sij_minus[i][j])/(Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_minus[i][j]));
      part2 = std::abs(w0)*beta;
    }
    double mj0 = mij0[i][j];
    if(std::abs(mj0)>TOL) {
      part1 = mj0*std::log((Rij_plus[i][j]+Sij_plus[i][j])/(Rij_minus[i][j]+Sij_minus[i][j]));
    }
    temp_k += part1 - part2;
  }
  
  k0 = temp_k;
  
  return;
  
}

/*
 Input: surface index i
 Output: k3=\iint_{T_{i}} 1/R^3 which is given in equations (10), (17), (18) and (19)
 */
void M3D::compute_1_R3(unsigned int i,  double &k3)
{
  double w0 = wi0[i];
  double w0_not_zero_k3=0.;
  double w0_is_zero_k3 =0.;

  // 1_R3!=0
  if(std::abs(w0)>TOL) {  
    for(int j=0; j<3; j++) {
       double beta = std::atan((mij0[i][j]*Sij_plus[i][j])/(Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_plus[i][j]))
	 - std::atan((mij0[i][j]*Sij_minus[i][j])/(Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_minus[i][j]));
        w0_not_zero_k3 += (1.0/std::abs(w0))*beta;
    }
  }

  if(std::abs(w0)<TOL) { 
    // when w0==0, site r must be outside the triangle
    // otherwise, there is a singularity in calculating 1/R3 over triangle
    for(int j=0; j<3; j++) {
      double m0 = mij0[i][j];
      if(std::abs(m0)>TOL) {
        double temp = (Sij_plus[i][j]/Rij_plus[i][j]-Sij_minus[i][j]/Rij_minus[i][j]);
        w0_is_zero_k3 += temp*(-1.0/m0);
      }
    }
  }


  if(std::abs(w0)>TOL) k3 = w0_not_zero_k3;
  else                 k3 = w0_is_zero_k3;


   return;

}
         
  
/*
 Input: surface index i
 Output: Ki=\iint_{T_{i}} \nabla 1/ R which is given in equations (5)
 where \nabla is on the observation site r
 */
void M3D::compute_grad_1_R(unsigned int i, Point &Ki)
{
  Point n   = ni[i];
  double w0 = wi0[i];
  Point temp_k_minus;
  
  for(int j=0; j<3; j++) {
    Point part1, part2;
    if(std::abs(w0)>TOL) {
      // betaij see equation  (11)
      double betaij= atan((mij0[i][j]*Sij_plus[i][j])/(Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_plus[i][j]))
	- atan((mij0[i][j]*Sij_minus[i][j])/(Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_minus[i][j]));
      part2  = n*sgn(w0)*betaij;
    }
    double Aij=0.; // see equations (20-22)
    if(std::abs(Rij0[i][j])>TOL) {
      Aij = std::log((long double)(Rij_plus[i][j]+Sij_plus[i][j])) - std::log((long double)(Rij_minus[i][j]+Sij_minus[i][j]));
    }else if(Sij_plus[i][j]>0.&&Sij_minus[i][j]>0.) {
      Aij = std::log(Sij_plus[i][j]) - std::log(Sij_minus[i][j]);
    }else if(Sij_plus[i][j]<0.&&Sij_minus[i][j]<0.) {
      Aij = std::log(Sij_minus[i][j]*-1.0)- std::log(Sij_plus[i][j]*-1.0);
    }else {
      std::cout<<"site must not on edge!\n";
      std::abort();
    }
    part1 = mij[i][j]*Aij;
    temp_k_minus = temp_k_minus + part1 + part2;
  }
  
  Ki = temp_k_minus*-1.0;
  
  return;
  
}



/*
 Input: 4 nodes in vector T
 Output: volume(size) of tetrahedron
 */
double M3D::get_tri_size(std::vector<Point>& T)
{
  assert(T.size()==3);
  Point a=T[1]+(T[0]*-1.0);
  Point b=T[2]+(T[0]*-1.0);
  Point c=a.cross(b);
  
  return c.size()*0.5;
}


/*
 Input: surface index i
 Output: k2=\iint_{T_{i}} \nabla \nabla 1/ R which is given in equations (25)
 where \nabla is on the observation site r
 */
void M3D::compute_grad_grad_1_R(unsigned int i, double k2[3][3])
{
  this->set(k2, 0.);
  double temp_k2[3][3];
  this->set(temp_k2, 0.);
  double w0 = wi0[i];
  Point  n  = ni[i];
  
  for(int j=0; j<3; j++) {
    Point grad_Aij(0,0,0);  // see equations (37) and (38)
    if(std::abs(Rij0[i][j])>TOL) {
      double factor_n_mij = (Sij_plus[i][j]/(Rij0[i][j]*Rij0[i][j]*Rij_plus[i][j]) - 
                             Sij_minus[i][j]/(Rij0[i][j]*Rij0[i][j]*Rij_minus[i][j]))*-1.0;
      double factor_tij   = (1.0/Rij_plus[i][j]-1.0/Rij_minus[i][j])*-1.0;
      grad_Aij = n*(w0*factor_n_mij) + tij[i][j]*factor_tij - mij[i][j]*(mij0[i][j]*factor_n_mij);
    }else {
      double factor_tij   = (-1.0/Rij_plus[i][j]+1.0/Rij_minus[i][j]);
      grad_Aij = tij[i][j]*factor_tij;
    }


    Point m = mij[i][j];
    for(int l=0; l<3; l++) {
      for(int k=0; k<3; k++) {
	double temp1 = grad_Aij(l)*m(k)*-1.0;
        temp_k2[l][k] += temp1;
      }
    }
             
    if(std::abs(w0)>TOL) {
      Node* a[2];
      tet.get_facet_edge_ends (a, i, j);
      Point grad_Rij_plus  = (r - (*a[1]))*(1.0/Rij_plus[i][j]);
      Point grad_Rij_minus = (r - (*a[0]))*(1.0/Rij_minus[i][j]);
      Point grad_Sij_plus  = tij[i][j]*-1.0;
      Point grad_Sij_minus = tij[i][j]*-1.0;
      Point grad_w0      = this->ni[i];
      Point grad_mij0    = this->mij[i][j]*-1.0;
      Point grad_Rij0    = (n*w0-mij[i][j]*mij0[i][j])*(1.0/Rij0[i][j]);
      Point grad_abs_wi0 = n*this->sgn(w0);
      double a_plus = Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_plus[i][j];
      double b_plus = mij0[i][j]*Sij_plus[i][j];
      Point grad_a_plus = grad_Rij0*(Rij0[i][j]*2.0) + grad_abs_wi0*Rij_plus[i][j]+grad_Rij_plus*std::abs(w0);
      Point grad_b_plus = grad_mij0*Sij_plus[i][j]+grad_Sij_plus*mij0[i][j];
      double a_minus = Rij0[i][j]*Rij0[i][j]+std::abs(w0)*Rij_minus[i][j];
      double b_minus = mij0[i][j]*Sij_minus[i][j];
      Point grad_a_minus = grad_Rij0*(Rij0[i][j]*2.0) + grad_abs_wi0*Rij_minus[i][j]+grad_Rij_minus*std::abs(w0);
      Point grad_b_minus = grad_mij0*Sij_minus[i][j]+grad_Sij_minus*mij0[i][j];
      Point grad_betaij_plus  = (grad_b_plus*a_plus-grad_a_plus*b_plus)*(1.0/(a_plus*a_plus+b_plus*b_plus));
      Point grad_betaij_minus = (grad_b_minus*a_minus-grad_a_minus*b_minus)*(1.0/(a_minus*a_minus+b_minus*b_minus));

      Point grad_betaij = grad_betaij_plus - grad_betaij_minus; // see equation (11)

      for(int l=0; l<3; l++) 
        for(int k=0; k<3; k++) {
	  double temp2 = this->sgn(w0)*grad_betaij(l)*n(k)*-1.0; 
          temp_k2[l][k] += temp2;
        }
     }

  } // loop each facets ends

  if(std::abs(w0)<TOL) {
    double k3 =0.;
    this->compute_1_R3(i, k3);
    for(int l=0; l<3; l++) {
      for(int k=0; k<3; k++) {
        double temp3 = k3*n(l)*n(k)*-1.0; // only need for w0=0  
        temp_k2[l][k]+=  temp3;  
      }
    }
  }

  // copy k2 = temp_k2
  for(int l=0; l<3; l++)
    for(int k=0; k<3; k++) 
      k2[l][k] = temp_k2[l][k];
  

  return;
  
}


/*
 a=b
 */
void M3D::set(double a[3][3], double b)
{
  for(int i=0; i<3; i++)
    for(int j=0; j<3; j++) a[i][j]=b;
  return;
}


/*
 sign of real number m
 */
double M3D::sgn(const double m) 
{
  if (std::abs(m)<TOL) return 0.; 
  else return m/std::abs(m);
}


/*
 calculate the magnetic potential V in equation (24)
 */
void M3D::V(double& v)
{
  double temp_V=0.;
  Point M  = this->tet.get_M();

  for(int i=0; i<4; i++) {
    double k0;
    this->compute_1_R(i, k0); 
    double mdotn = M*ni[i];
    temp_V+= k0*mdotn;
  }
  v = temp_V*Cm;  
  return;  
}


/*
 calculate the magnetic field B in equation (4)
 */
void M3D::B(Point& b)
{
  Point temp_B(0,0,0);
  Point M  = this->tet.get_M();

  for(int i=0; i<4; i++) {
    Point k1(0,0,0);
    this->compute_grad_1_R(i, k1);  
    double mdotn = M*ni[i];
    temp_B = temp_B + k1*mdotn;
  }
  b = temp_B*(Cm*-1.0);  
  
  return ;   
}

/*
 calculate the magnetic gradient tensor T in equation (26)
 */
void M3D::T(double tb[3][3])
{
  double temp_tb[3][3];
  this->set(temp_tb, 0);
  Point M  = this->tet.get_M();
  
  for(int i=0; i<4; i++) {
    double k2[3][3];
    this->compute_grad_grad_1_R(i, k2);  
    double mdotn = M*ni[i];
    for(int l=0; l<3; l++) {
      for(int k=0; k<3; k++) {
        temp_tb[l][k] += k2[l][k]*mdotn*Cm*-1.0;
      }
    }
  }
  
  // tb = temp_tb
  for(int i=0; i<3; i++)
    for(int j=0; j<3; j++) 
      tb[i][j] = temp_tb[i][j];

  return ;   
  
   
}

