// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include "gtest/gtest.h"
#include "theia/math/util.h"
#include "theia/util/random.h"
#include "theia/sfm/transformation/align_point_clouds.h"

namespace theia {
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::RowMajor;
using Eigen::Vector3d;

namespace {
double kEpsilon = 1e-6;

void UmeyamaSimpleTest() {
  std::vector<Vector3d> left = {
      Vector3d(0.4, -3.105, 2.147),  Vector3d(1.293, 7.1982, -.068),
      Vector3d(-5.34, 0.708, -3.69), Vector3d(-.345, 1.987, 0.936),
      Vector3d(0.93, 1.45, 1.079),   Vector3d(-3.15, -4.73, 2.49),
      Vector3d(2.401, -2.03, -1.87), Vector3d(3.192, -.573, 0.1),
      Vector3d(-2.53, 3.07, -5.19)};

  const Matrix3d rotation_mat =
      Eigen::AngleAxisd(DegToRad(15.0), Vector3d(1.0, -2.7, 1.9).normalized())
          .toRotationMatrix();
  const Vector3d translation_vec(0, 2, 2);
  const double expected_scale = 1.5;

  // Transform the points.
  std::vector<Vector3d> right;
  for (int i = 0; i < left.size(); i++) {
    Vector3d transformed_point =
        expected_scale * rotation_mat * left[i] + translation_vec;
    right.emplace_back(transformed_point);
  }

  // Compute the similarity transformation.
  Matrix3d rotation;
  Vector3d translation;
  double scale;
  AlignPointCloudsUmeyama(left, right, &rotation, &translation, &scale);

  // Ensure the calculated transformation is the same as the one we set.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      ASSERT_LT(std::abs(rotation(i, j) - rotation_mat(i, j)), kEpsilon);
    }
    ASSERT_LT(std::abs(translation(i) - translation_vec(i)), kEpsilon);
  }
  ASSERT_LT(fabs(expected_scale - scale), kEpsilon);
}

// It is not easy to find a formula for the weights that always works
// We need to make some statistics to see if the function works in most cases
//
// This test adds some noise on a given fraction of the points
// We try to estimate the weights from the errors of the first pass (setting
// weights to 1)
// We can expect that the errors are where the noise was added
// From this first result we can set a weight for each point.
// The weight is given by 1.0/(1.0 + distance)
// Where distance is the distance between right[i] and s * R * left[i] + T
// estimated by a first pass
// To see if the new estimation from these weights is better
// we need to check that the new parameters are closer to the expected
// parameters
void UmeyamaWithWeigthsAndNoise() {
  // Make some statistics
  // 1000 is not a problem the algorith is fast
  static const size_t kNumIterations = 1000;

  // 15 % of noise
  static const float kNoiseRatio = 0.15f;

  // Percentage to consider the test succeeds (is it enough ?)
  static const float kTestsSucceeded = 95.0f;

  size_t succeeded = 0;
  for (size_t iteration = 0; iteration < kNumIterations; ++iteration) {
    // 4 pts required
    const size_t num_points = static_cast<size_t>(theia::RandInt(4, 1000));
    std::vector<Vector3d> left(num_points);
    std::vector<double> weights(num_points, 1.0);

    for (size_t i = 0; i < num_points; ++i) {
      left[i] = Eigen::Vector3d::Random();
    }

    const Matrix3d rotation_mat =
        Eigen::AngleAxisd(DegToRad(theia::RandDouble(0.0, 360.0)),
                          Eigen::Vector3d::Random().normalized())
            .toRotationMatrix();
    const Vector3d translation_vec = Eigen::Vector3d::Random();
    const double expected_scale = theia::RandDouble(0.001, 10);

    // Transform the points.
    std::vector<Vector3d> right;
    for (size_t i = 0; i < left.size(); i++) {
      const Vector3d transformed_point =
          expected_scale * rotation_mat * left[i] + translation_vec;
      right.emplace_back(transformed_point);
    }

    // Add noise on scale, point and translation
    for (size_t i = 0, end = (size_t)(kNoiseRatio * num_points); i < end; ++i) {
      const size_t k = static_cast<size_t>(theia::RandInt(0, num_points - 1));
      const double noiseOnScale = expected_scale + theia::RandDouble(0, 10);
      right[k] = noiseOnScale * rotation_mat * (left[k] + Vector3d::Random()) +
                 translation_vec + Vector3d::Random();
    }

    // We need to find some weights
    Matrix3d rotation_noisy;
    Vector3d translation_noisy;
    double scale_noisy;
    AlignPointCloudsUmeyamaWithWeights(left, right, weights, &rotation_noisy,
                                       &translation_noisy, &scale_noisy);
    for (size_t i = 0; i < num_points; ++i) {
      const double dist = (right[i] - (scale_noisy * rotation_noisy * left[i] +
                                       translation_noisy))
                              .norm();
      weights[i] = 1.0 / (1.0 + dist);
    }
    //

    Matrix3d rotation_weighted;
    Vector3d translation_weighted;
    double scale_weighted;
    AlignPointCloudsUmeyamaWithWeights(left, right, weights, &rotation_weighted,
                                       &translation_weighted, &scale_weighted);

    // Check if the parameters are closer to real parameters ?
    const bool condition_on_scale = (std::abs(scale_weighted - expected_scale) <
                                     std::abs(scale_noisy - expected_scale));
    const bool condition_on_translation =
        (translation_vec - translation_weighted).norm() <
        (translation_vec - translation_noisy).norm();
    const bool condition_on_rotation =
        (rotation_mat - rotation_weighted).norm() <
        (rotation_mat - rotation_noisy).norm();

    if (condition_on_scale && condition_on_translation && condition_on_rotation)
      ++succeeded;
  }

  ASSERT_LE(kTestsSucceeded, 100.0 * succeeded / (0.0 + kNumIterations));
}

}  // namespace

TEST(AlignPointCloudsUmeyama, SimpleTest) {
    UmeyamaSimpleTest();
}

TEST(AlignPointCloudsUmeyamaWithWeights, WeightsAndNoise) {
  UmeyamaWithWeigthsAndNoise();
}

}  // namespace theia


// Static blocked LU Decomposition

#include <stdio.h>
#include <hpx/hpx_init.hpp>
#include <hpx/include/threads.hpp>
#include <hpx/include/lcos.hpp>
#include <hpx/lcos/local/dataflow.hpp>
#include <hpx/util/unwrapped.hpp>

#include "lu-local.h"

#include <boost/assign.hpp>

using std::vector;
using hpx::util::unwrapped;
using hpx::lcos::shared_future;
using hpx::lcos::wait_all;
using hpx::async;
using hpx::lcos::local::dataflow;
using hpx::when_all;
using hpx::make_ready_future;

vector<double> A;
auto diag_op  = unwrapped( &ProcessDiagonalBlock );
auto row_op   = unwrapped( &ProcessBlockOnRow );
auto col_op   = unwrapped( &ProcessBlockOnColumn );
auto inner_op = unwrapped( &ProcessInnerBlock );

void init_df(vector<vector<vector<shared_future<block>>>> &dfArray, int numBlocks, int size) {
    vector<vector<block>> blockList;
    getBlockList(blockList, numBlocks, size);

    dfArray[0].resize(numBlocks);
    dfArray[1].resize(numBlocks);

    for(int i = 0; i < numBlocks; i++){
        dfArray[0][i].resize( numBlocks );
        dfArray[1][i].resize( numBlocks );
    }

    shared_future<int> fsize = make_ready_future(size);

    dfArray[0][0][0] = async( ProcessDiagonalBlock, size, blockList[0][0] );

    for(int i = 1; i < numBlocks; i++) {
        dfArray[0][0][i] = dataflow( row_op, fsize, make_ready_future( blockList[0][i] ), dfArray[0][0][0]);
    }
    for(int i = 1; i < numBlocks; i++) {
        dfArray[0][i][0] = dataflow( col_op, fsize, make_ready_future( blockList[i][0] ), dfArray[0][0][0]);
        for(int j = 1; j < numBlocks; j++) {
            dfArray[0][i][j] = dataflow( inner_op, fsize, make_ready_future( blockList[i][j] ),
                                                   dfArray[0][0][j], dfArray[0][i][0] );
        }
    }
}

void LU( int size, int numBlocks)
{
    vector<vector<vector<shared_future<block>>>> dfArray(2);
    shared_future<int> fsize = make_ready_future(size);

    init_df(dfArray, numBlocks, size);

    for(int i = 1; i < numBlocks; i++) {
        dfArray[i%2][i][i] = dataflow( diag_op, fsize, dfArray[(i-1)%2][i][i]);
        for(int j = i + 1; j < numBlocks; j++){
            dfArray[i%2][i][j] = dataflow( row_op , fsize,
                                           dfArray[(i-1)%2][i][j],
                                           dfArray[ i   %2][i][i] );
        }
        for(int j = i + 1; j < numBlocks; j++){
            dfArray[i%2][j][i] = dataflow( col_op, fsize,
                                           dfArray[(i-1)%2][j][i],
                                           dfArray[ i   %2][i][i] );

            for(int k = i + 1; k < numBlocks; k++) {
                dfArray[i%2][j][k] = dataflow( inner_op, fsize,
                                               dfArray[(i-1)%2][j][k],
                                               dfArray[ i   %2][i][k],
                                               dfArray[ i   %2][j][i] );
            }
        }
    }
    wait_all(dfArray[(numBlocks-1)%2][numBlocks-1][numBlocks-1]);
}

int hpx_main(int argc, char *argv[])
{
    vector<double> originalA;
    int size = 1000;
    int numBlocks = 10;
    unsigned long t1, t2;
    bool runCheck = false;

    if( argc > 1 )
        size = atoi(argv[1]);
    if( argc > 2 )
        numBlocks = atoi(argv[2]);
    if( argc > 3 )
        runCheck = true;
    printf("size = %d, numBlocks = %d\n", size, numBlocks);

    A.resize(size*size, 0);
    if(runCheck) {
        printf("Error checking enabled\n");
        InitMatrix3( size );
        originalA.reserve(size*size);
        for(int i = 0; i < size * size; i++) {
            originalA[i] = A[i];
        }
    } else {
        printf("fast initialization\n");
        fastInitMatrix(size);
    }
    printf("initialization complete\n");

    if(numBlocks == 1) {
        t1 = GetTickCount();
        ProcessDiagonalBlock( size, block(size, 0, size));
        t2 = GetTickCount();
    } else if( numBlocks > 1) {
        t1 = GetTickCount();
        LU( size, numBlocks);
        t2 = GetTickCount();
    } else {
        printf("Error: numBlocks must be greater than 0.\n");
        return hpx::finalize();
    }
    printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000.0);

    if(runCheck) {
        checkResult( originalA,  size );
    }
    return hpx::finalize();
}

int main(int argc, char *argv[])
{
    using namespace boost::assign;
    std::vector<std::string> cfg;
    cfg += "hpx.os_threads=" +
        boost::lexical_cast<std::string>(hpx::threads::hardware_concurrency());

    return hpx::init(argc, argv, cfg);
}


