#ifndef __TERM_SIMPLIFIER_HPP__
#define __TERM_SIMPLIFIER_HPP__

#include <crab/common/types.hpp>
#include <boost/optional.hpp>

/*
   Simplifiers for table terms after giving meaning to functors.
*/

namespace crab {
  namespace domains {
    namespace term {

        // Common API to simplifiers
        template < class Num, class Ftor>
        class Simplifier
        {
         protected:

          typedef term_table <Num, Ftor> term_table_t;
          typedef typename term_table_t::term_id_t term_id_t;

          term_table_t  &_ttbl;

         public:

          Simplifier (term_table_t &term_table): _ttbl(term_table) {}
          virtual void simplify () = 0;

          // This should not modify the term table
          // FIXME: cannot make const the method without changing
          // constness of other methods.
          virtual boost::optional<term_id_t> simplify_term (term_id_t t) = 0;

        };

        // Trivial simplifier by giving standard mathematical meaning
        // to arithmetic operators assuming that conmutativity,
        // associativity etc properties hold as expected.
        template < class Num>
        class NumSimplifier: Simplifier<Num, binary_operation_t >
        {
          typedef Simplifier<Num, binary_operation_t> simplifier_t;

          typedef term_table <Num, binary_operation_t> term_table_t;
          typedef typename term_table_t::term_id_t term_id_t;
          typedef typename term_table_t::term_t term_t;

          // Simplify term f(left,right)
          boost::optional<term_id_t>
          simplify_term (binary_operation_t f, term_id_t left, term_id_t right)
          {
            // Only consider these two rules:
            //   '/'('*'(x,y),x) = y
            //   '/'('*'(x,y),y) = x
            switch (f)
            {
              case BINOP_SDIV:
              case BINOP_UDIV:
                {
                  term_t* tleft  = this->_ttbl.get_term_ptr (left);
                  term_t* tright = this->_ttbl.get_term_ptr (right);

                  if ((tleft->kind () == TERM_APP) && term_ftor(tleft) == BINOP_MUL)
                  {
                    std::vector<term_id_t>& args(term_args(tleft));
                    assert(args.size() == 2);
                    term_t* tl = this->_ttbl.get_term_ptr (args[0]);
                    term_t* tr = this->_ttbl.get_term_ptr (args[1]);

                    if (tl == tright) return args[1];
                    if (tr == tright) return args[0];
                  }
                }
              default:  return boost::optional<term_id_t> ();
            }
          }

         public:

          NumSimplifier (term_table_t &term_table): simplifier_t (term_table) {}

          void simplify () {}

          boost::optional<term_id_t> simplify_term (term_id_t t)
          {
            if (term_t* tt = this->_ttbl.get_term_ptr (t))
            {
              if (tt->kind () == TERM_APP)
              {
                std::vector<term_id_t>& args(term_args(tt));
                assert(args.size() == 2);

                return simplify_term (term_ftor (tt), args[0], args[1]);
              }
            }
            return boost::optional<term_id_t>();
          }

        };

     } //end namespace term
  } //end namespace domains
} // end namespace crab
#endif


#include <cucumber-cpp/autodetect.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/format.hpp>

using std::string;
using cucumber::ScenarioScope;

namespace {

    bool isCloseEnough(double value, double expected) {
        return (std::abs(value - expected) > 0.00001);
    }

    void verifyEqual(double value, double expected) {
        if (isCloseEnough(value, expected)) {
            throw (boost::format("Value %1% not equal to %2%") % value % expected).str();
        }
    }

    void verifyNotEqual(double value, double expected) {
        if (!isCloseEnough(value, expected)) {
            throw (boost::format("Value %1% equal to %2%") % value % expected).str();
        }
    }
}

GIVEN("^a calculator") {
    // nothing to do
}

WHEN("^the calculator computes PI$") {
    ScenarioScope<RpnCalculator> calc;
    calc->pi();
}

WHEN("^the calculator adds up ([\\d\\.]+) and ([\\d\\.]+)$") {
    REGEX_PARAM(string, n1);
    REGEX_PARAM(string, n2);

    ScenarioScope<RpnCalculator> calc;
    calc->push(n1);
    calc->push(n2);
    calc->push("+");
}

WHEN("^the calculator adds up \"([^\"]*)\" and \"([^\"]*)\"$") {
    REGEX_PARAM(string, n1);
    REGEX_PARAM(string, n2);

    ScenarioScope<RpnCalculator> calc;
    calc->push(n1);
    calc->push(n2);
    calc->push("+");
}

WHEN("^the calculator adds up \"([^\"]*)\", \"([^\"]*)\" and \"([^\"]*)\"$") {
    REGEX_PARAM(string, n1);
    REGEX_PARAM(string, n2);
    REGEX_PARAM(string, n3);

    ScenarioScope<RpnCalculator> calc;
    calc->push(n1);
    calc->push(n2);
    calc->push(n3);
    calc->push("+");
    calc->push("+");
}

WHEN("^the calculator adds up the following numbers:$") {
    REGEX_PARAM(string, numberString);

    ScenarioScope<RpnCalculator> calc;
    vector<string> numbers;
    boost::split(numbers, numberString,boost::is_any_of("\n"));

    for (size_t i=0; i < numbers.size(); ++i) {
        calc->push(numbers[i]);
        if (i != 0) {
            calc->push("+");
        }
    }
}

THEN("^the calculator returns PI$") {
    ScenarioScope<RpnCalculator> calc;
    verifyEqual(calc->value(), boost::math::constants::pi<double>());
}

THEN("^the calculator returns \"([^\"]*)\"$") {
    REGEX_PARAM(double, expected);

    ScenarioScope<RpnCalculator> calc;
    verifyEqual(calc->value(), expected);
}

