// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#include <iostream>
#include <fstream>
#include <string>
#include <iomanip>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <vw/Cartography/GeoReference.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/Statistics.h>
#include <vw/FileIO/DiskImageResource.h>
#include <vw/FileIO/DiskImageView.h>

using namespace vw;

/// \file imagestats.cc Computes a number of statistics about an image

// TODO: Move this class to a library file!
/// Class to compute a running standard deviation
/// - Code adapted from http://www.johndcook.com/standard_deviation.html
class RunningStatistics
{
public:
  RunningStatistics() : m_n(0), m_oldM(0), m_newM(0), m_oldS(0), m_newS(0)
  {
    m_min = std::numeric_limits<double>::max();
    m_max = std::numeric_limits<double>::min();
  }

  void Clear()
  {
    m_n = 0;
  }

  void Push(double x)
  {
    m_n++;

    if (x < m_min)
        m_min = x;
    if (x > m_max)
        m_max = x;

    // See Knuth TAOCP vol 2, 3rd edition, page 232
    if (m_n == 1)
    {
      m_oldM = m_newM = x;
      m_oldS = 0.0;
    }
    else
    {
      m_newM = m_oldM + (x - m_oldM)/m_n;
      m_newS = m_oldS + (x - m_oldM)*(x - m_newM);

      // set up for next iteration
      m_oldM = m_newM;
      m_oldS = m_newS;
    }
  }

  int NumDataValues() const
  {
    return m_n;
  }

  double Min() const
  {
    return m_min;
  }

  double Max() const
  {
    return m_max;
  }

  double Mean() const
  {
    return (m_n > 0) ? m_newM : 0.0;
  }

  double Variance() const
  {
    return ( (m_n > 1) ? m_newS/(m_n - 1) : 0.0 );
  }

  double StandardDeviation() const
  {
    return sqrt( Variance() );
  }

private:
  int    m_n;
  double m_oldM, m_newM, m_oldS, m_newS;
  double m_max, m_min;

};

/// Function to write the output statistics to an output stream
bool writeOutput(const std::vector<float > &cdfVector,
                 const std::vector<double> &levels,
                 const std::vector<size_t> &hist,
                 const RunningStatistics   &statCalc,
                       std::ostream        &stream)
{

  stream << "Mean elevation difference = " << statCalc.Mean()              << std::endl;
  stream << "Standard deviation        = " << statCalc.StandardDeviation() << std::endl;
  stream << "Num valid pixels          = " << statCalc.NumDataValues()     << std::endl << std::endl;

  // Print out the percentile distribution
  stream << "Image distribution (approximated 5 percent intervals): " << std::endl;
  stream.setf(std::ios::fixed, std::ios::floatfield);
  stream.precision(2);
  stream.fill(' ');
  for (size_t i=0; i<cdfVector.size(); ++i)
  {
    double percent = i * 0.05;
    stream << "Percentile " << std::right << std::setw(4) << percent << " = " << std::setw(7) << cdfVector[i] << std::endl;
  }

  stream << std::endl; // Put a space between the "charts"

  // Print out the histogram
  stream << "Image histogram (20 bins):" << std::endl;
  stream.fill(' ');
  double ratio = 100.0 / statCalc.NumDataValues();
  for (size_t i=0; i<hist.size(); ++i)
  {
    stream << std::right << std::setw(8) << levels[i] << " <-->" << std::setw(7) << levels[i+1] << " ="
           << std::setw(8) << hist[i] << " =" << std::setw(6) << hist[i]*ratio << "%" << std::endl;
  }

  return true;
}

//TODO: Print help when no input arguments are used!
int main( int argc, char *argv[] ) {

  const int numBins = 20; // 5% intervals

  std::string inputImagePath, outputPath="";
  int removeHistogramOutliers=0;
  bool absolute;

  po::options_description general_options("Options");
  general_options.add_options()
    ("help,h",        "Display this help message")
    ("output-file,o", po::value<std::string>(&outputPath)->default_value(""), "Specify an output text file to store the program output")
    ("limit-hist",    po::value<int        >(&removeHistogramOutliers)->default_value(0), "Limits the histogram to +/- N standard deviations from the mean");
    ("absolute",      po::value<bool       >(&absolute)->default_value(false), "Work in absolute values");

  po::options_description positional("");
  positional.add_options()
    ("input-image",   po::value(&inputImagePath), "Path to input image file");

  po::positional_options_description positional_desc;
  positional_desc.add("input-image",  1);

  std::string usage("[options] <input-image>\n");
  po::variables_map vm;
  try {
    po::options_description all_options;
    all_options.add(general_options).add(positional);

    po::store( po::command_line_parser( argc, argv ).options(all_options).positional(positional_desc).style( po::command_line_style::unix_style ).run(), vm );

    po::notify( vm );
  } catch (po::error const& e) {
    vw::vw_throw( vw::ArgumentErr() << "Error parsing input:\n"
                  << e.what() << "\n" << usage << general_options );
  }

  if ( !vm.count("input-image") )
    vw_throw( vw::ArgumentErr() << "Requires <input-image> input in order to proceed.\n\n"
              << usage << general_options );

  try {

    //TODO: Operate on multi-channel images of different data types!
    // Load the image from disk
    DiskImageView<PixelGray<float> > inputImage(inputImagePath);

    // First pass computes min, max, mean, and std_dev
    RunningStatistics statCalc;
    for (int row=0; row<inputImage.rows(); ++row)
    {

      for (int col=0; col<inputImage.cols(); ++col)
      {

        if (is_valid(inputImage(col,row))) // Skip invalid pixels
        {
          float diff = inputImage(col,row)[0];
          if (diff > -32767) // Avoid flag value
          {
            if (absolute)
              diff = fabs(diff);
            statCalc.Push(diff);
          }
        }
      } // End loop through cols

    } // End loop through rows

    double meanPixelValue   = statCalc.Mean();
    double stdDevPixelValue = statCalc.StandardDeviation();
    double minVal           = statCalc.Min();
    double maxVal           = statCalc.Max();

    if (removeHistogramOutliers > 0) // Cut off range at +/- N std
    {
      printf("Image min = %lf\n", minVal);
      printf("Image max = %lf\n", maxVal);

      minVal = meanPixelValue - removeHistogramOutliers*stdDevPixelValue;
      if (minVal < statCalc.Min())
        minVal = statCalc.Min();
      maxVal = meanPixelValue + removeHistogramOutliers*stdDevPixelValue;
      if (maxVal > statCalc.Max())
        maxVal = statCalc.Max();

      printf("Restricting histogram to +/- %d standard deviations: %lf <--> %lf\n", removeHistogramOutliers, minVal, maxVal);
    }

    double range   = maxVal - minVal;
    double binSize = range / numBins;
    double factor  = 1.0 / binSize;

    // Set up levels structure
    int numLevels = numBins + 1;
    std::vector<double> levels(numLevels);
    for (int i=0; i<numLevels; ++i)
      levels[i] = minVal + i*binSize;

    // CDF
    vw::math::CDFAccumulator<float> cdfCalc(1000, 251); //TODO: What values to pass in?

    // Next pass fill in histogram
    std::vector<vw::uint64> hist;
    hist.assign(numBins, 0);
    for (int row = 0; row < inputImage.rows(); row++)
    {
      for (int col = 0; col < inputImage.cols(); col++)
      {
        if (!is_valid(inputImage(col,row))) // Skip invalid pixels
          continue;
        float diff = inputImage(col,row)[0];
        if (diff <= -32767) // Avoid flag value
          continue;
        if (absolute)
          diff = fabs(diff);

        int bin = (int)floor( factor * (diff - minVal)  );
        if ((bin >= 0) && (bin < numBins)) // If removeHistogramOutliers is set some values will not fit in a bin
        {
          ++(hist[bin]);
          //std::cout << "bin " << bin << " = " << hist[bin] << std::endl;
        }
        else // Invalid bin
        {
          //printf("range = %lf, binSize = %lf, factor = %lf, minVal = %lf, diff = %lf\n", range, binSize, factor, minVal, diff);
          //std::cout << "bin " << bin << std::endl;
        }

        cdfCalc(diff);

      } // End column loop
    } // End row loop

    // Fill out the CDF
    std::vector<float> cdfVector(numLevels);
    for (int i=0; i<numLevels; ++i) // --> 0.0 to 1.0
    {
      double percent = 0.05 * i;
      cdfVector[i]   = cdfCalc.quantile(percent);
    }

    // Write output to console
    writeOutput(cdfVector, levels, hist, statCalc, std::cout);

    if (!outputPath.empty())  // Write output to file
    {
      std::ofstream file(outputPath.c_str());
      writeOutput(cdfVector, levels, hist, statCalc, file);
      file.close();
    }
  }
  catch (const Exception& e) {
    std::cerr << "Error: " << e.what() << std::endl;
  }

  return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the rank_features() function
    from the dlib C++ Library.

    This example creates a simple set of data and then shows
    you how to use the rank_features() function to find a good
    set of features (where "good" means the feature set will probably
    work well with a classification algorithm).

    The data used in this example will be 4 dimensional data and will
    come from a distribution where points with a distance less than 10
    from the origin are labeled +1 and all other points are labeled
    as -1.  Note that this data is conceptually 2 dimensional but we
    will add two extra features for the purpose of showing what
    the rank_features() function does.
*/

#include <iostream>
#include <dlib/svm.h>
#include <dlib/rand.h>
#include <vector>

using namespace std;
using namespace dlib;

int main()
{

    // This first typedef declares a matrix with 4 rows and 1 column.  It will be the
    // object that contains each of our 4 dimensional samples.
    typedef matrix<double, 4, 1> sample_type;

    // Now lets make some vector objects that can hold our samples
    std::vector<sample_type> samples;
    std::vector<double> labels;

    dlib::rand rnd;

    for (int x = -30; x <= 30; ++x)
    {
        for (int y = -30; y <= 30; ++y)
        {
            sample_type samp;

            // the first two features are just the (x,y) position of our points and so
            // we expect them to be good features since our two classes here are points
            // close to the origin and points far away from the origin.
            samp(0) = x;
            samp(1) = y;

            // This is a worthless feature since it is just random noise.  It should
            // be indicated as worthless by the rank_features() function below.
            samp(2) = rnd.get_random_double();

            // This is a version of the y feature that is corrupted by random noise.  It
            // should be ranked as less useful than features 0, and 1, but more useful
            // than the above feature.
            samp(3) = y*0.2 + (rnd.get_random_double()-0.5)*10;

            // add this sample into our vector of samples.
            samples.push_back(samp);

            // if this point is less than 15 from the origin then label it as a +1 class point.
            // otherwise it is a -1 class point
            if (sqrt((double)x*x + y*y) <= 15)
                labels.push_back(+1);
            else
                labels.push_back(-1);
        }
    }

    // Here we normalize all the samples by subtracting their mean and dividing by their standard deviation.
    // This is generally a good idea since it often heads off numerical stability problems and also
    // prevents one large feature from smothering others.
    const sample_type m(mean(mat(samples)));  // compute a mean vector
    const sample_type sd(reciprocal(stddev(mat(samples)))); // compute a standard deviation vector
    // now normalize each sample
    for (unsigned long i = 0; i < samples.size(); ++i)
        samples[i] = pointwise_multiply(samples[i] - m, sd);

    // This is another thing that is often good to do from a numerical stability point of view.
    // However, in our case it doesn't really matter.   It's just here to show you how to do it.
    randomize_samples(samples,labels);

    // This is a typedef for the type of kernel we are going to use in this example.
    // In this case I have selected the radial basis kernel that can operate on our
    // 4D sample_type objects.  In general, I would suggest using the same kernel for
    // classification and feature ranking.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // The radial_basis_kernel has a parameter called gamma that we need to set.  Generally,
    // you should try the same gamma that you are using for training.  But if you don't
    // have a particular gamma in mind then you can use the following function to
    // find a reasonable default gamma for your data.  Another reasonable way to pick a gamma
    // is often to use 1.0/compute_mean_squared_distance(randomly_subsample(samples, 2000)).
    // It computes the mean squared distance between 2000 randomly selected samples and often
    // works quite well.
    const double gamma = verbose_find_gamma_with_big_centroid_gap(samples, labels);

    // Next we declare an instance of the kcentroid object.  It is used by rank_features()
    // two represent the centroids of the two classes.  The kcentroid has 3 parameters
    // you need to set.  The first argument to the constructor is the kernel we wish to
    // use.  The second is a parameter that determines the numerical accuracy with which
    // the object will perform part of the ranking algorithm.  Generally, smaller values
    // give better results but cause the algorithm to attempt to use more dictionary vectors
    // (and thus run slower and use more memory).  The third argument, however, is the
    // maximum number of dictionary vectors a kcentroid is allowed to use.  So you can use
    // it to put an upper limit on the runtime complexity.
    kcentroid<kernel_type> kc(kernel_type(gamma), 0.001, 25);

    // And finally we get to the feature ranking. Here we call rank_features() with the kcentroid we just made,
    // the samples and labels we made above, and the number of features we want it to rank.
    cout << rank_features(kc, samples, labels) << endl;

    // The output is:
    /*
        0 0.749265
        1        1
        3 0.933378
        2 0.825179
    */

    // The first column is a list of the features in order of decreasing goodness.  So the rank_features() function
    // is telling us that the samples[i](0) and samples[i](1) (i.e. the x and y) features are the best two.  Then
    // after that the next best feature is the samples[i](3) (i.e. the y corrupted by noise) and finally the worst
    // feature is the one that is just random noise.  So in this case rank_features did exactly what we would
    // intuitively expect.

    // The second column of the matrix is a number that indicates how much the features up to that point
    // contribute to the separation of the two classes.  So bigger numbers are better since they
    // indicate a larger separation.  The max value is always 1.  In the case below we see that the bad
    // features actually make the class separation go down.

    // So to break it down a little more.
    //    0 0.749265   <-- class separation of feature 0 all by itself
    //    1        1   <-- class separation of feature 0 and 1
    //    3 0.933378   <-- class separation of feature 0, 1, and 3
    //    2 0.825179   <-- class separation of feature 0, 1, 3, and 2

}


#include "problemes.h"
#include "arithmetique.h"

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

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    bool intersection(const std::set<nombre> &a, const std::set<nombre> &b) {
        std::set<nombre> i;
        std::set_intersection(a.begin(), a.end(), b.begin(), b.end(), std::inserter(i, i.begin()));
        return i.empty();
    }
}

