/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2005 StatPro Italia srl
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef cl_adjoint_path_generator_impl_hpp
#define cl_adjoint_path_generator_impl_hpp
#pragma once

#include <boost/test/unit_test.hpp>
#include "utilities.hpp"
#include "adjointpathgeneratortest.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <ql/methods/montecarlo/mctraits.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/processes/ornsteinuhlenbeckprocess.hpp>
#include <ql/processes/squarerootprocess.hpp>
#include <ql/processes/stochasticprocessarray.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/utilities/dataformatters.hpp>
#include <ql/stochasticprocess.hpp>
#include <ql/processes/eulerdiscretization.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointPathGenerator"

namespace
{
    enum
    {
#if defined CL_GRAPH_GEN
        // Number of points for dependency plots.
        pointNo = 50,
        // Number of points for performance plot.
        iterNo = 50,
        // Step for portfolio size for performance testing .
        step = 1,
#else
        // Number of points for dependency plots.
        pointNo = 1,
        // Number of points for performance plot.
        iterNo = 1,
        // Step for portfolio size for performance testing .
        step = 1,
#endif
        // Defines performance accuracy. Its value is a minimum number
        // of calling of O(1) complexity methods per one performance test.
        iterNumFactor = 10,
    };

    inline Matrix generateCorrelation(Size size)
    {
        Matrix M(size, size);
        for (Size i = 0; i < size; i++)
        {
            M[i][i] = 1;
            for (Size j = i + 1; j < size; j++)
            {
                M[i][j] = 1 - j*0.6 / size;
                M[j][i] = M[i][j];
            }
        }
        return M;
    }

    struct BlackScholes
    {
        BlackScholes()
        {
        }

        std::string getName() { return "BlackScholes"; }

        static boost::shared_ptr<StochasticProcess> createProcess(std::vector<Real> sigmaValue)
        {
            std::vector<boost::shared_ptr<StochasticProcess1D> > processes;
            Size size = sigmaValue.size();

            Handle<Quote> x0(boost::shared_ptr<Quote>(new SimpleQuote(100.0)));
            Handle<YieldTermStructure> r(flatRate(0.05, Actual360()));
            Handle<YieldTermStructure> q(flatRate(0.02, Actual360()));

            Matrix correlation = generateCorrelation(size);

            for (Size i = 0; i < size; i++)
            {
                Handle<BlackVolTermStructure> sigma(flatVol(sigmaValue[i], Actual360()));
                processes.push_back(boost::shared_ptr<StochasticProcess1D>(
                    new BlackScholesMertonProcess(x0, q, r, sigma)));
            }

            return boost::shared_ptr<StochasticProcess>(
                new StochasticProcessArray(processes, correlation));
        }
    };

    struct OrnstUhlenbeck
    {
        OrnstUhlenbeck()
        {
        }

        std::string getName() { return "OrnsteinUhlenbeck"; }

        static boost::shared_ptr<StochasticProcess> createProcess(std::vector<Real> sigmaValue)
        {
            std::vector<boost::shared_ptr<StochasticProcess1D> > processes;
            Size size = sigmaValue.size();

            Matrix correlation = generateCorrelation(size);
            Real speed = 0.1;

            for (Size i = 0; i < size; i++)
            {
                Real sigma = sigmaValue[i];
                processes.push_back(boost::shared_ptr<StochasticProcess1D>(
                    new QuantLib::OrnsteinUhlenbeckProcess(0.1, sigma)));
            }

            return boost::shared_ptr<StochasticProcess>(
                new StochasticProcessArray(processes, correlation));
        }
    };

    struct SqRoot
    {
        SqRoot()
        {
        }

        std::string getName() { return "SquareRoot"; }

        static boost::shared_ptr<StochasticProcess> createProcess(std::vector<Real> sigmaValue)
        {
            std::vector<boost::shared_ptr<StochasticProcess1D>> processes;
            Size size = sigmaValue.size();

            Matrix correlation = generateCorrelation(size);
            Real mean = 0.5;
            Real speed = 0.1;
            Real x0 = 1.0;

            for (Size i = 0; i < size; i++)
            {
                Real sigma = sigmaValue[i];
                processes.push_back(boost::shared_ptr<StochasticProcess1D>(new QuantLib::SquareRootProcess(mean, speed, sigma, 1.0)));
            }

            return boost::shared_ptr<StochasticProcess>(
                new StochasticProcessArray(processes, correlation));
        }
    };

    template <class Process>
    struct PathGenerators : public Process
    {
        PathGenerators()
        {
        };

        // Generates a multipath from a random number generator.
        // Input parameter:
        //  - sigmaValue - sigma for stochastic process for generating random paths.
        static std::vector<Real> testMultiple(std::vector<Real> sigmaValue)
        {
            typedef PseudoRandom::rsg_type rsg_type;
            typedef MultiPathGenerator<rsg_type>::sample_type sample_type;

            boost::shared_ptr<StochasticProcess> process = createProcess(sigmaValue);

            BigNatural seed = 42;
            Time length = 10;
            Size timeSteps = 12;
            Size assets = process->size();
            std::vector<Real> calculated(2 * assets);
            rsg_type rsg = PseudoRandom::make_sequence_generator(timeSteps*assets,
                                                                 seed);
            MultiPathGenerator<rsg_type> generator(process,
                                                   TimeGrid(length, timeSteps),
                                                   rsg, false);
            Size i, j;
            for (i = 0; i < 100; i++)
                generator.next();

            sample_type sample = generator.next();

            Real error, tolerance = 2.0e-7;
            for (j = 0; j < assets; j++)
                calculated[j] = sample.value[j].back();

            sample = generator.antithetic();
            for (j = 0; j < assets; j++)
                calculated[assets + j] = sample.value[j].back();

            return calculated;
        }

        // Use forward finite difference for approximation of the derivatives.
        // dy/dx = (y(x+h) - y(x))/h
        // Input parameters:
        //  - sigma - vector of input sigma values;
        //  - sf_Finite - vector of calculated derivatives;
        //  - h - step size for finite difference method.
        static void calculateFiniteDifference(std::vector<Real>& sigma, std::vector<Real>& sf_Finite, double h)
        {
            Size numOfIndep = sigma.size();
            Size n = 2 * numOfIndep;
            sf_Finite.resize(n*numOfIndep, 0.0);

            std::vector<Real> value = testMultiple(sigma);
            std::vector<Real> rightValue;

            for (Size i = 0; i < numOfIndep; i++)
            {
                sigma[i] += h;
                rightValue = testMultiple(sigma);
                for (Size j = 0; j < 2; j++)
                    sf_Finite[(i + numOfIndep * j)*numOfIndep + i] = (rightValue[i + j*numOfIndep] - value[i + j*numOfIndep]) / h;
                sigma[i] -= h;
            }
        }
    };

    // Struct for plots recording.
    struct SigmaSens
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Sigma", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, SigmaSens& v)
        {
                stm << v.sigma_
                    << ";" << v.sensitivity_
                    << std::endl;
                return stm;
            }

        Real sigma_;
        Real sensitivity_;
    };

    template <class Process>
    struct TestData
        : public PathGenerators<Process>
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, TestData<Process>* data)
            : size_(size)
            , data_(data)
            , sigma_(size)
            , assets_()
            {
                setLogger(&data_->outPerform_);

                if (size_ != 1)
                {
                    for (Size i = 0; i < size_; i++)
                    {
                        sigma_[i] = 0.20 + i*0.001;
                    }
                }
                else
                {
                    sigma_[0] = 0.20;
                }
            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 2*size_; }

            Size minPerfIteration() { return iterNumFactor; }

            void recordTape()
            {
                cl::Independent(sigma_);
                generateAssets();
                f_ = std::make_unique<cl::tape_function<double>>(sigma_, assets_);
            }

            void calcReverse()
            {
                using namespace std;
                size_t n = indepVarNumber();
                size_t m = depVarNumber();
                reverseResults_.resize(m*n);
                vector<double> dw(m), dy(n);

                for (size_t i = 0; i < m; i++)
                {
                    dw[i] = 1;
                    dy = f_->Reverse(1, dw);
                    dw[i] = 0;
                    for (size_t j = 0; j < n; j++)
                        reverseResults_[i*n + j] = dy[j];
                }
            }
            // Calculates price of portfolio and each option.
            void generateAssets()
            {
                assets_ = data_->testMultiple(sigma_);
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-10;  // shift for finite diff. method
                analyticalResults_.resize(size_);
                data_->calculateFiniteDifference(sigma_, analyticalResults_, h);
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-8; }

            Size size_;
            TestData<Process>* data_;
            std::vector<cl::tape_double> sigma_;
            std::vector<cl::tape_double> assets_;
        };

        TestData()
            : PathGenerators<Process>()
        , outPerform_(OUTPUT_FOLDER_NAME"//" + getName() + "//"
        , { { "filename", "AdjointPerformance" }
        , { "not_clear", "Not" }
        , { "title", "Last-time step asset value differentiation performance with respect to sigma" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of sigmas" }
        , { "line_box_width", "-5" }
        , { "smooth", "default" }
        , { "cleanlog", "true" } })
            , outAdjoint_(OUTPUT_FOLDER_NAME"//" +  getName() + "//"
        , { { "filename", "Adjoint" }
        , { "not_clear", "Not" }
        , { "title", "Last-time step asset value adjoint differentiation performance with respect to sigma" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of sigmas" }
        , { "smooth", "default" }
        , { "cleanlog", "false" } })
            , outSize_(OUTPUT_FOLDER_NAME"//" +  getName() + "//"
        , { { "filename", "TapeSize" }
        , { "not_clear", "Not" }
        , { "title", "Tape size dependence on number of sigma" }
        , { "ylabel", "Memory (MB)" }
        , { "xlabel", "Number of sigmas" }
        , { "smooth", "default" }
        , { "cleanlog", "false" } })
            , out_(OUTPUT_FOLDER_NAME"//" +  getName() + "//output"
        , { { "filename", "SigmaDependence" }
        , { "not_clear", "Not" }
        , { "title", "Last-time step asset value sensitivity dependence on sigma" }
        , { "ylabel", "Last-time step asset value sensitivity" }
        , { "xlabel", "Sigma" }
        , { "smooth", "default" }
        , { "cleanlog", "false" } })
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo, step);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<SigmaSens> outData(pointNo);
            auto test = getTest(pointNo);
            for (Size i = 0; i < pointNo; i++)
            {
                test->sigma_[0] += 0.05;
                test->generateAssets();
                outData[i] = { test->sigma_[0], test->assets_[i] };
            }
            out_ << outData;
            return true;
        }

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;

    };

}

#endif


#include <vector>

#include <boost/test/unit_test.hpp>

#include "bithorded/lib/treestore.hpp"

using namespace std;

#include "test_storage.hpp"

