/*
 * Copyright 2020 Ryan Levy, Xiongjie Yu, and Bryan K. Clark
 *
 * 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 <iostream>
#include <complex>
#include <Eigen/Dense>

#include "tblis.h"

int main(int argc, char const *argv[]) {
	int s1=2, s2=3, s3=4;
	Eigen::MatrixXcd mA(s1,s2);
	mA.setRandom();
	Eigen::MatrixXcd mB(s2,s3);
	mB.setRandom();
	Eigen::MatrixXcd mC(s1,s3);
	mC.setRandom();
	std::cout << "/-------------------------------------------------/" << '\n';
	std::cout << "Matrix mA:" << '\n';
	std::cout << mA << '\n' << '\n';
	std::cout << "Matrix mB:" << '\n';
	std::cout << mB << '\n' << '\n';
	std::cout << "Matrix mA*mB+mC:" << '\n';
	std::cout << mA*mB+mC << '\n' << '\n';
	std::cout << "/-------------------------------------------------/" << '\n';

	tblis::tensor_view< std::complex<double> > A({s1,s2}, mA.data());
	tblis::tensor_view< std::complex<double> > B({s2,s3}, mB.data());
	tblis::tensor_view< std::complex<double> > C({s1,s3}, mC.data());

	tblis::mult(std::complex<double>(1.0),A,"ab",B,"bc",std::complex<double>(1.0),C,"ac");

	std::cout << "tblis results:" << '\n';
	std::cout << mC << '\n' << '\n';

	std::cout << "/-------------------------------------------------/" << '\n';
  return 0;
}


#include <math_lib/line_3d.h>

#include <gtest/gtest.h>

#include <boost/qvm/vec_operations.hpp>

using namespace pagoda;

TEST(Line3D, when_using_default_constructor_should_create_the_x_axis)
{
	Line3D<float> l;
	ASSERT_TRUE(l.GetPoint() == (Vec3F{0, 0, 0}));
	ASSERT_TRUE(l.GetSupportVector() == (Vec3F{1, 0, 0}));
}

TEST(Line3D, when_using_point_direction_constructor_should_create_the_respective_line)
{
	Line3D<float> l(Vec3F{1, 2, 3}, Vec3F{0, 1, 0});
	ASSERT_TRUE(l.GetPoint() == (Vec3F{1, 2, 3}));
	ASSERT_TRUE(l.GetSupportVector() == (Vec3F{0, 1, 0}));
}

TEST(Line3D, when_using_two_points_static_constructor_should_create_the_respective_line)
{
	Line3D<float> l = Line3D<float>::FromTwoPoints(Vec3F{1, 2, 3}, Vec3F{3, 2, 3});
	ASSERT_TRUE(l.GetPoint() == (Vec3F{1, 2, 3}));
	ASSERT_TRUE(l.GetSupportVector() == (Vec3F{1, 0, 0}));
}

TEST(Line3D, when_comparing_two_equal_lines_should_evaluate_to_equal)
{
	Line3D<float> l1;
	Line3D<float> l2(Vec3F{0, 0, 0}, Vec3F{1, 0, 0});
	ASSERT_TRUE(l1 == l2);
	ASSERT_FALSE(l1 != l2);
}

TEST(Line3D, when_comparing_two_different_lines_should_evaluate_to_not_equal)
{
	Line3D<float> l1(Vec3F{0, 0, 0}, Vec3F{1, 1, 0});
	Line3D<float> l2(Vec3F{0, 0, 0}, Vec3F{1, 0, 0});
	ASSERT_FALSE(l1 == l2);
	ASSERT_TRUE(l1 != l2);
}


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
/**
 * @file omni.hpp
 * @author Boston Cleek
 * @date 30 Oct 2020
 * @brief Kinematic omni directional models control wheel velocities or body twist
 */
#ifndef OMNI_HPP
#define OMNI_HPP

#include <cmath>
#include <armadillo>

#include <ergodic_exploration/numerics.hpp>

namespace ergodic_exploration
{
namespace models
{
using arma::mat;
using arma::vec;

/**
 * @brief Kinematic model of 4 mecanum wheel robot
 * @details The state is [x, y, theta] and controls are the angular velocities of each
 * wheel [u0, u1, u2, u3] corresponding to (front left, front right, rear right, rear
 * left). Assumes the mecanum wheel rollers are at +/- 45 degrees
 */
struct Mecanum
{
  /**
   * @brief Constructor
   * @param wheel_radius - radius of wheel
   * @param wheel_base_x - distance from chassis center to wheel center along x-axis
   * @param wheel_base_y - distance from chassis center to wheel center along y-axis
   */
  Mecanum(double wheel_radius, double wheel_base_x, double wheel_base_y)
    : wheel_radius(wheel_radius)
    , wheel_base_x(wheel_base_x)
    , wheel_base_y(wheel_base_y)
    , state_space(3)
  {
  }

  /**
   * @brief Convert wheel velocities to a body frame twist
   * @param u - control [u0, u1, u2, u3]
   * @return twist in body frame Vb = [vx, vy, w]
   */
  vec wheels2Twist(const vec u) const
  {
    const auto l = 1.0 / (wheel_base_x + wheel_base_y);

    // pseudo inverse of jacobian matrix
    const mat Hp = { { 1.0, 1.0, 1.0, 1.0 }, { -1.0, 1.0, -1.0, 1.0 }, { -l, l, l, -l } };

    const vec vb = (wheel_radius / 4.0) * Hp * u;

    return { vb(0), vb(1), vb(2) };
  }

  /**
   * @brief Kinematic model of 4 mecanum wheel robot
   * @param x - state [x, y, theta]
   * @param u - control [u0, u1, u2, u3]
   * @return [xdot, ydot, thetadot] = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    vec xdot(3);
    const auto s = (wheel_radius / 4.0) * std::sin(x(2));
    const auto c = (wheel_radius / 4.0) * std::cos(x(2));
    const auto l = wheel_radius / (4.0 * (wheel_base_x + wheel_base_y));

    xdot(0) = u(0) * (s + c) + u(1) * (-s + c) + u(2) * (s + c) + u(3) * (-s + c);
    xdot(1) = u(0) * (s - c) + u(1) * (s + c) + u(2) * (s - c) + u(3) * (s + c);
    xdot(2) = -u(0) * l + u(1) * l + u(2) * l - u(3) * l;

    return xdot;
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - control [u0, u1, u2, u3]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);

    const auto s = (wheel_radius / 4.0) * std::sin(x(2));
    const auto c = (wheel_radius / 4.0) * std::cos(x(2));

    const auto df0dth =
        u(0) * (-s + c) + u(1) * (-s - c) + u(2) * (-s + c) + u(3) * (-s - c);
    const auto df1dth =
        u(0) * (s + c) + u(1) * (-s + c) + u(2) * (s + c) + u(3) * (-s + c);

    A(0, 2) = df0dth;
    A(1, 2) = df1dth;

    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x4)
   */
  mat fdu(const vec x) const
  {
    const auto s = (wheel_radius / 4.0) * std::sin(x(2));
    const auto c = (wheel_radius / 4.0) * std::cos(x(2));
    const auto l = wheel_radius / (4.0 * (wheel_base_x + wheel_base_y));

    const mat B = { { s + c, -s + c, s + c, -s + c },
                    { s - c, s + c, s - c, s + c },
                    { -l, l, l, -l } };
    return B;
  }

  double wheel_radius;       // radius of wheel
  double wheel_base_x;       // distance from chassis center to wheel center along x-axis
  double wheel_base_y;       // distance from chassis center to wheel center along y-axis
  unsigned int state_space;  // states space dimension
};

/**
 * @brief Kinematic model of omni directonal robot
 * @details The state is [x, y, theta] and controls are the linear and
 * angular velocities [vx, vy, w] (body twist)
 */
struct Omni
{
  /** @brief Constructor */
  Omni() : state_space(3)
  {
  }

  /**
   * @brief Kinematic model of 4 mecanum wheel robot
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return [xdot, ydot, thetadot] = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    const auto xdot = u(0) * std::cos(x(2)) - u(1) * std::sin(x(2));
    const auto ydot = u(0) * std::sin(x(2)) + u(1) * std::cos(x(2));
    return { xdot, ydot, u(2) };
    // return { xdot, ydot, 0.0 };
    // return { u(0), u(1), u(2) };
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);
    A(0, 2) = -u(0) * std::sin(x(2)) - u(1) * std::cos(x(2));
    A(1, 2) = u(0) * std::cos(x(2)) - u(1) * std::sin(x(2));
    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x3)
   */
  mat fdu(const vec x) const
  {
    // mat B(3,3, arma::fill::eye);
    const mat B = { { std::cos(x(2)), -std::sin(x(2)), 0.0 },
                    { std::sin(x(2)), std::cos(x(2)), 0.0 },
                    { 0.0, 0.0, 1.0 } };
    return B;
  }

  unsigned int state_space;  // states space dimension
};
}  // namespace models
}  // namespace ergodic_exploration
#endif


#include <aslam/backend/MEstimatorPolicies.hpp>

#include <cmath>

#include <sstream>

#include <boost/math/distributions/chi_squared.hpp>

namespace aslam {
namespace backend {

MEstimator::~MEstimator() {}

NoMEstimator::~NoMEstimator() {}
double NoMEstimator::getWeight(double /* squaredError */) const { return 1.0; }
std::string NoMEstimator::name() const { return "none"; }

GemanMcClureMEstimator::GemanMcClureMEstimator(double sigma2) : _sigma2(sigma2) {}
GemanMcClureMEstimator::~GemanMcClureMEstimator() {}
double GemanMcClureMEstimator::getWeight(double error) const {
    double se = _sigma2 + error;
    return (_sigma2) / (se * se);
}
std::string GemanMcClureMEstimator::name() const {
    std::stringstream ss;
    ss << "Geman McClure (" << _sigma2 << ")";
    return ss.str();
}

CauchyMEstimator::CauchyMEstimator(double sigma2) : _sigma2(sigma2) {}
CauchyMEstimator::~CauchyMEstimator() {}
double CauchyMEstimator::getWeight(double error) const {
    double se = error / _sigma2;
    return 1.0 / (1.0 + se);
}
std::string CauchyMEstimator::name() const {
    std::stringstream ss;
    ss << "Cauchy (" << _sigma2 << ")";
    return ss.str();
}

HuberMEstimator::~HuberMEstimator() {}
HuberMEstimator::HuberMEstimator(double k) : _k(k), _k2(k * k) {}
double HuberMEstimator::getWeight(double error) const { return error < _k2 ? 1.0 : _k / sqrt(error); }
std::string HuberMEstimator::name() const {
    std::stringstream ss;
    ss << "Huber(" << _k << ")";
    return ss.str();
}

BlakeZissermanMEstimator::~BlakeZissermanMEstimator() {}
BlakeZissermanMEstimator::BlakeZissermanMEstimator(size_t df, double pCut, double wCut)
    : _df(df), _pCut(pCut), _wCut(wCut), _epsilon(computeEpsilon(df, pCut, wCut)) {}
BlakeZissermanMEstimator::BlakeZissermanMEstimator(const BlakeZissermanMEstimator& other)
    : MEstimator(other), _df(other._df), _pCut(other._pCut), _wCut(other._wCut), _epsilon(other._epsilon) {}
BlakeZissermanMEstimator& BlakeZissermanMEstimator::operator=(const BlakeZissermanMEstimator& other) {
    if (this != &other) {
        MEstimator::operator=(other);
        _df = other._df;
        _pCut = other._pCut;
        _wCut = other._wCut;
        _epsilon = other._epsilon;
    }
    return *this;
}
double BlakeZissermanMEstimator::getWeight(double mahalanobis2) const {
    return exp(-mahalanobis2) / (exp(-mahalanobis2) + _epsilon);
}
std::string BlakeZissermanMEstimator::name() const {
    std::stringstream ss;
    ss << "Blake-Zisserman(" << _epsilon << ")";
    return ss.str();
}
double BlakeZissermanMEstimator::chi2InvCDF(double p, size_t df) const {
    return boost::math::quantile(boost::math::chi_squared_distribution<>(df), p);
}
double BlakeZissermanMEstimator::computeEpsilon(size_t df, double pCut, double wCut) const {
    return (1 - wCut) / wCut * exp(-chi2InvCDF(pCut, df));
}

}  // namespace backend
}  // namespace aslam


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include math.hpp directly!"
#endif

#pragma once

#include <boost/math/constants/constants.hpp>

#include <cmath>

