/*
 * EigenvalueCovariance.cpp
 *
 *  Created on: May 6, 2015
 *      Author: dbazazian
 */

     #include <iostream>
     #include <Eigen/Eigenvalues>
     #include <Eigen/Dense>
     #include <vector>
     #include <math.h>
     #include <cmath>
     #include <fstream>
     #include <string>
     #include <vector>
     #include <pcl/io/io.h>
     #include <pcl/io/pcd_io.h>
     #include <pcl/point_types.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/common/common_headers.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/visualization/cloud_viewer.h>
     #include <pcl/filters/passthrough.h>
     #include <pcl/ModelCoefficients.h>
     #include <pcl/filters/project_inliers.h>
     #include <pcl/features/shot_omp.h>
     #include "pcl/features/fpfh.h"
     #include <pcl/io/ply_io.h>

using namespace std;
using namespace Eigen;

int
main (int argc, char*argv[])
{
	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);

	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Wedge/W0.2S45T45.pcd", *cloud);
	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TwoPlane22.pcd", *cloud);
	  //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/CubeSharpEdge.pcd", *cloud);
	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/OnePlane.pcd", *cloud);
	   // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/bunny.pcd",*cloud);
	   // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Statue.pcd", *cloud);
	     // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/dragon.pcd", *cloud);
	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/IntersectionThreePlanes.pcd", *cloud);

	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AddingNoise/Bunny03Noise50.pcd", *cloud);
	  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TetrahedronMultiple.pcd", *cloud);

	    // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/SpherMultiple.pcd", *cloud);

	      // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/trim-starC.pcd", *cloud);
	      //	pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/VaseC.pcd", *cloud);
	       //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/twirlC.pcd", *cloud);
	      	//  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/fandiskC.pcd", *cloud);
	      //pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/sharp_sphereC.pcd", *cloud);

	   std::cout << "Number of points in the Cube Input cloud is:"<< cloud->points.size() << std::endl;

	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Normals (new pcl::PointCloud<pcl::PointXYZRGBA>);
	  Normals->resize(cloud->size());

	  // K nearest neighbor search
	  int KNumbersNeighbor = 10; // numbers of neighbors 7 , 120
	  std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
	  std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

	  int* NumbersNeighbor = new  int [cloud ->points.size ()];
	  pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
	  kdtree.setInputCloud (cloud);
	  pcl::PointXYZRGBA searchPoint;

	  double* SmallestEigen = new  double [cloud->points.size() ];
	  double* MiddleEigen = new  double [cloud->points.size() ];
	  double* LargestEigen = new  double [cloud->points.size() ];

	  double* DLS = new  double [cloud->points.size() ];
	  double* DLM = new  double [cloud->points.size() ];
	  double* DMS = new  double [cloud->points.size() ];
	  double* Sigma = new  double [cloud->points.size() ];

//		std::vector<double> SmallestEigen;
//		std::vector<double> MiddleEigen;
//		std::vector<double> LargestEigen;
//
//		std::vector<double> DLS;
//		std::vector<double> DML;
//		std::vector<double> DMS;

	    //  ************ All the Points of the cloud *******************
	for (size_t i = 0; i < cloud ->points.size (); ++i) {

	searchPoint.x =   cloud->points[i].x;
	searchPoint.y =   cloud->points[i].y;
	searchPoint.z =   cloud->points[i].z;

	if ( kdtree.nearestKSearch (searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0 ) {
		 NumbersNeighbor[i]= NeighborsKNSearch.size (); }
	    else { NumbersNeighbor[i] = 0; }

	float Xmean; float Ymean; float Zmean;
	float sum= 0.00;
// Computing Covariance Matrix
	for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
      sum += cloud->points[ NeighborsKNSearch[ii] ].x; }
	Xmean = sum / NumbersNeighbor[i] ;
	sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].y;}
			Ymean = sum / NumbersNeighbor[i] ;
		 sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].z;}
			Zmean = sum / NumbersNeighbor[i] ;

			float	CovXX;  float CovXY; float CovXZ; float CovYX; float CovYY; float CovYZ; float CovZX; float CovZY; float CovZZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].x - Xmean )  );}
			CovXX = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovXY = sum / ( NumbersNeighbor[i]-1) ;

			CovYX = CovXY ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovXZ= sum / ( NumbersNeighbor[i]-1) ;

			CovZX = CovXZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovYY = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovYZ = sum / ( NumbersNeighbor[i]-1) ;

			CovZY = CovYZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].z - Zmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovZZ = sum / ( NumbersNeighbor[i]-1) ;

// Computing Eigenvalue and EigenVector
   Matrix3f Cov;
   Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

  SelfAdjointEigenSolver<Matrix3f> eigensolver(Cov);
  if (eigensolver.info() != Success) abort();

  double EigenValue1 = eigensolver.eigenvalues()[0];
  double EigenValue2 = eigensolver.eigenvalues()[1];
  double EigenValue3 = eigensolver.eigenvalues()[2];

  double Smallest = 0.00; double Middle = 0.00; double Largest= 0.00;
  if (EigenValue1<  EigenValue2 ) { Smallest =  EigenValue1 ; } else { Smallest = EigenValue2 ; }
  if (EigenValue3<  Smallest ) { Smallest =  EigenValue3 ; }

  if(EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3) {
	  Smallest = EigenValue1;
  if(EigenValue2 <= EigenValue3) {Middle = EigenValue2; Largest = EigenValue3;}
  else {Middle = EigenValue3; Largest = EigenValue2;}
  }

  if(EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
  {
	  Largest = EigenValue1;
  if(EigenValue2 <= EigenValue3) { Smallest = EigenValue2; Middle = EigenValue3; }
  else {Smallest = EigenValue3; Middle = EigenValue2;}
  }

  if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
  {
	  Middle = EigenValue1;
  if(EigenValue2 >= EigenValue3){Largest = EigenValue2; Smallest = EigenValue3;}
  else{Largest = EigenValue3; Smallest = EigenValue2;}
  }

SmallestEigen[i]= Smallest ;
MiddleEigen[i]= Middle;
LargestEigen[i]= Largest;

DLS[i] =    std::abs ( SmallestEigen[i] / LargestEigen[i]) ;          // std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
DLM[i] = std::abs ( MiddleEigen[i] /  LargestEigen[i]) ;             // std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
DMS[i] = std::abs ( SmallestEigen[i] / MiddleEigen[i]) ;       // std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;
Sigma[i] = (SmallestEigen[i] ) / ( SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i] ) ;
	} // For each point of the cloud

	  std::cout<< " Computing Sigma is Done! " << std::endl;
// Color Map For the difference of the eigen values

	  double MaxD=0.00 ;
	  double MinD= cloud ->points.size ();
	  int Ncolors=256;

	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  	if (  Sigma [i] < MinD) MinD= Sigma [i];
	  	if (  Sigma[i] > MaxD) MaxD = Sigma [i];
	  }

	  std::cout<< " Minimum is :" << MinD<< std::endl;
	  std::cout<< " Maximum  is :" << MaxD << std::endl;

//   *****************************************
	/*
	  // computing the standard deviation
	 double ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += Sigma [i] ;}
	  double avg = ss / cloud ->points.size () ;
	  ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += (Sigma [i] -  avg ) * (  Sigma [i] -  avg ) ;}
	  double stddvtion =   sqrt (  ss  /  ( cloud ->points.size () - 1 )  ) ;

	  std::cout<< " Standard Deviation is :" << stddvtion << std::endl;

	   MaxD = ( 2 )* stddvtion;
	  //MaxD = 10* stddvtion;

	  // Color table
		double line;
		double code[Ncolors][3];
	   ifstream colorcode ( "/Path/TO/ArtificialPointClouds/JetColorDensity/ColorCodes256.txt" );
	   //store color codes in array
	    int i=0,j=0;
	    while( colorcode>> line ) {
	    code[i][j]=line;
	    j++;
	    if (j == 3)
	    i++;
	}
	    code[1][0] = 0;
	    code[1][1] = 0;
	    code[1][2] = 135.468;

	    // jet color map

	int level = 0;
	float step = ( ( MaxD -  MinD) / Ncolors ) ;
	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
if (  SmallestEigen [i] <= MaxD ) {
	    level = floor( (SmallestEigen [i] - MinD ) /  step ) ;

	    cloud->points[i].r = code[ level ][0];
	    cloud->points[i].g =  code[ level ][1];
	    cloud->points[i].b =  code[ level ][2];
} // if sigma less than Max
	    }
*/
//    *****************************************

