/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

/*
*  BuildBEMatrix.cc:  class to build Boundary Elements matrix
*
*  Written by:
*   Saeed Babaeizadeh
*   Northeastern University
*   January 2006
*/

#include <Core/Algorithms/Legacy/Forward/BuildBEMatrixAlgo.h>

#include <algorithm>
#include <map>
#include <iostream>
#include <string>
#include <fstream>
#include <numeric>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/BlockMatrix.h>
#include <Core/Basis/TriLinearLgn.h>
#include <Core/Datatypes/Legacy/Field/Field.h>
#include <Core/Datatypes/Legacy/Field/TriSurfMesh.h>
#include <Core/GeometryPrimitives/Vector.h>
#include <Core/GeometryPrimitives/Point.h>
#include <Core/GeometryPrimitives/PointVectorOperators.h>

using namespace SCIRun;
using namespace SCIRun::Core::Algorithms::Forward;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Geometry;

ALGORITHM_PARAMETER_DEF(Forward, FieldNameList);
ALGORITHM_PARAMETER_DEF(Forward, FieldTypeList);
ALGORITHM_PARAMETER_DEF(Forward, BoundaryConditionList);
ALGORITHM_PARAMETER_DEF(Forward, InsideConductivityList);
ALGORITHM_PARAMETER_DEF(Forward, OutsideConductivityList);

void BuildBEMatrixBase::getOmega(
  const Vector& y1,
  const Vector& y2,
  const Vector& y3,
  DenseMatrix& coef)
{
  /*
  This function deals with the analytical solutions of the various integrals in the stiffness matrix
  The function is concerned with the integrals of the linear interpolations functions over the triangles
  and takes care of the solid spherical angle. As in most cases not all values are needed the computation
  is split up in integrals from one surface to another one

  The computational scheme follows the analytical formulas derived by the
  de Munck 1992 (IEEE Trans Biomed Engng, 39-9, pp 986-90)
  */
  const double epsilon  = 1e-12;
  Vector y21 = y2 - y1;
  Vector y32 = y3 - y2;
  Vector y13 = y1 - y3;

  Vector Ny( y1.length() , y2.length() , y3.length() );

  Vector Nyij( y21.length() , y32.length() , y13.length() );

  Vector gamma( 0 , 0 , 0 );
  double NomGamma , DenomGamma;

  NomGamma = Ny[0]*Nyij[0] + Dot(y1,y21);
  DenomGamma = Ny[1]*Nyij[0] + Dot(y2,y21);
  if (fabs(DenomGamma-NomGamma) > epsilon && (DenomGamma != 0) && NomGamma != 0 ){
    gamma[0] = -1/Nyij[0] * log(NomGamma/DenomGamma);
  }
  NomGamma = Ny[1]*Nyij[1] + Dot(y2,y32);
  DenomGamma = Ny[2]*Nyij[1] + Dot(y3,y32);
  if (fabs(DenomGamma-NomGamma) > epsilon && (DenomGamma != 0) && NomGamma != 0 ){
    gamma[1] = -1/Nyij[1] * log(NomGamma/DenomGamma);
  }
  NomGamma = Ny[2]*Nyij[2] + Dot(y3,y13);
  DenomGamma = Ny[0]*Nyij[2] + Dot(y1,y13);
  if (fabs(DenomGamma-NomGamma) > epsilon && (DenomGamma != 0) && NomGamma != 0 ){
    gamma[2] = -1/Nyij[2] * log(NomGamma/DenomGamma);
  }

  double d = Dot( y1, Cross(y2, y3) );

  Vector OmegaVec = (gamma[2]-gamma[0])*y1 + (gamma[0]-gamma[1])*y2 + (gamma[1]-gamma[2])*y3;

  /*
  In order to avoid problems with the arctan used in de Muncks paper
  the result is tested. A problem is that his formula under certain
  circumstances leads to unexpected changes of signs. Hence to avoid
  this, the denominator is checked and 2*pi is added if necessary.
  The problem without the two pi results in the following situation in
  which division of the triangle into three pieces results into
  an opposite sign compared to the spherical angle of the total
  triangle. These cases are rare but existing.
  */

  double Nn=0 , Omega=0 ;
  Nn = Ny[0]*Ny[1]*Ny[2] + Ny[0]*Dot(y2,y3) + Ny[2]*Dot(y1,y2) + Ny[1]*Dot(y3,y1);

  if (Nn > 0)  Omega = 2 * atan( d / Nn );
  if (Nn < 0)  Omega = 2 * atan( d / Nn ) + 2*M_PI ;
  if (Nn == 0)
  {
    if ( d > 0 )
      Omega = M_PI;
    else
      Omega = -M_PI;
  }

  Vector N = Cross(y21, -y13);
  double Zn1 = Dot(Cross(y2, y3) , N);
  double Zn2 = Dot(Cross(y3, y1) , N);
  double Zn3 = Dot(Cross(y1, y2) , N);

  double A2 = N.length2();
  coef(0,0) = (1/A2) * ( Zn1*Omega + d * Dot(y32, OmegaVec) );
  coef(0,1) = (1/A2) * ( Zn2*Omega + d * Dot(y13, OmegaVec) );
  coef(0,2) = (1/A2) * ( Zn3*Omega + d * Dot(y21, OmegaVec) );

}

void  BuildBEMatrixBase::get_cruse_weights(
  const Vector& p1,
  const Vector& p2,
  const Vector& p3,
  double s,
  double r,
  double area,
  DenseMatrix& cruse_weights)
{
  /*
  Inputs: p1,p2,p3= cartesian coordiantes of the triangle vertices ;
  area = triangle area
  Output: cruse_weights = The weighting factors for the 7 Radon points of the triangle
  Format of the cruse_weights matrix
  Radon point 1       Radon Point 2    ...     Radon Point 7
  Vertex 1 ->
  Vertex 2 ->
  Vertex 3 ->

  Set up the local coordinate system around the triangle. This is a 2-D system and
  for ease of use, the x-axis is chosen as the line between the first and second vertex
  of the triangle. From that the vertex of the third point is found in local coordinates.
  */

  // The angle between the F2 and F3, at vertex 1 is (pi - 'alpha').
  Vector fg2 = p1 - p3;
  double fg2_length = fg2.length();

  Vector fg3 = p2 - p1;
  double fg3_length = fg3.length();

  double cos_alpha = - Dot(fg3,fg2) / (fg2_length * fg3_length);
  double sin_alpha = sqrt(1 - cos_alpha * cos_alpha);

  // Now the vertices in local coordinates
  Vector locp1(0 , 0 , 0);
  Vector locp2(fg3_length , 0 , 0);
  Vector locp3(fg2_length * cos_alpha , fg2_length * sin_alpha , 0);

  DenseMatrix Fx(3, 1);
  Fx << locp3[0] - locp2[0],
    locp1[0] - locp3[0],
    locp2[0] - locp1[0];

  DenseMatrix Fy(3, 1);
  Fy << locp3[1] - locp2[1],
    locp1[1] - locp3[1],
    locp2[1] - locp1[1];

  Vector centroid = (locp1 + locp2 + locp3) / 3;
  DenseMatrix loc_radpt_x(1, 7);
  DenseMatrix loc_radpt_y(1, 7);
  loc_radpt_x(0,0) = centroid[0];
  loc_radpt_y(0,0) = centroid[1];
  Vector temp = (1-s) * centroid;
  loc_radpt_x(0,1) = temp[0] + locp1[0]*s;
  loc_radpt_y(0,1) = temp[1] + locp1[1]*s;
  loc_radpt_x(0,2) = temp[0] + locp2[0]*s;
  loc_radpt_y(0,2) = temp[1] + locp2[1]*s;
  loc_radpt_x(0,3) = temp[0] + locp3[0]*s;
  loc_radpt_y(0,3) = temp[1] + locp3[1]*s;
  temp = (1-r) * centroid;
  loc_radpt_x(0,4) = temp[0] + locp1[0]*r;
  loc_radpt_y(0,4) = temp[1] + locp1[1]*r;
  loc_radpt_x(0,5) = temp[0] + locp2[0]*r;
  loc_radpt_y(0,5) = temp[1] + locp2[1]*r;
  loc_radpt_x(0,6) = temp[0] + locp3[0]*r;
  loc_radpt_y(0,6) = temp[1] + locp3[1]*r;

  auto A = (0.5/area) * (Fy * loc_radpt_x - Fx * loc_radpt_y);

  DenseMatrix ones(1, 7, 1.0);
  DenseMatrix E(3, 1);
  /*
  E is a 1X3 matrix: [1st vertex  ;  2nd vertex  ;  3rd vertex]
  E = [1/3 ; 1/3 ; 1/3] + (0.5/area)*(Fy*xmid - Fx*ymid);
  but there is no need to compute the E because by our choice of the
  local coordinates, it is easy to show that the E is always [1 ; 0 ; 0]!
  */
  E << 1,0,0;
  cruse_weights = (E * ones) - A;
}

void BuildBEMatrixBase::get_g_coef(
  const Vector& p1,
  const Vector& p2,
  const Vector& p3,
  const Vector& op,
  double s,
  double r,
  const Vector& centroid,
  DenseMatrix& g_coef)
{
  // Inputs: p1,p2,p3= cartesian coordiantes of the triangle vertices ; op= Observation Point
  // Output: g_coef = G Values (Coefficients) at 7 Radon's points = 1/r
  Vector radpt = centroid - op;
  g_coef(0,0) = 1 / radpt.length();

  Vector temp = centroid * (1-s) - op;
  radpt = temp + p1 * s;
  g_coef(0,1) = 1 / radpt.length();
  radpt = temp + p2 * s;
  g_coef(0,2) = 1 / radpt.length();
  radpt = temp + p3 * s;
  g_coef(0,3) = 1 / radpt.length();

  temp = centroid * (1-r) - op;
  radpt = temp + p1 * r;
  g_coef(0,4) = 1 / radpt.length();
  radpt = temp + p2 * r;
  g_coef(0,5) = 1 / radpt.length();
  radpt = temp + p3 * r;
  g_coef(0,6) = 1 / radpt.length();
}

void BuildBEMatrixBase::bem_sing(
  const Vector& p1,
  const Vector& p2,
  const Vector& p3,
  unsigned int op_n,
  DenseMatrix& g_values,
  double s,
  double r,
  DenseMatrix& R_W)
{
  /*
  This is Jeroen's method, converted from his Matlab code, for dealing with weightings corresponding to singular triangles
  */
  Vector A,B,C,P,BC,BA,AC,AP;
  DenseMatrix WAPB(3,1);
  DenseMatrix WAPC(3,1);
  int one=0,two=1,three=2;

  switch(op_n)
  {
  case 0:
    A = p1; B=p2; C=p3;
    one=0; two=1; three=2;
    break;
  case 1:
    A = p2; B=p3; C=p1;
    one=1; two=2; three=0;
    break;
  case 2:
    A = p3; B=p1; C=p2;
    one=2; two=0; three=1;
    break;
  default:;
  }

  BC=C-B; BA=A-B; AC=C-A;
  double RL = Dot(BA,BC/BC.length())/BC.length();
  P=RL*BC+B;
  AP=A-P;
  double lAP=AP.length();
  double lBC=BC.length();
  double lBP=fabs(RL)*lBC;
  double lCP=fabs(1-RL)*lBC;
  double lAB=BA.length();
  double lAC=AC.length();
  double a,b,c,w,log_term;

  if(fabs(RL) > 0)
  {
    a=lAP; b=lBP; c=lAB;
    log_term=log( (b+c)/a );
    WAPB(0,0)=a/2 * log_term;
    w=1-RL;
    WAPB(1,0)=a* (( a-c)*(-1+w) + b*w*log_term )/(2*b);
    w=RL;
    WAPB(2,0)=a*w *( a-c  +  b*log_term )/(2*b);
  }
  else
  {
    WAPB(0,0)=0; WAPB(1,0)=0; WAPB(2,0)=0;
  }

  if(fabs(RL-1) > 0)
  {
    a = lAP; b = lCP; c = lAC;
    log_term = log( (b+c)/a );
    WAPC(0,0)=a/2 * log_term;
    w = 1-RL;
    WAPC(1,0)=a*w *( a-c  +  b*log_term )/(2*b);
    w = RL;
    WAPC(2,0)=a* (( a-c)*(-1+w) + b*w*log_term )/(2*b);
  }
  else
  {
    WAPC(0,0)=0; WAPC(1,0)=0; WAPC(2,0)=0;
  }

  if(RL<0)
  {
    WAPB(0,0)*=-1.0; WAPB(1,0)*=-1.0; WAPB(2,0)*=-1.0;
  }
  if(RL>1)
  {
    WAPC(0,0)*=-1.0; WAPC(1,0)*=-1.0; WAPC(2,0)*=-1.0;
  }

  g_values(one,0) = WAPB(0,0) + WAPC(0,0);
  g_values(two,0) = WAPB(1,0) + WAPC(1,0);
  g_values(three,0) = WAPB(2,0) + WAPC(2,0);
}