namespace matheval {

namespace math {

/// @brief Sign function
template <typename T>
T sgn(T x) {
    return (T(0) < x) - (x < T(0));
}

/// @brief isnan function with adjusted return type
template <typename T>
T isnan(T x) {
    return std::isnan(x);
}

/// @brief isinf function with adjusted return type
template <typename T>
T isinf(T x) {
    return std::isinf(x);
}

/// @brief Convert radians to degrees
template <typename T>
T deg(T x) {
    return x * boost::math::constants::radian<T>();
}

/// @brief Convert degrees to radians
template <typename T>
T rad(T x) {
    return x * boost::math::constants::degree<T>();
}

/// @brief unary plus
template <typename T>
T plus(T x) {
    return x;
}

/// @brief binary plus
template <typename T>
T plus(T x, T y) {
    return x + y;
}

/// @brief unary minus
template <typename T>
T minus(T x) {
    return -x;
}

/// @brief binary minus
template <typename T>
T minus(T x, T y) {
    return x - y;
}

/// @brief multiply
template <typename T>
T multiplies(T x, T y) {
    return x * y;
}

/// @brief divide
template <typename T>
T divides(T x, T y) {
    return x / y;
}

/// @brief unary not
template <typename T>
T unary_not(T x) {
    return !x;
}

/// @brief logical and
template <typename T>
T logical_and(T x, T y) {
    return x && y;
}

/// @brief logical or
template <typename T>
T logical_or(T x, T y) {
    return x || y;
}

/// @brief less
template <typename T>
T less(T x, T y) {
    return x < y;
}

/// @brief less equals
template <typename T>
T less_equals(T x, T y) {
    return x <= y;
}

/// @brief greater
template <typename T>
T greater(T x, T y) {
    return x > y;
}

/// @brief greater equals
template <typename T>
T greater_equals(T x, T y) {
    return x >= y;
}

/// @brief equals
template <typename T>
T equals(T x, T y) {
    return x == y;
}

/// @brief not equals
template <typename T>
T not_equals(T x, T y) {
    return x != y;
}

} // namespace math

} // namespace matheval


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
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 or http://www.mitk.org for details.

===================================================================*/

#include "mitkCommandLineParser.h"
#include "mitkImage.h"
#include "mitkImageStatisticsCalculator.h"
#include "mitkIOUtil.h"
#include <iostream>
#include <usAny.h>
#include <fstream>
#include <itkImageRegionConstIterator.h>
#include "mitkImageAccessByItk.h"
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <mitkImageMaskGenerator.h>
#include <mitkIgnorePixelMaskGenerator.h>

struct statistics_res{
    double mean, variance, min, max, count, moment;
};

void printstats(statistics_res s)
{
    std::cout << "mean: " << s.mean << std::endl
              << "variance: " << s.variance << std::endl
              << "min: " << s.min << std::endl
              << "max: " << s.max << std::endl
              << "count: " << s.count << std::endl
              << "moment: " << s.moment << std::endl;
}

template < typename TPixel, unsigned int VImageDimension >
void get_statistics_boost(itk::Image<TPixel, VImageDimension>* itkImage, statistics_res& res){
    typedef itk::Image<TPixel, VImageDimension> ImageType;

    itk::ImageRegionConstIterator<ImageType> it(itkImage, itkImage->GetLargestPossibleRegion());

    TPixel currentPixel;
    int ctr=0;
    double sum=0;

    boost::accumulators::accumulator_set<double, boost::accumulators::stats<
            boost::accumulators::tag::mean,
            boost::accumulators::tag::variance,
            boost::accumulators::tag::min,
            boost::accumulators::tag::max,
            boost::accumulators::tag::count,
            boost::accumulators::tag::moment<2>> > acc;

    for (it.GoToBegin(); !it.IsAtEnd(); ++it)
    {
        acc(it.Get());
//        currentPixel = it.Get();
//        sum+=currentPixel;
//        ctr+=1;
    }

//    res.mean=(double)sum/(double)ctr;
    res.mean = boost::accumulators::mean(acc);
    res.variance = boost::accumulators::variance(acc);
    res.min = boost::accumulators::min(acc);
    res.max = boost::accumulators::max(acc);
    res.count = boost::accumulators::count(acc);
    res.moment = boost::accumulators::moment<2>(acc);

    std::cout << "sum: " << sum << " N: " <<  ctr << " mean: " << res.mean << std::endl;
}

int main( int argc, char* argv[] )
{
    mitkCommandLineParser parser;

    parser.setTitle("Extract Image Statistics");
    parser.setCategory("Preprocessing Tools");
    parser.setDescription("");
    parser.setContributor("MBI");

    parser.setArgumentPrefix("--", "-");
    parser.addArgument("help", "h", mitkCommandLineParser::String, "Help:", "Show this help text");
    parser.addArgument("input", "i", mitkCommandLineParser::InputFile, "Input:", "input image", us::Any(),false);
    parser.addArgument("mask", "m", mitkCommandLineParser::InputFile, "Mask:", "mask image / roi image denotin area on which statistics are calculated", us::Any(),true);
    parser.addArgument("out", "o", mitkCommandLineParser::OutputFile, "Output", "output file (default: filenameOfRoi.nrrd_statistics.txt)", us::Any());

    std::cout << "test...." << std::endl;

    std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    std::cout << "parsedArgs.size()= " << parsedArgs.size() << std::endl;
    if (parsedArgs.size()==0 || parsedArgs.count("help") || parsedArgs.count("h"))
    {
      std::cout << "\n\n MiniApp Description: \nCalculates statistics on the supplied image using given mask." << endl;
      std::cout << "Output is written to the designated output file in this order:" << endl;
      std::cout << "Mean, Standard Deviation, RMS, Max, Min, Number of Voxels, Volume [mm3]" << endl;
      std::cout << "\n\n Parameters:"<< endl;
      std::cout << parser.helpText();
      return EXIT_SUCCESS;
    }

    // Parameters:
    bool ignoreZeroValues = false;
    unsigned int timeStep = 0;

    std::string inputImageFile = us::any_cast<std::string>(parsedArgs["input"]);
    mitk::Image::Pointer maskImage;
    if (parsedArgs.count("mask") || parsedArgs.count("m"))
    {
        std::string maskImageFile = us::any_cast<std::string>(parsedArgs["mask"]);
        maskImage = mitk::IOUtil::LoadImage(maskImageFile);
    }

    std::string outFile;
    if (parsedArgs.count("out") || parsedArgs.count("o") )
      outFile = us::any_cast<std::string>(parsedArgs["out"]);
    else
      outFile = inputImageFile + "_statistics.txt";

    // Load image and mask
    mitk::Image::Pointer inputImage = mitk::IOUtil::LoadImage(inputImageFile);

    // Calculate statistics
    mitk::ImageStatisticsCalculator::StatisticsContainer::Pointer statisticsStruct;
    mitk::ImageStatisticsCalculator::Pointer calculator = mitk::ImageStatisticsCalculator::New();
    try
    {
      calculator->SetInputImage(inputImage);
      if (parsedArgs.count("mask") || parsedArgs.count("m"))
      {
          mitk::ImageMaskGenerator::Pointer imgMask = mitk::ImageMaskGenerator::New();
          imgMask->SetImageMask(maskImage);
          imgMask->SetTimeStep(timeStep);
          calculator->SetMask(imgMask.GetPointer());
      }
      else
      {
          calculator->SetMask(nullptr);
      }

    }
    catch( const itk::ExceptionObject& e)
    {
      MITK_ERROR << "Statistic Calculation Failed - ITK Exception:" << e.what();
      return -1;
    }

    if (ignoreZeroValues)
    {
        // TODO, cannot have more than one mask, using ignore pixel value will override the image mask :-c
        mitk::IgnorePixelMaskGenerator::Pointer ignorePixelMask = mitk::IgnorePixelMaskGenerator::New();
        ignorePixelMask->SetInputImage(inputImage);
        ignorePixelMask->SetTimeStep(timeStep);
        ignorePixelMask->SetIgnoredPixelValue(0);
        calculator->SetMask(ignorePixelMask.GetPointer());
    }

    std::cout << "calculating statistics itk: " << std::endl;
    try
    {
        statisticsStruct = calculator->GetStatistics(timeStep);
    }
    catch ( mitk::Exception& e)
    {
      MITK_ERROR<< "MITK Exception: " << e.what();
      return -1;
    }

    // Calculate Volume
    double volume = 0;
    const mitk::BaseGeometry *geometry = inputImage->GetGeometry();
    if ( geometry != NULL )
    {
      const mitk::Vector3D &spacing = inputImage->GetGeometry()->GetSpacing();
      volume = spacing[0] * spacing[1] * spacing[2] * (double) statisticsStruct->GetN();
    }

    // Write Results to file
    std::ofstream output;
    output.open(outFile.c_str());
    output << statisticsStruct->GetMean() << " , ";
    output << statisticsStruct->GetStd() << " , ";
    output << statisticsStruct->GetRMS() << " , ";
    output << statisticsStruct->GetMax() << " , ";
    output << statisticsStruct->GetMin() << " , ";
    output << statisticsStruct->GetN() << " , ";
    output << volume << "\n";

    output.flush();
    output.close();

    std::cout << "calculating statistics boost: " << std::endl;

    statistics_res res;
    AccessByItk_n(inputImage, get_statistics_boost, (res));

    printstats(res);

    return EXIT_SUCCESS;
}


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [aligned_input_range]
#include <boost/simd/range/aligned_input_range.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>
#include <vector>

int main()
{
  std::vector<float, boost::simd::allocator<float>> x{1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};

  boost::simd::pack<float> r{0};
  auto pr = boost::simd::aligned_input_range(x);
  for(auto const& e : pr) r += e;

  std::cout << "Sum of [1 ... 16] is " << boost::simd::sum(r) << std::endl;

  return 0;
}
//! [aligned_input_range]


// Copyright 2019 Hans Dembinski
//
// 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)

//[ guide_fill_weighted_profile

#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <cassert>
#include <iostream>
#include <sstream>

int main() {
  using namespace boost::histogram;
  using namespace boost::histogram::literals; // _c suffix creates compile-time numbers

  // make 2D weighted profile
  auto h = make_weighted_profile(axis::integer<>(0, 2), axis::integer<>(0, 2));

  // The mean is computed from the values marked with the sample() helper function.
  // Weights can be passed as well. The `sample` and `weight` arguments can appear in any
  // order, but they must be the first or last arguments.
  h(0, 0, sample(1));            // sample goes to cell (0, 0); weight is 1
  h(0, 0, sample(2), weight(3)); // sample goes to cell (0, 0); weight is 3
  h(1, 0, sample(3));            // sample goes to cell (1, 0); weight is 1
  h(1, 0, sample(4));            // sample goes to cell (1, 0); weight is 1
  h(0, 1, sample(5));            // sample goes to cell (1, 0); weight is 1
  h(0, 1, sample(6));            // sample goes to cell (1, 0); weight is 1
  h(1, 1, weight(4), sample(7)); // sample goes to cell (1, 1); weight is 4
  h(weight(5), sample(8), 1, 1); // sample goes to cell (1, 1); weight is 5

  std::ostringstream os;
  for (auto&& x : indexed(h)) {
    const auto i = x.index(0_c);
    const auto j = x.index(1_c);
    const auto m = x->value();    // weighted mean
    const auto v = x->variance(); // estimated variance of weighted mean
    os << boost::format("index %i,%i mean %.1f variance %.1f\n") % i % j % m % v;
  }

  std::cout << os.str() << std::flush;

  assert(os.str() == "index 0,0 mean 1.8 variance 0.5\n"
                     "index 1,0 mean 3.5 variance 0.5\n"
                     "index 0,1 mean 5.5 variance 0.5\n"
                     "index 1,1 mean 7.6 variance 0.5\n");
}

//]


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

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/random.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // lazy weighted_variance
    accumulator_set<int, stats<tag::weighted_variance(lazy)>, int> acc1;

    acc1(1, weight = 2);    //  2
    acc1(2, weight = 3);    //  6
    acc1(3, weight = 1);    //  3
    acc1(4, weight = 4);    // 16
    acc1(5, weight = 1);    //  5

    // weighted_mean = (2+6+3+16+5) / (2+3+1+4+1) = 32 / 11 = 2.9090909090909090909090909090909

    BOOST_CHECK_EQUAL(5u, count(acc1));
    BOOST_CHECK_CLOSE(2.9090909, weighted_mean(acc1), 1e-5);
    BOOST_CHECK_CLOSE(10.1818182, weighted_moment<2>(acc1), 1e-5);
    BOOST_CHECK_CLOSE(1.7190083, weighted_variance(acc1), 1e-5);

    accumulator_set<int, stats<tag::weighted_variance>, int> acc2;

    acc2(1, weight = 2);
    acc2(2, weight = 3);
    acc2(3, weight = 1);
    acc2(4, weight = 4);
    acc2(5, weight = 1);

    BOOST_CHECK_EQUAL(5u, count(acc2));
    BOOST_CHECK_CLOSE(2.9090909, weighted_mean(acc2), 1e-5);
    BOOST_CHECK_CLOSE(1.7190083, weighted_variance(acc2), 1e-5);

    // check lazy and immediate variance with random numbers

    // two random number generators
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma(0,1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);

    accumulator_set<double, stats<tag::weighted_variance(lazy)>, double > acc_lazy;
    accumulator_set<double, stats<tag::weighted_variance>, double > acc_immediate;

    for (std::size_t i=0; i<10000; ++i)
    {
        double value = normal();
        acc_lazy(value, weight = rng());
        acc_immediate(value, weight = rng());
    }

    BOOST_CHECK_CLOSE(1., weighted_variance(acc_lazy), 1.);
    BOOST_CHECK_CLOSE(1., weighted_variance(acc_immediate), 1.);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_variance test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-ieee
    Function object implementing ldexp capabilities

    The function multiply a floating entry \f$x\f$
    by \f$2^{n}\f$

    @ref cardinal_of the types of x and n must be identical

    @par Semantic:

    @code
    T r = ldexp(x,n);
    @endcode

    is similar to:

    @code
    T r = x*pow(2, n);
    @endcode

  **/
  const boost::dispatch::functor<tag::ldexp_> ldexp = {};
} }
#endif

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