//   *****************************************
	    int Edgepoints = 0;
	    // Red and white (khaki)

	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
	    	    cloud->points[i].r = 240;
	    	    cloud->points[i].g =  230 ;
	    	    cloud->points[i].b =  140;
	            }

	  int level = 0;
	  float step = ( ( MaxD -  MinD) / Ncolors ) ;
	 //  level = floor( (Sigma [i] - MinD ) /  step ) ;
for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  if ( Sigma [i] > ( MinD + ( 6* step) ) ) {  //6*step
	    cloud->points[i].r = 255;
	    cloud->points[i].g =  0 ;
	    cloud->points[i].b =  0;

	    //    Dim gray....
//		    cloud->points[i].r = 105;
//		    cloud->points[i].g = 105 ;
//		    cloud->points[i].b = 105;
	    Edgepoints ++;
        }
     }

//   *****************************************

std::cout<< " Number of Edge points  is :" << Edgepoints << std::endl;

	    // writing the Sigma on the disk
//	    	   		std::ofstream ofsSigma;
//	    	   		ofsSigma.open("/Path/TO/SigmaDragon.txt");
//	    	            for (size_t i = 0; i < cloud ->points.size (); ++i) {
//	    	            	ofsSigma << Sigma [i]<< ","<< std::endl ;
//	    	                    }

  	pcl::PLYWriter writePLY;
 //  writePLY.write ("/Path/TO/RatioSmallestEigen22.ply", *cloud,  false);
	 //   writePLY.write ("/Path/TO/CloudEigeJnetTwirl.ply", *cloud,  false);
	// writePLY.write ("/Path/TO/CloudEigeJnetDragon.ply", *cloud,  false);
  // writePLY.write ("/Path/TO/EigenTwoPlane90N10.ply", *cloud,  false);
  	 // writePLY.write ("/Path/TO/DragonRedWhite.ply", *cloud,  false);
  	// writePLY.write ("/Path/TO/IntersectionThreePlanes.ply", *cloud,  false);
  	// writePLY.write ("/Path/TO/BunnyNoise50.ply", *cloud,  false);
  	//writePLY.write ("/Path/TO/BunnyEdges.ply", *cloud,  false);
  	 writePLY.write ("/Path/TO/TetahedronMultiple.ply", *cloud,  false);

  pcl::visualization::CloudViewer viewer("Cloud Viewer");
  viewer.showCloud(cloud);
  while (!viewer.wasStopped ())
  {}

  return 0;
  }


#include "KolmogorovSmirnov.hxx"

#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <vector>
#include <iostream>
#include <fstream>
#include <map>

int main(int argc, char * argv[])
{
  std::string S_fname, X_fname;
  std::string cdf_name;

  namespace po = boost::program_options;

  po::options_description desc("Options:");
  desc.add_options()
    ("help", "help message")
    ("standard", po::value<std::string>(&S_fname)->required(), "Histogram file for null hypothesis")
    ("experiment", po::value<std::string>(&X_fname)->required(), "Histogram file for experimental result")
    ("cdf", po::value<std::string>(&cdf_name)->default_value(""), "CDF output file") ;

  po::positional_options_description pos_opts ;
  pos_opts.add("standard", 1);
  pos_opts.add("experiment", 1);

  po::variables_map vm;

  std::string what_am_i("Calculate Kolmogorov-Smirnov significance from normative and experimental histograms");

  try {
    po::store(po::command_line_parser(argc, argv).options(desc)
	      .positional(pos_opts).run(), vm);

    if(vm.count("help")) {
      std::cout << what_am_i
		<< desc << std::endl;
      exit(-1);
    }

    po::notify(vm);
  }
  catch(po::required_option & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }
  catch(po::error & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }

  std::ifstream Sst(S_fname);
  std::ifstream Xst(X_fname);

  std::vector<int> S;
  std::vector<int> X;

  for(int n; Sst >> n; ) {
    S.push_back(n);
  }
  for(int n; Xst >> n; ) {
    X.push_back(n);
  }

  // now calculate the KS score
  float a = 0.5;
  float sv[] = {5.0, 2.0, 1.0};
  for(float a = 0.1; a > 0.0001; a = 0.1 * a) {
    for(int i = 0; i < 3; i++) {
      float aa = a * sv[i] ;
      bool reject = KolmogorovSmirnov::test(aa, S, X);
      std::cout << boost::format("alpha = %f  reject = %c\n")
	% aa % ((char) (reject ? 'T' : 'F'));
    }
  }

  int Ssum = 0;
  int Xsum = 0;
  if(cdf_name.length() != 0) {
    std::vector<float> Scdf = KolmogorovSmirnov::cdf(S, Ssum);
    std::vector<float> Xcdf = KolmogorovSmirnov::cdf(X, Xsum);
    std::ofstream cof(cdf_name);
    for(int i = 0; i < Scdf.size(); i++) {
      cof << boost::format("%d %g %g\n") % i % Scdf[i] % Xcdf[i];
    }
    cof.close();
  }
}


#include <spatialops/structured/FVStaggered.h>
#include <spatialops/structured/FieldHelper.h>

#include <expression/Functions.h>
#include <expression/ExpressionTree.h>
#include <expression/ExprPatch.h>
#include <expression/FieldWriter.h>
#include <expression/dualtime/BlockImplicitBDFDualTimeIntegrator.h>
#include <expression/matrix-assembly/DenseSubMatrix.h>
#include <expression/matrix-assembly/EigenvalueExpression.h>

#include "InitialConditions.h"
#include "RhsThreeSpecies.h"

#include <iostream>
#include <fstream>

#include <boost/program_options.hpp>

namespace so = SpatialOps;
typedef so::SVolField FieldT;

namespace lv3 = LotkaVolterra3;

namespace po = boost::program_options;

