// ===========================================================================
//
//       Filename:  FEM4EllipticPDE.cpp
//
//        Created:  08/16/12 18:19:57
//       Compiler:  Tested with g++, icpc, and MSVC 2010
//
//         Author:  Trevor Irons (ti)
//
//   Organisation:  Colorado School of Mines (CSM)
//                  United States Geological Survey (USGS)
//                  University of Utah (UU), 2016
//
//          Email:  tirons@egi.utah.edu
//
// ===========================================================================

/**
  @file
  @author   Trevor Irons
  @date     08/16/12
 **/

#include "FEM4EllipticPDE.h"

namespace Lemma {

    std::ostream &operator << (std::ostream &stream, const FEM4EllipticPDE &ob) {
        stream << ob.Serialize()  << "\n";
        return stream;
    }

	// ====================  LIFECYCLE     =======================

	FEM4EllipticPDE::FEM4EllipticPDE( const ctor_key& key ) :
			LemmaObject(key), BndryH(10), BndrySigma(10),
            vtkSigma(nullptr), vtkG(nullptr), vtkGrid(nullptr), gFcn3(nullptr) {
	}

    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  FEM4EllipticPDE
    // Description:  DeSerializing constructor (protected)
    //--------------------------------------------------------------------------------------
    FEM4EllipticPDE::FEM4EllipticPDE (const YAML::Node& node, const ctor_key& key) : LemmaObject(node, key) {
        // TODO impliment
        throw std::runtime_error("FEM4ELLIPTICPDE YAML CONSTRUCTOR NOT IMPLIMENTED!");
    }  // -----  end of method FEM4EllipticPDE::FEM4EllipticPDE  (constructor)  -----

	FEM4EllipticPDE::~FEM4EllipticPDE() {
	}

    std::shared_ptr<FEM4EllipticPDE> FEM4EllipticPDE::NewSP( ) {
        return std::make_shared<FEM4EllipticPDE>( ctor_key() );
    }

    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  Serialize
    //--------------------------------------------------------------------------------------
    YAML::Node  FEM4EllipticPDE::Serialize (  ) const {
        YAML::Node node = LemmaObject::Serialize();
        node.SetTag( this->GetName() );
        // node["thing"] = value;
        return node;
    }		// -----  end of method FEM4EllipticPDE::Serialize  -----

    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  DeSerialize
    //--------------------------------------------------------------------------------------
    std::shared_ptr<FEM4EllipticPDE> FEM4EllipticPDE::DeSerialize ( const YAML::Node& node  ) {
        if (node.Tag() != "FEM4EllipticPDE") {
            throw DeSerializeTypeMismatch( "FEM4EllipticPDE", node.Tag() );
        }
        return std::make_shared<FEM4EllipticPDE> (node, ctor_key());
    }		// -----  end of method FEM4EllipticPDE::DeSerialize  -----

    // ====================  OPERATIONS    =======================

    void FEM4EllipticPDE::SetSigmaFunction(vtkImplicitFunction* sigma) {
        vtkSigma = sigma;
    }

    void FEM4EllipticPDE::SetBoundaryStep(const Real& h) {
        BndryH = h;
    }

    void FEM4EllipticPDE::SetGFunction(vtkImplicitFunction* g) {
        vtkG = g;
    }

    void FEM4EllipticPDE::SetGFunction( Real (*gFcn)(const Real&, const Real&, const Real&) ) {
    //    vtkG = g;
        gFcn3 = gFcn;
    }

    void FEM4EllipticPDE::SetGrid(vtkUnstructuredGrid* grid) {
        vtkGrid = grid;
    }

    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  SetVTUGridFile
    //--------------------------------------------------------------------------------------
    void FEM4EllipticPDE::SetVTUGridFile ( const std::string& fname  ) {
        SetVTUGridFile( fname.c_str() );
        return ;
    }		// -----  end of method FEM4EllipticPDE::SetVTKGridFile  -----

    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  SetVTUGridFile
    //--------------------------------------------------------------------------------------
    void FEM4EllipticPDE::SetVTUGridFile ( const char* fname  ) {
        std::cout  << "Loading VTK .vtu file " << fname;
        vtkXMLUnstructuredGridReader* MeshReader = vtkXMLUnstructuredGridReader::New();
        MeshReader->SetFileName(fname);
        MeshReader->Update();
        //vtkGrid->DeepCopy( MeshReader->GetOutput() );
        //vtkGrid->ShallowCopy( MeshReader->GetOutput() );
        vtkGrid = MeshReader->GetOutput();
        if (!vtkGrid->GetNumberOfCells()) {
            throw std::runtime_error("In FEM4EllipticPDE::SetVTUGridFile NUMBER OF CELLS = 0");
	}
        if (!vtkGrid->GetNumberOfPoints()) {
            throw std::runtime_error("In FEM4EllipticPDE::SetVTUGridFile NUMBER OF POINTS = 0");
	}
	MeshReader->Delete();
        std::cout  << " Finished! " << std::endl;
        return ;
    }		// -----  end of method FEM4EllipticPDE::SetVTKGridFile  -----


    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  GetConnectedPoints
    //         (C) Joe Capriotti 2013
    //--------------------------------------------------------------------------------------
    vtkSmartPointer<vtkIdList> FEM4EllipticPDE::GetConnectedPoints(const int& id0) {
        vtkSmartPointer<vtkIdList> pointIds = vtkSmartPointer<vtkIdList>::New();
        vtkSmartPointer<vtkIdList> cellList = vtkSmartPointer<vtkIdList>::New();
        vtkGrid->GetPointCells(id0, cellList);
        for(int i=0;i<cellList->GetNumberOfIds(); ++i){
            vtkCell* cell = vtkGrid->GetCell(cellList->GetId(i));
            if(cell->GetNumberOfEdges() > 0){
                for(int j=0; j<cell->GetNumberOfEdges(); ++j){
                    vtkCell* edge = cell->GetEdge(j);
                    vtkIdList* edgePoints=edge->GetPointIds();
                    if(edgePoints->GetId(0)==id0){
                        pointIds->InsertUniqueId(edgePoints->GetId(1));
                    } else if(edgePoints->GetId(1)==id0){
                        pointIds->InsertUniqueId(edgePoints->GetId(0));
                    }
                }
            }
        }
        return pointIds;
    }		// -----  end of method FEM4EllipticPDE::GetConnectedPoints  -----

    Real FEM4EllipticPDE::dist(Real r0[3], Real r1[3]) {
        Real rm0 = r1[0] - r0[0];
        Real rm1 = r1[1] - r0[1];
        Real rm2 = r1[2] - r0[2];
        return std::sqrt( rm0*rm0 + rm1*rm1 + rm2*rm2 );
    }