#endif


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <boost/math/special_functions/fpclassify.hpp> // isnan
#include <vinecopulib/bicop/family.hpp>
#include <vinecopulib/misc/tools_interpolation.hpp>
#include <vinecopulib/misc/tools_stats.hpp>
#include <wdm/eigen.hpp>

namespace vinecopulib {
inline TllBicop::TllBicop()
{
  family_ = BicopFamily::tll;
}

inline Eigen::VectorXd
TllBicop::gaussian_kernel_2d(const Eigen::MatrixXd& x)
{
  return tools_stats::dnorm(x).rowwise().prod();
}

//! selects the bandwidth matrix for local líkelihood estimator (covariance
//! times appropriate factor).
inline Eigen::Matrix2d
TllBicop::select_bandwidth(const Eigen::MatrixXd& x,
                           std::string method,
                           const Eigen::VectorXd& weights)
{
  size_t n = x.rows();
  double cor = wdm::wdm(x, "cor", weights)(0, 1);
  cor = std::min(std::max(cor, -0.95), 0.95);
  Eigen::Matrix2d cov = Eigen::MatrixXd::Identity(2, 2);
  cov(0, 1) = cor;
  cov(1, 0) = cor;

  double mult;
  if (method == "constant") {
    mult = std::pow(n, -1.0 / 3.0);
  } else {
    double degree;
    if (method == "linear") {
      degree = 1.0;
    } else {
      degree = 2.0;
    }
    mult = 1.5 * std::pow(n, -1.0 / (2.0 * degree + 1.0));
  }
  double mcor = tools_stats::pairwise_mcor(x, weights);
  double scale = std::pow(std::fabs(cor / mcor), 0.5 * mcor);

  return mult * cov * scale;
}

//! calculates the cholesky root of a 2x2 matrix.
inline Eigen::Matrix2d
chol22(const Eigen::Matrix2d& B)
{

  Eigen::Matrix2d rB;

  rB(0, 0) = std::sqrt(B(0, 0));
  rB(0, 1) = 0.0;
  rB(1, 0) = B(1, 0) / rB(0, 0);
  rB(1, 1) = std::sqrt(B(1, 1) - rB(1, 0) * rB(1, 0));

  return rB;
}

//! evaluates local likleihood density estimate.
//!
//! @param x Evaluation points.
//! @param x_data Observations.
//! @param B Bandwidth matrix.
//! @param method Order of local polynomial approximation; either `"constant"`,
//!   `"linear"`, or `"quadratic"`.
//! @param weights Vector of weights for the observations
//! @return a two-column matrix; first column is estimated density, second
//!    column is influence of evaluation point.
inline Eigen::MatrixXd
TllBicop::fit_local_likelihood(const Eigen::MatrixXd& x,
                               const Eigen::MatrixXd& x_data,
                               const Eigen::Matrix2d& B,
                               std::string method,
                               const Eigen::VectorXd& weights)
{
  size_t m = x.rows();      // number of evaluation points
  size_t n = x_data.rows(); // number of observations

  // pre-calculate inverse root of bandwidth matrix and determinant
  Eigen::Matrix2d irB = chol22(B).inverse();
  double det_irB = irB.determinant();

  // de-correlate data by applying B^{-1/2}
  Eigen::MatrixXd z = (irB * x.transpose()).transpose();
  Eigen::MatrixXd z_data = (irB * x_data.transpose()).transpose();

  Eigen::MatrixXd res(m, 2);
  res.col(0) = Eigen::VectorXd::Ones(m); // result will be a product
  Eigen::VectorXd kernels(n);
  Eigen::Vector2d f1;
  Eigen::Vector2d b;
  Eigen::Matrix2d S(B);
  Eigen::MatrixXd zz(n, 2), zz2(n, 2);
  for (size_t k = 0; k < m; ++k) {
    zz = z_data - z.row(k).replicate(n, 1);
    kernels = gaussian_kernel_2d(zz) * det_irB;
    if (weights.size() > 0)
      kernels = kernels.cwiseProduct(weights);
    double f0 = kernels.mean();
    if (method != "constant") {
      zz = (irB * zz.transpose()).transpose();
      f1 = zz.cwiseProduct(kernels.replicate(1, 2)).colwise().mean();
      b = f1 / f0;
      if (method == "quadratic") {
        zz2 = zz.cwiseProduct(kernels.replicate(1, 2)) /
              (f0 * static_cast<double>(n));
        b = B * b;
        S = (B * (zz.transpose() * zz2) * B - b * b.transpose()).inverse();
        res(k) *= std::sqrt(S.determinant()) / det_irB;
      }
      res(k) *= std::exp(-0.5 * double(b.transpose() * S * b));
      if ((boost::math::isnan)(res(k)) | (boost::math::isinf)(res(k))) {
        // inverse operation might go wrong due to rounding when
        // true value is equal or close to zero
        res(k) = 0.0;
      }
    }
    res(k, 0) *= f0;
    if (weights.size() > 0) {
      // average weight in neighborhood of evaluation point (essentially a
      // kernel regression estimate);
      // kernels have already been multiplied with weights above
      double w = kernels.sum() / kernels.cwiseQuotient(weights).sum();
      res(k, 1) = calculate_infl(n, f0, b, B, det_irB, S, method, w);
    } else {
      res(k, 1) = calculate_infl(n, f0, b, B, det_irB, S, method, 1.0);
    }
  }

  if (weights.size() > 0) {
    // estimate can be negative if negative weights are used
    res.col(0) = res.col(0).array().max(0.0);
  }

  return res;
}

//! calculate influence for data point for density estimate based on
//! quantities pre-computed in `fit_local_likelihood()`.
inline double
TllBicop::calculate_infl(const size_t& n,
                         const double& f0,
                         const Eigen::Vector2d& b,
                         const Eigen::Matrix2d& B,
                         const double& det_irB,
                         const Eigen::Matrix2d& S,
                         const std::string& method,
                         const double& weight)
{
  Eigen::MatrixXd M;
  if (method == "constant") {
    M = Eigen::MatrixXd::Constant(1, 1, f0);
  } else if (method == "linear") {
    M = Eigen::MatrixXd(3, 3);
    M(0, 0) = f0;
    M.col(0).tail(2) = B * b * f0;
    M.row(0).tail(2) = M.col(0).tail(2);
    M.block(1, 1, 2, 2) = f0 * B + f0 * B * b * b.transpose() * B;
  } else if (method == "quadratic") {
    M = Eigen::MatrixXd::Zero(6, 6);
    M(0, 0) = f0;
    M.col(0).segment(1, 2) = f0 * b;
    M.row(0).segment(1, 2) = M.col(0).segment(1, 2);
    M.block(1, 1, 2, 2) = f0 * B + f0 * b * b.transpose();
    M(3, 0) = 0.5 * M(1, 1);
    M(4, 0) = 0.5 * M(2, 2);
    M(5, 0) = M(1, 2);
    M.row(0).tail(3) = M.col(0).tail(3);
    Eigen::MatrixXd Si = S.inverse();
    M(3, 1) = 0.5 * f0 * (3.0 * Si(0, 0) * b(0) + std::pow(b(0), 3));
    M(4, 2) = 0.5 * f0 * (3.0 * Si(1, 1) * b(1) + std::pow(b(1), 3));
    M(4, 1) = 0.5 * f0;
    M(4, 1) *= 2.0 * Si(0, 1) * b(1) + Si(1, 1) * b(0) + b(0) * b(1) * b(1);
    M(3, 2) = 0.5 * f0;
    M(3, 2) *= 2.0 * Si(0, 1) * b(0) + Si(0, 0) * b(1) + b(1) * b(0) * b(0);
    M(5, 1) = 2.0 * M(3, 2);
    M(5, 2) = 2.0 * M(4, 1);
    M.block(1, 3, 2, 3) = M.block(3, 1, 3, 2).transpose();
    M(3, 3) = 0.25 * f0;
    M(3, 3) *= 3.0 * Si(0, 0) * Si(0, 0) + 6.0 * Si(0, 0) * b(0) * b(0) +
               std::pow(b(0), 4);
    M(4, 4) = 0.25 * f0;
    M(4, 4) *= 3.0 * Si(1, 1) * Si(1, 1) + 6.0 * Si(1, 1) * b(1) * b(1) +
               std::pow(b(1), 4);
    M(5, 5) = Si(0, 0) * Si(1, 1) + 2.0 * S(0, 1) + b(0) * b(0) * b(1) * b(1);
    M(5, 5) += 4.0 * Si(0, 1) * b(0) * b(1);
    M(5, 5) += Si(0, 0) * b(1) * b(1) + Si(1, 1) * b(0) * b(0);
    M(5, 5) *= f0;
    M(4, 3) = M(5, 5) * 0.25;
    M(3, 4) = M(4, 3);
    M(5, 3) = 3.0 * Si(0, 0) * Si(0, 1) + 3.0 * Si(0, 1) * b(0) * b(0);
    M(5, 3) += 3.0 * Si(0, 0) * b(0) * b(1) + b(1) * std::pow(b(0), 3);
    M(5, 3) *= 0.5 * f0;
    M(3, 5) = M(5, 3);
    M(5, 4) = 3.0 * Si(1, 1) * Si(0, 1) + 3.0 * Si(0, 1) * b(1) * b(1);
    M(5, 4) += 3.0 * Si(1, 1) * b(0) * b(1) + b(0) * std::pow(b(1), 3);
    M(5, 4) *= 0.5 * f0;
    M(4, 5) = M(5, 4);
  }

  double infl = gaussian_kernel_2d(Eigen::MatrixXd::Zero(1, 2))(0) * det_irB;
  infl *= M.inverse()(0, 0) * weight / static_cast<double>(n);
  return infl;
}

inline void
TllBicop::fit(const Eigen::MatrixXd& data,
              std::string method,
              double mult,
              const Eigen::VectorXd& weights)
{
  using namespace tools_interpolation;

  // construct default grid (equally spaced on Gaussian scale)
  size_t m = 30;
  auto grid_points = this->make_normal_grid(m);

  // expand the interpolation grid; a matrix with two columns where each row
  // contains one combination of the grid points
  auto grid_2d = tools_eigen::expand_grid(grid_points);

  // transform evaluation grid and data by inverse Gaussian cdf
  Eigen::MatrixXd z = tools_stats::qnorm(grid_2d);

  // use jittering in case observations are discrete
  auto psobs = tools_stats::to_pseudo_obs(data.leftCols(2), "random");
  Eigen::MatrixXd z_data = tools_stats::qnorm(psobs);

  // find bandwidth matrix
  Eigen::Matrix2d B = select_bandwidth(z_data, method, weights);
  B *= mult;

  // compute the density estimator (first column estimate, second influence)
  Eigen::MatrixXd ll_fit = fit_local_likelihood(z, z_data, B, method, weights);

  // transform density estimate to copula scale
  Eigen::VectorXd c =
    ll_fit.col(0).cwiseQuotient(tools_stats::dnorm(z).rowwise().prod());
  // store values in mxm grid
  Eigen::MatrixXd values(m, m);
  values = Eigen::Map<Eigen::MatrixXd>(c.data(), m, m).transpose();

  // for interpolation, we shift the limiting gridpoints to 0 and 1
  grid_points(0) = 0.0;
  grid_points(m - 1) = 1.0;
  interp_grid_ = std::make_shared<InterpolationGrid>(grid_points, values);

  // compute effective degrees of freedom via interpolation ---------
  // stabilize interpolation by restricting to plausible range
  Eigen::VectorXd infl_vec = ll_fit.col(1).cwiseMin(1.3).cwiseMax(-0.2);
  Eigen::MatrixXd infl(m, m);
  infl = Eigen::Map<Eigen::MatrixXd>(infl_vec.data(), m, m).transpose();
  // don't normalize margins of the EDF! (norm_times = 0)
  auto infl_grid = InterpolationGrid(grid_points, infl, 0);
  if ((var_types_[0] == "d") | (var_types_[1] == "d")) {
    // for discrete, use mid ranks to compute EDF and log-likelihood
    // (this is closer to "observations" than jittered or "upper" pseudo data)
    psobs = 0.5 * (data.leftCols(2) + data.rightCols(2)).array();
    npars_ = tools_eigen::unique(infl_grid.interpolate(psobs)).sum();
    npars_ = std::max(npars_, 1.0);
  } else {
    npars_ = std::max(infl_grid.interpolate(data).sum(), 1.0);
  }
  set_loglik(pdf(data).array().log().sum());
}
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Alpha_shape_3.h>
#include <CGAL/Alpha_shape_cell_base_3.h>
#include <CGAL/Alpha_shape_vertex_base_3.h>
#include <CGAL/Delaunay_triangulation_3.h>

#include <fstream>
#include <vector>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Gt;

typedef CGAL::Alpha_shape_vertex_base_3<Gt>          Vb;
typedef CGAL::Alpha_shape_cell_base_3<Gt>            Fb;
typedef CGAL::Triangulation_data_structure_3<Vb,Fb>  Tds;
typedef CGAL::Delaunay_triangulation_3<Gt,Tds>       Triangulation_3;
typedef CGAL::Alpha_shape_3<Triangulation_3>         Alpha_shape_3;

typedef Gt::Point_3                                  Point;
typedef Alpha_shape_3::Alpha_iterator                Alpha_iterator;

int main()
{
  std::vector<Point> points;

//read input
  std::ifstream is("data/bunny_5000");
  int n;
  is >> n;
  double x, y, z;
  for (int i=0;i<n;++i)
  {
    is >> x >> y >> z;
    points.push_back( Point(x, y, z) );
  }

  std::cerr << points.size() << " points read.\n";
// compute alpha shape
  Alpha_shape_3 as(points.begin(), points.end());
  Alpha_shape_3::NT alpha_solid = as.find_alpha_solid();
  as.set_alpha(alpha_solid);

  std::cerr << "alpha_solid = " << alpha_solid << "\n";
  std::cerr << as.number_of_solid_components() << " number of solid components\n";

// collect alpha-shape facets accessible from the infinity
  // marks the cells that are in the same component as the infinite vertex by flooding
  boost::unordered_set< Alpha_shape_3::Cell_handle > marked_cells;
  std::vector< Alpha_shape_3::Cell_handle > queue;
  queue.push_back( as.infinite_cell() );

  while(!queue.empty())
  {
    Alpha_shape_3::Cell_handle back = queue.back();
    queue.pop_back();

    if ( !marked_cells.insert(back).second ) continue; //already visited

    for (int i=0; i<4; ++i)
    {
      if (as.classify(Alpha_shape_3::Facet(back, i))==Alpha_shape_3::EXTERIOR &&
          marked_cells.count(back->neighbor(i))==0)
        queue.push_back( back->neighbor(i) );
    }
  }

  // filter regular facets to restrict them to those adjacent to a marked cell
  std::vector< Alpha_shape_3::Facet > regular_facets;
  as.get_alpha_shape_facets(std::back_inserter( regular_facets ), Alpha_shape_3::REGULAR );

  std::vector<Alpha_shape_3::Facet> filtered_regular_facets;
  for(Alpha_shape_3::Facet f : regular_facets)
  {
    if ( marked_cells.count(f.first)==1 )
      filtered_regular_facets.push_back(f);
    else
    {
      f = as.mirror_facet(f);
      if ( marked_cells.count(f.first)==1 )
        filtered_regular_facets.push_back(f);
    }
  }

// dump into OFF format
  // assign an id per vertex
  boost::unordered_map< Alpha_shape_3::Vertex_handle, std::size_t> vids;
  points.clear();

  for(Alpha_shape_3::Facet f : filtered_regular_facets)
  {
    for (int i=1;i<4; ++i)
    {
      Alpha_shape_3::Vertex_handle vh = f.first->vertex((f.second+i)%4);
      if (vids.insert( std::make_pair(vh, points.size()) ).second)
        points.push_back( vh->point() );
    }
  }

  // writing
  std::ofstream output("out.off");
  output << "OFF\n " << points.size() << " " << filtered_regular_facets.size() << " 0\n";
  std::copy(points.begin(), points.end(), std::ostream_iterator<Point>(output, "\n"));
  for(const Alpha_shape_3::Facet& f : filtered_regular_facets)
  {
    output << 3;

    for (int i=0;i<3; ++i)
    {
      Alpha_shape_3::Vertex_handle vh = f.first->vertex( as.vertex_triple_index(f.second, i) );
      output << " " << vids[vh];
    }
    output << "\n";
  }

  return 0;
}


//============================================================================
// Name        : polynomfit.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C, Ansi-style
//============================================================================

//#include <stdio.h>
//#include <stdlib.h>
#include <Eigen/Dense>
#include "monopoly.h"
#include "include/tensor_serie.hh"
#include "full_correlation_tensor_serie.hh"
#include <random>

#include <iostream>

using namespace std;

int main(void) {
//	puts("Hello World!!!");
//	return EXIT_SUCCESS;

	cout << "Hello world" << endl;
	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic > m_eigen_matrix;
	m_eigen_matrix.resize(2,2);

	m_eigen_matrix << 1, 2,
					3,4;
	cout << m_eigen_matrix << endl;

	std::vector<std::vector<double> > data(10000, std::vector<double>(2,0.0) );
	std::random_device rd;
	std::mt19937 gen(rd());

	std::normal_distribution<> d(0,1);
	std::uniform_int_distribution<> distrib(1, 6);
	for(int i = 0; i!= data.size(); i++) {
		const double x =  distrib(gen) + d(gen);
		const double y = -0.1*x*x*x + 0.7*x*x + 0.01*x + 0.5 + d(gen);
		data[i][0] = x;
		data[i][1] = y;
	}

	full_correlation_tensor_serie<double> polynom(1,10);
	tensor_serie<double> m_x(1,10*4); //the internal tensors orders are 4 times larger

	for(int i=0; i!= data.size(); i++) {
		//cout<< i << data[i][0] << " " << data[i][1] << endl;
		m_x.create_diad_1dim(data[i][0]);
		polynom.fill_1dim(m_x, data[i][1], 1.0);
	}
	polynom.normalize();

	cout << "Solving for degree 4" << std::endl;
	tensor_serie_function<double> pol4 = polynom.solve(4);
	cout << "chi2\t expected chi2 \t the traditional bias \t full bias" << endl;
	cout << pol4.m_chi2 << "\t" << (pol4.m_chi2 + pol4.m_bias) << "\t" << pol4.m_biaso << "\t" << pol4.m_bias << endl;
	cout << "Solving for degree 10" << std::endl;
	tensor_serie_function<double> pol10 = polynom.solve(10);
	cout << "chi2\t expected chi2 \t the traditional bias \t full bias" << endl;
	cout << pol10.m_chi2 << "\t" << (pol10.m_chi2 + pol10.m_bias) << "\t" << pol10.m_biaso << "\t" << pol4.m_bias << endl;

	for(double x=0; x< 1.0; x+=0.1) {
		double y = (-0.1*x*x*x + 0.7*x*x + 0.01*x + 0.5);
		m_x.create_diad_1dim(x);
		cout << "p("<<x<<")=" << pol4.eval(m_x) << " vs " << y << endl;
	}

	//polynom.print("10 degree fit");

}


#pragma once

#include <Eigen/Dense>
#include <Eigen/StdVector>

template <int ROWS>
using VecNd = Eigen::Matrix<double, ROWS, 1>;

template <int ROWS, int COLS>
using MatNd = Eigen::Matrix<double, ROWS, COLS>;

template <int ROWS>
using VecNf = Eigen::Matrix<float, ROWS, 1>;

template <int ROWS, int COLS>
using MatNf = Eigen::Matrix<float, ROWS, COLS>;

using VecXd = Eigen::VectorXd;
using MatXd = Eigen::MatrixXd;

using VecXf = Eigen::VectorXf;
using MatXf = Eigen::MatrixXf;

template <int ROWS>
using SquareMatNd = Eigen::Matrix<double, ROWS, ROWS>;

template <typename Eig>
using StdVector = std::vector<Eig, Eigen::aligned_allocator<Eig>>;

namespace jcc {
using Vec1 = VecNd<1>;
using Vec2 = Eigen::Vector2d;
using Vec3 = Eigen::Vector3d;
using Vec4 = Eigen::Vector4d;
using Vec5 = VecNd<5>;
using Vec6 = VecNd<6>;

using Vec1f = VecNf<1>;
using Vec2f = Eigen::Vector2f;
using Vec3f = Eigen::Vector3f;
using Vec4f = Eigen::Vector4f;
using Vec5f = VecNf<5>;
using Vec6f = VecNf<6>;

using Vec2i = Eigen::Matrix<int, 2, 1>;
using Vec3i = Eigen::Matrix<int, 3, 1>;
using Vec4i = Eigen::Matrix<int, 4, 1>;

}  // namespace jcc


// 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 <utility>
#include <cmath>
#include <boost/test/minimal.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

using namespace std;
using namespace mtl;

template <typename Vector, typename Matrix>
class f_ftor
{
    typedef typename Collection<Vector>::value_type value_type;