//  Copyright 2017 Peter Dimov.
//
// Distributed under 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/mp11/algorithm.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/config.hpp>
#include <boost/config/workaround.hpp>
#include <tuple>

using boost::mp11::mp_size_t;
using boost::mp11::mp_for_each;
using boost::mp11::mp_with_index;
using boost::mp11::mp_iota_c;

struct F
{
    std::size_t i_;

    explicit F( std::size_t i ): i_( i ) {}

    template<std::size_t I> bool operator()( mp_size_t<I> ) const
    {
        BOOST_TEST_EQ( I, i_ );
        return false;
    }
};

struct G
{
    void operator()( mp_size_t<0> ) const
    {
    }

    template<std::size_t N> void operator()( mp_size_t<N> ) const
    {
        for( std::size_t i = 0; i < N; ++i )
        {
            mp_with_index<N>( i, F(i) );
            mp_with_index<mp_size_t<N>>( i, F(i) );
        }
    }
};

int main()
{
#if BOOST_WORKAROUND( BOOST_MSVC, < 1900 )

    G()( mp_size_t<1>{} );
    G()( mp_size_t<2>{} );
    G()( mp_size_t<3>{} );
    G()( mp_size_t<4>{} );
    G()( mp_size_t<5>{} );
    G()( mp_size_t<6>{} );
    G()( mp_size_t<7>{} );
    G()( mp_size_t<8>{} );
    G()( mp_size_t<9>{} );
    G()( mp_size_t<10>{} );
    G()( mp_size_t<11>{} );
    G()( mp_size_t<12>{} );
    G()( mp_size_t<13>{} );
    G()( mp_size_t<14>{} );
    G()( mp_size_t<15>{} );
    G()( mp_size_t<16>{} );

    G()( mp_size_t<32+1>{} );

    G()( mp_size_t<48+2>{} );

    G()( mp_size_t<64+3>{} );

    G()( mp_size_t<96+4>{} );

    G()( mp_size_t<112+5>{} );

    G()( mp_size_t<128+6>{} );

#else

    mp_for_each<mp_iota_c<134>>( G() );

#endif

    return boost::report_errors();
}


//  Copyright Nick Thompson 2017.
//  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)
//
// Basic sanity check that header <boost/math/special_functions/gamma.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/differentiation/autodiff.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

template <typename T>
T fourth_power(T const& x) {
   T x4 = x * x;  // retval in operator*() uses x4's memory via NRVO.
   x4 *= x4;      // No copies of x4 are made within operator*=() even when squaring.
   return x4;     // x4 uses y's memory in main() via NRVO.
}

void compile_and_link_test()
{
   using namespace boost::math::differentiation;
   auto const x = make_fvar<double, 5>(2.0);  // Find derivatives at x=2.
   auto const y = fourth_power(x);

   check_result<double>(y.derivative(1));
}


/*=============================================================================
    Copyright (c) 2001-2011 Joel de Guzman

    Distributed under 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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A calculator example demonstrating the grammar and semantic actions
//  using phoenix to do the actual expression evaluation. The parser is
//  essentially an "interpreter" that evaluates expressions on the fly.
//
//  [ JDG June 29, 2002 ]   spirit1
//  [ JDG March 5, 2007 ]   spirit2
//
///////////////////////////////////////////////////////////////////////////////

// Spirit v2.5 allows you to suppress automatic generation
// of predefined terminals to speed up complation. With
// BOOST_SPIRIT_NO_PREDEFINED_TERMINALS defined, you are
// responsible in creating instances of the terminals that
// you need (e.g. see qi::uint_type uint_ below).
#define BOOST_SPIRIT_NO_PREDEFINED_TERMINALS

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

#include <iostream>
#include <string>

namespace client
{
    namespace qi = boost::spirit::qi;
    namespace ascii = boost::spirit::ascii;

    ///////////////////////////////////////////////////////////////////////////
    //  Our calculator grammar
    ///////////////////////////////////////////////////////////////////////////
    template <typename Iterator>
    struct calculator : qi::grammar<Iterator, int(), ascii::space_type>
    {
        calculator() : calculator::base_type(expression)
        {
            qi::_val_type _val;
            qi::_1_type _1;
            qi::uint_type uint_;

            expression =
                term                            [_val = _1]
                >> *(   ('+' >> term            [_val += _1])
                    |   ('-' >> term            [_val -= _1])
                    )
                ;

            term =
                factor                          [_val = _1]
                >> *(   ('*' >> factor          [_val *= _1])
                    |   ('/' >> factor          [_val /= _1])
                    )
                ;

            factor =
                uint_                           [_val = _1]
                |   '(' >> expression           [_val = _1] >> ')'
                |   ('-' >> factor              [_val = -_1])
                |   ('+' >> factor              [_val = _1])
                ;
        }

        qi::rule<Iterator, int(), ascii::space_type> expression, term, factor;
    };
}

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Expression parser...\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Type an expression...or [q or Q] to quit\n\n";

    typedef std::string::const_iterator iterator_type;
    typedef client::calculator<iterator_type> calculator;

    boost::spirit::ascii::space_type space; // Our skipper
    calculator calc; // Our grammar

    std::string str;
    int result;
    while (std::getline(std::cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        std::string::const_iterator iter = str.begin();
        std::string::const_iterator end = str.end();
        bool r = phrase_parse(iter, end, calc, space, result);

        if (r && iter == end)
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing succeeded\n";
            std::cout << "result = " << result << std::endl;
            std::cout << "-------------------------\n";
        }
        else
        {
            std::string rest(iter, end);
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "stopped at: \" " << rest << "\"\n";
            std::cout << "-------------------------\n";
        }
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


// (C) Copyright Jeremy Siek 2000. Permission to copy, use, modify, sell and
// distribute this software is granted provided this copyright notice appears
// in all copies. This software is provided "as is" without express or implied
// warranty, and with no claim as to its suitability for any purpose.

#include <functional>
#include <algorithm>
#include <iostream>
#include <boost/iterator_adaptors.hpp>
#include <boost/pending/integer_range.hpp>

int
main(int, char*[])
{
  // This is a simple example of using the transform_iterators class to
  // generate iterators that multiply the value returned by dereferencing
  // the iterator. In this case we are multiplying by 2.
  // Would be cooler to use lambda library in this example.

  int x[] = { 1, 2, 3, 4, 5, 6, 7, 8 };

  typedef std::binder1st< std::multiplies<int> > Function;
  typedef boost::transform_iterator_generator<Function, int*
  >::type doubling_iterator;

  doubling_iterator i(x, std::bind1st(std::multiplies<int>(), 2)),
    i_end(x + sizeof(x)/sizeof(int), std::bind1st(std::multiplies<int>(), 2));

  std::cout << "multiplying the array by 2:" << std::endl;
  while (i != i_end)
    std::cout << *i++ << " ";
  std::cout << std::endl;

  // Here is an example of counting from 0 to 5 using the integer_range class.

  boost::integer_range<int> r(0,5);

  std::cout << "counting to from 0 to 4:" << std::endl;
  std::copy(r.begin(), r.end(), std::ostream_iterator<int>(std::cout, " "));
  std::cout << std::endl;

  return 0;
}


#include <chrono>
#include <filesystem>
#include <iostream>
#include <iterator>
#include <vector>

#include <Eigen/Sparse>
#include <unsupported/Eigen/SparseExtra>

#include <memory_xplatform.h>

#ifdef DEBUG
    #define D(x) (std::cerr << x << std::endl)
#else
    #define D(x) do{}while(0)
#endif

#define CSV_EOL "\n"

#if defined(_WIN32) || defined(__CYGWIN__)
    #define OUTPUT_FILE "windows-output.csv"
#elif defined(__linux__) || defined(unix) || defined(__unix__) || defined(__unix)
    #define OUTPUT_FILE "unix-output.csv"
#else
    #error Unknown environment!
#endif

typedef unsigned long long ull;
typedef Eigen::SparseMatrix<double> SpMat;

struct result {
    unsigned int size;
    ull memory_delta;
    std::chrono::duration<double> solve_time;
    double relative_error;
};

result analyze_matrix(std::string filename);
std::ostream& operator<<(std::ostream& stream, const result& r);

int main() {
    result _result;
    std::ofstream output(OUTPUT_FILE, std::ofstream::out);

    // Write headers
    output << "filename" << ","
        << "size" << ","
        << "memory_delta" << ","
        << "solve_time" << ","
        << "relative_error" << CSV_EOL;

    // Look for matrix in ../matlab/matrix_mtx folder
    std::string path = "../matlab/matrix_mtx";
    for (const auto& entry : std::filesystem::directory_iterator(path)) {
        if (entry.path().extension() == ".mtx") {
            if (! output.is_open()) {
                output.open(OUTPUT_FILE, std::ofstream::app);
            }

            std::cout << entry.path() << std::endl;
            _result = analyze_matrix(entry.path().string());

            D("Writing output...");
            output << entry.path().stem() << "," << _result << CSV_EOL;
            D("");

            output.close();
        }
    }

    return 0;
}

/**
 * Analyze a single matrix given it's filename
 * The matrix will be imported assuming it's in .mtx format.
 *
 * @param std::string filename
 * @return result
 */
result analyze_matrix(std::string filename) {
    result r;
    ull start_tot_virtual, start_proc_virtual, start_proc_physical, start_tot_physical,
        end_tot_virtual, end_proc_virtual, end_proc_physical, end_tot_physical;

    SpMat A; // Eigen::SparseMatrix<double>
    D("Loading matrix file: " << filename);
    Eigen::loadMarket(A, filename);

    // Debug memory usage to cout
    D("Memory Usage (proc/total):");
    start_proc_virtual = memory::process_current_virtual();
    start_tot_virtual = memory::total_virtual();
    D("> Virtual: " << start_proc_virtual << " / " << start_tot_virtual);
    start_proc_physical = memory::process_current_physical();
    start_tot_physical = memory::total_physical();
    D("> Physical: " << start_proc_physical << " / " << start_tot_physical);
    D("");

    D("Solve:");
    D("> Calculating b vector...");
    Eigen::VectorXd x_es = Eigen::VectorXd::Ones(A.rows());
    Eigen::VectorXd b(A.rows());
    b = A*x_es;

    D("> Applying CholeskySimplicial solver...");
    auto chol_start = std::chrono::high_resolution_clock::now();
    Eigen::SimplicialCholesky<SpMat> chol(A);
    Eigen::VectorXd x_ap = chol.solve(b);
    auto chol_finish = std::chrono::high_resolution_clock::now();
    D("");

    D("Memory Usage (proc/total):");
    end_proc_virtual = memory::process_current_virtual();
    end_tot_virtual = memory::total_virtual();
    D("> Virtual: " << end_proc_virtual << " / " << end_tot_virtual);
    end_proc_physical = memory::process_current_physical();
    end_tot_physical = memory::total_physical();
    D("> Physical: " << end_proc_physical << " / " << end_tot_physical);
    D("");

    r.size = A.rows();
    r.memory_delta = end_proc_physical - start_proc_physical;
    r.solve_time = chol_finish - chol_start;
    r.relative_error = (x_ap - x_es).norm() / x_es.norm();

    D("Results");
    D("> Solve time in seconds: " << r.solve_time.count());
    D("> Relative error: " << r.relative_error);
    D("");

    return r;
}