    Real FEM4EllipticPDE::dist(const Vector3r& r0, const Vector3r& r1) {
        Real rm0 = r1[0] - r0[0];
        Real rm1 = r1[1] - r0[1];
        Real rm2 = r1[2] - r0[2];
        return std::sqrt( rm0*rm0 + rm1*rm1 + rm2*rm2 );
    }


    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  SetupDC
    //--------------------------------------------------------------------------------------
    void FEM4EllipticPDE::SetupDC ( DCSurvey* Survey, const int& ij ) {

        ////////////////////////////////////////////////////////////
	    // Load vector g, solution vector u
        std::cout << "\nBuilding load vector (g)" << std::endl;
	    g = VectorXr::Zero(vtkGrid->GetNumberOfPoints());
        std::cout << "made g with "  << vtkGrid->GetNumberOfPoints() << " pnts" << std::endl;

        int iia(0);
        Real jja(0);
        Survey->GetA( ij, iia, jja );
        //g(ii) = jj;

        int iib(0);
        Real jjb(0);
        Survey->GetB( ij, iib, jjb );
        //g(ii) = jj;


        /* 3D Phi */

        for (int ic=0; ic < vtkGrid->GetNumberOfCells(); ++ic) {

//             Eigen::Matrix<Real, 4, 4> C = Eigen::Matrix<Real, 4, 4>::Zero() ;
//             for (int ii=0; ii<4; ++ii) {
//                 double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
//                 C(ii, 0) = 1;
//                 C(ii, 1) =  pts[0];
//                 C(ii, 2) =  pts[1];
//                 C(ii, 3) =  pts[2];
//             }

//             Real V = (1./6.) * C.determinant();          // volume of tetrahedra
//
            vtkIdList* Ids = vtkGrid->GetCell(ic)->GetPointIds();
            int ID[4];
                ID[0] = Ids->GetId(0);
                ID[1] = Ids->GetId(1);
                ID[2] = Ids->GetId(2);
                ID[3] = Ids->GetId(3);

            //Real V = C.determinant();          // volume of tetrahedra
            Real sum(0);
            if (ID[0] == iia || ID[1] == iia || ID[2] == iia || ID[3] == iia ) {
                std::cout << "Caught A electrode, injecting " <<  iia << std::endl;
                //sum = 10;
                //g(ID[iia]) += jja;
                g(iia) += jja;
            }
            if (ID[0] == iib || ID[1] == iib || ID[2] == iib || ID[3] == iib) {
                //sum = -10;
                std::cout << "Caught B electrode, injecting " << iib << std::endl;
                //g(ID[iib]) += jjb;
                g(iib) += jjb;
            }

            //g(ID[0]) = sum; //(V/4.) * sum; // Why 3, Why!!!?
            std::cout <<  "\r" << (int)(1e2*((float)(ic) / (float)(vtkGrid->GetNumberOfCells()))) << std::flush ;
        }



        return ;
    }		// -----  end of method FEM4EllipticPDE::SetupDC  -----