void BuildBEMatrixBase::get_auto_g(
  const Vector& p1,
  const Vector& p2,
  const Vector& p3,
  unsigned int op_n,
  DenseMatrix& g_values,
  double s,
  double r,
  DenseMatrix& R_W)
{
  /*
  A routine to solve the Auto G-parameter integral for a triangle from
  a "closed" observation point.
  The scheme is the standard one for all the BEM routines.
  Input are the observation point and triangle co-ordinates
  Input:
  op_n = observation point number (1, 2 or 3)
  p1,p2,p3 = triangle co-ordinates (REAL)

  Output:
  g_values
  = the total values for the 1/r integral for
  each of the subtriangles associated with each
  integration triangle vertex about the observation
  point defined by the calling
  program.
  */

  Vector p5 = (p1 + p2) / 2;
  Vector p6 = (p2 + p3) / 2;
  Vector p4 = (p1 + p3) / 2;
  Vector ctroid = (p1 + p2 + p3) / 3;
  Vector op;

  switch(op_n)
  {
  case 0:
    op = p1;
    g_values(0,0) = get_new_auto_g(op, p5, p4) + do_radon_g(p5, ctroid, p4, op, s, r, R_W);
    g_values(1,0) = do_radon_g(p2, p6, p5, op, s, r, R_W) + do_radon_g(p5, p6, ctroid, op, s, r, R_W);
    g_values(2,0) = do_radon_g(p3, p4, p6, op, s, r, R_W) + do_radon_g(p4, ctroid, p6, op, s, r, R_W);
    break;
  case 1:
    op = p2;
    g_values(0,0) = do_radon_g(p1, p5, p4, op, s, r, R_W) + do_radon_g(p5, ctroid, p4, op, s, r, R_W);
    g_values(1,0) = get_new_auto_g(op, p6, p5) + do_radon_g(p5, p6, ctroid, op, s, r, R_W);
    g_values(2,0) = do_radon_g(p3, p4, p6, op, s, r, R_W) + do_radon_g(p4, ctroid, p6, op, s, r, R_W);
    break;
  case 2:
    op = p3;
    g_values(0,0) = do_radon_g(p1, p5, p4, op, s, r, R_W) + do_radon_g(p5, ctroid, p4, op, s, r, R_W);
    g_values(1,0) = do_radon_g(p2, p6, p5, op, s, r, R_W) + do_radon_g(p5, p6, ctroid, op, s, r, R_W);
    g_values(2,0) = get_new_auto_g(op, p4, p6) + do_radon_g(p4, ctroid, p6, op, s, r, R_W);
    break;
  }
}

double BuildBEMatrixBase::get_new_auto_g(
  const Vector& op,
  const Vector& p2,
  const Vector& p3)
{
  //  Inputs: op,p2,p3= cartesian coordiantes of the triangle vertices ; op= Observation Point
  //  Output: g1 = G value for the triangle for "auto_g"
  //  This function is called from get_auto_g.m

  double delta_min = 0.00001;
  unsigned int max_number_of_divisions = 256;

  Vector a = p2 - op; double a_mag = a.length();
  Vector b = p3 - p2; double b_mag = b.length();
  Vector c = op - p3; double c_mag = c.length();

  Vector aV = Cross(p2 - op, p3 - p2)*0.5;
  double area = aV.length();
  double area2 = 2.0*area;
  double h = (area2) / b_mag;
  double alfa=0;  if (h<a_mag) alfa = acos(h/a_mag);
  double AC = a_mag*c_mag;
  double teta = 0; if (area2<=AC) teta = asin( area2 / AC );

  unsigned int nod = 1;
  double sai_old = sqrt(area2 * teta);
  double delta = 1;

  double gama, gama_j, rhoj_1, rhoj, sum, sai_new=0;

  while( (delta >= delta_min) && (nod <= max_number_of_divisions) )
  {
    nod = 2*nod;
    gama = teta / nod;
    sum = 0;
    gama_j = 0;
    rhoj_1 = a_mag;
    for ( unsigned int j = 1; j <= nod; j++)
    {
      gama_j = gama_j + gama;
      rhoj = h / cos(alfa - gama_j);
      sum = sum + sqrt( std::fabs(rhoj * rhoj_1) );
      rhoj_1 = rhoj;
    }
    sai_new = sum * sqrt(std::fabs(gama * sin(gama)));
    delta = 0;
    if (sai_new + sai_old)
      delta = std::fabs((sai_new - sai_old) / (sai_new + sai_old));
    sai_old = sai_new;
  }
  return sai_new;
}

double BuildBEMatrixBase::do_radon_g(
  const Vector& p1,
  const Vector& p2,
  const Vector& p3,
  const Vector& op,
  double s,
  double r,
  DenseMatrix& R_W)
{
  //  Inputs: p1,p2,p3= cartesian coordiantes of the triangle vertices ; op= Observation Point
  //  Output: g2 = G value for the triangle for "auto_g"
  //  This function is called from get_auto_g.m

  Vector centroid = (p1 + p2 + p3) / 3;

  DenseMatrix g_coef(1, 7);
  get_g_coef(p1, p2, p3, op, s, r, centroid, g_coef);

  double g2 = 0;
  for (int i=0; i<7; i++)   g2 = g2 + g_coef(0,i)*R_W(0,i);

  Vector aV = Cross(p2 - p1, p3 - p2)*0.5;

  return g2 * aV.length();
}

class BuildBEMatrixBaseCompute : public BuildBEMatrixBase
{
public:
  template <class MatrixType>
  static void make_auto_P_compute(VMesh* hsurf, MatrixType& auto_P, double in_cond, double out_cond, double op_cond);

  template <class MatrixType>
  static void make_cross_P_compute(VMesh* hsurf1, VMesh* hsurf2, MatrixType& cross_P, double in_cond, double out_cond, double op_cond);

  template <class MatrixType>
  static void make_auto_G_compute(VMesh* hsurf, MatrixType& auto_G, double in_cond, double out_cond, double op_cond, const std::vector<double>& avInn);

  template <class MatrixType>
  static void make_cross_G_compute( VMesh*,
  VMesh*,
  MatrixType&,
  double,
  double,
  double,
  const std::vector<double>& );
};

void BuildBEMatrixBase::make_auto_G_allocate(VMesh* hsurf, DenseMatrixHandle &h_GG_)
{
  auto nnodes = numNodes(hsurf);
  h_GG_.reset(new DenseMatrix(nnodes, nnodes, 0.0));
}

void BuildBEMatrixBase::make_auto_G(VMesh* hsurf, DenseMatrixHandle &h_GG_,
double in_cond, double out_cond, double op_cond, const std::vector<double>& avInn)
{
  make_auto_G_allocate(hsurf, h_GG_);
  BuildBEMatrixBaseCompute::make_auto_G_compute(hsurf, *h_GG_, in_cond, out_cond, op_cond, avInn);
}

template <class MatrixType>
void BuildBEMatrixBaseCompute::make_auto_G_compute(VMesh* hsurf, MatrixType& auto_G,
  double in_cond, double out_cond, double op_cond, const std::vector<double>& avInn)
{
  //const double mult = 1/(2*M_PI)*((out_cond - in_cond)/op_cond);  // op_cond=out_cond for all the surfaces but the outermost surface which in op_cond=in_cond
  const double mult = 1/(4*M_PI)*(out_cond - in_cond);  // op_cond=out_cond for all the surfaces but the outermost surface which in op_cond=in_cond

  VMesh::Node::array_type nodes;

  VMesh::Node::iterator ni, nie;
  VMesh::Face::iterator fi, fie;
  DenseMatrix cruse_weights(3, 7);
  DenseMatrix g_coef(1, 7);
  DenseMatrix R_W(1,7); // Radon Points Weights
  DenseMatrix temp(1,7);
  DenseMatrix g_values(3, 1);

  double area;

  double sqrt15 = sqrt(15.0);
  //R_W(0,0) = 9/40; // <- Burak! FIX ME!
  R_W(0,0) = 9.0/40.0;
  R_W(0,1) = (155 + sqrt15) / 1200;
  R_W(0,2) = R_W(0,1);
  R_W(0,3) = R_W(0,1);
  R_W(0,4) = (155 - sqrt15) / 1200;
  R_W(0,5) = R_W(0,4);
  R_W(0,6) = R_W(0,4);

  double s = (1 - sqrt15) / 7;
  double r = (1 + sqrt15) / 7;

  hsurf->begin(fi); hsurf->end(fie);
  for (; fi != fie; ++fi)
  { //! find contributions from every triangle
    hsurf->get_nodes(nodes, *fi);
    Vector p1(hsurf->get_point(nodes[0]));
    Vector p2(hsurf->get_point(nodes[1]));
    Vector p3(hsurf->get_point(nodes[2]));

    area = avInn[*fi];

    get_cruse_weights(p1, p2, p3, s, r, area, cruse_weights);
    Vector centroid = (p1 + p2 + p3) / 3.0;
    hsurf->begin(ni); hsurf->end(nie);
    for (; ni != nie; ++ni)
    { //! for every node
      VMesh::Node::index_type ppi = *ni;
      Vector op(hsurf->get_point(ppi));

      if (ppi == nodes[0])       bem_sing(p1, p2, p3, 0, g_values, s, r, R_W);
      else if (ppi == nodes[1])       bem_sing(p1, p2, p3, 1, g_values, s, r, R_W);
      else if (ppi == nodes[2])       bem_sing(p1, p2, p3, 2, g_values, s, r, R_W);
      else
      {
        get_g_coef(p1, p2, p3, op, s, r, centroid, g_coef);

        for (int i=0; i<7; i++)  temp(0,i) = g_coef(0,i)*R_W(0,i);

        g_values = area * (cruse_weights * temp.transpose());
      } // else

      for (int i=0; i<3; ++i)
        auto_G(ppi, nodes[i])+=g_values(i,0)*mult;
    }
  }
}

void BuildBEMatrixBase::make_cross_G_allocate(VMesh* hsurf1, VMesh* hsurf2, DenseMatrixHandle &h_GG_)
{
  h_GG_.reset(new DenseMatrix(numNodes(hsurf1), numNodes(hsurf2), 0.0));
}

void BuildBEMatrixBase::make_cross_G(VMesh* hsurf1, VMesh* hsurf2, DenseMatrixHandle &h_GG_,
  double in_cond, double out_cond, double op_cond, const std::vector<double>& avInn)
{
  make_cross_G_allocate(hsurf1, hsurf2, h_GG_);
  BuildBEMatrixBaseCompute::make_cross_G_compute(hsurf1, hsurf2, *h_GG_, in_cond, out_cond, op_cond, avInn);
}