/**
 * Override the << operator to allow an easy print of
 * the results struct to stdout or output file.
 *
 * @params std::ostream& stream
 * @params const result& r
 * @return std::ostream&
 */
std::ostream& operator<<(std::ostream& stream, const result& r) {
    stream << r.size << ","
        << r.memory_delta << ","
        << r.solve_time.count() << ","
        << r.relative_error;

    return stream;
}


/*
Copyright 2013 The Robotics Group, The Maersk Mc-Kinney Moller Institute,

    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.

*/

#ifndef RWLIBS_SOFTBODY_FDUTIL_HPP
#define RWLIBS_SOFTBODY_FDUTIL_HPP

#include <Eigen/Core>

namespace rwlibs { namespace softbody {
    /** @addtogroup softbody */
    /*@{*/

    /**
     * @brief Various numerical methods using finite-differences
     **/
    class FdUtil
    {
      public:
        /**
         * @brief calculates the derivatives of a vector
         *
         * calculates the derivatives of a vector using second-order accurate, centered FD
         *expressions at the interior points and first-order accurate forward/backward differences
         * at the endpoints
         *
         * @param f vector of function values
         * @param df vector to put the derivatives in
         * @param h stepsize
         **/
        static void vectorDerivative (const Eigen::VectorXd& f, Eigen::VectorXd& df,
                                      const double h);
    };
    /*@}*/
}}    // namespace rwlibs::softbody

#endif    // FDUTIL_HPP


/**************************************************************************************************/
// PNGpp copyright 2017 Foster Brereton. See LICENSE.txt for license details.
/**************************************************************************************************/

// stdc++
#include <iostream>
#include <numeric>
#include <random>

// tbb
#include <tbb/parallel_for_each.h>

// boost
#include <boost/program_options.hpp>

// application
#include <pngpp/files.hpp>
#include <pngpp/png.hpp>
#include <pngpp/rgba.hpp>
#include <pngpp/image_utils.hpp>

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

using namespace pngpp;

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

typedef std::vector<std::size_t> indexed_histogram_t;

indexed_histogram_t indexed_histogram(const image_t& image) {
    indexed_histogram_t result(PNG_MAX_PALETTE_LENGTH, 0);

    for (auto entry : image)
        ++result[entry];

    return result;
}

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

typedef std::pair<std::size_t, std::uint8_t> indexed_histogram_pair_t;
typedef std::vector<indexed_histogram_pair_t> indexed_histogram_table_t;

indexed_histogram_table_t make_indexed_histogram_table(const image_t& image) {
    std::size_t               count(image.color_table().size());
    indexed_histogram_table_t result(count);
    indexed_histogram_t       image_hist(indexed_histogram(image));

    for (std::size_t i(0); i < count; ++i) {
        result[i] = indexed_histogram_pair_t(image_hist[i], i);
    }

    return result;
}

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

typedef std::vector<std::uint8_t> index_map_t;

image_t reindex_image(image_t image, const index_map_t& map) {
    for (auto& entry : image)
        entry = map[entry];

    const auto&   src_table(image.color_table());
    color_table_t dst_table(src_table);
    std::size_t   count(image.color_table().size());

    for (std::size_t i(0); i < count; ++i)
        dst_table[map[i]] = src_table[i];

    image.set_color_table(dst_table);

    return image;
}

image_t reindex_image(const image_t& image, const indexed_histogram_table_t& table) {
    std::size_t count(image.color_table().size());
    index_map_t sorted_map(count);

    for (std::size_t i(0); i < count; ++i) {
        sorted_map[table[i].second] = i;
    }

    return reindex_image(image, sorted_map);
}

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

inline double grey(const rgba_t& c) {
    double r = c._r / 255.;
    double g = c._g / 255.;
    double b = c._b / 255.;
    double a = c._a / 255.;

    return 0.2126 * r + 0.7152 * g + 0.0722 * b * a;
}

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

typedef std::map<rgba_t, std::size_t> truecolor_histogram_t;

truecolor_histogram_t truecolor_histogram(const image_t& image) {
    truecolor_histogram_t result;
    auto                  bpp(image.bpp());
    auto                  p(image.begin());
    auto                  last(image.end());

    if (bpp == 3) {
        while (p != last) {
            rgba_t rgb{
                *p++, *p++, *p++, 255,
            };

            ++result[rgb];
        }
    } else if (bpp == 4) {
        while (p != last) {
            rgba_t rgb{
                *p++, *p++, *p++, *p++,
            };

            ++result[rgb];
        }
    }

    return result;
}

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

inline auto sq_distance(std::int64_t x, std::int64_t y) {
    std::int64_t diff(x - y);

    return diff * diff;
}

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

inline auto sq_distance(const rgba_t& x, const rgba_t& y) {
    return sq_distance(x._r, y._r) + sq_distance(x._g, y._g) + sq_distance(x._b, y._b) +
           sq_distance(x._a, y._a);
}

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

inline auto euclidean_distance(const rgba_t& x, const rgba_t& y) {
    return std::sqrt(sq_distance(x, y));
}

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

std::vector<std::int64_t> compute_sq_d(const std::vector<rgba_t>& colors,
                                       const std::vector<rgba_t>& seeds) {
    std::size_t               count(colors.size());
    std::vector<std::int64_t> values(count);

    tbb::parallel_for<std::size_t>(0,
                                   count,
                                   1,
                                   [& _colors = colors, &_seeds = seeds, &_values = values](
                                       std::size_t i) {
                                       const auto&  color = _colors[i];
                                       std::int64_t d(std::numeric_limits<std::int64_t>::max());

                                       for (const auto& seed : _seeds) {
                                           d = std::min(d, sq_distance(color, seed));

                                           // color is a seed; we're done here.
                                           if (d == 0)
                                               break;
                                       }

                                       _values[i] = d;
                                   });

    return values;
}

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

std::vector<rgba_t> k_means_pp(const std::vector<rgba_t>& v, std::size_t n) {
    if (v.empty() || v.size() <= n)
        return v;

    static std::random_device rd;
    static std::mt19937       gen(rd());

    std::uniform_int_distribution<> i_dist(0, v.size() - 1);
    std::vector<rgba_t>             result(1, v[i_dist(gen)]);

    while (result.size() < n) {
        auto                         d(compute_sq_d(v, result));
        std::discrete_distribution<> dist(d.begin(), d.end());
        std::size_t                  index(dist(gen));

        result.push_back(v[index]);
    }

    return result;
}

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

color_table_t make_grad_table() {
    // grayscale gradient color table from black to white. It might be better to
    // make this e.g., a green or yellow gradient instead, to ease visibility in
    // the dark range.
    std::size_t   count(PNG_MAX_PALETTE_LENGTH);
    color_table_t result(count);

    for (std::size_t i(0); i < count; ++i) {
        result[i]._r = i;
        result[i]._g = i;
        result[i]._b = i;
        result[i]._a = 255;
    }

    return result;
}

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

std::pair<std::size_t, double> quantize(const rgba_t& c, const color_table_t& table) {
    std::size_t  index{0};
    std::int64_t min_error{std::numeric_limits<std::int64_t>::max()};
    std::size_t  count(table.size());

    for (std::size_t i(0); i < count; ++i) {
        std::int64_t error(sq_distance(c, table[i]));

        if (error >= min_error)
            continue;

        index     = i;
        min_error = error;

        // exact match; no need to keep looking
        if (min_error == 0)
            break;
    }

    return std::make_pair(index, std::sqrt(min_error));
}

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

typedef std::pair<image_t, image_t> quantization_t;

quantization_t quantize(const image_t& image, color_table_t color_table) {
    image_t result(image.width(),
                   image.height(),
                   image.depth(),
                   image.width(),
                   PNG_COLOR_TYPE_PALETTE);
    image_t error_result(image.width(),
                         image.height(),
                         image.depth(),
                         image.width(),
                         PNG_COLOR_TYPE_PALETTE);
    auto dst(result.begin());
    auto err_dst(error_result.begin());
    auto area(image.area());

    tbb::parallel_for<decltype(
        area)>(0,
               area,
               1,
               [& _image = image, _dst = dst, _err_dst = err_dst, &_color_table = color_table](
                   auto i) {
                   auto q(quantize(_image.pixel<std::uint8_t>(i), _color_table));

                   _dst[i]     = q.first;
                   _err_dst[i] = static_cast<std::uint8_t>(
                       std::min<std::uint16_t>(std::lround(q.second), 255));
               });

    result.set_color_table(std::move(color_table));
    error_result.set_color_table(make_grad_table());

    return std::make_pair(std::move(result), std::move(error_result));
}

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

void palette_optimizations(const image_t& image, const path_t& output) {
    if (image.color_type() != PNG_COLOR_TYPE_PALETTE)
        return;

    indexed_histogram_table_t hist_table(make_indexed_histogram_table(image));

    std::sort(hist_table.begin(), hist_table.end(), [& _image = image](auto& x, auto& y) {
        // first compare histogram counts (then the actual colors if the counts
        // are the same).
        if (x.first < y.first) {
            return true;
        } else if (y.first < x.first) {
            return false;
        }

        const auto& color_table(_image.color_table());
        const auto& x_color(color_table[x.second]);
        const auto& y_color(color_table[y.second]);

#if 1
        return grey(x_color) < grey(y_color);
#else
        return x_color < y_color;
#endif
    });

    dump_image(reindex_image(image, hist_table),
               derived_filename(output, "sorted"),
               save_mode::max).get();

    std::reverse(hist_table.begin(), hist_table.end());

    dump_image(reindex_image(image, hist_table),
               derived_filename(output, "sorted_reverse"),
               save_mode::max).get();
}

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

void dump_quantization(const image_t& image, const image_t& error_image, const path_t& output) {
    dump_image(image, output, save_mode::one);
    save_png(error_image, associated_filename(output, "error"), save_options_t());
}

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

inline void dump_quantization(const quantization_t& q, const path_t& output) {
    dump_quantization(q.first, q.second, output);
}

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

struct centroid_cache_t {
    std::vector<rgba64_t>      _colors; // cumulative region color
    std::vector<std::uint64_t> _count;  // number of region members

    centroid_cache_t() = default;

    explicit centroid_cache_t(std::size_t count) : _colors(count), _count(count) {}

    std::size_t size() const {
        return _colors.size();
    }

    rgba64_t& centroid(std::size_t index) {
        return _colors[index];
    }
    const rgba64_t& centroid(std::size_t index) const {
        return _colors[index];
    }

    std::uint64_t& count(std::size_t index) {
        return _count[index];
    }
    std::uint64_t count(std::size_t index) const {
        return _count[index];
    }

    void add_member(std::size_t index, const rgba64_t& color) {
        ++count(index);

        centroid(index) += color;
    }