THEN("^the calculator does not return ([\\d\\.]+)$") {
    REGEX_PARAM(double, expected);

    ScenarioScope<RpnCalculator> calc;
    double value = calc->value();
    verifyNotEqual(calc->value(), expected);
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE NumpyBroadcastShape

#include <sstream>

#include <boost/test/unit_test.hpp>
#include <popart/error.hpp>
#include <popart/tensorinfo.hpp>

struct BroadcastTestCase {
  std::vector<int64_t> a_shape;
  std::vector<int64_t> b_shape;
  std::vector<int64_t> result_shape;
};

// clang-format off
static const BroadcastTestCase test_cases[] = {
    // Test cases taken from
    // https://docs.scipy.org/doc/numpy/user/basics.broadcasting.html#general-broadcasting-rules
    {{   256, 256, 3}, {       3}, {   256, 256, 3}},
    {{8,   1,   6, 1}, {7,  1, 5}, {8,   7,   6, 5}},
    {{          5, 4}, {       1}, {          5, 4}},
    {{          5, 4}, {       4}, {          5, 4}},
    {{    15,   3, 5}, {15, 1, 5}, {    15,   3, 5}},
    {{    15,   3, 5}, {    3, 5}, {    15,   3, 5}},
    {{    15,   3, 5}, {    3, 1}, {    15,   3, 5}},

    // Test cases taken from
    // https://github.com/onnx/onnx/blob/master/docs/Broadcasting.md
    {{2, 3, 4, 5}, {/* scalar */}, {2, 3, 4, 5}},
    {{2, 3, 4, 5}, {           5}, {2, 3, 4, 5}},
    {{      4, 5}, {  2, 3, 4, 5}, {2, 3, 4, 5}},
    {{   1, 4, 5}, {  2, 3, 1, 1}, {2, 3, 4, 5}},
    {{   3, 4, 5}, {  2, 1, 1, 1}, {2, 3, 4, 5}}
};
// clang-format on

BOOST_AUTO_TEST_CASE(NumpyBroadcastShape) {
  for (const auto &test_case : test_cases) {
    BOOST_TEST(popart::npBroadcastable(test_case.a_shape, test_case.b_shape));

    const auto new_shape = popart::npOut(test_case.a_shape, test_case.b_shape);
    BOOST_TEST(new_shape == test_case.result_shape,
               boost::test_tools::per_element());
  }
}

BOOST_AUTO_TEST_CASE(NumpyBroadcastTensorInfo) {
  for (const auto &test_case : test_cases) {
    BOOST_TEST(popart::npBroadcastable(
        popart::TensorInfo(popart::DataType::FLOAT16, test_case.a_shape),
        popart::TensorInfo(popart::DataType::FLOAT16, test_case.b_shape)));

    const auto new_tensor = popart::npOut(
        popart::TensorInfo(popart::DataType::FLOAT16, test_case.a_shape),
        popart::TensorInfo(popart::DataType::FLOAT16, test_case.b_shape));
    BOOST_TEST(new_tensor.shape() == test_case.result_shape,
               boost::test_tools::per_element());
    BOOST_TEST(new_tensor.dataType() == popart::DataType::FLOAT16);
  }
}

BOOST_AUTO_TEST_CASE(NumpyBroadcastTensoInfoDataTypeMismatch) {
  for (const auto &test_case : test_cases) {
    BOOST_TEST(!popart::npBroadcastable(
        popart::TensorInfo(popart::DataType::FLOAT16, test_case.a_shape),
        popart::TensorInfo(popart::DataType::INT32, test_case.b_shape)));

    const size_t ERR_LEN = 500;

    const auto addShape = [&](const std::vector<int64_t> &shape,
                              std::ostream &os) {
      auto it = shape.begin();
      if (it == shape.end()) {
        return;
      }

      while (true) {
        os << *it;
        it++;
        if (it == shape.end()) {
          return;
        }
        os << " ";
      }
    };

    const auto predicate = [&](popart::error e) {
      std::ostringstream errm;
      errm << "np broadcasting failed, incompatible types FLOAT16 and INT32 ";
      errm << "(shapes [";
      addShape(test_case.a_shape, errm);
      errm << "] and [";
      addShape(test_case.b_shape, errm);
      errm << "])";

      return e.what() == errm.str();
    };

    BOOST_CHECK_EXCEPTION(
        popart::npOut(
            popart::TensorInfo(popart::DataType::FLOAT16, test_case.a_shape),
            popart::TensorInfo(popart::DataType::INT32, test_case.b_shape)),
        popart::error,
        predicate);
  }
}

struct BroadcastBackwardTestCase {
  std::vector<int64_t> in_shape;
  std::vector<int64_t> out_shape;
  std::vector<int64_t> result_axes;
};

// clang-format off
static const BroadcastBackwardTestCase backward_test_cases[] = {
    {{7, 2, 3, 4, 5, 6}, {7, 2, 3, 4, 5, 6}, {                }},
    {{2, 3, 4, 5, 6   }, {7, 2, 3, 4, 5, 6}, {0               }},
    {{3, 4, 5, 6      }, {7, 2, 3, 4, 5, 6}, {0, 1            }},
    {{4, 5, 6         }, {7, 2, 3, 4, 5, 6}, {0, 1, 2         }},
    {{5, 6            }, {7, 2, 3, 4, 5, 6}, {0, 1, 2, 3      }},
    {{6               }, {7, 2, 3, 4, 5, 6}, {0, 1, 2, 3, 4   }},
    {{                }, {7, 2, 3, 4, 5, 6}, {0, 1, 2, 3, 4, 5}},
    {{1, 1, 1, 1, 1, 1}, {7, 2, 3, 4, 5, 6}, {0, 1, 2, 3, 4, 5}},
    {{1, 1, 1, 1, 1, 6}, {7, 2, 3, 4, 5, 6}, {0, 1, 2, 3, 4   }},
    {{1, 1, 1, 1, 5, 6}, {7, 2, 3, 4, 5, 6}, {0, 1, 2, 3      }},
    {{1, 1, 1, 4, 5, 6}, {7, 2, 3, 4, 5, 6}, {0, 1, 2         }},
    {{1, 1, 3, 4, 5, 6}, {7, 2, 3, 4, 5, 6}, {0, 1            }},
    {{1, 2, 3, 4, 5, 6}, {7, 2, 3, 4, 5, 6}, {0               }},
    {{7, 2, 3, 4, 5, 6}, {7, 2, 3, 4, 5, 6}, {                }},

    // Test cases taken from
    // https://docs.scipy.org/doc/numpy/user/basics.broadcasting.html#general-broadcasting-rules
    {{   256, 256, 3}, {   256, 256, 3}, {    }},
    {{8,   1,   6, 1}, {8,   7,   6, 5}, {1, 3}},
    {{             3}, {   256, 256, 3}, {0, 1}},
    {{     7,   1, 5}, {8,   7,   6, 5}, {0, 2}},
    {{             1}, {          5, 4}, {0, 1}},
    {{             4}, {          5, 4}, {   0}},
    {{    15,   1, 5}, {    15,   3, 5}, {   1}},
    {{          3, 5}, {    15,   3, 5}, {   0}},
    {{          3, 1}, {    15,   3, 5}, {0, 2}},

    // Test cases taken from
    // https://github.com/onnx/onnx/blob/master/docs/Broadcasting.md
    {{        4, 5}, {2, 3, 4, 5}, {      0, 1}},
    {{     1, 4, 5}, {2, 3, 4, 5}, {      0, 1}},
    {{     3, 4, 5}, {2, 3, 4, 5}, {         0}},
    {{/* scalar */}, {2, 3, 4, 5}, {0, 1, 2, 3}},
    {{           5}, {2, 3, 4, 5}, {   0, 1, 2}},
    {{  2, 3, 1, 1}, {2, 3, 4, 5}, {      2, 3}},
    {{  2, 1, 1, 1}, {2, 3, 4, 5}, {   1, 2, 3}}

};
// clang-format on

BOOST_AUTO_TEST_CASE(NumpyBroadcastBackwardShape) {
  for (const auto &test_case : backward_test_cases) {
    const auto axes =
        popart::npReductionAxis(test_case.in_shape, test_case.out_shape);
    BOOST_TEST(axes == test_case.result_axes, boost::test_tools::per_element());
  }
}

struct ExceptionTestCase {
  std::string name;
  std::vector<int64_t> a_shape;
  std::vector<int64_t> b_shape;
  std::string msg;
};

// clang-format off
ExceptionTestCase exception_test_cases[] = {
    {""   , {   3}, {   4}, "np broadcasting failed, frames [3] and [4] are not aligned"},
    {""   , {1, 3}, {   4}, "np broadcasting failed, frames [1, 3] and [4] are not aligned"},
    {""   , {4, 3}, {   4}, "np broadcasting failed, frames [4, 3] and [4] are not aligned"},
    {"foo", {   3}, {   4}, "np broadcasting failed on 'foo', frames [3] and [4] are not aligned"},
    {"foo", {1, 3}, {   4}, "np broadcasting failed on 'foo', frames [1, 3] and [4] are not aligned"},
    {"foo", {4, 3}, {   4}, "np broadcasting failed on 'foo', frames [4, 3] and [4] are not aligned"},
    {""   , {   3}, {3, 4}, "np broadcasting failed, frames [3] and [3, 4] are not aligned"},
    {""   , {   3}, {1, 4}, "np broadcasting failed, frames [3] and [1, 4] are not aligned"},
    {""   , {   3}, {   4}, "np broadcasting failed, frames [3] and [4] are not aligned"},
    {"foo", {   3}, {3, 4}, "np broadcasting failed on 'foo', frames [3] and [3, 4] are not aligned"},
    {"foo", {   3}, {1, 4}, "np broadcasting failed on 'foo', frames [3] and [1, 4] are not aligned"},
    {"foo", {   3}, {   4}, "np broadcasting failed on 'foo', frames [3] and [4] are not aligned"},
};
// clang-format on

BOOST_AUTO_TEST_CASE(NumpyBroadcastException) {
  for (const auto &test_case : exception_test_cases) {
    const auto predicate = [&](popart::error e) {
      return test_case.msg == e.what();
    };

    BOOST_TEST(!popart::npBroadcastable(test_case.a_shape, test_case.b_shape));

    BOOST_CHECK_EXCEPTION(
        popart::npOut(test_case.a_shape, test_case.b_shape, test_case.name),
        popart::error,
        predicate);
  }
}

BOOST_AUTO_TEST_CASE(NumpyBroadcastTensorInfoShapeException) {
  for (const auto &test_case : exception_test_cases) {

    // Skip tests with a debug name as not supported for TensorInfo use
    if (test_case.name != "") {
      continue;
    }

    const auto predicate = [&](popart::error e) {
      return test_case.msg == e.what();
    };

    BOOST_TEST(!popart::npBroadcastable(
        popart::TensorInfo(popart::DataType::FLOAT16, test_case.a_shape),
        popart::TensorInfo(popart::DataType::FLOAT16, test_case.b_shape)));

    BOOST_CHECK_EXCEPTION(
        popart::npOut(
            popart::TensorInfo(popart::DataType::FLOAT16, test_case.a_shape),
            popart::TensorInfo(popart::DataType::FLOAT16, test_case.b_shape)),
        popart::error,
        predicate);
  }
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_math/blob/master/LICENSE

#ifndef CBR_MATH__INTERP__PIECEWISE_CONSTANT_HPP_
#define CBR_MATH__INTERP__PIECEWISE_CONSTANT_HPP_

#include <Eigen/Dense>

#include <utility>
#include <vector>
#include <array>

#include "piecewise_poly.hpp"

namespace cbr
{

class PiecewiseConstant
{
protected:
  using row_t = Eigen::Matrix<double, 1, Eigen::Dynamic>;
  using matrix_t = Eigen::MatrixXd;

public:
  /**
   * @brief Create a scalar-valued piecewise constant polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::row_t
   * @param x sorted breakpoints
   * @param y values
   *
   * The resulting function f is s.t.
   *  f(t) = y[i] if x[i] <= t < x[i+1]
   *  f(t) = y[0] if t < x[0]
   *  f(t) = y[x.size() - 1] if t >= x[x.size() - 1]
   */
  template<typename T1, typename T2>
  static PiecewisePoly fit(
    T1 && x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    // add a dummy datapoint at the end
    const auto newsize = x.size() + 1;
    row_t x_ext(newsize);
    x_ext << x, x(x.size() - 1) + 1;
    row_t y_ext(newsize);
    y_ext << y, y(y.cols() - 1);

    auto coeffs = generateCoeffs(x_ext, y_ext);
    return PiecewisePoly(std::move(x_ext), std::move(coeffs));
  }

  /**
   * @brief Create a vector-valued piecewise constant polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::matrix_t or container_t<PiecewisePoly::row_t>
   * @param x sorted breakpoints
   * @param y values
   *
   * The resulting function f is s.t.
   *  f(t) = y[i] if x[i] <= t < x[i+1]
   *  f(t) = y[0] if t < x[0]
   *  f(t) = y[x.size() - 1] if t >= x[x.size() - 1]
   */
  template<typename T1, typename T2>
  static PiecewisePolyND fitND(
    T1 && x,
    const T2 & ys)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    if (x.size() == 0) {
      throw std::invalid_argument("x must be non-empty");
    }

    // add a dummy datapoint at the end
    const auto newsize = x.size() + 1;
    row_t x_ext(newsize);
    x_ext << x, x(x.size() - 1) + 1;

    std::vector<matrix_t> coefLists;

    if constexpr (is_eigen_dense_v<T2>) {
      if (ys.rows() < 1) {
        throw std::invalid_argument("Dimension of the data must be > 0.");
      }

      if (ys.cols() != x.size()) {
        throw std::invalid_argument("The number of columns of ys must be equal to the size of x.");
      }

      // add a dummy datapoint at the end
      matrix_t ys_ext(ys.rows(), newsize);
      ys_ext << ys, ys.col(ys.cols() - 1);

      coefLists.reserve(static_cast<std::size_t>(ys_ext.rows()));

      for (Eigen::Index i = 0; i < ys_ext.rows(); i++) {
        coefLists.push_back(generateCoeffs(x_ext, ys_ext.row(i)));
      }
    } else {
      static_assert(
        is_eigen_dense_v<typename T2::value_type>&& T2::value_type::IsVectorAtCompileTime,
        "ys must be a container of Eigen::DenseBase vector objects");

      if (ys.size() < 1) {
        throw std::invalid_argument("Dimension of the data must be > 0.");
      }

      coefLists.reserve(ys.size());

      for (auto & y : ys) {
        // add a dummy datapoint at the end
        row_t y_ext(newsize);
        y_ext << y, y(y.size() - 1);
        coefLists.push_back(generateCoeffs(x_ext, std::move(y_ext)));
      }
    }

    return PiecewisePolyND(std::move(x_ext), std::move(coefLists));
  }

protected:
  template<typename T1, typename T2>
  static matrix_t generateCoeffs(
    const Eigen::DenseBase<T1> & x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(
      T1::IsVectorAtCompileTime && T2::IsVectorAtCompileTime,
      "x and y must be vectors.");

    if (x.size() != y.size()) {
      throw std::invalid_argument("Each element of ys must have the same size as x.");
    }

    Eigen::Index nj = x.size() - 1;
    matrix_t coeffs(1, nj);

    for (Eigen::Index i = 0; i < nj; i++) {
      coeffs(0, i) = y[i];
    }

    return coeffs;
  }
};

}  // namespace cbr

#endif  // CBR_MATH__INTERP__PIECEWISE_CONSTANT_HPP_


/*    This file is part of the Gudhi Library. The Gudhi library
 *    (Geometric Understanding in Higher Dimensions) is a generic C++
 *    library for computational topology.
 *
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2014  INRIA Saclay (France)
 *
 *    This program is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    This program is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <gudhi/reader_utils.h>
#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>

#include <boost/program_options.hpp>

#include <string>

using namespace Gudhi;
using namespace Gudhi::persistent_cohomology;

typedef int Vertex_handle;
typedef double Filtration_value;

void program_options(int argc, char * argv[]
                     , std::string & simplex_tree_file
                     , std::string & output_file
                     , int & p
                     , Filtration_value & min_persistence);

int main(int argc, char * argv[]) {
  std::string simplex_tree_file;
  std::string output_file;
  int p;
  Filtration_value min_persistence;

  program_options(argc, argv, simplex_tree_file, output_file, p, min_persistence);

  std::cout << "Simplex_tree from file=" << simplex_tree_file.c_str() << " - output_file=" << output_file.c_str()
      << std::endl;
  std::cout << "     - p=" << p << " - min_persistence=" << min_persistence << std::endl;

  // Read the list of simplices from a file.
  Simplex_tree<> simplex_tree;

  std::ifstream simplex_tree_stream(simplex_tree_file);
  simplex_tree_stream >> simplex_tree;

  std::cout << "The complex contains " << simplex_tree.num_simplices() << " simplices" << std::endl;
  std::cout << "   - dimension " << simplex_tree.dimension() << std::endl;

  /*
  std::cout << std::endl << std::endl << "Iterator on Simplices in the filtration, with [filtration value]:" << std::endl;
  for( auto f_simplex : simplex_tree.filtration_simplex_range() )
  { std::cout << "   " << "[" << simplex_tree.filtration(f_simplex) << "] ";
  for( auto vertex : simplex_tree.simplex_vertex_range(f_simplex) )
  { std::cout << vertex << " "; }
  std::cout << std::endl;
  }*/

  // Sort the simplices in the order of the filtration
  simplex_tree.initialize_filtration();

  // Compute the persistence diagram of the complex
  Persistent_cohomology< Simplex_tree<>, Field_Zp > pcoh(simplex_tree);
  // initializes the coefficient field for homology
  pcoh.init_coefficients(p);

  pcoh.compute_persistent_cohomology(min_persistence);

  // Output the diagram in output_file
  if (output_file.empty()) {
    pcoh.output_diagram();
  } else {
    std::ofstream out(output_file);
    pcoh.output_diagram(out);
    out.close();
  }

  return 0;
}

