/*=============================================================================
  PHAS0100ASSIGNMENT1: PHAS0100 Assignment 1 Game of Life Simulation.
  Copyright (c) University College London (UCL). All rights reserved.
  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.
  See LICENSE.txt in the top level directory for details.
=============================================================================*/

#include <nbsimParticle.h>
#include <nbsimMassiveParticle.h>
#include <nbsimSolarSystemData.ipp>
#include <iostream>
#include <Eigen/Dense>
#include <chrono>

// Example, header-only library, included in project for simplicity's sake.

/**
 * \brief Demo file to check that includes and library linkage is correct.
 */

static void show_usage(std::string name)
{
    std::cerr << "Usage: " << name
    << "Options:\n"
    << "\tOption: (Solar System Simulator):\tUsers should input .1) step-size(unit: year) .2) length of time(unit: year)\n"
    << "Options:\n"
    << "\t-h,--help\t\t\tShow this help message.\n"
    << std::endl;
}

int main(int argc, char** argv)
{
    // -h and --help:
    if (argc == 2){
        if((argv[1] == "-h") or (argv[1] == "--help")){
            show_usage(argv[0]);
            return 0;
        }
        else{
            show_usage(argv[0]);
            return 0;
        }
    }

    // Option: Solar System:
    if (argc == 3){

        double step_size = std::stod(argv[1]);
        double totalTime = std::stod(argv[2]);

        int NPLANETS = 9;

        std::string name[NPLANETS];
        Eigen::Vector3d position;
        Eigen::Vector3d velocity;
        double mu;

        std::shared_ptr<nbsim::MassiveParticle> planet[NPLANETS];

        // Split the planet:
        for (int i=0;i<NPLANETS;i++){
            name[i] =  nbsim::solarSystemData.at(i).name;
            position = nbsim::solarSystemData.at(i).position;
            velocity = nbsim::solarSystemData.at(i).velocity;
            mu =  nbsim::solarSystemData.at(i).mu;

            std::shared_ptr<nbsim::MassiveParticle> particle(new nbsim::MassiveParticle(position,velocity,mu));
            planet[i] = particle;
        }

        // Add the attractor:
        for(int i=0;i<NPLANETS;i++){
            for(int j=0;j<NPLANETS;j++){
                if(i != j){
                    planet[i]->addAttractor(planet[j]);
                }
            }
        }

        // At the begining: calculate the Energy
        planet[0]->addAttractor(planet[0]); // add itself

        planet[0]->calculateEtotal();
        std::cout<<"Beginning:"<<"\n";
        std::cout<<"The kinetic energy of system is: "<<planet[0]->getEkinetic()<<"\n";
        std::cout<<"The potential Energy of system is: "<<planet[0]->getEpotential()<<"\n";
        std::cout<<"The Total Energy of system is: "<<planet[0]->getEtotal()<<"\n";
        planet[0]->removeAttractor(planet[0]); // remove itself

        // // Benchmark the time of the solar system: Begin
        std::clock_t c_start = std::clock();
        auto t_start = std::chrono::high_resolution_clock::now();

        // Parallel:
        omp_set_num_threads (8);

        #pragma omp parallel
        // Outer time:
        for (double t = 0;t<totalTime;t+=step_size){

            // Loop 1: Acceleration:
            #pragma omp for
            for (int i=0;i<NPLANETS;i++){
                planet[i]->calculateAcceleration();
            }
            // Loop 2: intergateTimestep:
            #pragma omp for
            for (int i=0;i<NPLANETS;i++){
                planet[i]->integrateTimestep(step_size);

            }
        }

        // Benchmark the time of the solar system: End
        std::clock_t c_end = std::clock();
        auto t_end = std::chrono::high_resolution_clock::now();

        // Summarising the position:
        for(int i=0;i<NPLANETS;i++){
            std::cout<<name[i] <<":\n Original Position: \n"<<nbsim::solarSystemData.at(i).position<<"\n";
            std::cout<<" Current Position: \n"<<planet[i]->getPosition()<<"\n\n";
        }

        // Calculate the System Energy:
        planet[0]->addAttractor(planet[0]); // add itself

        planet[0]->calculateEtotal();
        std::cout<<"End:"<<"\n";
        std::cout<<"The Kinetic Energy of system is: "<<planet[0]->getEkinetic()<<"\n";
        std::cout<<"The Potential Energy of system is: "<<planet[0]->getEpotential()<<"\n";
        std::cout<<"The Total Energy of system is: "<<planet[0]->getEtotal()<<"\n";

        planet[0]->removeAttractor(planet[0]); // remove itself

        // Benchmark and output the time:
        std::cout<<"\nThe run time is: "
                 <<1000.0*(c_end - c_start)/CLOCKS_PER_SEC<<" ms\n"
                 <<"Wall clock time passed: "
                 <<std::chrono::duration<double, std::milli>(t_end-t_start).count()<<" ms\n";

        return 1;
    }

    // Default Option:
    else{
        show_usage(argv[0]);
        return 0;
    }
}


#include <cmath>    // for std::sqrt
#include <cstdlib>  // for std::rand
#include <thread>   // for std::thread
#include <atomic>   // for std::atomic
#include <string>
#include <algorithm>
#include <vector>	// for std:vector
#include <iostream>
#include <chrono>   // for time calculations
// thread libraries to be tested
#include "CTPL/ctpl.h" // for testing CPTL thread pool
#ifndef TIME_UTC
#define TIME_UTC TIME_UTC_
#endif
#include "threadpool/boost/threadpool.hpp" // for testing old boost thread pool (by Philippe Henkel)
#include "ThreadPool/ThreadPool.h" // for testing thread pool using Jakob Progsch's thread pool
#if BOOST_VERSION > 105600
#include <boost/thread/executors/basic_thread_pool.hpp> // for testing new boost thread pool. available from ver 1.56 and up
#endif
#include "thread_pool.hpp" // for asio based pool

// the work being done is calculating if a number is prime or no and accumulating the result
bool IsPrime(unsigned long n)
{
    // special handle for 0,1,2
    if (n < 3)
    {
	if (n == 2) return true;
    }

    // no need to check above sqrt(n)
    const auto N = std::ceil(std::sqrt(n) + 1);

    for (auto i = 2; i < N; ++i)
    {
        if (n%i == 0)
        {
			return false;
        }
    }

	return true;
}

// check if a number is prime and accumulate into a counter
void CountIfPrime(unsigned long n, unsigned long& count)
{
	if (IsPrime(n)) ++count;
}

// check if a number is prime and accumulate into a counter - thred safe with thread id
void CountIfPrimeCTPL(int id, unsigned long n, std::atomic<unsigned long>& count)
{
	if (IsPrime(n)) ++count;
}

// check if a number is prime and accumulate into a counter - thred safe
void CountIfPrimeBoost(unsigned long n, std::atomic<unsigned long>& count)
{
	if (IsPrime(n)) ++count;
}

unsigned long count_primes_asio(const std::vector<unsigned long>& random_inputs, unsigned int NUMBER_OF_PROCS)
{
    std::atomic<unsigned long> number_of_primes(0);
	{
		asio_thread_pool pool(NUMBER_OF_PROCS);

		// loop over input to accumulate how many primes are there
    	std::for_each(random_inputs.begin(), random_inputs.end(),
            [&](unsigned long n)
    	{
    		while (!pool.run_task(boost::bind(CountIfPrimeBoost, n, std::ref(number_of_primes)))) {}
    	});

        // as pool go out of scope, it will wait for all threads to finish
	}

    return number_of_primes;
}

#if BOOST_VERSION > 105600
// using boost's experimental thread pool
unsigned long count_primes_boost(const std::vector<unsigned long>& random_inputs, unsigned int NUMBER_OF_PROCS)
{
    // here the main thread also does work
	boost::basic_thread_pool tp(NUMBER_OF_PROCS-1);
    std::atomic<unsigned long> number_of_primes(0);

	// loop over input to accumulate how many primes are there
    std::for_each(random_inputs.begin(), random_inputs.end(),
            [&](unsigned long n)
    {
    	tp.submit(boost::bind(CountIfPrimeBoost, n, std::ref(number_of_primes)));
    });

    // wait for all threads to finish and do work at the same time
	while(tp.try_executing_one()) {}

    tp.close();

    return number_of_primes;
}
#endif

// using Jakob Progsch's thread pool
unsigned long count_primes_JP(const std::vector<unsigned long>& random_inputs, unsigned int NUMBER_OF_PROCS)
{
    std::atomic<unsigned long> number_of_primes(0);
    {
        ThreadPool pool(NUMBER_OF_PROCS);

	    // loop over input to accumulate how many primes are there
        std::for_each(random_inputs.begin(), random_inputs.end(),
                [&](unsigned long n)
        {
            pool.enqueue(CountIfPrimeBoost, n, std::ref(number_of_primes));
        });

        // as pool go out of scope, it will wait for all threads to finish
    }

    return number_of_primes;
}

// using old boost thread pool
unsigned long count_primes_boost_old(const std::vector<unsigned long>& random_inputs, unsigned int NUMBER_OF_PROCS)
{
    std::atomic<unsigned long> number_of_primes(0);
    {
        boost::threadpool::pool tp(NUMBER_OF_PROCS);

	    // loop over input to accumulate how many primes are there
        std::for_each(random_inputs.begin(), random_inputs.end(),
                [&](unsigned long n)
        {
            boost::threadpool::schedule(tp, boost::bind(CountIfPrimeBoost, n , std::ref(number_of_primes)));
        });

        // as pool go out of scope, it will wait for all threads to finish
    }

    return number_of_primes;
}

// using CPTL thread pool
unsigned long count_primes_ctpl(const std::vector<unsigned long>& random_inputs, unsigned int NUMBER_OF_PROCS)
{
    ctpl::thread_pool pool(NUMBER_OF_PROCS);
    std::atomic<unsigned long> number_of_primes(0);

	// loop over input to accumulate how many primes are there
    std::for_each(random_inputs.begin(), random_inputs.end(),
            [&](unsigned long n)
    {
        pool.push(CountIfPrimeCTPL, n, std::ref(number_of_primes));
    });

    // wait for all threads to finish
    pool.stop(true);

    return number_of_primes;
}

// single threaded calculation of primes
unsigned long count_primes(const std::vector<unsigned long>& random_inputs)
{
    unsigned long number_of_primes = 0;
	// loop over input to accumulate how many primes are there
    std::for_each(random_inputs.begin(), random_inputs.end(),
            [&](unsigned long n)
			{
				CountIfPrime(n, number_of_primes);
			});

    return number_of_primes;
}