BOOST_AUTO_TEST_CASE( function_test )
{
	BOOST_CHECK_EQUAL( parentlayersize(0), 0 );
	BOOST_CHECK_EQUAL( parentlayersize(1), 0 );
	BOOST_CHECK_EQUAL( parentlayersize(2), 1 );
	BOOST_CHECK_EQUAL( parentlayersize(7), 4 );
	BOOST_CHECK_EQUAL( parentlayersize(8), 4 );
	BOOST_CHECK_EQUAL( parentlayersize(9), 5 );

	BOOST_CHECK_EQUAL( treesize(6), 6+3+2+1);
	BOOST_CHECK_EQUAL( treesize(7), 7+4+2+1);
	BOOST_CHECK_EQUAL( treesize(8), 8+4+2+1);
	BOOST_CHECK_EQUAL( treesize(9), 9+5+3+2+1);

	for (int i=1; i < 64; i++) {
		auto ts = treesize(i);
		auto leaves = calc_leaves(ts);
		BOOST_CHECK_EQUAL( leaves, i );
	}
}

BOOST_AUTO_TEST_CASE( idx_test )
{
	TestStorage<int> store(treesize(6));
	for (uint i=0; i < store.size(); i++)
		*store[i] = i;

	TreeStore<int, TestStorage<int>> tree(store);
	NodeIdx idx(0,1);

	BOOST_CHECK( NodeIdx(0,1).isValid() );
	BOOST_CHECK( not NodeIdx(0,0).isValid() );
	BOOST_CHECK( not NodeIdx(1,0).isValid() );

	BOOST_CHECK_EQUAL( tree.leaf(0), NodeIdx(0,6) );
	BOOST_CHECK_EQUAL( tree.leaf(5), NodeIdx(5,6) );

	idx = tree.leaf(0); BOOST_CHECK_EQUAL( *tree[idx], 6 );
	idx = tree.leaf(5); BOOST_CHECK_EQUAL( *tree[idx], 11 );

	BOOST_CHECK_EQUAL( NodeIdx(0,2).parent(), NodeIdx(0,1) );
	BOOST_CHECK_EQUAL( NodeIdx(1,2).parent(), NodeIdx(0,1) );
	BOOST_CHECK_EQUAL( NodeIdx(0,6).parent(), NodeIdx(0,3) );
	BOOST_CHECK_EQUAL( NodeIdx(2,6).parent(), NodeIdx(1,3) );
	BOOST_CHECK_EQUAL( NodeIdx(4,6).parent(), NodeIdx(2,3) );
	BOOST_CHECK_EQUAL( NodeIdx(5,6).parent(), NodeIdx(2,3) );

	BOOST_CHECK( tree.leaf(5).parent().parent().parent().isRoot() );
	BOOST_CHECK( not tree.leaf(5).parent().parent().isRoot() );

	BOOST_CHECK_EQUAL( NodeIdx(0,2).sibling(), NodeIdx(1,2) );
	BOOST_CHECK_EQUAL( NodeIdx(1,2).sibling(), NodeIdx(0,2) );
	BOOST_CHECK_EQUAL( NodeIdx(0,5).sibling(), NodeIdx(1,5) );
	BOOST_CHECK_EQUAL( NodeIdx(1,5).sibling(), NodeIdx(0,5) );
	BOOST_CHECK( not NodeIdx(4,5).sibling().isValid() );
}


//////////////////////////////////////////////////////////////////////////////////
// random::poisson_ex::poisson_devroye::detail::q_function::cross_check.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_Q_FUNCTION_CROSS_CHECK_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_Q_FUNCTION_CROSS_CHECK_ER_2010
#include <iostream>
#include <stdexcept>
#include <string>
#include <boost/format.hpp>
#include <boost/random/poisson_ext/devroye/q_function/self_checking.hpp>
#include <boost/numeric/conversion/converter.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace q{

    template<typename T,typename Int,typename P>
    void cross_check(
        Int i_mean_loops,
        Int i_mean, // 1
        Int i_mean_factor, // 10
        const T& tol,
        const P& pol
    )
    {
        BOOST_ASSERT(tol>=0);
        BOOST_ASSERT(i_mean>0);

        typedef boost::numeric::converter<T,Int> conv_;
        typedef boost::numeric::bounds<Int> bounds_;
        typedef std::string s_;
        typedef boost::format f_;
        conv_ conv;
        Int max = bounds_::highest();

        for(Int i1 = 0; i1<i_mean_loops; i1++){
            try{
                //fun<T>(i_mean,0,pol,conv_(),meth_());
                for (
                    Int delta = 1, i_y = -i_mean;
                    i_y <max; ++delta, i_y = -i_mean + (delta-1))
                {
                    T val1 = fun<T>(i_mean,i_y,pol,conv,using_factorial());
                    T val2 = fun<T>(i_mean,i_y,pol,conv,using_sum());
                    if((val1 > tol + val2) || (val2 > tol + val1)){
                        using namespace boost::math::policies;
                        s_ call = "(%1%,%2%)";
                        s_ msg = " (using_factorial) != %1% (using_sum)";
                        f_ f1(call); f1 % i_mean % i_y;
                        f_ f2(msg); f2 % val2;
                        call = "q_fun<%1%>" + f1.str();
                        msg = "%1%" + f2.str();
                        raise_evaluation_error(
                            call.c_str(), msg.c_str(), val1, pol);
                    }
                }
            }catch(std::exception& e){
                std::cout << e.what() << std::endl;
            }

            i_mean *= i_mean_factor;
        }
    }

    template<typename T,typename Int>
    void cross_check(
        Int i_mean_loops,
        Int i_mean, // 1
        Int i_mean_factor, // 10
        const T& tol
    )
    {
        typedef boost::math::policies::policy<> pol_;
        cross_check<T,Int>(i_mean_loops,i_mean,i_mean_factor,tol,pol_());
    }

}// q
}// devroye
}// poisson
}// random
}// boost