void program_options(int argc, char * argv[]
                     , std::string & simplex_tree_file
                     , std::string & output_file
                     , int & p
                     , Filtration_value & min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()
      ("input-file", po::value<std::string>(&simplex_tree_file),
       "Name of file containing a simplex set. Format is one simplex per line (cf. reader_utils.h - read_simplex): Dim1 X11 X12 ... X1d Fil1  ");

  po::options_description visible("Allowed options", 100);
  visible.add_options()
      ("help,h", "produce help message")
      ("output-file,o", po::value<std::string>(&output_file)->default_value(std::string()),
       "Name of file in which the persistence diagram is written. Default print in std::cout")
      ("field-charac,p", po::value<int>(&p)->default_value(11),
       "Characteristic p of the coefficient field Z/pZ for computing homology.")
      ("min-persistence,m", po::value<Filtration_value>(&min_persistence),
       "Minimal lifetime of homology feature to be recorded. Default is 0");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
            options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::cout << std::endl;
    std::cout << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::cout << "of a Rips complex defined on a set of input points.\n \n";
    std::cout << "The output diagram contains one bar per line, written with the convention: \n";
    std::cout << "   p   dim b d \n";
    std::cout << "where dim is the dimension of the homological feature,\n";
    std::cout << "b and d are respectively the birth and death of the feature and \n";
    std::cout << "p is the characteristic of the field Z/pZ used for homology coefficients." << std::endl << std::endl;

    std::cout << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::cout << visible << std::endl;
    std::abort();
  }
}


#include <fstream>
#include <vector>
#include <map>
#include <limits>
#include <cmath>
#include <cstdio>
#include "boost/tuple/tuple.hpp" //<boost/tuple/tuple.hpp>
#include "boost/foreach.hpp" // <boost/foreach.hpp>
#include <armadillo>

#ifndef MLMATH
#define MLMATH

#include "MLMath.h"

#endif

// Warn about use of deprecated functions.
#define GNUPLOT_DEPRECATE_WARN
//#include "plot.h"

#ifndef M_PI
#	define M_PI 3.14159265358979323846
#endif

// http://stackoverflow.com/a/1658429
#ifdef _WIN32
	#include <windows.h>
	inline void mysleep(unsigned millis) {
		::Sleep(millis);
	}
#else
	#include <unistd.h>
	inline void mysleep(unsigned millis) {
		::usleep(millis * 1000);
	}
#endif

void pause_if_needed() {
#ifdef _WIN32
	// For Windows, prompt for a keystroke before the Gnuplot object goes out of scope so that
	// the gnuplot window doesn't get closed.
	std::cout << "Press enter to exit." << std::endl;
	std::cin.get();
#endif
}

// Tell MSVC to not warn about using fopen.
// http://stackoverflow.com/a/4805353/1048959
#if defined(_MSC_VER) && _MSC_VER >= 1400
#pragma warning(disable:4996)
#endif

using namespace std;
using namespace arma;

void demo_basic() {
	Gnuplot gp;
	// For debugging or manual editing of commands:
	//Gnuplot gp(std::fopen("plot.gnu"));
	// or
	//Gnuplot gp("tee plot.gnu | gnuplot -persist");

	std::vector<std::pair<double, double> > xy_pts_A;
	for(double x=-2; x<2; x+=0.01) {
		double y = x*x*x;
		xy_pts_A.push_back(std::make_pair(x, y));
	}

	std::vector<std::pair<double, double> > xy_pts_B;
	for(double alpha=0; alpha<1; alpha+=1.0/24.0) {
		double theta = alpha*2.0*3.14159;
		xy_pts_B.push_back(std::make_pair(cos(theta), sin(theta)));
	}

	gp << "set xrange [-2:2]\nset yrange [-2:2]\n";
	gp << "plot '-' with lines title 'cubic', '-' with points title 'circle'\n";
	gp.send1d(xy_pts_A);
	gp.send1d(xy_pts_B);

	pause_if_needed();
}

void demo_binary() {
	Gnuplot gp;

	std::vector<std::pair<double, double> > xy_pts_A;
	for(double x=-2; x<2; x+=0.01) {
		double y = x*x;
		xy_pts_A.push_back(std::make_pair(x, y));
	}

	// std::vector<std::pair<double, double> > xy_pts_B;
	// for(double alpha=0; alpha<1; alpha+=1.0/24.0) {
	// 	double theta = alpha*2.0*3.14159;
	// 	xy_pts_B.push_back(std::make_pair(cos(theta), sin(theta)));
	// }

	gp << "set xrange [-2:2]\nset yrange [-2:2]\n";
	gp << "plot '-' binary" << gp.binFmt1d(xy_pts_A, "record") << "with lines title 'cubic'\n";
		//<< "'-' binary" << gp.binFmt1d(xy_pts_B, "record") << "with points title 'circle'\n";
	gp.sendBinary1d(xy_pts_A);
	//gp.sendBinary1d(xy_pts_B);

	pause_if_needed();
}

void demo_animation() {
  #ifdef _WIN32
  	// No animation demo for Windows.  The problem is that every time the plot
  	// is updated, the gnuplot window grabs focus.  So you can't ever focus the
  	// terminal window to press Ctrl-C.  The only way to quit is to right-click
  	// the terminal window on the task bar and close it from there.  Other than
  	// that, it seems to work.
  	std::cout << "Sorry, the animation demo doesn't work in Windows." << std::endl;
  	return;
  #endif

	Gnuplot gp;

	std::cout << "Press Ctrl-C to quit (closing gnuplot window doesn't quit)." << std::endl;

	//gp << "set yrange [0:100]\n";
  //gp << "set xrange [0:10]\n";

  //default for default animation
  gp << "set yrange [-1:1]\n";

	const int N = 100;
	std::vector<double> pts(N);
  std::vector<double> pts2(N);

	double theta = 0;

	while(1) {

		for(int i=0; i<N; i++) {
			double alpha = (double(i)/N-0.5) * 10;

      //default animations
			pts[i] = sin(alpha*8.0 + theta) * exp(-alpha*alpha/2.0);
      pts2[i] = sin(alpha*100.0 + theta) * exp(-alpha*alpha/2.0);

      //pts[i] = pow(i,2) + 1;
      //pts2[i] = i;
		}

		//gp << "plot '-' binary" << gp.binFmt1d(pts, "array") << "with lines notitle\n";

    //gp << "plot '-' binary" << gp.binFmt1d(pts, "array") << "with lines notitle\n";

    //working double plot
    gp << "plot '-' binary" << gp.binFmt1d(pts, "array")
		<< "with lines notitle, '-' binary" << gp.binFmt1d(pts2, "array")
		<< "with lines notitle\n";

    // gp << "plot '-' binary" << gp.binFmt1d(pts, "array") << "with lines notitle, '-' binary" << gp.binFmt1d(pts2,"array") << "with lines notitle\n";

    //<< "'-' binary" << gp.binFmt1d(pts2, "record") << "with points title 'circle'\n";

    //gp << "set xrange [-2:2]\nset yrange [-2:2]\n";
  	//gp << "plot '-' with lines title 'cubic', '-' with points title 'circle'\n";
  	//gp.send1d(xy_pts_A);
  	//gp.send1d(xy_pts_B);

		gp.sendBinary1d(pts);
    gp.sendBinary1d(pts2);
		gp.flush();

		theta += 0.2;
		mysleep(100);
	}

}

void demo_contour(){

  Gnuplot gp;

	gp << "set samples 20\nset isosamples 21\nset xlabel 'X axis'\nset ylabel 'Y axis'\n"<<
  "set zlabel 'Z '' offset 1, 0\nset view 60, 30, 0.85, 1.1\nset key at screen 1.0, 0.9\n"<<
  "set style textbox opaque noborder margins 0.5, 0.5\n";

  gp << "set title 'contour plot'\nset contour\n";
  gp << "splot x*y\n";

	//gp << "plot '-' with lines title 'cubic', '-' with points title 'circle'\n";
	//gp.send1d(xy_pts_A);
	//gp.send1d(xy_pts_B);

	//gp << "pause -1\n";

}

void callthis(){
  cout <<  "called this" <<  endl;
  // demo_binary();
  demo_animation();
  //demo_contour();
  //rm demo_basic();
}

template<class T>
void Plotter<T>::plotInputOutputScatter(arma::mat &hypotheses){

	cout << "***Plotting Input Output***" << endl <<  endl;

	cout << &hypotheses << endl << endl;
	cout << hypotheses << endl << endl;

	#ifdef _WIN32
  	std::cout << "Sorry, the animation demo doesn't work in Windows." << std::endl;
  	return;
  #endif

	Gnuplot gp,gp2;
	mat x = *this->x;
	mat y = *this->y;
	mat both;
	const int N = 100;
	std::vector<double> pts(N);

	gp << "set xrange [0:50]\nset yrange [0:50]\n";

while(1) {

		for(int i=0; i<N; i++) {
      //default animations
			//	i * x + 1
			double insideSum = 0;
			//cout << "hypotheses" << endl;
			//cout << hypotheses << endl;

			for(int j = 0; j < hypotheses.n_rows; j++){
				insideSum += ( hypotheses(j,0) ) * i;
				//cout << "--insideSum "<< j << " value: " << hypotheses(j,0) <<"--" << endl;
				//cout << insideSum << endl;
			}

			//theta + theta * i

			pts[i] = insideSum;
			//cout << pts[i] << endl;

		}

    //working double plot
    // gp << "plot '-' binary" << gp.binFmt1d(pts, "array") << "with lines notitle";

		//////////

		//cout << " ---- plotting input and output ----" << endl;
		//cout << x << endl;
		//cout << y << endl;
		//cout << " ---- end plotting input and output ----" << endl;

		for(int i = 0; i < x.n_rows; i++){
			mat xt = x;
			//remove identity column
			//xt.shed_col(0);
			mat yt = y;
			both = join_rows(xt,yt);
			both.save("scatter.dat", raw_ascii);
		}

		//cout << "------- Points -----" << endl;
		//cout << both << endl;

		//gp << "set xrange [-10:10]\nset yrange [-10:10]\n";

		//gp << "set data style points\n";
		//gp << "plot '-' with lines title 'cubic', '-' with points title 'circle'\n";
		//gp << "plot " << gp.binFmt1d(xy_pts_A, "record") << " using 1:2 with points\n";

		gp << "plot '-' binary"
		<< gp.binFmt1d(pts, "array")
		<< "with lines notitle, 'scatter.dat' \n";

		//gp << "plot 'scatter.dat' \n";

		pause_if_needed();

		gp.sendBinary1d(pts);
		gp.flush();

		mysleep(50);

}//while end

		///////animations

}

void demo_3d(){

	Gnuplot gp;

	std::cout << "Press Ctrl-C to quit (closing gnuplot window doesn't quit)." << std::endl;

	//gp << "set yrange [0:100]\n";
  //gp << "set xrange [0:10]\n";

  //default for default animation
  //gp << "set yrange [-1:1]\n";

	const int N = 100;
	std::vector<double> pts(N);
  std::vector<double> pts2(N);

	double theta = 0;

	while(1) {

		for(int i=0; i<N; i++) {
			double alpha = (double(i)/N-0.5) * 10;

      //default animations
			pts[i] = sin(alpha*8.0 + theta) * exp(-alpha*alpha/2.0);

      //pts[i] = pow(i,2) + 1;
      //pts2[i] = i;
		}

    //working double plot
    // gp << "plot '-' binary" << gp.binFmt1d(pts, "array")
		// << "with lines notitle, '-' binary" << gp.binFmt1d(pts2, "array")
		// << "with lines notitle\n";

		//gp << "set xrange [-10:10]\nset yrange [-10:10]\nset zrange[-10:10]\nset cbrange[-10:10]\n";

		gp << "set samples 20\nset isosamples 21\nset xlabel 'Theta_1'\nset ylabel 'Theta_2'\n"
		<< "set zlabel 'J'\nset view 60, 30, 0.85, 1.1\nset key at screen 1.0, 0.9\n"
		<< "set style textbox opaque noborder margins 0.5, 0.5\n";

	  gp << "set title 'Gradient Descent'\nset pm3d\n";

		gp << "unset surface\n";

		//												  h(x)						y
	  //gp << "splot (  (1/(2*3)) * ( ( ((x*0.8)+(y*0.5)) - (x**3) )**2 )  )  \n";

		//gp << "splot " << gp.binFmt1d(pts, "array") << "  \n";
		gp << "splot (1/1)*( ( ((x*1.8)+(y*1.5)) - (0.02) )**2 )  \n";

		//gp.sendBinary1d(pts);
		gp.flush();

		theta += 0.2;
		mysleep(1000);
	}

}