template <class MatrixType>
void BuildBEMatrixBaseCompute::make_cross_G_compute(VMesh* hsurf1, VMesh* hsurf2, MatrixType& cross_G,
  double in_cond, double out_cond, double op_cond, const std::vector<double>& avInn)
{
  const double mult = 1/(4*M_PI)*(out_cond - in_cond);
  //   out_cond and in_cond belong to hsurf2 and op_cond is the out_cond of hsurf1 for all the surfaces but the outermost surface which in op_cond=in_cond

  VMesh::Node::array_type nodes;

  VMesh::Node::iterator  ni, nie;
  VMesh::Face::iterator  fi, fie;

  DenseMatrix cruse_weights(3, 7);
  DenseMatrix g_coef(1, 7);
  DenseMatrix R_W(1,7); // Radon Points Weights
  DenseMatrix temp(1,7);
  DenseMatrix g_values(3, 1);

  double area;

  double sqrt15 = sqrt(15.0);
  //R_W(0,0) = 9/40; // <- Burak! FIX ME!
  R_W(0,0) = 9.0/40.0;
  R_W(0,1) = (155 + sqrt15) / 1200;
  R_W(0,2) = R_W(0,1);
  R_W(0,3) = R_W(0,1);
  R_W(0,4) = (155 - sqrt15) / 1200;
  R_W(0,5) = R_W(0,4);
  R_W(0,6) = R_W(0,4);

  double s = (1 - sqrt15) / 7;
  double r = (1 + sqrt15) / 7;

  hsurf2->begin(fi); hsurf2->end(fie);
  for (; fi != fie; ++fi)
  { //! find contributions from every triangle
    hsurf2->get_nodes(nodes, *fi);
    Vector p1(hsurf2->get_point(nodes[0]));
    Vector p2(hsurf2->get_point(nodes[1]));
    Vector p3(hsurf2->get_point(nodes[2]));

    area = avInn[*fi];

    get_cruse_weights(p1, p2, p3, s, r, area, cruse_weights);
    Vector centroid = (p1 + p2 + p3) / 3.0;

    hsurf1->begin(ni); hsurf1->end(nie);
    for (; ni != nie; ++ni)
    { //! for every node
      VMesh::Node::index_type ppi = *ni;
      Vector op(hsurf1->get_point(ppi));
      get_g_coef(p1, p2, p3, op, s, r, centroid, g_coef);

      for (int i=0; i<7; i++)  temp(0,i) = g_coef(0,i)*R_W(0,i);

      g_values = area * (cruse_weights * temp.transpose());

      for (int i=0; i<3; ++i)
        cross_G(ppi, nodes[i])+=g_values(i,0)*mult;
    }
  }
}

void BuildBEMatrixBase::make_cross_P_allocate(VMesh* hsurf1, VMesh* hsurf2, DenseMatrixHandle &h_PP_)
{
  h_PP_.reset(new DenseMatrix(numNodes(hsurf1), numNodes(hsurf2), 0.0));
}

void BuildBEMatrixBase::make_cross_P(VMesh* hsurf1, VMesh* hsurf2, DenseMatrixHandle &h_PP_,
  double in_cond, double out_cond, double op_cond)
{
  make_cross_P_allocate(hsurf1, hsurf2, h_PP_);
  BuildBEMatrixBaseCompute::make_cross_P_compute(hsurf1, hsurf2, *h_PP_, in_cond, out_cond, op_cond);
}

template <class MatrixType>
void BuildBEMatrixBaseCompute::make_cross_P_compute(VMesh* hsurf1, VMesh* hsurf2, MatrixType& cross_P, double in_cond, double out_cond, double op_cond)
{
  const double mult = 1/(4*M_PI)*(out_cond - in_cond);
  //   out_cond and in_cond belong to hsurf2 and op_cond is the out_cond of hsurf1 for all the surfaces but the outermost surface which in op_cond=in_cond
  VMesh::Node::array_type nodes;
  DenseMatrix coef(1, 3);
  int i;

  VMesh::Node::iterator  ni, nie;
  VMesh::Face::iterator  fi, fie;

  hsurf1->begin(ni); hsurf1->end(nie);
  for (; ni != nie; ++ni){ //! for every node
    VMesh::Node::index_type ppi = *ni;
    Point pp = hsurf1->get_point(ppi);

    hsurf2->begin(fi); hsurf2->end(fie);
    for (; fi != fie; ++fi){ //! find contributions from every triangle

      hsurf2->get_nodes(nodes, *fi);
      Vector v1 = hsurf2->get_point(nodes[0]) - pp;
      Vector v2 = hsurf2->get_point(nodes[1]) - pp;
      Vector v3 = hsurf2->get_point(nodes[2]) - pp;

      getOmega(v1, v2, v3, coef);

      for (i=0; i<3; ++i)
        cross_P(ppi, nodes[i])-=coef(0,i)*mult;
    }
  }
}

void BuildBEMatrixBase::make_auto_P_allocate(VMesh* hsurf, DenseMatrixHandle &h_PP_)
{
  auto nnodes = numNodes(hsurf);
  h_PP_.reset(new DenseMatrix(nnodes, nnodes, 0.0));
}

int BuildBEMatrixBase::numNodes(FieldHandle f)
{
  return numNodes(f->vmesh());
}

int BuildBEMatrixBase::numNodes(VMesh* hsurf)
{
  VMesh::Node::size_type nsize;
  hsurf->size(nsize);
  return static_cast<int>(nsize);
}

template <class MatrixType>
void BuildBEMatrixBaseCompute::make_auto_P_compute(VMesh* hsurf, MatrixType& auto_P, double in_cond, double out_cond, double op_cond)
{
  auto nnodes = auto_P.rows();

  //const double mult = 1/(2*M_PI)*((out_cond - in_cond)/op_cond);  // op_cond=out_cond for all the surfaces but the outermost surface which in op_cond=in_cond
  const double mult = 1/(4*M_PI)*(out_cond - in_cond);

  VMesh::Node::array_type nodes;
  DenseMatrix coef(1, 3);

  VMesh::Node::iterator ni, nie;
  VMesh::Face::iterator fi, fie;

  unsigned int i;

  hsurf->begin(ni); hsurf->end(nie);

  for (; ni != nie; ++ni){ //! for every node
    VMesh::Node::index_type ppi = *ni;
    Point pp = hsurf->get_point(ppi);

    hsurf->begin(fi); hsurf->end(fie);

    for (; fi != fie; ++fi) { //! find contributions from every triangle

      hsurf->get_nodes(nodes, *fi);
      if (ppi!=nodes[0] && ppi!=nodes[1] && ppi!=nodes[2]){
        Vector v1 = hsurf->get_point(nodes[0]) - pp;
        Vector v2 = hsurf->get_point(nodes[1]) - pp;
        Vector v3 = hsurf->get_point(nodes[2]) - pp;

        getOmega(v1, v2, v3, coef);

        for (i=0; i<3; ++i)
          auto_P(ppi, nodes[i])-=coef(0,i)*mult;
      }
    }
  }

  //! accounting for autosolid angle
  auto sumOfRows = auto_P.rowwise().sum().eval();
  for (i=0; i<nnodes; ++i)
  {
    auto_P(i,i) = out_cond - sumOfRows(i);
  }
}

void BuildBEMatrixBase::make_auto_P(VMesh* hsurf, DenseMatrixHandle &h_PP_,
  double in_cond, double out_cond, double op_cond)
{
  make_auto_P_allocate(hsurf, h_PP_);
  BuildBEMatrixBaseCompute::make_auto_P_compute(hsurf, *h_PP_, in_cond, out_cond, op_cond);
}

// precalculate triangles area
void BuildBEMatrixBase::pre_calc_tri_areas(VMesh* hsurf, std::vector<double>& areaV){

  VMesh::Face::iterator  fi, fie;

  hsurf->begin(fi);
  hsurf->end(fie);
  for (; fi != fie; ++fi)
    areaV.push_back(hsurf->get_area(*fi));
}

// C++ized MollerTrumbore97 Ray Triangle intersection test.
bool BuildBEMatrixBase::ray_triangle_intersect(double &t,
  const Point &point,
  const Vector &dir,
  const Point &p0,
  const Point &p1,
  const Point &p2)
{
  // Find vectors for two edges sharing p0.
  const Vector edge1 = p1 - p0;
  const Vector edge2 = p2 - p0;

  // begin calculating determinant - also used to calculate U parameter.
  const Vector pvec = Cross(dir, edge2);

  // if determinant is near zero, ray lies in plane of triangle.
  const double det = Dot(edge1, pvec);
  const double EPSILON = 1.0e-6;
  if (det > -EPSILON && det < EPSILON)
  {
    return false;
  }
  const double inv_det = 1.0 / det;

  // Calculate distance from vert0 to ray origin.
  const Vector tvec = point - p0;

  // Calculate U parameter and test bounds.
  const double u = Dot(tvec, pvec) * inv_det;
  if (u < 0.0 || u > 1.0)
  {
    return false;
  }

  // Prepare to test V parameter.
  const Vector qvec = Cross(tvec, edge1);

  // Calculate V parameter and test bounds.
  const double v = Dot(dir, qvec) * inv_det;
  if (v < 0.0 || u + v > 1.0)
  {
    return false;
  }

  // Calculate t, ray intersects triangle.
  t = Dot(edge2, qvec) * inv_det;

  return true;
}

void BuildBEMatrixBase::compute_intersections(std::vector<std::pair<double, int> >
  &results,
  const VMesh* mesh,
  const Point &p, const Vector &v,
  int marker)
{
  VMesh::Face::iterator itr, eitr;
  mesh->begin(itr);
  mesh->end(eitr);
  double t;
  while (itr != eitr)
  {
    VMesh::Node::array_type nodes;
    mesh->get_nodes(nodes, *itr);
    Point p0, p1, p2;
    mesh->get_center(p0, nodes[0]);
    mesh->get_center(p1, nodes[1]);
    mesh->get_center(p2, nodes[2]);
    if (ray_triangle_intersect(t, p, v, p0, p1, p2))
    {
      results.push_back(std::make_pair(t, marker));
    }
    ++itr;
  }
}

static bool
  pair_less(const std::pair<double, int> &a,
  const std::pair<double, int> &b)
{
  return a.first < b.first;
}

int BuildBEMatrixBase::compute_parent(const std::vector<VMesh*> &meshes, int index)
{
  Point point;
  meshes[index]->get_center(point, VMesh::Node::index_type(0));
  Vector dir(1.0, 1.0, 1.0);
  std::vector<std::pair<double, int> > intersections;

  unsigned int i;
  for (i = 0; i < (unsigned int)meshes.size(); i++)
  {
    compute_intersections(intersections, meshes[i], point, dir, i);
  }

  std::sort(intersections.begin(), intersections.end(), pair_less);

  std::vector<int> counts(meshes.size(), 0);
  for (i = 0; i < intersections.size(); i++)
  {
    if (intersections[i].second == index)
    {
      // First odd count is parent.
      for (int j = i-1; j >= 0; j--)
      {
        // TODO: unusual odd/even number test?
        if (counts[intersections[j].second] & 1)
        {
          return intersections[j].second;
        }
      }
      // No odd parent, is outside.
      return static_cast<int>( meshes.size() );
    }
    counts[intersections[i].second]++;
  }

  // Indeterminate, we should intersect with ourselves.
  return static_cast<int>( meshes.size() );
}

bool BuildBEMatrixBase::compute_nesting(std::vector<int> &nesting, const std::vector<VMesh*> &meshes)
{
  nesting.resize(meshes.size());

  unsigned int i;
  for (i = 0; i < (unsigned int)meshes.size(); i++)
  {
    nesting[i] = compute_parent(meshes, i);
  }

  return true;
}

class SurfaceAndPoints : public BEMAlgoImpl, public BuildBEMatrixBaseCompute
{
public:
  MatrixHandle compute(const bemfield_vector& fields) const override;
};

class SurfaceToSurface : public BEMAlgoImpl, public BuildBEMatrixBaseCompute
{
public:
  MatrixHandle compute(const bemfield_vector& fields) const override;
};