#endif


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/ifnotsub.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/logical.hpp>
STF_CASE_TPL (" ifnotsub real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotsub;
  using bs::logical;
  using r_t = decltype(ifnotsub(T(),T(),T()));

  // return type conformity test
  STF_TYPE_IS( r_t, T );

  // specific values tests
  STF_EQUAL(ifnotsub(T(0),T(1),T(2)), T(-1));
  STF_EQUAL(ifnotsub(T(1),T(1),T(2)), T(1));
  STF_EQUAL(ifnotsub(T(1),bs::Inf<T>(),bs::Inf<T>()), bs::Inf<r_t>());
  STF_IEEE_EQUAL(ifnotsub(T(0),bs::Minf<T>(),bs::Minf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(ifnotsub(T(0),bs::Nan<T>(),bs::Nan<T>()), bs::Nan<r_t>());
  STF_EQUAL(ifnotsub(T(0),bs::Zero<T>(),bs::One<T>()), bs::Mone<r_t>());
} // end of test for floating_

STF_CASE_TPL (" ifnotsub integer",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotsub;
  using bs::logical;
  using r_t = decltype(ifnotsub(T(),T(),T()));

  // return type conformity test
  STF_TYPE_IS( r_t, T );

  // specific values tests
  STF_EQUAL(ifnotsub(logical<T>(T(-1)),T(1),T(2)), T(1));
  STF_EQUAL(ifnotsub(logical<T>(T(0)),T(2),T(1)), T(1));
  STF_EQUAL(ifnotsub(logical<T>(bs::Zero<T>()),bs::Zero<T>(),bs::Zero<T>()), bs::Zero<r_t>());
  STF_EQUAL(ifnotsub(T(-1),T(1),T(2)), T(1));
  STF_EQUAL(ifnotsub(T(0),T(2),T(1)), T(1));
  STF_EQUAL(ifnotsub(bs::Zero<T>(),bs::Zero<T>(),bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for integer_


#include "cerealHelper.hpp"
#include <Eigen/Dense>
#include <boost/program_options.hpp>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

namespace po = boost::program_options;

struct DataCollection
{
    Eigen::Affine3d affine;
    cv::Mat image;
    pcl::PointCloud<pcl::PointXYZI> cloud;

    template<class Archive>
    void serialize(Archive& archive)
    {
        archive(affine, image, cloud);
    }
};

void colorizeCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
    // pack r/g/b into rgb
    std::uint8_t r = 0, g = 255, b = 0;
    std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
    for (auto& p : *cloud) {
        p.rgb = *reinterpret_cast<float*>(&rgb);
    }
}

int main(int argc, char** argv)
{
    // Program options
    po::options_description desc("Take data from binary archive and process\n\n"
                                 "Options");
    std::string cerealpath;
    desc.add_options()("help,h", "Print this help")("filecereal,f",
                                                    po::value<std::string>(&cerealpath)->required(),
                                                    "Set the input path for the cereal binary file");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    std::ifstream dataFile(cerealpath, std::ifstream::out | std::ios::binary);
    if (dataFile.fail()) {
        throw std::runtime_error("cannot open cereal binary file!");
    }

    DataCollection dataCollection;
    cereal::BinaryInputArchive inputArchive(dataFile);
    inputArchive(CEREAL_NVP(dataCollection));

    // --- Processing Normal (Numpy) Matrix

    std::cout << "loaded Homogeneous Transformation matrix: " << std::endl
              << dataCollection.affine.matrix() << std::endl;

    // ---

    // --- Image (OpenCV)

    cv::imshow("Image", dataCollection.image);
    cv::waitKey(0);

    // ---

    // --- PointCloud (PCL)

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::copyPointCloud(dataCollection.cloud, *cloud);
    colorizeCloud(cloud);
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {
    }

    // ---

    return 0;
}


/*
 *  Authors: Lana Mineh and John Scott
 *  Copyright 2021 Phasecraft Ltd. and John Scott
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 */

/**
 * \file test-utils.hpp
 * \brief Utilities for the test function
 *
 */

#include <armadillo>
#include <qsl/concepts.hpp>

/// Gets the bit in the nth position of val
unsigned getBit(unsigned val, unsigned n);

/// Set the nth bit of val to b
unsigned setBit(unsigned val, unsigned n, unsigned b);

/**
 * \brief Construct an armadillo matrix to apply an arbitrary gate
 */
template<std::floating_point Fp>
arma::SpMat<std::complex<Fp>>
makeMatrix(const arma::Mat<std::complex<Fp>> & gate, unsigned nqubits,
	   const std::vector<unsigned> & indices)
{
    // Check that the gate size matches the length of the indices
    if ((1ULL << indices.size()) != gate.n_rows) {
	throw std::logic_error("Length of indices vector does not "
			       "match the size of the gate in call "
			       "to makeMatrix()");
    }

    const std::size_t dim{ 1ULL << nqubits };
    arma::SpMat<std::complex<Fp>> mat(dim,dim);

    // Write columns of mat. The columns of the matrix
    // correspond to the images of the basis states under
    // the action of the unitary matrix.
    for (std::size_t col = 0; col < dim; col++) {

	// Get column bitstring and find the value of the ctrl and targ bits.
	// The values of the bits of col in the ctrl and targ positions
	// fix which column of the small matrix is used in populating
	// values of the big matrix at column col.
	//
	// Generalised for standard vector of qubit positions.
	std::vector<unsigned> vals;
	for (std::size_t k = 0; k < indices.size(); k++) {
	    vals.push_back(getBit(col, indices[k]));
	}

	// Write rows of mat. For each row index,
	for (std::size_t n = 0; n < gate.n_rows; n++) {

	    // Make the row index. The rows that are non-zero in a
	    // particular column are the ones whose bits agree with
	    // the bits in the col, apart from at the ctrl and targ
	    // positions. There, they take every possible ctrl and
	    // targ value.
	    std::size_t row = col;
	    for (std::size_t k = 0; k < indices.size(); k++) {
		row = setBit(row, indices[k], getBit(n,k));
	    }

	    // Make the column index for the small matrix
	    std::size_t m = 0;
	    for (std::size_t k = 0; k < vals.size(); k++) {
		m = setBit(m, k, vals[k]);
	    }

	    mat(row,col) = gate(n,m);
	}
    }
    return mat;
}

/**
 * \brief Convert a standard vector state to an armadillo vector
 */
template<std::floating_point Fp>
arma::Col<std::complex<Fp>> toArmaState(const std::vector<qsl::complex<Fp>> & res)
{
    arma::Col<std::complex<Fp>> qubit_v(res.size());
    for (std::size_t i = 0; i < res.size(); i++) {
	qubit_v(i) = std::complex<Fp>{res[i].real, res[i].imag};
    }
    return qubit_v;
}

/**
 * \brief Convert a Simulator state to an armadillo vector
 */
template<qsl::Simulator Sim>
arma::Col<std::complex<typename Sim::Fp_type>> toArmaState(const Sim & sim)
{
    using Fp = Sim::Fp_type;
    // Read qubit state into armadillo to check the state hasn't changed
    std::vector<qsl::complex<Fp>> res = sim.getState();
    return toArmaState<Fp>(res);
}

/**
 * \brief Make the projector onto a particular outcome of the target qubit
 *
 */
template<std::floating_point Fp>
arma::SpMat<std::complex<Fp>>
projector(unsigned num_qubits, unsigned targ, unsigned outcome)
{
    // Calculate the projector for the outcome, which is
    // the observable that has eigenvalue 0 for |~outcome) and eigenvalue
    // 1 for |outcome). The expectation value of this observable in a
    // state is the probability of getting 1 on measurement.
    arma::Mat<std::complex<Fp>> projector(2,2,arma::fill::zeros);
    if (outcome == 0) {
	projector(0,0) = 1;
    } else if (outcome == 1) {
	projector(1,1) = 1;
    } else {
	throw std::out_of_range("outcome must be 0 or 1 in projector() function");
    }
    arma::SpMat<std::complex<Fp>> M = makeMatrix(projector, num_qubits, {targ});
    return M;
}

/**
 * \brief Calculate the probability of measuring a projector outcome
 *
 * Uses the formula prob = (v|P|v), where v is the state and P is
 * the projector
 */
template<std::floating_point Fp>
Fp probability(const arma::SpMat<std::complex<Fp>> & P,
	       const arma::Col<std::complex<Fp>> & v)
{
    // Calculate the probability of the projector outcome
    Fp prob = arma::cdot(v, P*v).real();
    return prob;
}

/**
 * \brief Collapse a state v using a projector P
 *
 * The output is the state Pv/|Pv| (i.e. the normalised projected state)
 *
 */
template<std::floating_point Fp>
arma::Col<std::complex<Fp>>
applyProjector(const arma::SpMat<std::complex<Fp>> & P,
	       const arma::Col<std::complex<Fp>> & v)
{
    arma::Col<std::complex<Fp>> state = (P * v)/arma::norm(P * v);
    return state;
}


#include "eclat.h"

#include <boost/dynamic_bitset.hpp>
#include <map>
#include <utility> // for std::pair
#include <vector>

#include "verticaldatabase.h"

using boost::dynamic_bitset;
using std::pair;
using std::vector;

typedef pair<int, dynamic_bitset<>> Candidate;

void eclat_recursive(const vector<int>& prefix,
                     const vector<Candidate>& p,
                     unsigned int minsup,
                     FrequentItemsets& result) {
    auto size = p.size();
    for (unsigned int i = 0; i < size; ++i) {
        // take candidate a from p
        const Candidate& candidate_a = p[i];
        int a = candidate_a.first;
        vector<int> itemset_a = prefix;
        itemset_a.push_back(a);
        const dynamic_bitset<>& bitset_a = candidate_a.second;
        result[itemset_a] = bitset_a.count();

        // look for frequent itemsets with candidate a as prefix
        vector<Candidate> pa;
        for (unsigned int j = i+1; j < size; ++j) {
            // take candidate b from p
            const Candidate& candidate_b = p[j];
            int b = candidate_b.first;
            const dynamic_bitset<>& bitset_b = candidate_b.second;

            // new candidate ab
            dynamic_bitset<> bitset_ab = bitset_a & bitset_b;
            if (bitset_ab.count() >= minsup)
                pa.push_back(Candidate(b, bitset_ab));
        }

        // recurse!
        if ( ! pa.empty())
            eclat_recursive(itemset_a, pa, minsup, result);
    }
}

FrequentItemsets eclat(const VerticalDatabase& d, unsigned int minsup) {
    // initial candidates: frequent single-item itemsets
    vector<Candidate> candidates;
    int nItems = d.nItems();
    for (int i = 0; i < nItems; ++i) {
        dynamic_bitset<> b = d.bs[i];
        if (b.count() >= minsup)
            candidates.push_back(Candidate(i, b));
    }

    // run algorithm
    FrequentItemsets result;
    vector<int> prefix;
    eclat_recursive(prefix, candidates, minsup, result);

    return result;
}


#include <boost/math/special_functions/bessel_prime.hpp>


// Copyright 2020 LMNT, Inc. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ==============================================================================

#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <cuda.h>
#include <cuda_runtime_api.h>
#include <iostream>
#include <string>
#include <unsupported/Eigen/CXX11/Tensor>
#include <vector>

#include "device_ptr.h"
#include "haste.h"

using haste::v0::lstm::BackwardPass;
using haste::v0::lstm::ForwardPass;
using std::string;

using Tensor1 = Eigen::Tensor<float, 1>;
using Tensor2 = Eigen::Tensor<float, 2>;
using Tensor3 = Eigen::Tensor<float, 3>;

constexpr int BATCH_SIZE = 64;
constexpr int SEQUENCE_LEN = 1000;
constexpr int HIDDEN_DIMS = 512;
constexpr int INPUT_DIMS = 512;

static cublasHandle_t g_blas_handle;

class ScopeTimer {
  public:
    ScopeTimer(const string& msg) : msg_(msg) {
      cudaEventCreate(&start_);
      cudaEventCreate(&stop_);
      cudaDeviceSynchronize();
      cudaEventRecord(start_);
    }

    ~ScopeTimer() {
      float elapsed_ms;
      cudaEventRecord(stop_);
      cudaEventSynchronize(stop_);
      cudaEventElapsedTime(&elapsed_ms, start_, stop_);
      printf("%s %.1fms\n", msg_.c_str(), elapsed_ms);
      cudaEventDestroy(start_);
      cudaEventDestroy(stop_);
    }

  private:
    string msg_;
    cudaEvent_t start_, stop_;
};

void LstmInference(const Tensor2& W, const Tensor2& R, const Tensor1& b, const Tensor3& x) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  // Copy weights over to GPU.
  device_ptr<Tensor2> W_dev(W);
  device_ptr<Tensor2> R_dev(R);
  device_ptr<Tensor1> b_dev(b);
  device_ptr<Tensor3> x_dev(x);

  device_ptr<Tensor2> h_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor2> c_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> v_dev(time_steps * batch_size * hidden_size * 4);
  device_ptr<Tensor2> tmp_Rh_dev(batch_size * hidden_size * 4);

  h_dev.zero();
  c_dev.zero();

  ScopeTimer t("Inference:");

  ForwardPass<float> forward(
      false,  // training
      batch_size,
      input_size,
      hidden_size,
      g_blas_handle);

  forward.Run(
      time_steps,
      W_dev.data,
      R_dev.data,
      b_dev.data,
      x_dev.data,
      h_dev.data,
      c_dev.data,
      v_dev.data,
      tmp_Rh_dev.data,
      0.0f,      // zoneout prob
      nullptr);  // zoneout mask
}

void LstmTrain(const Tensor2& W, const Tensor2& R, const Tensor1& b, const Tensor3& x,
               const Tensor3& dh, const Tensor3& dc) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  // Copy weights over to GPU.
  device_ptr<Tensor2> W_dev(W);
  device_ptr<Tensor2> R_dev(R);
  device_ptr<Tensor1> b_dev(b);
  device_ptr<Tensor3> x_dev(x);

  // This is nearly the same as the inference code except we have an extra dimension
  // for h and c. We'll store those outputs of the cell for all time steps and use
  // them during the backward pass below.
  device_ptr<Tensor3> h_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> c_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> v_dev(batch_size * hidden_size * 4 * time_steps);
  device_ptr<Tensor2> tmp_Rh_dev(batch_size * hidden_size * 4);

  h_dev.zero();
  c_dev.zero();

  {
    ScopeTimer t("Train forward:");
    ForwardPass<float> forward(
        true,  // training
        batch_size,
        input_size,
        hidden_size,
        g_blas_handle);

    forward.Run(
        time_steps,
        W_dev.data,
        R_dev.data,
        b_dev.data,
        x_dev.data,
        h_dev.data,
        c_dev.data,
        v_dev.data,
        tmp_Rh_dev.data,
        0.0f,      // zoneout prob
        nullptr);  // zoneout mask
  }

  Eigen::array<int, 3> transpose_x({ 1, 2, 0 });
  Tensor3 x_t = x.shuffle(transpose_x);

  Eigen::array<int, 2> transpose({ 1, 0 });
  Tensor2 W_t = W.shuffle(transpose);
  Tensor2 R_t = R.shuffle(transpose);

  device_ptr<Tensor3> x_t_dev(x_t);
  device_ptr<Tensor2> W_t_dev(W_t);
  device_ptr<Tensor2> R_t_dev(R_t);

  // These gradients should actually come "from above" but we're just allocating
  // a bunch of uninitialized memory and passing it in.
  device_ptr<Tensor3> dh_new_dev(dh);
  device_ptr<Tensor3> dc_new_dev(dc);

  device_ptr<Tensor3> dx_dev(time_steps * batch_size * input_size);
  device_ptr<Tensor2> dW_dev(input_size * hidden_size * 4);
  device_ptr<Tensor2> dR_dev(hidden_size * hidden_size * 4);
  device_ptr<Tensor2> db_dev(hidden_size * 4);
  device_ptr<Tensor2> dh_dev(batch_size * hidden_size);
  device_ptr<Tensor2> dc_dev(batch_size * hidden_size);

  dW_dev.zero();
  dR_dev.zero();
  db_dev.zero();
  dh_dev.zero();
  dc_dev.zero();

  {
    ScopeTimer t("Train backward:");
    BackwardPass<float> backward(
        batch_size,
        input_size,
        hidden_size,
        g_blas_handle);

    backward.Run(
        time_steps,
        W_t_dev.data,
        R_t_dev.data,
        b_dev.data,
        x_t_dev.data,
        h_dev.data,
        c_dev.data,
        dh_new_dev.data,
        dc_new_dev.data,
        dx_dev.data,
        dW_dev.data,
        dR_dev.data,
        db_dev.data,
        dh_dev.data,
        dc_dev.data,
        v_dev.data,
        nullptr);
  }
}

void LstmTrainIterative(const Tensor2& W, const Tensor2& R, const Tensor1& b, const Tensor3& x,
                        const Tensor3& dh, const Tensor3& dc) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  // Copy weights over to GPU.
  device_ptr<Tensor2> W_dev(W);
  device_ptr<Tensor2> R_dev(R);
  device_ptr<Tensor1> b_dev(b);
  device_ptr<Tensor3> x_dev(x);

  device_ptr<Tensor3> h_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> c_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> v_dev(time_steps * batch_size * hidden_size * 4);
  device_ptr<Tensor2> tmp_Rh_dev(batch_size * hidden_size * 4);

  h_dev.zero();
  c_dev.zero();

  {
    ScopeTimer t("Train forward (iterative):");
    ForwardPass<float> forward(
        true,  // training
        batch_size,
        input_size,
        hidden_size,
        g_blas_handle);

    const int NC = batch_size * input_size;
    const int NH = batch_size * hidden_size;
    for (int t = 0; t < time_steps; ++t) {
      forward.Iterate(
          0,
          W_dev.data,
          R_dev.data,
          b_dev.data,
          x_dev.data + t * NC,
          h_dev.data + t * NH,
          c_dev.data + t * NH,
          h_dev.data + (t + 1) * NH,
          c_dev.data + (t + 1) * NH,
          v_dev.data + t * NH * 4,
          tmp_Rh_dev.data,
          0.0f,      // zoneout prob
          nullptr);  // zoneout mask
    }
  }

  Eigen::array<int, 3> transpose_x({ 1, 2, 0 });
  Tensor3 x_t = x.shuffle(transpose_x);

  Eigen::array<int, 2> transpose({ 1, 0 });
  Tensor2 W_t = W.shuffle(transpose);
  Tensor2 R_t = R.shuffle(transpose);

  device_ptr<Tensor3> x_t_dev(x_t);
  device_ptr<Tensor2> W_t_dev(W_t);
  device_ptr<Tensor2> R_t_dev(R_t);

  // These gradients should actually come "from above" but we're just allocating
  // a bunch of uninitialized memory and passing it in.
  device_ptr<Tensor3> dh_new_dev(dh);
  device_ptr<Tensor3> dc_new_dev(dc);

  device_ptr<Tensor3> dx_dev(time_steps * batch_size * input_size);
  device_ptr<Tensor2> dW_dev(input_size * hidden_size * 4);
  device_ptr<Tensor2> dR_dev(hidden_size * hidden_size * 4);
  device_ptr<Tensor2> db_dev(hidden_size * 4);
  device_ptr<Tensor2> dh_dev(batch_size * hidden_size);
  device_ptr<Tensor2> dc_dev(batch_size * hidden_size);

  dW_dev.zero();
  dR_dev.zero();
  db_dev.zero();
  dh_dev.zero();
  dc_dev.zero();

  {
    ScopeTimer t("Train backward (iterative):");
    BackwardPass<float> backward(
        batch_size,
        input_size,
        hidden_size,
        g_blas_handle);

    const int NC = batch_size * input_size;
    const int NH = batch_size * hidden_size;
    for (int t = time_steps - 1; t >= 0; --t) {
      backward.Iterate(
          0,
          W_t_dev.data,
          R_t_dev.data,
          b_dev.data,
          x_t_dev.data + t * NC,
          h_dev.data + t * NH,
          c_dev.data + t * NH,
          c_dev.data + (t + 1) * NH,
          dh_new_dev.data + t * NH,
          dc_new_dev.data + t * NH,
          dx_dev.data + t * NC,
          dW_dev.data,
          dR_dev.data,
          db_dev.data,
          dh_dev.data,
          dc_dev.data,
          v_dev.data + t * NH * 4,
          nullptr);
    }
  }
}

int main() {
  srand(time(0));

  cublasCreate(&g_blas_handle);

  // Weights.
  // W: input weight matrix
  // R: recurrent weight matrix
  // b: bias
  Tensor2 W(HIDDEN_DIMS * 4, INPUT_DIMS);
  Tensor2 R(HIDDEN_DIMS * 4, HIDDEN_DIMS);
  Tensor1 b(HIDDEN_DIMS * 4);

  // Input.
  Tensor3 x(INPUT_DIMS, BATCH_SIZE, SEQUENCE_LEN);

  // Gradients from upstream layers.
  Tensor3 dh(HIDDEN_DIMS, BATCH_SIZE, SEQUENCE_LEN + 1);
  Tensor3 dc(HIDDEN_DIMS, BATCH_SIZE, SEQUENCE_LEN + 1);

  W.setRandom();
  R.setRandom();
  b.setRandom();
  x.setRandom();
  dh.setRandom();
  dc.setRandom();

  LstmInference(W, R, b, x);
  LstmTrain(W, R, b, x, dh, dc);
  LstmTrainIterative(W, R, b, x, dh, dc);

  cublasDestroy(g_blas_handle);

  return 0;
}


#include <Eigen/Geometry>
#include <algorithm>
#include <elasty/constraint.hpp>
#include <elasty/particle.hpp>
#include <gtest/gtest.h>

using Eigen::Matrix;
using Eigen::Vector3d;

template <int Num>
void calculateNumericalDerivative(const std::shared_ptr<elasty::Particle>           particles[],
                                  const std::shared_ptr<elasty::AbstractConstraint> constraint,
                                  double*                                           grad)
{
    constexpr double delta = 1e-06;

    for (int i = 0; i < Num; ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            Vector3d eps3d = Vector3d::Zero();
            eps3d(j)       = delta;

            const Vector3d orig_pos = particles[i]->p;

            particles[i]->p        = orig_pos + eps3d;
            const double cost_plus = constraint->calculateValue();

            particles[i]->p         = orig_pos - eps3d;
            const double cost_minus = constraint->calculateValue();

            particles[i]->p = orig_pos;

            grad[i * 3 + j] = (cost_plus - cost_minus) / (2.0 * delta);
        }
    }
}