int main(int argc, char** argv)
{
    if (argc != 3)
    {
        std::cout << "Usage: " << argv[0] << " <size of input> <number of procs>" << std::endl;
        return 1;
    }

	const auto MAX_PROCS = std::thread::hardware_concurrency();
    const auto INPUT_SIZE = std::stol(argv[1]);
    const auto NUMBER_OF_PROC = std::stol(argv[2]);

    if (MAX_PROCS < NUMBER_OF_PROC)
    {
        std::cout << "maximum " << MAX_PROCS  << " concurrent threads are supported. use less threads" << std::endl;
        return 1;
    }

    std::vector<unsigned long> random_inputs;

    for (auto i = 0; i < INPUT_SIZE; ++i)
    {
        random_inputs.push_back(std::rand());
    }

    std::chrono::time_point<std::chrono::system_clock> start, end;

    start = std::chrono::system_clock::now();
    auto number_of_primes = count_primes(random_inputs);
    end = std::chrono::system_clock::now();
    std::cout << "count_primes:" << number_of_primes << " prime numbers were found. computation took " <<
        std::chrono::duration_cast<std::chrono::nanoseconds> (end - start).count()/INPUT_SIZE  << " nanosec per iteration" << std::endl;

    start = std::chrono::system_clock::now();
    number_of_primes = count_primes_ctpl(random_inputs, NUMBER_OF_PROC);
    end = std::chrono::system_clock::now();
    std::cout << "count_primes_ctpl:" << number_of_primes << " prime numbers were found. computation took " <<
        std::chrono::duration_cast<std::chrono::nanoseconds> (end - start).count()/INPUT_SIZE  << " nanosec per iteration" << std::endl;

    start = std::chrono::system_clock::now();
    number_of_primes = count_primes_boost_old(random_inputs, NUMBER_OF_PROC);
    end = std::chrono::system_clock::now();
    std::cout << "count_primes_boost_old:" << number_of_primes << " prime numbers were found. computation took " <<
        std::chrono::duration_cast<std::chrono::nanoseconds> (end - start).count()/INPUT_SIZE  << " nanosec per iteration" << std::endl;

    start = std::chrono::system_clock::now();
    number_of_primes = count_primes_JP(random_inputs, NUMBER_OF_PROC);
    end = std::chrono::system_clock::now();
    std::cout << "count_primes_JP:" << number_of_primes << " prime numbers were found. computation took " <<
        std::chrono::duration_cast<std::chrono::nanoseconds> (end - start).count()/INPUT_SIZE  << " nanosec per iteration" << std::endl;

#if BOOST_VERSION > 105600
    start = std::chrono::system_clock::now();
    number_of_primes = count_primes_boost(random_inputs, NUMBER_OF_PROC);
    end = std::chrono::system_clock::now();
    std::cout << "count_primes_boost:" << number_of_primes << " prime numbers were found. computation took " <<
        std::chrono::duration_cast<std::chrono::nanoseconds> (end - start).count()/INPUT_SIZE  << " nanosec per iteration" << std::endl;
#else
    std::cout << "count_primes_boost cannot be tested. boost version is: " << BOOST_VERSION << std::endl;
#endif

    start = std::chrono::system_clock::now();
    number_of_primes = count_primes_asio(random_inputs, NUMBER_OF_PROC);
    end = std::chrono::system_clock::now();
    std::cout << "count_primes_asio:" << number_of_primes << " prime numbers were found. computation took " <<
        std::chrono::duration_cast<std::chrono::nanoseconds> (end - start).count()/INPUT_SIZE  << " nanosec per iteration" << std::endl;

    return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <cmath>
#include <complex>

template <typename T, typename U>
inline bool about(T x, U y)
{
    return std::abs(x - y) < 0.001;
}

template <typename T, typename U>
void test(T x, U y)
{
    using namespace mtl::sfunctor; using mtl::sfunctor::abs; using mtl::sfunctor::negate;
    using mtl::sfunctor::plus;
    using std::cout;

    typedef compose<negate<typename abs<T>::result_type>, abs<T> > nabs;
    cout << "-abs(" << x << ") = " << nabs::apply(x) << "\n";
    MTL_THROW_IF(!about(nabs::apply(x), -std::abs(x)), mtl::runtime_error("Wrong result for -abs(x)"));

    typedef compose<square<typename nabs::result_type>, nabs> snabs;
    cout << "(-abs(" << x << "))^2 = " << snabs::apply(x) << "\n";
    cout << "-std::abs(x) * -std::abs(x) = " << -std::abs(x) * -std::abs(x) << "\n";
    MTL_THROW_IF(!about(snabs::apply(x), -std::abs(x) * -std::abs(x)), mtl::runtime_error("Wrong result for (-abs(x))^2"));

    typedef compose_first<plus<typename abs<T>::result_type, U>, abs<T> > plus_abs;
    cout << "abs(" << x << ") + " << y << " = " << plus_abs::apply(x, y) << "\n";
    MTL_THROW_IF(!about(plus_abs::apply(x, y), std::abs(x) + y), mtl::runtime_error("Wrong result for abs(x) + y"));

    typedef compose_second<plus<T, typename abs<U>::result_type>, abs<U> > x_plus_abs_y;
    cout << x << " + " << "abs(" << y << ") = " << x_plus_abs_y::apply(x, y) << "\n";
    MTL_THROW_IF(!about(x_plus_abs_y::apply(x, y), x + std::abs(y)), mtl::runtime_error("Wrong result for x + abs(y)"));

    typedef compose_both<plus<T, typename abs<U>::result_type>, negate<T>, abs<U> > minus_x_plus_abs_y;
    cout << "-" << x << " + " << "abs(" << y << ") = " << minus_x_plus_abs_y::apply(x, y) << "\n";
    MTL_THROW_IF(!about(minus_x_plus_abs_y::apply(x, y), -x + std::abs(y)), mtl::runtime_error("Wrong result for -x + abs(y)"));

    cout << "l_2(" << x << ", " << 2.0f*x << ") = " << l_2_2D<T>::apply(x, 2.0f*x)  << "\n";
    MTL_THROW_IF(!about(l_2_2D<T>::apply(x, 2.0f*x), std::sqrt(std::abs(5.0f*x*x))), mtl::runtime_error("Wrong result for l_2_2D(x, 2.0*x)"));

    cout << std::endl;
}

int main(int, char**)
{
    double              a= 3.0, b= -5.0;
    float               c= -9;
    std::complex<float> d(1., 2.);

    test(a, b);
    test(b, a);
    test(c, a);
    test(d, c);

    return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <time.h>
#include <memory>

//#include <boost/random/uniform_int_distribution.hpp>
//#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/sampler.hpp>

using namespace Eigen;
using std::cout;
using std::endl;

template<typename T>
class Cat : public Distribution<T>
{
public:
  uint32_t K_;
  Matrix<T,Dynamic,1> pdf_;
  Matrix<T,Dynamic,1> cdf_;

  /* constructor from pdf */
  Cat(const Matrix<T,Dynamic,1>& pdf, boost::mt19937 *pRndGen);
  /* constructor from indicators - estimates from counts */
  Cat(const VectorXu& z, boost::mt19937 *pRndGen);
  /* copy constructor */
  Cat(const Cat& other);
  virtual ~Cat();

  uint32_t sample();
  void sample(VectorXu& z);

  T logPdf(const Matrix<T,Dynamic,1>& x) const
  {
    //assuming x is all zeros except one element
    assert(x.rows()==K_); //data dimmension should agree with # categories
    for(uint32_t d=0; d<K_; ++d) {
      if(x(d)==1) {
        return(log(pdf_(d)));
      }
    }
    assert(false); //invalid data (at least one element must be one) [this should never happen]
    return(-1);
  };

  T logPdf(uint32_t x) const
  {
    assert(x < K_);
    return(log(pdf_(x)));
  };

  T logPdfOfSS(const Matrix<T,Dynamic,1>& x) const
  {

    assert(x.rows()==K_); //data dimmension should agree with # categories
    T logPdf = 0;
    for(uint32_t d=0; d<K_; ++d) {
      logPdf += x(d) * log(pdf_(d));
    }
    return logPdf;
  };

  const Matrix<T,Dynamic,1>& pdf() const {return pdf_;};
  void pdf(const Matrix<T,Dynamic,1>& pdf){
    pdf_ = pdf;
    updateCdf();
  };
  const Matrix<T,Dynamic,1>& cdf() const {return cdf_;};

  void print() const;

private:
  boost::uniform_01<T> unif_;
  void updateCdf();
};

typedef Cat<float> Catf;
typedef Cat<double> Catd;

template<typename T>
inline T logSumExpRow(const Matrix<T,Dynamic,Dynamic>& pdf, uint32_t i)
{
  T max = pdf.row(i).maxCoeff();
  return log((pdf.row(i).array()-max).exp().matrix().sum()) + max;
}

template<typename T>
inline T logSumExpCol(const Matrix<T,Dynamic,Dynamic>& pdf, uint32_t i)
{
  T max = pdf.col(i).maxCoeff();
  return log((pdf.col(i).array()-max).exp().matrix().sum()) + max;
}

template<typename T>
inline T logSumExp(const Matrix<T,Dynamic,1>& pdf)
{
  T max = pdf.maxCoeff();
  return log((pdf.array()-max).exp().matrix().sum()) + max;
}

//template<typename T>
//inline T logSumExp(const Matrix<T,1,Dynamic>& pdf)
//{
//  T max = pdf.maxCoeff();
//  return log((pdf.array()-max).exp().matrix().sum()) + max;
//}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/1/dc_motor_ex.cpp
 *  @author Jeferson Lima
 *  @brief  DC Motor Simulation Example
 *  @date   Mar 15, 2022
 **/

#include <iostream>
#include <boost/numeric/odeint.hpp>
#include <boost/array.hpp>
#include <robocore.hpp>
#ifdef USE_MATPLOTLIB
 #include <matplotlibcpp.h>
 namespace plt = matplotlibcpp;
#endif

 using namespace boost::numeric::odeint;

 const double j = 0.01;         // (J)     moment of inertia of the rotor     0.01 kg.m^2
 const double b = 0.1;          // (b)     motor viscous friction constant    0.1 N.m.s
 const double Ke = 0.01;        // (Ke)    electromotive force constant       0.01 V/rad/sec
 const double Kt = 0.01;        // (Kt)    motor torque constant              0.01 N.m/Amp
 const double R = 1;            // (R)     electric resistance                1 Ohm
 const double L = 0.5;          // (L)     electric inductance                0.5 H
 const double V = 12;           // (V)     motor votage                       12 V

 typedef boost::array< double , 2> state;
 std::vector<double> ts, dTheta, di;

void dc_motor_model(const state& x, state& dxdt, double t)
{
  dxdt[0] = -b*x[0]/j + Kt*x[1]/j;
  dxdt[1] = -R*x[1]/L + V/L - Ke*x[0];
}

void log_model(const state& x, const double t)
{
  ts.push_back(t);
  dTheta.push_back(x[0]);
  di.push_back(x[1]);
  std::cout << t << ';' << x[0] << ';' << x[1]  << std::endl;
}

int main(int argc, char** argv)
{
  state x = { 0.0 , 0.0 }; // initial conditions
  runge_kutta4< state > stepper;
  integrate_const( stepper , dc_motor_model, x , 0.0 , 20.0 , 0.1, log_model );

#ifdef USE_MATPLOTLIB
  plt::figure();
  plt::named_plot("Velocity (rad/s)", ts, dTheta);
  plt::named_plot("Current (A)", ts, di);
  plt::legend();
  plt::show();
#endif

#ifdef SAVE_OUTPUT_CSV
  columns vals = {{"Time", ts}, {"Velocity", dTheta}, {"Current", di}};
  RoboCore::write_csv("/tmp/dc_motor_output.csv", vals);
  std::cout << "Output file was saved in /tmp" << std::endl;
#endif

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// $Id: percentile.hpp 7503 2015-05-21 18:37:51Z chambm $
//
//  Copyright 2011 Vanderbilt University. 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_PERCENTILE_HPP_
#define BOOST_ACCUMULATORS_STATISTICS_PERCENTILE_HPP_

#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

namespace boost { namespace accumulators
{

BOOST_PARAMETER_KEYWORD(tag, percentile_number)

namespace impl
{
    template<typename Sample>
    struct percentile_impl : accumulator_base
    {
        typedef Sample result_type;

        percentile_impl(dont_care) : isSorted(false) {}

        template<typename Args>
        void operator ()(const Args& args)
        {
            buffer_.push_back(args[sample]);
            isSorted = false;
        }

        template<typename Args>
        result_type result(const Args& args) const
        {
            if (buffer_.empty())
                return result_type();

            if(!isSorted)
            {
                std::sort(buffer_.begin(), buffer_.end());
                isSorted = true;
            }

            size_t percentile_num = args[percentile_number];
            double percentile = percentile_num / 100.0;
            double integer, fraction = modf((buffer_.size()-1)*percentile, &integer);
            size_t index = static_cast<size_t>(integer);
            if (fraction == 0)
                return buffer_[index];
            else
                return static_cast<result_type>(buffer_[index] + fraction * (buffer_[index+1] - buffer_[index]));
        }

    private:
        mutable std::vector<Sample> buffer_;
        mutable bool isSorted;
    };
} // namespace impl

namespace tag
{
    struct percentile : depends_on<>
    {
        typedef impl::percentile_impl<mpl::_1> impl;
    };
}

namespace extract { extractor<tag::percentile> const percentile = {}; }
using extract::percentile;

}} // namespace boost::accumulators

#endif


#include <mpi.h>
#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

using namespace std;
using namespace Eigen;

typedef Matrix<double, Dynamic, Dynamic, RowMajor> RowMatrixXd;
typedef Matrix<complex<double>, Dynamic, Dynamic, RowMajor> RowMatrixXcd;

int main(int argc, char *argv[]) {
    int npes, myrank;

    MPI::Init(argc, argv);

    myrank = MPI::COMM_WORLD.Get_rank();
    npes = MPI::COMM_WORLD.Get_size();

    long xn = atol(argv[2]);
    long tn = atol(argv[3]);
    long long position = 0;

    int xnProc = ceil((xn + 0.0) / (npes + 0.0));
    int outputPrecision = 9;
    int modeCount = tn;

    RowMatrixXd snapshotsProc(xnProc, tn + 1);
    RowMatrixXd snapshotsInProdProc(tn, tn);
    RowMatrixXd r(tn, tn);
    RowMatrixXd sigmaPseudoInverse(tn, tn);
    RowMatrixXd v(tn, tn), uProc(xnProc, tn);
    RowMatrixXd aTildeProc(tn, tn), aTilde(tn, tn);
    RowMatrixXd x0TildeProc(tn, 1), x0Tilde(tn, 1);

    RowMatrixXcd eigenvec(tn, tn);
    RowMatrixXcd dmdModesAllProc(xnProc, tn);

    VectorXd dmdModeProc(xnProc);
    VectorXd dmdMode(xnProc * npes);

    double *snapshotsRecvBuf = new double[xnProc * (tn + 1)];
    double *tntnBuf = new double[tn * tn];
    double *tn1Buf = new double[tn * 1];

    double *tntn_pt = new double[tn * tn];
    double *tn1_pt = new double[tn * 1];
    double *tntn2_pt = new double[tn * tn];
    double *xnProc_pt = new double[xnProc];
    double *xnProcnpes_pt = new double[xnProc * npes];

    complex<double> *tntnc_pt = new complex<double>[tn * tn];

    MPI::COMM_WORLD.Barrier();
    double timeBegin = MPI::Wtime();

    /* input matrix from a file */
    double *snapshots = NULL;

    if (myrank == 0) {
        if (modeCount > tn) {
            cout << "Invalid output modes number" << endl;
            exit(0);
        }

        cout << "Initializing" << endl;

        snapshots = new double[xn * (tn + 1)];
        ifstream inputFile(argv[1]);
        if (inputFile.is_open()) {
            while (!inputFile.eof() && position < (xn * (tn + 1))) {
                inputFile >> snapshots[position];
                position++;
            }
        } else {
            cout << "input file not found." << endl;
            exit(0);
        }
        inputFile.close();

        cout << "Running" << endl;
    }
    /****************************/

    MPI::COMM_WORLD.Scatter(snapshots, xnProc * (tn + 1), MPI::DOUBLE,
                            snapshotsRecvBuf, xnProc * (tn + 1), MPI::DOUBLE,
                            0);

    // delete[] snapshots;

    snapshotsProc = Map<RowMatrixXd>(snapshotsRecvBuf, xnProc, tn + 1);

    // delete[] snapshotsRecvBuf;

    snapshotsInProdProc = snapshotsProc.block(0, 0, xnProc, tn).transpose() *
                          snapshotsProc.block(0, 0, xnProc, tn);

    tntn_pt = snapshotsInProdProc.data();

    MPI::COMM_WORLD.Reduce(tntn_pt, tntnBuf, tn * tn, MPI::DOUBLE, MPI::SUM, 0);

    if (myrank == 0) {
        VectorXd sigma(tn);

        RowMatrixXd snapshotsInProd = Map<RowMatrixXd>(tntnBuf, tn, tn);

        LLT<RowMatrixXd, Upper> cholesky(snapshotsInProd);
        r = cholesky.matrixU();

        JacobiSVD<RowMatrixXd> svd(r, ComputeThinV);
        sigma = svd.singularValues();
        v = svd.matrixV();

        for (int i = 0; i < tn; ++i) {
            for (int j = 0; j < tn; ++j) {
                if (i == j && sigma(i) != 0) {
                    sigmaPseudoInverse(i, j) = 1.0 / sigma(i);
                } else
                    sigmaPseudoInverse(i, j) = 0;
            }
        }

        tntn_pt = sigmaPseudoInverse.data();
        tntn2_pt = v.data();
    }

    MPI::COMM_WORLD.Bcast(tntn_pt, tn * tn, MPI::DOUBLE, 0);
    MPI::COMM_WORLD.Bcast(tntn2_pt, tn * tn, MPI::DOUBLE, 0);

    sigmaPseudoInverse = Map<RowMatrixXd>(tntn_pt, tn, tn);
    v = Map<RowMatrixXd>(tntn2_pt, tn, tn);

    uProc = snapshotsProc.block(0, 0, xnProc, tn) * v * sigmaPseudoInverse;

    if (myrank != npes - 1) {
        aTildeProc = uProc.adjoint() * snapshotsProc.block(0, 1, xnProc, tn) *
                     v * sigmaPseudoInverse;
        x0TildeProc = uProc.adjoint() * snapshotsProc.block(0, 0, xnProc, 1);

    } else {
        aTildeProc = uProc.block(0, 0, xn - xnProc * (npes - 1), tn).adjoint() *
                     snapshotsProc.block(0, 1, xn - xnProc * (npes - 1), tn) *
                     v * sigmaPseudoInverse;
        x0TildeProc =
          uProc.block(0, 0, xn - xnProc * (npes - 1), tn).adjoint() *
          snapshotsProc.block(0, 0, xn - xnProc * (npes - 1), 1);
    }

    tntn_pt = aTildeProc.data();
    tn1_pt = x0TildeProc.data();

    MPI::COMM_WORLD.Reduce(tntn_pt, tntnBuf, tn * tn, MPI::DOUBLE, MPI::SUM, 0);
    MPI::COMM_WORLD.Reduce(tn1_pt, tn1Buf, tn * 1, MPI::Double, MPI::SUM, 0);

    if (myrank == 0) {
        aTilde = Map<RowMatrixXd>(tntnBuf, tn, tn);
        x0Tilde = Map<RowMatrixXd>(tn1Buf, tn, 1);

        ComplexEigenSolver<RowMatrixXd> ces(aTilde);

        eigenvec = ces.eigenvectors();
        tntnc_pt = eigenvec.data();

        ofstream ofeigenvalReal, ofeigenvalImag, ofamplitudeReal,
          ofamplitudeImag;

        ofeigenvalReal.open("eigenvalReal.dat",
                            ofstream::trunc | ofstream::out);
        ofeigenvalImag.open("eigenvalImag.dat",
                            ofstream::trunc | ofstream::out);
        ofamplitudeReal.open("amplitudeReal.dat",
                             ofstream::trunc | ofstream::out);
        ofamplitudeImag.open("amplitudeImag.dat",
                             ofstream::trunc | ofstream::out);

        for (int i = 0; i < modeCount; ++i) {

          ofeigenvalReal << setprecision(outputPrecision)
                         << ces.eigenvalues().row(i).real() << endl;

          ofeigenvalImag << setprecision(outputPrecision)
                         << ces.eigenvalues().row(i).imag() << endl;

          ofamplitudeReal << setprecision(outputPrecision)
                          << (eigenvec.inverse() * x0Tilde).row(i).real()
                          << endl;

          ofamplitudeImag << setprecision(outputPrecision)
                          << (eigenvec.inverse() * x0Tilde).row(i).imag()
                          << endl;
        }
    }

    MPI::COMM_WORLD.Bcast(tntnc_pt, tn * tn, MPI::DOUBLE_COMPLEX, 0);
    eigenvec = Map<RowMatrixXcd>(tntnc_pt, tn, tn);

    dmdModesAllProc = uProc * eigenvec;

    for (int i = 0; i < modeCount; ++i) {
        dmdModeProc = dmdModesAllProc.col(i).real();

        xnProc_pt = dmdModeProc.data();

        MPI::COMM_WORLD.Barrier();

        MPI::COMM_WORLD.Gather(xnProc_pt, xnProc, MPI::DOUBLE, xnProcnpes_pt,
                               xnProc, MPI::DOUBLE, 0);

        if (myrank == 0) {
            dmdMode = Map<VectorXd>(xnProcnpes_pt, xnProc * npes);

            ofstream ofdmdMode;
            ofdmdMode.open("dmdMode" + to_string(i) + ".dat",
                           ofstream::trunc | ofstream::out);
            ofdmdMode << setprecision(outputPrecision) << dmdMode.head(xn)
                      << endl;
        }
    }

    MPI::COMM_WORLD.Barrier();

    double timeFinish = MPI::Wtime();
    double timing = timeFinish - timeBegin;
    double timingMax;

    MPI::COMM_WORLD.Reduce(&timing, &timingMax, 1, MPI::DOUBLE, MPI::MAX, 0);

    if (myrank == 0) {
        cout << "Calculation finished successfully." << endl
             << "Spatial: " << xn << " points." << endl
             << "Temporal: " << tn + 1 << " state vectors." << endl
             << "Parallelism: " << npes << " cores." << endl
             << "Time consumption: " << timingMax << " s." << endl
             << "Output DMD modes: " << modeCount << endl;
    }

    MPI::Finalize();
    return 0;
}


#pragma once

#include "enum.hpp"
#include "cc_trade_stream.hpp"
#include "utils.hpp"
#include "trade_data.hpp"

#include <boost/log/trivial.hpp>

#include <concepts>
#include <map>

namespace profitview
{

template<std::floating_point Float = double, std::integral Int = int>
class CcKaufman : public CcTradeStream<Float, Int>
{
public:
    CcKaufman(
        const std::string trade_stream_name,
        OrderExecutor* executor,
        Int lookback,
        Float base_quantity,
        Int er_period,
        Int fast_sc,
        Int slow_sc,
        Int kama_trend,
        const std::string& csv_name = "Kaufman.csv")
        : CcTradeStream<Float, Int>(trade_stream_name, executor, csv_name)
        , lookback_{lookback}
        , base_quantity_{base_quantity}
        , er_period_{er_period}
        , fast_sc_{fast_sc}
        , slow_sc_{slow_sc}
        , kama_trend_{kama_trend}
    {}

    void onStreamedTrade(TradeData const& trade_data) override
    {
        trade_data.print();

        auto& [prices, mean_reached, initial_mean, kama, kamas]{price_structure_[trade_data.symbol]};

        prices.emplace_back(trade_data.price);

        auto sc_factor{2.0 / (fast_sc_ + 1) - 2.0 / (slow_sc_ + 1)};
        auto sc_sum{2.0 / (slow_sc_ + 1)};

        if (not mean_reached && prices.size() + 1 == lookback_)
        {
            kama = initial_mean = util::ma(prices);
            BOOST_LOG_TRIVIAL(info) << "Initial mean: " << initial_mean << std::endl << std::endl;
            mean_reached = true;
        }
        else if (mean_reached)
        {
            auto [er_vols, change]{util::abs_differences(prices, er_period_)};
            auto er_vol{util::accumulate(er_vols, 0.0)};
            // Occasionally, the sequence will be constant:
            auto er{er_vol > 0 ? change / er_vol : 0.0};    // leading to er_vol of zero
            auto root_sc{er * sc_factor + sc_sum};
            auto sc{root_sc * root_sc};

            BOOST_LOG_TRIVIAL(info) << "ER: " << er << std::endl;
            BOOST_LOG_TRIVIAL(info) << "SC: " << sc << std::endl;

            // These could be done on the fly but the complexity would distract
            auto mean{util::ma(prices)};

            prices.pop_front();    // Now we have lookback_ prices already, remove the
                                   // oldest

            kamas.emplace_back(kama = kama + sc * (trade_data.price - kama));

            if (kamas.size() > kama_trend_)
            {
                auto [monotonic, up]{util::is_monotonic(std::ranges::subrange{kamas.end() - kama_trend_, kamas.end()})};
                if (not monotonic)    // Signal
                {
                    // @todo This will keep buying/selling when the market is not
                    // directional
                    //       It should have more refined behaviour
                    this->new_order(trade_data.symbol, up ? Side::Buy : Side::Sell, base_quantity_, OrderType::Market);
                }

                this->writeCsv(
                    trade_data.symbol,
                    trade_data.price,
                    toString(trade_data.side).data(),
                    trade_data.size,
                    trade_data.source,
                    trade_data.time,
                    kama,
                    monotonic ? (up ? "Up" : "Down") : "Not monotonic");

                kamas.pop_front();    // Remove oldest KAMA price
            }
        }
    }

    struct Data
    {
        std::deque<Float> prices;
        bool mean_reached;
        Float initial_mean, kama;
        std::deque<Float> kamas;
    };

private:
    const Int lookback_;

    double base_quantity_;
    Int er_period_, fast_sc_, slow_sc_, kama_trend_;

    std::map<std::string, Data> price_structure_;

};

}    // namespace profitview


/* Bubble Dynamics with Chebyshev Spectral Collocation */

#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <string>
#include <chrono>
#include <boost/numeric/odeint.hpp>
#include <Eigen/Core>

using namespace std;
using namespace boost::numeric::odeint;

const double rho_L = 9.970639504998557e+02;
const double p_inf = 1.0e+5;
const double sigma = 0.071977583160056;
const double gamma = 1.33;
const double c_L = 1.497251785455527e+03; // water 25 Celsius
const double mu_L = 8.902125058209557e-04; //25 Celsius
const double lambda = 0.6084; //water 25 Celsius
const double T_inf = 298.15; // 25 Celsius
const int N = 16;

string file_name = "../bubble_sim_rk4_p05_f100_Re10_t5.txt";

typedef double value_type;
typedef vector<value_type> state_type;
typedef Eigen::Matrix<value_type, N/2, N/2> matrix_type;

using namespace boost::numeric;

//ode function of bubble dynamic
class bubble {
public:
	std::vector<value_type> C; //constants of the right hand side
	Eigen::Matrix<value_type, N/2, 1> y; //collocation points (half)
	Eigen::Matrix<value_type, N/2, 1> y_sq; //same, every entry squared
	matrix_type D_E; //Derivative matrix for even functions
	matrix_type D_O; //Derivative matrix for odd functions
	Eigen::Matrix<value_type, 1, N/2> D_E0; //first row of even D matrix

	//p_A pressure amplitude, f pressure frequence, N number of collocation points
	bubble(value_type p_A, value_type f, value_type R_E){
		const double omega = 2*M_PI*f;
		value_type pi2wRE = 2*M_PI/(omega*R_E);

		C = std::vector<value_type>(13);
		C[0] = omega*R_E/(2*M_PI*c_L);
		C[1] = 4*mu_L/(c_L*rho_L*R_E);
		C[2] = 4*mu_L/(rho_L*R_E)*pi2wRE;
		C[3] = 2*sigma*pi2wRE*pi2wRE/(rho_L*R_E);
		C[4] = p_inf/rho_L*pi2wRE*pi2wRE;
		C[5] = p_A/rho_L*pi2wRE*pi2wRE;
		C[6] = pi2wRE*p_inf/(c_L*rho_L);
		C[7] = pi2wRE*p_A/(c_L*rho_L);
		C[8] = 2*M_PI*pi2wRE*p_A/(c_L*rho_L);
		C[9] = lambda*(gamma-1)/gamma*pi2wRE/R_E*T_inf/p_inf;
		C[10] = lambda*(gamma-1)*pi2wRE/R_E*T_inf/p_inf;
		C[11] = (gamma-1)/gamma;
		C[12] = 1.0/(3.0*gamma);

		Eigen::Matrix<value_type, N, 1> y_full(N);
		value_type rec_cpn =  1.0/(N-1);
		for(int i = 0; i < N; i++){
			y_full[i] = cos(M_PI*i*rec_cpn);
		}
		Eigen::Matrix<value_type, N, N> D(N, N);
		for(int i = 0; i < N; i++){
			for(int j = 0; j < N; j++){
				if(i == j){
					if(i == N-1){
						D(N-1,N-1) = -(1+2*(N-1)*(N-1))/6.0;
					}else if(i == 0){
						D(0,0) = (1+2*(N-1)*(N-1))/6.0;
					}else{
						D(i,i) = -y_full[i]/(2.0*(1.0-y_full[i]*y_full[i]));
					}
				}else{
					D(i,j) = std::pow(-1,i+j)*(i==0 || i==N-1?2.0:1.0)
							/((j==0 || j==N-1?2.0:1.0) * (y_full[i]-y_full[j]) );
				}
			}
		}
		D_E = matrix_type(N/2,N/2);
		D_O = matrix_type(N/2,N/2);
		for(int i = 0; i < N/2;i++){
		    for(int j = 0; j < N/2; j++){
		    	D_E(i,j) = D(i,j) + D(i,N-1-j);
		    	if(i==0) D_E0(j) = D_E(i,j);
			}
		}
		for(int i = 0; i < N/2;i++){
			for(int j = 0; j < N/2; j++){
				D_O(i,j) = D(i,j) - D(i,N-1-j);
		    }
		}
		y = y_full.head<N/2>();
		y_sq = y.cwiseProduct(y);
    }

	void operator()(const state_type &x, state_type &dxdt, const value_type t){
	    value_type rec_xR = 1.0 / x[0];
	    value_type rec_xp = 1.0 / x[2];

	    Eigen::Map<const Eigen::Matrix<value_type, N/2,1>> z(x.data()+3);
	    Eigen::Map<Eigen::Matrix<value_type, N/2,1>> dzdt(dxdt.data()+3);

	    Eigen::Matrix<value_type, N/2, 1> De_x = D_E*z; //derivative of z (dimless temperature)

	    //bubble pressure evolution
	    dxdt[2] = 3*rec_xR*(C[10]*rec_xR* De_x[0] - gamma*x[1]*x[2]);

	    //discretized PDE of bubble temperature
	    dzdt = De_x.cwiseProduct( x[1]*rec_xR*y - C[9]*rec_xR*rec_xR*rec_xp* De_x //this might show error, but it will not fail at compile time, valid syntax
	    		+ C[12]*rec_xp*dxdt[2]*y )
	    		+ C[11]*rec_xp*dxdt[2]*z + C[9]*rec_xp*rec_xR*rec_xR* z
				.cwiseProduct(y_sq.cwiseInverse()).cwiseProduct(D_O * (y_sq.cwiseProduct(De_x)));
	    dxdt[3] = 0.0; //Boundary condition

	    //Keller-Miksis equation
	    dxdt[0] = x[1];
	    value_type sin2pit = sin(2*M_PI*t);
	    value_type den = x[0] - C[0]*x[0]*x[1] + C[1];
	    value_type nom = 0.5*C[0] * x[1]*x[1]*x[1] - 1.5* x[1]*x[1] - C[2]*x[1]*rec_xR - C[3]*rec_xR
	    		+ C[4] * x[2] - C[4] - C[5] * sin2pit + C[6] * x[1]*x[2] - C[6] * x[1] - C[7] * x[1]*sin2pit
	            - C[8] * x[0]*cos(2*M_PI*t) + C[6] * x[0]*dxdt[2];
	    dxdt[1] = nom/den;
	}
};

class all_save_observer
{
	ostream &os;
	double file_size = 0;
public:
	int line_number = 0;

	all_save_observer(ostream &output):os(output){}

	void operator()(const state_type &x, const value_type t){
		os << t << ", " << x[0];
		for(int i =1; i < 3+N/2;i++){
			os << ", " << x[i];
		}
		os << endl;
		line_number++;
	}
};

int main() {
	double tolerance = 1e-10;
	cout << "Bubble dynamics started\n" << setprecision(17) << endl;

	typedef runge_kutta_cash_karp54< state_type , value_type , state_type , value_type > stepper_type;

	value_type f = 100e3;
	value_type p_A = 0.5e5;
	value_type R_E = 10e-6;
	state_type x(3+N/2);

	auto t1 = chrono::high_resolution_clock::now();

	bubble bubi(p_A, f, R_E);

	state_type dxdt(3+N/2);

	//auto stepper = euler< state_type , value_type>();
	auto stepper = make_controlled( tolerance , tolerance, stepper_type() );

	ofstream ofs(file_name);
	if(!ofs.is_open())exit(-1);
	ofs.precision(17);
	ofs.flags(ios::scientific);

	all_save_observer observer(ofs);

	//initial conditions
	x[0] = 1.0;
	x[1] = 0.0;
	x[2] = 1.0 + 2.0*sigma/(R_E*p_inf);
	for(int i=0; i < N/2;i++) x[3+i] = 1.0;

	double t_start = 0.0;
	integrate_adaptive(boost::ref(stepper), boost::ref(bubi), x, t_start, 5.0, 1e-5, boost::ref(observer));
	auto t2 = chrono::high_resolution_clock::now();

	cout << "Done" << endl;
	cout << "Time (ms):" << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << endl;

	ofs.flush();
	ofs.close();

	cout << "Ready"<< endl;

	return 0;
}


/**
 * @file linquadfelshaped.cc
 * @brief Creates convergence plots for experiment 3.2.3.10
 * @author Tobias Rohner
 * @date April 2020
 * @copyright MIT License
 */

#define _USE_MATH_DEFINES

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <cmath>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <memory>

namespace po = boost::program_options;

/**
 * @brief Solves the Poisson problem on a given mesh
 * @param mesh The mesh on which to solve the PDE
 * @param fe_space The Finite Element Space to use
 * @returns The dof vector corresponding to the solution of the PDE
 *
 * The problem that is solved has load zero and Dirichlet boundary conditions
 given by
 * \f[
        g(r, \phi) = r^{\frac{2}{3}}\sin(\frac{2}{3}\phi)
   \f]
 */
Eigen::VectorXd solvePoisson(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>>
        &fe_space) {
  // Define the boundary values
  const auto u_bd = [](const Eigen::Vector2d &x) -> double {
    const double r = x.norm();
    double phi = std::atan2(x[1], x[0]);
    if (phi < 0) {
      phi += 2 * M_PI;
    }
    return std::pow(r, 2. / 3) * std::sin(2. / 3 * phi);
  };

  // Initialize the matrix provider
  const lf::mesh::utils::MeshFunctionConstant<double> mf_alpha(1);
  const lf::mesh::utils::MeshFunctionConstant<double> mf_gamma(0);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider element_matrix_provider(
      fe_space, mf_alpha, mf_gamma);

  // Assemble the system matrix (RHS is zero because we have no load)
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
  lf::assemble::COOMatrix<double> A_COO(dofh.NumDofs(), dofh.NumDofs());
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  std::cout << "\t\t> Assembling System Matrix" << std::endl;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, element_matrix_provider,
                                      A_COO);

  // Enforce the dirichlet boundary conditions
  std::cout << "\t\t> Enforcing Boundary Conditions" << std::endl;
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);
  const auto selector = [&](unsigned int idx) -> std::pair<bool, double> {
    const lf::mesh::Entity &entity = dofh.Entity(idx);
    if (!boundary(entity)) {
      return {false, 0};
    }
    const lf::geometry::Geometry *geom = entity.Geometry();
    // Find out where the evaluation node corresponding to the DOF is
    const auto *const shape_function_layout =
        fe_space->ShapeFunctionLayout(entity.RefEl());
    const auto num_dofs = dofh.NumLocalDofs(entity);
    const auto glob_dof_idxs = dofh.GlobalDofIndices(entity);
    int dof_idx;
    for (dof_idx = 0; dof_idx < num_dofs; ++dof_idx) {
      if (glob_dof_idxs[dof_idx] == idx) {
        break;
      }
    }
    const Eigen::VectorXd eval_node =
        shape_function_layout->EvaluationNodes().col(dof_idx);
    const Eigen::Vector2d pos = geom->Global(eval_node);
    // Return the value on the boundary at the position of the evaluation node
    // corresponding to the dof
    return {true, u_bd(pos)};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A_COO, rhs);

  // Solve the LSE using Cholesky decomposition
  std::cout << "\t\t> Solving LSE" << std::endl;
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();
  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(A);
  Eigen::VectorXd solution = solver.solve(rhs);

  // Return the resulting solution vector
  return solution;
}

int main(int argc, char *argv[]) {
  const int num_meshes = 7;

  po::options_description desc("allowed options");
  desc.add_options()("output,o", po::value<std::string>(),
                     "Name of the output file");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("output") == 0) {
    std::cout << desc << std::endl;
    exit(1);
  }
  const std::string output_file = vm["output"].as<std::string>();

  // The analytic solution
  const auto u = [](const Eigen::Vector2d &x) -> double {
    const double r = x.norm();
    double phi = std::atan2(x[1], x[0]);
    if (phi < 0) {
      phi += 2 * M_PI;
    }
    return std::pow(r, 2. / 3) * std::sin(2. / 3 * phi);
  };
  lf::mesh::utils::MeshFunctionGlobal mf_u(u);
  // The gradient of the analytic solution
  const auto u_grad = [](const Eigen::Vector2d &x) -> Eigen::Vector2d {
    const double r = x.norm();
    double phi = std::atan2(x[1], x[0]);
    if (phi < 0) {
      phi += 2 * M_PI;
    }
    Eigen::Vector2d grad;
    grad[0] = 2. / 3 * std::pow(r, -4. / 3) *
              (x[0] * std::sin(2. / 3 * phi) - x[1] * std::cos(2. / 3 * phi));
    grad[1] = 2. / 3 * std::pow(r, -4. / 3) *
              (x[1] * std::sin(2. / 3 * phi) + x[0] * std::cos(2. / 3 * phi));
    return grad;
  };
  lf::mesh::utils::MeshFunctionGlobal mf_u_grad(u_grad);

  const std::filesystem::path here = __FILE__;
  const std::filesystem::path mesh_folder = here.parent_path() / "meshes";
  Eigen::MatrixXd results(num_meshes, 5);
  for (int mesh_idx = 0; mesh_idx < num_meshes; ++mesh_idx) {
    std::cout << "> Mesh Nr. " << mesh_idx << std::endl;

    // Load the mesh
    std::cout << "\t> Loading Mesh" << std::endl;
    const std::string mesh_name = "L" + std::to_string(mesh_idx) + ".msh";
    const std::filesystem::path mesh_file = mesh_folder / mesh_name;
    auto factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    const lf::io::GmshReader reader(std::move(factory), mesh_file.string());
    const auto mesh = reader.mesh();

    // Solve the problem with linear finite elements
    std::cout << "\t> Linear Lagrangian FE";
    const auto fe_space_o1 =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
    std::cout << " (" << fe_space_o1->LocGlobMap().NumDofs() << " DOFs)"
              << std::endl;
    const Eigen::VectorXd solution_o1 = solvePoisson(mesh, fe_space_o1);
    const lf::fe::MeshFunctionGradFE<double, double> mf_grad_o1(fe_space_o1,
                                                                solution_o1);

    // Solve the problem with quadratic finite elements
    std::cout << "\t> Quadratic Lagrangian FE";
    const auto fe_space_o2 =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO2<double>>(mesh);
    std::cout << " (" << fe_space_o2->LocGlobMap().NumDofs() << " DOFs)"
              << std::endl;
    const Eigen::VectorXd solution_o2 = solvePoisson(mesh, fe_space_o2);
    const lf::fe::MeshFunctionGradFE<double, double> mf_grad_o2(fe_space_o2,
                                                                solution_o2);

    // Compute the errors
    std::cout << "\t> Computing Error Norms" << std::endl;
    const auto quadrule_provider = [](const lf::mesh::Entity &entity) {
      return lf::quad::make_QuadRule(entity.RefEl(), 6);
    };
    const double H1_err_o1 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_grad_o1 - mf_u_grad),
        quadrule_provider));
    const double H1_err_o2 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_grad_o2 - mf_u_grad),
        quadrule_provider));

    // Store the mesh width, the number of DOFs and the errors
    results(mesh_idx, 0) = std::sqrt(1. / mesh->NumEntities(0));
    results(mesh_idx, 1) = fe_space_o1->LocGlobMap().NumDofs();
    results(mesh_idx, 2) = fe_space_o2->LocGlobMap().NumDofs();
    results(mesh_idx, 3) = H1_err_o1;
    results(mesh_idx, 4) = H1_err_o2;
  }

  // Output the resulting errors to a file
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream file;
  file.open(output_file);
  file << results.format(CSVFormat);
  file.close();

  return 0;
}