  public:
    /// Arguments: each stock's ROI, the aimed ROI, the Langrange factor and the covariance
    f_ftor(const Vector& rv, value_type r, value_type lg, const Matrix& S)
      : rv(rv), r(r), lg(lg), S(S) {}

    value_type operator()(const Vector& pi) const
    {
	Vector S_pi(S * pi);
	return lg * sq(sum(pi) - 1) + lg * sq(dot(pi, rv) - r) + trans(pi) * S_pi;
    }
    value_type sq(value_type x) const { return x * x; }

  private:
    Vector      rv;
    value_type  r, lg;
    Matrix      S;
};

template <typename Vector, typename Matrix>
class grad_f_ftor
{
    typedef typename Collection<Vector>::value_type value_type;

  public:
    /// Arguments: each stock's ROI, the aimed ROI, the Langrange factor and the covariance
    grad_f_ftor(const Vector& rv, value_type r, value_type lg, const Matrix& S)
      : rv(rv), onev(size(rv), 1), r(r), lg(lg), S(S) {}

    Vector operator()(const Vector& pi) const
    {
	value_type f1= 2.0 * lg * (sum(pi) - 1), f2= 2.0 * lg * (dot(pi, rv) - r);
	return Vector(f1 * onev + f2 * rv + 2.0 * S * pi);
    }

  private:
    Vector      rv, onev;
    value_type  r, lg;
    Matrix      S;
};

template <typename Vector, typename Matrix>
class portfolio_optimizer
{
    typedef typename Collection<Vector>::value_type value_type;

  public:
    /// Arguments: each stock's ROI, the aimed ROI and the covariance
    portfolio_optimizer(const Vector& rv, value_type r, const Matrix& S)
      : s(size(rv) + 2), A(s, s), b(s, value_type(0))
    {
	unsigned s2= size(rv);
	A[irange(s2)][irange(s2)]= S; A[irange(s2)][s2]= Vector(s2, 1); A[irange(s2)][s2+1]= rv;
	A[s2][irange(s2)]= trans(Vector(s2, 1)); A[irange(s2, s)][irange(s2, s)]= 0;
	A[s2+1][irange(s2)]= trans(rv);

	b[s2]= 1; b[s2+1]= r;
	cout << "A is\n" << A << "\nb is " << b << '\n';
    }

    Vector operator()() const {	return clone(lu_solve(A, b)[irange(s-2)]); }

  private:

    unsigned    s;
    Matrix      A;
    Vector      b;
 };

int test_main(int, char**)
{
    using namespace mtl;

    dense_vector<double>       pi(4, 0.25), rv(4);
    rv= 1.03, 1.14, 1.05, 1.08;

    dense2D<double>            S(4, 4);
#if 1
    S= 1.0, 0.1, 0.3, -0.2,
	0.1, 1., -0.4, 0.7,
	0.3, -0.4, 1., 0.4,
	-0.2, 0.7, 0.4, 1.;
#else
    S= 1.0, 0.1, 0.3, 0.2,
	0.1, 1., 0.4, 0.7,
	0.3, 0.4, 1., 0.4,
	0.2, 0.7, 0.4, 1.;
#endif

    const double lagrange= 10000.0;
    f_ftor< dense_vector<double>, dense2D<double> >  f(rv, 1.09, lagrange, S);
    cout << "f(pi) is " << f(pi) << '\n';

    grad_f_ftor< dense_vector<double>, dense2D<double> >  grad_f(rv, 1.09, lagrange, S);
    cout << "grad_f(pi) is " << grad_f(pi) << '\n';

    portfolio_optimizer< dense_vector<double>, dense2D<double> >  opt(rv, 1.09, S);
    dense_vector<double>       pi_opt(opt());
    cout << "Optimal portfolio is " << pi_opt << "\n";

    std::cout<< "Sum of pi is " << sum(pi_opt) << "\n";
    std::cout<< "Overall ROI is " << dot(pi_opt, rv) << "\n";
    std::cout<< "Variance is " << dot(pi_opt, dense_vector<double>(S * pi_opt)) << "\n";

#if 1
    std::cout<< "Sum of pi is " << sum(pi) << "\n";
    std::cout<< "Overall ROI is " << dot(pi, rv) << "\n";
    std::cout<< "Variance is " << dot(pi, dense_vector<double>(S * pi)) << "\n";

    itl::cyclic_iteration<double> iter(grad_f(pi), 1000, 0, 1e-5, 10);
    quasi_newton(pi, f, grad_f, itl::armijo<>(), itl::sr1(), iter);
    iter.error_code();

    std::cout<< "pi= " << pi << "\n";
    std::cout<< "grad_f(pi)= " << grad_f(pi) << "\n";

    std::cout<< "Sum of pi is " << sum(pi) << "\n";
    std::cout<< "Overall ROI is " << dot(pi, rv) << "\n";
    std::cout<< "Variance is " << dot(pi, dense_vector<double>(S * pi)) << "\n";
#endif

    return 0;
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing round capabilities

    round(x,n) rounds aways from 0 to n digits:

    @par semantic:
    For any given value @c x of type @c T and integer n :

    @code
    T r = round(x{, n});
    @endcode

    is equivalent to

    @code
    T r = round(x*exp10(n))*exp10(-n);
    @endcode

    @par Note:

    n default to 0,

    - n > 0: round to n digits to the right of the decimal point.

    - n = 0: round to the nearest integer.

    - n < 0: round to n digits to the left of the decimal point.

    aways from 0 means that half integer values are rounded to the nearest
    integer of greatest absolute value

    The current rounding mode has no effect.

    - If x is \f$\pm\infty\f$ or \f$\pm0\f$, it is returned, unmodified
    - If arg is a NaN, a NaN is returned
  **/
  Value round(Value const & v0);

  //@overload
  Value round(Value const & x, IntegerValue const &n);
} }
#endif

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