void demo_3d2(){

  Gnuplot gp;

	//gp << "set xrange [-10:10]\nset yrange [-10:10]\nset zrange[-10:10]\nset cbrange[-10:10]\n";

	gp << "set samples 20\nset isosamples 21\nset xlabel 'Theta_1'\nset ylabel 'Theta_2'\n"<<
  "set zlabel 'J'\nset view 60, 30, 0.85, 1.1\nset key at screen 1.0, 0.9\n"<<
  "set style textbox opaque noborder margins 0.5, 0.5\n";

  gp << "set title 'Gradient Descent'\nset pm3d\n";

	//gp << "set border 4095 front lt black linewidth 1.000 dashtype solid\n";
	//gp << "set pm3d implicit at s\nset pm3d scansbackward\n";
	gp << "unset surface\n";

	//												  h(x)						y
  //gp << "splot (  (1/(2*3)) * ( ( ((x*0.8)+(y*0.5)) - (x**3) )**2 )  )  \n";

	gp << "splot (1/1)*( ( ((x*1.8)+(y*1.5)) - (0.02) )**2 )  \n";

	//gp << "splot x*y**2 \n";

	//gp << "plot '-' with lines title 'cubic', '-' with points title 'circle'\n";
	//gp.send1d(xy_pts_A);
	//gp.send1d(xy_pts_B);

	//gp << "pause -1\n";

}


/**
 * This file implements the partial widths for the `SimpleRhNeutrino` model.
 */

#include "storm/constants.hpp"
#include "storm/rambo.hpp"
#include "storm/simple.hpp"
#include "storm/types.hpp"
#include <Pythia8/Basics.h>
#include <algorithm>
#include <array>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cmath>
#include <cstddef>
#include <functional>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_math.h>
#include <locale>
#include <numeric>
#include <stdexcept>
#include <utility>

namespace storm {

//=========================
//---- Two-Body Widths ----
//=========================

auto width_vr_to_vl_h(double mvr, double theta, int /*genl*/) -> double {
  if (mvr < kHIGGS_MASS) {
    return 0.0;
  }
  return ((-pow(kHIGGS_MASS, 2) + pow(mvr, 2)) * pow(theta, 2) *
          std::abs(pow(kHIGGS_MASS, 2) - pow(mvr, 2))) /
         (16. * mvr * M_PI * pow(kHIGGS_VEV, 2));
}

auto width_vr_to_vl_z(double mvr, double theta, int /*genl*/) -> double {
  if (mvr < kZ_BOSON_MASS) {
    return 0.0;
  }
  return (((pow(mvr, 4) + pow(mvr, 2) * pow(kZ_BOSON_MASS, 2) -
            2 * pow(kZ_BOSON_MASS, 4)) *
           kALPHA_EM * pow(theta, 2) *
           std::abs(pow(mvr, 2) - pow(kZ_BOSON_MASS, 2))) /
          (16.0 * pow(kCOS_THETA_WEAK, 2) * pow(mvr, 3) *
           pow(kZ_BOSON_MASS, 2) * pow(kSIN_THETA_WEAK, 2)));
}

auto width_vr_to_l_w(double mvr, double theta, int genl) -> double {

  if (genl < 0 || genl > 2) {
    // throw std::invalid_argument(
    //    "Invalid generation passed to width_vr_l_w. Use 0, 1 or 2.");
    return 0.0;
  }

  const double ml = kLEPTON_MASSES.at(genl);

  if (mvr < ml + kW_BOSON_MASS) {
    return 0.0;
  }

  return (std::sqrt((ml - mvr - kW_BOSON_MASS) * (ml + mvr - kW_BOSON_MASS) *
                    (ml - mvr + kW_BOSON_MASS) * (ml + mvr + kW_BOSON_MASS)) *
          (pow(pow(ml, 2) - pow(mvr, 2), 2) +
           (pow(ml, 2) + pow(mvr, 2)) * pow(kW_BOSON_MASS, 2) -
           2 * pow(kW_BOSON_MASS, 4)) *
          kALPHA_EM * pow(theta, 2)) /
         (16. * pow(mvr, 3) * pow(kW_BOSON_MASS, 2) * pow(kSIN_THETA_WEAK, 2));
}

//===========================
//---- Three-Body Widths ----
//===========================

/**
 * Compute the integration bounds on the Mandelstam variable `s` for three-body
 * phase-space integration.
 * @param m Mass of the decaying particle.
 * @param m1 Mass of final state particle 1.
 * @param m2 Mass of final state particle 2.
 * @param m3 Mass of final state particle 3.
 */
inline static auto compute_s_bounds(double m, double m1, double m2, double m3)
    -> std::pair<double, double> {
  return std::make_pair(pow(m2 + m3, 2), pow(m - m1, 2));
}

/**
 * Compute the integration bounds on the mandelstam variable t.
 */
inline static auto compute_t_bounds(double s, double m, double m1, double m2,
                                    double m3) -> std::pair<double, double> {
  const double t1 = (pow(m1, 2) * pow(m2, 2) - pow(m1, 2) * pow(m3, 2) +
                     pow(m1, 2) * s + pow(m2, 2) * s + pow(m3, 2) * s -
                     pow(s, 2) + pow(m, 2) * (-pow(m2, 2) + pow(m3, 2) + s) -
                     sqrt(pow(m, 4) + pow(pow(m1, 2) - s, 2) -
                          2 * pow(m, 2) * (pow(m1, 2) + s)) *
                         sqrt(pow(m2, 4) + pow(pow(m3, 2) - s, 2) -
                              2 * pow(m2, 2) * (pow(m3, 2) + s))) /
                    (2. * s);
  const double t2 = (pow(m1, 2) * pow(m2, 2) - pow(m1, 2) * pow(m3, 2) +
                     pow(m1, 2) * s + pow(m2, 2) * s + pow(m3, 2) * s -
                     pow(s, 2) + pow(m, 2) * (-pow(m2, 2) + pow(m3, 2) + s) +
                     sqrt(pow(m, 4) + pow(pow(m1, 2) - s, 2) -
                          2 * pow(m, 2) * (pow(m1, 2) + s)) *
                         sqrt(pow(m2, 4) + pow(pow(m3, 2) - s, 2) -
                              2 * pow(m2, 2) * (pow(m3, 2) + s))) /
                    (2. * s);
  return (t1 < t2) ? std::make_pair(t1, t2) : std::make_pair(t2, t1);
}

/**
 * Compute a three-body decay width given the squared matrix element, mass of
 * the decaying particle and final-state particle masses using RAMBO.
 */
static auto compute_width_3body(const SquaredMatrixElement &msqrd,
                                const double m, std::vector<double> fsp_masses,
                                size_t num_events)
    -> std::pair<double, double> {

  if (m < std::accumulate(fsp_masses.begin(), fsp_masses.end(), 0.0)) {
    return std::make_pair(0.0, 0.0);
  }

  Rambo rambo{std::move(fsp_masses), m, msqrd};
  const auto width = rambo.compute_width(num_events);
  return width;
}

/**
 * Compute a three-body decay width given a function for the
 * partially-integrated (over Mandelstam variable `t`).
 */
static auto compute_width_3body_quad(gsl_function *integrand, const double m,
                                     std::array<double, 3> fsp_masses)
    -> std::pair<double, double> {

  if (m < std::accumulate(fsp_masses.begin(), fsp_masses.end(), 0.0)) {
    return std::make_pair(0.0, 0.0);
  }

  gsl_set_error_handler_off();

  auto [s_low, s_high] =
      compute_s_bounds(m, fsp_masses[0], fsp_masses[1], fsp_masses[2]);

  // Break-points for integration
  std::vector<double> bpts;
  bpts.reserve(5);
  bpts.push_back(s_low);
  // If MW^2, MZ^2 or MH^2 is in the integration interval, add them to
  // break-points
  const double mw2 = kW_BOSON_MASS * kW_BOSON_MASS;
  const double mz2 = kZ_BOSON_MASS * kZ_BOSON_MASS;
  const double mh2 = kHIGGS_MASS * kHIGGS_MASS;
  if (s_low < mw2 && mw2 < s_high) {
    bpts.push_back(mw2);
  }
  if (s_low < mz2 && mz2 < s_high) {
    bpts.push_back(mz2);
  }
  if (s_low < mh2 && mh2 < s_high) {
    bpts.push_back(mh2);
  }
  bpts.push_back(s_high);

  gsl_integration_workspace *w = gsl_integration_workspace_alloc(1000);

  double integral = 0.0;
  double error = 0.0;
  gsl_integration_qagp(integrand, bpts.data(), bpts.size(), 0.0, 1e-7, 1000, w,
                       &integral, &error);

  const double pf = 1.0 / (32.0 * pow(2.0 * M_PI * m, 3));
  return std::make_pair(std::abs(integral * pf), error * pf);
}

/**
 * Compute the partial width for a right-handed neutrino to decay into an active
 * neutrino and two up-type quarks.
 * @param mvr Mass of the RH neutrino.
 * @param theta Mixing angle between active and RH neutrino.
 * @param genl Generation of the lepton.
 * @param genq Generation of the down-type quarks.
 * @returns The width and an error estimate.
 */
auto width_vr_to_vl_u_u(double mvr, double theta, int genl, int genq)
    -> double {
  const double mu = kUP_QUARK_MASSES.at(genq);
  // auto msqrd = [mvr, theta, genq](const std::vector<Pythia8::Vec4> &momenta)
  // {
  //  return msqrd_vr_to_vl_u_u(momenta, mvr, theta, genq);
  //};
  // auto msqrd = [mvr, theta, genl, genq](double s) {
  //  return partialy_integrated_msqrd_vr_to_vl_u_u(s, mvr, theta, genl, genq);
  //};

  PartiallyIntegratedMsqrdParams params{mvr, theta, genl, genq};

  gsl_function integrand;
  integrand.function = &partialy_integrated_msqrd_vr_to_vl_u_u;
  integrand.params = &params;

  return compute_width_3body_quad(&integrand, mvr, {0.0, mu, mu}).first;
}

/**
 * Compute the partial width for a right-handed neutrino to decay into an active
 * neutrino and two down-type quarks.
 * @param mvr Mass of the RH neutrino.
 * @param theta Mixing angle between active and RH neutrino.
 * @param genl Generation of the lepton.
 * @param genq Generation of the up-type quarks.
 * @returns The width and an error estimate.
 */
auto width_vr_to_vl_d_d(double mvr, double theta, int genl, int genq)
    -> double {
  const double md = kDOWN_QUARK_MASSES.at(genq);
  // auto msqrd = [mvr, theta, genq](const std::vector<Pythia8::Vec4> &momenta)
  // {
  //  return msqrd_vr_to_vl_d_d(momenta, mvr, theta, genq);
  //};
  // return compute_width_3body(msqrd, mvr, {0.0, md, md}, nevents);

  // auto msqrd = [mvr, theta, genl, genq](double s) {
  //  return partialy_integrated_msqrd_vr_to_vl_u_u(s, mvr, theta, genl, genq);
  //};

  PartiallyIntegratedMsqrdParams params{mvr, theta, genl, genq};

  gsl_function integrand;
  integrand.function = &partialy_integrated_msqrd_vr_to_vl_d_d;
  integrand.params = &params;

  return compute_width_3body_quad(&integrand, mvr, {0.0, md, md}).first;
}

/**
 * Compute the partial width for a right-handed neutrino to decay into a charged
 * lepton, an up-type quark and a down-type quark.
 * @param mvr Mass of the RH neutrino.
 * @param theta Mixing angle between active and RH neutrino.
 * @param genl Generation of the lepton.
 * @param genq Generation of the quarks.
 * @returns The width and an error estimate.
 */
auto width_vr_to_l_u_d(double mvr, double theta, int genl, int genq) -> double {
  const double ml = kLEPTON_MASSES.at(genl);
  const double mu = kUP_QUARK_MASSES.at(genq);
  const double md = kDOWN_QUARK_MASSES.at(genq);
  // auto msqrd = [mvr, theta, genl,
  //              genq](const std::vector<Pythia8::Vec4> &momenta) {
  //  return msqrd_vr_to_l_u_d(momenta, mvr, theta, genl, genq);
  //};
  // return compute_width_3body(msqrd, mvr, {ml, mu, md}, nevents);

  // auto msqrd = [mvr, theta, genl, genq](double s) {
  //  return partialy_integrated_msqrd_vr_to_l_u_d(s, mvr, theta, genl, genq);
  //};

  PartiallyIntegratedMsqrdParams params{mvr, theta, genl, genq};

  gsl_function integrand;
  integrand.function = &partialy_integrated_msqrd_vr_to_vl_d_d;
  integrand.params = &params;

  return compute_width_3body_quad(&integrand, mvr, {ml, mu, md}).first;
}

/**
 * Compute the partial width for a right-handed neutrino to decay into an active
 * neutrino, and two charged leptons with different generations than neutrino.
 * @param mvr Mass of the RH neutrino.
 * @param theta Mixing angle between active and RH neutrino.
 * @param genl Generation of the neutrino.
 * @param genlp Generation of charged leptons.
 * @returns The width.
 */
auto width_vr_to_vl_lp_lp(double mvr, double theta, int genl, int genlp)
    -> double {

  const double mlp = kLEPTON_MASSES.at(genlp);

  PartiallyIntegratedMsqrdParams params{mvr, theta, genl, genlp};

  gsl_function integrand;
  integrand.function = &partialy_integrated_msqrd_vr_to_vl_lp_lp;
  integrand.params = &params;

  return compute_width_3body_quad(&integrand, mvr, {0.0, mlp, mlp}).first;
}

/**
 * Compute the partial width for a right-handed neutrino to decay into an active
 * neutrino, and two charged leptons with different generations than neutrino.
 * @param mvr Mass of the RH neutrino.
 * @param theta Mixing angle between active and RH neutrino.
 * @param genl Generation of the neutrino.
 * @param genlp Generation of charged leptons.
 * @returns The width.
 */
auto width_vr_to_vlp_lp_l(double mvr, double theta, int genl, int genlp)
    -> double {

  const double ml = kLEPTON_MASSES.at(genl);
  const double mlp = kLEPTON_MASSES.at(genlp);

  PartiallyIntegratedMsqrdParams params{mvr, theta, genl, genlp};

  gsl_function integrand;
  integrand.function = &partialy_integrated_msqrd_vr_to_vlp_lp_l;
  integrand.params = &params;

  return compute_width_3body_quad(&integrand, mvr, {0.0, mlp, ml}).first;
}

/**
 * Compute the partial width for a right-handed neutrino to decay into an active
 * neutrino, and two charged leptons with of the same generation as neutrino.
 * @param mvr Mass of the RH neutrino.
 * @param theta Mixing angle between active and RH neutrino.
 * @param genl Generation of the final-state leptons.
 * @returns The width.
 */
auto width_vr_to_vl_l_l(double mvr, double theta, int genl) -> double {

  const double ml = kLEPTON_MASSES.at(genl);

  PartiallyIntegratedMsqrdParams params{mvr, theta, genl, -1};

  gsl_function integrand;
  integrand.function = &partialy_integrated_msqrd_vr_to_vl_l_l;
  integrand.params = &params;

  return compute_width_3body_quad(&integrand, mvr, {0.0, ml, ml}).first;
}

/**
 * Compute the partial width for a right-handed neutrino to decay into a three
 * active neutrinos, all of the same generation.
 * @param mvr Mass of the RH neutrino.
 * @param theta Mixing angle between active and RH neutrino.
 * @param genl Generation of the final-state leptons.
 * @returns The width.
 */
auto width_vr_to_vl_vl_vl(double mvr, double theta, int /*genl*/) -> double {

  PartiallyIntegratedMsqrdParams params{mvr, theta, -1, -1};

  gsl_function integrand;
  integrand.function = &partialy_integrated_msqrd_vr_to_vl_vl_vl;
  integrand.params = &params;

  // Extra factor of 1 / 6 us for identical final state particles.
  return compute_width_3body_quad(&integrand, mvr, {0.0, 0.0, 0.0}).first / 6.0;
}

} // namespace storm