#define ASSEMBLER(MatrixT, FieldT, name) \
  boost::shared_ptr<MatrixT<FieldT> > name = boost::make_shared<MatrixT<FieldT> >( #name );

using Expr::matrix::sensitivity;

int main( int iarg, char* carg[] )
{
  // set up initial conditions, parameters
  lv3::ModelParameters params;
  lv3::InitialConditions ics;

  // parse command line input
  {
    po::options_description desc( "Supported Options" );
    desc.add_options()
              ( "help", "print help message" )
              ( "r1"   , po::value<double>( &(params.r1    ) )->default_value( params.r1    ) , "intrinsic growth rate - evasive prey" )
              ( "r2"   , po::value<double>( &(params.r2    ) )->default_value( params.r2    ) , "intrinsic growth rate - bulky prey" )
              ( "K1"   , po::value<double>( &(params.K1    ) )->default_value( params.K1    ) , "carrying capacity - evasive prey" )
              ( "K2"   , po::value<double>( &(params.K2    ) )->default_value( params.K2    ) , "carrying capacity - bulky prey" )
              ( "beta1", po::value<double>( &(params.beta1 ) )->default_value( params.beta1 ) , "predator interaction term - evasive prey" )
              ( "beta2", po::value<double>( &(params.beta2 ) )->default_value( params.beta2 ) , "predator interaction term - evasive prey" )
              ( "alpha", po::value<double>( &(params.a12   ) )->default_value( params.a12   ) , "prey competition term, alpha = alpha_12 = 1/alpha_21" )
              ( "eta1" , po::value<double>( &(params.eta1  ) )->default_value( params.eta1  ) , "prey interaction term - evasive prey" )
              ( "eta2" , po::value<double>( &(params.eta2  ) )->default_value( params.eta2  ) , "prey interaction term - bulky prey" )
              ( "delta", po::value<double>( &(params.delta ) )->default_value( params.delta ) , "intrinsic death rate - predator" );
    po::variables_map args;
    try{
      po::store( po::parse_command_line( iarg, carg, desc ), args );
      po::notify( args );

      if( args.count( "alpha" ) ){
        params.a21 = 1.0 / params.a12;
      }
    }
    catch( std::exception& err ){
      std::cout << "\nError parsing command line options.\n\n"
                << err.what() << "\n\n" << desc << "\n";
      return -1;
    }
    if( args.count( "help" ) ){
      std::cout << desc << "\n";
      return 1;
    }
  }

  // generate tags
  lv3::Tags tagsN( Expr::STATE_N );
  lv3::Tags tagsE( Expr::STATE_NONE );

  // make a patch, operator database, and field manager list
  Expr::ExprPatch patch( 1 );
  SpatialOps::OperatorDatabase sodb;
  Expr::FieldManagerList& fml = patch.field_manager_list();

  // use an initialization tree to set initial values
  Expr::ExpressionFactory icFactory;
  Expr::ExpressionTree icTree( icFactory, patch.id(), "tree: initial conditions" );

  typedef Expr::ConstantExpr<FieldT>::Builder ConstantT;
  icTree.insert_tree( icFactory.register_expression( new ConstantT( tagsN.prey1StateTag   , ics.prey1    ) ) );
  icTree.insert_tree( icFactory.register_expression( new ConstantT( tagsN.prey2StateTag   , ics.prey2    ) ) );
  icTree.insert_tree( icFactory.register_expression( new ConstantT( tagsN.predatorStateTag, ics.predator ) ) );

  icTree.compute_sensitivities( tagsN.var_tags(), tagsN.var_tags() );

  icTree.register_fields( fml );
  icTree.bind_fields( fml );
  fml.allocate_fields( patch.field_info() );
  icTree.execute_tree();
  {
    std::ofstream f( "lotka-volterra-tree--initial-conditions.dot" );
    icTree.write_tree( f );
  }

  // make a dual time integrator
  typedef Expr::DualTime::BlockImplicitBDFDualTimeIntegrator<FieldT> IntegratorT;
  const Expr::Tag dtTag( "time step"     , Expr::STATE_NONE );
  const Expr::Tag dsTag( "dual time step", Expr::STATE_NONE );
  const int bdfOrder = 2;
  boost::shared_ptr<IntegratorT> integrator = boost::make_shared<IntegratorT>( patch.id(),
                                                                               "integrator",
                                                                               dtTag,
                                                                               dsTag,
                                                                               bdfOrder );

  // set the physical and dual time step sizes
  const double dtValue = 1e-3;
  const double dsValue = 1e16;
  typedef Expr::ConstantExpr<so::SingleValueField>::Builder ConstantSingleValueT;
  integrator->set_physical_time_step_expression( new ConstantSingleValueT( dtTag, dtValue ) );
  integrator->set_dual_time_step_expression    ( new ConstantSingleValueT( dsTag, dsValue ) );

  // register expressions for RHS calculation
  typedef lv3::Rhs<FieldT>::Builder RhsT;
  Expr::ExpressionFactory& factory = integrator->factory();
  factory.register_expression( new RhsT( params, tagsE ) );

  // add variables
  const int prey1Idx    = 2; const int prey1RhsIdx    = 0;
  const int prey2Idx    = 0; const int prey2RhsIdx    = 1;
  const int predatorIdx = 1; const int predatorRhsIdx = 2;

  integrator->add_variable<FieldT>( tagsE.prey1Name   , tagsE.prey1RhsTag   , prey1Idx, prey1RhsIdx );
  integrator->add_variable<FieldT>( tagsE.prey2Name   , tagsE.prey2RhsTag   , prey2Idx, prey2RhsIdx );
  integrator->add_variable<FieldT>( tagsE.predatorName, tagsE.predatorRhsTag, predatorIdx, predatorRhsIdx );
  integrator->done_adding_variables();

  // make the jacobian
  auto jacobian = [&]()
  {
    ASSEMBLER( Expr::matrix::DenseSubMatrix, FieldT, jac )

    jac->element<FieldT>(prey1RhsIdx,prey1Idx) = sensitivity( tagsE.prey1RhsTag   , tagsE.prey1StateTag    );
    jac->element<FieldT>(prey1RhsIdx,prey2Idx) = sensitivity( tagsE.prey1RhsTag   , tagsE.prey2StateTag    );
    jac->element<FieldT>(prey1RhsIdx,predatorIdx) = sensitivity( tagsE.prey1RhsTag   , tagsE.predatorStateTag );

    jac->element<FieldT>(prey2RhsIdx,prey1Idx) = sensitivity( tagsE.prey2RhsTag   , tagsE.prey1StateTag    );
    jac->element<FieldT>(prey2RhsIdx,prey2Idx) = sensitivity( tagsE.prey2RhsTag   , tagsE.prey2StateTag    );
    jac->element<FieldT>(prey2RhsIdx,predatorIdx) = sensitivity( tagsE.prey2RhsTag   , tagsE.predatorStateTag );

    jac->element<FieldT>(predatorRhsIdx,prey1Idx) = sensitivity( tagsE.predatorRhsTag, tagsE.prey1StateTag    );
    jac->element<FieldT>(predatorRhsIdx,prey2Idx) = sensitivity( tagsE.predatorRhsTag, tagsE.prey2StateTag    );
    jac->element<FieldT>(predatorRhsIdx,predatorIdx) = sensitivity( tagsE.predatorRhsTag, tagsE.predatorStateTag );

    jac->finalize();

    return jac;
  };

  typedef Expr::EigenvalueExpression<FieldT>::Builder EigT;
  integrator->register_root_expression( new EigT( tagsE.realPartEigTags,
                                                  tagsE.imagPartEigTags,
                                                  jacobian(),
                                                  3 ) );

  integrator->set_jacobian( jacobian() );

  integrator->prepare_for_integration( fml, sodb, patch.field_info() );
  {
    std::ofstream f("lotka-volterra-tree--nonlinear-solve-execution.dot");
    integrator->get_tree().write_tree( f );
  }

  integrator->lock_all_execution_fields();

  // set up output of fields of interest
  Expr::FieldOutputDatabase db( fml, "lotka-volterra-output", true );
  db.request_field_output<FieldT>( tagsN.prey1StateTag    );
  db.request_field_output<FieldT>( tagsN.prey2StateTag    );
  db.request_field_output<FieldT>( tagsN.predatorStateTag );
  db.request_field_output<FieldT>( tagsE.realPartEigTags[0] );
  db.request_field_output<FieldT>( tagsE.realPartEigTags[1] );
  db.request_field_output<FieldT>( tagsE.realPartEigTags[2] );
  db.request_field_output<FieldT>( tagsE.imagPartEigTags[0] );
  db.request_field_output<FieldT>( tagsE.imagPartEigTags[1] );
  db.request_field_output<FieldT>( tagsE.imagPartEigTags[2] );

  // set up the simulation and solve!
  const unsigned maxIterPerStep = 100; // maximum nonlinear iterations per time step
  unsigned totalIter = 0;
  unsigned stepCount = 0;
  unsigned dualIter  = 0;
  bool     converged = false;
  double t = 0;

  const double tend = 10.0; // final time

  // write out the initial condition
  db.write_database( "step-" + boost::lexical_cast<std::string>( stepCount ) );

  // run the time and nonlinear solver (dual time) loops
  std::cout << "-- running simulation...\n";
  while( t<tend ){
    dualIter = 0;
    integrator->begin_time_step();
    do{
      integrator->advance_dualtime( converged );
      dualIter++;
      totalIter++;
    } while( !converged && dualIter <= maxIterPerStep );
    integrator->end_time_step();
    stepCount++;
    t += dtValue;
    db.write_database( "step-" + boost::lexical_cast<std::string>( stepCount ) );
  }
  std::cout << "-- total iteration count: " << totalIter << '\n';
  std::cout << "-- finished running Lotka-Volterra 3-species example\n";
  return 0;
}


#ifndef _GLOB_H_
#define _GLOB_H_

#include <sys/resource.h>

#include <exception>
#include <iostream>
#include <utility>
#include <cstdlib>
#include <fstream>
#include <memory>
#include <string>
#include <mutex>
#include <list>
#include <map>
#include <random>
#include <omp.h>
#include <iomanip>
#include <stdexcept>
#include <chrono>
#include <cmath>
#include <cassert>
#include <algorithm>

#include <json.hpp>
#include <progressBar.hpp>

#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>

#include <CGAL/Line_2.h>
#include <CGAL/Origin.h>
#include <CGAL/Polygon_2.h>
//#include <CGAL/Cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>  //new
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/squared_distance_2.h>
#include <CGAL/Aff_transformation_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>

#define CGAL_HAS_THREADS

#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/Constants.hpp>
#include <GeographicLib/GeoCoords.hpp>
#include <GeographicLib/LocalCartesian.hpp>

#include <osrm/match_parameters.hpp>
#include <osrm/nearest_parameters.hpp>
#include <osrm/route_parameters.hpp>
#include <osrm/table_parameters.hpp>
#include <osrm/trip_parameters.hpp>
#include <osrm/coordinate.hpp>
#include <osrm/engine_config.hpp>
#include <osrm/json_container.hpp>
#include <osrm/osrm.hpp>
#include <osrm/status.hpp>

struct FaceInfo2 {
	FaceInfo2() {}
	int nesting_level;
	bool in_domain()
	{
		return nesting_level%2 == 1;
	}
};

typedef CGAL::Exact_predicates_inexact_constructions_kernel      K;//new
//typedef CGAL::Simple_cartesian<double> K;

typedef CGAL::Aff_transformation_2<K> Transformation;

typedef CGAL::Line_2<K>     Line2D;
typedef CGAL::Point_2<K>    Point2D;
typedef CGAL::Triangle_2<K> Triangle2D;//new
typedef CGAL::Vector_2<K>   Vector2D;
typedef CGAL::Polygon_2<K>  Polygon2D;

typedef CGAL::Triangulation_vertex_base_2<K>                      Vb;
typedef CGAL::Triangulation_face_base_with_info_2<FaceInfo2,K>    Fbb;//new
//typedef CGAL::Delaunay_mesh_face_base_2<K>                        Fb;
typedef CGAL::Constrained_triangulation_face_base_2<K,Fbb>        Fb;//new
typedef CGAL::Triangulation_data_structure_2<Vb, Fb>              Tds;

typedef CGAL::Exact_predicates_tag                                Itag;//new
typedef CGAL::Constrained_Delaunay_triangulation_2<K, Tds, Itag>  CDT;//new
//typedef CGAL::Constrained_Delaunay_triangulation_2<K, Tds>        CDT;

typedef CGAL::Delaunay_mesh_size_criteria_2<CDT>                  Mesh_2_criteria;

using json=nlohmann::json;
using namespace GeographicLib;

extern bool        g_showProgressBar;
extern float       g_closeEnough;
extern float       g_randomWalkwayRadius;
extern float       g_attractionRadius;
extern uint32_t    g_epochInitSim;
extern uint32_t    g_currTimeSim;
extern std::string g_baseDir;
extern uint32_t    g_AgentsMem;
extern float       g_deltaT;

//Variables globales para medir tiempo
extern uint32_t g_timeExecMakeAgents;
extern uint32_t g_timeExecCal;
extern uint32_t g_timeExecSim;

extern std::vector<std::string> g_logZonesDensity;
extern std::vector<uint32_t>    g_logUsePhone;
extern std::vector<std::string> g_logVelocity;

//enum model_t {ShortestPath=0, FollowTheCrowd=1, RandomWalkway=2, WorkingDay, SNITCH=666};
enum model_t {Residents=0, Visitors_I=1, Visitors_II=2};

extern std::map<std::string, model_t> model_map;

#endif


#define CATCH_CONFIG_MAIN

#include "CALPHADSpeciesPhaseGibbsEnergy.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <string>

namespace pt = boost::property_tree;

TEST_CASE("Loop CALPHADSpeciesPhaseGibbsEnergy",
    "[loop CALPHADSpeciesPhaseGibbsEnergy]")
{
    std::cout << std::setprecision(12);

    // Ni melting temperature
    double temperature = 1728.;

    // read calphad database
    std::string calphad_filename("../thermodynamic_data/calphadAuNi.json");
    pt::ptree calphad_db;
    std::cout << "Read " << calphad_filename << std::endl;
    try
    {
        pt::read_json(calphad_filename, calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    // Ni is species "B" in calphadAuNi.json
    std::cout << "Read species B data" << std::endl;
    pt::ptree spB;
    try
    {
        spB = calphad_db.get_child("SpeciesB");
    }
    catch (std::exception& e)
    {
        std::cerr << "Read species: exception caught: " << e.what()
                  << std::endl;
    }

    pt::ptree liqB;
    try
    {
        liqB = spB.get_child("PhaseL");
    }
    catch (std::exception& e)
    {
        std::cerr << "Read phase: exception caught: " << e.what() << std::endl;
    }

    // liquid
    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy energyNiLiquid;
    energyNiLiquid.initialize("NiL", liqB);

    const double Tmin     = temperature - 1.;
    const double Tmax     = temperature + 1.;
    const int nTintervals = 10;
    const double deltaT   = (Tmax - Tmin) / (double)nTintervals;

    std::vector<double> energy(nTintervals + 1);

    {
        // serial loop
        for (int i = 0; i < nTintervals + 1; i++)
        {
            double t  = Tmin + i * deltaT;
            energy[i] = energyNiLiquid.fenergy(t);

            std::cout << "-------------------------------" << std::endl;
            std::cout << "Temperature = " << t << std::endl;
            std::cout << "energy      = " << energy[i] << std::endl;
        }
    }

    // parallel loop
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double t = Tmin + i * deltaT;
        double e       = energyNiLiquid.fenergy(t);

        CHECK(e == Approx(energy[i]).margin(1.e-6));
    }

#ifdef HAVE_OPENMP_OFFLOAD
    double* xdev = new double[nTintervals + 1];

    std::cout << "=========================\n";
    std::cout << "GPU offload testing:" << std::endl;

// clang-format off
#pragma omp target map(to:energyNiLiquid) \
                   map(from: xdev[:(nTintervals+1)])
// clang-format on
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double t = Tmin + i * deltaT;
        double e       = energyNiLiquid.fenergy(t);

        xdev[i] = e;
    }

    // verify results
    for (int i = 0; i < nTintervals + 1; i++)
    {
        std::cout << "Device: x=" << xdev[i] << "," << std::endl;
        CHECK(xdev[i] == Approx(energy[i]).margin(1.e-6));
    }

    delete[] xdev;
#endif
}


#ifndef GP_HPP_INCLUDED
#define GP_HPP_INCLUDED
#include <vector>
#include <memory>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

namespace boat {
double normal_lnp(double x, double mean, double variance);
typedef enum { SQUARED_EXP, MATERN52 } kernel_t;

class GPParams{
  public:
  GPParams():mean_(0.0), amplitude_(1.0), default_noise_(0.1),
  kernel_(MATERN52) {}

  void amplitude(double amplitude) {
    amplitude_ = amplitude;
  }

  void stdev(double stdev) {
    amplitude_ = stdev * stdev;
  }

  void default_noise(double default_noise) {
    assert(default_noise >= 0.0);
    //default_noise_ = std::max(default_noise, 1e-4);
    default_noise_ = default_noise;
  }

  void mean(double mean) {
    mean_ = mean;
  }

  void linear_scales(const std::vector<double>& linear_scales){
    linear_scales_ = linear_scales;
    inv_linear_scales_ = Eigen::VectorXd::Map(linear_scales.data(), linear_scales.size())
                           .array()
                           .inverse();
  }

  void kernel(kernel_t kernel){
    kernel_ = kernel;
  }

  double stdev() const {
    return sqrt(amplitude_);
  }

  double amplitude() const {
    return amplitude_;
  }

  double mean() const {
    return mean_;
  }

  double default_noise() const {
    return default_noise_;
  }

  const Eigen::VectorXd& inv_linear_scales() const {
    return inv_linear_scales_;
  }

  const std::vector<double>& linear_scales() const {
    return linear_scales_;
  }

  kernel_t kernel() const {
    return kernel_;
  }

  private:
  double mean_;
  double amplitude_;
  double default_noise_;
  std::vector<double> linear_scales_;
  Eigen::VectorXd inv_linear_scales_;
  kernel_t kernel_;
};

struct GaussianDistrib {
  double mu_;
  double var_;
  double mu() {
    return mu_;
  }
  double var() {
    return var_;
  }
  double stdev() {
    return sqrt(var_);
  }
};

class GP {
  friend class TreedGPS;
  public:
  GP();
  GP(GPParams params);
  void set_params(GPParams params);
  int num_dims() const;
  double observe(const std::vector<double>& x_new, double y_new);
  double observe(const std::vector<double>& x_new, double y_new, double noise);

  double predict_mean(const std::vector<double>& x_new) const;
  GaussianDistrib predict_distrib(const std::vector<double>& x_new) const;
  void print() const;

   //private:
   double observe_i(const std::vector<double>& x_new,
                    double y_new, double noise_var);
   void compute_cholesky() const;
   int at_index(const Eigen::RowVectorXd& v) const;
   void remove_observation(int i);
   Eigen::MatrixXd covariance(const Eigen::MatrixXd& x1,
                              const Eigen::MatrixXd& x2) const;

  void compute_cholesky_from_scratch() const;

  // Parameters
  GPParams params_;

  // Processed data
  Eigen::MatrixXd observed_x_;
  Eigen::VectorXd observed_y_;
  Eigen::VectorXd noise_var_;

  mutable Eigen::MatrixXd covariance_cholesky_;
  mutable Eigen::VectorXd alpha_;
};

class TreedGPS {
  public:
  TreedGPS();
  TreedGPS(GPParams params, int observation_thresh = 64, int overlap = 3);
  TreedGPS(const TreedGPS& other);
  TreedGPS& operator=(const TreedGPS& other);
  void set_params(GPParams params);

  double observe(const std::vector<double>& x_new, double y_new);
  double observe(const std::vector<double>& x_new, double y_new, double noise);

  double predict_mean(const std::vector<double>& x_new) const;
  GaussianDistrib predict_distrib(const std::vector<double>& x_new) const;

  private:
  bool side(const std::vector<double>& x_new) const;

  //Lots of helpers for the splitting
  std::pair<std::vector<int>, std::vector<int>> compute_thresh_and_divide(double max_diff);
  std::pair<std::vector<int>, std::vector<int>> compute_thresh_props();
  void check_thresh();
  void copy_observation(bool s, int i);
  void compute_children_cholesky();

  int observation_thresh_;
  int overlap_;

  int thresh_dim_;
  double thresh_;
  double inv_ls_along_thresh_dim_;
  std::unique_ptr<GP> gp_;
  std::unique_ptr<TreedGPS> left_;
  std::unique_ptr<TreedGPS> right_;
};
}
#endif  // GP_HPP_INCLUDED


#include "nested_sampling.hpp"
#include "MathTools.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include "OrthogPolyApproximation.hpp"

namespace Dakota{

#define PI 4.0*atan(1.0)

void Sampler::generate_samples( int num_dims, int num_samples,
				RealMatrix &samples ){
  RealMatrix initial_samples;
  enrich_samples( num_dims, initial_samples, num_samples, samples );
}

void Sampler::enrich_samples( int num_dims, const RealMatrix &initial_samples,
			      int num_new_samples,
			      const RealMatrix & candidate_samples,
			      RealMatrix &selected_samples ){

  // Remove any candidate samples already in the initial sample set
  RealMatrix unique_candidate_samples;
  get_unique_samples( initial_samples, num_new_samples, candidate_samples,
		      unique_candidate_samples );

  // Get indices of best candidate samples
  IntVector selected_candidate_indices;
  get_enriched_sample_indices( num_dims, initial_samples, num_new_samples,
			       unique_candidate_samples,
			       selected_candidate_indices );

  // Allocate memory for selected samples
  int num_initial_samples = initial_samples.numCols(),
    num_total_samples = num_new_samples + num_initial_samples;
  // Be careful not to disconnect if this is a Teuchos::View
  if (selected_samples.numRows() != num_dims ||
      selected_samples.numCols() != num_total_samples)
    selected_samples.shapeUninitialized( num_dims, num_total_samples );

  // Copy initial samples into selected samples
  RealMatrix initial_samples_view( Teuchos::View, selected_samples,
				   num_dims, num_initial_samples );
  Pecos::copy_matrix( initial_samples, initial_samples_view, num_dims,
		      num_initial_samples );

  // Copy selected candidates into selected samples below the initial samples;
  RealMatrix candidate_samples_view( Teuchos::View, selected_samples,
				     num_dims, num_new_samples,
				     0, num_initial_samples );
  Pecos::extract_submatrix_from_column_indices( unique_candidate_samples,
						selected_candidate_indices,
						candidate_samples_view );

}

/// Remove any candidate samples that are already in an initial set of samples
void Sampler::get_unique_samples( const RealMatrix &initial_samples,
				  int num_new_samples,
				  const RealMatrix &candidate_samples,
				  RealMatrix &unique_candidate_samples ){

  IntVector unique_candidate_indices;
  Pecos::set_difference_matrix_columns( candidate_samples, initial_samples,
					unique_candidate_indices );
  if ( unique_candidate_indices.length() < num_new_samples ){
    std::string msg = "get_unique_candidates. Not enough unique candidate";
    msg += " samples";
    throw( std::runtime_error( msg ) );
  }

  Pecos::extract_submatrix_from_column_indices( candidate_samples,
						unique_candidate_indices,
						unique_candidate_samples );
}

void LejaSampler::
get_enriched_sample_indices( int num_dims,
			     const RealMatrix &initial_samples,
			     int num_new_samples,
			     const RealMatrix & candidate_samples,
			     IntVector &selected_candidate_indices ){
  // This function assumes the candidate samples are unique

  // Combine initial samples and candidate samples
  RealMatrix samples;
  Pecos::hstack( initial_samples, candidate_samples, samples );

  // Build the basis matrix, i.e. evaluate each basis function at each point
  RealMatrix basis_matrix;
  build_basis_matrix( samples, basis_matrix );

  if ( precondition_ )
    apply_preconditioning( basis_matrix );

  int num_initial_samples = initial_samples.numCols();
  int num_total_samples = num_new_samples + num_initial_samples;
  RealMatrix A, L_factor, U_factor;
  IntVector selected_sample_indices;
  Pecos::truncated_pivoted_lu_factorization( basis_matrix, L_factor, U_factor,
					     selected_sample_indices,
					     num_total_samples,
					     num_initial_samples );
  if ( selected_sample_indices.length() < num_total_samples ) {
    std::stringstream msg;
    msg << "enrich_samples: The basis matrix has rank "
	<< selected_sample_indices.length()<< " less than num_total_samples "
	<< num_total_samples<< ". Try increasing the degree of the basis.";
    throw( std::runtime_error( msg.str() ) );
  }

  // Get indices of columns to keep in candidate sample matrix
  // selected_samples_indices contains indexes into the larger
  // samples matrix which consists of both initial samples and candidate samples
  Pecos::resize( selected_candidate_indices, num_new_samples );
  for ( int i=0; i<num_new_samples; i++ )
    selected_candidate_indices[i] =
      selected_sample_indices[i+num_initial_samples] - num_initial_samples;
}

void LejaSampler::get_candidate_samples( int num_dims, int num_samples, int seed,
					 RealMatrix &candidate_samples ){
  boost::mt19937 rng;
  if (seed)
    rng.seed(seed);
  boost::uniform_real<double> uniform_sampler(0.,PI);
  Pecos::reshape( candidate_samples, num_dims, num_samples );
  for (int j=0; j<num_samples; j++)
    for (int i=0; i<num_dims; i++)
      candidate_samples(i,j) = -std::cos( uniform_sampler(rng) );
}

void LejaSampler::enrich_samples( int num_dims,
				  const RealMatrix &initial_samples,
				  int num_new_samples,
				  RealMatrix &samples ) {
  RealMatrix candidate_samples;
  get_candidate_samples( num_dims, numCandidateSamples_, seed_,
			 candidate_samples );
  Sampler::enrich_samples( num_dims, initial_samples, num_new_samples,
			   candidate_samples, samples );
}

void LejaSampler::set_seed( int seed ){
  seed_ = seed;
}

void LejaSampler::set_num_candidate_samples( int num_candidate_samples ){
  numCandidateSamples_ = num_candidate_samples;
}

void LejaSampler::set_precondition( bool precondition ){
  precondition_ = precondition;
}

void LejaSampler::apply_preconditioning( RealMatrix &basis_matrix ) {
  RealVector wts( basis_matrix.numRows() ); // initialize to zero
  for (int j=0; j<basis_matrix.numCols(); j++)
    for (int i=0; i<basis_matrix.numRows(); i++)
      wts[i] += basis_matrix(i,j)*basis_matrix(i,j);

  for (int i=0; i<basis_matrix.numRows(); i++){
    wts[i] = std::sqrt( (double)basis_matrix.numCols() / wts[i] );
    for (int j=0; j<basis_matrix.numCols(); j++)
      basis_matrix(i,j) *= wts[i];
  }
}

void LejaSampler::build_basis_matrix( const RealMatrix &samples,
				      RealMatrix &basis_matrix ){
  // this function cannot be const because PolynomialBasis
  // is used by multivariate_polynomial which calls
  // PolynomialBasis[i].type1_value(x,order). But there is at least one instance
  // umericGenOrthogPolynomial where type1_value is not const. It needs to
  // update internal sturctures based upon polynomial order specified
  Pecos::OrthogPolyApproximation::basis_matrix(samples, polynomialBasis,
					       multiIndex, basis_matrix);
}

void LejaSampler::set_total_degree_basis_from_num_samples( int num_vars, int num_samples ){
  int degree = Pecos::get_total_degree_from_num_samples( num_vars, num_samples );
  UShortArray exp_order(num_vars, degree);
  Pecos::SharedPolyApproxData::total_order_multi_index(exp_order, multiIndex);
}

void LejaSampler::
set_polynomial_basis( std::vector<Pecos::BasisPolynomial> &polynomial_basis ){
  polynomialBasis = polynomial_basis;
}

} // namespace Dakota


/**
 * Copyright (c) 2021 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "multipolymesh.hpp"

#include "math/math.hpp"

#include <boost/numeric/ublas/matrix.hpp>

namespace geometry
{
namespace ublas = boost::numeric::ublas;

FacePlaneCrs::FacePlaneCrs(const math::Point3& p1,
                           const math::Point3& p2,
                           const math::Point3& p3)
{
    // get base vectors
    math::Point3 n1 = math::normalize(p2 - p1);
    normal_ = math::normalize(math::crossProduct(p2 - p1, p3 - p1));
    math::Point3 n2 = math::normalize(math::crossProduct(normal_, n1));

    p2g_ = math::identity4();

    auto col1 = ublas::column(p2g_, 0);
    auto col2 = ublas::column(p2g_, 1);
    auto col3 = ublas::column(p2g_, 2);
    auto col4 = ublas::column(p2g_, 3);
    ublas::subrange(col1, 0, 3) = n1;
    ublas::subrange(col2, 0, 3) = n2;
    ublas::subrange(col3, 0, 3) = normal_;
    ublas::subrange(col4, 0, 3) = p1;

    g2p_ = math::matrixInvert(p2g_);
}

} // namespace geometry


// ---------------------------------------------------------------------
//
// Copyright (c) 2017 - 2020 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files
#include <SAMRAI_config.h>

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_function.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/IBTK_MPI.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

// Elasticity model data.
namespace ModelData
{
// The tether penalty functions each require some data that is set in the
// input file. This data is passed to each object through the void *ctx
// context data pointer. Here we collect all relevant tether data in a struct:
struct ElasticityData
{
    const double c1_s;
    const double kappa_s;
    const double mu_s;
    const double lambda_s;

    ElasticityData(Pointer<Database> input_db)
        : c1_s(input_db->getDouble("C1_S")),
          kappa_s(input_db->getDouble("KAPPA_S")),
          mu_s(input_db->getDouble("MU_S")),
          lambda_s(input_db->getDouble("LAMBDA_S"))
    {
    }
};

// Tether (penalty) force function for the solid block.
void
block_tether_force_function(VectorValue<double>& F,
                            const TensorValue<double>& /*FF*/,
                            const libMesh::Point& X,
                            const libMesh::Point& s,
                            Elem* const /*elem*/,
                            const std::vector<const std::vector<double>*>& /*var_data*/,
                            const std::vector<const std::vector<VectorValue<double> >*>& /*grad_var_data*/,
                            double /*time*/,
                            void* ctx)
{
    const ElasticityData* const elasticity_data = reinterpret_cast<ElasticityData*>(ctx);

    F = elasticity_data->kappa_s * (s - X);
    return;
} // block_tether_force_function

