// ===========================================================================
//
//       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)
//
//          Email:  tirons@mines.edu, tirons@usgs.gov
//
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
// ===========================================================================

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

#include "FEM4EllipticPDE.h"

namespace Lemma {

	std::ostream &operator<<(std::ostream &stream,
				const FEM4EllipticPDE &ob) {

		stream << *(LemmaObject*)(&ob);
		return stream;
	}

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

	FEM4EllipticPDE::FEM4EllipticPDE(const std::string&name) :
			LemmaObject(name), BndryH(1), BndrySigma(1),
            vtkSigma(NULL), vtkG(NULL), vtkGrid(NULL), gFcn3(NULL) {
	}

	FEM4EllipticPDE::~FEM4EllipticPDE() {
	}

    void FEM4EllipticPDE::Release() {
        delete this;
    }

    FEM4EllipticPDE* FEM4EllipticPDE::New( ) {
        FEM4EllipticPDE* Obj = new FEM4EllipticPDE("FEM4EllipticPDE");
        Obj->AttachTo(Obj);
        return Obj;
    }

    void FEM4EllipticPDE::Delete() {
        this->DetachFrom(this);
    }

    // ====================  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(vtkDataSet* grid) {
        vtkGrid = grid;
    }

    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;
    }

    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();
        //ConstructLoadVector();

        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::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);

        std::cout <<"          rows\tcols\n";
        std::cout << "A: "  << A.rows() << "\t" << A.cols() << std::endl;
        std::cout << "g: "  << g.rows() << "\t" << g.cols() << 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(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("vtkValidPointMask") ) {
            throw std::runtime_error("No vtkValidPointMask");
        }

        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);
            if (GCell) {
                sigma_bar = vtkGrid->GetCellData()->GetScalars("G")->GetTuple1(ic);
            } else {
                sigma_bar  = vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[0]);
                sigma_bar += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[1]);
                sigma_bar += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[2]);
                sigma_bar += vtkGrid->GetPointData()->GetScalars("G")->GetTuple1(ID[3]);
                sigma_bar /= 4.;
            }
            sigma_bar = 1.;

            for (int ii=0; ii<4; ++ii) {
                for (int jj=0; jj<4; ++jj) {
                    if (jj == ii) {
                        // Apply Homogeneous Dirichlet Boundary conditions
                        Real bb = vtkGrid->GetPointData()->GetScalars("vtkValidPointMask")->GetTuple(ID[ii])[0];
                        Real bdry = (1./(BndryH*BndryH))*BndrySigma*bb; // + sum;
                        //Real bdry = GradPhi.col(ii).tail<3>().dot(GradPhi.col(ii).tail<3>())*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());
        //std::cout << "A\n" << A << std::endl;
    }

    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

            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 avg(0);
            Real GG[4];
            for (int ii=0; ii<4; ++ii) {
                GG[ii] = vtkGrid->GetPointData()->GetScalars("G")->GetTuple(ID[ii])[0];
                //avg /= 4.;
            }
            if ( std::abs( (GG[0]+GG[1]+GG[2]+GG[3])/4. - GG[0])  < 1e-5) {
                avg = GG[0];
            }

            for (int ii=0; ii<4; ++ii) {
            //     avg += vtkGrid->GetPointData()->GetScalars()->GetTuple(ID[ii])[0];
            //     //if ( std::abs(vtkGrid->GetPointData()->GetScalars()->GetTuple(ID[ii])[0]) > 1e-3 )
            //}
            //TODO this seems wrong!
            //avg /= 4.;
                //g(ID[ii]) +=  (V) * ( vtkGrid->GetPointData()->GetScalars("G")->GetTuple(ID[ii])[0] )  ;
                g(ID[ii]) += PI* (V) * avg;
                  //g(ID[ii]) +=  6.67 *(V/4.) * avg;
            }
            //g(ID[0]) +=  (V/4.) * avg;
        }

    }

    void FEM4EllipticPDE::SolveOLD(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::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();

    }

    // 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  -----