#ifndef _COCONUT_PULP_MATH_VECTOR_HPP_
#define _COCONUT_PULP_MATH_VECTOR_HPP_

#include <cmath>
#include <cassert>
#include <array>
#include <algorithm>
#include <functional>
#include <memory>
#include <numeric>
#include <iosfwd>
#include <initializer_list>
#include <type_traits>

#include <boost/operators.hpp>

#include "coconut-tools/utils/InfixOstreamIterator.hpp"
#include "ScalarEqual.hpp"

namespace coconut {
namespace pulp {
namespace math {

template <class ScalarType, size_t DIMENSIONS_PARAM, class ScalarEqualityFunc = ScalarEqual<ScalarType>>
class Vector :
	boost::equality_comparable<Vector<ScalarType, DIMENSIONS_PARAM, ScalarEqualityFunc>,
	boost::additive<Vector<ScalarType, DIMENSIONS_PARAM, ScalarEqualityFunc>,
	boost::multiplicative<Vector<ScalarType, DIMENSIONS_PARAM, ScalarEqualityFunc>, ScalarType
	>>>
{
public:

	using Scalar = ScalarType;

	static const auto DIMENSIONS = DIMENSIONS_PARAM;

	using Elements = std::array<Scalar, DIMENSIONS>;

	// --- CONSTRUCTORS AND OPERATORS

	template <
		class... CompatibleTypes,
		class = std::enable_if_t<sizeof...(CompatibleTypes) == DIMENSIONS || sizeof...(CompatibleTypes) == 0>
		>
	explicit constexpr Vector(CompatibleTypes&&... values) noexcept :
		elements_{ std::forward<CompatibleTypes>(values)... }
	{
		static_assert(sizeof...(values) == DIMENSIONS || sizeof...(values) == 0, "Bad number of arguments");
	}

	template <
		class CompatibleScalarType,
		size_t OTHER_DIMENSIONS,
		class OtherScalarEqualityFunc,
		class... TailTypes,
		class = std::enable_if_t<sizeof...(TailTypes) + OTHER_DIMENSIONS == DIMENSIONS>
		>
	explicit constexpr Vector(
		const Vector<CompatibleScalarType, OTHER_DIMENSIONS, OtherScalarEqualityFunc>& other,
		TailTypes&&... tail
		)
	{
		std::copy(other.elements().begin(), other.elements().end(), elements_.begin());
		setTail_(elements_, OTHER_DIMENSIONS, std::forward<TailTypes>(tail)...);
	}

	// TODO: consider removing std::initializer_list constructor, as the number of values cannot be
	// checked at compile-time
	constexpr Vector(std::initializer_list<Scalar> values) noexcept {
		assert(values.size() == DIMENSIONS || values.size() == 0);
		std::copy(values.begin(), values.end(), elements_.begin());
		std::uninitialized_fill(
			elements_.begin() + values.size(),
			elements_.end(),
			Scalar(0)
			);
	}

	friend std::ostream& operator<<(std::ostream& os, const Vector& vector) {
		os << '<';
		std::copy(vector.elements_.begin(), vector.elements_.end(),
			coconut_tools::InfixOstreamIterator<ScalarType>(os, ", "));
		os << '>';
		return os;
	}

	friend bool operator==(const Vector& lhs, const Vector& rhs) noexcept {
		return std::equal(lhs.elements_.begin(), lhs.elements_.end(), rhs.elements_.begin(), ScalarEqualityFunc());
	}

	Vector& operator+=(const Vector& other) noexcept {
		std::transform(elements_.begin(), elements_.end(), other.elements_.begin(), elements_.begin(), std::plus<>());
		return *this;
	}

	Vector& operator-=(const Vector& other) noexcept {
		std::transform(elements_.begin(), elements_.end(), other.elements_.begin(), elements_.begin(), std::minus<>());
		return *this;
	}

	Vector operator-() const noexcept {
		auto result = Vector();
		std::transform(elements_.begin(), elements_.end(), result.elements_.begin(), std::negate<>());
		return result;
	}

	Vector& operator*=(Scalar scalar) noexcept {
		std::transform(elements_.begin(), elements_.end(), elements_.begin(), [scalar](auto element) {
				return element * scalar;
			});
		return *this;
	}

	Vector& operator/=(Scalar scalar) noexcept {
		std::transform(elements_.begin(), elements_.end(), elements_.begin(), [scalar](auto element) {
			return element / scalar;
		});
		return *this;
	}

	// --- CONVERTERS

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 2), Vector<Scalar, 3, ScalarEqualityFunc>>
		xyz() const noexcept
	{
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return Vector<Scalar, 3, ScalarEqualityFunc>(x(), y(), z());
	}