//  (C) Copyright Victor Ananyev 2021.
//  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 <random>
#include <vector>
#include <boost/math/distributions.hpp>
#include <benchmark/benchmark.h>

template <class Z, template<typename> class dist >
void test_mode_2param(benchmark::State& state)
{
    using boost::math::normal_distribution;

    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::normal_distribution<Z> noise(0., 1E-6);

    for (auto _ : state)
    {
        state.PauseTiming();
        Z p1 = state.range(0) + noise(mt);
        Z p2 = state.range(1) + noise(mt);
        dist<Z> the_dist(p1, p2);
        state.ResumeTiming();
        try {
            benchmark::DoNotOptimize(mode(the_dist));
        }
        catch (boost::wrapexcept<boost::math::evaluation_error>& e) {
            state.SkipWithError(e.what());
            break;
        }
    }
}

static void fixed_ratio_2args(benchmark::internal::Benchmark* b, long double left_div_right, std::vector<int64_t> lefts) {
    for (const long double &left: lefts) {
        b->Args({static_cast<int64_t>(left), static_cast<int64_t>((left/left_div_right))});
    }
}

using boost::math::non_central_chi_squared_distribution;

BENCHMARK_TEMPLATE(test_mode_2param, long double, non_central_chi_squared_distribution)->ArgsProduct({
    {2, 15, 50},
    benchmark::CreateRange(4, 1024, /*multi=*/2)
})->Name("fixed_k");

BENCHMARK_TEMPLATE(test_mode_2param, long double, non_central_chi_squared_distribution)->ArgsProduct({
    benchmark::CreateRange(4, 4096, /*multi=*/2),
    {1, 30, 100, 500}
})->Name("fixed_nc");

BENCHMARK_TEMPLATE(test_mode_2param, long double, non_central_chi_squared_distribution)
    -> Apply([](benchmark::internal::Benchmark*b) {
                fixed_ratio_2args(b, 0.05, benchmark::CreateRange(4, 4096, /*multi=*/2));
    }) -> Name("fixed_scale_0_05");

BENCHMARK_TEMPLATE(test_mode_2param, long double, non_central_chi_squared_distribution)
    -> Apply([](benchmark::internal::Benchmark*b) {
                fixed_ratio_2args(b, 0.15, benchmark::CreateRange(4, 4096, /*multi=*/2));
    }) -> Name("fixed_scale_0_15");

BENCHMARK_TEMPLATE(test_mode_2param, long double, non_central_chi_squared_distribution)
    -> Apply([](benchmark::internal::Benchmark*b) {
                fixed_ratio_2args(b, 0.25, benchmark::CreateRange(4, 4096, /*multi=*/2));
    }) -> Name("fixed_scale_0_25");

BENCHMARK_MAIN();


#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <dials/array_family/reflection_table.h>

namespace xfel {
namespace merging {
namespace error_model {
namespace sdfac_refine {

typedef
 scitbx::af::versa<cctbx::miller::index<>, scitbx::af::flex_grid<> > shared_miller;

using namespace dials::af;

static scitbx::af::shared<double>
compute_normalized_deviations(reflection_table ISIGI, shared_miller hkl_list) {
  /*
   * This formulation of the normalized deviations of a set of intensities and sigmas is similar to that
   * described in Evans 2011, but includes the nn term as currently implmented by aimless
   *
   */
  SCITBX_ASSERT(ISIGI.contains("scaled_intensity"));
  SCITBX_ASSERT(ISIGI.contains("isigi"));
  SCITBX_ASSERT(ISIGI.contains("miller_id"));

  scitbx::af::shared<double>         result(ISIGI.size(), 0);
  scitbx::af::shared<bool>           accepted(ISIGI.size(), false);
  scitbx::af::shared<double>         sigmas(ISIGI.size(), 0);

  scitbx::af::shared<double>         isum(hkl_list.size(), 0);
  scitbx::af::shared<double>         n_accept(hkl_list.size(), 0);

  scitbx::af::const_ref<double>      scaled_intensity = ISIGI["scaled_intensity"];
  scitbx::af::const_ref<double>      isigi = ISIGI["isigi"];
  scitbx::af::const_ref<std::size_t> miller_id = ISIGI["miller_id"];

  for (std::size_t i = 0; i < ISIGI.size(); i++) {
    // scaled intensity (iobs/slope)
    // corrected sigma (original sigma/slope)
    accepted[i] = isigi[i] != 0;
    if (isigi[i] == 0)
      continue;

    sigmas[i] = scaled_intensity[i] / isigi[i];
    accepted[i] = sigmas[i] > 0;
    if (sigmas[i] <= 0)
      continue;

    isum[miller_id[i]] += scaled_intensity[i];
    n_accept[miller_id[i]]++;
  }

  scitbx::af::shared<double> nn(hkl_list.size(), 0);
  for (std::size_t i = 0; i < hkl_list.size(); i++) {
    if (n_accept[i] > 0) {
      nn[i] = std::sqrt((n_accept[i]-1.0)/n_accept[i]);
    }
  }

  for (std::size_t i = 0; i < ISIGI.size(); i++) {
    if (!accepted[i]) continue;

    std::size_t n = n_accept[miller_id[i]];
    double meanIprime = (isum[miller_id[i]]-scaled_intensity[i]) / (n>1 ? (n-1) : 1);
    result[i] = nn[miller_id[i]] * (scaled_intensity[i] - meanIprime) / sigmas[i];
  }
  return result;
}

void
apply_sd_error_params(reflection_table ISIGI, const double sdfac, const double sdb, const double sdadd, const bool squared_params) {
  /*
   * Apply a set of sd params (sdfac, sdb and sdd) to an ISIGI reflection table

   Squared not only uses the squared formulation of sigma', but also fixes 2 bugs:
   1) Use meanI not meanIprime
   2) When returning isigi, don't multiply by slope

   If using squared, it is assumed that sdfac, sdb and sdadd have already been squared
   */
  SCITBX_ASSERT(ISIGI.contains("scaled_intensity"));
  SCITBX_ASSERT(ISIGI.contains("isigi"));
  SCITBX_ASSERT(ISIGI.contains("slope"));
  SCITBX_ASSERT(ISIGI.contains("miller_id"));

  scitbx::af::const_ref<double>      scaled_intensity = ISIGI["scaled_intensity"];
  scitbx::af::ref<double>            isigi = ISIGI["isigi"];
  scitbx::af::const_ref<double>      slope = ISIGI["slope"];
  scitbx::af::shared<double>         sigmas(ISIGI.size(), 0);
  scitbx::af::const_ref<std::size_t> miller_id = ISIGI["miller_id"];

  std::size_t max_miller_id = scitbx::af::max(miller_id);
  scitbx::af::shared<std::size_t>    n_refl(max_miller_id+1, 0);
  scitbx::af::shared<double>         isum(max_miller_id+1, 0);

  for (std::size_t i = 0; i < ISIGI.size(); i++) {
    // scaled intensity (iobs/slope)
    // corrected sigma (original sigma/slope)
    sigmas[i] = scaled_intensity[i] / isigi[i];

    isum[miller_id[i]] += scaled_intensity[i];
    n_refl[miller_id[i]]++;
  }

  double tmp = 0;
  double sigma_corrected = 0;
  for (std::size_t i = 0; i < ISIGI.size(); i++) {
    // apply correction parameters
    if (squared_params) {
      // compute meanI, which is the mean of all observations of this hkl
      double meanI = isum[miller_id[i]] / n_refl[miller_id[i]];
      tmp = std::pow(sigmas[i],2) + sdb * meanI + sdadd * std::pow(meanI,2);
    }
    else {
      // compute meanIprime, which for each observation, is the mean of all other observations of this hkl
      double meanIprime = (isum[miller_id[i]]-scaled_intensity[i]) / (n_refl[miller_id[i]]>1 ? (n_refl[miller_id[i]]-1) : 1);
      tmp = std::pow(sigmas[i],2) + sdb * meanIprime + std::pow(sdadd*meanIprime,2);
    }

    // avoid rare negatives
    double minimum = 0.1 * std::pow(sigmas[i],2);
    if (tmp < minimum)
      tmp = minimum;

    if (squared_params) {
      sigma_corrected = std::sqrt(sdfac * tmp);
      SCITBX_ASSERT(sigma_corrected != 0.0);
      isigi[i] = scaled_intensity[i] / sigma_corrected;
    }
    else {
      sigma_corrected = sdfac * std::sqrt(tmp);
      SCITBX_ASSERT(sigma_corrected != 0.0);
      isigi[i] = scaled_intensity[i] * slope[i]/ sigma_corrected;
    }
  }
}

namespace boost_python { namespace {
  void
  init_module() {
    using namespace boost::python;
    def("compute_normalized_deviations", &compute_normalized_deviations);
    def("apply_sd_error_params", &apply_sd_error_params);
}
}}
}}}} // namespace