    void remove_member(std::size_t index, const rgba64_t& color) {
        --count(index);

        centroid(index) -= color;
    }

    void move_member(std::size_t src_index, std::size_t dst_index, const rgba64_t& color) {
        if (src_index == dst_index)
            return;

        remove_member(src_index, color);
        add_member(dst_index, color);
    }

    rgba_t color(std::size_t index) const {
        rgba_t result{0, 0, 0, 255};
        double c(count(index));

        if (c) {
            result = shorten<rgba_t>(centroid(index) / c);
        }

        return result;
    }

    color_table_t table() const {
        auto          count(size());
        color_table_t result;

        for (std::size_t i(0); i < count; ++i)
            result.push_back(color(i));

        return result;
    }
};

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

struct round_state_t {
    round_state_t() = default;

    explicit round_state_t(std::size_t count) : _centroids(count) {}

    auto size() const {
        return _centroids.size();
    }

    color_table_t centroid_table() const {
        return _centroids.table();
    }

    std::uint64_t error() const {
        return std::accumulate(_image_error.begin(), _image_error.end(), std::uint64_t(0));
    }

    std::size_t      _r{0};        // iteration count
    image_t          _image;       // original image quantized with current color table
    image_t          _image_error; // rounded per-pixel quantization error
    centroid_cache_t _centroids;   // cache of cumulative centroid values
};

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

void dump_round(const round_state_t& round, const path_t& output) {
    auto error(round.error());
    auto epp(static_cast<double>(error) / round._image.area());

    std::cout << "r" << round._r << " error: " << round.error() << " (" << epp << ")\n";

    dump_quantization(round._image, // image contains this round's color table
                      round._image_error,
                      derived_filename(output, "_r" + std::to_string(round._r)));
}

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

round_state_t k_means_init_state(const image_t& original, color_table_t seed) {
    round_state_t result(seed.size());

    std::tie(result._image, result._image_error) = quantize(original, std::move(seed));

    auto bpp(original.bpp());
    auto p_index(result._image.begin());
    auto p(original.begin());
    auto last(original.end());

    while (p != last) {
        std::size_t index(*p_index++);
        rgba64_t color{p[0], p[1], p[2], static_cast<rgba64_t::value_type>(bpp == 4 ? p[3] : 255)};

        result._centroids.add_member(index, color);

        p += bpp;
    }

    return result;
}

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

round_state_t k_means_round(const image_t& original,
                            const image_t& prev_image,
                            round_state_t  state) {
    ++state._r;

    // requantize the original image with the updated centroid color table
    std::tie(state._image, state._image_error) = quantize(original, state.centroid_table());

    auto bpp(original.bpp());
    auto p_prior_index(prev_image.begin());
    auto p_index(state._image.begin());
    auto p(original.begin());
    auto last(original.end());

    while (p != last) {
        std::size_t prior_index(*p_prior_index++);
        std::size_t index(*p_index++);

        if (prior_index == index) {
            p += bpp;
            continue;
        }

        rgba64_t color{*p++, *p++, *p++, std::uint64_t(bpp == 4 ? *p++ : 255)};

        state._centroids.move_member(prior_index, index, color);
    }

    return state;
}

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

color_table_t k_means(const image_t& image, color_table_t color_table, const path_t& output) {
    round_state_t round_state(k_means_init_state(image, std::move(color_table)));
    std::uint64_t best_error(std::numeric_limits<std::uint64_t>::max());
    color_table_t best_table;
    image_t       prev_image;

    while (true) {
        dump_round(round_state, output);

        std::uint64_t error(round_state.error());

        if (error < best_error) {
            best_table = round_state._image.color_table(); // copy
            best_error = error;

            // exact quantization found
            if (best_error == 0)
                break;
        }

        if (prev_image == round_state._image)
            break;

        prev_image = std::move(round_state._image);

        round_state._image_error = image_t();

        round_state = k_means_round(image, prev_image, std::move(round_state));
    };

    return best_table;
}

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

void k_means_quantization(const image_t& image, const path_t& output) {
    truecolor_histogram_t histogram(truecolor_histogram(image));
    std::vector<rgba_t>   colors;

    for (const auto& color : histogram)
        colors.push_back(color.first);

    //auto tests = {2, 4, 8, 16, 32, 64, 128, 256};
    auto tests = {256};

    for (const auto& table_size : tests) {
        std::vector<rgba_t> seed_table(k_means_pp(colors, table_size));

        dump_quantization(quantize(image, seed_table),
                          derived_filename(output, std::to_string(table_size) + "_seed"));

        color_table_t km_table(k_means(image, seed_table, output));
        auto          km(quantize(image, km_table));

        dump_quantization(km, derived_filename(output, std::to_string(table_size) + "_km"));

        palette_optimizations(km.first, output);
    }
}

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

void truecolor_optimizations(const image_t& image, const path_t& output) {
    if (image.color_type() == PNG_COLOR_TYPE_PALETTE)
        return;

#if 0
    k_means_quantization(image, output);
#else
    k_means_quantization(premultiply(image), output);
#endif
}

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

int main(int argc, char** argv) try {
    if (argc <= 1)
        throw std::runtime_error("Source file not specified");

    if (argc <= 2)
        throw std::runtime_error("Destination directory not specified");

    path_t        input(canonical(argv[1]));
    path_t        output(argv[2]);
    const image_t original(read_png(input.string()));

    // make the output directory fresh
    remove_all(output);
    create_directory(output);

    output = canonical(output) / input.leaf();

    dump_image(original, output, save_mode::max);

    truecolor_optimizations(original, output);

    palette_optimizations(original, output);

    return 0;
} catch (const std::exception& error) {
    std::cerr << "Fatal error: " << error.what() << '\n';
    return 0;
} catch (...) {
    std::cerr << "Fatal error: unknown\n";
    return 0;
}
/**************************************************************************************************/


#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsBinary3Ph2Sl.h"
#include "InterpolationType.h"

#include "catch.hpp"

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

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

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD binary three phase, two sublattice KKS",
    "[binary three phase, two sublattice kks]")
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 820.;

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

    pt::ptree newton_db;
    newton_db.put("alpha", 0.5);
    newton_db.put("max_its", 100);

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary3Ph2Sl cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // initial guesses
    double c_init0 = 0.8;
    double c_init1 = 0.6;
    double c_init2 = 0.67;

    double sol[3] = { c_init0, c_init1, c_init2 };

    // compute concentrations satisfying KKS equations
    // I think the system at 820K is best behaved in the conc = 0.67-0.8 range
    double conc = 0.7;

    // This phi doesn't quite give Sum(h(phi)) = 1, but it is close
    double phi[3] = { 0.3, 0.45, 0.5 };

    cafe.computePhaseConcentrations(temperature, &conc, phi, sol);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;
    std::cout << "Result for c = " << conc << " and phi = " << phi[0] << " "
              << phi[1] << " " << phi[2] << std::endl;
    std::cout << "   cL = " << sol[0] << std::endl;
    std::cout << "   cA = " << sol[1] << std::endl;
    std::cout << "   cB = " << sol[2] << std::endl;

    const Thermo4PFM::PhaseIndex pi0 = Thermo4PFM::PhaseIndex::phaseL;
    const Thermo4PFM::PhaseIndex pi1 = Thermo4PFM::PhaseIndex::phaseA;
    const Thermo4PFM::PhaseIndex pi2 = Thermo4PFM::PhaseIndex::phaseB;

    std::cout << "Verification:" << std::endl;

    double derivL;
    cafe.computeDerivFreeEnergy(temperature, &sol[0], pi0, &derivL);
    std::cout << "   dfL/dcL = " << derivL << std::endl;

    double derivS1;
    cafe.computeDerivFreeEnergy(temperature, &sol[1], pi1, &derivS1);
    std::cout << "   dfS1/dcS1 = " << derivS1 << std::endl;

    REQUIRE(derivS1 == Approx(derivL).margin(1.e-5));

    double derivS2;
    cafe.computeDerivFreeEnergy(temperature, &sol[2], pi2, &derivS2);
    std::cout << "   dfS2/dcS2 = " << derivS2 << std::endl;

    REQUIRE(derivS2 == Approx(derivL).margin(1.e-5));
}

TEST_CASE("CALPHAD binary three phase, two sublattice KKS #2",
    "[binary three phase, two sublattice kks #2]")
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 820.;

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

    pt::ptree newton_db;
    newton_db.put("alpha", 1.0);
    newton_db.put("max_its", 20000);

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary3Ph2Sl cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // initial guesses
    double c_init0 = 0.79267;
    double c_init1 = 0.79267;
    double c_init2 = 0.79267;

    double sol[3] = { c_init0, c_init1, c_init2 };

    // compute concentrations satisfying KKS equations
    // I think the system at 820K is best behaved in the conc = 0.67-0.8 range
    double conc = 0.79267;

    // This phi doesn't quite give Sum(h(phi)) = 1, but it is close
    double phi[3] = { 0.938956, 5.88418e-15, 0.0610444 };

    cafe.computePhaseConcentrations(temperature, &conc, phi, sol);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;
    std::cout << "Result for c = " << conc << " and phi = " << phi[0] << " "
              << phi[1] << " " << phi[2] << std::endl;
    std::cout << "   cL = " << sol[0] << std::endl;
    std::cout << "   cA = " << sol[1] << std::endl;
    std::cout << "   cB = " << sol[2] << std::endl;

    const Thermo4PFM::PhaseIndex pi0 = Thermo4PFM::PhaseIndex::phaseL;
    const Thermo4PFM::PhaseIndex pi1 = Thermo4PFM::PhaseIndex::phaseA;
    const Thermo4PFM::PhaseIndex pi2 = Thermo4PFM::PhaseIndex::phaseB;

    std::cout << "Verification:" << std::endl;

    double derivL;
    cafe.computeDerivFreeEnergy(temperature, &sol[0], pi0, &derivL);
    std::cout << "   dfL/dcL = " << derivL << std::endl;

    double derivS1;
    cafe.computeDerivFreeEnergy(temperature, &sol[1], pi1, &derivS1);
    std::cout << "   dfS1/dcS1 = " << derivS1 << std::endl;

    // REQUIRE(derivS1 == Approx(derivL).margin(1.e-5));

    double derivS2;
    cafe.computeDerivFreeEnergy(temperature, &sol[2], pi2, &derivS2);
    std::cout << "   dfS2/dcS2 = " << derivS2 << std::endl;

    REQUIRE(derivS2 == Approx(derivL).margin(1.e-5));
}