	// --- VECTOR-SPECIFIC OPERATIONS

	Scalar dot(const Vector& other) const noexcept {
		return std::inner_product(elements_.begin(), elements_.end(), other.elements_.begin(), Scalar(0));
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_ == 3), Vector> cross(const Vector& other) const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return {
			(y() * other.z()) - (z() * other.y()),
			(z() * other.x()) - (x() * other.z()),
			(x() * other.y()) - (y() * other.x())
			};
	}

	Scalar length() const noexcept {
		using std::sqrt;
		return sqrt(lengthSq());
	}

	Scalar lengthSq() const noexcept {
		return dot(*this);
	}

	Vector& normalise() noexcept {
		const auto l = length();
		if (l > Scalar(0)) {
			*this /= l;
		}
		return *this;
	}

	Vector normalised() const noexcept {
		auto result = *this;
		return result.normalise();
	}

	// --- ACCESSORS

	constexpr const Scalar& operator[](size_t index) const noexcept {
		assert(index < DIMENSIONS);
		return elements_[index];
	}

	Scalar& operator[](size_t index) noexcept {
		assert(index < DIMENSIONS);
		return elements_[index];
	}

	template <size_t INDEX>
	constexpr std::enable_if_t<(DIMENSIONS > INDEX), const Scalar&> get() const noexcept {
		return elements_[INDEX];
	}