ENREGISTRER_PROBLEME(215, "Crack-free Walls") {
    // Consider the problem of building a wall out of 2×1 and 3×1 bricks (horizontal×vertical dimensions) such that, for
    // extra strength, the gaps between horizontally-adjacent bricks never line up in consecutive layers, i.e. never
    // form a "running crack".
    //
    // For example, the following 9×3 wall is not acceptable due to the running crack shown in red:
    //
    // There are eight ways of forming a crack-free 9×3 wall, written W(9,3) = 8.
    //
    // Calculate W(32,10).
    nombre taille = 32;
    nombre hauteur = 10;
    std::vector<std::set<nombre>> murs;
    for (nombre n = 0; 3 * n <= taille; ++n) {
        if ((taille - 3 * n) % 2 == 0) {
            vecteur mur((taille - 3 * n) / 2, 2);
            mur.insert(mur.end(), n, 3);
            do {
                std::set<nombre> set_mur;
                nombre l = 0;
                for (auto m: mur) {
                    set_mur.insert(l);
                    l += m;
                }
                set_mur.erase(set_mur.begin());
                murs.push_back(set_mur);
            } while (std::next_permutation(mur.begin(), mur.end()));
        }
    }

    std::map<std::set<nombre>, std::vector<std::set<nombre>>> I;
    for (const auto &m1: murs)
        for (const auto &m2: murs) {
            if (intersection(m1, m2))
                I[m1].push_back(m2);
        }

    std::map<std::set<nombre>, nombre> dp;
    for (const auto&[k, v]: I)
        dp[k] = v.size();

    for (nombre h = 2; h < hauteur; ++h) {
        std::map<std::set<nombre>, nombre> suite_dp;
        for (const auto&[k, v]: dp) {
            for (const auto &i: I[k])
                suite_dp[i] += v;
        }

        std::swap(suite_dp, dp);
    }

    nombre resultat = 0;
    for (const auto&[k, v]: dp)
        resultat += v;

    return std::to_string(resultat);
}


// =============================================================================
//  BoostIncludes.hpp
//
//  MIT License
//
//  Copyright (c) 2007-2018 Dairoku Sekiguchi
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//  SOFTWARE.
// =============================================================================
/*!
	\file		BoostIncludes.hpp
	\author		Dairoku Sekiguchi
	\version	1.0
	\date		2008/02/22
	\brief
*/
#ifndef __BOOST_INCLUDES_H
#define __BOOST_INCLUDES_H

#pragma warning(disable:4996)		// to suppress std::uninitialized_copy warning

// -----------------------------------------------------------------------------
// 	include files
// -----------------------------------------------------------------------------
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>

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

#endif	// #ifdef __BOOST_INCLUDES_H


#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/container/sequence_vector.hpp>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( creation_incr )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Creation - Increasing Sequence");

	typedef short value_type;

	const std::size_t n(3);

	ublasx::sequence_vector<value_type> res;
	ublas::vector<value_type> expect_res(n);

	res = ublasx::sequence_vector<value_type>(0, 2, n);

	value_type x(0);
	for (std::size_t i = 0; i < n; ++i)
	{
		expect_res(i) = x;
		x += 2;
	}

	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
	BOOST_UBLASX_DEBUG_TRACE( "expect res = " << expect_res );
	BOOST_UBLASX_TEST_CHECK_VECTOR_EQ( res, expect_res, n );
}

BOOST_UBLASX_TEST_DEF( creation_decr )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Creation - Decreasing Sequence");

	typedef short value_type;

	const std::size_t n(3);

	ublasx::sequence_vector<value_type> res;
	ublas::vector<value_type> expect_res(n);

	res = ublasx::sequence_vector<value_type>(0, -2, n);

	value_type x(0);
	for (std::size_t i = 0; i < n; ++i)
	{
		expect_res(i) = x;
		x -= 2;
	}

	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
	BOOST_UBLASX_DEBUG_TRACE( "expect res = " << expect_res );
	BOOST_UBLASX_TEST_CHECK_VECTOR_EQ( res, expect_res, n );
}

BOOST_UBLASX_TEST_DEF( creation_from_range )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Creation - Range");

	typedef short value_type;

	const std::size_t n(3);

	ublasx::sequence_vector<value_type> res;
	ublas::vector<value_type> expect_res(n);

	res = ublasx::sequence_vector<value_type>(ublas::range(4, 4+n));

	value_type x(4);
	for (std::size_t i = 0; i < n; ++i)
	{
		expect_res(i) = x;
		++x;
	}

	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
	BOOST_UBLASX_DEBUG_TRACE( "expect res = " << expect_res );
	BOOST_UBLASX_TEST_CHECK_VECTOR_EQ( res, expect_res, n );
}

BOOST_UBLASX_TEST_DEF( creation_from_slice )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Creation - Slice");

	typedef short value_type;

	const std::size_t n(3);

	ublasx::sequence_vector<value_type> res;
	ublas::vector<value_type> expect_res(n);

	res = ublasx::sequence_vector<value_type>(ublas::slice(5, 3, n));

	value_type x(5);
	for (std::size_t i = 0; i < n; ++i)
	{
		expect_res(i) = x;
		x += 3;
	}

	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
	BOOST_UBLASX_DEBUG_TRACE( "expect res = " << expect_res );
	BOOST_UBLASX_TEST_CHECK_VECTOR_EQ( res, expect_res, n );
}

int main()
{
	BOOST_UBLASX_DEBUG_TRACE("Test Suite: Sequence Vector class");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( creation_incr );
	BOOST_UBLASX_TEST_DO( creation_decr );
	BOOST_UBLASX_TEST_DO( creation_from_range );
	BOOST_UBLASX_TEST_DO( creation_from_slice );

	BOOST_UBLASX_TEST_END();
}


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_MATRIX_SVD_H__
#define __SOT_MATRIX_SVD_H__

/* --- Matrix --- */
#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {

typedef Eigen::JacobiSVD<Matrix> SVD_t;

void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
                   const double threshold = 1e-6);

void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
                   const double threshold = 1e-6);

void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
                   Matrix &Uref, Vector &Sref, Matrix &Vref,
                   const double threshold = 1e-6);

void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
                   const double threshold = 1e-6);

} // namespace dynamicgraph

#endif /* #ifndef __SOT_MATRIX_SVD_H__ */


#include <Eigen/Eigen>
#include <iostream>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Vector4d v = Vector4d::Random();
Projective3d P(Matrix4d::Random());
cout << "v                   = " << v.transpose() << "]^T" << endl;
cout << "v.hnormalized()     = " << v.hnormalized().transpose() << "]^T" << endl;
cout << "P*v                 = " << (P*v).transpose() << "]^T" << endl;
cout << "(P*v).hnormalized() = " << (P*v).hnormalized().transpose() << "]^T" << endl;
  return 0;
}


/**
 * @file cne_test.cpp
 * @author Marcus Edel
 * @author Kartik Nighania
 *
 * Test file for CNE (Conventional Neural Evolution).
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <mlpack/core/optimizers/cne/cne.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::optimization;

using namespace mlpack::distribution;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(CNETest);

/**
 * Training a vanilla network for 2 input XOR function
 */