TEST_CASE("CALPHAD binary three phase, two sublattice KKS #3",
    "[binary three phase, two sublattice kks #3]")
{
    // This case may require a restart of the solver with new initial conditions
    // to pass.

    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 820.;

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

    pt::ptree newton_db;
    newton_db.put("alpha", 1.0);
    newton_db.put("max_its", 30);

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary3Ph2Sl cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // initial guesses
    double c_init0 = 0.800000011921;
    double c_init1 = 0.800000011921;
    double c_init2 = 0.800000011921;

    double sol[3] = { c_init0, c_init1, c_init2 };

    // compute concentrations satisfying KKS equations
    // I think the system at 820K is best behaved in the conc = 0.67-0.8 range
    double conc = 0.800000011921;

    // Want phi that corresponds to hphi = [0.982551, 0.0174491, 4.79499e-25]
    // Want phi that corresponds to hphi = [1.0, 6.18271e-09, 3.69779e-32]
    // double phi[3] = { 0.998, 8.5e-4, 1.5e-11 };
    double phi[3] = { 0.998, 8.5e-4, 1.5e-11 };

    cafe.computePhaseConcentrations(temperature, &conc, phi, sol);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;
    std::cout << "Result for c = " << conc << " and phi = " << phi[0] << " "
              << phi[1] << " " << phi[2] << std::endl;
    std::cout << "   cL = " << sol[0] << std::endl;
    std::cout << "   cA = " << sol[1] << std::endl;
    std::cout << "   cB = " << sol[2] << std::endl;

    const Thermo4PFM::PhaseIndex pi0 = Thermo4PFM::PhaseIndex::phaseL;
    const Thermo4PFM::PhaseIndex pi1 = Thermo4PFM::PhaseIndex::phaseA;
    const Thermo4PFM::PhaseIndex pi2 = Thermo4PFM::PhaseIndex::phaseB;

    std::cout << "Verification:" << std::endl;

    double derivL;
    cafe.computeDerivFreeEnergy(temperature, &sol[0], pi0, &derivL);
    std::cout << "   dfL/dcL = " << derivL << std::endl;

    double derivS1;
    cafe.computeDerivFreeEnergy(temperature, &sol[1], pi1, &derivS1);
    std::cout << "   dfS1/dcS1 = " << derivS1 << std::endl;

    REQUIRE(derivS1 == Approx(derivL).margin(1.e-5));

    double derivS2;
    cafe.computeDerivFreeEnergy(temperature, &sol[2], pi2, &derivS2);
    std::cout << "   dfS2/dcS2 = " << derivS2 << std::endl;

    REQUIRE(derivS2 == Approx(derivL).margin(1.e-5));
}


#ifndef OTHELLO_AI_ALPHABETAPRUNINGPLAYER_HPP
#define OTHELLO_AI_ALPHABETAPRUNINGPLAYER_HPP

//Boost headers:
#include <boost/random.hpp>
//Othello headers:
#include <othello/game/IPlayer.hpp>
#include <othello/game/Game.hpp>
#include <othello/util/WorkerThreadManager.hpp>

namespace othello
{

    namespace ai
    {

        ////////////////////////////////////////////////////////////////
        /// \class AlphaBetaPruningPlayer
        ///
        /// \brief An AI player that plays moves using the minimax and
        ///        alpha-beta pruning algorithms
        ///
        ////////////////////////////////////////////////////////////////
        class AlphaBetaPruningPlayer : public game::IPlayer
        {
            private:

                ////////////////////////////////////////////////////////////////
                /// \brief The search depth in number of moves ahead
                ///
                ////////////////////////////////////////////////////////////////
                unsigned int searchDepth;

                ////////////////////////////////////////////////////////////////
                /// \brief A mersenne_twister_engine for generating random
                ///        numbers
                ///
                ////////////////////////////////////////////////////////////////
                boost::random::mt19937 randomNumberGenerator;

                ////////////////////////////////////////////////////////////////
                /// \brief The alpha-beta recursive algorithm
                ///
                ////////////////////////////////////////////////////////////////
                static int64_t alphaBeta(const game::Board& board, const uint8_t& player, const game::Move& move,
                        uint8_t depth, int64_t alpha, int64_t beta);

                ////////////////////////////////////////////////////////////////
                /// \brief The worker thread manager
                ///
                ////////////////////////////////////////////////////////////////
                util::WorkerThreadManager<int64_t, const game::Board&, const uint8_t&,
                        const game::Move&, uint8_t, int64_t, int64_t> workerManager;

            public:

                ////////////////////////////////////////////////////////////////
                /// \brief Class constructor that initialises the members
                ///
                /// \param game A const reference to the game
                /// \param player The player number. 0 = player1, 1 = player2
                /// \param searchDepth The search depth of the AI
                /// \param numThreads The number of worker threads
                /// \param seed The seed for the random generator
                ///
                ////////////////////////////////////////////////////////////////
                AlphaBetaPruningPlayer(const unsigned int& searchDepth, const uint8_t& numThreads, const unsigned int& seed)
                    : searchDepth(searchDepth), randomNumberGenerator(seed),
                        workerManager(&AlphaBetaPruningPlayer::alphaBeta, numThreads) {}

                ////////////////////////////////////////////////////////////////
                /// \brief Function that is called when the player should make a
                ///        move. This function uses the negamax and alpha-beta
                ///        pruning algorithms to make a decision
                ///
                /// \param game A const reference to the game to make a move in
                /// \param player The index of this player in the game. (0 is
                ///        player 1, 1 is player 2)
                /// \param possibleMoves A vector of the possible moves
                ///
                /// \return A const pointer to a const move that will be played
                ///         by the current player. This pointer must point to a
                ///         move in possibleMoves
                ///
                ////////////////////////////////////////////////////////////////
                const game::Move* makeMove(const game::Game& game, const uint8_t& player,
                        const std::vector<game::Move>& possibleMoves) override;

        };

    }

}

#endif //OTHELLO_AI_ALPHABETAPRUNINGPLAYER_HPP


#include <assert.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <algorithm>

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

#include "gcm.h"

using namespace std;
using namespace NTL;

#define NELEMS(arr) ((sizeof arr) / (sizeof arr[0]))

void initfield(void)
{
    GF2X gf2e128mod;

    // builds the right poly for GMAC's GF(2^128) modulus
    BuildIrred(gf2e128mod, 128);
    GF2E::init(gf2e128mod);
}

void asserthex(const char *s)
{
    size_t i, len;

    len = strlen(s);
    assert(len % 2 == 0);
    for (i = 0; i < len; i += 1) {
        char c;

        c = s[i];
        assert((c >= '0' && c <= '9') ||
               (c >= 'a' && c <= 'f') ||
               (c >= 'A' && c <= 'F'));
    }
}

void hex2bytes(struct slice *s, const char *hex)
{
    size_t i;
    unsigned tmp;

    asserthex(hex);

    s->len = strlen(hex) / 2;
    s->data = (uint8_t *)malloc(s->len);

    for (i = 0; i < s->len; i += 1) {
        sscanf(hex + (2*i), "%02x", &tmp);
        s->data[i] = tmp;
    }
}

void block2felem(GF2E &x, const struct slice *block)
{
    GF2X p;
    size_t i, j;

    for (i = 0; i < block->len; i += 1) {
        uint8_t b = block->data[i];
        for (j = 0; j < 8; j += 1) {
            SetCoeff(p, 8*i + j, b >> (7-j));
        }
    }

    conv(x, p);
}

void packuint64(uint8_t *buf, uint64_t x)
{
    size_t i;

    for (i = 0; i < 8; i += 1) {
        buf[i] = x >> (8*(7-i));
    }
}

void buildpoly(GF2EX &p, const struct slice *a,
               const struct slice *c, const struct slice *t)
{
    GF2E x;
    const struct slice *sp[] = {a, c};
    struct slice s, block;
    size_t i, j;
    uint8_t lenblock[blocklen];

    i = 0;
    for (j = 0; j < NELEMS(sp); j += 1) {
        s = *sp[j];
        while (s.len > 0) {
            block.data = s.data;
            block.len = min(blocklen, s.len);

            block2felem(x, &block);
            SetCoeff(p, i, x);

            i += 1;
            s.data += block.len;
            s.len -= block.len;
        }
    }

    block.data = &lenblock[0];
    block.len = blocklen;
    packuint64(block.data, a->len << 3);
    packuint64(block.data + 8, c->len << 3);

    block2felem(x, &block);
    SetCoeff(p, i, x);

    i += 1;

    assert(t->len == blocklen);

    block2felem(x, t);
    SetCoeff(p, i, x);

    reverse(p, p);
}

void felem2hex(char *out, const GF2E &x)
{
    GF2X p;
    size_t i;
    uint b;
    const char *hex = "0123456789abcdef";

    p = rep(x);

    for (i = 0; i < 32; i += 1) {
        size_t j;

        j = 4 * i;
        b = ((conv<uint>(coeff(p, j+0)) << 3) |
             (conv<uint>(coeff(p, j+1)) << 2) |
             (conv<uint>(coeff(p, j+2)) << 1) |
             (conv<uint>(coeff(p, j+3))));
        out[i] = hex[b];
    }

    out[32] = '\0';
}


#include <boost/cstdint.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <nt2/sdk/aligned/is_power_of_2.hpp>

using nt2::meta::is_power_of_2_c;

int main()
{
  BOOST_MPL_ASSERT(( is_power_of_2_c<2>::type ));
  BOOST_MPL_ASSERT(( is_power_of_2_c<4>::type ));
  BOOST_MPL_ASSERT(( is_power_of_2_c<8>::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2_c<0>::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2_c<10>::type ));
}


#ifndef CONCEPTS_DIMENSIONS_MONOID_HPP_INCLUDED
#define CONCEPTS_DIMENSIONS_MONOID_HPP_INCLUDED

#include <limits>

#include <boost/hana/assert.hpp>
#include <boost/hana/zero.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/less.hpp>

#include <gem/fwd/dimensions.hpp>

namespace boost::hana {

template<typename T>
static constexpr
auto safe_add(const T & v1, const T & v2)
{
    return v1 < std::numeric_limits<T>::max() - v2 ?
        v1 + v2 : std::numeric_limits<T>::max();
}

gem::concepts::detail::Dimension {T, cv, max, min}
struct zero_impl<gem::Dimension<T, cv, max, min>>
{
    static constexpr auto apply(void) -> gem::Dimension<T, 0, 0, 0>
    {
        return {};
    }
};

gem::concepts::detail::DimensionPair {T1, cv1, max1, min1, T2, cv2, max2, min2}
struct plus_impl<gem::Dimension<T1, cv1, max1, min1>,
                 gem::Dimension<T2, cv2, max2, min2>>
{
private:
    using ctype = typename boost::hana::common<T1, T2>::type;

public:
    static constexpr auto
    apply(const gem::Dimension<T1, cv1, max1, min1>& d1,
          const gem::Dimension<T2, cv2, max2, min2>& d2)
    {
        BOOST_HANA_RUNTIME_CHECK_MSG(d1.value() <
                                     std::numeric_limits<ctype>::max() -
                                     d2.value(),
                                     "Dimension overflow...");
        return gem::Dimension<ctype, safe_add<ctype>(cv1, cv2),
                                     safe_add<ctype>(max1, max2),
                                     safe_add<ctype>(min1, min2)> {d1.value() +
                                                                   d2.value()};
    }
};

template<typename T1, T1 cv1, typename T2, T2 cv2>
struct plus_impl<gem::Dimension<T1, cv1, cv1, cv1>,
                 gem::Dimension<T2, cv2, cv2, cv2>>
{
private:
    using ctype = typename boost::hana::common<T1, T2>::type;

public:
    static constexpr auto
    apply(const gem::Dimension<T1, cv1, cv1, cv1> &,
          const gem::Dimension<T2, cv2, cv2, cv2> &)
    {
        constexpr auto c = integral_c<ctype, cv1>;
        constexpr auto m = integral_c<ctype,
                                      std::numeric_limits<ctype>::max() - cv2>;
        BOOST_HANA_CONSTANT_CHECK_MSG(c < m, "Dimension overflow...");
        constexpr ctype s = static_cast<ctype>(cv1) +
                            static_cast<ctype>(cv2);
        return gem::Dimension<ctype, s, s, s> {};
    }
};

}  // namespace boost::hana