	template <size_t INDEX>
	std::enable_if_t<(DIMENSIONS > INDEX), Scalar&> get() noexcept {
		return elements_[INDEX];
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_> 0), const Scalar&> x() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<0>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 0), Scalar&> x() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<0>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_> 1), const Scalar&> y() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<1>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 1), Scalar&> y() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<1>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_ > 2), const Scalar&> z() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<2>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 2), Scalar&> z() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<2>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_> 3), const Scalar&> w() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<3>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 3), Scalar&> w() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<3>();
	}

	constexpr const Elements& elements() const noexcept {
		return elements_;
	}

private:

	Elements elements_;

	template <class HeadType, class... TailTypes>
	static constexpr void setTail_(
		Elements& elems,
		size_t index,
		HeadType&& head,
		TailTypes&&... tailTypes
		) noexcept
	{
		elems[index] = std::forward<HeadType>(head);
		setTail_(elems, index + 1, std::forward<TailTypes>(tailTypes)...);
	}

	static constexpr void setTail_(Elements&, size_t) noexcept {
	}

};

template <class S, size_t D, class SEF>
S dot(const Vector<S, D, SEF>& lhs, const Vector<S, D, SEF>& rhs) noexcept {
	return lhs.dot(rhs);
}

template <class S, size_t D, class SEF>
constexpr std::enable_if_t<D == 3, Vector<S, D, SEF>>
	cross(const Vector<S, D, SEF>& lhs, const Vector<S, D, SEF>& rhs) noexcept
{
	return lhs.cross(rhs);
}

using Vec2 = Vector<float, 2>;
static_assert(sizeof(Vec2) == sizeof(float) * 2, "Empty base optimisation didn't work");
static_assert(std::is_trivially_copyable<Vec2>::value, "Vector is not trivially copiable");

using Vec3 = Vector<float, 3>;
static_assert(sizeof(Vec3) == sizeof(float) * 3, "Empty base optimisation didn't work");
static_assert(std::is_trivially_copyable<Vec3>::value, "Vector is not trivially copiable");

using Vec4 = Vector<float, 4>;
static_assert(sizeof(Vec4) == sizeof(float) * 4, "Empty base optimisation didn't work");
static_assert(std::is_trivially_copyable<Vec4>::value, "Vector is not trivially copiable");

} // namespace math

// TODO: re-enable when primitive::Vector is removed
// using math::Vector;
using math::Vec2;
using math::Vec3;
using math::Vec4;

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_MATH_VECTOR_HPP_ */


#include <iostream>
#include <cassert>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/max_cardinality_matching.hpp>
#include <string>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;