BOOST_AUTO_TEST_CASE(CNEXORTest)
{
  /*
   * Create the four cases for XOR with two variable
   *
   *  Input    Output
   * 0 XOR 0  =  0
   * 1 XOR 1  =  0
   * 0 XOR 1  =  1
   * 1 XOR 0  =  1
   */
  arma::mat train("1, 0, 0, 1; 1, 0, 1, 0");
  arma::mat labels("1, 1, 2, 2");

  // CNE may fail to find a good optimum.  But if it can succeed one out of 6
  // times I think that is sufficient to say it is working.
  size_t successes = 0;
  for (size_t trial = 0; trial < 6; ++trial)
  {
    // Build a network with 2 input, 2 hidden, and 2 output layers.
    FFN<NegativeLogLikelihood<> > network;

    network.Add<Linear<> >(2, 2);
    network.Add<SigmoidLayer<> >();
    network.Add<Linear<> >(2, 2);
    network.Add<LogSoftMax<> >();

    // CNE object.
    CNE opt(60, 5000, 0.1, 0.02, 0.2, 0.1, -1);

    // Training the network with CNE
    network.Train(train, labels, opt);

    // Predicting for the same train data
    arma::mat predictionTemp;
    network.Predict(train, predictionTemp);

    arma::mat prediction = arma::zeros<arma::mat>(1, predictionTemp.n_cols);

    for (size_t i = 0; i < predictionTemp.n_cols; ++i)
    {
      prediction(i) = arma::as_scalar(arma::find(
          arma::max(predictionTemp.col(i)) == predictionTemp.col(i), 1)) + 1;
    }

    // 1 means 0 and 2 means 1 as the output to XOR.
    if ((prediction[0] == 1) &&
        (prediction[1] == 1) &&
        (prediction[2] == 2) &&
        (prediction[3] == 2))
    {
      ++successes;
      break;
    }
  }

  BOOST_REQUIRE_GT(successes, 0);
}

/**
 * Train and test a logistic regression function using CNE optimizer
 */
BOOST_AUTO_TEST_CASE(CNELogisticRegressionTest)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 1000);
  arma::Row<size_t> responses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0,
      data.n_cols - 1, data.n_cols));
  arma::mat shuffledData(3, 1000);
  arma::Row<size_t> shuffledResponses(1000);
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    shuffledData.col(i) = data.col(indices[i]);
    shuffledResponses[i] = responses[indices[i]];
  }

  // Create a test set.
  arma::mat testData(3, 1000);
  arma::Row<size_t> testResponses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    testData.col(i) = g1.Random();
    testResponses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    testData.col(i) = g2.Random();
    testResponses[i] = 1;
  }

  CNE opt(200, 10000, 0.2, 0.2, 0.3, 65, -1);

  LogisticRegression<> lr(shuffledData, shuffledResponses, opt, 0.5);

  // Ensure that the error is close to zero.
  const double acc = lr.ComputeAccuracy(data, responses);
  BOOST_REQUIRE_CLOSE(acc, 100.0, 0.3); // 0.3% error tolerance.

  const double testAcc = lr.ComputeAccuracy(testData, testResponses);
  BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.6); // 0.6% error tolerance.
}

/**
 * Training a vanilla network on a larger dataset using CNE optimizer.
 */
BOOST_AUTO_TEST_CASE(VanillaNetworkWithCNETest)
{
  // Load the datasets.
  arma::mat trainData;
  data::Load("iris_train.csv", trainData, true);

  arma::mat testData;
  data::Load("iris_test.csv", testData, true);

  arma::mat trainLabels;
  data::Load("iris_train_labels.csv", trainLabels, true);
  trainLabels += 1;

  arma::mat testLabels;
  data::Load("iris_test_labels.csv", testLabels, true);
  testLabels += 1;

  // Training the network may fail, so we will try a few times.
  size_t successes = 0;
  for (size_t trial = 0; trial < 4; ++trial)
  {
    // Create vanilla network with 4 input, 4 hidden and 3 output nodes.
    FFN<NegativeLogLikelihood<> > model;
    model.Add<Linear<> >(trainData.n_rows, 4);
    model.Add<SigmoidLayer<> >();
    model.Add<Linear<> >(4, 3);
    model.Add<LogSoftMax<> >();

    // Creating CNE object.
    // The tolerance and objectiveChange are not taken into consideration.
    CNE opt(30, 200, 0.2, 0.2, 0.3, -1, -1);

    model.Train(trainData, trainLabels, opt);

    arma::mat predictionTemp;
    model.Predict(testData, predictionTemp);
    arma::mat prediction = arma::zeros<arma::mat>(1, predictionTemp.n_cols);

    for (size_t i = 0; i < predictionTemp.n_cols; ++i)
    {
      prediction(i) = arma::as_scalar(arma::find(
          arma::max(predictionTemp.col(i)) == predictionTemp.col(i), 1)) + 1;
    }

    size_t error = 0;
    for (size_t i = 0; i < testData.n_cols; i++)
    {
      if (int(arma::as_scalar(prediction.col(i))) ==
          int(arma::as_scalar(testLabels.col(i))))
      {
        error++;
      }
    }

    double classificationError = 1 - double(error) / testData.n_cols;
    if (classificationError <= 0.1)
    {
      ++successes;
      break;
    }
  }

  BOOST_REQUIRE_GT(successes, 0);
}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FNMS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FNMS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing fnms capabilities

    Computes the fused substract-multiply of three value.

    @par semantic:
    For any given value @c x,  @c y,  @c z of type @c T:

    @code
    T r = fnms(x, y, z);
    @endcode

    The code is similar to:

    @code
    T r = -(x*y-s);
    @endcode

    @par Note:

    fnms can be called with the same modalities as @ref fma
    and can use the decorator conformant_ to ensure the correct
    one rounding, no intermediate overflow  behaviour

    @see  fms, fma, fnma

  **/
  Value fnms(Value const& v0, Value const& v1, Value const& v2);
} }
#endif

#include <boost/simd/function/scalar/fnms.hpp>
#include <boost/simd/function/simd/fnms.hpp>

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the dlib C++ library's
    implementation of the pegasos algorithm for online training of support
    vector machines.

    This example creates a simple binary classification problem and shows
    you how to train a support vector machine on that data.

    The data used in this example will be 2 dimensional data and will
    come from a distribution where points with a distance less than 10
    from the origin are labeled +1 and all other points are labeled
    as -1.

*/

#include <iostream>
#include <ctime>
#include <vector>
#include <dlib/svm.h>

using namespace std;
using namespace dlib;