#endif


//
// Created by kerail on 14.07.16.
//
#include <iostream>
#include <map>
#include <vector>
#include <algorithm>

#include <boost/function.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include "geom/SO3.hpp"

typedef boost::multiprecision::cpp_dec_float_100 float_100;
typedef Eigen::Matrix<float_100, 3, 3> Matrix3f100;
typedef SO3<float_100> ReferenceSO3Impl;

typedef std::vector<Eigen::Vector3d> Vector3dList;
typedef std::vector<Matrix3f100> Matrix3f100List;
typedef std::map<std::string, Matrix3f100List> Matrix3f100ListMap;
typedef std::map<std::string, Vector3dList> Vector3dListMap;

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

class AbstractTestSO3 {
 public:
  AbstractTestSO3(const Eigen::Vector3d &v) { }
  virtual Matrix3f100 getMatrix() = 0;
  static std::string name();
};

//class ReferenceSO3: public AbstractTestSO3 {
// public:
//  ReferenceSO3(const Eigen::Vector3d &v) : AbstractTestSO3(v), m_so3(v.cast<float_100>()) { }
//  virtual Matrix3f100 getMatrix() {
//    return m_so3.getQuaternion().toRotationMatrix();
//  }
//  static std::string name() {
//    return "Reference";
//  }
//  ReferenceSO3Impl m_so3;
//};
//
//class ReferenceSO3: public AbstractTestSO3 {
// public:
//  ReferenceSO3(const Eigen::Vector3d &v) : AbstractTestSO3(v) {
//    Eigen::Matrix<float_100, 3, 1> axis = v.cast<float_100>();
//    float_100 angle = axis.norm();
//    if (angle > 0) {
//      axis /= angle;
//    }
//    m_aa = Eigen::AngleAxis<float_100>(angle, axis);
//  }
//  virtual Matrix3f100 getMatrix() {
//    return m_aa.toRotationMatrix();
//  }
//  static std::string name() {
//    return "Reference";
//  }
//  Eigen::AngleAxis<float_100> m_aa;
//};
class ReferenceSO3: public AbstractTestSO3 {
 public:
  ReferenceSO3(const Eigen::Vector3d &v) : AbstractTestSO3(v) {
    Eigen::Matrix<float_100, 3, 1> axis = v.cast<float_100>();
    float_100 angle = axis.norm();
    if (angle > 0) {
      axis /= angle;
    }
    m_aa = Eigen::AngleAxis<float_100>(angle, axis);
  }
  virtual Matrix3f100 getMatrix() {
    return Eigen::Quaternion<float_100>(m_aa).toRotationMatrix();
  }
  static std::string name() {
    return "Reference";
  }
  Eigen::AngleAxis<float_100> m_aa;
};

class TestMySO3Direct: public AbstractTestSO3 {
 public:
  TestMySO3Direct(const Eigen::Vector3d &v) : AbstractTestSO3(v), m_so3(v) { }
  virtual Matrix3f100 getMatrix() {
    return m_so3.getMatrix().cast<float_100>();
  }

  static std::string name() {
    return "My.Direct";
  }
  SO3d m_so3;
};

class TestMySO3Quat: public AbstractTestSO3 {
 public:
  TestMySO3Quat(const Eigen::Vector3d &v) : AbstractTestSO3(v), m_so3(double(v.cast<float_100>().norm()),
                                                                      v.cast<float_100>().norm() > 0
                                                                      ? v.cast<float_100>().normalized().cast<double>()
                                                                      : v) { }
  virtual Matrix3f100 getMatrix() {
    return m_so3.getQuaternion().toRotationMatrix().cast<float_100>();
  }
  static std::string name() {
    return "My.Quater";
  }

  SO3d m_so3;
};

class TestAngleAxisSO3: public AbstractTestSO3 {
 public:
  TestAngleAxisSO3(const Eigen::Vector3d &v)
      : AbstractTestSO3(v), m_aa(v.norm(),
                                 v.norm() > 0 ? v.normalized().cast<double>()
                                              : v) { }
  virtual Matrix3f100 getMatrix() {
    return m_aa.toRotationMatrix().cast<float_100>();
  }

  static std::string name() {
    return "Eigen.AA";
  }
  Eigen::AngleAxisd m_aa;
};

template<size_t TEST_COUNT, class Reference, class... T>
class PrecisionTests;

template<size_t TEST_COUNT, class Reference, class T, class... Ts>
class PrecisionTests<TEST_COUNT, Reference, T, Ts...>: public PrecisionTests<TEST_COUNT, Reference, Ts...> {
  BOOST_STATIC_ASSERT(std::is_base_of<AbstractTestSO3, T>::value);
 public:
  void run() {
    PrecisionTests<TEST_COUNT, Reference, Ts...>::run();
    Vector3dListMap::const_iterator tests_it = getTests().begin();
    const Matrix3f100ListMap &refMap = getReferences();
    for (; tests_it != getTests().end(); ++tests_it) {
      Matrix3f100ListMap::const_iterator refIt = refMap.find(tests_it->first);
      cout << T::name() << "\t" << tests_it->first;
      runTests(tests_it->second, refIt->second);
      cout << endl;
    }
  };
 protected:
  void runTests(const Vector3dList &v, const Matrix3f100List &references) {
    std::vector<float_100> errors(v.size());
#pragma omp parallel for shared(v, errors)
    for (size_t i = 0; i < v.size(); ++i) {
      Eigen::Vector3d coeffs(v[i]);
      T so3(coeffs);
      errors[i] = matrixDiff(references[i], so3.getMatrix());
    }
    float_100 avg, stdDev, stdDevSq, maxErr, minErr;
    calcMetrics(errors, avg, stdDevSq, minErr, maxErr);
    std::cout << "\t" << avg << "\t" << sqrt(stdDevSq)
    << "\t" << minErr << "\t" << maxErr;
  };

  const Vector3dListMap &getTests() {
    return PrecisionTests<TEST_COUNT, Reference, Ts...>::getTests();
  }
  const Matrix3f100ListMap &getReferences() {
    return PrecisionTests<TEST_COUNT, Reference, Ts...>::getReferences();
  }

 private :
  float_100 matrixDiff(const Matrix3f100 &a, const Matrix3f100 &b, const uint8_t type = 2) {
    switch (type) {
      case 0:
        return (a - b).lpNorm<Eigen::Infinity>();
      case 1:
        return (a - b).lpNorm<1>();
      case 2:
      default:
        return (a - b).norm();
    }
  }
  void calcMetrics(const std::vector<float_100> &v,
                   float_100 &avg, float_100 &stdDev,
                   float_100 &minErr, float_100 &maxErr
  ) {
    BOOST_ASSERT(v.size() > 0);
    float_100 sum = 0;
    float_100 divider = 1 / float_100(v.size());
    minErr = v[0];
    maxErr = v[0];
    for (size_t i = 0; i < v.size(); ++i) {
      sum += v[i];
      minErr = std::min<float_100>(v[i], minErr);
      maxErr = std::max<float_100>(v[i], maxErr);
    }
    sum *= divider;
    avg = sum;

    sum = 0;
    for (size_t i = 0; i < v.size(); ++i) {
      float_100 t = (v[i] - avg);
      sum += t * t;
    }
    sum *= divider;
    stdDev = sum;
  }
};

template<size_t TEST_COUNT, class Reference>
class PrecisionTests<TEST_COUNT, Reference> {
  BOOST_STATIC_ASSERT(TEST_COUNT > 0);
 public:
  void run() {
    generateTests();
    calculateReference();
    std::cout << "Method\tTest\tAVG Error\tStdDev Err\tMin Error\tMax Error" << endl;
  };
 protected:

  const Vector3dListMap &getTests() {
    return m_tests;
  }
  const Matrix3f100ListMap &getReferences() {
    return m_referenceResults;
  }

 private:
  void generateTests() {
    cout << "Generation of Tests" << endl;
    m_tests.insert(std::make_pair("USUAL", Vector3dList(TEST_COUNT)));
    m_tests.insert(std::make_pair("ZERO15", Vector3dList(TEST_COUNT)));
    m_tests.insert(std::make_pair("ZERO18", Vector3dList(TEST_COUNT)));
    m_tests.insert(std::make_pair("ZERO22", Vector3dList(TEST_COUNT)));
    Vector3dListMap::iterator usualIt = m_tests.find("USUAL");
    Vector3dListMap::iterator zero15It = m_tests.find("ZERO15");
    Vector3dListMap::iterator zero18It = m_tests.find("ZERO18");
    Vector3dListMap::iterator zero22It = m_tests.find("ZERO22");

#pragma omp parallel for shared(usualIt, zero15It, zero18It, zero22It)
    for (size_t i = 0; i < TEST_COUNT; ++i) {
      Eigen::Vector3d rnd = Eigen::Vector3d::Random();
      rnd.normalize();
      rnd *= M_PI * 2;
      usualIt->second[i] = rnd;
      zero15It->second[i] = rnd * 1e-15;
      zero18It->second[i] = rnd * 1e-18;
      zero22It->second[i] = rnd * 1e-22;
    }
  }

  void calculateReference() {
    cout << "Calculation of precise reference" << endl;
    for (Vector3dListMap::iterator it = m_tests.begin(); it != m_tests.end(); it++) {
      cout << "\tTest: " << it->first << endl;
      m_referenceResults.insert(make_pair(it->first, Matrix3f100List(TEST_COUNT)));
      Matrix3f100ListMap::iterator vIt = m_referenceResults.find(it->first);
#pragma omp parallel for shared(vIt, it)
      for (size_t i = 0; i < TEST_COUNT; ++i) {
        Eigen::Vector3d coeffs(it->second[i]);
        Reference so3Ref(coeffs);
        vIt->second[i] = so3Ref.getMatrix();
      }
    }

  }

  Matrix3f100ListMap m_referenceResults;
  Vector3dListMap m_tests;
};

int main() {
  PrecisionTests<size_t(1000000), ReferenceSO3, TestMySO3Direct, TestMySO3Quat, TestAngleAxisSO3>
      testMachine;
  testMachine.run();
}


#include "delaunay_cpu_interpolator_base.h"
#include <CGAL/Delaunay_triangulation.h>
#include <CGAL/Epick_d.h>
#include <Eigen/Dense>
#include <bitset>

template<typename index_t, int N_DIMS, int N_OPS>
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::DelaunayCPUInterpolatorBase(
        operator_set_evaluator_iface *supporting_point_evaluator,
        const std::array<int, N_DIMS> &axes_points,
        const std::array<double, N_DIMS> &axes_min,
        const std::array<double, N_DIMS> &axes_max)
        : InterpolatorBase<index_t, N_DIMS, N_OPS>(supporting_point_evaluator,
                    axes_points, axes_min, axes_max) {
    typedef CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<N_DIMS>>> T;
    std::array<typename T::Point, 1 << N_DIMS> points;
    for (int j = 0; j < points.size(); ++j) {
        std::bitset<N_DIMS> binary(j);
        double point[N_DIMS];
        for (int i = 0; i < N_DIMS; i++)
            point[i] = binary[i];
        typename T::Point p(&point[0], &point[N_DIMS]);
        points[j] = p;
    }
    delaunay_triangulation_.insert(points.begin(), points.end());
}

template<typename index_t, int N_DIMS, int N_OPS>
int DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::Interpolate(
        const std::vector<double> &point, std::vector<double> &values) {
    std::array<int, N_DIMS> hypercube;
    std::array<double, N_DIMS> scaled_point;
    this->FindHypercube(point, hypercube, scaled_point);

    Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> vertex_matrix;
    std::array<std::array<int, N_DIMS>, N_DIMS + 1> simplex;
    FindSimplex(hypercube, scaled_point, vertex_matrix, simplex);

    Eigen::Matrix<double, N_DIMS + 1, 1> weights;
    ComputeBarycentricCoordinates(vertex_matrix, scaled_point, weights);

    values.assign(N_OPS, 0.0);
    for (int dim_i = 0; dim_i <= N_DIMS; dim_i++) {
        std::array<double, N_OPS> supp_values;
        this->GetSupportingPoint(simplex[dim_i], supp_values);
        for (int op_i = 0; op_i < N_OPS; op_i++)
            values[op_i] += weights[dim_i] * supp_values[op_i];
    }

    return 0;
}