template <int Num> bool checkZeroSumCondition(double* grad)
{
    constexpr double epsilon = 1e-12;

    Vector3d sum = Vector3d::Zero();
    for (int i = 0; i < Num; ++i)
    {
        sum(0) += grad[i * 3 + 0];
        sum(1) += grad[i * 3 + 1];
        sum(2) += grad[i * 3 + 2];
    }

    return sum.norm() < epsilon;
}

TEST(ConstraintTest, BendingRestState)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_1 = std::make_shared<elasty::Particle>(Vector3d(0.0, 1.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_2 = std::make_shared<elasty::Particle>(Vector3d(-0.5, 0.5, 0.0), Vector3d::Zero(), 1.0);
    auto p_3 = std::make_shared<elasty::Particle>(Vector3d(+0.5, 0.5, 0.0), Vector3d::Zero(), 1.0);

    const Vector3d& x_0 = p_0->x;
    const Vector3d& x_1 = p_1->x;
    const Vector3d& x_2 = p_2->x;
    const Vector3d& x_3 = p_3->x;

    const Vector3d p_10 = x_1 - x_0;
    const Vector3d p_20 = x_2 - x_0;
    const Vector3d p_30 = x_3 - x_0;

    const Vector3d n_0 = p_10.cross(p_20).normalized();
    const Vector3d n_1 = p_10.cross(p_30).normalized();

    EXPECT_FALSE(n_0.hasNaN());
    EXPECT_FALSE(n_1.hasNaN());

    const double dihedral_angle = std::acos(std::clamp(n_0.dot(n_1), -1.0, 1.0));

    EXPECT_FALSE(std::isnan(dihedral_angle));

    const auto constraint =
        std::make_shared<elasty::BendingConstraint>(p_0, p_1, p_2, p_3, 1.0, 0.0, dt, dihedral_angle);

    const double value = constraint->calculateValue();

    Matrix<double, 12, 1> grad;
    constraint->calculateGrad(grad.data());

    constexpr double epsilon = 1e-20;

    EXPECT_TRUE(std::abs(value) < epsilon);
    EXPECT_TRUE(grad.norm() < epsilon);
}

TEST(ConstraintTest, BendingDerivative)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_1 = std::make_shared<elasty::Particle>(Vector3d(0.0, 1.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_2 = std::make_shared<elasty::Particle>(Vector3d(-0.5, 0.5, 0.0), Vector3d::Zero(), 1.0);
    auto p_3 = std::make_shared<elasty::Particle>(Vector3d(+0.5, 0.5, 0.0), Vector3d::Zero(), 1.0);

    const std::shared_ptr<elasty::Particle> particles[] = {p_0, p_1, p_2, p_3};

    const Vector3d& x_0 = p_0->x;
    const Vector3d& x_1 = p_1->x;
    const Vector3d& x_2 = p_2->x;
    const Vector3d& x_3 = p_3->x;

    const Vector3d p_10 = x_1 - x_0;
    const Vector3d p_20 = x_2 - x_0;
    const Vector3d p_30 = x_3 - x_0;

    const Vector3d n_0 = p_10.cross(p_20).normalized();
    const Vector3d n_1 = p_10.cross(p_30).normalized();

    EXPECT_FALSE(n_0.hasNaN());
    EXPECT_FALSE(n_1.hasNaN());

    const double dihedral_angle = std::acos(std::clamp(n_0.dot(n_1), -1.0, 1.0));

    EXPECT_FALSE(std::isnan(dihedral_angle));

    const auto constraint =
        std::make_shared<elasty::BendingConstraint>(p_0, p_1, p_2, p_3, 1.0, 0.0, dt, dihedral_angle);

    constexpr double epsilon = 1e-04;

    for (auto particle : particles)
    {
        particle->p = particle->x + Vector3d::Random();
    }

    Matrix<double, 12, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    Matrix<double, 12, 1> numerical_grad;
    calculateNumericalDerivative<4>(particles, constraint, numerical_grad.data());

    EXPECT_TRUE((numerical_grad - analytic_grad).cwiseAbs().maxCoeff() < epsilon);
    EXPECT_TRUE(checkZeroSumCondition<4>(analytic_grad.data()));
}

TEST(ConstraintTest, IsometricBendingDerivative)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_1 = std::make_shared<elasty::Particle>(Vector3d(0.0, 1.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_2 = std::make_shared<elasty::Particle>(Vector3d(-0.5, 0.5, 0.0), Vector3d::Zero(), 1.0);
    auto p_3 = std::make_shared<elasty::Particle>(Vector3d(+0.5, 0.5, 0.0), Vector3d::Zero(), 1.0);

    const std::shared_ptr<elasty::Particle> particles[] = {p_0, p_1, p_2, p_3};

    const auto constraint = std::make_shared<elasty::IsometricBendingConstraint>(p_0, p_1, p_2, p_3, 1.0, 0.0, dt);

    constexpr double epsilon = 1e-04;

    for (auto particle : particles)
    {
        particle->p = particle->x + Vector3d::Random();
    }

    Matrix<double, 12, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    Matrix<double, 12, 1> numerical_grad;
    calculateNumericalDerivative<4>(particles, constraint, numerical_grad.data());

    EXPECT_TRUE((numerical_grad - analytic_grad).cwiseAbs().maxCoeff() < epsilon);
    EXPECT_TRUE(checkZeroSumCondition<4>(analytic_grad.data()));
}

TEST(ConstraintTest, DistanceDegenerated)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_1 = std::make_shared<elasty::Particle>(Vector3d(0.0, 1.0, 0.0), Vector3d::Zero(), 1.0);

    const double dist = (p_0->x - p_1->x).norm();

    const auto constraint = std::make_shared<elasty::DistanceConstraint>(p_0, p_1, 1.0, 0.0, dt, dist);

    const std::shared_ptr<elasty::Particle> particles[] = {p_0, p_1};

    constexpr double epsilon = 1e-04;

    for (auto particle : particles)
    {
        particle->p = Vector3d::Zero();
    }

    Matrix<double, 6, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    EXPECT_TRUE(std::abs(analytic_grad.segment<3>(0 * 3).norm() - 1.0) < epsilon);
    EXPECT_TRUE(std::abs(analytic_grad.segment<3>(1 * 3).norm() - 1.0) < epsilon);
    EXPECT_TRUE(checkZeroSumCondition<2>(analytic_grad.data()));
}

TEST(ConstraintTest, DistanceZeroDegenerated)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_1 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);

    const double dist = (p_0->x - p_1->x).norm();

    const auto constraint = std::make_shared<elasty::DistanceConstraint>(p_0, p_1, 1.0, 0.0, dt, dist);

    const std::shared_ptr<elasty::Particle> particles[] = {p_0, p_1};

    constexpr double epsilon = 1e-04;

    Matrix<double, 6, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    const double value = constraint->calculateValue();

    EXPECT_TRUE(std::abs(value) < epsilon);
    EXPECT_TRUE(std::abs(analytic_grad.segment<3>(0 * 3).norm() - 1.0) < epsilon);
    EXPECT_TRUE(std::abs(analytic_grad.segment<3>(1 * 3).norm() - 1.0) < epsilon);
    EXPECT_TRUE(checkZeroSumCondition<2>(analytic_grad.data()));
}