namespace gem {

template <typename T1, T1 cv1, T1 cv_max1, T1 cv_min1,
          typename T2, T2 cv2, T2 cv_max2, T2 cv_min2>
constexpr inline auto
operator+(const gem::Dimension<T1, cv1, cv_max1, cv_min1> & d1,
          const gem::Dimension<T2, cv2, cv_max2, cv_min2> & d2)
{
    return boost::hana::plus(d1, d2);
}

}  // namespace gem

#endif  // !CONCEPTS_DIMENSIONS_MONOID_HPP_INCLUDED


#include <stan/math/prim.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <gtest/gtest.h>
#include <cmath>
#include <limits>

TEST(MathFunctions, digamma) {
  EXPECT_FLOAT_EQ(boost::math::digamma(0.5), stan::math::digamma(0.5));
  EXPECT_FLOAT_EQ(boost::math::digamma(-1.5), stan::math::digamma(-1.5));
}

TEST(MathFunctions, digamma_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_TRUE(std::isnan(stan::math::digamma(nan)));

  EXPECT_TRUE(std::isnan(stan::math::digamma(-1)));

  EXPECT_TRUE(std::isnormal(stan::math::digamma(1.0E50)));
}

TEST(MathFunctions, digamma_works_with_other_functions) {
  Eigen::VectorXd a(5);
  a << 1.1, 1.2, 1.3, 1.4, 1.5;
  Eigen::RowVectorXd b(5);
  b << 1.1, 1.2, 1.3, 1.4, 1.5;
  stan::math::multiply(a, stan::math::digamma(b));
}


/***
 * @Author: JoeyforJoy
 * @Date: 2021-05-06 21:55:55
 * @LastEditTime: 2021-10-23 00:13:45
 * @LastEditors: JoeyforJoy
 * @Description: A simple KD Tree implementation
 * @Usage:
 *      Eigen::MatrixXd cloud_eigen;
 *      cloud_eigen = ... ; // assign point cloud for cloud_eigen
 *      int leaf_size = 5; // the minimum number of points in the leaf nodes
 *      KDTree kdtree(cloud_eigen, leaf_size); // build kdtree
 *      // knn search
 *      std::vector<int> pts_idx; // indices of result points
 *      std::vector<double> pts_dist; // distance of result points
 *      int k = 5;
 *      kdtree.knnSearch(point, k, pts_idx, pts_dist);
 *      // radius search
 *      int radius = 5;
 *      kdtree.radiusSearch(point, radius, pts_idx, pts_dist);
 */

#ifndef KDTREE_HPP
#define KDTREE_HPP
#include <memory>
#include <numeric>
#include <Eigen/Core>

#include "resultSet.hpp"

namespace NNSearch {
    struct KDNode {
        typedef std::shared_ptr<KDNode> Ptr;
        public:
            int dim; // 0 for X, 1 for Y, 2 for X
            double value;
            KDNode::Ptr left;
            KDNode::Ptr right;
            bool isLeaf;
            std::vector<int> points_idx;
        public:
            KDNode(): dim(0), value(0), isLeaf(false), left(nullptr), right(nullptr){}
            KDNode(int dim_): dim(dim_), value(0), left(nullptr), right(nullptr), isLeaf(false) {}
    };

    class KDTree {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        public:
            KDTree(): _root(nullptr), _leaf_size(0) {}
            // copy constructor
            KDTree(const KDTree& tree): _root(tree.getRoot()), _leaf_size(tree._leaf_size) {}
            // copy assign
            KDTree& operator=(const KDTree& tree) {_root = tree.getRoot(); _leaf_size = tree._leaf_size;}
            // build KDTree
            KDTree(Eigen::MatrixXd &points, int leaf_size): _points(points), _leaf_size(leaf_size) {
                std::vector<int> pts_idx(points.rows(), 0);
                std::iota(pts_idx.begin(), pts_idx.end(), 0);
                _root = _buildKDTree(pts_idx, 0, pts_idx.size(), 0);
            }

            ~KDTree() {}

            KDNode::Ptr getRoot() const {return _root;}
            int getLeafSize() const {return _leaf_size;}

            // knn search
            void knnSearch(const Eigen::Vector3d &point, int k, std::vector<int> &pts_idx, std::vector<double> &pts_dist) {
                // KNN search
                KNNResultSet result_set(k);
                _SearchIdxDist(_root, point, result_set);
                // unpack result
                result_set.unpackResultSet(pts_idx, pts_dist);
            }

            // radius search
            void radiusSearch(const Eigen::Vector3d &point, double radius, std::vector<int> &pts_idx, std::vector<double> &pts_dist) {
                // radius search
                RadiusResultSet result_set(radius);
                _SearchIdxDist(_root, point, result_set);
                // unpack result
                result_set.unpackResultSet(pts_idx, pts_dist);
            }

        private:
            void _SearchIdxDist(KDNode::Ptr root, const Eigen::Vector3d &point, AbstractResultSet &result_set) {
                if (!root) return;
                if (root->isLeaf) {
                    // process leaf
                    for (int idx: root->points_idx) {
                        double dist = (_points.row(idx) - point.transpose()).norm();
                        result_set.addOnePoint(idx, dist);
                    }
                } else {
                    if (point(root->dim) <= root->value) { // If the value is less than the root, search the left.
                        _SearchIdxDist(root->left, point, result_set);
                        // If the point is too far, don't search the right tree.
                        if (fabs(point(root->dim) - root->value) <= result_set.worst_dist()) {
                            _SearchIdxDist(root->right, point, result_set);
                        }
                    } else {
                        _SearchIdxDist(root->right, point, result_set);
                        // If the point is too far, don't search the left tree.
                        if (fabs(point(root->dim) - root->value) <= result_set.worst_dist()) {
                            _SearchIdxDist(root->left, point, result_set);
                        }
                    }
                }
            }

            // 构造 KD 树
            KDNode::Ptr _buildKDTree(std::vector<int> &pts_idx,
                                    int left, int right, int dim) {
                if (right - left <= 0) return nullptr;
                KDNode::Ptr root(new KDNode(dim));
                if (right - left <= _leaf_size ) {
                    root->isLeaf = true;
                    root->points_idx.assign(pts_idx.begin()+left, pts_idx.begin()+right);
                } else {
                    // current splitting axis
                    auto min_max_x = std::minmax_element(
                        pts_idx.begin() + left,
                        pts_idx.begin() + right,
                        [&](int lhs, int rhs) { return _points(lhs, 0) < _points(rhs, 0); });
                    auto min_max_y = std::minmax_element(
                        pts_idx.begin() + left,
                        pts_idx.begin() + right,
                        [&](int lhs, int rhs) { return _points(lhs, 1) < _points(rhs, 1); });
                    auto min_max_z = std::minmax_element(
                        pts_idx.begin() + left,
                        pts_idx.begin() + right,
                        [&](int lhs, int rhs) { return _points(lhs, 2) < _points(rhs, 2); });
                    auto dx = _points((*min_max_x.second),0) - _points((*min_max_x.first),0);
                    auto dy = _points((*min_max_x.second),1) - _points((*min_max_x.first),1);
                    auto dz = _points((*min_max_x.second),2) - _points((*min_max_x.first),2);
                    dim = (dx > dy ? (dx > dz ? 0 : 2) : (dy > dz ? 1 : 2));

                    // find middle o(NlogN)
                    std::nth_element (
                        pts_idx.begin() + left,
                        pts_idx.begin() + (right - left) / 2 + left,
                        pts_idx.begin() + right,
                        [&]
                        (int lhs, int rhs)
                        { return _points(lhs, dim) < _points(rhs, dim);});

                    // find the middle and split
                    int mid = (right - left) / 2 + left;

                    root->value = (_points(pts_idx[mid], dim) + _points(pts_idx[mid+1], dim) + 1e-3) /2 ;
                    root->left = _buildKDTree(pts_idx, left, mid, dim);
                    root->right = _buildKDTree(pts_idx, mid, right, dim);
                }
                return root;
            }

        private:
            Eigen::MatrixXd _points;
            KDNode::Ptr _root;
            int _leaf_size;
    };
}
#endif