int main()
{
    // The svm functions use column vectors to contain a lot of the data on which they
    // operate. So the first thing we do here is declare a convenient typedef.

    // This typedef declares a matrix with 2 rows and 1 column.  It will be the
    // object that contains each of our 2 dimensional samples.   (Note that if you wanted
    // more than 2 features in this vector you can simply change the 2 to something else.
    // Or if you don't know how many features you want until runtime then you can put a 0
    // here and use the matrix.set_size() member function)
    typedef matrix<double, 2, 1> sample_type;

    // This is a typedef for the type of kernel we are going to use in this example.
    // In this case I have selected the radial basis kernel that can operate on our
    // 2D sample_type objects
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we create an instance of the pegasos svm trainer object we will be using.
    svm_pegasos<kernel_type> trainer;
    // Here we setup the parameters to this object.  See the dlib documentation for a
    // description of what these parameters are.
    trainer.set_lambda(0.00001);
    trainer.set_kernel(kernel_type(0.005));

    // Set the maximum number of support vectors we want the trainer object to use
    // in representing the decision function it is going to learn.  In general,
    // supplying a bigger number here will only ever give you a more accurate
    // answer.  However, giving a smaller number will make the algorithm run
    // faster and decision rules that involve fewer support vectors also take
    // less time to evaluate.
    trainer.set_max_num_sv(10);

    std::vector<sample_type> samples;
    std::vector<double> labels;

    // make an instance of a sample matrix so we can use it below
    sample_type sample, center;

    center = 20, 20;

    // Now let's go into a loop and randomly generate 1000 samples.
    srand(time(0));
    for (int i = 0; i < 10000; ++i)
    {
        // Make a random sample vector.
        sample = randm(2,1)*40 - center;

        // Now if that random vector is less than 10 units from the origin then it is in
        // the +1 class.
        if (length(sample) <= 10)
        {
            // let the svm_pegasos learn about this sample
            trainer.train(sample,+1);

            // save this sample so we can use it with the batch training examples below
            samples.push_back(sample);
            labels.push_back(+1);
        }
        else
        {
            // let the svm_pegasos learn about this sample
            trainer.train(sample,-1);

            // save this sample so we can use it with the batch training examples below
            samples.push_back(sample);
            labels.push_back(-1);
        }
    }

    // Now we have trained our SVM.  Let's see how well it did.
    // Each of these statements prints out the output of the SVM given a particular sample.
    // The SVM outputs a number > 0 if a sample is predicted to be in the +1 class and < 0
    // if a sample is predicted to be in the -1 class.

    sample(0) = 3.123;
    sample(1) = 4;
    cout << "This is a +1 example, its SVM output is: " << trainer(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This is a -1 example, its SVM output is: " << trainer(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This is a -1 example, its SVM output is: " << trainer(sample) << endl;

    // The previous part of this example program showed you how to perform online training
    // with the pegasos algorithm.  But it is often the case that you have a dataset and you
    // just want to perform batch learning on that dataset and get the resulting decision
    // function.  To support this the dlib library provides functions for converting an online
    // training object like svm_pegasos into a batch training object.

    // First let's clear out anything in the trainer object.
    trainer.clear();

    // Now to begin with, you might want to compute the cross validation score of a trainer object
    // on your data.  To do this you should use the batch_cached() function to convert the svm_pegasos object
    // into a batch training object.  Note that the second argument to batch_cached() is the minimum
    // learning rate the trainer object must report for the batch_cached() function to consider training
    // complete.  So smaller values of this parameter cause training to take longer but may result
    // in a more accurate solution.
    // Here we perform 4-fold cross validation and print the results
    cout << "cross validation: " << cross_validate_trainer(batch_cached(trainer,0.1), samples, labels, 4);

    // Here is an example of creating a decision function.  Note that we have used the verbose_batch_cached()
    // function instead of batch_cached() as above.  They do the same things except verbose_batch_cached() will
    // print status messages to standard output while training is under way.
    decision_function<kernel_type> df = verbose_batch_cached(trainer,0.1).train(samples, labels);

    // At this point we have obtained a decision function from the above batch mode training.
    // Now we can use it on some test samples exactly as we did above.

    sample(0) = 3.123;
    sample(1) = 4;
    cout << "This is a +1 example, its SVM output is: " << df(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This is a -1 example, its SVM output is: " << df(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This is a -1 example, its SVM output is: " << df(sample) << endl;

}


//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/float128.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/tools/ulps_plot.hpp>
#include <boost/math/special_functions/rsqrt.hpp>

int main()
{
    using boost::multiprecision::number;
    using PreciseReal = number<boost::multiprecision::mpfr_float_backend<1000>>;
    using CoarseReal = boost::multiprecision::float128;
    using boost::math::tools::ulps_plot;
    std::string filename = "rsqrt_quad_0_100.svg";
    int samples = 2500;
    int width = 1100;
    auto f = [](PreciseReal x) {
        using boost::math::rsqrt;
        return rsqrt(x);
    };
    auto plot03 = ulps_plot<decltype(f), PreciseReal, CoarseReal>(f, std::numeric_limits<CoarseReal>::min(), CoarseReal(100), samples);
    plot03.width(width);
    std::string title = "rsqrt ULPs plot at quad precision";
    plot03.title(title);
    plot03.vertical_lines(6);
    auto g = [](CoarseReal x) {
        return boost::math::rsqrt(x);
    };
    plot03.add_fn(g);
    plot03.write(filename);
}


static char help[] = "Solve small CMEs to benchmark intranode performance.\n\n";

#include<iomanip>
#include <petscmat.h>
#include <petscvec.h>
#include <petscviewer.h>
#include <Sys.h>
#include <armadillo>
#include <cmath>
#include <sys/stat.h>
#include "pacmensl_all.h"

namespace repressilator_cme {
// stoichiometric matrix of the toggle switch model
arma::Mat<PetscInt> SM{{1,-1,0,0,0,0},
                       {0,0,1,-1,0,0},
                       {0,0,0,0,1,-1},};

// reaction parameters
const PetscReal k1{100.0},ka{20.0},ket{6.0},kg{1.0};

// Function to constraint the shape of the Fsp
int lhs_constr(PetscInt num_species,
               PetscInt num_constrs,
               PetscInt num_states,
               PetscInt *states,
               int *vals,
               void *args)
{
  if (num_species != 3) return -1;
  if (num_constrs != 6) return -1;
  for (int i{0}; i < num_states; ++i)
  {
    vals[i * num_constrs]     = (states[num_species * i]);
    vals[i * num_constrs + 1] = (states[num_species * i + 1]);
    vals[i * num_constrs + 2] = (states[num_species * i + 2]);
    vals[i * num_constrs + 3] = (states[num_species * i]) * (states[num_species * i + 1]);
    vals[i * num_constrs + 4] = (states[num_species * i + 2]) * (states[num_species * i + 1]);
    vals[i * num_constrs + 5] = (states[num_species * i]) * (states[num_species * i + 2]);
  }
  return 0;
}

arma::Row<int>    rhs_constr{22,2,2,44,4,44};
arma::Row<double> expansion_factors{0.2,0.2,0.2,0.2,0.2,0.2};
arma::Row<int>    rhs_constr_hyperrec{22,2,2};
arma::Row<double> expansion_factors_hyperrec{0.2,0.2,0.2};

// propensity function
inline PetscReal propensity_rep(const PetscInt *X,const PetscInt k)
{
  switch (k)
  {
    case 0:return k1 / (1.0 + ka * pow(1.0 * PetscReal(X[1]),ket));
    case 1:return kg * PetscReal(X[0]);
    case 2:return k1 / (1.0 + ka * pow(1.0 * PetscReal(X[2]),ket));
    case 3:return kg * PetscReal(X[1]);
    case 4:return k1 / (1.0 + ka * pow(1.0 * PetscReal(X[0]),ket));
    case 5:return kg * PetscReal(X[2]);
    default:return 0.0;
  }
}

int propensity(const int reaction,
               const int num_species,
               const int num_states,
               const int *states,
               PetscReal *outputs,
               void *args)
{
  int (*X)[3] = ( int (*)[3] ) states;
  for (int i = 0; i < num_states; ++i)
  {
    outputs[i] = propensity_rep(X[i],reaction);
  }
  return 0;
}

}

using arma::dvec;
using arma::Col;
using arma::Row;
using std::cout;
using std::endl;

using namespace repressilator_cme;
using namespace pacmensl;

void output_marginals(MPI_Comm comm,std::string model_name,PartitioningType fsp_par_type,
                      PartitioningApproach fsp_repart_approach,std::string constraint_type,
                      DiscreteDistribution &solution,arma::Row<int> constraints);

void output_performance(MPI_Comm comm,std::string &model_name,PartitioningType fsp_par_type,
                        PartitioningApproach fsp_repart_approach,std::string constraint_type,
                        ODESolverType ode_type,
                        FspSolverMultiSinks &fsp_solver);

int ParseOptions(MPI_Comm comm,
                 PartitioningType &fsp_par_type,
                 PartitioningApproach &fsp_repart_approach,
                 PetscBool &output_marginal,
                 PetscBool &fsp_log_events,
                 ODESolverType &ode_solver);

int main(int argc,char *argv[])
{
  Environment my_env(&argc,&argv,help);

  PetscMPIInt    ierr,myRank,num_procs;
  PetscErrorCode petsc_err;
  MPI_Comm       comm;
  std::string    part_type;
  std::string    part_approach;

  MPI_Comm_dup(PETSC_COMM_WORLD,&comm);
  MPI_Comm_size(comm,&num_procs);

  // Register PETSc stages
  PetscLogStage stages[4];
  petsc_err = PetscLogStageRegister("Solve with adaptive custom state set shape",&stages[0]);
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStageRegister("Solve with fixed custom state set shape",&stages[1]);
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStageRegister("Solve with adaptive default state set shape",&stages[2]);
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStageRegister("Solve with fixed default state set shape",&stages[3]);
  CHKERRQ(petsc_err);

  // Default problem
  PetscReal           t_final    = 10.0;
  PetscReal           fsp_tol    = 1.0e-4;
  std::string         model_name = "repressilator";
  Model               repressilator_model(SM,nullptr,propensity,nullptr,nullptr,std::vector<int>());
  arma::Mat<PetscInt> X0         = {21,0,0};
  X0 = X0.t();
  arma::Col<PetscReal> p0         = {1.0};
  arma::Mat<PetscInt>  stoich_mat = SM;

  // Default options
  PartitioningType     fsp_par_type        = PartitioningType::GRAPH;
  PartitioningApproach fsp_repart_approach = PartitioningApproach::REPARTITION;
  ODESolverType        fsp_odes_type       = CVODE;
  PetscBool            output_marginal     = PETSC_FALSE;
  PetscBool            fsp_log_events      = PETSC_FALSE;

  ierr = ParseOptions(comm,fsp_par_type,fsp_repart_approach,output_marginal,fsp_log_events,fsp_odes_type);
  CHKERRQ(ierr);

  FspSolverMultiSinks fsp_solver(comm,fsp_par_type,fsp_odes_type);
  fsp_solver.SetFromOptions();
  fsp_solver.SetModel(repressilator_model);
  fsp_solver.SetInitialDistribution(X0,p0);
  DiscreteDistribution solution;

  petsc_err = PetscLogStagePush(stages[0]);
  CHKERRQ(petsc_err);
  // Solve using adaptive custom constraints
  fsp_solver.SetConstraintFunctions(lhs_constr,nullptr);
  fsp_solver.SetInitialBounds(rhs_constr);
  fsp_solver.SetExpansionFactors(expansion_factors);
  fsp_solver.SetOdeTolerances(1.0e-4,1.0e-14);
  fsp_solver.SetUp();
  solution = fsp_solver.Solve(t_final,fsp_tol,0);

  std::shared_ptr<const StateSetConstrained>
                 fss                 = std::static_pointer_cast<const StateSetConstrained>(fsp_solver.GetStateSet());
  arma::Row<int> final_custom_constr = fss->GetShapeBounds();
  if (fsp_log_events)
  {
    output_performance(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                       std::string("adaptive_custom"),fsp_odes_type,fsp_solver);
  }
  if (output_marginal)
  {
    output_marginals(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                     std::string("adaptive_custom"),solution,final_custom_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm,"\n ================ \n");

  petsc_err = PetscLogStagePop();
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStagePush(stages[1]);
  CHKERRQ(petsc_err);
  // Solve using fixed custom constraints
  fsp_solver.SetConstraintFunctions(lhs_constr,nullptr);
  fsp_solver.SetInitialBounds(final_custom_constr);
  fsp_solver.SetOdeTolerances(1.0e-4,1.0e-14);
  fsp_solver.SetUp();
  solution = fsp_solver.Solve(t_final,fsp_tol,0);
  if (fsp_log_events)
  {
    output_performance(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                       std::string("fixed_custom"),fsp_odes_type,fsp_solver);
  }
  if (output_marginal)
  {
    output_marginals(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                     std::string("fixed_custom"),solution,final_custom_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm,"\n ================ \n");

  petsc_err = PetscLogStagePop();
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStagePush(stages[2]);
  CHKERRQ(petsc_err);
  // Solve using adaptive default constraints
  fsp_solver.SetInitialBounds(rhs_constr_hyperrec);
  fsp_solver.SetExpansionFactors(expansion_factors_hyperrec);
  fsp_solver.SetFromOptions();
  fsp_solver.SetOdeTolerances(1.0e-4,1.0e-14);
  fsp_solver.SetUp();
  solution = fsp_solver.Solve(t_final,fsp_tol,0);
  fss      = std::static_pointer_cast<const StateSetConstrained>(fsp_solver.GetStateSet());
  arma::Row<int> final_hyperrec_constr = fss->GetShapeBounds();
  if (fsp_log_events)
  {
    output_performance(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                       std::string("adaptive_default"),fsp_odes_type,fsp_solver);
  }
  if (output_marginal)
  {
    output_marginals(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                     std::string("adaptive_default"),solution,final_hyperrec_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm,"\n ================ \n");

  petsc_err = PetscLogStagePop();
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStagePush(stages[3]);
  CHKERRQ(petsc_err);
  // Solve using fixed default constraints
  fsp_solver.SetInitialBounds(final_hyperrec_constr);
  fsp_solver.SetExpansionFactors(expansion_factors_hyperrec);
  fsp_solver.SetFromOptions();
  fsp_solver.SetOdeTolerances(1.0e-4,1.0e-14);
  fsp_solver.SetUp();
  solution = fsp_solver.Solve(t_final,fsp_tol,0);
  if (fsp_log_events)
  {
    output_performance(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                       std::string("fixed_default"),fsp_odes_type,fsp_solver);
  }
  if (output_marginal)
  {
    output_marginals(PETSC_COMM_WORLD,model_name,fsp_par_type,fsp_repart_approach,
                     std::string("fixed_default"),solution,final_hyperrec_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm,"\n ================ \n");
  return ierr;
}

int ParseOptions(MPI_Comm comm,
                 PartitioningType &fsp_par_type,
                 PartitioningApproach &fsp_repart_approach,
                 PetscBool &output_marginal,
                 PetscBool &fsp_log_events,
                 ODESolverType &ode_solver)
{
  std::string part_type;
  std::string part_approach;
  part_type     = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  // Read options for fsp
  char      opt[100];
  PetscBool opt_set;
  int       ierr;
  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_partitioning_type",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    fsp_par_type = str2part(std::string(opt));
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_repart_approach",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    fsp_repart_approach = str2partapproach(std::string(opt));
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_output_marginal",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    if (strcmp(opt,"1") == 0 || strcmp(opt,"true") == 0)
    {
      output_marginal = PETSC_TRUE;
    }
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_log_events",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    if (strcmp(opt,"1") == 0 || strcmp(opt,"true") == 0)
    {
      fsp_log_events = PETSC_TRUE;
    }
  }

  ierr = PetscOptionsGetString(NULL,PETSC_NULL,"-fsp_use_solver",opt,100,&opt_set);
  CHKERRQ(ierr);
  if (opt_set)
  {
    if (strcmp(opt,"krylov") == 0)
    {
      ode_solver = KRYLOV;
    } else
    {
      ode_solver = CVODE;
    }
  }

  PetscPrintf(comm,"Partitiniong option %s \n",part2str(fsp_par_type).c_str());
  PetscPrintf(comm,"Repartitoning option %s \n",partapproach2str(fsp_repart_approach).c_str());
  return 0;
}

void output_performance(MPI_Comm comm,std::string &model_name,PartitioningType fsp_par_type,
                        PartitioningApproach fsp_repart_approach,std::string constraint_type,
                        ODESolverType ode_type,
                        FspSolverMultiSinks &fsp_solver)
{
  int myRank,num_procs;
  MPI_Comm_rank(comm,&myRank);
  MPI_Comm_size(comm,&num_procs);

  std::string ode;
  if (ode_type == KRYLOV)
  {
    ode = "krylov";
  } else
  {
    ode = "cvode";
  }

  std::string part_type;
  std::string part_approach;
  part_type     = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  // Output time breakdowns
  FspSolverComponentTiming    sum_times, min_times, max_times;
  sum_times = fsp_solver.ReduceComponentTiming("sum");
  min_times = fsp_solver.ReduceComponentTiming("min");
  max_times = fsp_solver.ReduceComponentTiming("max");

  if (myRank == 0)
  {
    struct stat buffer;
    int fstat;

    std::string   filename =
                      model_name + "_time_breakdown.dat";

    fstat = stat (filename.c_str(), &buffer);

    std::ofstream file;
    file.open(filename,std::ios_base::app);

    if (fstat != 0){
      file << "ncpu,partitioner,fsp_shape,ode_solver,min_cput,max_cput,avg_cput,mat_gen_time,ode_time,state_expand_time,min_flops,max_flops,avg_flops \n";
    }

    file << num_procs << ","
        << part_type << ","
        << constraint_type << ","
        << ode << ","
        << min_times.TotalTime << ","
        << max_times.TotalTime << ","
        << sum_times.TotalTime/num_procs << ","
        << sum_times.MatrixGenerationTime/num_procs << ","
        << sum_times.ODESolveTime/num_procs << ","
        << sum_times.StatePartitioningTime/num_procs << ","
        << min_times.TotalFlops << ","
        << max_times.TotalFlops << ","
        << sum_times.TotalFlops/num_procs << "\n"
        ;
    file.close();
  }

  FiniteProblemSolverPerfInfo perf_info   = fsp_solver.GetSolverPerfInfo();

  if (myRank == 0){
    std::string filename =
        model_name + "_perf_info_" + std::to_string(num_procs) + "_" + part_type + "_" + part_approach + "_" +
            constraint_type + ".dat";
    std::ofstream file;
    file.open(filename);
    file << "Model time, ODEs size, Average processor time (sec) \n";
    for (auto i{0}; i < perf_info.n_step; ++i)
    {
      file << perf_info.model_time[i] << "," << perf_info.n_eqs[i] << "," << perf_info.cpu_time[i] << "\n";
    }
    file.close();
  }
}

void output_marginals(MPI_Comm comm,std::string model_name,PartitioningType fsp_par_type,
                      PartitioningApproach fsp_repart_approach,std::string constraint_type,
                      DiscreteDistribution &solution,arma::Row<int> constraints)
{
  int myRank,num_procs;
  MPI_Comm_rank(comm,&myRank);
  MPI_Comm_size(comm,&num_procs);

  std::string part_type;
  std::string part_approach;
  part_type     = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  /* Compute the marginal distributions */
  std::vector<arma::Col<PetscReal>> marginals(solution.states_.n_rows);
  for (PetscInt                     i{0}; i < marginals.size(); ++i)
  {
    marginals[i] = Compute1DMarginal(solution,i);
  }

  MPI_Comm_rank(PETSC_COMM_WORLD,&myRank);
  if (myRank == 0)
  {
    for (PetscInt i{0}; i < marginals.size(); ++i)
    {
      std::string filename =
                      model_name.append("_marginal_").append(std::to_string(i)).append("_").append(std::to_string(num_procs)).append("_").append(part_type + "_" + part_approach + "_" + constraint_type + ".dat");
      marginals[i].save(filename,arma::raw_ascii);
    }
    std::string   filename =
                      model_name + "_constraint_bounds_" + std::to_string(num_procs) + "_" + part_type + "_"
                          + part_approach +
                          "_" + constraint_type + ".dat";
    constraints.save(filename,arma::raw_ascii);
  }
}


#include <Eigen/Eigenvalues>
#include <cmath>
#include <iostream>
#include <solvers/bfgs.hpp>
#include <solvers/sqp.hpp>

#ifndef SOLVER_ASSERT
#define SOLVER_ASSERT(x) eigen_assert(x)
#endif

namespace sqp {

template <typename T>
SQP<T>::SQP() {
    // TODO(mi): Performance strongly depends on QP solver settings, which is bad.
    qp_solver_.settings().warm_start = true;
    qp_solver_.settings().check_termination = 10;
    qp_solver_.settings().eps_abs = 1e-4;
    qp_solver_.settings().eps_rel = 1e-4;
    qp_solver_.settings().max_iter = 100;
    qp_solver_.settings().adaptive_rho = true;
    qp_solver_.settings().adaptive_rho_interval = 50;
    qp_solver_.settings().alpha = 1.6;
}

template <typename T>
void SQP<T>::solve(Problem& prob, const Vector& x0, const Vector& lambda0) {
    x_ = x0;
    lambda_ = lambda0;
    run_solve(prob);
}

template <typename T>
void SQP<T>::solve(Problem& prob) {
    const int nx = prob.num_var;
    const int nc = prob.num_constr;

    x_.setZero(nx);
    lambda_.setZero(nc);
    run_solve(prob);
}

template <typename T>
void SQP<T>::run_solve(Problem& prob) {
    Vector p;         // search direction
    Vector p_lambda;  // dual search direction
    Scalar alpha;     // step size

    const int nx = prob.num_var;
    const int nc = prob.num_constr;

    p.resize(nx);
    p_lambda.resize(nc);

    step_prev_.resize(nx);
    grad_L_.resize(nx);
    delta_grad_L_.resize(nx);

    Hess_.resize(nx, nx);
    grad_obj_.resize(nx);
    Jac_constr_.resize(nc, nx);
    constr_.resize(nc);
    l_.resize(nc);
    u_.resize(nc);

    info_.qp_solver_iter = 0;

    if (settings_.iteration_callback) {
        settings_.iteration_callback(*this);
    }

    int& iter = info_.iter;
    for (iter = 1; iter <= settings_.max_iter; iter++) {
        // Solve QP
        solve_qp(prob, p, p_lambda);
        p_lambda -= lambda_;

        alpha = line_search(prob, p);

        // take step
        x_ = x_ + alpha * p;
        lambda_ = lambda_ + alpha * p_lambda;

        // update step info
        step_prev_ = alpha * p;
        primal_step_norm_ = alpha * p.template lpNorm<Eigen::Infinity>();
        dual_step_norm_ = alpha * p_lambda.template lpNorm<Eigen::Infinity>();

        if (settings_.iteration_callback) {
            settings_.iteration_callback(*this);
        }

        if (termination_criteria(x_, prob)) {
            info_.status = SOLVED;
            break;
        }
    }
    if (iter > settings_.max_iter) {
        info_.status = MAX_ITER_EXCEEDED;
    }
}

template <typename Matrix>
bool is_posdef_eigen(Matrix H) {
    Eigen::EigenSolver<Matrix> eigensolver(H);
    for (int i = 0; i < eigensolver.eigenvalues().rows(); i++) {
        double v = eigensolver.eigenvalues()(i).real();
        if (v <= 0) {
            return false;
        }
    }
    return true;
}

template <typename Matrix>
bool is_posdef(Matrix H) {
    Eigen::LLT<Matrix> llt(H);
    if (llt.info() == Eigen::NumericalIssue) {
        return false;
    }
    return true;
}

template <typename T>
bool SQP<T>::termination_criteria(const Vector& x, Problem& prob) {
    if (primal_step_norm_ <= settings_.eps_prim && dual_step_norm_ <= settings_.eps_dual &&
        max_constraint_violation(x, prob) <= settings_.eps_prim) {
        return true;
    }
    return false;
}

template <typename Derived>
inline bool is_nan(const Eigen::MatrixBase<Derived>& x) {
    // return ((x.array() == x.array())).all();
    return x.array().isNaN().any();
}

template <typename T>
void SQP<T>::solve_qp(Problem& prob, Vector& step, Vector& lambda) {
    /* QP from linearized NLP:
     * minimize     0.5 x'.P.x + q'.x
     * subject to   l <= A.x + b <= u
     *
     * with:
     *   P      Hessian of Lagrangian
     *   q      objective gradient
     *   A,b    linearized constraint at current iterate
     *   l,u    constraint bounds
     *
     * transform to:
     * minimize     0.5 x'.P.x + q'.x
     * subject to   l <= A.x <= u
     *
     * Where the constraint bounds l,u set to l=u for equality constraints or
     * set to +/-INFINITY if unbounded.
     */
    prob.objective_linearized(x_, grad_obj_, obj_);
    prob.constraint_linearized(x_, Jac_constr_, constr_, l_, u_);

    delta_grad_L_ = -grad_L_;
    grad_L_ = grad_obj_ + Jac_constr_.transpose() * lambda_;

    // BFGS update
    if (info_.iter == 1) {
        Hess_.setIdentity();
    } else {
        delta_grad_L_ += grad_L_;  // delta_grad_L_ = grad_L_prev - grad_L
        BFGS_update(Hess_, step_prev_, delta_grad_L_);
    }

    if (!is_posdef(Hess_)) {
        std::cout << "Hessian not positive definite\n";
        Scalar tau = 1e-3;
        Vector v = Vector(prob.num_var);
        while (!is_posdef(Hess_)) {
            v.setConstant(tau);
            Hess_ += v.asDiagonal();
            tau *= 10;
        }
    }
    if (is_nan(Hess_)) {
        std::cout << "Hessian is NaN\n";
    }

    SOLVER_ASSERT(is_posdef(Hess_));
    SOLVER_ASSERT(!is_nan(Hess_));

    // Constraints
    // from   l <= A.x + b <= u
    // to   l-b <= A.x     <= u-b
    Vector l = l_ - constr_;
    Vector u = u_ - constr_;
    Matrix& A = Jac_constr_;
    Matrix& P = Hess_;
    Vector& q = grad_obj_;

    // solve the QP
    run_solve_qp(P, q, A, l, u, step, lambda);

    if (settings_.second_order_correction) {
        second_order_correction(prob, step, lambda);
    }

    // TODO:
    // B is not convex then use grad_L as step direction
    // i.e. fallback to steepest descent of Lagrangian
}

template <typename T>
bool SQP<T>::run_solve_qp(const Matrix& P, const Vector& q, const Matrix& A, const Vector& l,
                          const Vector& u, Vector& prim, Vector& dual) {
    qp_solver::QuadraticProblem<Scalar> qp_;

    qp_.P = &P;
    qp_.q = &q;
    qp_.A = &A;
    qp_.l = &l;
    qp_.u = &u;

    qp_solver_.setup(qp_);
    qp_solver_.solve(qp_);

    info_.qp_solver_iter += qp_solver_.info().iter;

    if (qp_solver_.info().status == qp_solver::NUMERICAL_ISSUES) {
        std::cout << "QPSolver NUMERICAL_ISSUES\n";
        return false;
    }
    // if (qp_solver_.info().status == qp_solver::MAX_ITER_EXCEEDED) {
    //     std::cout << "QPSolver MAX_ITER_EXCEEDED\n";
    //     return false;
    // }

    prim = qp_solver_.primal_solution();
    dual = qp_solver_.dual_solution();

    SOLVER_ASSERT(!is_nan(prim));
    SOLVER_ASSERT(!is_nan(dual));

    return true;
}

template <typename T>
void SQP<T>::second_order_correction(Problem& prob, Vector& p, Vector& lambda) {
    // Scalar mu, constr_l1, phi_l1;
    // constr_l1 = constraint_norm(constr_, l_, u_);
    // mu = (grad_obj_.dot(p) + 0.5 * p.dot(Hess_ * p)) / ((1 - settings_.rho) * constr_l1);
    // phi_l1 = obj_ + mu * constr_l1;

    // Scalar obj_step, constr_l1_step, phi_l1_step;
    // Vector x_step = x_ + p;
    // prob.objective(x_step, obj_step);
    // constr_l1_step = constraint_norm(x_step, prob);
    // phi_l1_step = obj_step + mu * constr_l1_step;

    // printf("phi_l1_step %f  phi_l1 %f  constr_l1_step %f  constr_l1 %f\n", phi_l1_step, phi_l1,
    //        constr_l1_step, constr_l1);
    // if (phi_l1_step >= phi_l1 && constr_l1_step >= constr_l1) {
    {
        Vector x_step = x_ + p;
        Vector constr_step(constr_.rows());
        prob.constraint(x_step, constr_step, l_, u_);

        Matrix& A = Jac_constr_;
        Matrix& P = Hess_;
        Vector& q = grad_obj_;

        Vector d = constr_step - A * p;
        Vector l = l_ - d;
        Vector u = u_ - d;

        // TODO: only l and u change, possible to update QP solver more efficiently
        run_solve_qp(P, q, A, l, u, p, lambda);
    }
}
template <typename T>
typename SQP<T>::Scalar SQP<T>::line_search(Problem& prob, const Vector& p) {
    // Note: using members obj_ and grad_obj_, which are updated in solve_qp().

    Scalar mu, phi_l1, Dp_phi_l1;
    const Scalar tau = settings_.tau;  // line search step decrease, 0 < tau < settings.tau

    Scalar constr_l1 = constraint_norm(constr_, l_, u_);

    // get mu from merit function model using hessian of Lagrangian instead
    mu = (grad_obj_.dot(p) + 0.5 * p.dot(Hess_ * p)) / ((1 - settings_.rho) * constr_l1);

    phi_l1 = obj_ + mu * constr_l1;
    Dp_phi_l1 = grad_obj_.dot(p) - mu * constr_l1;

    Scalar alpha = 1.0;
    int i;
    for (i = 1; i < settings_.line_search_max_iter; i++) {
        Scalar obj_step;
        Vector x_step = x_ + alpha * p;
        prob.objective(x_step, obj_step);

        Scalar phi_l1_step = obj_step + mu * constraint_norm(x_step, prob);
        if (phi_l1_step <= phi_l1 + alpha * settings_.eta * Dp_phi_l1) {
            // accept step
            break;
        } else {
            alpha = tau * alpha;
        }
    }
    return alpha;
}

template <typename T>
typename SQP<T>::Scalar SQP<T>::constraint_norm(const Vector &constr, const Vector &l, const Vector &u) const {
    Scalar c_l1 = DIV_BY_ZERO_REGUL;

    // l <= c(x) <= u
    c_l1 += (l - constr).cwiseMax(0.0).sum();
    c_l1 += (constr - u).cwiseMax(0.0).sum();

    return c_l1;
}

template <typename T>
typename SQP<T>::Scalar SQP<T>::constraint_norm(const Vector& x, Problem& prob) {
    // Note: uses members constr_, l_ and u_ as temporary
    prob.constraint(x, constr_, l_, u_);

    return constraint_norm(constr_, l_, u_);
}

template <typename T>
typename SQP<T>::Scalar SQP<T>::max_constraint_violation(const Vector& x, Problem& prob) {
    // Note: uses members constr_, l_ and u_ as temporary

    Scalar c_max = 0;
    prob.constraint(x, constr_, l_, u_);

    // l <= c(x) <= u
    if (prob.num_constr > 0) {
        c_max = fmax(c_max, (l_ - constr_).maxCoeff());
        c_max = fmax(c_max, (constr_ - u_).maxCoeff());
    }

    return c_max;
}

template class SQP<double>;
template class SQP<float>;

}  // namespace sqp


#include "PhysicsTools/Utilities/interface/BreitWigner.h"
#include "PhysicsTools/Utilities/interface/HistoChiSquare.h"
#include "PhysicsTools/Utilities/interface/HistoPoissonLikelihoodRatio.h"
#include "PhysicsTools/Utilities/interface/RootMinuitCommands.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/rootTf1.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "TFile.h"
#include "TH1.h"
#include "TF1.h"
#include "TCanvas.h"
#include "TROOT.h"
#include <boost/shared_ptr.hpp>
#include <iostream>
#include "PhysicsTools/Utilities/interface/Operations.h"
//using namespace std;
//using namespace boost;

typedef funct::Product<funct::Parameter, funct::BreitWigner>::type FitFunction;
typedef fit::HistoChiSquare<FitFunction> ChiSquared;
typedef fit::HistoPoissonLikelihoodRatio<FitFunction> PoissonLR;

template <typename T>
int main_t(const std::string tag) {
  try {
    fit::RootMinuitCommands<T> commands("PhysicsTools/Utilities/test/testZMassFit.txt");

    const char* kYield = "Yield";
    const char* kMass = "Mass";
    const char* kGamma = "Gamma";

    funct::Parameter yield(kYield, commands.par(kYield));
    funct::Parameter mass(kMass, commands.par(kMass));
    funct::Parameter gamma(kGamma, commands.par(kGamma));
    funct::BreitWigner bw(mass, gamma);

    FitFunction f = yield * bw;
    TF1 startFun = root::tf1("startFun", f, 0, 200, yield, mass, gamma);
    TH1D histo("histo", "Z mass (GeV/c)", 200, 0, 200);
    histo.FillRandom("startFun", yield);
    TCanvas canvas;
    startFun.Draw();
    canvas.SaveAs((tag + "breitWigner.eps").c_str());
    histo.Draw();
    canvas.SaveAs((tag + "breitWignerHisto.eps").c_str());
    startFun.Draw("same");
    canvas.SaveAs((tag + "breitWignerHistoFun.eps").c_str());
    histo.Draw("e");
    startFun.Draw("same");

    T chi2(f, &histo, 80, 120);
    int fullBins = chi2.numberOfBins();
    std::cout << "N. deg. of freedom: " << fullBins << std::endl;
    fit::RootMinuit<T> minuit(chi2, true);
    commands.add(minuit, yield);
    commands.add(minuit, mass);
    commands.add(minuit, gamma);
    commands.run(minuit);
    ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepSym<double, 3> > err;
    minuit.getErrorMatrix(err);
    std::cout << "error matrix:" << std::endl;
    for (size_t i = 0; i < 3; ++i) {
      for (size_t j = 0; j < 3; ++j) {
        std::cout << err(i, j) << "\t";
      }
      std::cout << std::endl;
    }
    root::plot<FitFunction>((tag + "breitWignerHistoFunFit.eps").c_str(), histo, f, 80, 120, yield, mass, gamma);
  } catch (std::exception& err) {
    std::cerr << "Exception caught:\n" << err.what() << std::endl;
    return 1;
  }

  return 0;
}

int main() {
  gROOT->SetStyle("Plain");
  std::cout << "=== chi-2 fit ===" << std::endl;
  int ret1 = main_t<ChiSquared>("chi2_");
  if (ret1 != 0)
    return ret1;
  std::cout << "=== poisson LR fit ===" << std::endl;
  int ret2 = main_t<PoissonLR>("possLR_");
  if (ret2 != 0)
    return ret2;
  return 0;
}


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 2020/7/3 5:48:55
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "INF" << endl;
  exit(0);
}

// ----- 2D, 3D, 4D vectors -----
// Referring to ymatsux-san's source code: https://atcoder.jp/contests/abc138/submissions/7018300

template <typename T>
vector<vector<T>> Make2DVector(size_t d0, size_t d1, T v = T{})
{
  return vector<vector<T>>(d0, vector<T>(d1, v));
}

template <typename T>
vector<vector<vector<T>>> Make3DVector(size_t d0, size_t d1, size_t d2, T v = T{})
{
  return vector<vector<vector<T>>>(d0, Make2DVector(d1, d2, v));
}

template <typename T>
vector<vector<vector<vector<T>>>> Make4DVector(size_t d0, size_t d1, size_t d2, size_t d3, T v = T{})
{
  return vector<vector<vector<vector<T>>>>(d0, Make3DVector(d1, d2, d3, v));
}

// ----- Compressor -----
// referring to ymatsux-san's code:
// https://atcoder.jp/contests/abc168/submissions/13337691

template <typename T = ll>
class Compressor
{
  vector<T> raw;
  map<T, int> index;

public:
  Compressor() {}

  template <typename Container>
  Compressor(Container const &V) { append(V); }

  template <typename Iter>
  Compressor(Iter first, Iter last) { append(first, last); }

  template <typename Container>
  void append(Container const &V) { append(V.begin(), V.end()); }

  template <typename Iter>
  void append(Iter first, Iter last)
  {
    set<T> S(first, last);
    raw = vector<T>(S.begin(), S.end());
    sort(raw.begin(), raw.end());
    for (auto i = size_t{0}; i < raw.size(); ++i)
    {
      index[raw[i]] = i;
    }
  }

  T to_raw(int i) { return raw[i]; }
  int to_index(T t) { return index[t]; }
  size_t size() { return raw.size(); }
};

constexpr int dx[4] = {1, 0, -1, 0};
constexpr int dy[4] = {0, 1, 0, -1};

// ----- Solve -----

struct Point
{
  int x, y;
};

ostream &operator<<(ostream &os, vector<vector<bool>> v)
{
  for (auto i{size_t{0}}; i < v.size(); ++i)
  {
    for (auto j{size_t{0}}; j < v[i].size(); ++j)
    {
      os << (v[i][j] ? '#' : '.');
    }
    os << endl;
  }
  return os;
}

class Solve
{
  int n, m;
  vector<ll> a, b, c, d, e, f;
  Compressor<ll> x, y;
  vector<vector<bool>> visited;
  vector<vector<vector<bool>>> dir;
  int size_x, size_y;

public:
  Solve(int n, int m) : n{n}, m{m}, a(n), b(n), c(n), d(m), e(m), f(m)
  {
    for (auto i{0}; i < n; ++i)
    {
      cin >> a[i] >> b[i] >> c[i];
    }
    for (auto i{0}; i < m; ++i)
    {
      cin >> d[i] >> e[i] >> f[i];
    }
    vector<ll> xs{mInfty<ll>(), 0LL, Infty<ll>()}, ys{mInfty<ll>(), 0LL, Infty<ll>()};
    copy(a.begin(), a.end(), back_inserter(xs));
    copy(b.begin(), b.end(), back_inserter(xs));
    copy(c.begin(), c.end(), back_inserter(ys));
    copy(d.begin(), d.end(), back_inserter(xs));
    copy(e.begin(), e.end(), back_inserter(ys));
    copy(f.begin(), f.end(), back_inserter(ys));
    x.append(xs);
    y.append(ys);
    size_x = x.size() - 1;
    size_y = y.size() - 1;
    visited = Make2DVector<bool>(size_x, size_y, false);
    dir = Make3DVector<bool>(size_x, size_y, 4, true);
    set_segment();
  }

  void flush()
  {
    bfs();
#if DEBUG == 1
    cerr << visited;
#endif
    cout << answer() << endl;
  }

private:
  void bfs()
  {
    queue<Point> q;
    {
      int i{x.to_index(0)};
      int j{y.to_index(0)};
      q.push(Point{i, j});
    }
    while (!q.empty())
    {
      auto p{q.front()};
      q.pop();
      for (auto k{0}; k < 4; ++k)
      {
        if (!dir[p.x][p.y][k])
        {
          continue;
        }
        auto nx{p.x + dx[k]};
        auto ny{p.y + dy[k]};
        if (!valid(nx, ny))
        {
          continue;
        }
        if (visited[nx][ny])
        {
          continue;
        }
        visited[nx][ny] = true;
        q.push(Point{nx, ny});
      }
    }
  }

  void set_segment()
  {
    for (auto i{0}; i < n; ++i)
    {
      set_segment_x(a[i], b[i], c[i]);
    }
    for (auto i{0}; i < m; ++i)
    {
      set_segment_y(d[i], e[i], f[i]);
    }
  }

  void set_segment_x(ll xa, ll xb, ll yc)
  {
    int ia = x.to_index(xa);
    int ib = x.to_index(xb);
    int jc = y.to_index(yc);
    for (auto i{ia}; i < ib; ++i)
    {
      dir.at(i).at(jc - 1).at(1) = false;
      dir.at(i).at(jc).at(3) = false;
    }
  }

  void set_segment_y(ll xd, ll ye, ll yf)
  {
    int id = x.to_index(xd);
    int je = y.to_index(ye);
    int jf = y.to_index(yf);
    for (auto j{je}; j < jf; ++j)
    {
      dir.at(id - 1).at(j).at(0) = false;
      dir.at(id).at(j).at(2) = false;
    }
  }

  bool valid(int i, int j)
  {
    return 0 <= i && i < size_x && 0 <= j && j < size_y;
  }

  ll area(int i, int j)
  {
    ll width{x.to_raw(i + 1) - x.to_raw(i)};
    ll height{y.to_raw(j + 1) - y.to_raw(j)};
    return height * width;
  }

  ll answer()
  {
    for (auto i{0}; i < size_x; ++i)
    {
      if (visited[i][0] || visited[i][size_y - 1])
      {
        No();
      }
    }
    for (auto j{0}; j < size_y; ++j)
    {
      if (visited[0][j] || visited[size_x - 1][j])
      {
        No();
      }
    }
    ll ans{0};
    for (auto i{1}; i < size_x - 1; ++i)
    {
      for (auto j{1}; j < size_y - 1; ++j)
      {
        if (visited[i][j])
        {
          ans += area(i, j);
        }
      }
    }
    return ans;
  }
};

// ----- main() -----

int main()
{
  int n, m;
  cin >> n >> m;
  Solve solve(n, m);
  solve.flush();
}


#include <blitzml/sparse_linear/logreg_solver.h>

#include <blitzml/base/math_util.h>
#include <blitzml/base/vector_util.h>

#include "math.h"

namespace BlitzML {

inline value_t SparseLogRegSolver::compute_prob(index_t j) {
  return is_positive_label[j] ? 1 / (1 + exp_Aomega[j])
                              : 1 - 1 / (1 + exp_Aomega[j]);
}

inline value_t SparseLogRegSolver::compute_x_value(index_t j) {
  return is_positive_label[j] ? -1 / (1 + exp_Aomega[j])
                              : 1 - 1 / (1 + exp_Aomega[j]);
}

void SparseLogRegSolver::initialize_blitz_variables(value_t* initial_conditions) {
  initialize_is_positive_label();
  initialize_model(initial_conditions);
  check_for_degenerate_problem();
  initialize_x_variables();
  check_for_poor_initialization();
  update_bias(30);
  initialize_y_and_z_variables();
}

void SparseLogRegSolver::initialize_is_positive_label() {
  is_positive_label.assign(num_examples, false);
  const value_t* labels = data->b_values();
  num_positive_labels = 0;
  for (index_t j = 0; j < num_examples; ++j) {
    if (labels[j] > 0.) {
      is_positive_label[j] = true;
      ++num_positive_labels;
    }
  }
}

void SparseLogRegSolver::initialize_x_variables() {
  compute_Aomega();

  if (l0_norm(Aomega) == 0) {
    exp_Aomega.assign(num_examples, 1);
  } else {
    exp_Aomega.resize(num_examples);
    for (index_t j = 0; j < num_examples; ++j) {
      exp_Aomega[j] = exp(Aomega[j]);
    }
  }

  x.resize(num_examples);
  ATx.resize(num_components);
  kappa_x = 1.;
  sum_x = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    x[j] = compute_x_value(j);
    sum_x += x[j];
  }
}

void SparseLogRegSolver::initialize_y_and_z_variables() {
  y.assign(num_examples, 0.);
  ATy.assign(num_components, 0.);

  z = x;
  ATz.assign(num_components, 0.);
  kappa_z = 1.0;
  z_match_x = true;
  z_match_y = false;
}

void SparseLogRegSolver::check_for_degenerate_problem() {
  problem_is_degenerate = false;
  if (use_bias) {
    if (num_positive_labels == 0) {
      problem_is_degenerate = true;
      bias = -100.;
    } else if (num_positive_labels == num_examples) {
      problem_is_degenerate = true;
      bias = 100.;
    }
    if (problem_is_degenerate) {
      omega.assign(num_components, 0.);
    }
  }
}

void SparseLogRegSolver::check_for_poor_initialization() {
  value_t max_exp_Aomega = max_vector(exp_Aomega);
  if (max_exp_Aomega > 1e30 || max_exp_Aomega != max_exp_Aomega) {
    omega.assign(num_components, 0.);
    initialize_x_variables();
  }
}

void SparseLogRegSolver::update_bias(int max_newton_itr) {
  if (!use_bias || problem_is_degenerate) {
    return;
  }

  value_t exp_delta_total = 1.0;
  if (is_vector_const(exp_Aomega)) {
    // Special case closed-form solution:
    // (this case occurs when we initialize model as all zeros)
    exp_delta_total = (exp_Aomega[0] * num_positive_labels) /
                      (num_examples - num_positive_labels);
    scale_vector(exp_Aomega, exp_delta_total);
    max_newton_itr = 0;
  }

  bool last_update_positive = false;
  for (int itr = 0; itr < max_newton_itr; ++itr) {
    // Compute derivative:
    value_t sum_p = 0.;
    value_t h = 0.;
    for (index_t j = 0; j < num_examples; ++j) {
      value_t p = 1 / (1 + exp_Aomega[j]);
      sum_p += (1 - p);
      h += p * (1 - p);
    }

    // Compute update:
    value_t deriv = num_positive_labels - sum_p;
    value_t exp_delta = 1 + deriv / h;

    if (exp_delta > 1.) {
      last_update_positive = true;
    } else if (last_update_positive) {
      break;
    } else if (exp_delta < 0.01) {
      exp_delta = num_positive_labels / sum_p;
    }

    // Apply update:
    exp_delta_total *= exp_delta;
    scale_vector(exp_Aomega, exp_delta);
  }

  value_t change = log(exp_delta_total);
  bias += change;

  sum_x = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    Aomega[j] += change;
    x[j] = compute_x_value(j);
    sum_x += x[j];
  }
  z_match_x = false;
}

void SparseLogRegSolver::perform_backtracking() {
  if (problem_is_degenerate) {
    return;
  }

  std::vector<value_t> low_exp_Aomega = exp_Aomega;

  sum_x = 0.;
  for (int j = 0; j < num_examples; ++j) {
    exp_Aomega[j] = exp(Aomega[j] + Delta_Aomega[j] + Delta_bias);
    x[j] = compute_x_value(j);
    sum_x += x[j];
  }
  value_t deriv_high = compute_backtracking_step_size_derivative(1.0);

  value_t step_size = 1.0;

  if (deriv_high > 0) {
    value_t high_step = 1.0;
    std::vector<value_t> high_exp_Aomega = exp_Aomega;
    value_t low_step = 0.;

    step_size = 0.5;
    int backtrack_itr = 0;
    while (++backtrack_itr) {
      sum_x = 0.;
      for (index_t j = 0; j < num_examples; ++j) {
        if (high_exp_Aomega[j] < 1e15 && high_exp_Aomega[j] > 1e-15 &&
             low_exp_Aomega[j] < 1e15 &&  low_exp_Aomega[j] > 1e-15) {
          exp_Aomega[j] = sqrt(high_exp_Aomega[j] * low_exp_Aomega[j]);
        } else {
          exp_Aomega[j] = exp(Aomega[j] +
                              step_size * (Delta_Aomega[j] + Delta_bias));
        }
        x[j] = compute_x_value(j);
        sum_x += x[j];
      }

      value_t deriv = compute_backtracking_step_size_derivative(step_size);
      if (backtrack_itr >= 5 && deriv < 0) {
        break;
      } else if (backtrack_itr >= 20) {
        break;
      }

      if (deriv < 0) {
        low_step = step_size;
        low_exp_Aomega = exp_Aomega;
      } else {
        high_step = step_size;
        high_exp_Aomega = exp_Aomega;
      }
      step_size = (high_step + low_step) / 2;
    }
  }

  for (const_index_itr ind = ws.begin_indices();
       ind != ws.end_indices();
       ++ind) {
    index_t i = ws.ith_member(*ind);
    omega[i] += step_size * Delta_omega[*ind];
  }
  bias += step_size * Delta_bias;
  for (index_t j = 0; j < num_examples; ++j) {
    Aomega[j] += step_size * (Delta_Aomega[j] + Delta_bias);
  }
}

value_t SparseLogRegSolver::compute_dual_obj() const {
  value_t loss = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    if (is_positive_label[j]) {
      if (Aomega[j] > -50) {
        loss += log1p(1/exp_Aomega[j]);
      }  else {
        loss -= Aomega[j];
      }
    } else {
      loss += log1p(exp_Aomega[j]);
    }
  }
  return -(loss + l1_penalty * l1_norm(omega));
}

void SparseLogRegSolver::update_subproblem_obj_vals() {
  obj_vals.set_dual_obj(compute_dual_obj());

  value_t gap = 0.;
  if (kappa_x >= 1) {
    kappa_x = 1;
  }
  value_t log_kappa = log1p(kappa_x - 1);
  if (log_kappa != log_kappa) {
    throw log_kappa;
  }
  for (int j = 0; j < num_examples; ++j) {
    value_t prob, mid_term, last_term;
    if (is_positive_label[j]) {
      prob = kappa_x / (1 + exp_Aomega[j]);
      mid_term = log1p(exp_Aomega[j] - kappa_x);
      last_term = -Aomega[j];
    } else {
      prob = kappa_x - kappa_x / (1 + exp_Aomega[j]);
      mid_term = log1p(1 / exp_Aomega[j] - kappa_x);
      last_term = Aomega[j];
    }
    gap += prob * log_kappa + (1 - prob) * mid_term + last_term;
  }

  gap += l1_penalty * l1_norm(omega);

  obj_vals.set_primal_obj_x(obj_vals.dual_obj() + gap);
}

void SparseLogRegSolver::update_newton_2nd_derivatives(value_t epsilon_to_add) {
  sum_newton_2nd_derivatives = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    value_t prob = compute_prob(j);
    newton_2nd_derivatives[j] = prob * (1 - prob) + epsilon_to_add;
    sum_newton_2nd_derivatives += newton_2nd_derivatives[j];
  }
}

} // namespace BlitzML