void testcase()
{
  int w, h;
  std::cin >> w >> h;
  assert(w >= 1 && w <= 50 && h >= 1 && h <= 50);

  std::vector<std::vector<bool>> should_tile_at_cell(h, std::vector<bool>(w, false));
  int num_nodes = w * h;
  int num_nodes_to_cover = 0;
  for (int y = 0; y < h; y++)
  {
    std::string row;
    std::cin >> row;
    assert(int(row.size()) == w);
    for (int x = 0; x < w; x++)
    {
      if (row.at(x) == '.')
      {
        num_nodes_to_cover++;
        should_tile_at_cell.at(y).at(x) = true;
      }
    }
  }

  if (num_nodes_to_cover % 2 != 0)
  {
    DEBUG(2, "fast path");
    std::cout << "no\n";
    return;
  }

  Graph G(num_nodes);
  auto node_at_y_x = [w, h, num_nodes](int y, int x) -> Graph::vertex_descriptor {
    assert(x >= 0 && x < w && y >= 0 && y < h);
    int node = y * w + x;
    assert(node >= 0 && node < num_nodes);
    return node;
  };

  for (int y = 0; y < h; y++)
  {
    for (int x = 0; x < w; x++)
    {
      if (!should_tile_at_cell.at(y).at(x))
      {
        continue;
      }
      if (y > 0 && should_tile_at_cell.at(y - 1).at(x))
      {
        DEBUG(3, "adding vertical edge " << y << " " << x);
        boost::add_edge(node_at_y_x(y - 1, x), node_at_y_x(y, x), G);
      }
      if (x > 0 && should_tile_at_cell.at(y).at(x - 1))
      {
        DEBUG(3, "adding horizontal edge " << y << " " << x);
        boost::add_edge(node_at_y_x(y, x - 1), node_at_y_x(y, x), G);
      }
    }
  }

  std::vector<Graph::vertex_descriptor> mate(num_nodes);
  assert(boost::checked_edmonds_maximum_cardinality_matching(G, mate.data()));
  int num_nodes_covered = 2 * boost::matching_size(G, mate.data());
  DEBUG(2, "num_nodes_covered " << num_nodes_covered);
  assert(num_nodes_covered >= 0 && num_nodes_covered <= num_nodes_to_cover);
  std::cout << (num_nodes_covered == num_nodes_to_cover ? "yes\n" : "no\n");
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  assert(t >= 0 && t <= 100);
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// standard_error_autocorrelated.hpp                                         //
//                                                                           //
//  Copyright 2008 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_STANDARD_ERROR_AUTOCORRELATED_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_STANDARD_ERROR_AUTOCORRELATED_HPP_ER_2008_04
#include <cmath>

#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/call_traits.hpp>
//#include <boost/assert.hpp>
#include <boost/array.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

#include <boost/accumulators/statistics/integrated_acvf.hpp>
#include <boost/accumulators/statistics/count.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // standard_error_autocorrelated
    template<typename T,typename I>
    class standard_error_autocorrelated_impl
      : public accumulator_base
    {
    public:
        typedef T result_type;

        standard_error_autocorrelated_impl(dont_care)
        {}

        template<typename Args>
        void operator()(Args const &args)
        {
            T iacv = integrated_acvf<I>(args[accumulator]);
            T n = (T)(count(args));
            val = static_cast<T>(0);
            if((iacv>static_cast<T>(0)) && (n>static_cast<T>(0))){val = sqrt(iacv/n);}//also = sqrt(acv0/ess)
        }

        result_type result(dont_care) const
        {
            return val;
        }
    private:
        T val;
    };

} // namespace impl
///////////////////////////////////////////////////////////////////////////////
// tag::integrated_acvf
//

namespace tag
{
    template <typename I = default_delay_discriminator>
    struct standard_error_autocorrelated
      : depends_on<count,integrated_acvf<I> >
    {
        /// INTERNAL ONLY
      typedef
        accumulators::impl::standard_error_autocorrelated_impl<
            mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::standard_error_autocorrelated
//

namespace extract
{

//  extractor<tag::standard_error_autocorrelated<> >
//    const standard_error_autocorrelated = {};

  // see acvf about default_delay_discriminator
  template<typename I,typename AccumulatorSet>
  typename mpl::apply<
    AccumulatorSet,tag::standard_error_autocorrelated<I>
    >::type::result_type
  standard_error_autocorrelated(AccumulatorSet const& acc){
    typedef tag::standard_error_autocorrelated<I> the_tag;
    return extract_result<the_tag>(acc);
  }

//  TODO
//  overload (default) see acvf

}

using extract::standard_error_autocorrelated;

}}

#endif


/*
 * This file is part of MXE.
 * See index.html for further information.
 */

#include <armadillo>

using namespace arma;

int main()
{
    mat A = randu<mat>(50,50);
    mat B = trans(A)*A;  // generate a symmetric matrix

    vec eigval;
    mat eigvec;

    // use standard algorithm by default
    eig_sym(eigval, eigvec, B);

    // use divide & conquer algorithm
    eig_sym(eigval, eigvec, B, "dc");
    return 0;
}


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

using namespace Eigen;
using namespace std;

int main() {
  ArrayXXf a(3, 3);
  ArrayXXf b(3, 3);
  a << 1, 2, 3,
      4, 5, 6,
      7, 8, 9;
  b << 1, 2, 3,
      1, 2, 3,
      1, 2, 3;

  // Adding two arrays
  cout << "a + b = " << endl << a + b << endl << endl;

  // Subtracting a scalar from an array
  cout << "a - 2 = " << endl << a - 2 << endl;
}


#pragma once
#include "Optimizer.hpp"

#include <Eigen/Dense>
#include <array>
#include <nlohmann/json.hpp>

namespace yavque
{
class SGDMomentum : public Optimizer
{
public:
	static constexpr std::array<double, 4> DEFAULT_PARAMS = {0.01, 0.0, 0.9, 1e-4};

private:
	double alpha_;
	double p_;
	double gamma_;
	double min_alpha_;

	Eigen::VectorXd m_;
	int t_ = 0;

public:
	explicit SGDMomentum(double alpha = DEFAULT_PARAMS[0], double p = DEFAULT_PARAMS[1],
	                     double gamma = DEFAULT_PARAMS[2],
	                     double min_alpha = DEFAULT_PARAMS[3])
		: alpha_{alpha}, p_{p}, gamma_{gamma}, min_alpha_{min_alpha}
	{
	}

	explicit SGDMomentum(const nlohmann::json& params)
		: alpha_{params.value("alpha", DEFAULT_PARAMS[0])},
		  p_{params.value("p", DEFAULT_PARAMS[1])}, gamma_{params.value(
														"gamma", DEFAULT_PARAMS[2])},
		  min_alpha_{params.value("min_alpha", DEFAULT_PARAMS[3])}
	{
	}

	[[nodiscard]] nlohmann::json desc() const override
	{
		return nlohmann::json{{"name", "SGD"},
		                      {"alhpa", alpha_},
		                      {"gamma", gamma_},
		                      {"p", p_},
		                      {"min_alpha", min_alpha_}};
	}

	Eigen::VectorXd getUpdate(const Eigen::VectorXd& v) override
	{
		using std::pow;
		if(t_ == 0)
		{
			m_ = Eigen::VectorXd::Zero(v.size());
		}

		++t_;
		m_ *= gamma_;
		m_ += (1 - gamma_) * v;
		double eta
			= std::max((alpha_ / pow(t_, p_)), min_alpha_) / (1.0 - pow(gamma_, t_));
		return -eta * m_;
	}
};
} // namespace yavque


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/geometry/geometry_compute_distance_to_triangle.h>
#include <cmath> // needed for std::sqrt

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_geometry_util_compute_distance_to_triangle);

  BOOST_AUTO_TEST_CASE(case_by_case_testing)
  {
    using std::sqrt;

    typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
    typedef math_types::vector3_type                         vector3_type;
    typedef math_types::real_type                            real_type;

    real_type tol = 0.0001;

    vector3_type p;
    vector3_type pi(-5,0,0);
    vector3_type pj(5,0,0);
    vector3_type pk(0,5,0);

    p = vector3_type(0,2.5,2);

    real_type d0 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d0, 2.0, tol);

    p = vector3_type(0,2.5,-2);
    real_type d1 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d1, 2.0, tol);

    p = vector3_type(0,7,0);
    real_type d2 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d2, 2.0, tol);

    p = vector3_type(-7,0,0);
    real_type d3 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d3, 2.0, tol);

    p = vector3_type(7,0,0);
    real_type d4 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d4, 2.0, tol);

    p = vector3_type(0,-2,0);
    real_type d5 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d5, 2.0, tol);

    p = vector3_type(5,5,0);
    real_type d6 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d6, 3.5355339059327376220042218105242, tol);

    p = vector3_type(-5,5,0);
    real_type d7 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d7, 3.5355339059327376220042218105242, tol);

    p = vector3_type(-5,-2,0);
    real_type d8 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d8, 2.0, tol);

    p = vector3_type(5,-2,0);
    real_type d9 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d9, 2.0, tol);

    p = vector3_type(6,1,0);
    real_type d10 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d10, sqrt(2.0), tol );

    p = vector3_type(-6,1,0);
    real_type d11 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d11, sqrt(2.0), tol );

    p = vector3_type(1,6,0);
    real_type d12 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d12, sqrt(2.0), tol );

    p = vector3_type(-1,6,0);
    real_type d13 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d13, sqrt(2.0), tol );

    p = vector3_type(0,0,1);
    real_type d14 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d14, 1.0, tol );

    p = vector3_type(5,0,1);
    real_type d15 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d15, 1.0, tol );

    p = vector3_type(-5,0,1);
    real_type d16 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d16, 1.0, tol );

    p = vector3_type(0,5,1);
    real_type d17 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d17, 1.0, tol );

    p = vector3_type(2.5,2.5,1);
    real_type d18 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d18, 1.0, tol );

    p = vector3_type(-2.5,2.5,1);
    real_type d19 = OpenTissue::geometry::compute_distance_to_triangle(p,pi,pj,pk);
    BOOST_CHECK_CLOSE(d19, 1.0, tol );

  }

BOOST_AUTO_TEST_SUITE_END();


//////////////////////////////////////////////////////////////////////////////
// random::poisson_ext::poisson_devroye::detail::parameters.hpp         	//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_PARAMETERS_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_PARAMETERS_HPP_ER_2010
#include <boost/random/poisson_ext/devroye/detail/math.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace detail{

    // These are quantities that remain fixed throughout sampling and are only
    // modified if the poisson mean is changed.
	template<typename Int,typename T, typename P>
    struct parameters{

		typedef devroye::detail::math<Int,T,P> ma_;

    	public:

        typedef Int result_type;
        typedef T input_type;

        parameters(){}
    	explicit parameters(const Int& mean)
        {
			this->m1_ 		= 	ma_::to_float(mean);
			this->m2_ 		= 	ma_::to_float(2*mean);
			input_type m8 	= 	ma_::to_float(8*mean);
            input_type m32 	= 	ma_::to_float(32*mean);

            this->c1_ = ma_::to_float(1)/m8; // Equation (6)

            // Equation (10)
            this->delta_ 	= 	ma_::log1p( m32/ma_::pi(), P() );
            this->delta_ 	= 	ma_::sqrt(this->delta() * this->m1());

			// Below equation (7)
			this->sd1_ = ma_::sqrt(
            	this->m1() + this->delta() / ma_::to_float(2)
            );
			this->shape2_ = this->delta() / (this->m2()+this->delta());

            input_type a1, a2, a3, a;

			a1 = ma_::sqrt( ma_::pi() * (this->m2() + this->delta()) );
            a1 *= ma_::exp(this->c1());
            a2 = ma_::exp(-(this->delta()+ma_::to_float(1)) * this->shape2());
            a2 /= this->shape2();
            a3 = ma_::to_float(1);
            a = a1 + a2 + a3;
            this->p1_ = a1 / a;
            this->p2_ = a2 / a;
            this->p3_ = a3 / a;

			// Reconciliation with Fortran code
            // RL					m1
            // TWO					m2
			// CON 					c1
            // D 					delta
            // D2					delta + m2
			// D3 					1 / shape2
            // STDDEV 				sd1
            // SUM 					a1 + a2 + a3
            // PBODY 				p3 = a3/(a+a2+a3)
			// PTAIL 				p2 + p3

        }

		std::ostream& parameters_description(std::ostream& os)const{
        	return os
                << '('
                << "m1 = " 		<< this->m1()
                << ','
                << " m2 = "		<< this->m2()
                << ','
                << " delta = " 	<< this->delta()
                << ','
                << " loc1 = "	<< loc1()
                << ','
                << " sd1 = " 	<< this->sd1()
                << ','
                << " shape2 = "	<< this->shape2()
                << ','
                << " c1 = "		<< this->c1()
                << ','
                << " p1 = "		<< this->p1()
                << ','
                << " p2 = "		<< this->p2()
                << ','
                << " p3 = "		<< this->p3()
                << ')';
        }

		const input_type& m1()const{ return this->m1_; }
		const input_type& m2()const{ return this->m2_; }
		const input_type& delta()const{ return this->delta_; }
		static input_type loc1(){ return -ma_::to_float(1)/ma_::to_float(2); }
		const input_type& sd1()const{ return this->sd1_; }
		const input_type& shape2()const{ return this->shape2_; }
        const input_type& c1()const{ return this->c1_; }
		const input_type& p1()const{ return this->p1_; }
		const input_type& p2()const{ return this->p2_; }
		const input_type& p3()const{ return this->p3_; }

		private:
        input_type inv_m1_;
		input_type m1_;
		input_type m2_;
        input_type delta_;
		input_type sd1_;
		input_type shape2_;

        input_type c1_;
        input_type p1_;
        input_type p2_;
		input_type p3_;

    };

}// detail
}// devroye
}// poisson
}// random
}// boost