template<typename index_t, int N_DIMS, int N_OPS>
int
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::InterpolateWithDerivatives(
        const std::vector<double> &points, const std::vector<int> &points_idxs,
        std::vector<double> &values, std::vector<double> &derivatives) {

    for (std::size_t point_i = 0; point_i < points_idxs.size(); point_i++) {
        int point_offset = points_idxs[point_i];

        std::array<int, N_DIMS> hypercube;
        std::array<double, N_DIMS> scaled_point;
        this->FindHypercube(points, hypercube, scaled_point,
                            point_offset * N_DIMS);

        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> vertex_matrix;
        std::array<std::array<int, N_DIMS>, N_DIMS + 1> simplex;
        FindSimplex(hypercube, scaled_point, vertex_matrix, simplex);

        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> vertex_matrix_inv;
        Eigen::Matrix<double, N_DIMS + 1, 1> weights;
        ComputeBarycentricCoordinatesAndVertexMatrixInv(vertex_matrix,
                scaled_point, weights, vertex_matrix_inv);

        std::fill(values.begin() + point_offset * N_OPS, values.begin() + (point_offset + 1) * N_OPS, 0);
        std::fill(derivatives.begin() + point_offset * N_OPS * N_DIMS, derivatives.begin() + (point_offset + 1) * N_OPS * N_DIMS, 0);
        for (int dim_i = 0; dim_i <= N_DIMS; dim_i++) {
            std::array<double, N_OPS> supp_values;
            this->GetSupportingPoint(simplex[dim_i], supp_values);
            for (int op_i = 0; op_i < N_OPS; op_i++) {
                values[point_offset * N_OPS + op_i] +=
                        weights[dim_i] * supp_values[op_i];
                for (int dim_j = 0; dim_j < N_DIMS; dim_j++)
                    derivatives[(point_offset * N_OPS + op_i) * N_DIMS + dim_j] +=
                            vertex_matrix_inv(dim_i, dim_j) * supp_values[op_i];
            }
        }
        for (int op_i = 0; op_i < N_OPS; op_i++)
            for (int dim_j = 0; dim_j < N_DIMS; dim_j++)
                derivatives[(point_offset * N_OPS + op_i) * N_DIMS + dim_j] *=
                        this->axes_step_inv_[dim_j];
    }
    return 0;
}

template<typename index_t, int N_DIMS, int N_OPS>
void DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::FindSimplex(
        const std::array<int, N_DIMS> &hypercube,
        const std::array<double, N_DIMS> &scaled_point,
        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix,
        std::array<std::array<int, N_DIMS>, N_DIMS + 1> &simplex) {
    typedef CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<N_DIMS>>> T;
    typename T::Point t_scaled_point(scaled_point.begin(), scaled_point.end());
    typename T::Full_cell_handle handle = delaunay_triangulation_.locate(
            t_scaled_point);

    vertex_matrix.row(N_DIMS) = Eigen::Matrix<double, 1, N_DIMS + 1>::Constant(
            1.0);
    for (int vertex_i = 0; vertex_i <= N_DIMS; vertex_i++) {
        typename T::Point vertex = handle->vertex(vertex_i)->point();
        for (int dim_i = 0; dim_i < N_DIMS; dim_i++) {
            simplex[vertex_i][dim_i] = hypercube[dim_i] + vertex[dim_i];
            vertex_matrix(dim_i, vertex_i) = vertex[dim_i];
        }
    }
}

template<typename index_t, int N_DIMS, int N_OPS>
void
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::ComputeBarycentricCoordinates(
        const Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix,
        const std::array<double, N_DIMS> &scaled_point,
        Eigen::Matrix<double, N_DIMS + 1, 1> &weights) {
    Eigen::Matrix<double, N_DIMS + 1, 1> scaled_point_vector;
    for (int dim_i = 0; dim_i < N_DIMS; dim_i++)
        scaled_point_vector[dim_i] = scaled_point[dim_i];
    scaled_point_vector[N_DIMS] = 1.0;

    weights = vertex_matrix.colPivHouseholderQr().solve(scaled_point_vector);
}

template<typename index_t, int N_DIMS, int N_OPS>
void
DelaunayCPUInterpolatorBase<index_t, N_DIMS, N_OPS>::
        ComputeBarycentricCoordinatesAndVertexMatrixInv(
        const Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix,
        const std::array<double, N_DIMS> &scaled_point,
        Eigen::Matrix<double, N_DIMS + 1, 1> &weights,
        Eigen::Matrix<double, N_DIMS + 1, N_DIMS + 1> &vertex_matrix_inv) {
    Eigen::Matrix<double, N_DIMS + 1, 1> scaled_point_vector;
    for (int dim_i = 0; dim_i < N_DIMS; dim_i++)
        scaled_point_vector[dim_i] = scaled_point[dim_i];
    scaled_point_vector[N_DIMS] = 1.0;

    vertex_matrix_inv = vertex_matrix.inverse();
    weights = vertex_matrix_inv * scaled_point_vector;
}


// clang-format off
// MUST BE at the beginning before any other <cmath> include (e.g. in armadillo's headers)
#define _USE_MATH_DEFINES // required for Visual Studio
#include <cmath>
// clang-format on

#include "libKriging/OrdinaryKriging.hpp"

#include <armadillo>
#include <optim.hpp>
#include <tuple>
#include <cassert>

// #include "libKriging/covariance.h"

//' @ref: https://github.com/psbiomech/dace-toolbox-source/blob/master/dace.pdf
//'  (where CovMatrix<-R, Ft<-M, C<-T, rho<-z)
//' @ref: https://github.com/cran/DiceKriging/blob/master/R/kmEstimate.R (same variables names)

//' @ref https://github.com/cran/DiceKriging/blob/master/src/CovFuns.c
// Covariance function on normalized data
std::function<double(arma::subview_col<double>&&, arma::subview_col<double>&&)> CovNorm_fun_gauss
    = [](arma::subview_col<double>&& xi, arma::subview_col<double>&& xj) {
        //    double temp = 0;
        //    for (arma::uword k = 0; k < xi.n_elem; k++) {
        //      double d = (xi(k) - xj(k));
        //      temp += d * d;
        //    }

        auto&& diff = (xi - xj);
        const double temp = arma::dot(diff, diff);

        return exp(-0.5 * temp);
      };

std::function<double(arma::subview_col<double>&&, arma::subview_col<double>&&, int)> CovNorm_deriv_gauss
    = [](arma::subview_col<double>&& xi, arma::subview_col<double>&& xj, int dim) {
        //    double temp = 0;
        //    for (arma::uword k = 0; k < xi.n_elem; k++) {
        //      double d = (xi(k) - xj(k));
        //      temp += d*d;
        //    }

        auto&& diff = (xi - xj);
        const double temp = arma::dot(diff, diff);

        return exp(-.5 * temp) * (xi(dim) - xj(dim)) * (xi(dim) - xj(dim));
      };

std::function<double(arma::subview_col<double>&&, arma::subview_col<double>&&)> CovNorm_fun_exp
    = [](arma::subview_col<double>&& xi, arma::subview_col<double>&& xj) {
        auto&& diff = (xi - xj);
        return exp(-arma::sum(arma::abs(diff)));
      };

std::function<double(arma::subview_col<double>&&, arma::subview_col<double>&&, int)> CovNorm_deriv_exp
    = [](arma::subview_col<double>&& xi, arma::subview_col<double>&& xj, int dim) {
        auto&& diff = (xi - xj);
        return exp(-arma::sum(arma::abs(diff))) * fabs(xi(dim) - xj(dim));
      };

/************************************************/
/** implementation details forward declaration **/
/************************************************/

namespace {  // anonymous namespace for local implementation details
auto regressionModelMatrix(const OrdinaryKriging::RegressionModel& regmodel,
                           const arma::mat& newX,
                           arma::uword n,
                           arma::uword d) -> arma::mat;
}  // namespace

/************************************************/
/**      OrdinaryKriging implementation        **/
/************************************************/

// returns distance matrix form Xp to X
LIBKRIGING_EXPORT
arma::mat OrdinaryKriging::Cov(const arma::mat& X, const arma::mat& Xp) {
  arma::mat Xtnorm = trans(X);
  Xtnorm.each_col() /= m_theta;
  arma::mat Xptnorm = trans(Xp);
  Xptnorm.each_col() /= m_theta;

  arma::uword n = X.n_rows;
  arma::uword np = Xp.n_rows;

  // Should bre replaced by for_each
  arma::mat R(n, np);
  R.zeros();
  for (arma::uword i = 0; i < n; i++) {
    for (arma::uword j = 0; j < np; j++) {
      R.at(i, j) = OrdinaryKriging::CovNorm_fun(Xtnorm.col(i), Xptnorm.col(j));
    }
  }
  return R;
}

// Optimized version when Xp=X
LIBKRIGING_EXPORT
arma::mat OrdinaryKriging::Cov(const arma::mat& X) {
  // Should be tyaken from covariance.h from nestedKriging ?
  // return getCrossCorrMatrix(X,Xp,parameters,covType);

  arma::mat Xtnorm = trans(X);
  Xtnorm.each_col() /= m_theta;
  arma::uword n = X.n_rows;

  // Should bre replaced by for_each
  arma::mat R(n, n);
  R.zeros();
  for (arma::uword i = 0; i < n; i++) {
    for (arma::uword j = 0; j < i; j++) {
      R.at(i, j) = OrdinaryKriging::CovNorm_fun(Xtnorm.col(i), Xtnorm.col(j));
    }
  }

  R = arma::symmatl(R);  // R + trans(R);
  R.diag().ones();
  return R;
}
//// same for one point
// LIBKRIGING_EXPORT arma::colvec OrdinaryKriging::Cov(const arma::mat& X,
//                                                    const arma::rowvec& x,
//                                                    const arma::colvec& theta) {
//  // FIXME mat(x) : an arma::mat from a arma::rowvec ?
//  return OrdinaryKriging::Cov(&X, arma::mat(&x), &theta).col(1);  // TODO to be optimized...
//}

// This will create the dist(xi,xj) function above. Need to parse "covType".
void OrdinaryKriging::make_Cov(const std::string& covType) {
  if (covType.compare("gauss") == 0) {
    CovNorm_fun = CovNorm_fun_gauss;
    CovNorm_deriv = CovNorm_deriv_gauss;
  } else if (covType.compare("exp") == 0) {
    CovNorm_fun = CovNorm_fun_exp;
    CovNorm_deriv = CovNorm_deriv_exp;
  } else
    throw std::invalid_argument("Unsupported covariance: " + covType);

  // arma::cout << "make_Cov done." << arma::endl;
}

// at least, just call make_Cov(kernel)
LIBKRIGING_EXPORT OrdinaryKriging::OrdinaryKriging(const std::string& covType) {
  make_Cov(covType);
}