// Tether (penalty) force function for the thin beam.
void
beam_tether_force_function(VectorValue<double>& F,
                           const TensorValue<double>& /*FF*/,
                           const libMesh::Point& X,
                           const libMesh::Point& s,
                           Elem* const /*elem*/,
                           const std::vector<const std::vector<double>*>& /*var_data*/,
                           const std::vector<const std::vector<VectorValue<double> >*>& /*grad_var_data*/,
                           double /*time*/,
                           void* ctx)
{
    const double r = sqrt((s(0) - 0.2) * (s(0) - 0.2) + (s(1) - 0.2) * (s(1) - 0.2));
    if (r <= 0.05)
    {
        const ElasticityData* const elasticity_data = reinterpret_cast<ElasticityData*>(ctx);
        F = elasticity_data->kappa_s * (s - X);
    }
    else
    {
        F.zero();
    }
    return;
} // beam_tether_force_function

// (Penalty) stress tensor function for the solid block.
void
block_PK1_stress_function(TensorValue<double>& PP,
                          const TensorValue<double>& FF,
                          const libMesh::Point& /*X*/,
                          const libMesh::Point& /*s*/,
                          Elem* const /*elem*/,
                          const std::vector<const std::vector<double>*>& /*var_data*/,
                          const std::vector<const std::vector<VectorValue<double> >*>& /*grad_var_data*/,
                          double /*time*/,
                          void* ctx)
{
    const ElasticityData* const elasticity_data = reinterpret_cast<ElasticityData*>(ctx);

    PP = 2.0 * elasticity_data->c1_s * (FF - tensor_inverse_transpose(FF, NDIM));
    return;
} // block_PK1_stress_function