    void FEM4EllipticPDE::Solve( const std::string& resfile ) {
        ConstructAMatrix();
        //SetupLineSourcePotential();

        if (vtkGrid->GetPointData()->GetScalars("G") ) {
            SetupSurfaceSourcePotential();
        } else if (vtkGrid->GetCellData()->GetScalars("G") ) {
            SetupPotential();
        }


        //ConstructLoadVector();

        std::cout << "\nSolving" << std::endl;

        std::cout << std::setw(5) << "  " << std::setw(14) << "rows"     << std::setw(14) << "cols"     << std::endl;
        std::cout << std::setw(5) << "  " << std::setw(14) << "--------" << std::setw(14) << "--------" << std::endl;
        std::cout << std::setw(5) << "A:" << std::setw(14) << A.rows()   << std::setw(14) << A.cols()   << std::endl;
        std::cout << std::setw(5) << "g:" << std::setw(14) << g.rows()   << std::setw(14) << g.cols()   << std::endl;

        ////////////////////////////////////////////////////////////
        // Solving:
        //Eigen::SimplicialCholesky<Eigen::SparseMatrix<Real>, Eigen::Lower > chol(A);  // performs a Cholesky factorization of A
        //VectorXr u = chol.solve(g);
        //#define LUSOLVE
        #ifdef LUSOLVE
        Eigen::SparseLU<Eigen::SparseMatrix<Real, Eigen::ColMajor>, Eigen::COLAMDOrdering<int> > solver;
        std::cout << "LU analyze pattern" << std::endl;
        solver.analyzePattern(A);
        std::cout << "LU factorizing" << std::endl;
        solver.factorize(A);
        std::cout << "LU solving" << std::endl;
        solver.factorize(A);
        VectorXr u = solver.solve(g);
        #endif

        #define CGSOLVE
        #ifdef CGSOLVE
        // TODO try IncompleteLUT preconditioner
        Eigen::BiCGSTAB<Eigen::SparseMatrix<Real> > cg(A);
        //cg.setMaxIterations(3000);
        //cg.setTolerance(1e-28);
        VectorXr u = cg.solve(g);
        std::cout << "#iterations:     " << cg.iterations() << std::endl;
        std::cout << "estimated error: " << cg.error()      << std::endl;
        #endif

        vtkDoubleArray *gArray = vtkDoubleArray::New();
        vtkDoubleArray *uArray = vtkDoubleArray::New();
            uArray->SetNumberOfComponents(1);
            gArray->SetNumberOfComponents(1);
        for (int iu = 0; iu<u.size(); ++iu) {
            uArray->InsertTuple1(iu, u[iu]);
            gArray->InsertTuple1(iu, g[iu]);
        }
        uArray->SetName("u");
        gArray->SetName("g");
        vtkGrid->GetPointData()->AddArray(uArray);
        vtkGrid->GetPointData()->AddArray(gArray);

        vtkXMLDataSetWriter *Writer = vtkXMLDataSetWriter::New();
            Writer->SetInputData(vtkGrid);
            Writer->SetFileName(resfile.c_str());
            Writer->Write();
            Writer->Delete();

        gArray->Delete();
        uArray->Delete();

    }


    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  ConstructAMatrix
    //--------------------------------------------------------------------------------------
    void FEM4EllipticPDE::ConstructAMatrix (  ) {

        /////////////////////////////////////////////////////////////////////////
        // Build stiffness matrix (A)
        std::cout << "Building Stiffness Matrix (A) " << std::endl;
        std::cout << "\tMesh has " << vtkGrid->GetNumberOfCells()  << " cells "  << std::endl;
        std::cout << "\tMesh has " << vtkGrid->GetNumberOfPoints()  << " points "  << std::endl;

  	    //Eigen::SparseMatrix<Real>
        A.resize(vtkGrid->GetNumberOfPoints(), vtkGrid->GetNumberOfPoints());
        std::vector< Eigen::Triplet<Real> > coeffs;

        if ( !vtkGrid->GetPointData()->GetScalars("HomogeneousDirichlet") ) {
            throw std::runtime_error("No HomogeneousDirichlet boundary conditions in input file.");
        }

        if ( !vtkGrid->GetCellData()->GetScalars("G") && !vtkGrid->GetPointData()->GetScalars("G") ) {
            throw std::runtime_error("No Cell or Point Data G");
        }

        bool GCell = false;
        if ( vtkGrid->GetCellData()->GetScalars("G") ) {
            GCell = true;
        }

        // Here we iterate over all of the cells
        for (int ic=0; ic < vtkGrid->GetNumberOfCells(); ++ic) {

            assert ( vtkGrid->GetCell(ic)->GetNumberOfPoints() == 4 );
            // TODO, in production code we might not want to do this check here
            if ( vtkGrid->GetCell(ic)->GetNumberOfPoints() != 4 ) {
                throw std::runtime_error("Non-tetrahedral mesh encountered!");
            }

            // construct coordinate matrix C
            Eigen::Matrix<Real, 4, 4> C = Eigen::Matrix<Real, 4, 4>::Zero() ;
            for (int ii=0; ii<4; ++ii) {
                double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
                C(ii, 0) = 1;
                C(ii, 1) =  pts[0] ;
                C(ii, 2) =  pts[1] ;
                C(ii, 3) =  pts[2] ;
            }

            Eigen::Matrix<Real, 4, 4> GradPhi = C.inverse(); // nabla \phi
            Real V = (1./6.) * C.determinant();          // volume of tetrahedra

            vtkIdList* Ids = vtkGrid->GetCell(ic)->GetPointIds();
            int ID[4];
                ID[0] = Ids->GetId(0);
                ID[1] = Ids->GetId(1);
                ID[2] = Ids->GetId(2);
                ID[3] = Ids->GetId(3);

            Real sigma_bar(0);
            // TEST VOID IN SIGMA!! TODO DON"T KEEP THIS
            /*
            Real xc = C.col(1).array().mean();
            Real yc = C.col(2).array().mean();
            Real zc = C.col(3).array().mean();
            if ( xc >= 2.5 && xc <= 5. && yc>=2.5 && yc <= 5.) {
                sigma_bar = 0.;
            } else {
                sigma_bar = 1.;
            }
            */
            sigma_bar = 1.;


            for (int ii=0; ii<4; ++ii) {
                int bbi = vtkGrid->GetPointData()->GetScalars("HomogeneousDirichlet")->GetTuple(ID[ii])[0];
                if (bbi) {
                    /* Dirichlet boundary */
                    coeffs.push_back( Eigen::Triplet<Real> ( ID[ii], ID[ii], 1));
                } else {
                    for (int jj=0; jj<4; ++jj) {
                        coeffs.push_back( Eigen::Triplet<Real> ( ID[ii], ID[jj], GradPhi.col(ii).tail<3>().dot(GradPhi.col(jj).tail<3>() ) * V * sigma_bar ) );
                    }
                }
            }
            std::cout <<  "\r" << (int)(1e2*((float)(ic) / (float)(vtkGrid->GetNumberOfCells()))) << std::flush ;
        }
        A.setFromTriplets(coeffs.begin(), coeffs.end());
        A.finalize();
        A.makeCompressed();
    }

    void FEM4EllipticPDE::SetupPotential() {

        ////////////////////////////////////////////////////////////
	    // Load vector g
        std::cout << "\nBuilding load vector (g)" << std::endl;
        g = VectorXr::Zero(vtkGrid->GetNumberOfPoints());
        std::cout << "made g with "  << vtkGrid->GetNumberOfPoints() << " points" << std::endl;

        for (int ic=0; ic < vtkGrid->GetNumberOfCells(); ++ic) {

            Eigen::Matrix<Real, 4, 4> C = Eigen::Matrix<Real, 4, 4>::Zero() ;
            for (int ii=0; ii<4; ++ii) {
                double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
                C(ii, 0) =  1;
                C(ii, 1) =  pts[0];
                C(ii, 2) =  pts[1];
                C(ii, 3) =  pts[2];
            }

            Real V = (1./6.) * C.determinant();              // volume of tetrahedra
            //Eigen::Matrix<Real, 4, 4> GradPhi = C.inverse(); // nabla \phi

            /* The indices */
            vtkIdList* Ids = vtkGrid->GetCell(ic)->GetPointIds();
            int ID[4];
                ID[0] = Ids->GetId(0);
                ID[1] = Ids->GetId(1);
                ID[2] = Ids->GetId(2);
                ID[3] = Ids->GetId(3);

            /* Fill the RHS vector with Dirichlet conditions or fuction value */
            for (int ii=0; ii<4; ++ii) {
                if (vtkGrid->GetPointData()->GetScalars("HomogeneousDirichlet")->GetTuple(ID[ii])[0]) {
                    g(ID[ii]) += vtkGrid->GetPointData()->GetScalars("analytic_phi")->GetTuple(ID[ii])[0];
                } else {
                    g(ID[ii]) += (V/4.)*(vtkGrid->GetCellData()->GetScalars("G")->GetTuple(ic)[0]); // Cell data
                    /* for point data, determine if it is a point or line source */
                    //g(ID[ii]) = (vtkGrid->GetPointData()->GetScalars("G")->GetTuple(ID[ii])[0]); // Point source
                    //g(ID[ii]) = (vtkGrid->GetPointData()->GetScalars("G")->GetTuple(ID[ii])[0]); // Point source
                }
            }



        }
    }