#include "pso.h"

#include <Eigen/StdVector>

template <
  typename Scalar,
  typename DerivedX,
  typename DerivedLB,
  typename DerivedUB>
IGL_INLINE Scalar igl::pso(
  const std::function< Scalar (DerivedX &) > f,
  const Eigen::MatrixBase<DerivedLB> & LB,
  const Eigen::MatrixBase<DerivedUB> & UB,
  const int max_iters,
  const int population,
  DerivedX & X)
{
  const Eigen::Array<bool,Eigen::Dynamic,1> P =
    Eigen::Array<bool,Eigen::Dynamic,1>::Zero(LB.size(),1);
  return igl::pso(f,LB,UB,P,max_iters,population,X);
}

template <
  typename Scalar,
  typename DerivedX,
  typename DerivedLB,
  typename DerivedUB,
  typename DerivedP>
IGL_INLINE Scalar igl::pso(
  const std::function< Scalar (DerivedX &) > f,
  const Eigen::MatrixBase<DerivedLB> & LB,
  const Eigen::MatrixBase<DerivedUB> & UB,
  const Eigen::DenseBase<DerivedP> & P,
  const int max_iters,
  const int population,
  DerivedX & X)
{
  const int dim = LB.size();
  assert(UB.size() == dim && "UB should match LB size");
  assert(P.size() == dim && "P should match LB size");
  typedef std::vector<DerivedX,Eigen::aligned_allocator<DerivedX> > VectorList;
  VectorList position(population);
  VectorList best_position(population);
  VectorList velocity(population);
  Eigen::Matrix<Scalar,Eigen::Dynamic,1> best_f(population);
  // https://en.wikipedia.org/wiki/Particle_swarm_optimization#Algorithm
  //
  // g → X
  // p_i → best[i]
  // v_i → velocity[i]
  // x_i → position[i]
  Scalar min_f = std::numeric_limits<Scalar>::max();
  for(int p=0;p<population;p++)
  {
    {
      const DerivedX R = DerivedX::Random(dim).array()*0.5+0.5;
      position[p] = LB.array() + R.array()*(UB-LB).array();
    }
    best_f[p] = f(position[p]);
    best_position[p] = position[p];
    if(best_f[p] < min_f)
    {
      min_f = best_f[p];
      X = best_position[p];
    }
    {
      const DerivedX R = DerivedX::Random(dim);
      velocity[p] = (UB-LB).array() * R.array();
    }
  }

  int iter = 0;
  Scalar omega = 0.98;
  Scalar phi_p = 0.01;
  Scalar phi_g = 0.01;
  while(true)
  {
    //if(iter % 10 == 0)
    //{
    //  std::cout<<iter<<":"<<std::endl;
    //  for(int p=0;p<population;p++)
    //  {
    //    std::cout<<"  "<<best_f[p]<<", "<<best_position[p]<<std::endl;
    //  }
    //  std::cout<<std::endl;
    //}

    for(int p=0;p<population;p++)
    {
      const DerivedX R_p = DerivedX::Random(dim).array()*0.5+0.5;
      const DerivedX R_g = DerivedX::Random(dim).array()*0.5+0.5;
      velocity[p] =
        omega * velocity[p].array() +
        phi_p * R_p.array() *(best_position[p] - position[p]).array() +
        phi_g * R_g.array() *(               X - position[p]).array();
      position[p] += velocity[p];
      // Clamp to bounds
      for(int d = 0;d<dim;d++)
      {
//#define IGL_PSO_REFLECTION
#ifdef IGL_PSO_REFLECTION
        assert(!P(d));
        // Reflect velocities if exceeding bounds
        if(position[p](d) < LB(d))
        {
          position[p](d) = LB(d);
          if(velocity[p](d) < 0.0) velocity[p](d) *= -1.0;
        }
        if(position[p](d) > UB(d))
        {
          position[p](d) = UB(d);
          if(velocity[p](d) > 0.0) velocity[p](d) *= -1.0;
        }
#else
//#warning "trying no bounds on periodic"
//        // TODO: I'm not sure this is the right thing to do/enough. The
//        // velocities could be weird. Suppose the current "best" value is ε and
//        // the value is -ε and the "periodic bounds" [0,2π]. Moding will send
//        // the value to 2π-ε but the "velocity" term will now be huge pointing
//        // all the way from 2π-ε to ε.
//        //
//        // Q: Would it be enough to try (all combinations) of ±(UB-LB) before
//        // computing velocities to "best"s? In the example above, instead of
//        //
//        //     v += best - p = ε - (2π-ε) = -2π+2ε
//        //
//        // you'd use
//        //
//        //     v +=  / argmin  |b - p|            \  - p = (ε+2π)-(2π-ε) = 2ε
//        //          |                              |
//        //           \ b∈{best, best+2π, best-2π} /
//        //
//        // Though, for multivariate b,p,v this would seem to explode
//        // combinatorially.
//        //
//        // Maybe periodic things just shouldn't be bounded and we hope that the
//        // forces toward the current minima "regularize" them away from insane
//        // values.
//        if(P(d))
//        {
//          position[p](d) = std::fmod(position[p](d)-LB(d),UB(d)-LB(d))+LB(d);
//        }else
//        {
//          position[p](d) = std::max(LB(d),std::min(UB(d),position[p](d)));
//        }
        position[p](d) = std::max(LB(d),std::min(UB(d),position[p](d)));
#endif
      }
      const Scalar fp = f(position[p]);
      if(fp<best_f[p])
      {
        best_f[p] = fp;
        best_position[p] = position[p];
        if(best_f[p] < min_f)
        {
          min_f = best_f[p];
          X = best_position[p];
        }
      }
    }
    iter++;
    if(iter>=max_iters)
    {
      break;
    }
  }
  return min_f;
}