void
beam_PK1_stress_function(TensorValue<double>& PP,
                         const TensorValue<double>& FF,
                         const libMesh::Point& /*X*/,
                         const libMesh::Point& /*s*/,
                         Elem* const /*elem*/,
                         const std::vector<const std::vector<double>*>& /*var_data*/,
                         const std::vector<const std::vector<VectorValue<double> >*>& /*grad_var_data*/,
                         double /*time*/,
                         void* ctx)
{
    const ElasticityData* const elasticity_data = reinterpret_cast<ElasticityData*>(ctx);
    static const TensorValue<double> II(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
    const TensorValue<double> CC = FF.transpose() * FF;
    const TensorValue<double> EE = 0.5 * (CC - II);
    const TensorValue<double> SS = elasticity_data->lambda_s * EE.tr() * II + 2.0 * elasticity_data->mu_s * EE;
    PP = FF * SS;
    return;
} // beam_PK1_stress_function
} // namespace ModelData
using namespace ModelData;

// Function prototypes
static ofstream drag_stream, lift_stream, A_x_posn_stream, A_y_posn_stream;
void postprocess_data(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                      Pointer<INSHierarchyIntegrator> navier_stokes_integrator,
                      Mesh& beam_mesh,
                      EquationSystems* beam_equation_systems,
                      Mesh& block_mesh,
                      EquationSystems* block_equation_systems,
                      const int iteration_num,
                      const double loop_time,
                      const string& data_dump_dirname);