/**
 *  @copyright Copyright 2018 The J-PET Framework Authors. All rights reserved.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may find a copy of the License in the LICENCE file.
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *  @file HelperMathFunctionsTest.h
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE HelperMathFunctionsTest

#include "JPetSimplePhysSignalReco/HelperMathFunctions.h"
#include <boost/test/unit_test.hpp>

using namespace boost::numeric::ublas;

BOOST_AUTO_TEST_SUITE(FirstSuite)

BOOST_AUTO_TEST_CASE(polynomialFitTest1)
{
  vector<float> time(4);
  vector<float> volt(4);
  int alfa = 1;
  float v0 = -0.10;
  time(0) = 1035.0;
  time(1) = 1542.0;
  time(2) = 2282.0;
  time(3) = 2900.0;
  volt(0) = -0.06;
  volt(1) = -0.20;
  volt(2) = -0.35;
  volt(3) = -0.50;
  float result = polynomialFit(time, volt, alfa, v0);
  float epsilon = 0.1;
  BOOST_REQUIRE_CLOSE(result, 1171.98, epsilon);
}

BOOST_AUTO_TEST_CASE(polynomialFitTest2)
{
  vector<float> time(4);
  vector<float> volt(4);
  int alfa = 2;
  float v0 = -0.05;
  time(0) = 1035.0;
  time(1) = 1542.0;
  time(2) = 2282.0;
  time(3) = 2900.0;
  volt(0) = -0.06;
  volt(1) = -0.20;
  volt(2) = -0.35;
  volt(3) = -0.50;
  float result = polynomialFit(time, volt, alfa, v0);
  float epsilon = 0.1;
  BOOST_REQUIRE_CLOSE(result, 793.1, epsilon);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <queue>
#include <vector>
#include <chrono>
#include <boost/graph/graphviz.hpp>
#include "boostHelper/squareboost.h"
#include "leda/ledaRandomBiMaxFlow.h"
using namespace boost;
struct NodeInfo;
struct EdgeInfo;
typedef boost::adjacency_list<vecS, vecS, bidirectionalS, NodeInfo, EdgeInfo> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor vertex_d;
typedef boost::graph_traits<Graph>::edge_descriptor edge_d;
vertex_d s, t;
Graph G_random;

std::queue<vertex_d> overflowQ;

// vertex_d A = add_vertex(G_random);
// vertex_d B = add_vertex(G_random);
// vertex_d C = add_vertex(G_random);
// vertex_d D = add_vertex(G_random);
// vertex_d E = add_vertex(G_random);
// vertex_d F = add_vertex(G_random);

// std::pair<edge_d, bool> AB = add_boost::edge(A, B, G_random);
// std::pair<edge_d, bool> AC = add_boost::edge(A, C, G_random);
// std::pair<edge_d, bool> BD = add_boost::edge(B, D, G_random);
// std::pair<edge_d, bool> CE = add_boost::edge(C, E, G_random);
// std::pair<edge_d, bool> DC = add_boost::edge(D, C, G_random);
// std::pair<edge_d, bool> EF = add_boost::edge(E, F, G_random);

// std::pair<edge_d, bool> BA = add_boost::edge(B, A, G_random);
// std::pair<edge_d, bool> CA = add_boost::edge(C, A, G_random);
// std::pair<edge_d, bool> DB = add_boost::edge(D, B, G_random);
// std::pair<edge_d, bool> EC = add_boost::edge(E, C, G_random);
// std::pair<edge_d, bool> CD = add_boost::edge(C, D, G_random);
// std::pair<edge_d, bool> FE = add_boost::edge(F, E, G_random);

int main()
{
	int n1_length = 0;
	bool logging;
	int a_vertices = 0, b_vertices = 0, m_edges = 0;
	std::cout << "Num of A vertices?\n";
	std::cin >> a_vertices;
	std::cout << "Num of B vertices?\n";
	std::cin >> b_vertices;
	std::cout << "Logging (1/0)\n";
	std::cin >> logging;
	// std::cout << "Num of M edges?\n";
	// std::cin >> m_edges;
	ledaRandomBiMaxFlow(G_random, s, t, a_vertices, b_vertices, m_edges, n1_length); //generate_square_bipartite(8, s, t, n1_length);
	// std::cout << "Returned";
	boost::property_map<Graph, int EdgeInfo::*>::type residual = get(&EdgeInfo::residual_flow, G_random);
	boost::property_map<Graph, int NodeInfo::*>::type v_id = get(&NodeInfo::id, G_random);
	boost::property_map<Graph, int NodeInfo::*>::type height = get(&NodeInfo::height, G_random);
	boost::property_map<Graph, int NodeInfo::*>::type excess = get(&NodeInfo::excess, G_random);
	// write_graphviz(std::cout, G_random, make_label_writer(v_id), make_label_writer(residual));
	auto end = std::chrono::system_clock::now(); //declaring timers
	auto start = std::chrono::system_clock::now();
	auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
	start = std::chrono::system_clock::now();

	graph_traits<Graph>::vertex_iterator vi, vi_end;
	graph_traits<Graph>::edge_iterator ei, ei_end;
	// -----------------------------Initialization----------------------------------
	int nums = 0;
	for (tie(vi, vi_end) = vertices(G_random); vi != vi_end; vi++)
	{ //initialise heights and excess
		v_id[*vi] = nums;
		height[*vi] = 1000000;
		excess[*vi] = 0;
		nums++;
	}
	// std::cout << "S is: " << v_id[s] << " and t is: " << v_id[t] << "\n";

	// residual[AB.first] = 5;
	// residual[AC.first] = 4;
	// residual[BD.first] = 4;
	// residual[DC.first] = 2;
	// residual[CE.first] = 4;
	// residual[EF.first] = 4;
	// residual[BA.first] = 0;
	// residual[CA.first] = 0;
	// residual[DB.first] = 0;
	// residual[CD.first] = 0;
	// residual[EC.first] = 0;
	// residual[FE.first] = 0;
	// std::cout << "inited heights and excess\n";
	height[t] = 0;
	graph_traits<Graph>::out_edge_iterator edgei, edgei_end;
	graph_traits<Graph>::out_edge_iterator edgej, edgej_end;
	graph_traits<Graph>::in_edge_iterator bi, bi_end;

	for (tie(edgei, edgei_end) = out_edges(s, G_random); edgei != edgei_end; edgei++) // max flow on outgoing edges of S and update excess of S's neighbours, add residual edge
	{
		// std::cout << "from S to : " << target(*edgei, G_random) << "\n The edge is: " << residual[*edgei] << "\n";
		excess[target(*edgei, G_random)] = residual[*edgei];
		// std::cout << "updated excess of node\n";
		if (excess[target(*edgei, G_random)] != 0 && target(*edgei, G_random) != t)
			overflowQ.push(target(*edgei, G_random));
		// std::cout << "prob added q\n";
		// std::pair<edge_d, bool> resEdge = boost::edge(target(*edgei, G_random), s, G_random);
		// std::cout << "made edge to back\n";
		residual[boost::edge(target(*edgei, G_random), s, G_random).first] = residual[*edgei];
		// std::cout << "updated rev cap\n";
		residual[*edgei] = 0;
		// std::cout << "zeroed cap\n";
	}
	// std::cout << "inited S things\n";
	std::queue<vertex_d> q;
	q.push(t);
	vertex_d temp;

	while (!q.empty())
	{
		temp = q.front();
		q.pop();
		int alt = 0;
		for (tie(bi, bi_end) = in_edges(temp, G_random); bi != bi_end; bi++)
		{
			// std::cout << "Reading Edge: " << source(*bi, G_random) << " ---> " << target(*bi, G_random);
			alt = height[temp] + 1;
			if (alt < height[source(*bi, G_random)])
			{
				height[source(*bi, G_random)] = alt;
				q.push(source(*bi, G_random));
			}
		}
	}
	// std::cout << "n1_length: " << n1_length << "\n";
	height[s] = 2 * n1_length + 1;

	//-----------------------------------------------------------------------------------------
	//--------------------------Algorithm------------------------------------------------------
	bool admissableEdge1 = false;
	bool admissableEdge2 = false;
	bool elementExistsInQ = false;

	int j_minimum_height = 10000, i_minimum_height = 10000;
	int minimum_flow = 0;
	int times = 0;
	vertex_d k, j, i;
	std::pair<edge_d, bool> newEdge;
	while (!overflowQ.empty())
	{
		// std::cout << "The q has: " << overflowQ.size() << "elements \n";
		vertex_d overflower = overflowQ.front();
		overflowQ.pop();
		// std::cout << "The excess node is: " << overflower << "\n and it's excess is: " << excess[overflower] << "\n";
		admissableEdge1 = false;
		if (overflower == s || overflower == t || excess[overflower] == 0)
			continue;

		for (tie(edgei, edgei_end) = out_edges(overflower, G_random); edgei != edgei_end; edgei++) // max flow on outgoing edges of S and update excess of S's neighbours, add residual edge
		{
			j = target(*edgei, G_random);
			i = overflower;
			// std::cout << "the edge goes from " << i << " To " << j << "and has a residual of: " << residual[*edgei] << "\n";

			if (height[i] > height[j] && residual[*edgei] > 0)
			{

				admissableEdge1 = true;
				// std::cout << "admissable i-j: " << i << "-" << j << "\n";
				admissableEdge2 = false;
				for (tie(edgej, edgej_end) = out_edges(j, G_random); edgej != edgej_end; edgej++)
				{
					if (j == s)
					{
						// std::cout << "found only S to push";
						minimum_flow = std::min(excess[i], residual[*edgei]);
						residual[*edgei] -= minimum_flow;
						// std::cout << "MINFLOW: " << minimum_flow << "\n";
						residual[boost::edge(s, i, G_random).first] += minimum_flow;
						// std::cout << "Pushing from " << i << "to s, flow: " << minimum_flow << "\n";
						excess[i] -= minimum_flow;
						excess[s] += minimum_flow;
						admissableEdge2 = true;
						continue;
					}
					if (height[j] > height[target(*edgej, G_random)] && residual[*edgej] > 0)
					{

						k = target(*edgej, G_random);
						// std::cout << "admissable j-k :" << j << "-" << k << "\n";
						admissableEdge2 = true;
						minimum_flow = std::min({excess[i], residual[*edgei], residual[*edgej]});
						// std::cout << "minimum flow is: " << minimum_flow << "\n";
						excess[i] -= minimum_flow;
						residual[boost::edge(j, i, G_random).first] += minimum_flow;
						residual[*edgei] -= minimum_flow;

						residual[boost::edge(k, j, G_random).first] += minimum_flow;
						residual[boost::edge(j, k, G_random).first] -= minimum_flow;
						excess[k] += minimum_flow;
						if (k != t && k != s)
						{
							overflowQ.push(k);
						}

						// std::cout << "pushed: " << minimum_flow << "\n";
						// excess[j] = 0;
					}
				}
				if (!admissableEdge2)
				{
					// std::cout << "Relabel J\n";

					for (tie(edgej, edgej_end) = out_edges(j, G_random); edgej != edgej_end; edgej++)
					{
						// std::cout << "MinHJ: " << j_minimum_height << " and targetJ: " << height[target(*edgej, G_random)] << "\n";
						if (residual[*edgej] == 0)
							continue;
						if (j_minimum_height > height[target(*edgej, G_random)])
						{
							j_minimum_height = height[target(*edgej, G_random)];
						}
					}
					// std::cout << "The j minimum height is: " << j_minimum_height << " so j's height becomes: " << j_minimum_height + 1 << "\n";
					height[j] = j_minimum_height + 1;
					j_minimum_height = 10000;
				}
			}
		}
		if (!admissableEdge1)
		{
			// std::cout << "Relabel I, the overflower: " << overflower << "\n";
			for (tie(edgei, edgei_end) = out_edges(i, G_random); edgei != edgei_end; edgei++)
			{
				// std::cout << "MinH: " << i_minimum_height << " and target: " << height[target(*edgei, G_random)] << "\n";
				if (residual[*edgei] == 0)
					continue;
				if (i_minimum_height > height[target(*edgei, G_random)])
				{
					i_minimum_height = height[target(*edgei, G_random)];
				}
			}
			// std::cout << "The i minimum height is: " << i_minimum_height << " so i's height becomes: " << i_minimum_height + 1 << "\n";
			height[i] = i_minimum_height + 1;
			i_minimum_height = 10000;
		}
		if (excess[overflower] > 0 && overflower != s && overflower != t)
		{
			overflowQ.push(overflower);
		}

		// //
		if (logging){
			std::cout << "--------------------------------\n";
			write_graphviz(std::cout, G_random, make_label_writer(excess), make_label_writer(residual));
		}
	}
	// std::cout << "Done";
	// write_graphviz(std::cout, G_random, make_label_writer(excess), make_label_writer(residual));

	end = std::chrono::system_clock::now();
	duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
	std::cout << "-------------BOOST-------------\n";
	std::cout << "Duration :" << duration.count() << "\n";
	std::cout << "S is: " << s << "and t is: " << t << "\n";
	std::cout << "Maximum Flow is: " << excess[t] << "\n";
	std::cout << "To visualise the graphs visit: https://dreampuf.github.io/GraphvizOnline \n and paste the dot code which looks like digraph G {...}\n";
	return 0;
}


#include <cellogram/image_reader.h>
#include <Eigen/Dense>
#include <iostream>

int main(int argc, char** argv) {
	const std::string root = DATA_DIR;
	const std::string filename = root + "cell.tif";

	Eigen::MatrixXd img;
	cellogram::read_tif_image(filename, img);
	std::cout<<img<<std::endl;

	return 0;
}


#include "IntegratorTrap.hh"

#include <Eigen/Dense>
#include <iterator>

#include "TypesFunctions.hh"

using namespace Eigen;
using namespace std;

IntegratorTrap::IntegratorTrap(int orders) : IntegratorBase(orders, true)
{
  init_sampler();
}

IntegratorTrap::IntegratorTrap(size_t bins, int orders, double* edges) : IntegratorBase(bins, orders, edges, true)
{
  init_sampler();
}

IntegratorTrap::IntegratorTrap(size_t bins, int* orders, double* edges) : IntegratorBase(bins, orders, edges, true)
{
  init_sampler();
}

void IntegratorTrap::sample(FunctionArgs& fargs) {
  auto& rets=fargs.rets;
  rets[1].x = m_edges.cast<double>();
  rets[2].x = 0.0;
  auto npoints=m_edges.size()-1;
  rets[3].x = 0.5*(m_edges.tail(npoints)+m_edges.head(npoints));

  auto& abscissa=rets[0].x;

  auto nbins=m_edges.size()-1;
  auto& binwidths=m_edges.tail(nbins) - m_edges.head(nbins);
  ArrayXd samplewidths=binwidths/(m_orders.cast<double>()-1.0);

  auto* edge_a=m_edges.data();
  auto* edge_b{next(edge_a)};

  size_t offset=0;
  for (size_t i = 0; i < static_cast<size_t>(m_orders.size()); ++i) {
    auto n=m_orders[i];
    abscissa.segment(offset, n)=ArrayXd::LinSpaced(n, *edge_a, *edge_b);

    auto swidth=samplewidths[i];
    m_weights[offset]=swidth*0.5;
    if(n>2) {
      m_weights.segment(offset+1, n-2)=swidth;
    }

    offset+=n-1;
    advance(edge_a, 1);
    advance(edge_b, 1);
  }
  m_weights.tail(1)=samplewidths.tail(1)*0.5;
  rets.untaint();
  rets.freeze();
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_DETAIL_EXCLUSIVE_LESS_THAN_HPP_JOFA_100929
#define BOOST_ICL_DETAIL_EXCLUSIVE_LESS_THAN_HPP_JOFA_100929

#include <boost/icl/concept/interval.hpp>

namespace boost{ namespace icl
{

/// Comparison functor on intervals implementing an overlap free less
template <class IntervalT>
struct exclusive_less_than
{
    /** Operator <tt>operator()</tt> implements a strict weak ordering on intervals. */
    bool operator()(const IntervalT& left, const IntervalT& right)const
    {
        return icl::non_empty::exclusive_less(left, right);
    }
};

}} // namespace boost icl