TEST(ConstraintTest, DistanceDerivative)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_1 = std::make_shared<elasty::Particle>(Vector3d(0.0, 1.0, 0.0), Vector3d::Zero(), 1.0);

    const double dist = (p_0->x - p_1->x).norm();

    const auto constraint = std::make_shared<elasty::DistanceConstraint>(p_0, p_1, 1.0, 0.0, dt, dist);

    const std::shared_ptr<elasty::Particle> particles[] = {p_0, p_1};

    constexpr double epsilon = 1e-04;

    for (auto particle : particles)
    {
        particle->p = particle->x + Vector3d::Random();
    }

    Matrix<double, 6, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    Matrix<double, 6, 1> numerical_grad;
    calculateNumericalDerivative<2>(particles, constraint, numerical_grad.data());

    EXPECT_TRUE((numerical_grad - analytic_grad).cwiseAbs().maxCoeff() < epsilon);
}

TEST(ConstraintTest, ContinuumTriangleDerivative)
{
    constexpr double dt = 1.0 / 60.0;

    constexpr double youngs_modulus = 1000.0;
    constexpr double poisson_ratio  = 0.10;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d(0.0, 0.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_1 = std::make_shared<elasty::Particle>(Vector3d(0.0, 1.0, 0.0), Vector3d::Zero(), 1.0);
    auto p_2 = std::make_shared<elasty::Particle>(Vector3d(1.0, 0.0, 0.0), Vector3d::Zero(), 1.0);

    const auto constraint = std::make_shared<elasty::ContinuumTriangleConstraint>(
        p_0, p_1, p_2, 1.0, 0.0, dt, youngs_modulus, poisson_ratio);

    const std::shared_ptr<elasty::Particle> particles[] = {p_0, p_1, p_2};

    constexpr double epsilon = 1e-04;

    for (auto particle : particles)
    {
        particle->p = particle->x + Vector3d::Random();
    }

    Matrix<double, 9, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    Matrix<double, 9, 1> numerical_grad;
    calculateNumericalDerivative<3>(particles, constraint, numerical_grad.data());

    EXPECT_TRUE((numerical_grad - analytic_grad).cwiseAbs().maxCoeff() < epsilon);
}

TEST(ConstraintTest, FixedPointDerivative)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d::Random(), Vector3d::Zero(), 1.0);

    const auto constraint = std::make_shared<elasty::FixedPointConstraint>(p_0, 1.0, 1.0, dt, Vector3d::Random());

    const std::shared_ptr<elasty::Particle> particles[] = {p_0};

    constexpr double epsilon = 1e-04;

    Matrix<double, 3, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    Matrix<double, 3, 1> numerical_grad;
    calculateNumericalDerivative<1>(particles, constraint, numerical_grad.data());

    EXPECT_TRUE((numerical_grad - analytic_grad).cwiseAbs().maxCoeff() < epsilon);
}

TEST(ConstraintTest, FixedPointRestState)
{
    constexpr double dt = 1.0 / 60.0;

    auto p_0 = std::make_shared<elasty::Particle>(Vector3d::Random(), Vector3d::Zero(), 1.0);

    const auto constraint = std::make_shared<elasty::FixedPointConstraint>(p_0, 1.0, 1.0, dt, p_0->x);

    const std::shared_ptr<elasty::Particle> particles[] = {p_0};

    constexpr double epsilon = 1e-04;

    Matrix<double, 3, 1> analytic_grad;
    constraint->calculateGrad(analytic_grad.data());

    Matrix<double, 3, 1> numerical_grad;
    calculateNumericalDerivative<1>(particles, constraint, numerical_grad.data());

    const double value = constraint->calculateValue();

    EXPECT_TRUE(std::abs(value) < epsilon);
    EXPECT_TRUE(std::abs(analytic_grad.segment<3>(0 * 3).norm() - 1.0) < epsilon);
}

int main(int argc, char** argv)
{
    ::testing::InitGoogleTest(&argc, argv);

    return RUN_ALL_TESTS();
}


#include <boost/math/special_functions/asinh.hpp>


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <memory>
#include <numeric>
#include <random>

#include "MeshLib/Elements/Element.h"
#include "MeshLib/Mesh.h"
#include "MeshLib/MeshGenerators/MeshGenerator.h"
#include "MeshLib/MeshQuality/ElementQualityInterface.h"
#include "MeshLib/Node.h"

class TetElementQuality : public ::testing::Test
{
public:
    TetElementQuality()
    {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<> distrib(1, 10);

        lengths = {1, 1, 1};
        n_subdivisions = {distrib(gen), distrib(gen), distrib(gen)};
    }

    std::vector<double> getElementQualityVectorFromRegularTetMesh(
        MeshLib::MeshQualityType const type)
    {
        std::vector<std::unique_ptr<BaseLib::ISubdivision>> vec_div;
        for (int i = 0; i < 3; ++i)
        {
            vec_div.emplace_back(
                new BaseLib::UniformSubdivision(lengths[i], n_subdivisions[i]));
        }

        std::unique_ptr<MeshLib::Mesh> mesh(
            MeshLib::MeshGenerator::generateRegularTetMesh(
                *vec_div[0], *vec_div[1], *vec_div[2]));
        MeshLib::ElementQualityInterface element_quality(*mesh, type);
        return element_quality.getQualityVector();
    }

    std::array<int, 3> lengths;
    std::array<int, 3> n_subdivisions;
};

TEST_F(TetElementQuality, ElementSize)
{
    auto const type = MeshLib::MeshQualityType::ELEMENTSIZE;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTetMesh(type);
    auto const expected_value =
        1.0 / (6 * std::accumulate(n_subdivisions.begin(), n_subdivisions.end(),
                                   1, std::multiplies<int>()));
    for (auto const element_quality : element_quality_vector)
    {
        ASSERT_NEAR(expected_value, element_quality,
                    std::numeric_limits<double>::epsilon());
    }
}

TEST_F(TetElementQuality, SizeDifference)
{
    auto const type = MeshLib::MeshQualityType::SIZEDIFFERENCE;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTetMesh(type);
    // all elements have the same size, the quality value has to be 1.0
    auto constexpr expected_value = 1.0;
    for (auto const element_quality : element_quality_vector)
    {
        ASSERT_NEAR(expected_value, element_quality,
                    10 * std::numeric_limits<double>::epsilon());
    }
}

TEST_F(TetElementQuality, EdgeRatio)
{
    auto const type = MeshLib::MeshQualityType::EDGERATIO;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTetMesh(type);
    std::array const d = {1.0 / n_subdivisions[0], 1.0 / n_subdivisions[1],
                          1.0 / n_subdivisions[2]};
    std::array const d2 = {std::pow(d[0], 2), std::pow(d[1], 2),
                           std::pow(d[2], 2)};
    auto const& min_max = std::minmax_element(d.begin(), d.end());
    auto const diagonal_length = std::sqrt(d2[0] + d2[1] + d2[2]);
    auto const expected_value_tet0 = *min_max.first / diagonal_length;
    auto const min_x_z = std::min(d[0], d[2]);
    auto const expected_value_tet1 = min_x_z / diagonal_length;
    auto const expected_value_tet2 =
        *min_max.first /
        std::max({std::sqrt(d2[0] + d2[1]), std::sqrt(d2[1] + d2[2]),
                  std::sqrt(d2[0] + d2[2])});
    auto const expected_value_tet3 = expected_value_tet2;
    auto const expected_value_tet4 = expected_value_tet1;
    auto const expected_value_tet5 = expected_value_tet0;

    for (std::size_t i = 0; i < element_quality_vector.size(); i = i + 6)
    {
        ASSERT_NEAR(expected_value_tet0, element_quality_vector[i],
                    10 * std::numeric_limits<double>::epsilon());
        ASSERT_NEAR(expected_value_tet1, element_quality_vector[i + 1],
                    10 * std::numeric_limits<double>::epsilon());
        ASSERT_NEAR(expected_value_tet2, element_quality_vector[i + 2],
                    10 * std::numeric_limits<double>::epsilon());
        ASSERT_NEAR(expected_value_tet3, element_quality_vector[i + 3],
                    10 * std::numeric_limits<double>::epsilon());
        ASSERT_NEAR(expected_value_tet4, element_quality_vector[i + 4],
                    10 * std::numeric_limits<double>::epsilon());
        ASSERT_NEAR(expected_value_tet5, element_quality_vector[i + 5],
                    10 * std::numeric_limits<double>::epsilon());
    }
}