    void FEM4EllipticPDE::SetupLineSourcePotential() {

        std::cerr << " FEM4EllipticPDE::SetupLineSourcePotential is not completed\n";
        exit(1);

        ////////////////////////////////////////////////////////////
	    // Load vector g
        std::cout << "\nBuilding load vector (g)" << std::endl;
        g = VectorXr::Zero(vtkGrid->GetNumberOfPoints());
        std::cout << "made g with "  << vtkGrid->GetNumberOfPoints() << " points" << std::endl;

        for (int ic=0; ic < vtkGrid->GetNumberOfCells(); ++ic) {

            Eigen::Matrix<Real, 4, 4> C = Eigen::Matrix<Real, 4, 4>::Zero() ;
            for (int ii=0; ii<4; ++ii) {
                double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
                C(ii, 0) =  1;
                C(ii, 1) =  pts[0];
                C(ii, 2) =  pts[1];
                C(ii, 3) =  pts[2];
            }

            Real V = (1./6.) * C.determinant();              // volume of tetrahedra
            //Eigen::Matrix<Real, 4, 4> GradPhi = C.inverse(); // nabla \phi

            /* The indices */
            vtkIdList* Ids = vtkGrid->GetCell(ic)->GetPointIds();
            int ID[4];
                ID[0] = Ids->GetId(0);
                ID[1] = Ids->GetId(1);
                ID[2] = Ids->GetId(2);
                ID[3] = Ids->GetId(3);

            /* Line source between nodes */
            for (int ii=0; ii<4; ++ii) {
                if (vtkGrid->GetPointData()->GetScalars("HomogeneousDirichlet")->GetTuple(ID[ii])[0]) {
                    g(ID[ii]) += vtkGrid->GetPointData()->GetScalars("analytic_phi")->GetTuple(ID[ii])[0];
                } else {
                    Real nv1 = vtkGrid->GetPointData()->GetScalars("G")->GetTuple(ID[ii])[0];
                    for (int jj=ii+1; jj<4; ++jj) {
                        Real nv2 = vtkGrid->GetPointData()->GetScalars("G")->GetTuple(ID[jj])[0];
                        if ( std::abs(nv1) > 1e-12 && std::abs(nv2) > 1e-12) {
                            Real length = ( C.row(ii).tail<3>()-C.row(jj).tail<3>()  ).norm();
                            g(ID[ii]) += ((nv1+nv2)/2.)/(length);
                        }
                    }
                }
            }
        }

    }