#endif


#include <iostream>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>

#include "../../../core/format_helper.h"
#include "../../../core/math_helper.h"

void SavePointCloud(const cv::Mat& img, const cv::Mat& disp_img, const double* calib)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

  Eigen::Vector4d pt3d;
  double min_disp = 1.0;
  for (int y = 0; y < disp_img.rows; y++) {
    for (int x = 0; x < disp_img.cols; x++) {
      double disp = static_cast<double>(disp_img.at<uint16_t>(y,x)) / 256.0;
      //std::cout << "D = " << disp << "\n";
      if (disp > min_disp) {
        core::MathHelper::Triangulate(calib, x, y, disp, pt3d);
        pcl::PointXYZRGB point;
        point.x = pt3d[0];
        point.y = pt3d[1];
        point.z = pt3d[2];
        if (img.channels() == 3) {
          point.b = img.at<cv::Vec3b>(y,x)[0];
          point.g = img.at<cv::Vec3b>(y,x)[1];
          point.r = img.at<cv::Vec3b>(y,x)[2];
        }
        else if (img.channels() == 1) {
          point.b = img.at<uint8_t>(y,x);
          point.g = img.at<uint8_t>(y,x);
          point.r = img.at<uint8_t>(y,x);
        }
        else throw 1;
        point_cloud->points.push_back(point);
      }
    }
  }
  point_cloud->width = point_cloud->points.size();
  point_cloud->height = 1;
  pcl::io::savePCDFile("pcl.pcd", *point_cloud);
}

int main(int argc, char** argv)
{
  if (argc != 4) {
    std::cerr << "Usage:\n\t" << argv[0] << " img disp_img calib_file\n";
    return 1;
  }
  std::string img_path = argv[1];
  std::string dispimg_path = argv[2];
  std::string calib_path = argv[3];
  double mono_cam[5];
  double color_cam[5];
  core::FormatHelper::ReadCalibKitti(calib_path, mono_cam, color_cam);

  cv::Mat img = cv::imread(img_path, CV_LOAD_IMAGE_COLOR);
  cv::Mat disp_img = cv::imread(dispimg_path, CV_LOAD_IMAGE_ANYDEPTH);
  std::cout << "Bytes per pixel = " << disp_img.step / disp_img.cols << "\n";

  if (img.channels() == 3)
    SavePointCloud(img, disp_img, color_cam);
  else if (img.channels() == 1)
    SavePointCloud(img, disp_img, mono_cam);
  else throw 1;

  return 0;
}


#include <iostream>

#include <harp_test.hpp>

#include <boost/random.hpp>

using namespace std;
using namespace harp;

void harp::test_tinyKLT ( string const & datadir ) {

  cerr << "Testing tiny KLT transform..." << endl;

  cerr.precision(16);

  // construct synthetic design matrix

  size_t nbins = 25;
  size_t npix = 225;

  mat_compcol projmat ( npix, nbins );

  // loop over traces
  for ( size_t i = 0; i < 5; ++i ) {

    // loop over bins
    for ( size_t j = 0; j < 5; ++j ) {
      size_t pixrow = 3 * j + 1;
      size_t pixcol = 3 * i + 1;

      // center pixel = 100
      projmat( 15 * pixrow + pixcol, 5 * i + j ) = 100.0;

      // 4 neigbors = 10
      projmat( 15 * (pixrow - 1) + pixcol, 5 * i + j ) = 10.0;
      projmat( 15 * (pixrow + 1) + pixcol, 5 * i + j ) = 10.0;
      projmat( 15 * pixrow + (pixcol - 1), 5 * i + j ) = 10.0;
      projmat( 15 * pixrow + (pixcol + 1), 5 * i + j ) = 10.0;
    }
  }

  // construct fake spectra

  vec_dense truespec ( nbins );
  for ( size_t i = 0; i < 5; ++i ) {
    for ( size_t j = 0; j < 5; ++j ) {
      truespec( 5 * i + j ) = (double)( 4 * (i + 1) * (j + 1) );
    }
  }

  // compute noiseless image

  vec_dense noiseless ( npix );

  boost::numeric::ublas::axpy_prod ( projmat, truespec, noiseless, true );

  // compute noise realization

  vec_dense imgnoise ( npix );
  vec_dense measured ( npix );

  typedef boost::ecuyer1988 base_generator_type;
  base_generator_type generator(42u);

  vec_dense rms ( npix );

  for ( size_t i = 0; i < npix; ++i ) {
    rms[i] = sqrt( 16.0 + noiseless[i] );

    boost::normal_distribution < double > dist ( 0.0, rms[i] );

    boost::variate_generator < base_generator_type&, boost::normal_distribution < double > > gauss ( generator, dist );

    imgnoise[i] = gauss();

    measured[i] = noiseless[i] + imgnoise[i];
  }

  // construct inverse pixel noise covariance

  mat_comprow invnoise ( npix, npix, npix );

  for ( size_t i = 0; i < npix; ++i ) {
    invnoise ( i, i ) = 1.0 / ( rms[i] * rms[i] );
  }

  // construct rhs

  vec_dense z ( nbins );

  noise_weighted_spec ( projmat, invnoise, measured, z );
  //noise_weighted_spec < mat_compcol, mat_comprow, vec_dense > ( projmat, invnoise, measured, z );

  // construct the inverse spectral covariance matrix

  mat_comprow invcov ( nbins, nbins );

  mat_dynrow builder ( nbins, nbins );

  mat_compcol temp ( npix, nbins );

  boost::numeric::ublas::axpy_prod ( invnoise, projmat, temp, true );

  boost::numeric::ublas::axpy_prod ( boost::numeric::ublas::trans ( projmat ), temp, builder, true );

  mat_dynrow::iterator2 itcol;
  mat_dynrow::iterator1 itrow;

  for ( itcol = builder.begin2(); itcol != builder.end2(); ++itcol ) {
    for ( itrow = itcol.begin(); itrow != itcol.end(); ++itrow ) {
      invcov ( itrow.index1(), itrow.index2() ) = (*itrow);
    }
  }

  // extraction

  vec_dense outspec ( nbins );

  extract_dense ( invcov, z, outspec );

  /*
  for ( size_t i = 0; i < 5; ++i ) {
    for ( size_t j = 0; j < 5; ++j ) {
      cerr << "(" << i << "," << j << ") " << truespec( 5 * i + j ) << " " << outspec( 5 * i + j) << " +/- " << 1.0 / sqrt( invcov(5*i+j, 5*i+j) ) << endl;
    }
  }
  */

  string kltspecout = datadir + "/test_klt_spec.out";
  string kltimgout = datadir + "/test_klt_img.out";

  fstream out;
  out.precision(16);

  out.open ( kltimgout.c_str(), ios::out );
  out.precision(16);

  for ( int64_t i = 0; i < npix; ++i ) {
    out << noiseless[i] << " " << imgnoise[i] << " " << measured[i] << endl;
  }

  out.close();

  out.open ( kltspecout.c_str(), ios::out );
  out.precision(16);

  for ( int64_t i = 0; i < nbins; ++i ) {
    out << truespec[i] << " " << outspec[i] << " " << 1.0 / sqrt( invcov(i,i) ) << endl;
  }

  out.close();

  cerr << "  (PASSED)" << endl;

  return;
}


// BLAS level 2

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::cout;
using std::endl;

typedef ublas::vector<double> vct_t;
typedef ublas::matrix<double, ublas::row_major> rm_t;
typedef ublas::matrix<double, ublas::column_major> cm_t;

int main() {

  cout << endl;

  vct_t vx (2);
  vct_t vy (4);

  // row major matrix
  rm_t rm (2, 4);
  init_m (rm, const_val<double> (0));
  print_m (rm, "row major matrix m");
  cout << endl;

  vx(0) = 1.;
  vy(1) = 1.;
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  atlas::ger (vx, vy, rm);
  print_m (rm, "m += x y^T");
  cout << endl << endl;

  init_m (rm, const_val<double> (1));
  print_m (rm, "m");
  cout << endl;

  atlas::set (1., vx);
  atlas::set (1., vy);
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += 2 x y^T
  atlas::ger (2., vx, vy, rm);
  print_m (rm, "m += 2 x y^T");
  cout << endl << endl;

  init_v (vx, iplus1());
  init_v (vy, iplus1());
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  atlas::ger (vx, vy, rm);
  print_m (rm, "m += x y^T");
  cout << endl << endl;

  // column major matrix
  cm_t cm (2, 4);
  init_m (cm, const_val<double> (0));
  print_m (cm, "column major matrix m");
  cout << endl;

  vx(0) = 1.;
  vy(1) = 1.;
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  atlas::ger (vx, vy, cm);
  print_m (cm, "m += x y^T");
  cout << endl << endl;

  init_m (cm, const_val<double> (1));
  print_m (cm, "m");
  cout << endl;

  atlas::set (1., vx);
  atlas::set (1., vy);
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += 2 x y^T
  atlas::ger (2., vx, vy, cm);
  print_m (cm, "m += 2 x y^T");
  cout << endl << endl;

  init_v (vx, iplus1());
  init_v (vy, iplus1());
  print_v (vx, "vx");
  cout << endl;
  print_v (vy, "vy");
  cout << endl;

  // m += x y^T
  atlas::ger (vx, vy, cm);
  print_m (cm, "m += x y^T");
  cout << endl << endl;

}


/*!
 * @file
 * @brief
 * @copyright alphya 2018-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_VALUE_INCLUDING_ERROR_HPP
#define NYARUGA_UTIL_VALUE_INCLUDING_ERROR_HPP

#pragma once

#include <nyaruga_util/partial_diff.hpp>
#include <nyaruga_util/diff.hpp>
#include <boost/hana/functional/arg.hpp>

namespace nyaruga {

namespace util {

// 誤差を含む値
struct value_including_error {
   num_t value; // 値
   num_t error; // 誤差
   operator num_t() { return value; }
   operator num_t() const { return value; }
};

namespace nyaruga_util_impl {

// 偏微分^2 * 誤差^2 を、一変数分だけ計算して、再帰
template <std::size_t count, std::size_t max, typename F, typename... Args>
decltype(auto) generate_error_impl(F && f, Args &&... args)
{
   if constexpr (count > max)
      return num_t{ 0 };
   else {
      auto && current_val_obj = boost::hana::arg<count>(args...);
      auto && current_val_error = current_val_obj.error;
      auto && partial_diff = nyaruga::util::partial_diff<count>(f)(static_cast<num_t>(args)...);
      auto && result = boost::multiprecision::pow(partial_diff, 2) *
                       boost::multiprecision::pow(current_val_error, 2);
      return static_cast<num_t>(result + generate_error_impl<count + 1, max>(std::forward<F>(f), std::forward<Args>(args)...));
   }
}

} // namespace nyaruga_util_impl

// 誤差の伝播式より、計算。誤差を返す
// Args には、value_including_errorを使用してください
template <typename F, typename... Args>
decltype(auto) generate_error(F && f, Args &&... args)
{
   return static_cast<num_t>(boost::multiprecision::pow(
      nyaruga_util_impl::generate_error_impl<1, sizeof...(args)>(std::forward<F>(f), std::forward<decltype(args)>(args)...), 0.5l));
}

} // namespace nyaruga_util

/*
#include "value_including_error.hpp"
#include <iostream>
auto main() -> int
{
   // 誤差を含む値の定義 { 値, 誤差 }
   auto&& x = nyaruga::util::value_including_error{ 20, 0.5 };
   auto&& y = nyaruga::util::value_including_error{ 50, 0.8 };
   // 誤差を含む値の関数定義　これは単純な掛け算
   decltype(auto) f = [](nyaruga::util::num_t x, nyaruga::util::num_t y) {
      return static_cast<nyaruga::util::num_t>(x * y);
   };
   // 普通に関数を実行した値
   auto&& v = f(x, y);
   // 誤差を計算
   auto&& e = nyaruga::util::generate_error(f, x, y);
   // だいたいあってる程度の値です
   // 単純な掛け算で、有効数字17桁くらいでした
   std::cout << std::setprecision(18)
      << "値は：" << v << "\n誤差は：" << e << "\n";
}
*/