/*******************************************************************************
 * For each run, the input filename and restart information (if needed) must   *
 * be given on the command line.  For non-restarted case, command line is:     *
 *                                                                             *
 *    executable <input file name>                                             *
 *                                                                             *
 * For restarted run, command line is:                                         *
 *                                                                             *
 *    executable <input file name> <restart directory> <restart number>        *
 *                                                                             *
 *******************************************************************************/

int
main(int argc, char* argv[])
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    const LibMeshInit& init = ibtk_init.getLibMeshInit();

    { // cleanup dynamically allocated objects prior to shutdown

        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");
        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Get various standard options set in the input file.
        const bool dump_viz_data = app_initializer->dumpVizData();
        const int viz_dump_interval = app_initializer->getVizDumpInterval();
        const bool uses_visit = dump_viz_data && app_initializer->getVisItDataWriter();
#ifdef LIBMESH_HAVE_EXODUS_API
        const bool uses_exodus = dump_viz_data && !app_initializer->getExodusIIFilename().empty();
#else
        const bool uses_exodus = false;
        if (!app_initializer->getExodusIIFilename().empty())
        {
            plog << "WARNING: libMesh was compiled without Exodus support, so no "
                 << "Exodus output will be written in this program.\n";
        }
#endif
        const string block_exodus_filename = app_initializer->getExodusIIFilename("block");
        const string beam_exodus_filename = app_initializer->getExodusIIFilename("beam");

        const bool dump_restart_data = app_initializer->dumpRestartData();
        const int restart_dump_interval = app_initializer->getRestartDumpInterval();
        const string restart_dump_dirname = app_initializer->getRestartDumpDirectory();
        const string restart_read_dirname = app_initializer->getRestartReadDirectory();
        const int restart_restore_num = app_initializer->getRestartRestoreNumber();

        const bool dump_postproc_data = app_initializer->dumpPostProcessingData();
        const int postproc_data_dump_interval = app_initializer->getPostProcessingDataDumpInterval();
        const string postproc_data_dump_dirname = app_initializer->getPostProcessingDataDumpDirectory();
        if (dump_postproc_data && (postproc_data_dump_interval > 0) && !postproc_data_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(postproc_data_dump_dirname);
        }

        const bool dump_timer_data = app_initializer->dumpTimerData();
        const int timer_dump_interval = app_initializer->getTimerDumpInterval();

        // Create a simple FE mesh.
        const double dx = input_db->getDouble("DX");
        const double ds = input_db->getDouble("MFAC") * dx;

        Mesh block_mesh(init.comm(), NDIM);
        string block_elem_type = input_db->getString("BLOCK_ELEM_TYPE");
        const double R = 0.05;
        if (block_elem_type == "TRI3" || block_elem_type == "TRI6")
        {
#ifdef LIBMESH_HAVE_TRIANGLE
            const int num_circum_nodes = ceil(2.0 * M_PI * R / ds);
            for (int k = 0; k < num_circum_nodes; ++k)
            {
                const double theta = 2.0 * M_PI * static_cast<double>(k) / static_cast<double>(num_circum_nodes);
                block_mesh.add_point(libMesh::Point(R * cos(theta), R * sin(theta)));
            }
            TriangleInterface triangle(block_mesh);
            triangle.triangulation_type() = TriangleInterface::GENERATE_CONVEX_HULL;
            triangle.elem_type() = Utility::string_to_enum<ElemType>(block_elem_type);
            triangle.desired_area() = sqrt(3.0) / 4.0 * ds * ds;
            triangle.insert_extra_points() = true;
            triangle.smooth_after_generating() = true;
            triangle.triangulate();
            block_mesh.prepare_for_use();
#else
            TBOX_ERROR("ERROR: libMesh appears to have been configured without support for Triangle,\n"
                       << "       but Triangle is required for TRI3 or TRI6 elements.\n");
#endif
        }
        else
        {
            // NOTE: number of segments along boundary is 4*2^r.
            const double num_circum_segments = ceil(2.0 * M_PI * R / ds);
            const int r = log2(0.25 * num_circum_segments);
            MeshTools::Generation::build_sphere(block_mesh, R, r, Utility::string_to_enum<ElemType>(block_elem_type));
        }
        for (MeshBase::node_iterator n_it = block_mesh.nodes_begin(); n_it != block_mesh.nodes_end(); ++n_it)
        {
            Node& n = **n_it;
            n(0) += 0.2;
            n(1) += 0.2;
        }

        Mesh beam_mesh(init.comm(), NDIM);
        string beam_elem_type = input_db->getString("BEAM_ELEM_TYPE");
        MeshTools::Generation::build_square(beam_mesh,
                                            ceil(0.4 / ds),
                                            ceil(0.02 / ds),
                                            0.2,
                                            0.6,
                                            0.19,
                                            0.21,
                                            Utility::string_to_enum<ElemType>(beam_elem_type));
        beam_mesh.prepare_for_use();

        vector<MeshBase*> meshes(2);
        meshes[0] = &block_mesh;
        meshes[1] = &beam_mesh;

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<INSHierarchyIntegrator> navier_stokes_integrator;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"));
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"));
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }
        Pointer<IBFEMethod> ib_method_ops =
            new IBFEMethod("IBFEMethod",
                           app_initializer->getComponentDatabase("IBFEMethod"),
                           meshes,
                           app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                           /*register_for_restart*/ true,
                           restart_read_dirname,
                           restart_restore_num);
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator);
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"));
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry);
        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer);

        // Configure the IBFE solver.
        ElasticityData elasticity_data(input_db);
        void* const elasticity_data_ptr = reinterpret_cast<void*>(&elasticity_data);
        IBFEMethod::LagBodyForceFcnData block_tether_force_data(
            block_tether_force_function, std::vector<IBTK::SystemData>(), elasticity_data_ptr);
        IBFEMethod::PK1StressFcnData block_PK1_stress_data(
            block_PK1_stress_function, std::vector<IBTK::SystemData>(), elasticity_data_ptr);
        ib_method_ops->registerLagBodyForceFunction(block_tether_force_data, 0);
        ib_method_ops->registerPK1StressFunction(block_PK1_stress_data, 0);
        string block_kernel_fcn = input_db->getStringWithDefault("BLOCK_KERNEL_FUNCTION", "PIECEWISE_LINEAR");
        FEDataManager::InterpSpec block_interp_spec = ib_method_ops->getDefaultInterpSpec();
        block_interp_spec.kernel_fcn = block_kernel_fcn;
        ib_method_ops->setInterpSpec(block_interp_spec, 0);
        FEDataManager::SpreadSpec block_spread_spec = ib_method_ops->getDefaultSpreadSpec();
        block_spread_spec.kernel_fcn = block_kernel_fcn;
        ib_method_ops->setSpreadSpec(block_spread_spec, 0);

        IBFEMethod::LagBodyForceFcnData beam_tether_force_data(
            beam_tether_force_function, std::vector<IBTK::SystemData>(), elasticity_data_ptr);
        IBFEMethod::PK1StressFcnData beam_PK1_stress_data(
            beam_PK1_stress_function, std::vector<IBTK::SystemData>(), elasticity_data_ptr);
        ib_method_ops->registerLagBodyForceFunction(beam_tether_force_data, 1);
        ib_method_ops->registerPK1StressFunction(beam_PK1_stress_data, 1);
        string beam_kernel_fcn = input_db->getStringWithDefault("BEAM_KERNEL_FUNCTION", "IB_3");
        FEDataManager::InterpSpec beam_interp_spec = ib_method_ops->getDefaultInterpSpec();
        beam_interp_spec.kernel_fcn = beam_kernel_fcn;
        ib_method_ops->setInterpSpec(beam_interp_spec, 1);
        FEDataManager::SpreadSpec beam_spread_spec = ib_method_ops->getDefaultSpreadSpec();
        beam_spread_spec.kernel_fcn = beam_kernel_fcn;
        ib_method_ops->setSpreadSpec(beam_spread_spec, 1);

        ib_method_ops->initializeFEEquationSystems();
        EquationSystems* block_equation_systems = ib_method_ops->getFEDataManager(0)->getEquationSystems();
        EquationSystems* beam_equation_systems = ib_method_ops->getFEDataManager(1)->getEquationSystems();

        // Create Eulerian initial condition specification objects.
        if (input_db->keyExists("VelocityInitialConditions"))
        {
            Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
                "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerVelocityInitialConditions(u_init);
        }

        if (input_db->keyExists("PressureInitialConditions"))
        {
            Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
                "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerPressureInitialConditions(p_init);
        }

        // Create Eulerian boundary condition specification objects (when necessary).
        const IntVector<NDIM>& periodic_shift = grid_geometry->getPeriodicShift();
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM);
        if (periodic_shift.min() > 0)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                u_bc_coefs[d] = NULL;
            }
        }
        else
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                const std::string bc_coefs_name = "u_bc_coefs_" + std::to_string(d);

                const std::string bc_coefs_db_name = "VelocityBcCoefs_" + std::to_string(d);

                u_bc_coefs[d] = new muParserRobinBcCoefs(
                    bc_coefs_name, app_initializer->getComponentDatabase(bc_coefs_db_name), grid_geometry);
            }
            navier_stokes_integrator->registerPhysicalBoundaryConditions(u_bc_coefs);
        }

        // Create Eulerian body force function specification objects.
        if (input_db->keyExists("ForcingFunction"))
        {
            Pointer<CartGridFunction> f_fcn = new muParserCartGridFunction(
                "f_fcn", app_initializer->getComponentDatabase("ForcingFunction"), grid_geometry);
            time_integrator->registerBodyForceFunction(f_fcn);
        }

        // Set up visualization plot file writers.
        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        if (uses_visit)
        {
            time_integrator->registerVisItDataWriter(visit_data_writer);
        }
        std::unique_ptr<ExodusII_IO> block_exodus_io(uses_exodus ? new ExodusII_IO(block_mesh) : NULL);
        std::unique_ptr<ExodusII_IO> beam_exodus_io(uses_exodus ? new ExodusII_IO(beam_mesh) : NULL);

        // Check to see if this is a restarted run to append current exodus files
        if (uses_exodus)
        {
            const bool from_restart = RestartManager::getManager()->isFromRestart();
            block_exodus_io->append(from_restart);
            beam_exodus_io->append(from_restart);
        }

        // Initialize hierarchy configuration and data on all patches.
        ib_method_ops->initializeFEData();
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // Deallocate initialization objects.
        app_initializer.setNull();

        // Print the input database contents to the log file.
        plog << "Input database:\n";
        input_db->printClassData(plog);

        // Write out initial visualization data.
        int iteration_num = time_integrator->getIntegratorStep();
        double loop_time = time_integrator->getIntegratorTime();
        if (dump_viz_data)
        {
            pout << "\n\nWriting visualization files...\n\n";
            if (uses_visit)
            {
                time_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
            }
            if (uses_exodus)
            {
                block_exodus_io->write_timestep(
                    block_exodus_filename, *block_equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
                beam_exodus_io->write_timestep(
                    beam_exodus_filename, *beam_equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
            }
        }

        // Open streams to save lift and drag coefficients.
        if (IBTK_MPI::getRank() == 0)
        {
            drag_stream.open("C_D.curve", ios_base::out | ios_base::trunc);
            lift_stream.open("C_L.curve", ios_base::out | ios_base::trunc);
            A_x_posn_stream.open("A_x.curve", ios_base::out | ios_base::trunc);
            A_y_posn_stream.open("A_y.curve", ios_base::out | ios_base::trunc);
        }

        // Main time step loop.
        double loop_time_end = time_integrator->getEndTime();
        double dt = 0.0;
        while (!MathUtilities<double>::equalEps(loop_time, loop_time_end) && time_integrator->stepsRemaining())
        {
            iteration_num = time_integrator->getIntegratorStep();
            loop_time = time_integrator->getIntegratorTime();

            pout << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "At beginning of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";

            dt = time_integrator->getMaximumTimeStepSize();
            time_integrator->advanceHierarchy(dt);
            loop_time += dt;

            pout << "\n";
            pout << "At end       of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "\n";

            // At specified intervals, write visualization and restart files,
            // print out timer data, and store hierarchy data for post
            // processing.
            iteration_num += 1;
            const bool last_step = !time_integrator->stepsRemaining();
            if (dump_viz_data && (iteration_num % viz_dump_interval == 0 || last_step))
            {
                pout << "\nWriting visualization files...\n\n";
                if (uses_visit)
                {
                    time_integrator->setupPlotData();
                    visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
                }
                if (uses_exodus)
                {
                    block_exodus_io->write_timestep(block_exodus_filename,
                                                    *block_equation_systems,
                                                    iteration_num / viz_dump_interval + 1,
                                                    loop_time);
                    beam_exodus_io->write_timestep(
                        beam_exodus_filename, *beam_equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
                }
            }
            if (dump_restart_data && (iteration_num % restart_dump_interval == 0 || last_step))
            {
                pout << "\nWriting restart files...\n\n";
                RestartManager::getManager()->writeRestartFile(restart_dump_dirname, iteration_num);
                ib_method_ops->writeFEDataToRestartFile(restart_dump_dirname, iteration_num);
            }
            if (dump_timer_data && (iteration_num % timer_dump_interval == 0 || last_step))
            {
                pout << "\nWriting timer data...\n\n";
                TimerManager::getManager()->print(plog);
            }
            if (dump_postproc_data && (iteration_num % postproc_data_dump_interval == 0 || last_step))
            {
                pout << "\nWriting state data...\n\n";
                postprocess_data(patch_hierarchy,
                                 navier_stokes_integrator,
                                 beam_mesh,
                                 beam_equation_systems,
                                 block_mesh,
                                 block_equation_systems,
                                 iteration_num,
                                 loop_time,
                                 postproc_data_dump_dirname);
            }
        }

        // Close the logging streams.
        if (IBTK_MPI::getRank() == 0)
        {
            drag_stream.close();
            lift_stream.close();
            A_x_posn_stream.close();
            A_y_posn_stream.close();
        }

        // Cleanup Eulerian boundary condition specification objects (when
        // necessary).
        for (unsigned int d = 0; d < NDIM; ++d) delete u_bc_coefs[d];

    } // cleanup dynamically allocated objects prior to shutdown
} // main

void
postprocess_data(Pointer<PatchHierarchy<NDIM> > /*patch_hierarchy*/,
                 Pointer<INSHierarchyIntegrator> /*navier_stokes_integrator*/,
                 Mesh& beam_mesh,
                 EquationSystems* beam_equation_systems,
                 Mesh& block_mesh,
                 EquationSystems* block_equation_systems,
                 const int /*iteration_num*/,
                 const double loop_time,
                 const string& /*data_dump_dirname*/)
{
    double F_integral[NDIM];
    for (unsigned int d = 0; d < NDIM; ++d) F_integral[d] = 0.0;
    Mesh* mesh[2] = { &beam_mesh, &block_mesh };
    EquationSystems* equation_systems[2] = { beam_equation_systems, block_equation_systems };
    for (unsigned int k = 0; k < 2; ++k)
    {
        System& F_system = equation_systems[k]->get_system<System>(IBFEMethod::FORCE_SYSTEM_NAME);
        NumericVector<double>* F_vec = F_system.solution.get();
        NumericVector<double>* F_ghost_vec = F_system.current_local_solution.get();
        copy_and_synch(*F_vec, *F_ghost_vec);
        DofMap& F_dof_map = F_system.get_dof_map();
        std::vector<std::vector<unsigned int> > F_dof_indices(NDIM);
        std::unique_ptr<FEBase> fe(FEBase::build(NDIM, F_dof_map.variable_type(0)));
        std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, NDIM, FIFTH);
        fe->attach_quadrature_rule(qrule.get());
        const std::vector<std::vector<double> >& phi = fe->get_phi();
        const std::vector<double>& JxW = fe->get_JxW();
        boost::multi_array<double, 2> F_node;
        const MeshBase::const_element_iterator el_begin = mesh[k]->active_local_elements_begin();
        const MeshBase::const_element_iterator el_end = mesh[k]->active_local_elements_end();
        for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
        {
            Elem* const elem = *el_it;
            fe->reinit(elem);
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                F_dof_map.dof_indices(elem, F_dof_indices[d], d);
            }
            const int n_qp = qrule->n_points();
            const int n_basis = static_cast<int>(F_dof_indices[0].size());
            get_values_for_interpolation(F_node, *F_ghost_vec, F_dof_indices);
            for (int qp = 0; qp < n_qp; ++qp)
            {
                for (int k = 0; k < n_basis; ++k)
                {
                    for (int d = 0; d < NDIM; ++d)
                    {
                        F_integral[d] += F_node[k][d] * phi[k][qp] * JxW[qp];
                    }
                }
            }
        }
    }
    IBTK_MPI::sumReduction(F_integral, NDIM);
    if (IBTK_MPI::getRank() == 0)
    {
        drag_stream.precision(12);
        drag_stream.setf(ios::fixed, ios::floatfield);
        drag_stream << loop_time << " " << -F_integral[0] << endl;
        lift_stream.precision(12);
        lift_stream.setf(ios::fixed, ios::floatfield);
        lift_stream << loop_time << " " << -F_integral[1] << endl;
    }

    System& X_system = beam_equation_systems->get_system<System>(IBFEMethod::COORDS_SYSTEM_NAME);
    NumericVector<double>* X_vec = X_system.solution.get();
    std::unique_ptr<NumericVector<Number> > X_serial_vec = NumericVector<Number>::build(X_vec->comm());
    X_serial_vec->init(X_vec->size(), true, SERIAL);
    X_vec->localize(*X_serial_vec);
    DofMap& X_dof_map = X_system.get_dof_map();
    vector<unsigned int> vars(2);
    vars[0] = 0;
    vars[1] = 1;
    MeshFunction X_fcn(*beam_equation_systems, *X_serial_vec, X_dof_map, vars);
    X_fcn.init();
    DenseVector<double> X_A(2);
    X_fcn(libMesh::Point(0.6, 0.2, 0), 0.0, X_A);
    if (IBTK_MPI::getRank() == 0)
    {
        A_x_posn_stream.precision(12);
        A_x_posn_stream.setf(ios::fixed, ios::floatfield);
        A_x_posn_stream << loop_time << " " << X_A(0) << endl;
        A_y_posn_stream.precision(12);
        A_y_posn_stream.setf(ios::fixed, ios::floatfield);
        A_y_posn_stream << loop_time << " " << X_A(1) << endl;
    }
    return;
} // postprocess_data