    void FEM4EllipticPDE::SetupSurfaceSourcePotential() {

        ////////////////////////////////////////////////////////////
	    // Load vector g
        std::cout << "\nBuilding load vector (g)" << std::endl;
        g = VectorXr::Zero(vtkGrid->GetNumberOfPoints());

        for (int ic=0; ic < vtkGrid->GetNumberOfCells(); ++ic) {
            Eigen::Matrix<Real, 4, 4> C = Eigen::Matrix<Real, 4, 4>::Zero() ;
            for (int ii=0; ii<4; ++ii) {
                double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
                C(ii, 0) =  1;
                C(ii, 1) =  pts[0];
                C(ii, 2) =  pts[1];
                C(ii, 3) =  pts[2];
            }

            Real V = (1./6.) * C.determinant();              // volume of tetrahedra
            //Eigen::Matrix<Real, 4, 4> GradPhi = C.inverse(); // nabla \phi

            /* The indices */
            vtkIdList* Ids = vtkGrid->GetCell(ic)->GetPointIds();
            int ID[4];
                ID[0] = Ids->GetId(0);
                ID[1] = Ids->GetId(1);
                ID[2] = Ids->GetId(2);
                ID[3] = Ids->GetId(3);

            // Apply Dirichlet conditions
            for (int ii=0; ii<4; ++ii) {
                //if (vtkGrid->GetPointData()->GetScalars("InHomogeneousDirichlet")->GetTuple(ID[ii])[0]) {
                //    g(ID[ii]) += vtkGrid->GetPointData()->GetScalars("analytic_phi")->GetTuple(ID[ii])[0];
                //}
            }

            /* the 4 faces of the tetrahedra
            ID[0] ID[1] ID[2]
            ID[0] ID[1] ID[3]
            ID[0] ID[2] ID[3]
            ID[1] ID[2] ID[3]
            */
            Real i4pi = .5;
            /* surfaces of tetrahedra */
            // Face 0, ID 0,1,2
            Eigen::Matrix<Real, 3, 3> CC = Eigen::Matrix<Real, 3, 3>::Ones() ;
            if (    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[0])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[1])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[2])) > 1e-12    ) {
                CC.col(1) = C.row(0).tail<3>() - C.row(1).tail<3>();
                CC.col(2) = C.row(0).tail<3>() - C.row(2).tail<3>();
                Real TA = .5*CC.col(1).cross(CC.col(2)).norm();
                g(ID[0]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[0])*TA/3. * i4pi ;
                g(ID[1]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[1])*TA/3. * i4pi ;
                g(ID[2]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[2])*TA/3. * i4pi ;
            }
            // Face 1, ID 0,1,3
            if (    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[0])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[1])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[3])) > 1e-12    ) {
                CC.col(1) = C.row(0).tail<3>() - C.row(1).tail<3>();
                CC.col(2) = C.row(0).tail<3>() - C.row(3).tail<3>();
                Real TA = .5*CC.col(1).cross(CC.col(2)).norm();
                g(ID[0]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[0])*TA/3. * i4pi ;
                g(ID[1]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[1])*TA/3. * i4pi ;
                g(ID[3]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[3])*TA/3. * i4pi ;
            }
            // Face 2, ID 0,2,3
            if (    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[0])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[2])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[3])) > 1e-12    ) {
                CC.col(1) = C.row(0).tail<3>() - C.row(2).tail<3>();
                CC.col(2) = C.row(0).tail<3>() - C.row(3).tail<3>();
                Real TA = .5*CC.col(1).cross(CC.col(2)).norm();
                g(ID[0]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[0])*TA/3. * i4pi ;
                g(ID[2]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[2])*TA/3. * i4pi ;
                g(ID[3]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[3])*TA/3. * i4pi ;
            }
            // Face 3, ID 1,2,3
            if (    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[1])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[2])) > 1e-12 &&
                    std::abs(vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[3])) > 1e-12    ) {
                CC.col(1) = C.row(1).tail<3>() - C.row(2).tail<3>();
                CC.col(2) = C.row(1).tail<3>() - C.row(3).tail<3>();
                Real TA = .5*CC.col(1).cross(CC.col(2)).norm();
                g(ID[1]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[1])*TA/3. * i4pi ;
                g(ID[2]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[2])*TA/3. * i4pi ;
                g(ID[3]) += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[3])*TA/3. * i4pi ;
            }
        }
        std::cout << "made g with "  << vtkGrid->GetNumberOfPoints() << " points" << std::endl;
    }

    #if 0
    void FEM4EllipticPDE::SolveOLD2(const std::string& fname) {

        Real r0[3];
        Real r1[3];

        /////////////////////////////////////////////////////////////////////////
        // Surface filter, to determine if points are on boundary, and need
        // boundary conditions applied
        vtkDataSetSurfaceFilter* Surface = vtkDataSetSurfaceFilter::New();
            Surface->SetInputData(vtkGrid);
            Surface->PassThroughPointIdsOn(	);
            Surface->Update();
            vtkIdTypeArray* BdryIds = static_cast<vtkIdTypeArray*>
                (Surface->GetOutput()->GetPointData()->GetScalars("vtkOriginalPointIds"));

        // Expensive search for whether or not point is on boundary. O(n) cost.
        VectorXi bndryCnt = VectorXi::Zero(vtkGrid->GetNumberOfPoints());
        for (int isp=0; isp < Surface->GetOutput()->GetNumberOfPoints(); ++isp) {
            //double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
            // x \in -14.5 to 14.5
            // y \in 0 to 30
            bndryCnt(BdryIds->GetTuple1(isp)) += 1;
        }

        /////////////////////////////////////////////////////////////////////////
        // Build stiffness matrix (A)
        std::cout << "Building Stiffness Matrix (A) " << std::endl;
        std::cout << "\tMesh has " << vtkGrid->GetNumberOfCells()  << " cells "  << std::endl;
        std::cout << "\tMesh has " << vtkGrid->GetNumberOfPoints()  << " points "  << std::endl;


  	    Eigen::SparseMatrix<Real> A(vtkGrid->GetNumberOfPoints(), vtkGrid->GetNumberOfPoints());
        std::vector< Eigen::Triplet<Real> > coeffs;

        // Here we iterate over all of the cells
        for (int ic=0; ic < vtkGrid->GetNumberOfCells(); ++ic) {

            assert ( vtkGrid->GetCell(ic)->GetNumberOfPoints() == 4 );
            // TODO, in production code we might not want to do this check here
            if ( vtkGrid->GetCell(ic)->GetNumberOfPoints() != 4 ) {
                std::cout << "DOOM FEM4EllipticPDE encountered non-tetrahedral mesh\n";
                std::cout << "Number of points in cell " << vtkGrid->GetCell(ic)->GetNumberOfPoints() << std::endl ;
                exit(1);
            }

            // construct coordinate matrix C
            Eigen::Matrix<Real, 4, 4> C = Eigen::Matrix<Real, 4, 4>::Zero() ;
            for (int ii=0; ii<4; ++ii) {
                double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
                C(ii, 0) = 1;
                C(ii, 1) =  pts[0] ;
                C(ii, 2) =  pts[1] ;
                C(ii, 3) =  pts[2] ;
            }

            Eigen::Matrix<Real, 4, 4> GradPhi = C.inverse(); // nabla \phi
            Real V = (1./6.) * C.determinant();          // volume of tetrahedra

            vtkIdList* Ids = vtkGrid->GetCell(ic)->GetPointIds();
            int ID[4];
                ID[0] = Ids->GetId(0);
                ID[1] = Ids->GetId(1);
                ID[2] = Ids->GetId(2);
                ID[3] = Ids->GetId(3);
            Real sum(0);
            Real sigma_bar = vtkGrid->GetCellData()->GetScalars()->GetTuple1(ic);

            for (int ii=0; ii<4; ++ii) {
                for (int jj=0; jj<4; ++jj) {
                    if (jj == ii) {
                        // I apply boundary to Stiffness matrix, it's common to take the other approach and apply to the load vector and then
                        //   solve for the boundaries? Is one better? This seems to work, which is nice.
                        //Real bdry = V*(1./(BndryH*BndryH))*BndrySigma*bndryCnt( ID[ii] ); // + sum;
                        Real bb = vtkGrid->GetPointData()->GetScalars("vtkValidPointMask")->GetTuple(ID[ii])[0];
                        //std::cout << "bb " << bb  << std::endl;
                        Real bdry = V*(1./(BndryH*BndryH))*BndrySigma*bb; // + sum;
                        coeffs.push_back( Eigen::Triplet<Real> ( ID[ii], ID[jj], bdry +  GradPhi.col(ii).tail<3>().dot(GradPhi.col(jj).tail<3>() ) * V * sigma_bar ) );
                    } else {
                        coeffs.push_back( Eigen::Triplet<Real> ( ID[ii], ID[jj],         GradPhi.col(ii).tail<3>().dot(GradPhi.col(jj).tail<3>() ) * V * sigma_bar ) );
                    }
                    // Stiffness matrix no longer contains boundary conditions...
                    //coeffs.push_back( Eigen::Triplet<Real> ( ID[ii], ID[jj],         GradPhi.col(ii).tail<3>().dot(GradPhi.col(jj).tail<3>() ) * V * sigma_bar ) );
                }
            }
            std::cout <<  "\r" << (int)(1e2*((float)(ic) / (float)(vtkGrid->GetNumberOfCells()))) << std::flush ;
        }
        A.setFromTriplets(coeffs.begin(), coeffs.end());
        //A.makeCompressed();

        ////////////////////////////////////////////////////////////
	    // Load vector g, solution vector u
        std::cout << "\nBuilding load vector (g)" << std::endl;
	    VectorXr g = VectorXr::Zero(vtkGrid->GetNumberOfPoints());
        std::cout << "made g with "  << vtkGrid->GetNumberOfPoints() << " pnts" << std::endl;
        // If the G function has been evaluated at each *node*
        //        --> but still needs to be integrated at the surfaces
        // Aha, requires that there is in fact a pointdata memeber  // BUG TODO BUG!!!
        std::cout  << "Point Data ptr " <<  vtkGrid->GetPointData() << std::endl;
        //if (  vtkGrid->GetPointData() != NULL && std::string( vtkGrid->GetPointData()->GetScalars()->GetName() ).compare( std::string("G") ) == 0 ) {
        bool pe(false);
        bool ne(false);
        if ( true ) {

            std::cout << "\nUsing G from file" << std::endl;

            /* 3D Phi */
            for (int ic=0; ic < vtkGrid->GetNumberOfCells(); ++ic) {

                Eigen::Matrix<Real, 4, 4> C = Eigen::Matrix<Real, 4, 4>::Zero() ;
                for (int ii=0; ii<4; ++ii) {
                    double* pts =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(ii); //[ipc] ;
                    C(ii, 0) = 1;
                    C(ii, 1) =  pts[0];
                    C(ii, 2) =  pts[1];
                    C(ii, 3) =  pts[2];
                }

                Real V = (1./6.) * C.determinant();          // volume of tetrahedra
                //Real V = C.determinant();          // volume of tetrahedra

                vtkIdList* Ids = vtkGrid->GetCell(ic)->GetPointIds();
                int ID[4];
                    ID[0] = Ids->GetId(0);
                    ID[1] = Ids->GetId(1);
                    ID[2] = Ids->GetId(2);
                    ID[3] = Ids->GetId(3);

                /* bad news bears for magnet */
                double* pt =  vtkGrid->GetCell(ic)->GetPoints()->GetPoint(0);
                Real sum(0);
                /*
                if (!pe) {
                    if (std::abs(pt[0]) < .2 && std::abs(pt[1]-15) < .2 && pt[2] < 3.25 ) {
                        sum = 1; //vtkGrid->GetPointData()->GetScalars()->GetTuple(ID[ii])[0];
                        pe = true;
                    }
                }*/
                if (ID[0] == 26) {
                    sum = 10;
                }
                if (ID[0] == 30) {
                    sum = -10;
                }

/*
                if (!ne) {
                    if (std::abs(pt[0]+1.) < .2 && std::abs(pt[1]-15) < .2 && pt[2] < 3.25 ) {
                        sum = -1; //vtkGrid->GetPointData()->GetScalars()->GetTuple(ID[ii])[0];
                        std::cout << "Negative Electroce\n";
                        ne = true;
                    }
                }
*/
                //for (int ii=0; ii<4; ++ii) {
                    //g(ID[ii]) +=  (V/4.) *  ( vtkGrid->GetPointData()->GetScalars()->GetTuple(ID[ii])[0] )  ;
                    //if ( std::abs(vtkGrid->GetPointData()->GetScalars()->GetTuple(ID[ii])[0]) > 1e-3 )
                //}
                // TODO check Load Vector...
                g(ID[0]) = sum; //(V/4.) * sum; // Why 3, Why!!!?
                std::cout <<  "\r" << (int)(1e2*((float)(ic) / (float)(vtkGrid->GetNumberOfCells()))) << std::flush ;
            }

            /*
            for (int ic=0; ic < vtkGrid->GetNumberOfPoints(); ++ic) {
                vtkGrid->GetPoint(ic, r0);
                vtkSmartPointer<vtkIdList> connectedVertices = GetConnectedPoints(ic);
                double g0 = vtkGrid->GetPointData()->GetScalars()->GetTuple(ic)[0] ;
                //std::cout << "num conn " << connectedVertices->GetNumberOfIds() << std::endl;
                if ( std::abs(g0) > 1e-3 ) {

                    for(vtkIdType i = 0; i < connectedVertices->GetNumberOfIds(); ++i) {

                        int ii = connectedVertices->GetId(i);
                        vtkGrid->GetPoint(ii, r1);
                        double g1 = vtkGrid->GetPointData()->GetScalars()->GetTuple(ii)[0] ;
                        //g(ic) += g0*dist(r0,r1); //CompositeSimpsons2(g0, r0, r1, 8);
                        if ( std::abs(g1) > 1e-3 ) {
                            g(ic) += CompositeSimpsons2(g1, g0, r1, r0, 1000);
                        }
                        //g(ic) += CompositeSimpsons2(g0, r1, r0, 8);
                        //if ( std::abs(g1) > 1e-3 ) {
                            //g(ic) += CompositeSimpsons2(g0, g1, r0, r1, 8);
                            //g(ic) += CompositeSimpsons2(g0, g1, r0, r1, 100); // / (2*dist(r0,r1)) ;
                        //    g(ic) += g0*dist(r0,r1); //CompositeSimpsons2(g0, r0, r1, 8);
                            //g(ic) += CompositeSimpsons2(g0, r0, r1, 8);
                        //    g(ic) += g0; //CompositeSimpsons2(g0, r0, r1, 8);
                        //} //else {
                        //    g(ic) += g0; //CompositeSimpsons2(g0, r0, r1, 8);
                        //}
                    }
                }
                //g(ic) = 2.* vtkGrid->GetPointData()->GetScalars()->GetTuple(ic)[0] ; // Why 2?
                //std::cout <<  "\r" << (int)(1e2*((float)(ic) / (float)(vtkGrid->GetNumberOfPoints()))) << std::flush ;
            }
            */

        } else if (vtkG) { // VTK implicit function, proceed with care
            std::cout << "\nUsing implicit file from file" << std::endl;
            // OpenMP right here
            for (int ic=0; ic < vtkGrid->GetNumberOfPoints(); ++ic) {
                vtkSmartPointer<vtkIdList> connectedVertices = GetConnectedPoints(ic);
                //vtkGrid->GetPoint(ic, r0);
                //g(ic) += vtkG->FunctionValue(r0[0], r0[1], r0[2]) ;
                // std::cout << vtkG->FunctionValue(r0[0], r0[1], r0[2])  << std::endl;
                //g(ic) += vtkGrid->GetPointData()->GetScalars()->GetTuple1(ic);// FunctionValue(r0[0], r0[1], r0[2]) ;
                /*
                for(vtkIdType i = 0; i < connectedVertices->GetNumberOfIds(); ++i) {
                    int ii = connectedVertices->GetId(i);
                    vtkGrid->GetPoint(ii, r1);
                    g(ic) += CompositeSimpsons2(vtkG, r0, r1, 8); // vtkG->FunctionValue(r0[0], r0[1], r0[2]) ;
                }
                */
                std::cout <<  "\r" << (int)(1e2*((float)(ic) / (float)(vtkGrid->GetNumberOfPoints()))) << std::flush ;
            }
        } else if (gFcn3) {
            std::cout << "\nUsing gFcn3" << std::endl;
            for (int ic=0; ic < vtkGrid->GetNumberOfPoints(); ++ic) {
                vtkSmartPointer<vtkIdList> connectedVertices = GetConnectedPoints(ic);
                vtkGrid->GetPoint(ic, r0);
                // TODO, test OMP sum reduction here. Is vtkGrid->GetPoint thread safe?
                //Real sum(0.);
                //#ifdef LEMMAUSEOMP
                //#pragma omp parallel for reduction(+:sum)
                //#endif
                for(vtkIdType i = 0; i < connectedVertices->GetNumberOfIds(); ++i) {
                    int ii = connectedVertices->GetId(i);
                    vtkGrid->GetPoint(ii, r1);
                    g(ic) += CompositeSimpsons2(gFcn3, r0, r1, 8); // vtkG->FunctionValue(r0[0], r0[1], r0[2]) ;
                    //sum += CompositeSimpsons2(gFcn3, r0, r1, 8); // vtkG->FunctionValue(r0[0], r0[1], r0[2]) ;
                }
                std::cout <<  "\r" << (int)(1e2*((float)(ic) / (float)(vtkGrid->GetNumberOfPoints()))) << std::flush ;
                //g(ic) = sum;
            }
        } else {
            std::cout << "No source specified\n";
            exit(EXIT_FAILURE);
        }
        // std::cout << g << std::endl;

        //g(85) = 1;

        std::cout << "\nSolving" << std::endl;
        ////////////////////////////////////////////////////////////
        // Solving:
        //Eigen::SimplicialCholesky<Eigen::SparseMatrix<Real>, Eigen::Lower > chol(A);  // performs a Cholesky factorization of A
        //VectorXr u = chol.solve(g);

        //Eigen::SparseLU<Eigen::SparseMatrix<Real, Eigen::ColMajor>, Eigen::COLAMDOrdering<int> > solver;
        //solver.analyzePattern(A);
        //solver.factorize(A);
        //VectorXr u = solver.solve(g);

        //Eigen::ConjugateGradient<Eigen::SparseMatrix<Real, Eigen::Lower >  Eigen::DiagonalPreconditioner > cg;
        Eigen::ConjugateGradient< Eigen::SparseMatrix<Real>  > cg(A);
        //Eigen::BiCGSTAB<Eigen::SparseMatrix<Real> > cg(A);
        cg.setMaxIterations(3000);
        //cg.compute(A);
        //std::cout << "Computed     " << std::endl;
        VectorXr u = cg.solve(g);
        std::cout << "#iterations:     " << cg.iterations() << std::endl;
        std::cout << "estimated error: " << cg.error()      << std::endl;

        vtkDoubleArray *gArray = vtkDoubleArray::New();
        vtkDoubleArray *uArray = vtkDoubleArray::New();
            uArray->SetNumberOfComponents(1);
            gArray->SetNumberOfComponents(1);
        for (int iu = 0; iu<u.size(); ++iu) {
            uArray->InsertTuple1(iu, u[iu]);
            gArray->InsertTuple1(iu, g[iu]);
        }
        uArray->SetName("u");
        gArray->SetName("g");
        vtkGrid->GetPointData()->AddArray(uArray);
        vtkGrid->GetPointData()->AddArray(gArray);

        vtkXMLDataSetWriter *Writer = vtkXMLDataSetWriter::New();
            Writer->SetInputData(vtkGrid);
            Writer->SetFileName(fname.c_str());
            Writer->Write();
            Writer->Delete();

        Surface->Delete();
        gArray->Delete();
        uArray->Delete();

    }
    #endif

    // Uses simpon's rule to perform a definite integral of a
    // function (passed as a pointer). The integration occurs from
    // (Shamelessly adapted from http://en.wikipedia.org/wiki/Simpson's_rule
    Real FEM4EllipticPDE::CompositeSimpsons(vtkImplicitFunction* f, Real r0[3], Real r1[3], int n) {

        Vector3r R0(r0[0], r0[1], r0[2]);
        Vector3r R1(r1[0], r1[1], r1[2]);

        // force n to be even
        assert(n > 0);
        //n += (n % 2);

 	    Real h  = dist(r0, r1) / (Real)(n) ;
        Real S = f->FunctionValue(r0) + f->FunctionValue(r1);

        Vector3r dr = (R1 - R0).array() / Real(n);

        Vector3r rx;
        rx.array() = R0.array() + dr.array();
        for (int i=1; i<n; i+=2) {
            S += 4. * f->FunctionValue(rx[0], rx[1], rx[2]);
            rx += 2.*dr;
        }

        rx.array() = R0.array() + 2*dr.array();
        for (int i=2; i<n-1; i+=2) {
            S += 2.*f->FunctionValue(rx[0], rx[1], rx[2]);
            rx += 2.*dr;
        }

        return h * S / 3.;

    }

    // Uses simpon's rule to perform a definite integral of a
    // function (passed as a pointer). The integration occurs from
    // (Shamelessly adapted from http://en.wikipedia.org/wiki/Simpson's_rule
    // This is just here as a convenience
    Real FEM4EllipticPDE::CompositeSimpsons(const Real& f, Real r0[3], Real r1[3], int n) {

        return dist(r0,r1)*f;
        /*
        Vector3r R0(r0[0], r0[1], r0[2]);
        Vector3r R1(r1[0], r1[1], r1[2]);

        // force n to be even
        assert(n > 0);
        //n += (n % 2);

 	    Real h  = dist(r0, r1) / (Real)(n) ;
        Real S = f + f;

        Vector3r dr = (R1 - R0).array() / Real(n);

        //Vector3r rx;
        //rx.array() = R0.array() + dr.array();
        for (int i=1; i<n; i+=2) {
            S += 4. * f;
            //rx += 2.*dr;
        }

        //rx.array() = R0.array() + 2*dr.array();
        for (int i=2; i<n-1; i+=2) {
            S += 2. * f;
            //rx += 2.*dr;
        }

        return h * S / 3.;
        */
    }


    /*
     *  Performs numerical integration of two functions, one is passed as a vtkImplicitFunction, the other is the FEM
     *  test function owned by the FEM implimentaion.
     */
    Real FEM4EllipticPDE::CompositeSimpsons2(vtkImplicitFunction* f,
            Real r0[3], Real r1[3], int n) {

        Vector3r R0(r0[0], r0[1], r0[2]);
        Vector3r R1(r1[0], r1[1], r1[2]);

        // force n to be even
        assert(n > 0);
        //n += (n % 2);

 	    Real h  = dist(r0, r1) / (Real)(n) ;
        // For Gelerkin (most) FEM, we can omit this one as test functions are zero valued at element boundaries
        Real S = f->FunctionValue(r0) + f->FunctionValue(r1);
        //Real S = f->FunctionValue(r0) + f->FunctionValue(r1);

        Vector3r dr = (R1 - R0).array() / Real(n);

        Vector3r rx;
        rx.array() = R0.array() + dr.array();
        for (int i=1; i<n; i+=2) {
            S += 4. * f->FunctionValue(rx[0], rx[1], rx[2]) * Hat(rx, R0, R1) * Hat(rx, R1, R0);
            rx += 2.*dr;
        }

        rx.array() = R0.array() + 2*dr.array();
        for (int i=2; i<n-1; i+=2) {
            S += 2. * f->FunctionValue(rx[0], rx[1], rx[2]) * Hat(rx, R0, R1) * Hat(rx, R1, R0);
            rx += 2.*dr;
        }

        return h * S / 3.;

    }

    /*
     *  Performs numerical integration of two functions, one is passed as a vtkImplicitFunction, the other is the FEM
     *  test function owned by the FEM implimentaion.
     */
    Real FEM4EllipticPDE::CompositeSimpsons2( Real (*f)(const Real&, const Real&, const Real&),
            Real r0[3], Real r1[3], int n) {

        Vector3r R0(r0[0], r0[1], r0[2]);
        Vector3r R1(r1[0], r1[1], r1[2]);

        // force n to be even
        assert(n > 0);
        //n += (n % 2);

 	    Real h  = dist(r0, r1) / (Real)(n) ;
        // For Gelerkin (most) FEM, we can omit this one as test functions are zero valued at element boundaries
        //Real S = f(r0[0], r0[1], r0[2])*Hat(R0, R0, R1) + f(r1[0], r1[1], r1[2])*Hat(R1, R0, R1);
        Real S = f(r0[0], r0[1], r0[2]) + f(r1[0], r1[1], r1[2]);

        Vector3r dr = (R1 - R0).array() / Real(n);

        Vector3r rx;
        rx.array() = R0.array() + dr.array();
        for (int i=1; i<n; i+=2) {
            S += 4. * f(rx[0], rx[1], rx[2]) * Hat(rx, R0, R1) * Hat(rx, R1, R0);
            rx += 2.*dr;

        }

        rx.array() = R0.array() + 2*dr.array();
        for (int i=2; i<n-1; i+=2) {
            S += 2. * f(rx[0], rx[1], rx[2]) * Hat(rx, R0, R1) * Hat(rx, R1, R0);
            rx += 2.*dr;
        }

        return h * S / 3.;

    }

    /*
     *  Performs numerical integration of two functions, one is constant valued f, the other is the FEM
     *  test function owned by the FEM implimentaion.
     */
    Real FEM4EllipticPDE::CompositeSimpsons2( const Real& f, Real r0[3], Real r1[3], int n) {

        Vector3r R0(r0[0], r0[1], r0[2]);
        Vector3r R1(r1[0], r1[1], r1[2]);

        // force n to be even
        assert(n > 0);
        //n += (n % 2);

 	    Real h  = dist(r0, r1) / (Real)(n) ;
        // For Gelerkin (most) FEM, we can omit this one as test functions are zero valued at element boundaries
        Real S = 2*f; //*Hat(R0, R0, R1) + f*Hat(R1, R0, R1);

        Vector3r dr = (R1 - R0).array() / Real(n);

        Vector3r rx;
        rx.array() = R0.array() + dr.array();
        for (int i=1; i<n; i+=2) {
            S += 4. * f * Hat(rx, R0, R1) * Hat(rx, R1, R0);
            rx += 2.*dr;
        }

        rx.array() = R0.array() + 2*dr.array();
        for (int i=2; i<n-1; i+=2) {
            S += 2. * f * Hat(rx, R0, R1) * Hat(rx, R1, R0);
            rx += 2.*dr;
        }

        return h * S / 3.;

    }

    /*
     *  Performs numerical integration of two functions, one is passed as a vtkImplicitFunction, the other is the FEM
     *  test function owned by the FEM implimentaion.
     */
    Real FEM4EllipticPDE::CompositeSimpsons2( const Real& f0, const Real& f1, Real r0[3], Real r1[3], int n) {

        Vector3r R0(r0[0], r0[1], r0[2]);
        Vector3r R1(r1[0], r1[1], r1[2]);

        // force n to be even
        assert(n > 0);
        //n += (n % 2);

 	    Real h  = dist(r0, r1) / (Real)(n) ;

        // For Gelerkin (most) FEM, we can omit this one as test functions are zero valued at element boundaries
        // NO! We are looking at 1/2 hat often!!! So S = f1!
        Real S = f1; //f0*Hat(R0, R0, R1) + f1*Hat(R1, R0, R1);

        Vector3r dr = (R1 - R0).array() / Real(n);

        // linear interpolate function
        //Vector3r rx;
        //rx.array() = R0.array() + dr.array();
        for (int i=1; i<n; i+=2) {
            double fx = f0 + (f1 - f0) * ((i*h)/(h*n));
            S += 4. * fx * Hat(R0.array() + i*dr.array(), R0, R1);// * Hat(R1.array() + i*dr.array(), R1, R0) ;
        }

        //rx.array() = R0.array() + 2*dr.array();
        for (int i=2; i<n-1; i+=2) {
            double fx = f0 + (f1 - f0) * ((i*h)/(h*n));
            S += 2. * fx * Hat(R0.array() + i*dr.array(), R0, R1);// * Hat(R1.array() + i*dr.array(), R1, R0);
        }

        return h * S / 3.;

    }

    /*
     *  Performs numerical integration of two functions, one is passed as a vtkImplicitFunction, the other is the FEM
     *  test function owned by the FEM implimentaion.
     */
    Real FEM4EllipticPDE::CompositeSimpsons3( const Real& f0, const Real& f1, Real r0[3], Real r1[3], int n) {

        Vector3r R0(r0[0], r0[1], r0[2]);
        Vector3r R1(r1[0], r1[1], r1[2]);

        // force n to be even
        assert(n > 0);
        //n += (n % 2);

 	    Real h  = dist(r0, r1) / (Real)(n) ;

        // For Gelerkin (most) FEM, we can omit this one as test functions are zero valued at element boundaries
        // NO! We are looking at 1/2 hat often!!! So S = f1!
        Real S = f0+f1; //Hat(R0, R0, R1) + f1*Hat(R1, R0, R1);

        Vector3r dr = (R1 - R0).array() / Real(n);

        // linear interpolate function
        //Vector3r rx;
        //rx.array() = R0.array() + dr.array();
        for (int i=1; i<n; i+=2) {
            double fx = 1;// f0 + (f1 - f0) * ((i*h)/(h*n));
            S += 4. * fx * Hat(R0.array() + i*dr.array(), R0, R1) * Hat(R1.array() + i*dr.array(), R1, R0) ;
        }

        //rx.array() = R0.array() + 2*dr.array();
        for (int i=2; i<n-1; i+=2) {
            double fx = 1; // f0 + (f1 - f0) * ((i*h)/(h*n));
            S += 2. * fx * Hat(R0.array() + i*dr.array(), R0, R1)*  Hat(R1.array() + i*dr.array(), R1, R0);
        }

        return h * S / 3.;

    }


    //--------------------------------------------------------------------------------------
    //       Class:  FEM4EllipticPDE
    //      Method:  Hat
    //--------------------------------------------------------------------------------------
    Real FEM4EllipticPDE::Hat ( const Vector3r& r, const Vector3r& r0, const Vector3r& r1 ) {
        //return  (r-r0).norm() / (r1-r0).norm() ;
        return  dist(r, r0) / dist(r1, r0) ;
    }		// -----  end of method FEM4EllipticPDE::Hat  -----


}		// -----  end of Lemma  name  -----