#endif


/********************************************************************************
 * Author:   Gerrit Wellecke
 * Project:  Chimera states in populations of Kuramoto oscillators
 * Date:     March 2020
 *
 * Main function for the project on chimera states. As this project grew
 * gradually I only wrote one main for both systems (explicit and reduced). In
 * order to run the program properly only one of the two integration blocks
 * should be active at once, see below.
 * The integration functions were left here to simplify adaptations of code. All
 * functions that are "set in stone" are outsourced to other files.
 *
 * If no parallelisation is wanted the #pragma's in main can simply be commented
 * out.
 * Further the makefile might need to be adapted, depending on whether or not
 * parallelisation is wanted and what system architecture is used.
 *
 * OUTPUT is written to binary files. Each timestep of pop*osci oscillators is
 * written in sequence using 8-byte double precision floats.
 *******************************************************************************/
#include "functions.hpp"        // functions used in integration() and main()
#include "kuramoto.hpp"         // kuramoto model class
#include "ottantonsen.hpp"      // Ott-Antonsen reduction

#include <boost/numeric/odeint.hpp>
#include <omp.h>

#include <chrono>
#include <cmath>
#include <ctime>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

using namespace boost::numeric::odeint;

using state_type = std::vector<double>;

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

// integration of explicit Kuramoto system
void integration(const double omega, const double alpha, const int pop, const int osci,
        std::vector< std::vector<double> > &K, const uint maxIteration, std::string simName,
        const std::vector<double> phase, const std::vector<double> variance, const double dt,
        const Distribution dist, const long seedGiven
        ) {
    // state vector with initial phases
    state_type x;
    x.resize(pop * osci);

    // set initial conditions and return seed of RNG
    long seed {setInitCond(x, pop, osci, variance, phase, dist, seedGiven)};

    // filename w/ given parameters
    std::string fileName {"data/" + simName};

    // fileIO
    std::ofstream output(fileName, std::ios::binary);
    std::ofstream ordParamOut("data/" + simName + ".order", std::ios::binary);

    // initialize stepper
    runge_kutta4<state_type> stepper;

    // initialize system
    Kuramoto kuramoto_func{omega, K, alpha, pop, osci};

    // write log-file
    kuramoto_func.writeLog(simName, maxIteration, dt, seed);

    // write initial time step
    output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

    // write initial order parameter
    std::vector<double> ordP {orderParam(x, pop, osci)};
    ordParamOut.write(reinterpret_cast<char*>(ordP.data()), ordP.size() * sizeof(double));

    // integration loop
    uint iteration {0};

    while (iteration < maxIteration) {
        // as t isn't explicitly considered it is just set 0 here
        stepper.do_step(kuramoto_func, x, 0., dt);

        // renormalize phases in state vector
        renorm(x);

        // write time step to file
        output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

        // compute order parameter -- write to separate file
        ordP = orderParam(x, pop, osci);
        ordParamOut.write(reinterpret_cast<char*>(ordP.data()), ordP.size() * sizeof(double));

        // iterate integration step
        iteration++;
    }

    output.close();
    ordParamOut.close();
}

// integration of reduced OA system (analogous to integration of Kuramoto sys.)
void integrationOA(const double omega, const double alpha, const int pop, const int osci,
        std::vector< std::vector<double> > &K, const uint maxIteration, std::string simName,
        const double dt, /* const std::string Kname, */ const long seed
        ) {
    // state vector with init. cond.
    state_type x;
    x.resize(3);

    // set initial conditions and get seed
    // uncomment line below if wanting to reproduce initial state of Kuramoto sys.

    // long seed {setInitCondOAfromKuramoto(x, pop, osci, Kname)};

    // if using above line, comment the line below
    setInitCondOA(x, seed);

    // filename w/ given parameters
    std::string fileName {"data/" + simName + ".order"};

    // fileIO
    std::ofstream output(fileName, std::ios::binary);

    // initialize stepper
    runge_kutta4<state_type> stepper;

    // initialize system
    OttAntonsen OA_func{omega, K, alpha, pop, osci};

    // write log-file
    OA_func.writeLog(simName, maxIteration, dt, seed);

    // write initial time step
    output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

    // integration loop
    uint iteration {0};

    while (iteration < maxIteration) {
        // as t isn't explicitly considered it is just set 0 here
        stepper.do_step(OA_func, x, 0., dt);

        // renormalize mean phase difference
        x[2] = fmod(x[2], 2*M_PI);
        if (x[2] < 0)
            x[2] += 2*M_PI;

        // write time step
        output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

        // iterate integration step
        iteration++;
    }

    output.close();
}

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

int main(void) {
    // system parameters as needed by Kuramoto and OttAntonsen
    double omega {0.};
    double beta {.01}; // for beta -> 0, P(Chimera) -> 1;
    double alpha {.5 * M_PI - beta};
    int pop {2};
    int osci {128};

    // parameters for initial conditions
    std::vector<double> phase{0., 0.}; // (0, 0) -- in phase
    std::vector<double> variance{.2, M_PI};
    Distribution distribution {Distribution::NORMAL}; // only for explicit case

    // parameters for integration
    uint maxIteration {500000};
    double dt{.1};

    // vectors to iterate over
    std::vector<double> Avec {.05, .15, .20, .40};
    std::vector<long> seedVec {4578, 130547, 1337, 4578};

    std::vector<std::string> Kname {"data/seed_4578_A_0.050000",
        "data/seed_130547_A_0.150000", "data/seed_1337_A_0.200000",
        "data/seed_4578_A_0.400000"};

    // Numerical solution of explicit Kuramoto system
    /*
#pragma omp parallel for
    for (int par = 0; par < static_cast<int>(Avec.size()); par++) {
        // set seed
        long seedGiven {seedVec[par]};

        // coupling matrix
        double A {Avec[par]};
        double mu {.5 + A/2.};
        double nu {.5 - A/2.};

        std::vector< std::vector<double> > K {{mu, nu}, {nu, mu}};

        // name for given integration
        // std::string simName {"seed_" + std::to_string(seedGiven) + "_A_" + std::to_string(A)};
        std::string simName{"OttAntonsen_1"};

        // just benchmarking fun
        auto start {std::chrono::high_resolution_clock::now()};

        // integration of Kuramoto system w/ above setup
        integration(omega, alpha, pop, osci, K, maxIteration, simName,
                    phase, variance, dt, distribution, seedGiven);

        // continue benchmarking fun
        auto stop {std::chrono::high_resolution_clock::now()};

        auto benchmark {std::chrono::duration_cast<std::chrono::microseconds>(stop - start)};

        std::cout.precision(6);
        // std::cout << "Thread " << omp_get_thread_num() << " has finished calculation "
                  // << par + 1 << '\n';
        std::cout << "Execution time: " << std::fixed << benchmark.count() * 1e-6 << "s\n";
    }
    */

    // Numerical solution of Ott-Antonsen reduction
    // set seed
    long seedGiven {std::time(nullptr)};

#pragma omp parallel for
    //for (int par = 0; par < static_cast<int>(Avec.size()); par++) {
    for (int par = 0; par <= 50 ; par++) {
        //seedGiven = seedVec[par];

        // coupling matrix
        //double A {Avec[par]};
        double A {.01 * par};
        double mu {.5 + A/2.};
        double nu {.5 - A/2.};

        std::vector< std::vector<double> > K {{mu, nu}, {nu, mu}};

        // name for given integration
        std::string simName{"OttAntonsen_A" + std::to_string(A)};

        // just benchmarking fun
        auto start {std::chrono::high_resolution_clock::now()};

        integrationOA(omega, alpha, pop, osci, K, maxIteration, simName,
                        dt, /* Kname[par], */ seedGiven);

        // continue benchmarking fun
        auto stop {std::chrono::high_resolution_clock::now()};

        auto benchmark {std::chrono::duration_cast<std::chrono::microseconds>(stop - start)};

        std::cout.precision(6);
        std::cout << "Execution time: " << std::fixed << benchmark.count() * 1e-6 << "s\n";
    }

    return 0;
}