#endif // #ifndef NYARUGA_UTIL_VALUE_INCLUDING_ERROR_HPP


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

    This is an example illustrating the use of the Hough transform tool in the
    dlib C++ Library.

    In this example we are going to draw a line on an image and then use the
    Hough transform to detect the location of the line.  Moreover, we do this in
    a loop that changes the line's position slightly each iteration, which gives
    a pretty animation of the Hough transform in action.
*/

#include <dlib/gui_widgets.h>
#include <dlib/image_transforms.h>

using namespace dlib;

int main()
{
    // First let's make a 400x400 image.  This will form the input to the Hough transform.
    array2d<unsigned char> img(400,400);
    // Now we make a hough_transform object.  The 300 here means that the Hough transform
    // will operate on a 300x300 subwindow of its input image.
    hough_transform ht(300);

    image_window win, win2;
    double angle1 = 0;
    double angle2 = 0;
    while(true)
    {
        // Generate a line segment that is rotating around inside the image.  The line is
        // generated based on the values in angle1 and angle2. So each iteration creates a
        // slightly different line.
        angle1 += pi/130;
        angle2 += pi/400;
        const point cent = center(get_rect(img));
        // A point 90 pixels away from the center of the image but rotated by angle1.
        const point arc = rotate_point(cent, cent + point(90,0), angle1);
        // Now make a line that goes though arc but rotate it by angle2.
        const point l = rotate_point(arc, arc + point(500,0), angle2);
        const point r = rotate_point(arc, arc - point(500,0), angle2);

        // Next, blank out the input image and then draw our line on it.
        assign_all_pixels(img, 0);
        draw_line(img, l, r, 255);

        const point offset(50,50);
        array2d<int> himg;
        // pick the window inside img on which we will run the Hough transform.
        const rectangle box = translate_rect(get_rect(ht),offset);
        // Now let's compute the hough transform for a subwindow in the image.  In
        // particular, we run it on the 300x300 subwindow with an upper left corner at the
        // pixel point(50,50).  The output is stored in himg.
        ht(img, box, himg);
        // Now that we have the transformed image, the Hough image pixel with the largest
        // value should indicate where the line is.  So we find the coordinates of the
        // largest pixel:
        point p = max_point(mat(himg));
        // And then ask the ht object for the line segment in the original image that
        // corresponds to this point in Hough transform space.
        std::pair<point,point> line = ht.get_line(p);

        // Finally, let's display all these things on the screen.  We copy the original
        // input image into a color image and then draw the detected line on top in red.
        array2d<rgb_pixel> temp;
        assign_image(temp, img);
        // Note that we must offset the output line to account for our offset subwindow.
        // We do this by just adding in the offset to the line endpoints.
        draw_line(temp, line.first+offset, line.second+offset, rgb_pixel(255,0,0));
        win.clear_overlay();
        win.set_image(temp);
        // Also show the subwindow we ran the Hough transform on as a green box.  You will
        // see that the detected line is exactly contained within this box and also
        // overlaps the original line.
        win.add_overlay(box, rgb_pixel(0,255,0));

        // We can also display the Hough transform itself using the jet color scheme.
        win2.set_image(jet(himg));
    }
}


#include <bits/stdc++.h>
#include <boost/tokenizer.hpp>

int64_t departure_time;
std::vector<int64_t> buses;
std::vector<std::pair<int64_t, int64_t>> buses_ids;

struct Euclid {
  int64_t mi, mj;
};

int64_t next_time(const int64_t bus, int64_t departure_time) {
  return bus * (departure_time / bus + 1) % departure_time;
}

void part1() {
  auto comp = [&] (const int64_t b1, const int64_t b2) {
    return next_time(b1, departure_time) < next_time(b2, departure_time);
  };
  int64_t best_bus = *(std::min_element(buses.begin(), buses.end(), comp));
  int64_t time = next_time(best_bus, departure_time);
  int64_t res = best_bus * time;

  std::cout << "Part 1 : " << res << std::endl;
}

int64_t get_modulo(int64_t ni, int64_t bi) {
  int64_t xi = (ni - bi);
  while (xi < 0)
    xi += ni;
  return xi % ni;
}

void part2() {
  int64_t res = 0;

  bool first = true;
  int64_t xi, ni;

  // Init
  auto cur = buses_ids.begin();
  ni = cur->second;
  xi = get_modulo(ni, cur->first);
  auto next = std::next(cur);

  // Chinese remainder theorem !
  while (next != buses_ids.end()) {
    int64_t nip1 = next->second;
    int64_t ai = get_modulo(nip1, next->first);
    std::cout << xi << " " << nip1 << " " << ai << std::endl;
    while (xi % nip1 != ai)
      xi += ni;

    ni *= nip1;

    cur = next;
    std::advance(next, 1);
  }

  std::cout << "Part 2 : " << xi << std::endl;
}

int main(int argc, char **argv) {
  std::ifstream f_in;
  f_in.open("13.in");
  std::string line;
  std::getline(f_in, line);
  departure_time = std::stoi(line);
  std::getline(f_in, line);
  f_in.close();

  boost::char_separator<char> sep{",", ""};
  boost::tokenizer tokenizer{line, sep};

  int tok = -1;
  for (const auto &t: tokenizer) {
    tok++;
    if (t == "x")
      continue;
    int64_t bus_id = std::stoi(t);
    buses.push_back(bus_id);
    buses_ids.push_back(std::make_pair(tok, bus_id));
    std::cout << tok << " " << bus_id << std::endl;
  }

  part1();
  part2();

  return 0;
}


// solving A * X = B
// A symmetric/hermitian positive definite in packed format
// driver function posv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/ppsv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include "utils.h"

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

using std::size_t;
using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::lower, ublas::column_major> symml_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::lower, ublas::column_major> herml_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::upper, ublas::column_major> symmu_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::upper, ublas::column_major> hermu_t;

int main() {

  cout << endl;

  // symmetric
  cout << "real symmetric\n" << endl;

  size_t n = 5;
  size_t nrhs = 2;
  symml_t sal (n, n);   // symmetric matrix
  symmu_t sau (n, n);   // symmetric matrix
  m_t x (n, nrhs);
  m_t bl (n, nrhs), bu (n, nrhs);  // RHS matrices

  init_symm (sal, 'l');
  //        [5 0 0 0 0]
  //        [4 5 0 0 0]
  //   al = [3 4 5 0 0]
  //        [2 3 4 5 0]
  //        [1 2 3 4 5]

  init_symm (sau, 'u');
  //        [5 4 3 2 1]
  //        [0 5 4 3 2]
  //   au = [0 0 5 4 3]
  //        [0 0 0 5 4]
  //        [0 0 0 0 5]

  print_m (sal, "sal");
  cout << endl;
  print_m_data (sal, "sal");
  cout << endl;

  print_m (sau, "sau");
  cout << endl;
  print_m_data (sau, "sau");
  cout << endl;

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  bl = prod (sal, x);
  bu = prod (sau, x);

  print_m (bl, "bl");
  cout << endl;
  print_m (bu, "bu");
  cout << endl;

  lapack::ppsv (sal, bl);
  print_m (bl, "xl");
  cout << endl;

  lapack::ppsv (sau, bu);
  print_m (bu, "xu");
  cout << endl;

  //////////////////////////////////////////////////////////
  // hermitian
  cout << "\n==========================================\n" << endl;
  cout << "complex hermitian (well, not really ;o)\n" << endl;

  herml_t hal (3, 3);   // hermitian matrix
  hermu_t hau (3, 3);   // hermitian matrix
  cm_t cx (3, 1);
  cm_t cbl (3, 1), cbu (3, 1);  // RHS

  init_symm (hal, 'l');
  init_symm (hau, 'u');

  print_m (hal, "hal");
  cout << endl;
  print_m (hau, "hau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hal, cx);
  cbu = prod (hau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  int ierr = lapack::ppsv (hal, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::ppsv (hau, cbu);
  if (ierr == 0)
    print_m (cbu, "cxu");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

  // regular (see ublas_gesv.cc), but not positive definite:

  hal (0, 0) = cmplx_t (3, 0);
  hal (1, 0) = cmplx_t (4, -2);
  hal (1, 1) = cmplx_t (5, 0);
  hal (2, 0) = cmplx_t (-7, -5);
  hal (2, 1) = cmplx_t (0, 3);
  hal (2, 2) = cmplx_t (2, 0);

  hau (0, 0) = cmplx_t (3, 0);
  hau (0, 1) = cmplx_t (4, 2);
  hau (0, 2) = cmplx_t (-7, 5);
  hau (1, 1) = cmplx_t (5, 0);
  hau (1, 2) = cmplx_t (0, -3);
  hau (2, 2) = cmplx_t (2, 0);

  print_m (hal, "hal");
  cout << endl;
  print_m (hau, "hau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, 1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hal, cx);
  cbu = prod (hau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  ierr = lapack::ppsv (hal, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::ppsv (hau, cbu);
  if (ierr == 0)
    print_m (cbu, "cxu");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

  // positive definite:

  hal (0, 0) = cmplx_t (25, 0);
  hal (1, 0) = cmplx_t (-5, 5);
  hal (1, 1) = cmplx_t (51, 0);
  hal (2, 0) = cmplx_t (10, -5);
  hal (2, 1) = cmplx_t (4, 6);
  hal (2, 2) = cmplx_t (71, 0);

  hau (0, 0) = cmplx_t (25, 0);
  hau (0, 1) = cmplx_t (-5, -5);
  hau (0, 2) = cmplx_t (10, 5);
  hau (1, 1) = cmplx_t (51, 0);
  hau (1, 2) = cmplx_t (4, -6);
  hau (2, 2) = cmplx_t (71, 0);

  print_m (hal, "hal");
  cout << endl;
  print_m (hau, "hau");
  cout << endl;

  cm_t cbl2 (3, 2);
  cbl2 (0, 0) = cmplx_t (60, -55);
  cbl2 (1, 0) = cmplx_t (34, 58);
  cbl2 (2, 0) = cmplx_t (13, -152);
  cbl2 (0, 1) = cmplx_t (70, 10);
  cbl2 (1, 1) = cmplx_t (-51, 110);
  cbl2 (2, 1) = cmplx_t (75, 63);
  cm_t cbu2 (cbl2);
  print_m (cbl2, "cbl");
  cout << endl;

  ierr = lapack::ppsv (hal, cbl2);
  if (ierr == 0)
    print_m (cbl2, "cxl");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl << endl;
  cout << endl;

  ierr = lapack::ppsv (hau, cbu2);
  if (ierr == 0)
    print_m (cbu2, "cxu");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl << endl;
  cout << endl;

}


#define BOOST_TEST_MODULE pcraster geo fraction_filter
#include <boost/test/unit_test.hpp>
#include "dal_MathUtils.h"
#include "geo_mooreneighbourhood.h"
#include "geo_filterengine.h"
#include "geo_fractionfilter.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace geo;

  // Fraction of true cells in a filter.

  // 1 0 0
  // 0 1 MV
  // 0 0 1
  SimpleRaster<UINT1> source(3, 3);
  source.cell(0) = 1;
  source.cell(1) = 0;
  source.cell(2) = 0;
  source.cell(3) = 0;
  source.cell(4) = 1;
  source.setMV(5);
  source.cell(6) = 0;
  source.cell(7) = 0;
  source.cell(8) = 1;

  SquareNeighbourhood weights(1);
  FractionFilter<UINT1> filter(weights, 1);

  // 2/4 2/5 1/3
  // 2/6 3/8 MV
  // 1/4 2/5 2/3
  SimpleRaster<REAL8> destination(3, 3);

  FilterEngine<UINT1, REAL8> engine(source, filter, destination);
  engine.calc();

  BOOST_CHECK(dal::comparable(destination.cell(0), 2.0 / 4.0));
  BOOST_CHECK(dal::comparable(destination.cell(1), 2.0 / 5.0));
  BOOST_CHECK(dal::comparable(destination.cell(2), 1.0 / 3.0));
  BOOST_CHECK(dal::comparable(destination.cell(3), 2.0 / 6.0));
  BOOST_CHECK(dal::comparable(destination.cell(4), 3.0 / 8.0));
  BOOST_CHECK(destination.isMV(5));
  BOOST_CHECK(dal::comparable(destination.cell(6), 1.0 / 4.0));
  BOOST_CHECK(dal::comparable(destination.cell(7), 2.0 / 5.0));
  BOOST_CHECK(dal::comparable(destination.cell(8), 2.0 / 3.0));
}