// Objective function for fit : -logLikelihood
double OrdinaryKriging::fit_ofn(const arma::vec& _theta,
                                arma::vec* grad_out,
                                OrdinaryKriging::OKModel* okm_data) const {
  OrdinaryKriging::OKModel* fd = okm_data;

  // arma::cout << "_theta:" << _theta << arma::endl;

  //' @ref https://github.com/cran/DiceKriging/blob/master/R/logLikFun.R
  //  model@covariance <- vect2covparam(model@covariance, param)
  //  model@covariance@sd2 <- 1		# to get the correlation matrix
  //
  //  aux <- covMatrix(model@covariance, model@X)
  //
  //  R <- aux[[1]]
  //  T <- chol(R)
  //
  //  x <- backsolve(t(T), model@y, upper.tri = FALSE)
  //  M <- backsolve(t(T), model@F, upper.tri = FALSE)
  //  z <- compute.z(x=x, M=M, beta=beta)
  //  sigma2.hat <- compute.sigma2.hat(z)
  //  logLik <- -0.5*(model@n * log(2*pi*sigma2.hat) + 2*sum(log(diag(T))) + model@n)

  arma::mat Xtnorm = trans(m_X);
  Xtnorm.each_col() /= _theta;

  arma::uword n = m_X.n_rows;

  // Define regression matrix
  arma::uword nreg = 1;
  arma::mat F = arma::ones(n, nreg);

  // Allocate the matrix // arma::mat R = Cov(fd->X, _theta);
  // Should be replaced by for_each
  arma::mat R = arma::zeros(n, n);
  for (arma::uword i = 0; i < n; i++) {
    for (arma::uword j = 0; j < i; j++) {
      R.at(i, j) = CovNorm_fun(Xtnorm.col(i), Xtnorm.col(j));
    }
  }
  R = arma::symmatl(R);  // R + trans(R);
  R.diag().ones();
  // arma::cout << "R:" << R << arma::endl;

  // Cholesky decompostion of covariance matrix
  fd->T = trans(chol(R));

  // Compute intermediate useful matrices
  fd->M = solve(trimatl(fd->T), m_F, arma::solve_opts::fast);
  arma::mat Q;
  arma::mat G;
  qr_econ(Q, G, fd->M);
  arma::colvec Yt = solve(trimatl(fd->T), m_y, arma::solve_opts::fast);
  fd->beta = solve(trimatu(G), trans(Q) * Yt, arma::solve_opts::fast);
  fd->z = Yt - fd->M * fd->beta;

  //' @ref https://github.com/cran/DiceKriging/blob/master/R/computeAuxVariables.R
  double sigma2_hat = arma::accu(fd->z % fd->z) / n;
  // arma::cout << "sigma2_hat:" << sigma2_hat << arma::endl;

  double minus_ll = /*-*/ 0.5 * (n * log(2 * M_PI * sigma2_hat) + 2 * sum(log(fd->T.diag())) + n);
  // arma::cout << "ll:" << -minus_ll << arma::endl;

  if (grad_out != nullptr) {
    //' @ref https://github.com/cran/DiceKriging/blob/master/R/logLikGrad.R
    //  logLik.derivative <- matrix(0,nparam,1)
    //  x <- backsolve(T,z)			# compute x := T^(-1)*z
    //  Rinv <- chol2inv(T)			# compute inv(R) by inverting T
    //
    //  Rinv.upper <- Rinv[upper.tri(Rinv)]
    //  xx <- x%*%t(x)
    //  xx.upper <- xx[upper.tri(xx)]
    //
    //  for (k in 1:nparam) {
    //    gradR.k <- CovMatrixDerivative(model@covariance, X=model@X, C0=R, k=k)
    //    gradR.k.upper <- gradR.k[upper.tri(gradR.k)]
    //
    //    terme1 <- sum(xx.upper*gradR.k.upper)   / sigma2.hat
    //    # quick computation of t(x)%*%gradR.k%*%x /  ...
    //    terme2 <- - sum(Rinv.upper*gradR.k.upper)
    //    # quick computation of trace(Rinv%*%gradR.k)
    //    logLik.derivative[k] <- terme1 + terme2
    //  }

    arma::mat Linv = solve(trimatl(fd->T), arma::eye(n, n), arma::solve_opts::fast);
    arma::mat Rinv = trans(Linv) * Linv;  // inv_sympd(R);

    arma::mat x = solve(trimatu(trans(fd->T)), fd->z, arma::solve_opts::fast);
    arma::mat xx = x * trans(x);
    // arma::mat xx_upper = trimatu(xx);

    for (arma::uword k = 0; k < m_X.n_cols; k++) {
      arma::mat gradR_k_upper = arma::zeros(n, n);
      for (arma::uword i = 0; i < n; i++) {
        for (arma::uword j = 0; j < i; j++) {
          gradR_k_upper.at(j, i) = CovNorm_deriv(Xtnorm.col(i), Xtnorm.col(j), k);
        }
      }
      gradR_k_upper /= _theta(k);
      gradR_k_upper = trans(gradR_k_upper);
      // arma::mat gradR_k = symmatu(gradR_k_upper);
      // gradR_k.diag().zeros();

      double terme1
          = arma::accu(xx /*_upper*/ % gradR_k_upper) / sigma2_hat;  // as_scalar((trans(x) * gradR_k) * x)/ sigma2_hat;
      double terme2 = -arma::accu(Rinv /*_upper*/ % gradR_k_upper);  //-arma::trace(Rinv * gradR_k);
      (*grad_out).at(k) = -(terme1 + terme2);
      // (*grad_out)(k) = - arma::accu(dot(xx / sigma2_hat - Rinv, gradR_k_upper));
    }
    // arma::cout << "Grad: " << *grad_out <<  arma::endl;
  }

  return minus_ll;
}

// Utility function for LOO
arma::colvec DiagABA(const arma::mat& A, const arma::mat& B) {
  arma::mat D = trimatu(2 * B);
  D.diag() = B.diag();
  D = (A * D) % A;
  arma::colvec c = sum(D, 1);

  return c;
}

// Objective function for fit : -LOO
double OrdinaryKriging::fit_ofn2(const arma::vec& _theta,
                                 arma::vec* grad_out,
                                 OrdinaryKriging::OKModel* okm_data) const {
  OrdinaryKriging::OKModel* fd = okm_data;

  arma::mat Xtnorm = trans(m_X);
  Xtnorm.each_col() /= _theta;

  arma::uword n = m_X.n_rows;

  // Allocate the matrix // arma::mat R = Cov(fd->X, _theta);
  // Should be replaced by for_each
  arma::mat R(n, n);
  R.zeros();
  for (arma::uword i = 0; i < n; i++) {
    for (arma::uword j = 0; j < i; j++) {
      R(i, j) = CovNorm_fun(Xtnorm.col(i), Xtnorm.col(j));
    }
  }
  R = arma::symmatl(R);  // R + trans(R);
  R.diag().ones();
  // arma::cout << "R:" << R << arma::endl;

  // Cholesky decompostion of covariance matrix
  fd->T = trans(chol(R));

  // Compute intermediate useful matrices
  // arma::mat M = solve(fd->T, F);
  fd->M = solve(trimatl(fd->T), m_F, arma::solve_opts::fast);
  // arma::mat Rinv = inv_sympd(R); // didn't find chol2inv equivalent in armadillo
  arma::mat Rinv = inv(trimatl(fd->T));
  Rinv = trimatl(Rinv) * trimatl(Rinv);
  arma::mat RinvF = Rinv * m_F;
  arma::mat TM = chol(trans(fd->M) * fd->M);  // Can be optimized with a crossprod equivalent in armadillo ?
  // arma::mat aux = solve(trans(TM), trans(RinvF));
  arma::mat aux = solve(trimatl(trans(TM)), trans(RinvF), arma::solve_opts::fast);
  arma::mat Q = Rinv - trans(aux) * aux;  // Can be optimized with a crossprod equivalent in armadillo ?
  arma::mat Qy = Q * m_y;
  arma::colvec sigma2LOO = 1 / Q.diag();
  arma::colvec errorsLOO = sigma2LOO % Qy;
  double minus_loo = -arma::accu(errorsLOO % errorsLOO) / n;

  if (grad_out != nullptr) {
    //' @ref hhttps://github.com/cran/DiceKriging/blob/master/R/leaveOneOutGrad.R
    // LOOfunDer <- matrix(0, nparam, 1)
    // for (k in 1:nparam) {
    //	gradR.k <- covMatrixDerivative(model@covariance, X=model@X, C0=R, k=k)
    //	diagdQ <- - diagABA(A=Q, B=gradR.k)
    //	dsigma2LOO <- - (sigma2LOO^2) * diagdQ
    //	derrorsLOO <- dsigma2LOO * Q.y - sigma2LOO * (Q%*%(gradR.k%*%Q.y))
    //	LOOfunDer[k] <- 2*crossprod(errorsLOO, derrorsLOO)/model@n
    //}

    for (arma::uword k = 0; k < m_X.n_cols; k++) {
      arma::mat gradR_k(n, n);
      gradR_k.zeros();
      for (arma::uword i = 0; i < n; i++) {
        for (arma::uword j = 0; j < i; j++) {
          gradR_k(i, j) = CovNorm_deriv(Xtnorm.col(i), Xtnorm.col(j), k);
        }
      }
      gradR_k /= _theta(k);
      gradR_k = arma::symmatl(gradR_k);  // gradR_k + trans(gradR_k);
      gradR_k.diag().zeros();

      arma::colvec diagdQ = -DiagABA(Q, gradR_k);
      arma::colvec dsigma2LOO = -sigma2LOO % sigma2LOO % diagdQ;
      arma::colvec derrorsLOO = dsigma2LOO % Qy - sigma2LOO % (Q * (gradR_k * Qy));
      (*grad_out)(k) = -2 * dot(errorsLOO, derrorsLOO) / n;
    }
    // arma::cout << "Grad: " << *grad_out <<  arma::endl;
  }

  return minus_loo;
}

LIBKRIGING_EXPORT double OrdinaryKriging::logLikelihood(const arma::vec& _theta) {
  arma::mat T;
  arma::mat M;
  arma::mat z;
  arma::colvec beta;
  OrdinaryKriging::OKModel okm_data{T, M, z, beta};

  return -fit_ofn(_theta, nullptr, &okm_data);
}

LIBKRIGING_EXPORT arma::vec OrdinaryKriging::logLikelihoodGrad(const arma::vec& _theta) {
  arma::mat T;
  arma::mat M;
  arma::mat z;
  arma::colvec beta;
  OrdinaryKriging::OKModel okm_data{T, M, z, beta};

  arma::vec grad(_theta.n_elem);

  double ll = fit_ofn(_theta, &grad, &okm_data);

  return -grad;
}

LIBKRIGING_EXPORT double OrdinaryKriging::loofun(const arma::vec& _theta) {
  arma::mat T;
  arma::mat M;
  arma::mat z;
  arma::colvec beta;
  OrdinaryKriging::OKModel okm_data{T, M, z, beta};

  return -fit_ofn2(_theta, nullptr, &okm_data);
}

LIBKRIGING_EXPORT arma::vec OrdinaryKriging::loofungrad(const arma::vec& _theta) {
  arma::mat T;
  arma::mat M;
  arma::mat z;
  arma::colvec beta;
  OrdinaryKriging::OKModel okm_data{T, M, z, beta};

  arma::vec grad(_theta.n_elem);

  double ll = fit_ofn2(_theta, &grad, &okm_data);

  return -grad;
}

/** Fit the kriging object on (X,y):
 * @param y is n length column vector of output
 * @param X is n*d matrix of input
 * @param regmodel is the regression model to be used for the GP mean (choice between contant, linear, quadratic)
 * @param normalize is a boolean to enforce inputs/output normalization
 * @param parameters is starting value for hyper-parameters
 * @param optim_method is an optimizer name from OptimLib, or 'none' to keep parameters unchanged
 * @param optim_objective is 'loo' or 'loglik'. Ignored if optim_method=='none'.
 */
LIBKRIGING_EXPORT void OrdinaryKriging::fit(const arma::colvec& y,
                                            const arma::mat& X,
                                            const RegressionModel& regmodel,
                                            bool normalize) {  //,
                                                               // const Parameters& parameters,
  // const std::string& optim_objective, // will support "logLik" or "leaveOneOut"
  // const std::string& optim_method) {

  std::string optim_objective = "ll";
  std::string optim_method = "bfgs";
  Parameters parameters{0, false, arma::vec(1), false};

  arma::uword n = X.n_rows;
  arma::uword d = X.n_cols;
  arma::rowvec centerX(d);
  arma::rowvec scaleX(d);
  double centerY;
  double scaleY;
  // Normalization of inputs and output
  if (normalize) {
    centerX = min(X, 0);
    scaleX = max(X, 0) - min(X, 0);
    centerY = min(y);
    scaleY = max(y) - min(y);
  } else {
    centerX.zeros();
    scaleX.ones();
    centerY = 0;
    scaleY = 1;
  }
  m_centerX = centerX;
  m_scaleX = scaleX;
  m_centerY = centerY;
  m_scaleY = scaleY;
  {  // FIXME why copies of newX and newy
    arma::mat newX = X;
    newX.each_row() -= centerX;
    newX.each_row() /= scaleX;
    arma::colvec newy = (y - centerY) / scaleY;
    this->m_X = newX;
    this->m_y = newy;
  }

  // Define regression matrix
  m_regmodel = regmodel;
  m_F = regressionModelMatrix(regmodel, m_X, n, d);

  // arma::cout << "optim_method:" << optim_method << arma::endl;

  if (optim_method == "none") {  // just keep given theta, no optimisation of ll
    m_theta = parameters.theta;
  } else if (optim_method.rfind("bfgs", 0) == 0) {
    arma::mat theta0;
    // FIXME parameters.has needs to implemtented (no use case in current code)
    if (!parameters.has_theta) {      // no theta given, so draw 10 random uniform starting values
      int multistart = 10;            // TODO? stoi(substr(optim_method,)) to hold 'bfgs10' as a 10 multistart bfgs
      arma::arma_rng::set_seed(123);  // FIXME arbitrary seed for reproducible random sequences
      theta0 = arma::randu(multistart, X.n_cols);
    } else {  // just use given theta(s) as starting values for multi-bfgs
      theta0 = arma::mat(parameters.theta);
    }

    // arma::cout << "theta0:" << theta0 << arma::endl;

    optim::algo_settings_t algo_settings;
    algo_settings.iter_max = 10;  // TODO change by default?
    algo_settings.err_tol = 1e-5;
    algo_settings.vals_bound = true;
    algo_settings.lower_bounds = 0.001 * arma::ones<arma::vec>(X.n_cols);
    algo_settings.upper_bounds = 2 * sqrt(X.n_cols) * arma::ones<arma::vec>(X.n_cols);
    double minus_ll = std::numeric_limits<double>::infinity();
    for (arma::uword i = 0; i < theta0.n_rows; i++) {  // TODO: use some foreach/pragma to let OpenMP work.
      arma::vec theta_tmp = trans(theta0.row(i));
      arma::mat T;
      arma::mat M;
      arma::mat z;
      arma::colvec beta;
      OrdinaryKriging::OKModel okm_data{T, M, z, beta};
      bool bfgs_ok = optim::lbfgs(
          theta_tmp,
          [&okm_data, this](const arma::vec& vals_inp, arma::vec* grad_out, void*) -> double {
            return fit_ofn(vals_inp, grad_out, &okm_data);
          },
          nullptr,
          algo_settings);

      // if (bfgs_ok) { // FIXME always succeeds ?
      double minus_ll_tmp
          = fit_ofn(theta_tmp,
                    nullptr,
                    &okm_data);  // this last call also ensure that T and z are up-to-date with solution found.
      if (minus_ll_tmp < minus_ll) {
        m_theta = std::move(theta_tmp);
        minus_ll = minus_ll_tmp;
        m_T = std::move(okm_data.T);
        m_M = std::move(okm_data.M);
        m_z = std::move(okm_data.z);
        m_beta = std::move(okm_data.beta);
      }
      // }
    }
  } else
    throw std::runtime_error("Not a suitable optim_method: " + optim_method);

  // arma::cout << "theta:" << m_theta << arma::endl;

  if (!parameters.has_sigma2) {
    m_sigma2 = arma::as_scalar(sum(pow(m_z, 2)) / X.n_rows);
    m_sigma2 = arma::as_scalar(accu(m_z % m_z) / X.n_rows);
    // Un-normalize
    m_sigma2 *= scaleY * scaleY;
  } else {
    m_sigma2 = parameters.sigma2;
  }

  // arma::cout << "sigma2:" << m_sigma2 << arma::endl;
}