// compute angle formed by (n0, n1, n2)
double getAngleFromTriangle(Eigen::Vector3d n0, Eigen::Vector3d n1,
                            Eigen::Vector3d n2)
{
    auto const u = n1 - n0;
    auto const v = n1 - n2;
    return std::acos(((u.transpose() * v) / (u.norm() * v.norm()))(0, 0));
}

// get min and max angle from triangle
std::pair<double, double> getMinMaxAngleFromTriangle(Eigen::Vector3d n0,
                                                     Eigen::Vector3d n1,
                                                     Eigen::Vector3d n2)
{
    std::array const angles = {getAngleFromTriangle(n0, n1, n2),
                               getAngleFromTriangle(n1, n2, n0),
                               getAngleFromTriangle(n2, n0, n1)};
    auto const pair = std::minmax_element(angles.begin(), angles.end());
    return {*pair.first, *pair.second};
}

double computeCriterionForTet(Eigen::Vector3d n0, Eigen::Vector3d n1,
                              Eigen::Vector3d n2, Eigen::Vector3d n3)
{
    using namespace boost::math::double_constants;
    auto const alpha0 = getMinMaxAngleFromTriangle(n0, n2, n1);
    auto const alpha1 = getMinMaxAngleFromTriangle(n0, n1, n3);
    auto const alpha2 = getMinMaxAngleFromTriangle(n1, n2, n3);
    auto const alpha3 = getMinMaxAngleFromTriangle(n2, n0, n3);
    auto const min =
        std::min({alpha0.first, alpha1.first, alpha2.first, alpha3.first});
    auto const max =
        std::max({alpha0.second, alpha1.second, alpha2.second, alpha3.second});
    return std::max((max - third_pi) / two_thirds_pi,
                    (third_pi - min) / third_pi);
}

TEST_F(TetElementQuality, EquiAngleSkew)
{
    auto const type = MeshLib::MeshQualityType::EQUIANGLESKEW;
    auto const element_quality_vector =
        getElementQualityVectorFromRegularTetMesh(type);
    std::array d = {1.0 / n_subdivisions[0], 1.0 / n_subdivisions[1],
                    1.0 / n_subdivisions[2]};
    // 6 tets are within a hexahedron, the hex nodes are
    Eigen::Vector3d const n0(0, 0, 0);
    Eigen::Vector3d const n1(d[0], 0, 0);
    Eigen::Vector3d const n2(0, d[1], 0);
    Eigen::Vector3d const n3(d[0], d[1], 0);
    Eigen::Vector3d const n4(0, 0, d[2]);
    Eigen::Vector3d const n5(d[0], 0, d[2]);
    Eigen::Vector3d const n6(0, d[1], d[2]);
    Eigen::Vector3d const n7(d[0], d[1], d[2]);
    // tet 0 consist of nodes n0, n3, n2, n4
    auto const expected_value_tet0 = computeCriterionForTet(n0, n3, n2, n4);
    // tet 1 consist of nodes n3, n2, n4, n7
    auto const expected_value_tet1 = computeCriterionForTet(n3, n2, n4, n7);
    // tet 2 consist of nodes n2, n4, n7, n6
    auto const expected_value_tet2 = computeCriterionForTet(n2, n4, n7, n6);
    // tet 3 consist of nodes n0, n1, n3, n5
    auto const expected_value_tet3 = computeCriterionForTet(n0, n1, n3, n5);
    // tet 4 consist of nodes n0, n3, n4, n5
    auto const expected_value_tet4 = computeCriterionForTet(n0, n3, n4, n5);
    // tet 5 consist of nodes n3, n4, n5, n7
    auto const expected_value_tet5 = computeCriterionForTet(n3, n4, n5, n7);

    auto constexpr eps = 11 * std::numeric_limits<double>::epsilon();
    for (std::size_t i = 0; i < element_quality_vector.size(); i = i + 6)
    {
        ASSERT_NEAR(expected_value_tet0, element_quality_vector[i], eps);
        ASSERT_NEAR(expected_value_tet1, element_quality_vector[i + 1], eps);
        ASSERT_NEAR(expected_value_tet2, element_quality_vector[i + 2], eps);
        ASSERT_NEAR(expected_value_tet3, element_quality_vector[i + 3], eps);
        ASSERT_NEAR(expected_value_tet4, element_quality_vector[i + 4], eps);
        ASSERT_NEAR(expected_value_tet5, element_quality_vector[i + 5], eps);
    }
}


/** \file   cmr_parametric_mapping.cpp
    \brief  Implement CMR parametric mapping for 2D acquisition
    \author Hui Xue
*/

#include "cmr_parametric_mapping.h"
#include "log.h"

#include "hoNDArray_reductions.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_math.h"
#include "ho2DArray.h"
#include "ho3DArray.h"

#include "morphology.h"
#include "BSplineFFD2D.h"
#include "hoNDBSpline.h"

#include "hoNDArray_linalg.h"

#include <boost/math/special_functions/sign.hpp>