BOOST_PYTHON_MODULE(xfel_sdfac_refine_ext)
{
  xfel::merging::error_model::sdfac_refine::boost_python::init_module();

}


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef KDL_CHAIN_IKSOLVERVEL_WDLS_HPP
#define KDL_CHAIN_IKSOLVERVEL_WDLS_HPP

#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include <Eigen/Core>

namespace KDL
{
    /**
     * Implementation of a inverse velocity kinematics algorithm based
     * on the weighted pseudo inverse with damped least-square to calculate the velocity
     * transformation from Cartesian to joint space of a general
     * KDL::Chain. It uses a svd-calculation based on householders
     * rotations.
     *
     * J# = M_q*Vb*pinv_dls(Db)*Ub'*M_x
     *
     * where B = Mx*J*Mq
     *
     * and B = Ub*Db*Vb' is the SVD decomposition of B
     *
     * Mq and Mx represent, respectively, the joint-space and task-space weighting
     * matrices.
     * Please refer to the documentation of setWeightJS(const Eigen::MatrixXd& Mq)
     * and setWeightTS(const Eigen::MatrixXd& Mx) for details on the effects of
     * these matrices.
     *
     * For more details on Weighted Pseudo Inverse, see :
     * 1) [Ben Israel 03] A. Ben Israel & T.N.E. Greville.
     * Generalized Inverses : Theory and Applications,
     * second edition. Springer, 2003. ISBN 0-387-00293-6.
     *
     * 2) [Doty 93] K. L. Doty, C. Melchiorri & C. Boniveto.
     * A theory of generalized inverses applied to Robotics.
     * The International Journal of Robotics Research,
     * vol. 12, no. 1, pages 1-19, february 1993.
     *
     *
     * @ingroup KinematicFamily
     */
    class ChainIkSolverVel_wdls : public ChainIkSolverVel
    {
    public:
        /// solution converged but (pseudo)inverse is singular
        static const int E_CONVERGE_PINV_SINGULAR = +100;

        /**
         * Constructor of the solver
         *
         * @param chain the chain to calculate the inverse velocity
         * kinematics for
         * @param eps if a singular value is below this value, its
         * inverse is set to zero, default: 0.00001
         * @param maxiter maximum iterations for the svd calculation,
         * default: 150
         *
         */

        explicit ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150);
        //=ublas::identity_matrix<double>
        ~ChainIkSolverVel_wdls();

        /**
         * Find an output joint velocity \a qdot_out, given a starting joint pose
         * \a q_init and a desired cartesian velocity \a v_in
         *
         * @return
         *  E_NOERROR=svd solution converged in maxiter
         *  E_SVD_FAILED=svd solution failed
         *  E_CONVERGE_PINV_SINGULAR=svd solution converged but (pseudo)inverse singular
         *
         * @note if E_CONVERGE_PINV_SINGULAR returned then converged and can
         * continue motion, but have degraded solution
         *
         * @note If E_SVD_FAILED returned, then getSvdResult() returns the error
         * code from the SVD algorithm.
		 */
        virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
        /**
         * not (yet) implemented.
         *
         */
        virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};

        /**
         * Set the joint space weighting matrix
         *
         * @param weight_js joint space weighting symmetric matrix,
         * default : identity.  M_q : This matrix being used as a
         * weight for the norm of the joint space speed it HAS TO BE
         * symmetric and positive definite. We can actually deal with
         * matrices containing a symmetric and positive definite block
         * and 0s otherwise. Taking a diagonal matrix as an example, a
         * 0 on the diagonal means that the corresponding joints will
         * not contribute to the motion of the system. On the other
         * hand, the bigger the value, the most the corresponding
         * joint will contribute to the overall motion. The obtained
         * solution q_dot will actually minimize the weighted norm
         * sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal
         * with, it does not make sense to invert M_q but what is
         * important is the physical meaning of all this : a joint
         * that has a zero weight in M_q will not contribute to the
         * motion of the system and this is equivalent to saying that
         * it gets an infinite weight in the norm computation.  For
         * more detailed explanation : vincent.padois@upmc.fr
         *
         * @return success/error code
         */
        int setWeightJS(const Eigen::MatrixXd& Mq);

        /**
         * Set the task space weighting matrix
         *
         * @param weight_ts task space weighting symmetric matrix,
         * default: identity M_x : This matrix being used as a weight
         * for the norm of the error (in terms of task space speed) it
         * HAS TO BE symmetric and positive definite. We can actually
         * deal with matrices containing a symmetric and positive
         * definite block and 0s otherwise. Taking a diagonal matrix
         * as an example, a 0 on the diagonal means that the
         * corresponding task coordinate will not be taken into
         * account (ie the corresponding error can be really big). If
         * the rank of the jacobian is equal to the number of task
         * space coordinates which do not have a 0 weight in M_x, the
         * weighting will actually not impact the results (ie there is
         * an exact solution to the velocity inverse kinematics
         * problem). In cases without an exact solution, the bigger
         * the value, the most the corresponding task coordinate will
         * be taken into account (ie the more the corresponding error
         * will be reduced). The obtained solution will minimize the
         * weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|).
         * For more detailed explanation : vincent.padois@upmc.fr
         *
         * @return success/error code
         */
        int setWeightTS(const Eigen::MatrixXd& Mx);

        /**
         * Set lambda
         */
        void setLambda(const double lambda);
        /**
         * Set eps
         */
        void setEps(const double eps_in);
        /**
         * Set maxIter
         */
        void setMaxIter(const int maxiter_in);

        /**
         * Request the number of singular values of the jacobian that are < eps;
         * if the number of near zero singular values is > jac.col()-jac.row(),
         * then the jacobian pseudoinverse is singular
         */
        unsigned int getNrZeroSigmas()const {return nrZeroSigmas;};

        /**
         * Request the minimum of the first six singular values
         */
        double getSigmaMin()const {return sigmaMin;};

        /**
         * Request the six singular values of the Jacobian
         */
        int getSigma(Eigen::VectorXd& Sout);

        /**
         * Request the value of eps
         */
        double getEps()const {return eps;};

        /**
         * Request the value of lambda for the minimum
         */
        double getLambda()const {return lambda;};

        /**
         * Request the scaled value of lambda for the minimum
         * singular value 1-6
         */
        double getLambdaScaled()const {return lambda_scaled;};

        /**
         * Retrieve the latest return code from the SVD algorithm
         * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code.
         */
        int getSVDResult()const {return svdResult;};

        /// @copydoc KDL::SolverI::strError()
        virtual const char* strError(const int error) const;

        /// @copydoc KDL::SolverI::updateInternalDataStructures()
        virtual void updateInternalDataStructures();

    private:
        const Chain& chain;
        ChainJntToJacSolver jnt2jac;
        unsigned int nj;
        Jacobian jac;
        Eigen::MatrixXd U;
        Eigen::VectorXd S;
        Eigen::MatrixXd V;
        double eps;
        int maxiter;
        Eigen::VectorXd tmp;
        Eigen::MatrixXd tmp_jac;
        Eigen::MatrixXd tmp_jac_weight1;
        Eigen::MatrixXd tmp_jac_weight2;
        Eigen::MatrixXd tmp_ts;
        Eigen::MatrixXd tmp_js;
        Eigen::MatrixXd weight_ts;
        Eigen::MatrixXd weight_js;
        double lambda;
		double lambda_scaled;
		unsigned int nrZeroSigmas ;
		int svdResult;
		double sigmaMin;
    };
}
#endif


#include "sbs/rendering/trackball_rotation_adapter.h"

#include <Eigen/Geometry>

namespace sbs {
namespace rendering {
namespace detail {

static Eigen::Vector3d geometric_center(common::dynamic_surface_mesh const& mesh)
{
    Eigen::Vector3d mu{0., 0., 0.};

    for (std::size_t vi = 0u; vi < mesh.vertex_count(); ++vi)
    {
        auto const& v = mesh.vertex(vi);
        mu += v.position;
    }

    mu = mu.array() / mesh.vertex_count();
    return mu;
}

static void rotate(common::dynamic_surface_mesh& mesh, Eigen::Matrix3d const& rotation)
{
    for (std::size_t vi = 0u; vi < mesh.vertex_count(); ++vi)
    {
        auto& v            = mesh.mutable_vertex(vi);
        Eigen::Vector3d& p = v.position;
        p                  = rotation * p;
    }
}

static void translate(common::dynamic_surface_mesh& mesh, Eigen::Vector3d const& t)
{
    for (std::size_t vi = 0u; vi < mesh.vertex_count(); ++vi)
    {
        auto& v = mesh.mutable_vertex(vi);
        v.position += t;
    }
}

} // namespace detail

trackball_rotation_adapter_t::trackball_rotation_adapter_t(
    common::dynamic_surface_mesh* mesh,
    double rotation_speed,
    double translation_speed)
    : rotated_mesh_(mesh),
      rotation_speed_(rotation_speed),
      translation_speed_(translation_speed),
      pitch_angle_(0.),
      yaw_angle_(0.),
      pitch_axis_({1., 0., 0.}),
      yaw_axis_({0., 0., 1.})
{
}

double trackball_rotation_adapter_t::rotation_speed() const
{
    return rotation_speed_;
}

void trackball_rotation_adapter_t::set_rotation_speed(double speed)
{
    rotation_speed_ = speed;
}

double trackball_rotation_adapter_t::translation_speed() const
{
    return translation_speed_;
}

void trackball_rotation_adapter_t::set_translation_speed(double speed)
{
    translation_speed_ = speed;
}

void trackball_rotation_adapter_t::rotate(double dx, double dy)
{
    yaw_angle_   = rotation_speed_ * dx;
    pitch_angle_ = rotation_speed_ * dy;

    Eigen::AngleAxisd const yaw(yaw_angle_, yaw_axis_);
    Eigen::AngleAxisd const pitch(pitch_angle_, pitch_axis_);
    Eigen::Matrix3d const rotation = pitch.toRotationMatrix() * yaw.toRotationMatrix();

    Eigen::Vector3d const geometric_center = detail::geometric_center(*rotated_mesh_);
    detail::translate(*rotated_mesh_, -geometric_center);
    detail::rotate(*rotated_mesh_, rotation);
    detail::translate(*rotated_mesh_, geometric_center);
}

void trackball_rotation_adapter_t::translate(double dx, double dy)
{
    Eigen::Vector3d const t            = dx * pitch_axis_ + dy * yaw_axis_;
    Eigen::Vector3d const displacement = translation_speed_ * t;
    detail::translate(*rotated_mesh_, displacement);
}

void trackball_rotation_adapter_t::set_pitch_axis(Eigen::Vector3d const& pitch_axis)
{
    pitch_axis_ = pitch_axis.normalized();
}

void trackball_rotation_adapter_t::set_yaw_axis(Eigen::Vector3d const& yaw_axis)
{
    yaw_axis_ = yaw_axis.normalized();
}

common::dynamic_surface_mesh* trackball_rotation_adapter_t::mesh() const
{
    return rotated_mesh_;
}

common::dynamic_surface_mesh* trackball_rotation_adapter_t::mesh()
{
    return rotated_mesh_;
}

} // namespace rendering
} // namespace sbs


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

  This source file is part of the Avogadro project.

  Copyright 2012-2014 Kitware, Inc.

  This source code is released under the New BSD License, (the "License").

  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 "camera.h"