/** Compute the prediction for given points X'
 * @param Xp is m*d matrix of points where to predict output
 * @param std is true if return also stdev column vector
 * @param cov is true if return also cov matrix between Xp
 * @return output prediction: m means, [m standard deviations], [m*m full covariance matrix]
 */
LIBKRIGING_EXPORT std::tuple<arma::colvec, arma::colvec, arma::mat> OrdinaryKriging::predict(const arma::mat& Xp,
                                                                                bool withStd,
                                                                                bool withCov) {
  arma::uword m = Xp.n_rows;
  arma::uword n = m_X.n_rows;
  arma::colvec pred_mean(m);
  arma::colvec pred_stdev(m);
  arma::mat pred_cov(m, m);
  pred_stdev.zeros();
  pred_cov.zeros();

  arma::mat Xtnorm = trans(m_X);
  Xtnorm.each_col() /= m_theta;
  arma::mat Xpnorm = Xp;
  // Normalize Xp
  Xpnorm.each_row() -= m_centerX;
  Xpnorm.each_row() /= m_scaleX;

  // Define regression matrix
  arma::uword d = m_X.n_cols;
  arma::mat Ftest = regressionModelMatrix(m_regmodel, Xpnorm, m, d);

  // Compute covariance between training data and new data to predict
  arma::mat R(n, m);
  Xpnorm = trans(Xpnorm);
  Xpnorm.each_col() /= m_theta;
  for (arma::uword i = 0; i < n; i++) {
    for (arma::uword j = 0; j < m; j++) {
      R.at(i, j) = CovNorm_fun(Xtnorm.col(i), Xpnorm.col(j));
    }
  }
  arma::mat Tinv_newdata = solve(trimatl(m_T), R, arma::solve_opts::fast);
  pred_mean = Ftest * m_beta + trans(Tinv_newdata) * m_z;
  // Un-normalize predictor
  pred_mean = m_centerY + m_scaleY * pred_mean;

  if (withStd) {
    double total_sd2 = m_sigma2;
    // s2.predict.1 <- apply(Tinv.c.newdata, 2, crossprod)
    arma::colvec s2_predict_1 = m_sigma2 * trans(sum(Tinv_newdata % Tinv_newdata, 0));
    // Type = "UK"
    // T.M <- chol(t(M)%*%M)
    arma::mat TM = trans(chol(trans(m_M) * m_M));
    // s2.predict.mat <- backsolve(t(T.M), t(F.newdata - t(Tinv.c.newdata)%*%M) , upper.tri = FALSE)
    arma::mat s2_predict_mat = solve(trimatl(TM), trans(Ftest - trans(Tinv_newdata) * m_M), arma::solve_opts::fast);
    // s2.predict.2 <- apply(s2.predict.mat, 2, crossprod)
    arma::colvec s2_predict_2 = m_sigma2 * trans(sum(s2_predict_mat % s2_predict_mat, 0));
    // s2.predict <- pmax(total.sd2 - s2.predict.1 + s2.predict.2, 0)
    arma::mat s2_predict = total_sd2 - s2_predict_1 + s2_predict_2;
    s2_predict.elem(find(pred_stdev < 0)).zeros();
    pred_stdev = sqrt(s2_predict);
    if (withCov) {
      // C.newdata <- covMatrix(object@covariance, newdata)[[1]]
      arma::mat C_newdata(m, m);
      for (arma::uword i = 0; i < m; i++) {
        for (arma::uword j = 0; j < m; j++) {
          C_newdata.at(i, j) = CovNorm_fun(Xpnorm.col(i), Xpnorm.col(j));
        }
      }
      // cond.cov <- C.newdata - crossprod(Tinv.c.newdata)
      // cond.cov <- cond.cov + crossprod(s2.predict.mat)
      pred_cov = m_sigma2 * (C_newdata - trans(Tinv_newdata) * Tinv_newdata + trans(s2_predict_mat) * s2_predict_mat);
    }
  } else if (withCov) {
    arma::mat C_newdata(m, m);
    for (arma::uword i = 0; i < m; i++) {
      for (arma::uword j = 0; j < m; j++) {
        C_newdata.at(i, j) = CovNorm_fun(Xpnorm.col(i), Xpnorm.col(j));
      }
    }
    // Need to compute matrices computed in withStd case
    arma::mat TM = trans(chol(trans(m_M) * m_M));
    arma::mat s2_predict_mat = solve(trimatl(TM), trans(Ftest - trans(Tinv_newdata) * m_M), arma::solve_opts::fast);
    pred_cov = m_sigma2 * (C_newdata - trans(Tinv_newdata) * Tinv_newdata + trans(s2_predict_mat) * s2_predict_mat);
  }

  return std::make_tuple(std::move(pred_mean), std::move(pred_stdev), std::move(pred_cov));
  /*if (withStd)
    if (withCov)
      return std::make_tuple(std::move(pred_mean), std::move(pred_stdev), std::move(pred_cov));
    else
      return std::make_tuple(std::move(pred_mean), std::move(pred_stdev), nullptr);
  else if (withCov)
    return std::make_tuple(std::move(pred_mean), std::move(pred_cov), nullptr);
  else
    return std::make_tuple(std::move(pred_mean), nullptr, nullptr);*/
}

/** Draw sample trajectories of kriging at given points X'
 * @param Xp is m*d matrix of points where to simulate output
 * @param nsim is number of simulations to draw
 * @return output is m*nsim matrix of simulations at Xp
 */
LIBKRIGING_EXPORT arma::mat OrdinaryKriging::simulate(const int nsim, const arma::mat& Xp) {
  // Here nugget.sim = 1e-10 to avoid chol failures of Sigma_cond)
  double nugget_sim = 1e-10;
  arma::uword m = Xp.n_rows;
  arma::uword n = m_X.n_rows;
  arma::mat yp(m, nsim);

  arma::mat Xpnorm = Xp;

  // Normalize Xp
  Xpnorm.each_row() -= m_centerX;
  Xpnorm.each_row() /= m_scaleX;

  // Define regression matrix
  arma::uword d = m_X.n_cols;
  arma::mat F_newdata = regressionModelMatrix(m_regmodel, Xpnorm, m, d);

  arma::colvec y_trend = F_newdata * m_beta;

  // Compute covariance between new data
  arma::mat Sigma(m, m);
  Xpnorm = trans(Xpnorm);
  Xpnorm.each_col() /= m_theta;
  for (arma::uword i = 0; i < m; i++) {
    for (arma::uword j = 0; j < i; j++) {
      Sigma.at(i, j) = CovNorm_fun(Xpnorm.col(i), Xpnorm.col(j));
    }
  }
  Sigma = arma::symmatl(Sigma);  // R + trans(R);
  Sigma.diag().ones();
  // arma::mat T_newdata = chol(Sigma);
  // Compute covariance between training data and new data to predict
  // Sigma21 <- covMat1Mat2(object@covariance, X1 = object@X, X2 = newdata, nugget.flag = FALSE)
  arma::mat Sigma21(n, m);
  arma::mat Xtnorm = trans(m_X);
  Xtnorm.each_col() /= m_theta;
  for (arma::uword i = 0; i < n; i++) {
    for (arma::uword j = 0; j < m; j++) {
      Sigma21.at(i, j) = CovNorm_fun(Xtnorm.col(i), Xpnorm.col(j));
    }
  }
  // Tinv.Sigma21 <- backsolve(t(object@T), Sigma21, upper.tri = FALSE
  arma::mat Tinv_Sigma21 = solve(trimatl(m_T), Sigma21, arma::solve_opts::fast);
  // y.trend.cond <- y.trend + t(Tinv.Sigma21) %*% object@z
  y_trend += trans(Tinv_Sigma21) * m_z;
  // Sigma.cond <- Sigma11 - t(Tinv.Sigma21) %*% Tinv.Sigma21
  arma::mat Sigma_cond = Sigma - trans(Tinv_Sigma21) * Tinv_Sigma21;
  // T.cond <- chol(Sigma.cond + diag(nugget.sim, m, m))
  Sigma_cond.diag() += nugget_sim;
  arma::mat T_cond = chol(m_sigma2 * Sigma_cond);
  // white.noise <- matrix(rnorm(m*nsim), m, nsim)
  // y.rand.cond <- t(T.cond) %*% white.noise
  // y <- matrix(y.trend.cond, m, nsim) + y.rand.cond
  yp.each_col() = y_trend;
  yp += trans(T_cond) * arma::randn(m, nsim);
  // Un-normalize simulations
  yp = m_centerY + m_scaleY * yp;

  return yp;  // NB: move not required due to copy ellision mechanism
}

/** Add new conditional data points to previous (X,y)
 * @param newy is m length column vector of new output
 * @param newX is m*d matrix of new input
 * @param optim_method is an optimizer name from OptimLib, or 'none' to keep previously estimated parameters unchanged
 * @param optim_objective is 'loo' or 'loglik'. Ignored if optim_method=='none'.
 */
LIBKRIGING_EXPORT void OrdinaryKriging::update(const arma::vec& newy,
                                               const arma::mat& newX,
                                               const std::string& optim_objective,
                                               const std::string& optim_method) {
  // rebuild data
  m_X = join_rows(m_X, newX);
  m_y = join_rows(m_y, newy);

  // rebuild starting parameters
  Parameters parameters{this->m_sigma2, true, this->m_theta, true};
  // re-fit
  this->fit(m_y, m_X);  //, parameters, optim_objective, optim_method);
}

/************************************************/
/**          implementation details            **/
/************************************************/

namespace {  // anonymous namespace for local implementation details

auto regressionModelMatrix(const OrdinaryKriging::RegressionModel& regmodel,
                           const arma::mat& newX,
                           arma::uword n,
                           arma::uword d) -> arma::mat {
  arma::mat F;  // uses modern RTO to avoid returned object copy
  switch (regmodel) {
    case OrdinaryKriging::RegressionModel::Constant: {
      F.set_size(n, 1);
      F = arma::ones(n, 1);
      return F;
    } break;

    case OrdinaryKriging::RegressionModel::Linear: {
      F.set_size(n, 1 + d);
      F.col(0) = arma::ones(n, 1);
      for (arma::uword i = 0; i < d; i++) {
        F.col(i + 1) = newX.col(i);
      }
      return F;
    } break;

    case OrdinaryKriging::RegressionModel::Quadratic: {
      F.set_size(n, 1 + 2 * d + d * (d - 1) / 2);
      F.col(0) = arma::ones(n, 1);
      arma::uword count = 1;
      for (arma::uword i = 0; i < d; i++) {
        F.col(count) = newX.col(i);
        count += 1;
        for (arma::uword j = 0; j <= i; j++) {
          F.col(count) = newX.col(i) % newX.col(j);
          count += 1;
        }
      }
      return F;
    } break;
  }
}

static char const* enum_RegressionModel_strings[] = {"constant", "linear", "quadratic"};

}  // namespace

OrdinaryKriging::RegressionModel OrdinaryKriging::RegressionModelUtils::fromString(const std::string& value) {
  static auto begin = std::begin(enum_RegressionModel_strings);
  static auto end = std::end(enum_RegressionModel_strings);

  auto find = std::find(begin, end, value);
  if (find != end) {
    return static_cast<RegressionModel>(std::distance(begin, find));
  } else {
    // FIXME use std::optional as returned type
    throw std::exception();
  }
}

std::string OrdinaryKriging::RegressionModelUtils::toString(const OrdinaryKriging::RegressionModel& e) {
  assert(static_cast<std::size_t>(e) < sizeof(enum_RegressionModel_strings));
  return enum_RegressionModel_strings[static_cast<int>(e)];
}