namespace Gadgetron {

template <typename T>
void perform_hole_filling(hoNDArray<T>& map, T hole, size_t max_size_of_holes, bool is_8_connected)
{
    try
    {
        hoNDArray<unsigned int> label;

        size_t RO = map.get_size(0);
        size_t E1 = map.get_size(1);

        size_t num = map.get_number_of_elements() / (RO*E1);

        std::vector<size_t> dim(2);
        dim[0] = RO;
        dim[1] = E1;

        size_t t;
        for (t = 0; t < num; t++)
        {
            hoNDImage<T, 2> curr_map;
            curr_map.create(dim, map.begin() + t*RO*E1);

            T v = Gadgetron::nrm2(curr_map);
            if (v <= 1e-3) continue; // empty map

            T maxV;
            Gadgetron::maxValue(curr_map, maxV);

            T minV;
            Gadgetron::minValue(map, minV);

            label.clear();
            GADGET_CATCH_THROW(Gadgetron::bwlabel_2d(curr_map, hole, label, is_8_connected));

            std::vector<unsigned int> labels, areas;
            GADGET_CATCH_THROW(Gadgetron::bwlabel_area_2d(label, labels, areas));

            bool needHoleFilling = false;
            size_t numHoles = labels.size();

            size_t n;
            for (n = 0; n < numHoles; n++)
            {
                if (areas[n] <= max_size_of_holes)
                {
                    needHoleFilling = true;
                    break;
                }
            }

            if (needHoleFilling)
            {
                size_t gridSize[2];
                gridSize[0] = 3;
                gridSize[1] = 3;

                size_t numOfRefinement = 0;
                size_t numOfControlPts = gridSize[0];
                while (numOfControlPts < RO)
                {
                    numOfRefinement++;
                    numOfControlPts = 2 * numOfControlPts - 1;
                }

                typedef typename Gadgetron::BSplineFFD2D<T, double, 1>::MaskArrayType MaskArrayType;

                MaskArrayType mask;
                mask.create(RO, E1);
                Gadgetron::fill(mask, float(1));

                for (n = 0; n < RO*E1; n++)
                {
                    unsigned int l = label(n);

                    if (l > 0)
                    {
                        size_t p;
                        for (p = 0; p < labels.size(); p++)
                        {
                            if (l == labels[p]) break;
                        }

                        if (areas[p] <= max_size_of_holes)
                        {
                            mask(n) = 0;
                        }
                    }
                }

                hoNDImage<double, 2> curr_map_used;
                curr_map_used.copyFrom(curr_map);

                double totalResidual(0);
                Gadgetron::BSplineFFD2D<double, double, 1> ffd(curr_map_used, gridSize[0], gridSize[1]);

                ffd.ffdApproxImage(&curr_map_used, mask, totalResidual, numOfRefinement);

                hoNDImage<double, 2> flowDual(curr_map_used);
                ffd.evaluateFFDOnImage(flowDual);

                for (n = 0; n < RO*E1; n++)
                {
                    if (mask(n) == 0)
                    {
                        std::vector<size_t> pt(3);
                        curr_map_used.calculate_index(n, pt);

                        T v(0);
                        v = flowDual(n);
                        if (v >= 0.9*minV && v <= 1.1*maxV) curr_map(n) = (T)(v);
                    }
                }
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Error happened in perform_hole_filling(...) ... ");
    }
}

template EXPORTCMR void perform_hole_filling(hoNDArray<float>& map, float hole, size_t max_size_of_holes, bool is_8_connected);
template EXPORTCMR void perform_hole_filling(hoNDArray<double>& map, double hole, size_t max_size_of_holes, bool is_8_connected);

// ---------------------------------------------------------------------

template <typename T>
void perform_hole_filling(Gadgetron::hoNDImageContainer2D< hoMRImage<T, 2> >& maps, T hole, size_t max_size_of_holes, bool is_8_connected)
{
    try
    {
        size_t row = maps.rows();
        std::vector<size_t> cols = maps.cols();

        size_t r, c;
        for (r = 0; r < row; r++)
        {
            for (c = 0; c < cols[r]; c++)
            {
                GADGET_CATCH_THROW( Gadgetron::perform_hole_filling(maps(r, c), hole, max_size_of_holes, is_8_connected) );
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Error happened in perform_hole_filling(hoNDImageContainer2D) ... ");
    }
}

template EXPORTCMR void perform_hole_filling(Gadgetron::hoNDImageContainer2D< hoMRImage<float, 2> >& maps, float hole, size_t max_size_of_holes, bool is_8_connected);
template EXPORTCMR void perform_hole_filling(Gadgetron::hoNDImageContainer2D< hoMRImage<double, 2> >& maps, double hole, size_t max_size_of_holes, bool is_8_connected);

// ---------------------------------------------------------------------

template <typename T>
CmrParametricMapping<T>::CmrParametricMapping()
{
    fill_holes_in_maps_ = true;
    max_size_of_holes_ = 20;
    hole_marking_value_ = 0;

    compute_SD_maps_ = false;;

    max_iter_ = 50;
    max_fun_eval_ = 100;
    thres_fun_ = 1e-5;

    max_map_value_ = -1;
    min_map_value_ = 0;

    verbose_ = false;
    perform_timing_ = false;

    gt_timer_.set_timing_in_destruction(false);
    gt_timer_local_.set_timing_in_destruction(false);
}

template <typename T>
CmrParametricMapping<T>::~CmrParametricMapping()
{
}

template <typename T>
void CmrParametricMapping<T>::perform_parametric_mapping()
{
    try
    {
        size_t n, s, slc;

        size_t RO = data_.get_size(0);
        size_t E1 = data_.get_size(1);
        size_t N = data_.get_size(2);
        size_t S = data_.get_size(3);
        size_t SLC = data_.get_size(4);

        GADGET_CHECK_THROW(N >= ti_.size());

        size_t num_ti = ti_.size();
        size_t NUM = this->get_num_of_paras();

        map_.create(RO, E1, S, SLC);
        Gadgetron::clear(map_);

        para_.create(RO, E1, NUM, S, SLC);
        Gadgetron::clear(para_);

        if (this->compute_SD_maps_)
        {
            sd_map_.create(RO, E1, S, SLC);
            Gadgetron::clear(sd_map_);

            sd_para_.create(RO, E1, NUM, S, SLC);
            Gadgetron::clear(sd_para_);
        }

        if (!debug_folder_.empty()) gt_exporter_.export_array(this->data_, debug_folder_ + "CmrParametricMapping_data");

        if (this->verbose_)
        {
            this->data_.print(std::cout);

            GDEBUG_STREAM("Time points of mapping : " << ti_.size());
            for (n = 0; n < ti_.size(); n++)
            {
                GDEBUG_STREAM("Time point " << n << " - " << ti_[n]);
            }
        }

        T* pMask = NULL;
        if (mask_for_mapping_.get_size(0) == RO && mask_for_mapping_.get_size(1) == E1 && mask_for_mapping_.get_size(2) == SLC)
        {
            pMask = mask_for_mapping_.get_data_ptr();

            if (!debug_folder_.empty()) gt_exporter_.export_array(this->mask_for_mapping_, debug_folder_ + "CmrParametricMapping_mask_for_mapping");
        }

        if (this->perform_timing_) { gt_timer_.start("perform pixel-wise mapping ... "); }

        long long ro, e1;

        for (slc = 0; slc < SLC; slc++)
        {
            for (s = 0; s < S; s++)
            {
                T* pData = &data_(0, 0, 0, s, slc);

                T* pMap = &map_(0, 0, s, slc);
                T* pPara = &para_(0, 0, 0, s, slc);

                T* pMapSD = NULL;
                T* pParaSD = NULL;
                if (this->compute_SD_maps_)
                {
                    pMapSD = &sd_map_(0, 0, s, slc);
                    pParaSD = &sd_para_(0, 0, 0, s, slc);
                }

                T* pMaskCurr = NULL;
                if (pMask != NULL)
                {
                    pMaskCurr = pMask + s*RO*E1 + slc*S*RO*E1;
                }

#pragma omp parallel private(e1, ro, n) shared(RO, E1, pMask, pMaskCurr, pData, pMap, pMapSD, pPara, pParaSD, num_ti, NUM)
                {
                    std::vector<T> yi(num_ti, 0);
                    std::vector<T> guess(NUM + 1, 0);
                    std::vector<T> bi(NUM + 1, 0);
                    std::vector<T> sd(NUM + 1, 0);

                    T map_v(0), map_sd(0);

#pragma omp for
                    for (e1 = 0; e1 < E1; e1++)
                    {
                        for (ro = 0; ro < RO; ro++)
                        {
                            long long offset = ro + e1*RO;

                            if (pMask != NULL)
                            {
                                if (pMaskCurr[offset] <= 0)
                                    continue;
                            }

                            // get data vector
                            for (n = 0; n < num_ti; n++)
                            {
                                yi[n] = pData[offset + n*RO*E1];
                            }

                            // estimate initial para
                            this->get_initial_guess(ti_, yi, guess);

                            // perform mapping
                            this->compute_map(ti_, yi, guess, bi, map_v);

                            pMap[offset] = map_v;
                            for (n = 0; n < NUM; n++)
                            {
                                pPara[offset + n*RO*E1] = bi[n];
                            }

                            // compute SD if needed
                            if (this->compute_SD_maps_)
                            {
                                try
                                {
                                    this->compute_sd(ti_, yi, bi, sd, map_sd);
                                }
                                catch(...)
                                {
                                    for (n = 0; n < NUM; n++)
                                    {
                                        sd[n] = 0;
                                    }

                                    map_sd = 0;
                                }

                                pMapSD[offset] = map_sd;
                                for (n = 0; n < NUM; n++)
                                {
                                    pParaSD[offset + n*RO*E1] = sd[n];
                                }
                            }
                        }
                    }
                } // openmp
            }
        }

        if (this->perform_timing_) { gt_timer_.stop(); }

        if (!debug_folder_.empty()) gt_exporter_.export_array(this->map_, debug_folder_ + "CmrParametricMapping_map");
        if (!debug_folder_.empty()) gt_exporter_.export_array(this->para_, debug_folder_ + "CmrParametricMapping_para");

        if (this->compute_SD_maps_)
        {
            if (!debug_folder_.empty()) gt_exporter_.export_array(this->sd_map_, debug_folder_ + "CmrParametricMapping_sd_map");
            if (!debug_folder_.empty()) gt_exporter_.export_array(this->sd_para_, debug_folder_ + "CmrParametricMapping_sd_para");
        }

        if (this->fill_holes_in_maps_)
        {
            if (this->perform_timing_) { gt_timer_.start("perform hole filling for pixel-wise mapping ... "); }

            bool is_8_connected = true;

            GADGET_CATCH_THROW( Gadgetron::perform_hole_filling(this->map_, (T)(this->hole_marking_value_), this->max_size_of_holes_, is_8_connected) );
            GADGET_CATCH_THROW( Gadgetron::perform_hole_filling(this->para_, (T)(this->hole_marking_value_), this->max_size_of_holes_, is_8_connected) );

            if (this->compute_SD_maps_)
            {
                GADGET_CATCH_THROW( Gadgetron::perform_hole_filling(this->sd_map_, (T)(this->hole_marking_value_), this->max_size_of_holes_, is_8_connected) );
                GADGET_CATCH_THROW( Gadgetron::perform_hole_filling(this->sd_para_, (T)(this->hole_marking_value_), this->max_size_of_holes_, is_8_connected) );
            }

            if (this->perform_timing_) { gt_timer_.stop(); }

            if (!debug_folder_.empty()) gt_exporter_.export_array(this->map_, debug_folder_ + "CmrParametricMapping_map_after_hole_filling");
            if (!debug_folder_.empty()) gt_exporter_.export_array(this->para_, debug_folder_ + "CmrParametricMapping_para_after_hole_filling");

            if (this->compute_SD_maps_)
            {
                if (!debug_folder_.empty()) gt_exporter_.export_array(this->sd_map_, debug_folder_ + "CmrParametricMapping_sd_map_after_hole_filling");
                if (!debug_folder_.empty()) gt_exporter_.export_array(this->sd_para_, debug_folder_ + "CmrParametricMapping_sd_para_after_hole_filling");
            }
        }

        size_t N_pts = this->map_.get_number_of_elements();

        size_t ii;
        for (ii = 0; ii<N_pts; ii++)
        {
            T v = this->map_(ii);
            if (v >= this->max_map_value_) v = this->max_map_value_;
            if (v <= this->min_map_value_) v = this->min_map_value_;

            this->map_(ii) = v;
        }

        if (this->compute_SD_maps_)
        {
            for (ii = 0; ii<N_pts; ii++)
            {
                T v = this->sd_map_(ii);
                if (v <= 0) v = 0;

                this->sd_map_(ii) = v;
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Error happened in CmrParametricMapping<T>::perform_parametric_mapping ... ");
    }
}

template <typename T>
void CmrParametricMapping<T>::get_initial_guess(const std::vector<T>& ti, const std::vector<T>& yi, std::vector<T>& guess)
{
    guess.clear();
    guess.resize(this->get_num_of_paras(), 0);
}

template <typename T>
void CmrParametricMapping<T>::compute_map(const std::vector<T>& ti, const std::vector<T>& yi, const std::vector<T>& guess, std::vector<T>& bi, T& map_v)
{
    bi = guess;
    map_v = 0;
}

template <typename T>
void CmrParametricMapping<T>::compute_sd(const std::vector<T>& ti, const std::vector<T>& yi, const std::vector<T>& bi, std::vector<T>& sd, T& map_sd)
{
    sd.clear();
    sd.resize(bi.size(), 0);

    map_sd = 0;
}

template <typename T>
void CmrParametricMapping<T>::compute_sd_impl(const VectorType& ti, const VectorType& yi, const VectorType& bi, const VectorType& res, const hoNDArray<T>& grad, VectorType& sd)
{
    try
    {
        sd.clear();
        sd.resize(bi.size(), 0);

        size_t num = ti.size();
        size_t N = this->get_num_of_paras();

        size_t n;

        // rank
        size_t rank = num - (N - 1);

        VectorType res_sorted(res);
        std::sort(res_sorted.begin(), res_sorted.end());

        //hoNDArray<T> res_array(res.size(), const_cast<T*>(&res[0]));
        //hoNDArray<T> res_array
        //Gadgetron::sort(res_array, res_array, true);

        VectorType res_trunc(rank);
        memcpy(&res_trunc[0], &res_sorted[0] + N - 1, sizeof(T)*res_trunc.size());

        T std;
        if (res_trunc.size() % 2 == 0)
        {
            std = (res_trunc[res_trunc.size() / 2] + res_trunc[res_trunc.size() / 2 - 1]) / 2 / 0.6745;
        }
        else
        {
            std = res_trunc[std::floor(res_trunc.size() / 2)] / 0.6745;
        }

        if (std::abs(std) < FLT_EPSILON)
        {
            return; // deviation is too small to compute sd
        }

        hoNDArray<T> hessian(N, N);
        Gadgetron::clear(hessian);

        size_t r, c;
        for (n = 0; n<num; n++)
        {
            for (c = 0; c < N; c++)
            {
                for (r = 0; r < N; r++)
                {
                    hessian(r, c) += grad(r, n) * grad(c, n);
                }
            }
        }

        Gadgetron::scal((T)(1.0 / (std*std)), hessian);

        // take inversion
        Gadgetron::invert(hessian);

        for (n = 0; n < N; n++)
        {
            sd[n] = std::sqrt(hessian(n, n));
        }
    }
    catch (...)
    {
        GADGET_THROW("Exceptions happened in CmrParametricMapping<T>::compute_sd_impl(...) ... ");
    }
}

template <typename T>
size_t CmrParametricMapping<T>::get_num_of_paras() const
{
    return 1;
}

// ------------------------------------------------------------
// Instantiation
// ------------------------------------------------------------

template class EXPORTCMR CmrParametricMapping< float >;

}


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#include <algorithm>
#include <functional>

#include <boost/range/numeric.hpp>

#include "mc/classic/reachability_ast.hh"

namespace pnmc { namespace mc { namespace classic {

using boost::apply_visitor;
using boost::accumulate;

namespace /* anonymous */ {

/*------------------------------------------------------------------------------------------------*/

template <typename T>
struct is
  : boost::static_visitor<bool>
{
                            bool operator()(const T&)     const noexcept {return true;}
  template <typename Other> bool operator()(const Other&) const noexcept {return false;}
};

/*------------------------------------------------------------------------------------------------*/

bool
is_constant(const integer_ast& ast)
noexcept
{
  return apply_visitor(is<integer_constant>{}, ast);
}

/*------------------------------------------------------------------------------------------------*/

struct integer_ast_builder
{
  using result_type = integer_ast;

  const std::unordered_map<std::string /* place id */, pn::valuation_type>& bounds;

  integer_ast_builder(const std::unordered_map<std::string, pn::valuation_type>& b)
    : bounds(b)
  {}

  result_type
  operator()(const properties::integer_constant& e)
  const noexcept
  {
    return integer_constant{e.value};
  }

  template <typename Result>
  struct nary
  {
    const integer_ast_builder& builder;

    template <typename Expression, typename Fn>
    result_type
    operator()(const Expression& e, Fn&& fn)
    const
    {
      /// @todo Merge consecutive sums or products

      auto sub = std::vector<integer_ast>{};
      sub.reserve(e.expressions.size());
      for (const auto& sub_e : e.expressions)
      {
        sub.emplace_back(apply_visitor(builder, sub_e));
      }
      const auto point = std::partition( begin(sub), end(sub)
                                       , [](const auto& s){return not is_constant(s);});
      const auto value = [&]
      {
        int res = 0;
        for (auto cit = point; cit != end(sub); ++cit)
        {
          res = fn(res, boost::get<integer_constant>(*cit).value);
        }
        return res;
      }();
      if (point == end(sub)) // no constants
      {
        return Result{sub};
      }
      else if (point == begin(sub)) // only constants
      {
        return integer_constant{value};
      }
      else
      {
        sub.erase(point, end(sub)); // remove all constants
        sub.emplace_back(integer_constant{value});
        return Result{sub};
      }
    }
  };

  result_type
  operator()(const properties::integer_sum& e)
  const
  {
    return nary<integer_sum>{*this}(e, std::plus<int>{});
  }

  result_type
  operator()(const properties::integer_product& e)
  const
  {
    return nary<integer_product>{*this}(e, std::multiplies<int>{});
  }

  template <typename Result>
  struct binary
  {
    const integer_ast_builder& builder;

    template <typename Expression, typename Fn>
    result_type
    operator()(const Expression& e, Fn&& fn)
    const
    {
      const auto lhs = apply_visitor(builder, e.lhs_expression);
      const auto rhs = apply_visitor(builder, e.rhs_expression);
      if (is_constant(lhs) and is_constant(rhs))
      {
        const auto value = fn( boost::get<integer_constant>(lhs).value
                             , boost::get<integer_constant>(rhs).value);
        return integer_constant{value};
      }
      else
      {
        return Result{lhs, rhs};
      }
    }
  };

  result_type
  operator()(const properties::integer_difference& e)
  const noexcept
  {
    return binary<integer_difference>{*this}(e, std::minus<int>{});
  }

  result_type
  operator()(const properties::integer_division& e)
  const noexcept
  {
    return binary<integer_division>{*this}(e, std::divides<int>{});
  }

  result_type
  operator()(const properties::place_bound& e)
  const
  {
    const auto sum = accumulate(e.places, 0, [&](auto x, const auto& p){return x + bounds.at(p);});
    return integer_constant{sum};
  }

  result_type
  operator()(const properties::tokens_count&)
  const
  {
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }
};

/*------------------------------------------------------------------------------------------------*/

bool
is_true(const boolean_ast& ast)
noexcept
{
  return apply_visitor(is<true_>{}, ast);
}

/*------------------------------------------------------------------------------------------------*/

bool
is_false(const boolean_ast& ast)
noexcept
{
  return apply_visitor(is<false_>{}, ast);
}

/*------------------------------------------------------------------------------------------------*/

struct boolean_ast_builder
{
  using result_type = boolean_ast;
  const bool has_deadlock;
  const std::set<std::string>& dead_transitions;
  const integer_ast_builder& integer_builder;

  boolean_ast_builder( bool deadlock, const std::set<std::string>& dead
                     , const integer_ast_builder& builder)
    : has_deadlock(deadlock), dead_transitions(dead), integer_builder(builder)
  {}

  result_type
  operator()(const properties::invariant&)
  const
  {
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }

  result_type
  operator()(const properties::impossibility& e)
  const
  {
    const auto rec = apply_visitor(*this, e.expression);
    if (is_true(rec))       {return false_{};}
    else if (is_false(rec)) {return true_{};}
    else                    {return impossibility{rec};}
  }

  result_type
  operator()(const properties::possibility& e)
  const
  {
    const auto rec = apply_visitor(*this, e.expression);
    if      (is_true(rec))  {return true_{};}
    else if (is_false(rec)) {return false_{};}
    else                    {return invariant{rec};}
  }

  result_type
  operator()(const properties::deadlock&)
  const noexcept
  {
    return has_deadlock ? result_type{true_{}} : result_type{false_{}};
  }

  result_type
  operator()(const properties::is_fireable& e)
  const noexcept
  {
    for (const auto& t : e.transitions)
    {
      if (dead_transitions.count(t)) {return false_{};}
    }
    return true_{};
  }

  result_type
  operator()(const properties::true_&)
  const noexcept
  {
    return true_{};
  }

  result_type
  operator()(const properties::false_&)
  const noexcept
  {
    return false_{};
  }

  result_type
  operator()(const properties::negation& e)
  const
  {
    const auto rec = apply_visitor(*this, e.expression);
    if      (is_true(rec))  {return false_{};}
    else if (is_false(rec)) {return true_{};}
    else                    {return negation{rec};}
  }

  result_type
  operator()(const properties::conjunction& e)
  const
  {
    auto rec = std::vector<boolean_ast>{};
    rec.reserve(e.expressions.size());
    for (const auto& sub_e : e.expressions)
    {
      auto tmp = apply_visitor(*this, sub_e);
      if      (is_false(tmp)) {return false_{};}
      else if (is_true(tmp))  {continue;}
      rec.emplace_back(std::move(tmp));
    }
    if  (rec.empty()) {return true_{};}
    else              {return conjunction{rec};}
  }

  result_type
  operator()(const properties::disjunction& e)
  const
  {
    auto rec = std::vector<boolean_ast>{};
    rec.reserve(e.expressions.size());
    for (const auto& sub_e : e.expressions)
    {
      auto tmp = apply_visitor(*this, sub_e);
      if      (is_true(tmp))  {return true_{};}
      else if (is_false(tmp)) {continue;}
      rec.emplace_back(std::move(tmp));
    }
    if   (rec.empty()) {return false_{};}
    else               {return disjunction{rec};}
  }

  result_type
  operator()(const properties::exclusive_disjonction& e)
  const
  {
    auto rec = std::vector<boolean_ast>{};
    rec.reserve(e.expressions.size());
    auto has_true = false;
    for (const auto& sub_e : e.expressions)
    {
      auto tmp = apply_visitor(*this, sub_e);
      if      (is_true(tmp) and not has_true) {has_true = true;}
      else if (is_true(tmp) and has_true)     {return false_{};}
      else if (is_false(tmp))                 {continue;}
      rec.emplace_back(std::move(tmp));
    }
    if   (rec.empty()) {return false_{};}
    else               {return exclusive_disjonction{rec};}
  }

  result_type
  operator()(const properties::implication& e)
  const
  {
    const auto lhs = apply_visitor(*this, e.lhs_expression);
    if (is_false(lhs))
    {
      return true_{};
    }
    const auto rhs = apply_visitor(*this, e.lhs_expression);
    if (is_true(lhs))
    {
      return rhs;
    }
    return implication{lhs, rhs};
  }

  result_type
  operator()(const properties::equivalence& e)
  const
  {
    const auto lhs = apply_visitor(*this, e.lhs_expression);
    const auto rhs = apply_visitor(*this, e.rhs_expression);
    if (is_true(lhs))
    {
      if      (is_true(rhs))  {return true_{};}
      else if (is_false(rhs)) {return false_{};}
      else                    {return rhs;}
    }
    else if (is_false(lhs))
    {
      if      (is_true(rhs))  {return false_{};}
      else if (is_false(rhs)) {return true_{};}
      else                    {return negation{rhs};}
    }
    else if (is_true(rhs))
    {
      if      (is_true(lhs))  {return true_{};}
      else if (is_false(lhs)) {return false_{};}
      else                    {return lhs;}
    }
    else if (is_false(rhs))
    {
      if      (is_true(lhs))  {return false_{};}
      else if (is_false(lhs)) {return true_{};}
      else                    {return negation{lhs};}
    }
    else
    {
      return equivalence{lhs, rhs};
    }
  }

  template <typename Result>
  struct integer_cmp
  {
    const integer_ast_builder& builder;

    template <typename Expression, typename Fn>
    result_type
    operator()(const Expression& e, Fn&& fn)
    const
    {
      const auto lhs = apply_visitor(builder, e.lhs_expression);
      const auto rhs = apply_visitor(builder, e.rhs_expression);
      if (is_constant(lhs) and is_constant(rhs))
      {
        return fn(boost::get<integer_constant>(lhs).value, boost::get<integer_constant>(rhs).value)
             ? result_type{true_{}}
             : result_type{false_{}};
      }
      else
      {
        return Result{lhs, rhs};
      }
    }
  };

  result_type
  operator()(const properties::integer_eq& e)
  const
  {
    return integer_cmp<integer_eq>{integer_builder}(e, std::equal_to<int>{});
  }

  result_type
  operator()(const properties::integer_ne& e)
  const
  {
    return integer_cmp<integer_ne>{integer_builder}(e, std::not_equal_to<int>{});
  }

  result_type
  operator()(const properties::integer_lt& e)
  const
  {
    return integer_cmp<integer_lt>{integer_builder}(e, std::less<int>{});
  }

  result_type
  operator()(const properties::integer_le& e)
  const
  {
    return integer_cmp<integer_le>{integer_builder}(e, std::less_equal<int>{});
  }

  result_type
  operator()(const properties::integer_gt& e)
  const
  {
    return integer_cmp<integer_gt>{integer_builder}(e, std::greater<int>{});
  }

  result_type
  operator()(const properties::integer_ge& e)
  const
  {
    return integer_cmp<integer_ge>{integer_builder}(e, std::greater_equal<int>{});
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace anonymous

/*------------------------------------------------------------------------------------------------*/

integer_ast
make_ast( const properties::integer_expression& e
        , const std::unordered_map<std::string, pn::valuation_type>& bounds)
{
  return apply_visitor(integer_ast_builder{bounds}, e);
}

/*------------------------------------------------------------------------------------------------*/

boolean_ast
make_ast( const properties::boolean_expression& e,  bool has_deadlock
        , const std::set<std::string>& dead_transitions
        , const std::unordered_map<std::string, pn::valuation_type>& bounds)
{
  const auto integer = integer_ast_builder{bounds};
  return apply_visitor(boolean_ast_builder{has_deadlock, dead_transitions, integer}, e);
}

/*------------------------------------------------------------------------------------------------*/

}}} // namespace pnmc::mc::classic