#include <Eigen/LU>

#include <cmath>

namespace Avogadro {
namespace Rendering {

Camera::Camera()
  : m_width(0), m_height(0), m_projectionType(Perspective),
    m_orthographicScale(1.0), m_data(new EigenData)
{
  m_data->projection.setIdentity();
  m_data->modelView.setIdentity();
}

Camera::Camera(const Camera& o)
  : m_width(o.m_width), m_height(o.m_height),
    m_projectionType(o.m_projectionType),
    m_orthographicScale(o.m_orthographicScale), m_data(new EigenData(*o.m_data))
{}

Camera& Camera::operator=(const Camera& o)
{
  if (this != &o) {
    m_width = o.m_width;
    m_height = o.m_height;
    m_projectionType = o.m_projectionType;
    m_orthographicScale = o.m_orthographicScale;
    m_data = std::move(std::unique_ptr<EigenData>(new EigenData(*o.m_data)));
  }

  return *this;
}

Camera::~Camera() {}

void Camera::translate(const Vector3f& translate_)
{
  m_data->modelView.translate(translate_);
}

void Camera::preTranslate(const Vector3f& translate_)
{
  m_data->modelView.pretranslate(translate_);
}

void Camera::rotate(float angle, const Vector3f& axis)
{
  m_data->modelView.rotate(Eigen::AngleAxisf(angle, axis));
}

void Camera::preRotate(float angle, const Vector3f& axis)
{
  m_data->modelView.prerotate(Eigen::AngleAxisf(angle, axis));
}

void Camera::scale(float s)
{
  if (m_projectionType == Perspective)
    m_data->modelView.scale(s);
  else
    m_orthographicScale *= s;
}

void Camera::lookAt(const Vector3f& eye, const Vector3f& center,
                    const Vector3f& up)
{
  Vector3f f = (center - eye).normalized();
  Vector3f u = up.normalized();
  Vector3f s = f.cross(u).normalized();
  u = s.cross(f);

  m_data->modelView.setIdentity();
  m_data->modelView(0, 0) = s.x();
  m_data->modelView(0, 1) = s.y();
  m_data->modelView(0, 2) = s.z();
  m_data->modelView(1, 0) = u.x();
  m_data->modelView(1, 1) = u.y();
  m_data->modelView(1, 2) = u.z();
  m_data->modelView(2, 0) = -f.x();
  m_data->modelView(2, 1) = -f.y();
  m_data->modelView(2, 2) = -f.z();
  m_data->modelView(0, 3) = -s.dot(eye);
  m_data->modelView(1, 3) = -u.dot(eye);
  m_data->modelView(2, 3) = f.dot(eye);
}

float Camera::distance(const Vector3f& point) const
{
  return (m_data->modelView * point).norm();
}

Vector3f Camera::project(const Vector3f& point) const
{
  Eigen::Matrix4f mvp =
    m_data->projection.matrix() * m_data->modelView.matrix();
  Vector4f tPoint(point.x(), point.y(), point.z(), 1.0f);
  tPoint = mvp * tPoint;
  Vector3f result(
    static_cast<float>(m_width) * (tPoint.x() / tPoint.w() + 1.0f) / 2.0f,
    static_cast<float>(m_height) * (tPoint.y() / tPoint.w() + 1.0f) / 2.0f,
    (tPoint.z() / tPoint.w() + 1.0f) / 2.0f);
  return result;
}

Vector3f Camera::unProject(const Vector3f& point) const
{
  Eigen::Matrix4f mvp =
    m_data->projection.matrix() * m_data->modelView.matrix();
  Vector4f result(
    2.0f * point.x() / static_cast<float>(m_width) - 1.0f,
    2.0f * (static_cast<float>(m_height) - point.y()) /
        static_cast<float>(m_height) -
      1.0f,
    2.0f * point.z() - 1.0f, 1.0f);
  result = mvp.matrix().inverse() * result;
  return Vector3f(result.x() / result.w(), result.y() / result.w(),
                  result.z() / result.w());
}

Vector3f Camera::unProject(const Vector2f& point,
                           const Vector3f& reference) const
{
  return unProject(Vector3f(point.x(), point.y(), project(reference).z()));
}

void Camera::calculatePerspective(float fieldOfView, float aspectRatio,
                                  float zNear, float zFar)
{
  m_data->projection.setIdentity();
  float f = 1.0f / std::tan(fieldOfView * float(M_PI) / 360.0f);
  m_data->projection(0, 0) = f / aspectRatio;
  m_data->projection(1, 1) = f;
  m_data->projection(2, 2) = (zNear + zFar) / (zNear - zFar);
  m_data->projection(2, 3) = (2.0f * zFar * zNear) / (zNear - zFar);
  m_data->projection(3, 2) = -1;
  m_data->projection(3, 3) = 0;
}

void Camera::calculatePerspective(float fieldOfView, float zNear, float zFar)
{
  calculatePerspective(
    fieldOfView, static_cast<float>(m_width) / static_cast<float>(m_height),
    zNear, zFar);
}

void Camera::calculateOrthographic(float left, float right, float bottom,
                                   float top, float zNear, float zFar)
{
  left *= m_orthographicScale;
  right *= m_orthographicScale;
  bottom *= m_orthographicScale;
  top *= m_orthographicScale;
  m_data->projection.setIdentity();
  m_data->projection(0, 0) = 2.0f / (right - left);
  m_data->projection(0, 3) = -(right + left) / (right - left);
  m_data->projection(1, 1) = 2.0f / (top - bottom);
  m_data->projection(1, 3) = -(top + bottom) / (top - bottom);
  m_data->projection(2, 2) = -2.0f / (zFar - zNear);
  m_data->projection(2, 3) = -(zFar + zNear) / (zFar - zNear);
  m_data->projection(3, 3) = 1;
}

void Camera::setViewport(int w, int h)
{
  m_width = w;
  m_height = h;
}

void Camera::setProjection(const Eigen::Affine3f& transform)
{
  m_data->projection = transform;
}

void Camera::setModelView(const Eigen::Affine3f& transform)
{
  m_data->modelView = transform;
}

} // namespace Rendering
} // namespace Avogadro


//  (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)

// Test case for tail_mean.hpp

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/tail_mean.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>

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

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    double epsilon = 1;

    std::size_t n = 100000; // number of MC steps
    std::size_t c =  10000; // cache size

    typedef accumulator_set<double, stats<tag::non_coherent_tail_mean<right>, tag::tail_quantile<right> > > accumulator_t_right1;
    typedef accumulator_set<double, stats<tag::non_coherent_tail_mean<left>, tag::tail_quantile<left> > > accumulator_t_left1;
    typedef accumulator_set<double, stats<tag::coherent_tail_mean<right>, tag::tail_quantile<right> > > accumulator_t_right2;
    typedef accumulator_set<double, stats<tag::coherent_tail_mean<left>, tag::tail_quantile<left> > > accumulator_t_left2;

    accumulator_t_right1 acc0( right_tail_cache_size = c );
    accumulator_t_left1 acc1( left_tail_cache_size = c );
    accumulator_t_right2 acc2( right_tail_cache_size = c );
    accumulator_t_left2 acc3( left_tail_cache_size = c );

    // a random number generator
    boost::lagged_fibonacci607 rng;

    for (std::size_t i = 0; i < n; ++i)
    {
        double sample = rng();
        acc0(sample);
        acc1(sample);
        acc2(sample);
        acc3(sample);
    }

    // check uniform distribution
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc0, quantile_probability = 0.95), 0.975, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc0, quantile_probability = 0.975), 0.9875, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc0, quantile_probability = 0.99), 0.995, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc0, quantile_probability = 0.999), 0.9995, epsilon );
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc1, quantile_probability = 0.05), 0.025, 5*epsilon );
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc1, quantile_probability = 0.025), 0.0125, 6*epsilon );
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc1, quantile_probability = 0.01), 0.005, 8*epsilon );
    BOOST_CHECK_CLOSE( non_coherent_tail_mean(acc1, quantile_probability = 0.001), 0.0005, 25*epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc2, quantile_probability = 0.95), 0.975, epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc2, quantile_probability = 0.975), 0.9875, epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc2, quantile_probability = 0.99), 0.995, epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc2, quantile_probability = 0.999), 0.9995, epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc3, quantile_probability = 0.05), 0.025, 5*epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc3, quantile_probability = 0.025), 0.0125, 6*epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc3, quantile_probability = 0.01), 0.005, 8*epsilon );
    BOOST_CHECK_CLOSE( tail_mean(acc3, quantile_probability = 0.001), 0.0005, 25*epsilon );
}

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

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

    return test;
}


#include <boost/math/fft.hpp>
#include <boost/math/fft/bsl_backend.hpp>

#include <complex>
#include <vector>
#include <array>

void compile_test()
{
    // test same type of iterator
    std::vector<int> A(3);
    std::vector<double> B(A.size());

    // fails static_assert: D and A types are different
    boost::math::fft::dft(A.begin(),A.end(),B.begin());
}

int main()
{
    compile_test();
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_SCALAR_MNORMFRO_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_SCALAR_MNORMFRO_HPP_INCLUDED
#include <nt2/linalg/functions/mnormfro.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/ismatrix.hpp>
#include <nt2/include/functions/norm2.hpp>
#include <nt2/core/container/dsl.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/core/container/dsl/forward.hpp>
#include <boost/assert.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( mnormfro_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef typename meta::as_real<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return nt2::abs(a0);
    }

  };

  BOOST_DISPATCH_IMPLEMENT  ( mnormfro_, tag::cpu_
                            , (A0)
                            , ((ast_<A0, nt2::container::domain>))
                            )
  {
    typedef typename A0::value_type                   type_t;
    typedef typename meta::as_real<type_t>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG(nt2::ismatrix(a0), "a0 is not a matrix");
      return norm2(a0(nt2::_));
    }
  };
} }

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_fft
#include <boost/test/unit_test.hpp>
#include <complex>
#include <stdexcept>
#include <vector>
#include <iostream>
#include <string>
#include "../fft.hpp"