#ifdef IGL_STATIC_LIBRARY
template float igl::pso<float, Eigen::Matrix<float, 1, -1, 1, 1, -1>, Eigen::Matrix<float, 1, -1, 1, 1, -1>, Eigen::Matrix<float, 1, -1, 1, 1, -1> >(std::function<float (Eigen::Matrix<float, 1, -1, 1, 1, -1>&)>, Eigen::MatrixBase<Eigen::Matrix<float, 1, -1, 1, 1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, -1, 1, 1, -1> > const&, int, int, Eigen::Matrix<float, 1, -1, 1, 1, -1>&);
#endif


/* The Computer Language Benchmarks Game
 * http://benchmarksgame.alioth.debian.org/
 *
 * contributed by Marcin Zalewski & Jeremiah Willcock
 */

#include <iostream>
#include <gmpxx.h>
#include <boost/lexical_cast.hpp>
#include <boost/tuple/tuple.hpp>
#include <algorithm>

using namespace boost;

class Digits {
private:
  unsigned int j;
  tuple<mpz_class, mpz_class, mpz_class> nad;
  mpz_class tmp1, tmp2;

public:
  Digits() { j = 0; get<0>(nad) = 1; get<1>(nad) = 0; get<2>(nad) = 1; }

  inline char operator()() {
    ++j;
    next_term();

    if(get<0>(nad) > get<1>(nad)) return (*this)();

    mpz_mul_2exp(tmp1.get_mpz_t(), get<0>(nad).get_mpz_t(), 1);
    tmp1 += get<0>(nad);
    tmp1 += get<1>(nad);

    mpz_fdiv_qr(tmp1.get_mpz_t(), tmp2.get_mpz_t(), tmp1.get_mpz_t(), get<2>(nad).get_mpz_t());

    tmp2 += get<0>(nad);

    if(tmp2 >= get<2>(nad)) {
      return (*this)();
    } else {
      unsigned int d = tmp1.get_ui();
      eliminate_digit(d);
      return d + '0';
    }
  }

private:

  inline void next_term() {
    unsigned int y = j * 2 + 1;
    mpz_mul_2exp(tmp1.get_mpz_t(), get<0>(nad).get_mpz_t(), 1);
    get<1>(nad) += tmp1;
    get<1>(nad) *= y;
    get<0>(nad) *= j;
    get<2>(nad) *= y;
  }

  inline void eliminate_digit(unsigned int d) {
    mpz_submul_ui(get<1>(nad).get_mpz_t(), get<2>(nad).get_mpz_t(), d);
    get<0>(nad) *= 10;
    get<1>(nad) *= 10;
  }

};

void pi(unsigned int n) {
  unsigned int i = 0;
  Digits digits;

  while((i += 10) <= n) {
    for(int count = 0; count < 10; ++count) {
      std::cout << digits();
    }
    std::cout << "\t:" << i << '\n';
  }

  i -= 10;
  if(n > i) {
    for(int count = 0; count < n - i; ++count) {
      std::cout << digits();
    }
    i += 10;
    for(int count = 0; count < i - n; ++count) {
      std::cout << ' ';
    }
    std::cout << "\t:" << n << '\n';
  }
}

int main(int argc, char** argv) {
  std::cout.sync_with_stdio(false);
  unsigned int count = (argc >= 2 ? boost::lexical_cast<unsigned int>(argv[1]) : 10000);
  pi(count);
  return 0;
}