BEMAlgoPtr BEMAlgoImplFactory::create(const bemfield_vector& fields)
{
  ///////////////////////////////////////////////////////////////////////////////////////////////////
  // Check for special case where the potentials need to be evaluated at the nodes of a lead
  // This case assumes the first input is the surface mesh and the second is the location of the
  // nodes

  const bemfield_vector::size_type SPECIAL_CASE_LEN = 2;

  if ( fields.size() == SPECIAL_CASE_LEN )
  {
    VMesh *surface, *nodes;
    int surfcount=0, pointcloudcount=0;

    bool meets_conditions = true;

    for (bemfield_vector::size_type i = 0; i < SPECIAL_CASE_LEN; i++)
    {
      if (fields[i].surface)
      {
        surface = fields[i].field_->vmesh();
        if (! surface->is_trisurfmesh() ) meets_conditions = false;
        surfcount++;
      }
      else
      {
        nodes = fields[i].field_->vmesh();
        if (! ( nodes->is_pointcloudmesh() ) || nodes->is_curvemesh() )
        {
          meets_conditions = false;
        }
        pointcloudcount++;
      }
    }

    if ( (surfcount == 0) || (pointcloudcount == 0) )
    {
      meets_conditions = false;
    }

    // If all of the checks above don't flag meets_conditions as false,
    // return a value that indicates the algorithm to use is the surface-to-nodes case
    if ( meets_conditions )
      return boost::make_shared<SurfaceAndPoints>();
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////
  // Check for case where all inputs are triangle surfaces, and there is at least one source and
  // one measurement surface

  bool allsurfaces=true, hasmeasurementsurf=false, hassourcesurf=false;

  for (bemfield_vector::size_type i = 0; i < fields.size(); i++)
  {
    // if the current field is not marked to be used as a surface OR it's not of trisurfmesh type, this algorithm does not apply
    if ( (! fields[i].field_->vmesh()->is_trisurfmesh()) || (! fields[i].surface) )
    {
      allsurfaces=false;
      break;
    }

    if (fields[i].measurement) hasmeasurementsurf = true;
    if (fields[i].source) hassourcesurf = true;
  }

  // if all fields are surfaces, there exists a measurement and a source surface, then use the surface-to-surface algorithm... else fail
  if (allsurfaces && hasmeasurementsurf && hassourcesurf)
  {
    return boost::make_shared<SurfaceToSurface>();
  }
  else
  {
    return nullptr;
  }
}

static void printInfo(const DenseMatrix& m, const std::string& name)
{
#if 0
  std::cout << name << ": " << m.rows() << " x " << m.cols() << std::endl;
  std::cout << name << " min: " << m.minCoeff() << std::endl;
  std::cout << name << " max: " << m.maxCoeff() << std::endl;
#endif
}

MatrixHandle SurfaceToSurface::compute(const bemfield_vector& fields) const
{
  // Math for surface-to-surface BEM algorithm (based on Jeroen Stinstra's BEM Matlab code that's part of SCIRun)
  // -------------------------------------------------------------------------------------------------------------
  // EE = matrix relating potentials on surfaces to potentials on other surfaces
  // EJ = matrix relating current density on surfaces (normal to surface) to potentials on surfaces
  // u  = potentials on the surfaces
  // j  = current density normal to the surfaces
  //
  // General equation: EE*u + EJ*j = (dipolar sources not on the surfaces)
  // Assuming all sources are on surfaces: EE*u + EJ*j = 0
  // (below assumes that measurement=Neumann boundary conditions and source=Dirichlet boundary conditions)
  //
  // s = source indices
  // m = measurement indices
  //
  // Pmm = EE(m,m)
  // Pss = EE(s,s)
  // Pms = EE(m,s)
  // Psm = EE(s,m)
  //
  // Gms = EJ(m,s)
  // Gss = EJ(s,s)
  //
  // After some block-matrix math to eliminate j from the equation and find T s.t. u(m)=T*u(s), we get:
  // iGss = inv(Gss)
  // T = inv(Pmm - Gms*iGss*Psm)*(Gms*iGss*Pss - Pms)
  //

  const size_t Nfields = fields.size();
  double op_cond=0.0; // op_cond is not used in this formulation -- someone needs to check this math and make a better decision about how to handle this value below

  // Count the number of fields that have been specified as being "sources" or "measurements" (and keep track of indices)
  int Nsources = 0;
  std::vector<int> sourcefieldindices;
  int Nmeasurements = 0;
  std::vector<int> measurementfieldindices;

  for(int i=0; i < Nfields; i++)
  {
    if(fields[i].source)
    {
      Nsources++;
      sourcefieldindices.push_back(i);
    }
    else if(fields[i].measurement)
    {
      Nmeasurements++;
      measurementfieldindices.push_back(i);
    }
  }

  std::vector<int> fieldNodeSize(fields.size());
  std::transform(fields.begin(), fields.end(), fieldNodeSize.begin(), [](const bemfield& f) { return numNodes(f.field_); } );
  DenseBlockMatrix EE(fieldNodeSize, fieldNodeSize);

  // Calculate EE in block matrix form
  for(int i = 0; i < Nfields; i++)
  {
    for(int j = 0; j < Nfields; j++)
    {
      if (i == j)
      {
        auto block = EE.blockRef(i, j);
        make_auto_P_compute(fields[i].field_->vmesh(), block, fields[i].insideconductivity, fields[i].outsideconductivity, op_cond);
      }
      else
      {
        auto block = EE.blockRef(i, j);
        make_cross_P_compute(fields[i].field_->vmesh(), fields[j].field_->vmesh(), block, fields[j].insideconductivity, fields[j].outsideconductivity, op_cond);
      }
    }
  }

  printInfo(EE.matrix(), "EE");

  std::vector<int> sourceFieldNodeSize(sourcefieldindices.size());
  auto sourceFields = fields | boost::adaptors::filtered([](const bemfield& f) { return f.source; });
  //following doesn't compile in VS2010, but does in clang: enable it after upgrading to 2013
  //auto transformer = [this](const bemfield& f) -> int { return numNodes(f.field_); };
  //auto trans = filt | boost::adaptors::transformed(transformer);
  //boost::copy(trans, sourceFieldNodeSize.begin());
  std::transform(sourceFields.begin(), sourceFields.end(), sourceFieldNodeSize.begin(), [](const bemfield& f) { return numNodes(f.field_); } );

  DenseBlockMatrix EJ(fieldNodeSize, sourceFieldNodeSize);

  // Calculate EJ(:,s) in block matrix form
  // ***NOTE THE CHANGE IN INDEXING!!!***
  // (The indices of block columns of EJ correspond to field indices according to "sourcefieldindices", and this affects everything with EJ below this point too!)
  for(int j = 0; j < Nsources; j++)
  {
    // Precalculate triangle areas for this source field/surface
    std::vector<double> triangleareas;
    pre_calc_tri_areas(fields[sourcefieldindices[j]].field_->vmesh(), triangleareas);

    for(int i = 0; i < Nfields; i++)
    {
      if (i == sourcefieldindices[j])
      {
        auto block = EJ.blockRef(i,j);
        make_auto_G_compute(fields[i].field_->vmesh(), block, fields[i].insideconductivity, fields[i].outsideconductivity, op_cond, triangleareas);
      }
      else
      {
        auto block = EJ.blockRef(i,j);
        make_cross_G_compute(fields[i].field_->vmesh(), fields[sourcefieldindices[j]].field_->vmesh(), block, fields[j].insideconductivity, fields[j].outsideconductivity, op_cond, triangleareas);
      }
    }
  }

  printInfo(EJ.matrix(), "EJ");

  // This needs to be checked.  It was taken out because the deflation was producing errors
  // Jeroen's matlab code, which was the basis of this code, only does a defation in test cases.

  // Perform deflation on EE matrix
  //const double deflationconstant = 1.0/EE.matrix().ncols();
  //EE.matrix() = EE.matrix().array() + deflationconstant;

  std::vector<int> measurementNodeSize(measurementfieldindices.size());
  auto measFields = fields | boost::adaptors::filtered([](const bemfield& f) { return f.measurement; });
  //following doesn't compile in VS2010, but does in clang: enable it after upgrading to 2013
  //auto transformer = [this](const bemfield& f) -> int { return numNodes(f.field_); };
  //auto trans = filt | boost::adaptors::transformed(transformer);
  //boost::copy(trans, sourceFieldNodeSize.begin());
  std::transform(measFields.begin(), measFields.end(), measurementNodeSize.begin(), [](const bemfield& f) { return numNodes(f.field_); } );

  // Split EE apart into Pmm, Pss, Pms, and Psm
  // -----------------------------------------------
  // Pmm:
  DenseBlockMatrix Pmm(measurementNodeSize, measurementNodeSize);
  for(int i = 0; i < Nmeasurements; i++)
  {
    for(int j = 0; j < Nmeasurements; j++)
    {
      Pmm.blockRef(i,j) = EE.blockRef(measurementfieldindices[i], measurementfieldindices[j]);
    }
  }
  printInfo(Pmm.matrix(), "Pmm");

  // Pss:
  DenseBlockMatrix Pss(sourceFieldNodeSize, sourceFieldNodeSize);
  for(int i = 0; i < Nsources; i++)
  {
    for(int j = 0; j < Nsources; j++)
    {
      Pss.blockRef(i,j) = EE.blockRef(sourcefieldindices[i],sourcefieldindices[j]);
    }
  }
  printInfo(Pss.matrix(), "Pss");

  // Pms:
  DenseBlockMatrix Pms(measurementNodeSize, sourceFieldNodeSize);
  for(int i = 0; i < Nmeasurements; i++)
  {
    for(int j = 0; j < Nsources; j++)
    {
      Pms.blockRef(i,j) = EE.blockRef(measurementfieldindices[i],sourcefieldindices[j]);
    }
  }
  printInfo(Pms.matrix(), "Pms");

  // Psm:
  DenseBlockMatrix Psm(sourceFieldNodeSize, measurementNodeSize);
  for(int i = 0; i < Nsources; i++)
  {
    for(int j = 0; j < Nmeasurements; j++)
    {
      Psm.blockRef(i,j) = EE.blockRef(sourcefieldindices[i],measurementfieldindices[j]);
    }
  }
  printInfo(Psm.matrix(), "Psm");

  // Split EJ apart into Gms and Gss (see ALL-CAPS note above about differences in block row vs column indexing in EJ matrix)
  // -----------------------------------------------
  // Gms:
  DenseBlockMatrix Gms(measurementNodeSize, sourceFieldNodeSize);
  for(int i = 0; i < Nmeasurements; i++)
  {
    for(int j = 0; j < Nsources; j++)
    {
      Gms.blockRef(i,j) = EJ.blockRef(measurementfieldindices[i],j);
    }
  }
  printInfo(Gms.matrix(), "Gms");

  // Gss:
  DenseBlockMatrix Gss(sourceFieldNodeSize, sourceFieldNodeSize);
  for(int i = 0; i < Nsources; i++)
  {
    for(int j = 0; j < Nsources; j++)
    {
      Gss.blockRef(i,j) = EJ.blockRef(sourcefieldindices[i],j);
    }
  }
  printInfo(Gss.matrix(), "Gss");

  // TODO: add deflation step

  // Compute T here (see math in comments above)
  // TransferMatrix = T = inv(Pmm - Gms*iGss*Psm)*(Gms*iGss*Pss - Pms) = inv(C)*D

  auto Y = Gms.matrix() * Gss.matrix().inverse();
  auto C = Pmm.matrix() - Y * Psm.matrix();
  auto D = Y * Pss.matrix() - Pms.matrix();

  auto T = C.inverse() * D; // T = inv(C)*D
  return boost::make_shared<DenseMatrix>(T);

  //This could be done on one line (see below), but Y (see above) would need to be calculated twice:
  //MatrixHandle TransferMatrix1 = inv(Pmm - Gms * Gss * Psm) * (Gms * Gss * Pss - Pms);
}

MatrixHandle SurfaceAndPoints::compute(const bemfield_vector& fields) const
{
  // NOTE: This is Jeroen's code that has been adapted to fit the new module structure
  //
  // Math:
  // The boundary element formulation is based on Matlab code
  // bemMatrixPP2.m, which can be found in the matlab package

  // The BEM formulation assumes the following matrix equations
  // P_surf_surf * PHI_surf + G_surf_surf * J_surf =  sources_in_volume
  //
  // PHI_surf are the potentials on the surface
  // J_surf are the currents passing perpendicular to the surface
  // sources_in_volume is empty in this case
  //
  // P_surf_surf is the matrix that connects the potentials at the nodes to the integral over the
  // potential at the surface. Its terms consist of Green's function ( 1/ ( 4pi*||r-r'|| ) ) over
  // the surface of each element. As this integral becomes singular for a node and a triangle that
  // share a corner node, we use a trick to avoid computing this integral as we know that the
  // the system should reference potential invariant. Hence the rows of the matrix need to sum to
  // to zero
  //
  // G_surf_surf is the matrix that connects the potentials at the nodes to the integral over the
  // currents flowing through the surface.
  //
  // The second equation that we use is the expression of the potentials at an arbitrary point
  // to the potentials at the surface and the current flowing through the surface
  //
  // PHI_nodes = P_nodes_surf * PHI_surf + G_nodes_surf * J_surf
  //
  // Here matrix P_nodes_surf is the matrix that projects the contribution of the potentials of the
  // surface to the nodes within the volume
  //
  // Here G_nodes_surf is the matrix that projects the contribution of the currents flowing through
  // the surface to the nodes within the volume
  //
  // Adding both equations together will result in
  //
  // PHI_nodes = P_nodes_surf* PHI_surf - G_nodes_surf * inv( G_surf_surf) * P_surf_surf * PHI_surf
  //
  // In other words the transfer matrix is
  // P_nodes_surf - G_nodes_surf * inv( G_surf_surf) * P_surf_surf

  VMesh *nodes = nullptr;
  VMesh *surface = nullptr;

  for (int i=0; i<2; i++)
  {
    if (fields[i].surface)
      surface = fields[i].field_->vmesh();
    else
      nodes = fields[i].field_->vmesh();
  }

  DenseMatrixHandle Pss;
  DenseMatrixHandle Gss;
  DenseMatrixHandle Pns;
  DenseMatrixHandle Gns;
  make_auto_P( surface, Pss, 1.0, 0.0, 1.0 );
  make_cross_P( nodes, surface, Pns, 1.0, 0.0, 1.0 );

  std::vector<double> area;
  pre_calc_tri_areas( surface, area );

  make_auto_G( surface, Gss, 1.0, 0.0, 1.0, area );
  make_cross_G( nodes, surface, Gns, 1.0, 0.0, 1.0, area );

  return boost::make_shared<DenseMatrix>(*Pns - (*Gns * Gss->inverse() * *Pss));
}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with GNMF. */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
#ifndef NDEBUG
	LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

	// parameters for the factorization
	mf_size_type size1 = 100; //10000;
	mf_size_type size2 = 100; //10000;
	mf_size_type nnz = 50; //5000000;
	mf_size_type r = 2; // 10

	// parameters for GNMF
	unsigned epochs = 1600;
	mf_size_type testNnz = nnz/100;

	BalanceType type = BALANCE_L2;;
	BalanceMethod method = BALANCE_OPTIMAL;

	// generate original factors by sampling from a uniform[0,1] distribution
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, r);
	DenseMatrixCM hIn(r, size2);
	generateRandom(wIn, random, boost::uniform_real<>(0,1));
	generateRandom(hIn, random, boost::uniform_real<>(0,1));

	// clear one row
	for (mf_size_type k=0; k<r; k++) {
		wIn(0,k) = 0;
	}

	// generate a sparse matrix by selecting random entries from the generated factors
	// and add small Gaussian noise
	SparseMatrix v;
	generateRandom(v, nnz, wIn, hIn, random);
	//addRandom(v, random, boost::normal_distribution<>(0, 0.1));
	LOG4CXX_INFO(logger, "Data matrix: "
		<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
	v.sort();
	//LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));
	SparseMatrixCM vc;
	copyCm(v, vc);

	// create a test matrix (without noise)
	SparseMatrix vTest;
	generateRandom(vTest, testNnz, wIn, hIn, random);
	LOG4CXX_INFO(logger, "Test matrix: "
		<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

	// generate initial factors by sampling from a uniform[0,1] distribution
	DenseMatrix w(size1, r);
	DenseMatrixCM h(r, size2);
	generateRandom(w, random, boost::uniform_real<>(0,1));
	LOG4CXX_INFO(logger, "Row factors: " << w.size1() << " x " << w.size2());
	generateRandom(h, random, boost::uniform_real<>(0,1));
	LOG4CXX_INFO(logger, "Column factors: " << h.size1() << " x " << h.size2());

	// initialize
	FactorizationData<> data(v, w, h, 1, &vc);
	FactorizationData<> testJob(vTest,w,h);
	Trace trace;
	Timer t;

	LOG4CXX_INFO(logger, "Start");
	// run GNMF to try to reconstruct the original factors
	t.start();
	gnmf(data, epochs, trace, type, method, &testJob);
	t.stop();
	LOG4CXX_INFO(logger, "Total time: " << t);

	// write trace to an R file
	LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/gnmf-trace.R");
	trace.toRfile("/tmp/gnmf-trace.R", "gnmf");

	return 0;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef std::complex<double> complex;
  typedef ublas::vector<complex> vector;
  typedef ublas::matrix<complex, ublas::column_major> matrix;
  typedef typename vector::size_type size_type;

  rand_normal<complex>::reset();
  int n=128;
  // generate a random unitary matrix
  matrix U(n, n);
  for (size_type j=0; j<n; ++j)
    for (size_type i=0; i<n; ++i)
      U(i, j)=i==j ? complex(1) : complex(0);
  for (int k=0; k<n*n; ++k) {
    // generate a random 2x2 unitary matrix
    double phi(rand_uniform<double>::get(0, 1.5707963267948966192));
    double alpha(rand_uniform<double>::get(0, 6.2831853071795864770));
    double psi(rand_uniform<double>::get(0, 6.2831853071795864770));
    double chi(rand_uniform<double>::get(0, 6.2831853071795864770));
    matrix u(2, 2);
    u(0, 0)=complex(std::cos(alpha+psi), std::sin(alpha+psi))*std::cos(phi);
    u(1, 0)=-complex(std::cos(alpha-chi), std::sin(alpha-chi))*std::sin(phi);
    u(0, 1)=complex(std::cos(alpha+chi), std::sin(alpha+chi))*std::sin(phi);
    u(1, 1)=complex(std::cos(alpha-psi), std::sin(alpha-psi))*std::cos(phi);
    int j0, j1;
    j0=static_cast<int>(rand_uniform<double>::get(0, n));
    do {
      j1=static_cast<int>(rand_uniform<double>::get(0, n));
    } while (j0==j1);
    for (size_type i=0; i<n; ++i) {
      vector Uc(2);
      Uc(0)=U(j0, i);
      Uc(1)=U(j1, i);
      Uc=ublas::prod(u, Uc);
      U(j0, i)=Uc(0);
      U(j1, i)=Uc(1);
    }
  }
  matrix R(ublas::prod(ublas::trans(ublas::conj(U)), U));
  // generate random positive definite hermitian matrix
  matrix A(n, n);
  for (size_type j=0; j<n; ++j)
    for (size_type i=0; i<n; ++i)
      if (i==j)
	// eigenvalues drawn from the positive half of the normal distribution
	do {
	  A(i, j)=std::abs(rand_normal<complex>::get());
	} while (A(i, j)==complex(0));
      else
	A(i, j)=complex(0);
  // apply a random unitary transform to A
  A=ublas::prod(ublas::trans(ublas::conj(U)), A);
  A=ublas::prod(A, U);
  matrix A_bak(A);
  vector b(n);
  for (size_type i=0; i<n; ++i)
    b(i)=rand_normal<complex>::get();
  vector x(b);
  int info=lapack::posv(lapack::upper(A), x); // solve
  if (info==0) {
    // res <- A*x - b
    vector res(b);
    blas::gemv(complex(1, 0), A_bak, x, complex(-1, 0), res);
    std::cout << "norm of residual : " << blas::nrm2(res) << '\n';
  } else
    if (info>0)
      std::cout << "singular matrix\n";
    else
      std::cout << "illegal arguments\n";
  return EXIT_SUCCESS;
}


/* Author: Wolfgang Bangerth, University of Heidelberg, 1999 */

/*    $Id: step-2.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 1999-2003, 2006, 2008-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// The first few includes are just like in the previous program, so do not
// require additional comments:
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_boundary_lib.h>

// However, the next file is new. We need this include file for the
// association of degrees of freedom ("DoF"s) to vertices, lines, and cells:
#include <deal.II/dofs/dof_handler.h>

// The following include contains the description of the bilinear finite
// element, including the facts that it has one degree of freedom on each
// vertex of the triangulation, but none on faces and none in the interior of
// the cells.
//
// (In fact, the file contains the description of Lagrange elements in
// general, i.e. also the quadratic, cubic, etc versions, and not only for 2d
// but also 1d and 3d.)
#include <deal.II/fe/fe_q.h>
// In the following file, several tools for manipulating degrees of freedom
// can be found:
#include <deal.II/dofs/dof_tools.h>
// We will use a sparse matrix to visualize the pattern of nonzero entries
// resulting from the distribution of degrees of freedom on the grid. That
// class can be found here:
#include <deal.II/lac/sparse_matrix.h>
// We will also need to use an intermediate sparsity patter structure, which
// is found in this file:
#include <deal.II/lac/compressed_sparsity_pattern.h>

// We will want to use a special algorithm to renumber degrees of freedom. It
// is declared here:
#include <deal.II/dofs/dof_renumbering.h>

// And this is again needed for C++ output:
#include <fstream>

// Finally, as in step-1, we import the deal.II namespace into the global
// scope:
using namespace dealii;

// @sect3{Mesh generation}

// This is the function that produced the circular grid in the previous step-1
// example program. The sole difference is that it returns the grid it
// produces via its argument.
//
// The details of what the function does are explained in step-1. The only
// thing we would like to comment on is this:
//
// Since we want to export the triangulation through this function's
// parameter, we need to make sure that the boundary object lives at least as
// long as the triangulation does. However, in step-1, the boundary object is
// a local variable, and it would be deleted at the end of the function, which
// is too early. We avoid the problem by declaring it 'static' which makes
// sure that the object is initialized the first time control the program
// passes this point, but at the same time assures that it lives until the end
// of the program.
void make_grid (Triangulation<2> &triangulation)
{
  const Point<2> center (1,0);
  const double inner_radius = 0.5,
               outer_radius = 1.0;
  GridGenerator::hyper_shell (triangulation,
                              center, inner_radius, outer_radius,
                              10);

  static const HyperShellBoundary<2> boundary_description(center);
  triangulation.set_boundary (0, boundary_description);

  for (unsigned int step=0; step<5; ++step)
    {
      Triangulation<2>::active_cell_iterator
      cell = triangulation.begin_active(),
      endc = triangulation.end();

      for (; cell!=endc; ++cell)
        for (unsigned int v=0;
             v < GeometryInfo<2>::vertices_per_cell;
             ++v)
          {
            const double distance_from_center
              = center.distance (cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) < 1e-10)
              {
                cell->set_refine_flag ();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement ();
    }
}

// @sect3{Creation of a DoFHandler}

// Up to now, we only have a grid, i.e. some geometrical (the position of the
// vertices) and some topological information (how vertices are connected to
// lines, and lines to cells, as well as which cells neighbor which other
// cells). To use numerical algorithms, one needs some logic information in
// addition to that: we would like to associate degree of freedom numbers to
// each vertex (or line, or cell, in case we were using higher order elements)
// to later generate matrices and vectors which describe a finite element
// field on the triangulation.
//
// This function shows how to do this. The object to consider is the
// DoFHandler class template.  Before we do so, however, we first
// need something that describes how many degrees of freedom are to be
// associated to each of these objects. Since this is one aspect of the
// definition of a finite element space, the finite element base class stores
// this information. In the present context, we therefore create an object of
// the derived class FE_Q that describes Lagrange elements. Its
// constructor takes one argument that states the polynomial degree of the
// element, which here is one (indicating a bi-linear element); this then
// corresponds to one degree of freedom for each vertex, while there are none
// on lines and inside the quadrilateral. A value of, say, three given to the
// constructor would instead give us a bi-cubic element with one degree of
// freedom per vertex, two per line, and four inside the cell. In general,
// FE_Q denotes the family of continuous elements with complete
// polynomials (i.e. tensor-product polynomials) up to the specified order.
//
// We first need to create an object of this class and then pass it on to the
// DoFHandler object to allocate storage for the degrees of
// freedom (in deal.II lingo: we distribute degrees of
// freedom). Note that the DoFHandler object will store a reference to
// this finite element object, so we have to make sure its lifetime is at
// least as long as that of the DoFHandler; one way to make sure
// this is so is to make it static as well, in order to prevent its preemptive
// destruction. (However, the library would warn us if we forgot about this
// and abort the program if that occured. You can check this, if you want, by
// removing the 'static' declaration.)
void distribute_dofs (DoFHandler<2> &dof_handler)
{
  // As described above, let us first create a finite element object, and then
  // use it to allocate degrees of freedom on the triangulation with which the
  // dof_handler object is associated:
  static const FE_Q<2> finite_element(1);
  dof_handler.distribute_dofs (finite_element);

  // Now that we have associated a degree of freedom with a global number to
  // each vertex, we wonder how to visualize this?  There is no simple way to
  // directly visualize the DoF number associated with each vertex. However,
  // such information would hardly ever be truly important, since the
  // numbering itself is more or less arbitrary. There are more important
  // factors, of which we will demonstrate one in the following.
  //
  // Associated with each vertex of the triangulation is a shape
  // function. Assume we want to solve something like Laplace's equation, then
  // the different matrix entries will be the integrals over the gradient of
  // each pair of such shape functions. Obviously, since the shape functions
  // are nonzero only on the cells adjacent to the vertex they are associated
  // with, matrix entries will be nonzero only if the supports of the shape
  // functions associated to that column and row %numbers intersect. This is
  // only the case for adjacent shape functions, and therefore only for
  // adjacent vertices. Now, since the vertices are numbered more or less
  // randomly by the above function (DoFHandler::distribute_dofs), the pattern
  // of nonzero entries in the matrix will be somewhat ragged, and we will
  // take a look at it now.
  //
  // First we have to create a structure which we use to store the places of
  // nonzero elements. This can then later be used by one or more sparse
  // matrix objects that store the values of the entries in the locations
  // stored by this sparsity pattern. The class that stores the locations is
  // the SparsityPattern class. As it turns out, however, this class has some
  // drawbacks when we try to fill it right away: its data structures are set
  // up in such a way that we need to have an estimate for the maximal number
  // of entries we may wish to have in each row. In two space dimensions,
  // reasonable values for this estimate are available through the
  // DoFHandler::max_couplings_between_dofs() function, but in three
  // dimensions the function almost always severely overestimates the true
  // number, leading to a lot of wasted memory, sometimes too much for the
  // machine used, even if the unused memory can be released immediately after
  // computing the sparsity pattern. In order to avoid this, we use an
  // intermediate object of type CompressedSparsityPattern that uses a
  // different %internal data structure and that we can later copy into the
  // SparsityPattern object without much overhead. (Some more information on
  // these data structures can be found in the @ref Sparsity module.) In order
  // to initialize this intermediate data structure, we have to give it the
  // size of the matrix, which in our case will be square with as many rows
  // and columns as there are degrees of freedom on the grid:
  CompressedSparsityPattern compressed_sparsity_pattern(dof_handler.n_dofs(),
                                                        dof_handler.n_dofs());

  // We then fill this object with the places where nonzero elements will be
  // located given the present numbering of degrees of freedom:
  DoFTools::make_sparsity_pattern (dof_handler, compressed_sparsity_pattern);

  // Now we are ready to create the actual sparsity pattern that we could
  // later use for our matrix. It will just contain the data already assembled
  // in the CompressedSparsityPattern.
  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from (compressed_sparsity_pattern);

  // With this, we can now write the results to a file:
  std::ofstream out ("sparsity_pattern.1");
  sparsity_pattern.print_gnuplot (out);
  // The result is in GNUPLOT format, where in each line of the output file,
  // the coordinates of one nonzero entry are listed. The output will be shown
  // below.
  //
  // If you look at it, you will note that the sparsity pattern is
  // symmetric. This should not come as a surprise, since we have not given
  // the DoFTools::make_sparsity_pattern any information that
  // would indicate that our bilinear form may couple shape functions in a
  // non-symmetric way. You will also note that it has several distinct
  // region, which stem from the fact that the numbering starts from the
  // coarsest cells and moves on to the finer ones; since they are all
  // distributed symmetrically around the origin, this shows up again in the
  // sparsity pattern.
}

// @sect3{Renumbering of DoFs}

// In the sparsity pattern produced above, the nonzero entries extended quite
// far off from the diagonal. For some algorithms, for example for incomplete
// LU decompositions or Gauss-Seidel preconditioners, this is unfavorable, and
// we will show a simple way how to improve this situation.
//
// Remember that for an entry $(i,j)$ in the matrix to be nonzero, the
// supports of the shape functions i and j needed to intersect (otherwise in
// the integral, the integrand would be zero everywhere since either the one
// or the other shape function is zero at some point). However, the supports
// of shape functions intersected only if they were adjacent to each other, so
// in order to have the nonzero entries clustered around the diagonal (where
// $i$ equals $j$), we would like to have adjacent shape functions to be
// numbered with indices (DoF numbers) that differ not too much.
//
// This can be accomplished by a simple front marching algorithm, where one
// starts at a given vertex and gives it the index zero. Then, its neighbors
// are numbered successively, making their indices close to the original
// one. Then, their neighbors, if not yet numbered, are numbered, and so on.
//
// One algorithm that adds a little bit of sophistication along these lines is
// the one by Cuthill and McKee. We will use it in the following function to
// renumber the degrees of freedom such that the resulting sparsity pattern is
// more localized around the diagonal. The only interesting part of the
// function is the first call to DoFRenumbering::Cuthill_McKee,
// the rest is essentially as before:
void renumber_dofs (DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee (dof_handler);

  CompressedSparsityPattern compressed_sparsity_pattern(dof_handler.n_dofs(),
                                                        dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern (dof_handler, compressed_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from (compressed_sparsity_pattern);

  std::ofstream out ("sparsity_pattern.2");
  sparsity_pattern.print_gnuplot (out);
}

// Again, the output is shown below. Note that the nonzero entries are
// clustered far better around the diagonal than before. This effect is even
// more distinguished for larger matrices (the present one has 1260 rows and
// columns, but large matrices often have several 100,000s).

// It is worth noting that the DoFRenumbering class offers a
// number of other algorithms as well to renumber degrees of freedom. For
// example, it would of course be ideal if all couplings were in the lower or
// upper triangular part of a matrix, since then solving the linear system
// would among to only forward or backward substitution. This is of course
// unachievable for symmetric sparsity patterns, but in some special
// situations involving transport equations, this is possible by enumerating
// degrees of freedom from the inflow boundary along streamlines to the
// outflow boundary. Not surprisingly, DoFRenumbering also has
// algorithms for this.

// @sect3{The main function}

// Finally, this is the main program. The only thing it does is to allocate
// and create the triangulation, then create a DoFHandler object
// and associate it to the triangulation, and finally call above two functions
// on it:
int main ()
{
  Triangulation<2> triangulation;
  make_grid (triangulation);

  DoFHandler<2> dof_handler (triangulation);

  distribute_dofs (dof_handler);
  renumber_dofs (dof_handler);
}


// Copyright (c) 2019 Vsevolod Vlaskine

/// @author vsevolod vlaskine

#include <iostream>
#include <sstream>
#include <type_traits>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/version.hpp>
#if defined( CV_VERSION_EPOCH ) && CV_VERSION_EPOCH == 2
#include <opencv2/calib3d/calib3d.hpp>
#else
#include <opencv2/calib3d.hpp>
#endif
#include <tbb/parallel_for.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/tuple/tuple.hpp>
#include <comma/base/exception.h>
#include <comma/csv/ascii.h>
#include <comma/visiting/traits.h>
#include "../../../math/range_bearing_elevation.h"
#include "../../../visiting/eigen.h"
#include "../../equirectangular.h"
#include "equirectangular_map.h"

namespace snark { namespace cv_calc { namespace equirectangular_map {

static cv::Mat rotation_matrix( double x,double y,double z ) // quick and dirty
{
    cv::Mat r_x = ( cv::Mat_<double>(3,3) <<
        1,            0,            0,
        0,  std::cos(x), -std::sin(x),
        0,  std::sin(x),  std::cos(x) );
    cv::Mat r_y = ( cv::Mat_<double>(3,3) <<
        std::cos(y),    0,  std::sin(y),
                  0,    1,            0,
       -std::sin(y),    0,  std::cos(y) );
    cv::Mat r_z = ( cv::Mat_<double>(3,3) <<
        std::cos(z), -std::sin(z),      0,
        std::sin(z),  std::cos(z),      0,
                  0,            0,      1 );
    return r_z * r_y * r_x;
}

// see: inverse formula for spherical projection, Szeliski, "Computer Vision: Algorithms and Applications" p439.
struct calculator
{
    calculator( const Eigen::Vector3d& orientation, cv::Mat K, unsigned int w, unsigned int h )
        : RK( rotation_matrix( orientation.y(), orientation.z(), orientation.x() ) * K.inv() ) // todo: validate euler angle order
        , w( w )
        , h( h )
    {
        xyz = ( cv::Mat_< double >( 3, 1 ) << 0, 0, 1 );
    }

    std::pair< double, double > projected_pixel( unsigned int x, unsigned int y )
    {
        xyz.at< double >( 0, 0 ) = x;
        xyz.at< double >( 1, 0 ) = y;
        cv::Mat ray3d = RK * ( xyz / cv::norm( xyz ) );
        double xp = ray3d.at< double >( 0, 0 );
        double yp = ray3d.at< double >( 0, 1 );
        double zp = ray3d.at< double >( 0, 2 );
        double phi = std::atan2( xp, zp );
        double sx = ( phi / ( M_PI * 2 ) + 0.5 ) * w;
        if( sx >= w - 0.00001 ) { sx = 0; }
        double theta = std::atan2( yp, std::sqrt( xp * xp + zp * zp ) );
        double sy = ( theta / M_PI + 0.5 ) * h;
        return std::make_pair( sx, sy );
    };

    cv::Mat RK;
    cv::Mat xyz;
    unsigned int w;
    unsigned int h;
};

std::string options()
{
    std::ostringstream oss;
    oss << "        --cubes,--cube-size=[<width>]:" << std::endl;
    oss << "            direct: output map width for top,back,left,front,right,bottom cubes with a given original orientation" << std::endl;
    oss << "            reverse: cube size" << std::endl;
    oss << "        --focal-length=<pixels>; camera focal length" << std::endl;
    oss << "        --map-size,--size=[<width>,<height>]; output map size" << std::endl;
    oss << "        --orientation=<roll>,<pitch>,<yaw>; default=0,0,0; orientation in radians" << std::endl;
    oss << "        --reverse,--from-cubes; generate reverse map, i.e. mapping cubes to spherical images; if no --cube-size given, spherical width / 4 used" << std::endl;
    oss << "        --spherical-size=<width>[,<height>]; spherical image size" << std::endl;
    oss << std::endl;
    return oss.str();
}

int run( const comma::command_line_options& options )
{
    static_assert( sizeof( float ) == 4, "expected float of size 4" );
    options.assert_mutually_exclusive( "--reverse", "--focal-length,--map-size,--size,--orientation" );
    unsigned int spherical_width, spherical_height; // todo? make doubles?
    const auto& s = comma::split( options.value< std::string >( "--spherical-size" ), ',' );
    switch( s.size() )
    {
        case 1: spherical_width = boost::lexical_cast< unsigned int >( s[0] ); spherical_height = spherical_width / 2; break;
        case 2: spherical_width = boost::lexical_cast< unsigned int >( s[0] ); spherical_height = boost::lexical_cast< unsigned int >( s[1] ); break;
        default: std::cerr << "cv-calc: equirectangular-map: expected spherical size as <width>[,<height>]; got '" << comma::join( s, ',' ) << "'" << std::endl; return 1;
    }
    if( spherical_width != spherical_height * 2 ) { std::cerr << "cv-calc: equirectangular-map: expected spherical height half of width; got width: " << spherical_width << " height: " << spherical_height << std::endl; }
    if( options.exists( "--reverse" ) )
    {
        unsigned int cube_size = options.value< unsigned int >( "--cubes,--cube-size", spherical_width / 4 ); // quick and dirty
        cv::Mat x( spherical_height, spherical_width, CV_32F );
        cv::Mat y( spherical_height, spherical_width, CV_32F );
        tbb::parallel_for( tbb::blocked_range< std::size_t >( 0, spherical_height ), [&]( const tbb::blocked_range< std::size_t >& r )
        {
            for( unsigned int v = r.begin(); v < r.end(); ++v )
            {
                for( unsigned int u = 0; u < spherical_width; ++u )
                {
                    auto pixel = snark::equirectangular::to_cube( Eigen::Vector2d( u, v ), spherical_width );
                    pixel.first.y() += pixel.second; // quick and dirty
                    pixel.first *= cube_size;
                    x.at< float >( v, u ) = pixel.first.x();
                    y.at< float >( v, u ) = pixel.first.y();
                }
            }
        } );
        std::cout.write( reinterpret_cast< const char* >( x.datastart ), x.dataend - x.datastart );
        std::cout.write( reinterpret_cast< const char* >( y.datastart ), y.dataend - y.datastart );
        std::cout.flush();
    }
    else // todo? reimplement using equirectangular::... methods, which potentially may speed it up 2-3 times
    {
        options.assert_mutually_exclusive( "--focal-length", "--cubes,--cube-size" ); // quick and dirty for now
        auto focal_length = options.optional< double >( "--focal-length" );
        unsigned int map_width, map_height;
        bool cubes = options.exists( "--cubes,--cube-size" );
        if( cubes ) { map_width = map_height = options.value< unsigned int >( "--cubes" ); }
        else { boost::tie( map_width, map_height ) = comma::csv::ascii< std::pair< unsigned int, unsigned int > >().get( options.value< std::string >( "--map-size,--size" ) ); }
        if( !focal_length )
        {
            if( map_width != map_height ) { std::cerr << "cv-calc: equirectangular-map: please specify --focal-length" << std::endl; return 1; }
            focal_length = map_width / 2;
        }
        cv::Mat camera = ( cv::Mat_< double >( 3, 3 ) << *focal_length,             0,  map_width / 2,
                                                                        0, *focal_length, map_height / 2,
                                                                        0,             0,              1 );
        auto orientation = comma::csv::ascii< Eigen::Vector3d >().get( options.value< std::string >( "--orientation", "0,0,0" ) );
        auto make_map = [&]( const Eigen::Vector3d& o ) -> std::pair< cv::Mat, cv::Mat >
        {
            cv::Mat x( map_height, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
            cv::Mat y( map_height, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
            tbb::parallel_for( tbb::blocked_range< std::size_t >( 0, map_height ), [&]( const tbb::blocked_range< std::size_t >& r )
            {
                calculator calc( o, camera, spherical_width, spherical_height );
                for( unsigned int v = r.begin(); v < r.end(); ++v )
                {
                    for( unsigned int u = 0; u < map_width; ++u )
                    {
                        boost::tie( x.at< float >( v, u ), y.at< float >( v, u ) ) = calc.projected_pixel( u, v );
                    }
                }
            } );
            return std::make_pair( x, y );
        };
        auto face = make_map( orientation );
        if( !cubes )
        {
            std::cout.write( reinterpret_cast< const char* >( face.first.datastart ), face.first.dataend - face.first.datastart );
            std::cout.write( reinterpret_cast< const char* >( face.second.datastart ), face.second.dataend - face.second.datastart );
            std::cout.flush();
            return 0;
        }
        if( orientation != Eigen::Vector3d::Zero() ) { std::cerr << "cv-calc: equirectangular-map: for --cubes, expected --orientation 0,0,0; got: " << options.value< std::string >( "--orientation" ) << " (not supported)" << std::endl; return 1; }
        cv::Mat x( map_height * 6, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
        cv::Mat y( map_height * 6, map_width, CV_32F ); // quick and dirty; if output to stdout has problems, use serialisation class
        tbb::parallel_for( tbb::blocked_range< std::size_t >( 0, 6 ), [&]( const tbb::blocked_range< std::size_t >& r )
        {
            for( unsigned int face = r.begin(); face < r.end(); ++face )
            {
                unsigned int offset = face * map_width;
                for( unsigned int v = 0; v < map_width; ++v )
                {
                    for( unsigned int u = 0; u < map_width; ++u )
                    {
                        auto p = snark::equirectangular::from_cube( Eigen::Vector2d( u, v ) / map_width
                                                                  , static_cast< equirectangular::cube::faces::values >( face ) ) * spherical_width;
                        x.at< float >( v + offset, u ) = p.x();
                        y.at< float >( v + offset, u ) = p.y();
                    }
                }
            }
        } );
        std::cout.write( reinterpret_cast< const char* >( x.datastart ), x.dataend - x.datastart );
        std::cout.write( reinterpret_cast< const char* >( y.datastart ), y.dataend - y.datastart );
    }
//     auto top = make_map( Eigen::Vector3d( 0, M_PI / 2, orientation.z() ) );
//     auto bottom = make_map( Eigen::Vector3d( 0, -M_PI / 2, orientation.z() ) );
//     std::cout.write( reinterpret_cast< const char* >( top.first.datastart ), top.first.dataend - top.first.datastart );
//     int width = spherical_width / 4;
//     for( int i = -2; i < 2; ++i )
//     {
//         cv::Mat f = face.first + width * i; // todo? waste to allocate it each time (currently, the only reason for it is the back face
//         if( i == -2 ) { cv::Mat( f, cv::Rect( 0, 0, f.cols / 2, f.rows ) ) += spherical_width; }
//         std::cout.write( reinterpret_cast< const char* >( f.datastart ), f.dataend - f.datastart );
//     }
//     std::cout.write( reinterpret_cast< const char* >( bottom.first.datastart ), bottom.first.dataend - bottom.first.datastart );
//     std::cout.write( reinterpret_cast< const char* >( top.second.datastart ), top.second.dataend - top.second.datastart );
//     for( unsigned int i = 0; i < 4; ++i ) { std::cout.write( reinterpret_cast< const char* >( face.second.datastart ), face.second.dataend - face.second.datastart ); }
//     std::cout.write( reinterpret_cast< const char* >( bottom.second.datastart ), bottom.second.dataend - bottom.second.datastart );
//     std::cout.flush();
    return 0;
}

} } } // namespace snark { namespace cv_calc { namespace equirectangular_map {


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <Core/Algorithms/Math/ComputeTensorUncertaintyAlgorithm.h>
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Datatypes/Dyadic3DTensor.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/Mesh/MeshFacade.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>
#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/GeometryPrimitives/Point.h>
#include <boost/tuple/tuple.hpp>
#include <unsupported/Eigen/MatrixFunctions>

using namespace SCIRun;
using namespace Core;
using namespace Core::Datatypes;
using namespace Core::Algorithms;
using namespace Core::Algorithms::Math;
using namespace Core::Geometry;

AlgorithmOutputName ComputeTensorUncertaintyAlgorithm::MeanTensorField("MeanTensorField");
AlgorithmOutputName ComputeTensorUncertaintyAlgorithm::CovarianceMatrix("CovarianceMatrix");

ALGORITHM_PARAMETER_DEF(Math, MeanInvariantMethod);
ALGORITHM_PARAMETER_DEF(Math, MeanOrientationMethod);

// TODO move helpers
Dyadic3DTensor scirunTensorToEigenTensor(const Geometry::Tensor& t)
{
  Dyadic3DTensor newTensor(t.xx(), t.xy(), t.xz(), t.yy(), t.yz(), t.zz());
  return newTensor;
}

Tensor eigenTensorToScirunTensor(const Dyadic3DTensor& t)
{
  return Tensor(t(0, 0), t(1, 0), t(2, 0), t(1, 1), t(2, 1), t(2, 2));
}

// TODO move to separate algo
enum FieldDataType
{ node, edge, face, cell };

class ComputeTensorUncertaintyAlgorithmImpl
{
public:
  ComputeTensorUncertaintyAlgorithmImpl();
  void run(const FieldList& fields);
  void setInvariantMethod(AlgoOption method);
  void setOrientationMethod(AlgoOption method);
  void getPoints(const FieldList& fields);
  void getPointsForFields(FieldHandle field, std::vector<int>& indices, std::vector<Point>& points);
  FieldHandle getMeanTensors() const;
  MatrixHandle getCovarianceMatrices() const;
private:
  size_t fieldCount_ = 0;
  size_t fieldSize_ = 0;
  std::vector<std::vector<Point>> points_;
  std::vector<std::vector<int>> indices_;
  std::vector<std::vector<Dyadic3DTensor>> tensors_;
  std::vector<Dyadic3DTensor> meanTensors_;
  std::vector<DyadicTensor<6>> covarianceMatrices_;
  AlgoOption invariantMethod_;
  AlgoOption orientationMethod_;

  void verifyData(const FieldList& fields);
  void getTensors(const FieldList& fields);
  void computeCovarianceMatrices();
  void computeMeanTensors();
  Dyadic3DTensor computeMeanMatrixAverage(int index) const;
  Dyadic3DTensor computeMeanLinearInvariant(int t) const;
  Dyadic3DTensor computeMeanLogEuclidean(int t) const;
  void computeMeanTensorsSameMethod();
  void computeMeanTensorsDifferentMethod();
};

void ComputeTensorUncertaintyAlgorithmImpl::setInvariantMethod(AlgoOption method)
{
  invariantMethod_ = method;
}

void ComputeTensorUncertaintyAlgorithmImpl::setOrientationMethod(AlgoOption method)
{
  orientationMethod_ = method;
}

FieldHandle ComputeTensorUncertaintyAlgorithmImpl::getMeanTensors() const
{
  FieldInformation ofinfo("PointCloudMesh", 0, "Tensor");
  auto ofield = CreateField(ofinfo);
  auto mesh = ofield->vmesh();
  auto field = ofield->vfield();

  std::vector<VMesh::index_type> meshIndices(fieldSize_);
  for (size_t i = 0; i < fieldSize_; ++i)
    meshIndices[i] = mesh->add_point(points_[0][i]);

  field->resize_fdata();

  for (size_t i = 0; i < fieldSize_; ++i)
    field->set_value(eigenTensorToScirunTensor(meanTensors_[i]), meshIndices[i]);

  return ofield;
}

void ComputeTensorUncertaintyAlgorithmImpl::computeMeanTensorsSameMethod()
{
  if (orientationMethod_.option_ == "Matrix Average")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      meanTensors_[t] = computeMeanMatrixAverage(t);
  }
  else if (orientationMethod_.option_ == "Log-Euclidean")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      meanTensors_[t] = computeMeanLogEuclidean(t);
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::computeMeanTensorsDifferentMethod()
{
  std::vector<Dyadic3DTensor> orientationTensors(fieldSize_);
  std::vector<Dyadic3DTensor> invariantTensors(fieldSize_);
  if (orientationMethod_.option_ == "Matrix Average")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      orientationTensors[t] = computeMeanMatrixAverage(t);
  }
  else if (orientationMethod_.option_ == "Log-Euclidean")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      orientationTensors[t] = computeMeanLogEuclidean(t);
  }

  if (invariantMethod_.option_ == "Matrix Average")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      invariantTensors[t] = computeMeanMatrixAverage(t);
  }
  else if (invariantMethod_.option_ == "Log-Euclidean")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      invariantTensors[t] = computeMeanLogEuclidean(t);
  }
  else if (invariantMethod_.option_ == "Linear Invariant")
  {
    for (size_t t = 0; t < fieldSize_; ++t)
      invariantTensors[t] = computeMeanLinearInvariant(t);
  }

  for (size_t t = 0; t < fieldSize_; ++t)
  {
    invariantTensors[t].setDescendingRHSOrder();
    orientationTensors[t].setDescendingRHSOrder();
    meanTensors_[t] = Dyadic3DTensor(orientationTensors[t].getEigenvectors(),
                                     invariantTensors[t].getEigenvalues());
    meanTensors_[t].setDescendingRHSOrder();
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::computeMeanTensors()
{
  meanTensors_ = std::vector<Dyadic3DTensor>(fieldSize_);
  if (invariantMethod_.option_ == orientationMethod_.option_)
    computeMeanTensorsSameMethod();
  else
    computeMeanTensorsDifferentMethod();
}

Dyadic3DTensor ComputeTensorUncertaintyAlgorithmImpl::computeMeanMatrixAverage(int t) const
{
  Dyadic3DTensor sum = Dyadic3DTensor();
  for (size_t f = 0; f < fieldCount_; ++f) {
    sum += tensors_[f][t];
  }
  sum = sum / (double)fieldCount_;

  return sum;
}

// Mean calculation using Linear Invariant interpolation
Dyadic3DTensor ComputeTensorUncertaintyAlgorithmImpl::computeMeanLinearInvariant(int t) const
{
  const static double oneThird = 1.0 / 3.0;
  const static double sqrtThreeHalves = sqrt(1.5);
  const static double threeSqrtSix = 3.0 * sqrt(6.0);

  const static Dyadic3DTensor identity = Dyadic3DTensor(Eigen::Vector3d(1, 0, 0),
                                                        Eigen::Vector3d(0, 1, 0),
                                                        Eigen::Vector3d(0, 0, 1));
  const static Dyadic3DTensor identityThird = oneThird * identity;

  double K1 = 0.0;
  double R2 = 0.0;
  double R3 = 0.0;

  for (size_t f = 0; f < fieldCount_; ++f)
  {
    const double trace = tensors_[f][t].trace();
    Dyadic3DTensor trThird = trace * identityThird;
    K1 += trace;

    const double fro = tensors_[f][t].frobeniusNorm();
    Dyadic3DTensor anisotropicDeviation = tensors_[f][t] - trThird;
    anisotropicDeviation.setDescendingRHSOrder();
    const double anisotropicDeviationFro = anisotropicDeviation.frobeniusNorm();

    R2 += sqrtThreeHalves * anisotropicDeviationFro / fro;
    R3 += threeSqrtSix * (anisotropicDeviation / anisotropicDeviationFro).asMatrix().determinant();
  }

  // Equally weight all of the coeffecients
  K1 /= static_cast<double>(fieldCount_);
  R2 /= static_cast<double>(fieldCount_);
  R3 /= static_cast<double>(fieldCount_);

  // Clamp to avoid nan results with acos
  if (R3 > 1.0) R3 = 1.0;
  if (R3 < -1.0) R3 = -1.0;

  const double arccosR3 = std::acos(R3);
  Eigen::Vector3d eigvals;
  const double x = oneThird * K1;
  const double y = (2.0 * K1 * R2) / (3.0 * sqrt(3.0 - 2.0 * std::pow(R2, 2.0)));
  eigvals(0) = x + y * std::cos(arccosR3 * oneThird);
  eigvals(1) = x + y * std::cos((arccosR3 - 2.0 * M_PI) * oneThird);
  eigvals(2) = x + y * std::cos((arccosR3 + 2.0 * M_PI) * oneThird);

  // Using axis aligned orientation because this mean method is only for invariants
  Dyadic3DTensor ret = Dyadic3DTensor({Eigen::Vector3d({1,0,0}), Eigen::Vector3d({0,1,0}), Eigen::Vector3d({0,0,1})}, eigvals);
  return ret;
}

Dyadic3DTensor ComputeTensorUncertaintyAlgorithmImpl::computeMeanLogEuclidean(int t) const
{
  Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
  for (size_t f = 0; f < fieldCount_; ++f)
  {
    Dyadic3DTensor tensor = tensors_[f][t];
    tensor.setDescendingRHSOrder();
    Eigen::Vector3d eigvalsLog = tensor.getEigenvalues();
    Eigen::Matrix3d rotation = tensor.getEigenvectorsAsMatrix();
    for (size_t i = 0; i < eigvalsLog.size(); ++i)
      eigvalsLog[i] = std::log(eigvalsLog[i]);
    sum += rotation * eigvalsLog.asDiagonal() * rotation.transpose();
  }
  sum /= (double)fieldCount_;
  Eigen::Matrix3d mean = sum.exp();
  Dyadic3DTensor ret = Dyadic3DTensor(mean);
  return ret;
}

ComputeTensorUncertaintyAlgorithmImpl::ComputeTensorUncertaintyAlgorithmImpl()
{}

void ComputeTensorUncertaintyAlgorithmImpl::computeCovarianceMatrices()
{
  covarianceMatrices_ = std::vector<DyadicTensor<6>>(fieldSize_);

  Eigen::Matrix<double, 6, 6> covarianceMat;
  for (size_t t = 0; t < fieldSize_; ++t)
  {
    covarianceMat.fill(0.0);
    for (size_t f = 0; f < fieldCount_; ++f)
    {
      Eigen::Matrix<double, 6, 1> diffTensor = (tensors_[f][t] - meanTensors_[t]).mandel();
      covarianceMat += diffTensor * diffTensor.transpose();
    }
    covarianceMat /= static_cast<double>(fieldCount_);
    covarianceMatrices_[t] = DyadicTensor<6>(covarianceMat);
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::getPointsForFields(
    FieldHandle field, std::vector<int>& indices, std::vector<Point>& points)
{
  // Collect indices and points from facades
  FieldDataType fieldLocation;
  FieldInformation finfo(field);
  if (finfo.is_point() || finfo.is_linear())
    fieldLocation = FieldDataType::node;
  else if (finfo.is_line())
    fieldLocation = FieldDataType::edge;
  else if (finfo.is_surface())
    fieldLocation = FieldDataType::face;
  else
    fieldLocation = FieldDataType::cell;

  auto mesh = field->vmesh();
  auto primaryFacade = field->mesh()->getFacade();
  switch (fieldLocation)
  {
  case FieldDataType::node:
    for (const auto& node : primaryFacade->nodes())
    {
      indices.push_back(node.index());
      Point p;
      mesh->get_center(p, node.index());
      points.push_back(p);
    }
    break;
  case FieldDataType::edge:
    for (const auto& edge : primaryFacade->edges())
    {
      indices.push_back(edge.index());
      Point p;
      mesh->get_center(p, edge.index());
      points.push_back(p);
    }
    break;
  case FieldDataType::face:
    for (const auto& face : primaryFacade->faces())
    {
      indices.push_back(face.index());
      Point p;
      mesh->get_center(p, face.index());
      points.push_back(p);
    }
    break;
  case FieldDataType::cell:
    for (const auto& cell : primaryFacade->cells())
    {
      indices.push_back(cell.index());
      Point p;
      mesh->get_center(p, cell.index());
      points.push_back(p);
    }
    break;
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::getPoints(const FieldList& fields)
{
  fieldCount_ = fields.size();

  indices_ = std::vector<std::vector<int>>(fieldCount_);
  points_ = std::vector<std::vector<Point>>(fieldCount_);

  for (size_t f = 0; f < fieldCount_; ++f)
    getPointsForFields(fields[f], indices_[f], points_[f]);
}

void ComputeTensorUncertaintyAlgorithmImpl::verifyData(const FieldList& fields)
{
  fieldSize_ = indices_[0].size();
  for (size_t f = 1; f < fieldCount_; ++f)
    if (indices_[f].size() != fieldSize_)
      THROW_ALGORITHM_INPUT_ERROR_SIMPLE("All field inputs must have the same size.");

  // Verify all are tensors
  for (auto field : fields)
  {
    FieldInformation finfo(field);
    if (!finfo.is_tensor())
      THROW_ALGORITHM_INPUT_ERROR_SIMPLE("This module only supports tensor fields.");
  }
}

void ComputeTensorUncertaintyAlgorithmImpl::getTensors(const FieldList& fields)
{
  tensors_ = std::vector<std::vector<Dyadic3DTensor>>(fieldCount_);
  for (size_t f = 0; f < fieldCount_; ++f)
  {
    tensors_[f] = std::vector<Dyadic3DTensor>(fieldSize_);
    auto vfield = fields[f]->vfield();
    Tensor temp;
    for (size_t v = 0; v < fieldSize_; ++v)
    {
      vfield->get_value(temp, v);
      tensors_[f][v] = scirunTensorToEigenTensor(temp);
      tensors_[f][v].setDescendingRHSOrder();
    }
  }
}

MatrixHandle ComputeTensorUncertaintyAlgorithmImpl::getCovarianceMatrices() const
{
  auto m = std::make_shared<DenseMatrix>(21, fieldSize_);
  for (size_t i = 0; i < fieldSize_; ++i)
  {
    m->col(i) = covarianceMatrices_[i].mandel();
    Eigen::Matrix<double,21,1> recov_m = m->col(i);
    DyadicTensor<6> recov = DyadicTensor<6>(recov_m);
  }
  return m;
}

void ComputeTensorUncertaintyAlgorithmImpl::run(const FieldList& fields)
{
  getPoints(fields);
  verifyData(fields);
  getTensors(fields);
  computeMeanTensors();
  computeCovarianceMatrices();
}

ComputeTensorUncertaintyAlgorithm::ComputeTensorUncertaintyAlgorithm()
{
  addOption(Parameters::MeanInvariantMethod, "Linear Invariant",
            "Linear Invariant|Log-Euclidean|Matrix Average");
  addOption(Parameters::MeanOrientationMethod, "Log-Euclidean",
            "Log-Euclidean|Matrix Average");
}

AlgorithmOutput ComputeTensorUncertaintyAlgorithm::run(const AlgorithmInput& input) const
{
  auto fields = input.getList<Field>(Variables::InputFields);
  auto invariantMethod = get(Parameters::MeanInvariantMethod).toOption();
  auto orientMethod = get(Parameters::MeanOrientationMethod).toOption();
  auto data = runImpl(fields, invariantMethod, orientMethod);

  AlgorithmOutput output;
  output[MeanTensorField] = data.get<0>();
  output[CovarianceMatrix] = data.get<1>();
  return output;
}

boost::tuple<FieldHandle, MatrixHandle> ComputeTensorUncertaintyAlgorithm::runImpl(const FieldList& fields, const AlgoOption& invariantMethod, const AlgoOption& orientationOption) const
{
  if (fields.empty())
    THROW_ALGORITHM_INPUT_ERROR("No input fields given");

  auto impl = ComputeTensorUncertaintyAlgorithmImpl();
  impl.setInvariantMethod(invariantMethod);
  impl.setOrientationMethod(orientationOption);
  impl.run(fields);

  return boost::make_tuple(impl.getMeanTensors(), impl.getCovarianceMatrices());
}