template <typename T>
std::vector<std::complex<T> > make_complex(const std::vector<T>& real_values,
                                           const std::vector<T>& imag_values) {
    auto n = real_values.size();
    if (imag_values.size() != n) throw std::runtime_error("Size mismatch");
    std::vector<std::complex<T> > res;
    res.reserve(n);
    for (size_t i = 0; i < n; i++) {
        res.push_back(std::complex<T>(real_values[i], imag_values[i]));
    }
    return res;
}

template <typename T>
void check_close_complex_vector(const std::vector<std::complex<T> >& v1,
                                const std::vector<std::complex<T> >& v2) {
    auto n = v1.size();
    if (v2.size() != n) throw std::runtime_error("Size mismatch");
    for (size_t i = 0; i < n; i++) {
        const T TOLERANCE = 1e-3;
        const T real_diff = std::abs(v1[i].real()-v2[i].real());
        const T imag_diff = std::abs(v1[i].imag()-v2[i].imag());

        if (real_diff >= TOLERANCE) {
            std::cout << "real failes at " << i << " : " << v1[i].real() << " vs. " << v2[i].real() << std::endl;
        }
        if (imag_diff >= TOLERANCE) {
            std::cout << "imag fails at " << i << " : " << v1[i].imag() << " vs. " << v2[i].imag() << std::endl;
        }

        BOOST_REQUIRE(real_diff < TOLERANCE);
        BOOST_REQUIRE(imag_diff < TOLERANCE);
    }
}

template <typename T>
void check_close_vector(const std::vector<T>& x, const std::vector<T>& y) {
    auto n = x.size();
    if (n != y.size()) throw std::runtime_error("Size mismatch");
    for (size_t i = 0; i < n; i++) {
        BOOST_CHECK_CLOSE(x[i], y[i], 0.01f);
    }
}

BOOST_AUTO_TEST_CASE(FFT_convolve1) {
    std::vector<float> x{1.0f, 2.0f, 3.0f};
    std::vector<float> h{1.0f};
    std::vector<float> desired_res{1.0f, 2.0f, 3.0f};

    auto res = fft_conv(x, h);
    BOOST_REQUIRE(res.size() == x.size());
    check_close_vector(res, desired_res);
}

BOOST_AUTO_TEST_CASE(FFT_convolve2) {
    std::vector<float> x{1.0f, 2.0f, 3.0f, -0.23f, 0.001f, 32.3f, 4.0f};
    std::vector<float> h{-0.33f, 0.9f, -0.002f, 1.1f, 2.3f};

    std::vector<float> desired_res{-0.33f, 0.24f, 0.808f, 3.8719f, 4.28667f,
        -2.75764f, 34.396998f, 3.0075f, 35.5243f, 78.69f, 9.2f};

    auto res = fft_conv(x, h);
    BOOST_REQUIRE(res.size() == (x.size() + h.size() - 1));
    check_close_vector(res, desired_res);
}

BOOST_AUTO_TEST_CASE(TestNextPowerOfTwo) {
    BOOST_CHECK_EQUAL(next_power_of_two(1), 1);
    BOOST_CHECK_EQUAL(next_power_of_two(2), 2);
    BOOST_CHECK_EQUAL(next_power_of_two(3), 4);
    BOOST_CHECK_EQUAL(next_power_of_two(4), 4);
    BOOST_CHECK_EQUAL(next_power_of_two(5), 8);
    BOOST_CHECK_EQUAL(next_power_of_two(7), 8);
    BOOST_CHECK_EQUAL(next_power_of_two(8), 8);
    BOOST_CHECK_EQUAL(next_power_of_two(15), 16);
    BOOST_CHECK_EQUAL(next_power_of_two(16), 16);
    BOOST_CHECK_EQUAL(next_power_of_two(32767), 32768);
    BOOST_CHECK_EQUAL(next_power_of_two(65535), 65536);
    BOOST_CHECK_EQUAL(next_power_of_two(65536), 65536);
}


//  (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)

// Test case for tail_variate_means.hpp

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/tail_variate_means.hpp>

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

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    std::size_t c = 5; // cache size

    typedef double variate_type;
    typedef std::vector<variate_type> variate_set_type;

    typedef accumulator_set<double, stats<tag::tail_variate_means<right, variate_set_type, tag::covariate1>(relative)> > accumulator_t1;
    typedef accumulator_set<double, stats<tag::tail_variate_means<right, variate_set_type, tag::covariate1>(absolute)> > accumulator_t2;
    typedef accumulator_set<double, stats<tag::tail_variate_means<left, variate_set_type, tag::covariate1>(relative)> > accumulator_t3;
    typedef accumulator_set<double, stats<tag::tail_variate_means<left, variate_set_type, tag::covariate1>(absolute)> > accumulator_t4;

    accumulator_t1 acc1( right_tail_cache_size = c );
    accumulator_t2 acc2( right_tail_cache_size = c );
    accumulator_t3 acc3( left_tail_cache_size = c );
    accumulator_t4 acc4( left_tail_cache_size = c );

    variate_set_type cov1, cov2, cov3, cov4, cov5;
    double c1[] = { 10., 20., 30., 40. }; // 100
    double c2[] = { 26.,  4., 17.,  3. }; // 50
    double c3[] = { 46., 64., 40., 50. }; // 200
    double c4[] = {  1.,  3., 70.,  6. }; // 80
    double c5[] = {  2.,  2.,  2., 14. }; // 20
    cov1.assign(c1, c1 + sizeof(c1)/sizeof(variate_type));
    cov2.assign(c2, c2 + sizeof(c2)/sizeof(variate_type));
    cov3.assign(c3, c3 + sizeof(c3)/sizeof(variate_type));
    cov4.assign(c4, c4 + sizeof(c4)/sizeof(variate_type));
    cov5.assign(c5, c5 + sizeof(c5)/sizeof(variate_type));

    acc1(100., covariate1 = cov1);
    acc1( 50., covariate1 = cov2);
    acc1(200., covariate1 = cov3);
    acc1( 80., covariate1 = cov4);
    acc1( 20., covariate1 = cov5);

    acc2(100., covariate1 = cov1);
    acc2( 50., covariate1 = cov2);
    acc2(200., covariate1 = cov3);
    acc2( 80., covariate1 = cov4);
    acc2( 20., covariate1 = cov5);

    acc3(100., covariate1 = cov1);
    acc3( 50., covariate1 = cov2);
    acc3(200., covariate1 = cov3);
    acc3( 80., covariate1 = cov4);
    acc3( 20., covariate1 = cov5);

    acc4(100., covariate1 = cov1);
    acc4( 50., covariate1 = cov2);
    acc4(200., covariate1 = cov3);
    acc4( 80., covariate1 = cov4);
    acc4( 20., covariate1 = cov5);

    // check relative risk contributions
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.7).begin()     ), 14./75. ); // (10 + 46) / 300 = 14/75
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.7).begin() + 1),  7./25. ); // (20 + 64) / 300 =  7/25
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.7).begin() + 2),  7./30. ); // (30 + 40) / 300 =  7/30
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.7).begin() + 3),  3./10. ); // (40 + 50) / 300 =  3/10
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.3).begin()    ), 14./35. ); // (26 +  2) /  70 = 14/35
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.3).begin() + 1),  3./35. ); // ( 4 +  2) /  70 =  3/35
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.3).begin() + 2), 19./70. ); // (17 +  2) /  70 = 19/70
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.3).begin() + 3), 17./70. ); // ( 3 + 14) /  70 = 17/70

    // check absolute risk contributions
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.7).begin()    ), 28 ); // (10 + 46) / 2 = 28
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.7).begin() + 1), 42 ); // (20 + 64) / 2 = 42
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.7).begin() + 2), 35 ); // (30 + 40) / 2 = 35
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.7).begin() + 3), 45 ); // (40 + 50) / 2 = 45
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.3).begin()    ), 14 ); // (26 +  2) / 2 = 14
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.3).begin() + 1),  3 ); // ( 4 +  2) / 2 =  3
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.3).begin() + 2),9.5 ); // (17 +  2) / 2 =  9.5
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.3).begin() + 3),8.5 ); // ( 3 + 14) / 2 =  8.5

    // check relative risk contributions
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.9).begin()    ), 23./100. ); // 46/200 = 23/100
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.9).begin() + 1),  8./25.  ); // 64/200 =  8/25
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.9).begin() + 2),  1./5.   ); // 40/200 =  1/5
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc1, quantile_probability = 0.9).begin() + 3),  1./4.   ); // 50/200 =  1/4
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.1).begin()    ),  1./10.  ); //  2/ 20 =  1/10
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.1).begin() + 1),  1./10.  ); //  2/ 20 =  1/10
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.1).begin() + 2),  1./10.  ); //  2/ 20 =  1/10
    BOOST_CHECK_EQUAL( *(relative_tail_variate_means(acc3, quantile_probability = 0.1).begin() + 3),  7./10.  ); // 14/ 20 =  7/10

    // check absolute risk contributions
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.9).begin()    ), 46 ); // 46
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.9).begin() + 1), 64 ); // 64
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.9).begin() + 2), 40 ); // 40
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc2, quantile_probability = 0.9).begin() + 3), 50 ); // 50
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.1).begin()    ),  2 ); //  2
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.1).begin() + 1),  2 ); //  2
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.1).begin() + 2),  2 ); //  2
    BOOST_CHECK_EQUAL( *(tail_variate_means(acc4, quantile_probability = 0.1).begin() + 3), 14 ); // 14
}

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

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

    return test;
}


#include <iostream>
#include <iomanip>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

#include <pangolin/pangolin.h>

struct RotationMatrix {
  Matrix3d matrix = Matrix3d::Identity();
};

ostream &operator<<(ostream &out, const RotationMatrix &r) {
  out.setf(ios::fixed);
  Matrix3d matrix = r.matrix;
  out << '=';
  out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
      << "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
      << "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
  return out;
}

istream &operator>>(istream &in, RotationMatrix &r) {
  return in;
}

struct TranslationVector {
  Vector3d trans = Vector3d(0, 0, 0);
};

ostream &operator<<(ostream &out, const TranslationVector &t) {
  out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";
  return out;
}

istream &operator>>(istream &in, TranslationVector &t) {
  return in;
}

struct QuaternionDraw {
  Quaterniond q;
};

ostream &operator<<(ostream &out, const QuaternionDraw quat) {
  auto c = quat.q.coeffs();
  out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
  return out;
}

istream &operator>>(istream &in, const QuaternionDraw quat) {
  return in;
}

int main(int argc, char **argv) {
  pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);
  glEnable(GL_DEPTH_TEST);
  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
    pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY)
  );

  const int UI_WIDTH = 500;

  pangolin::View &d_cam = pangolin::CreateDisplay().
    SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).
    SetHandler(new pangolin::Handler3D(s_cam));

  // ui
  pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
  pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
  pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
  pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
  pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

  while (!pangolin::ShouldQuit()) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);

    pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
    Matrix<double, 4, 4> m = matrix;

    RotationMatrix R;
    for (int i = 0; i < 3; i++)
      for (int j = 0; j < 3; j++)
        R.matrix(i, j) = m(j, i);
    rotation_matrix = R;

    TranslationVector t;
    t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));
    t.trans = -R.matrix * t.trans;
    translation_vector = t;

    TranslationVector euler;
    euler.trans = R.matrix.eulerAngles(2, 1, 0);
    euler_angles = euler;

    QuaternionDraw quat;
    quat.q = Quaterniond(R.matrix);
    quaternion = quat;

    glColor3f(1.0, 1.0, 1.0);

    pangolin::glDrawColouredCube();
    // draw the original axis
    glLineWidth(3);
    glColor3f(0.8f, 0.f, 0.f);
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(10, 0, 0);
    glColor3f(0.f, 0.8f, 0.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 10, 0);
    glColor3f(0.2f, 0.2f, 1.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 10);
    glEnd();

    pangolin::FinishFrame();
  }
}