//---------------------------------------------------------------------------//
// Copyright (c) 2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/find_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/zip_iterator.hpp>

namespace compute = boost::compute;

// this example shows how to use the find_if() algorithm to detect the
// point at which two vectors of prices (such as stock prices) cross.
int main()
{
    // get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);

    // prices #1 (from 10.0 to 11.0)
    std::vector<float> prices1;
    for(float i = 10.0; i <= 11.0; i += 0.1){
        prices1.push_back(i);
    }

    // prices #2 (from 11.0 to 10.0)
    std::vector<float> prices2;
    for(float i = 11.0; i >= 10.0; i -= 0.1){
        prices2.push_back(i);
    }

    // create gpu vectors
    compute::vector<float> gpu_prices1(prices1.size(), context);
    compute::vector<float> gpu_prices2(prices2.size(), context);

    // copy prices to gpu
    compute::copy(prices1.begin(), prices1.end(), gpu_prices1.begin(), queue);
    compute::copy(prices2.begin(), prices2.end(), gpu_prices2.begin(), queue);

    // function returning true if the second price is less than the first price
    BOOST_COMPUTE_FUNCTION(bool, check_price_cross, (boost::tuple<float, float> prices),
    {
        // first price
        const float first = boost_tuple_get(prices, 0);

        // second price
        const float second = boost_tuple_get(prices, 1);

        // return true if second price is less than first
        return second < first;
    });

    // find cross point (should be 10.5)
    compute::vector<float>::iterator iter = boost::get<0>(
        compute::find_if(
            compute::make_zip_iterator(
                boost::make_tuple(gpu_prices1.begin(), gpu_prices2.begin())
            ),
            compute::make_zip_iterator(
                boost::make_tuple(gpu_prices1.end(), gpu_prices2.end())
            ),
            check_price_cross,
            queue
        ).get_iterator_tuple()
    );

    // print out result
    int index = std::distance(gpu_prices1.begin(), iter);
    std::cout << "price cross at index: " << index << std::endl;

    float value;
    compute::copy_n(iter, 1, &value, queue);
    std::cout << "value: " << value << std::endl;

    return 0;
}


<% render(instance) %>
<%= includes((0..x).size) %>

#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/foldable/foldable.hpp>
#include <boost/hana/integral.hpp>

BOOST_HANA_CONSTEXPR_LAMBDA auto is_odd = [](auto x) {
    return x % boost::hana::int_<2> != boost::hana::int_<0>;
};

int main() {
    auto go = boost::hana::count(
        <%= list(
            (0..x).map { |i| "decltype(boost::hana::int_<#{i}>)" },
            (0..x).map { |i| "boost::hana::int_<#{i}>" }
        ) %>,
        is_odd
    );
}


#pragma once
#include <limits>
#include <Eigen/Dense>
#include "Object.hpp"
#include "../Intersect.hpp"
#include "../Ray.hpp"

class SdfObject : public Object
{
public:
    SdfObject(Eigen::Vector3f position) : Object(position){};

    Intersect intersect_ray(const Ray &ray) const override
    {
        constexpr auto infinity = std::numeric_limits<float>::infinity();
        return intersect_ray(ray, 0.0f, infinity);
    };

    // Ray Matching Algorithm
    Intersect intersect_ray(const Ray &ray, const float &tMin, const float &tMax) const override
    {
        constexpr auto max_try_matching = 256;

        auto t = tMin;
        for (int i = 0; i < max_try_matching && ray.is_valid(t) && t < tMax; i++)
        {
            auto dis = sdf(ray(t));
            if (dis < std::numeric_limits<float>::epsilon())
                return Intersect(t, get_color_at(0, 0));

            t += dis;
        }

        return Intersect();
    };

    // Signed Distance Function
    virtual float sdf(const Eigen::Vector3f &position) const = 0;
};


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2014 by Synge Todo <wistaria@comp-phys.org>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

// C++ version of TITPACK Ver.2 by H. Nishimori

/************* Sample main program #1 *****************
* 1d Heisenberg antiferromagnet with 16 spins
* Eigenvalues and an eigenvector / lnc1, lncv1
* Precision check and correlation functions
******************************************************/

#include <mpi.h>
#include <iostream>
#include <rokko/rokko.hpp>
#include <boost/timer.hpp>
#include "titpack.hpp"
#include "options.hpp"

typedef rokko::parallel_sparse_solver solver_type;

int main(int argc, char** argv) {
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
  rokko::grid_1d g;
  if (g.get_nprocs() > 1) {
    std::cerr << "currently works only for Np = 1\n";
  }

  std::cout.precision(10);
  options opt(argc, argv, 16, solver_type::default_solver(), g.get_myrank() == 0);
  if (!opt.valid) MPI_Abort(MPI_COMM_WORLD, 1);
  boost::timer tm;
  MPI_Barrier(g.get_comm());
  double t1 = tm.elapsed();

  // lattice structure
  int n = opt.N;
  int ibond = n;
  std::vector<int> ipair;
  for (int i = 0; i < ibond; ++i) {
    ipair.push_back(i);
    ipair.push_back((i + 1) % n);
  }

  // Hamiltonian parameters
  std::vector<double> bondwt(ibond, -1);
  std::vector<double> zrtio(ibond, 1);

  // table of configurations and Hamiltonian operator
  subspace ss(n, 0);
  hamiltonian hop(ss, ipair, bondwt, zrtio);
  solver_type solver(opt.solver);
  solver.initialize(argc, argv);
  MPI_Barrier(g.get_comm());
  double t2 = tm.elapsed();

  // Eigenvalues
  int nev = 10;
  int blockSize = 5;
  int maxIters = 500;
  double tol = 1.0e-8;
  solver.diagonalize(hop, nev, blockSize, maxIters, tol);
  double t3 = tm.elapsed();

  if (g.get_myrank() == 0) {
    std::cout << "[Number of converged eigenpairs]\n\t" << solver.num_conv() << std::endl;
    // std::cout << "[Iteration number]\n\t" << itr << std::endl;
    std::cout << "[Eigenvalues]\n";
    for (int i = 0; i < solver.num_conv(); ++i) std::cout << '\t' << solver.eigenvalue(i);
    std::cout << std::endl;
  }

  // Ground-state eigenvector
  rokko::distributed_vector eigvec;
  solver.eigenvector(0, eigvec);
  if (g.get_myrank() == 0) std::cout << "[Eigenvector components (selected)]";
  std::cout << std::flush;
  MPI_Barrier(g.get_comm());
  int count = 0;
  for (int i = 12; i < ss.dimension(); i += ss.dimension()/20, ++count) {
    if (eigvec.is_gindex(i)) {
      if (count % 4 == 0) std::cout << std::endl;
      std::cout << '\t' << eigvec.get_global(i);
    }
    std::cout << std::flush;
    MPI_Barrier(g.get_comm());
  }
  if (g.get_myrank() == 0) std::cout << std::endl;
  std::cout << std::flush;
  MPI_Barrier(g.get_comm());

  // Precision check and correlation functions
  // double Hexpec = check2(mat, x, 0, v, 0);

  // std::vector<int> npair;
  // npair.push_back(1);
  // npair.push_back(2);
  // std::vector<double> sxx(1), szz(1);
  // xcorr(ss, npair, x, 0, sxx);
  // zcorr(ss, npair, x, 0, szz);
  // std::cout << "[Nearest neighbor correlation functions]\n\t"
  //           << "sxx : " << sxx[0]
  //           << ", szz : " << szz[0] << std::endl;

  if (g.get_myrank() == 0) {
    std::cerr << "initialize      " << (t2-t1) << " sec\n"
              << "diagonalization " << (t3-t2) << " sec\n";
    // << "check           " << (t4-t3) << " sec\n"
    // << "correlation     " << (t5-t4) << " sec\n";
  }

  MPI_Finalize();
}


#include <iostream>
#include <GLFW/glfw3.h>
#include <chrono>
#include <algorithm>
#include <memory>

#include "Camera.h"
#include <Eigen/Dense>

using namespace Eigen;
using namespace RayMarching;

int main() {
    // Prepare the Scene and the Camera
    const auto loc = Vector3d(10, -10, 0);
    const auto dir = Vector3d(0, 0, 0);
    const auto up = Vector3d(0, 0, 3);
    Settings_t settings = Settings_t();
    // Set custom settings
    settings.interlace = false;
    settings.width = 720;
    settings.height = 480;
    settings.maxDistance = 8;
    Camera camera = Camera(loc, dir, up, settings);
    std::shared_ptr<Scene> scene = camera.getScene();

    // Create the OpenGL window with the same shape
    glfwInit();
    GLFWwindow* window = glfwCreateWindow(settings.width, settings.height, "RayMarching", nullptr, nullptr);
    if (window == nullptr) {
        std::cout << "Could not open window";
        glfwTerminate();
        exit(EXIT_FAILURE);
    }

    // Create shapes and push them into the scene
    std::unique_ptr<SDFObject> q( new Cube(Vector3d(0, -1, 0), 1, RED));
    std::unique_ptr<SDFObject> s(new Sphere(Vector3d(0, -1, 0), 1.4, BLUE));
    std::unique_ptr<SDFObject> u(new SDFCombination(std::move(q), std::move(s), SDFCombination::SDFOperation::INTERSECTION));
    TransformataObject trans(std::move(u));
    scene->pushShape(&trans);

    Sphere sp(Vector3d(0, 2, 0), 1, GREEN);
    scene->pushShape(&sp);

    // Basic benchmark before the launch
    std::cout << "Context prepared" << std::endl;
    constexpr unsigned int TRIALS = 16;
    unsigned int times[TRIALS];
    pixel* buffer;

    for (unsigned int & time : times) {
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
        camera.takePhoto();
        std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
        time = std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count();
    }
    std::cout << "RENDER STATISTICS FOR " << TRIALS << " TRIALS" << std::endl;
    std::cout << "Min time (ms) " << *std::min_element(times, times+TRIALS) << std::endl;
    std::cout << "Max time (ms) " << *std::max_element(times, times+TRIALS) << std::endl;
    unsigned int total = 0;
    for (auto && time : times) {
        total += time;
    }
    std::cout << "Avg time (ms) " << total / TRIALS << std::endl;
    std::cout << "Avg FPS " << (1000.0f * TRIALS) / total << std::endl;

    // Prepare the OpenGL window
    glfwMakeContextCurrent(window);
    glViewport(0, 0, settings.width, settings.height);

    double angle = 0;
    while (!glfwWindowShouldClose(window)) {
        // Render and draw onto window
        buffer = camera.takePhoto();
        glDrawPixels(settings.width, settings.height, GL_RGB, GL_UNSIGNED_BYTE, buffer);
        glfwPollEvents();
        glfwSwapBuffers(window);

        // Move the camera and shapes
        camera.setCamera(Vector3d(4 * cos(angle), 4 * sin(angle), 0),
                         Vector3d(0, 0, 0),
                         Vector3d(0, 0, 3));
        angle += 0.03;
        trans.rotate(2, 2, 2);
    }

    glfwDestroyWindow(window);
    glfwTerminate();
    exit(EXIT_SUCCESS);
}


#include <omp.h>
#include <iostream>
#include <string>
#include <sstream>
#include <cmath>
#include <cstring>
#include <sys/time.h>
#include <boost/random.hpp>
#include <boost/program_options.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/random/random_device.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#define N 10000000

using namespace boost::random;
using namespace boost::program_options;

int* seq_nums = new int[N];
int* par_nums = new int[N];

int random_gen(){
	boost::random::random_device seeder;
	boost::random::mt19937 gen(seeder());
	boost::random::uniform_int_distribution<> dist(1,10000);

	int rand_num = dist(gen);
	return rand_num;
}

void init(){
	#pragma omp parallel for shared(seq_nums)
	for(int i = 0; i< N; i++){
		seq_nums[i] = random_gen();
	}
	std::cout << std::endl << "Finished gen data" << std::endl;
	memcpy(par_nums, seq_nums, sizeof(int)*N);
}

void swap(int *a, int *b){
	int temp = *a;
	*a = *b;
	*b = temp;
}

int partition(int *nums, int low, int high){
	int i = low;
	int j = high;
	int pivot = nums[low];
	while( i < j){
		if(nums[j] < pivot){
			swap(&nums[i], &nums[j]);
			i++;

		}
		else if(nums[i] > pivot){
			swap(&nums[i], &nums[j]);
			j--;
		}
		else{
			i++;
		}
	}
	nums[i] = pivot;
	return i;

}

void seq_qsort(int *nums, int low, int high){
	if(low < high){
		int pivot_idx = partition(nums, low, high);

		seq_qsort(nums, low, pivot_idx-1 );
		seq_qsort(nums, pivot_idx+1, high );
	}
}

void seq_qsort_wrap(){
	int count;
	int i;
	int n = N;

	seq_qsort(seq_nums, 0, n-1);

	std::cout << std::endl;
}

void par_qsort(int *nums, int low, int high){
	if(low < high){
		int pivot_idx;
		pivot_idx = partition(nums, low, high);

		#pragma omp task
		par_qsort(nums, low, pivot_idx-1);
		#pragma omp task
		par_qsort(nums, pivot_idx+1, high);

	}
}

void par_qsort_wrap(){
	int count;
	int i;
	int n = N;
	#pragma omp parallel default(none) shared(par_nums, n)
	{
		#pragma omp single nowait
		{
			par_qsort(par_nums, 0, n-1);
		}
	}

}

std::string time_cal(void (*func)()){

	struct timespec start,end;

	clock_gettime(CLOCK_REALTIME, &start);
	(*func)();
	clock_gettime(CLOCK_REALTIME, &end);
	std::stringstream ss;
	ss <<  end.tv_sec - start.tv_sec;
	ss <<  " ";
	ss <<  end.tv_nsec - start.tv_nsec;

	return ss.str();
}

void print_nums(){
	for(int i = 0; i< N; i++){
		std::cout << seq_nums[i] << " ";
	}
	std::cout << std::endl;

	for(int i = 0; i< N; i++){
		std::cout << par_nums[i] << " ";
	}
	std::cout << std::endl;
}

bool check_same(){
	for(int i=0;i <N; i++){
		if(seq_nums[i] != par_nums[i]){
			return false;
		}
	}
	return true;
}

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

	init();
	auto seq_time = time_cal(seq_qsort_wrap);
	auto par_time = time_cal(par_qsort_wrap);
	std::cout << "Same:" << check_same() << std::endl;

	std::cout << seq_time << std::endl;
	std::cout << par_time << std::endl;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, 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.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 */

// @sect3{Include files}

// 库中最基本的类是Triangulation类，它在这里声明。

#include <deal.II/grid/tria.h>

// 这里有一些生成标准网格的函数。

#include <deal.II/grid/grid_generator.h>

// 输出各种图形格式的网格。

#include <deal.II/grid/grid_out.h>

// 这对于C++输出来说是需要的。

#include <iostream>
#include <fstream>

// 这是对 `std::sqrt` 和 `std::fabs` 函数声明的说明。

#include <cmath>

//导入deal.II的最后一步是这样的。所有deal.II的函数和类都在一个命名空间 dealii 中，以确保它们不会与你可能想和deal.II一起使用的其他库的符号发生冲突。我们可以在使用这些函数和类时，在每个名字前加上 dealii:: 的前缀，但这很快就会变得繁琐和令人厌烦。相反，我们只是简单地导入整个deal.II的名字空间，以供一般使用。

using namespace dealii;
// @sect3{Creating the first mesh}

// 在下面的第一个函数中，我们简单地使用单位方格作为域，并从中产生一个全局细化网格。

void first_grid()
{

// 首先要做的是为二维域的三角化定义一个对象。

  Triangulation<2> triangulation;

// 在这里和下面的许多情况下，类名后面的字符串"<2>"表示这是一个在两个空间维度上工作的对象。同样，也有一些三角形类的版本是在一个（"<1>"）和三个（"<3>"）空间维度上工作的。这种工作方式是通过一些模板魔法实现的，我们将在后面的示例程序中详细研究；在那里，我们也将看到如何以一种基本独立于维度的方式编写程序。

// 接下来，我们要用一个正方形领域的单个单元来填充三角结构。三角形被细化了四次，总共得到 $4^4=256$ 个单元。

  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(4);

// 现在我们要将网格的图形表示写到输出文件中。deal.II的GridOut类可以用多种不同的输出格式来实现；在这里，我们选择可扩展矢量图（SVG）格式，你可以用你选择的网络浏览器来进行可视化。

  std::ofstream out("grid-1.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-1.svg" << std::endl;
}

//  @sect3{Creating the second mesh}

// 下面第二个函数中的网格略微复杂一些，因为我们使用了一个环形域，并对结果进行了一次全局细化。

void second_grid()
{

// 我们再次开始定义一个二维域的三角化对象。

  Triangulation<2> triangulation;

// 然后我们用一个环形域来填充它。环的中心应是点(1,0)，内半径和外半径应是0.5和1。圆周单元的数量可以由这个函数自动调整，但我们选择在最后一个参数中明确设置为10。

  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);

// 默认情况下，三角测量假定所有边界都是直线，所有单元都是双线性四边形或三线性六边形，并且它们是由粗略网格（我们刚刚创建的）的单元定义的。除非我们做一些特别的事情，否则当需要引入新的点时，域被假定为由粗网格的直线划定，而新的点将简单地位于周围的中间。然而，在这里，我们知道领域是弯曲的，我们想让三角法根据底层的几何形状来放置新的点。幸运的是，一些优秀的灵魂实现了一个描述球状域的对象，而环是球状域的一个部分；它只需要环的中心，并自动计算出如何指示三角计算在哪里放置新的点。这在deal.II中的工作方式是，你用一个通常被称为 "流形指标 "的数字来标记你想要弯曲的三角形部分，然后告诉三角形在所有有这个流形指标的地方使用一个特定的 "流形对象"。具体如何操作在此并不重要（你可以在 step-53 和 @ref manifold 中阅读）。GridGenerator中的函数在大多数情况下为我们处理这个问题：它们将正确的流形附加到一个域上，这样当三角形被细化时，新的单元就会被放置在正确的位置上。在目前的情况下， GridGenerator::hyper_shell 为所有的单元格附加了一个球形流形：这将导致单元格在球面坐标的计算下被细化（因此新的单元格的边缘要么是径向的，要么是位于原点周围的同心圆）。

// 默认情况下（即对于手工创建的三角图或未调用GridGenerator函数（如 GridGenerator::hyper_shell 或 GridGenerator::hyper_ball), ），三角图的所有单元格和面都将其manifold_id设置为 numbers::flat_manifold_id, ，如果您想要一个产生直线边缘的流形，这是默认的，但您可以为个别单元格和面改变这个数字。在这种情况下，因此与数字0相关的曲面流形将不适用于那些流形指标为非零的部分，但其他流形描述对象可以与这些非零指标相关联。如果没有流形描述与特定的流形指标相关联，则暗示产生直角边缘的流形。(流形指标是一个略微复杂的话题；如果你对这里到底发生了什么感到困惑，你可能想看看 @ref GlossManifoldIndicator "关于这个话题的词汇表条目")。既然 GridGenerator::hyper_shell 选择的默认值是合理的，我们就不去管它。

// 为了演示如何在所有单元格上写一个循环，我们将分五个步骤向域的内圈细化网格。

  for (unsigned int step = 0; step < 5; ++step)
    {

// 接下来，我们需要对三角形的活动单元进行循环。你可以把三角形看作一个单元格的集合。如果它是一个数组，你只需要得到一个指针，用操作符`++`从一个元素递增到下一个元素。三角形的单元不是作为一个简单的数组来存储的，但是iterator的概念将指针的工作方式概括为任意的对象集合（更多信息见wikipedia）。通常情况下，C++中的任何容器类型都会返回一个迭代器，指向集合的开始，方法称为`begin'，而迭代器则指向集合结束后的1，方法称为`end'。我们可以用操作符`++it`来增加一个迭代器`it`，用`*it`来解除引用以获得底层数据，并通过比较`it != collection.end()`来检查我们是否完成。

// 第二个重要的部分是我们只需要活动单元。活动单元是那些没有被进一步细化的单元，也是唯一可以被标记为进一步细化的单元。deal.II提供了迭代器类别，允许我们在all单元（包括活动单元的父单元）或只在活动单元上迭代。因为我们要的是后者，所以我们需要调用方法 Triangulation::active_cell_iterators().  。

//把所有这些放在一起，我们可以用
// @code{.cpp}
//      for (auto it = triangulation.active_cell_iterators().begin();
//           it != triangulation.active_cell_iterators().end();
//           ++it)
//        {
//          auto cell = *it;
//  //Then a miracle occurs...
//        }
//  @endcode
//  在一个三角形的所有活动单元上循环。 在这个循环的初始化器中，我们使用了`auto`关键字作为迭代器`it`的类型。`auto`关键字意味着被声明的对象的类型将从上下文中推断出来。当实际的类型名称很长，甚至可能是多余的时候，这个关键字很有用。如果你不确定类型是什么，想查一下结果支持什么操作，你可以去看方法的文档  Triangulation::active_cell_iterators().  在这个例子中，`it`的类型是  `Triangulation::active_cell_iterator`.

// 虽然`auto`关键字可以让我们不用输入长长的数据类型名称，但我们仍然要输入大量冗余的关于开始和结束迭代器以及如何递增的声明。与其这样，我们不如使用range-based for loops，它将上面显示的所有语法包成一个更短的形式。

      for (auto &cell : triangulation.active_cell_iterators())
        {
// @note  关于deal.II中使用的迭代器类的更多信息，见 @ref Iterators ，关于基于范围的for循环和`auto`关键字的更多信息，见 @ref CPP11 。

// 接下来，我们在单元格的所有顶点上循环。为此，我们查询一个顶点索引的迭代器（在2D中，这是一个包含元素`{0,1,2,3}`的数组，但是由于`cell->vertex_indices()`知道单元格所处的维度，因此返回的数组在所有维度上都是正确的，这使得无论我们在2D还是3D中运行这段代码都是正确的，也就是说，它实现了 "维度无关的编程" - 我们将在  step-4  中讨论一个重要部分）。

          for (const auto v : cell->vertex_indices())
            {

// 如果这个单元格位于内边界，那么它至少有一个顶点必须位于内环上，因此与中心的径向距离正好是0.5，达到浮点精度。所以我们计算这个距离，如果我们发现一个顶点具有这个属性，我们就标记这个单元，以便以后进行细化。然后我们也可以打破所有顶点的循环，转到下一个单元。        因为离中心的距离是以浮点数计算的，所以我们必须期望我们所计算的东西只能精确到[round-off](https:en.wikipedia.org/wiki/Round-off_error)以内。因此，我们永远不能指望通过平等的方式来比较距离和内半径。诸如 "if (distance_from_center == inner_radius) "这样的语句将会失败，除非我们运气特别好。相反，我们需要以一定的容忍度进行比较，通常的方法是写成`if  (std::abs(distance_from_center  。

// - inner_radius) <= tolerance)`，其中`tolerance'是比四舍五入大的某个小数字。问题是如何选择它。我们可以直接选择，比如说，`1e-10'，但这只适合于我们比较的对象是大小为1的情况。如果我们创建了一个单元大小为`1e+10'的网格，那么`1e-10'将远远低于四舍五入，就像以前一样，只有在我们特别幸运的情况下，比较才会成功。相反，使公差*相对于被比较对象的典型 "比例 "几乎总是有用的。在这里，"尺度 "是指内半径，或者是细胞的直径。我们选择前者，并将公差设置为 $10^{-6}$ 倍环形物的内半径。

              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

// 现在我们已经标记了所有我们想要细化的单元格，我们让三角化实际做这个细化。这样做的函数的名字很长，因为我们也可以标记单元格进行粗化，该函数一次完成粗化和细化。

      triangulation.execute_coarsening_and_refinement();
    }

// 最后，在这五次细化迭代之后，我们要再次将得到的网格写入文件，同样是SVG格式。这和上面的工作一样。

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-2.svg" << std::endl;
}

//  @sect3{The main function}

// 最后是主函数。这里没有什么可做的，只是调用两个子函数，产生两个网格。

int main()
{
  first_grid();
  second_grid();
}


#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/ZLineShape.h"
#include "PhysicsTools/Utilities/interface/Gaussian.h"
#include "PhysicsTools/Utilities/interface/Numerical.h"
#include "PhysicsTools/Utilities/interface/Exponential.h"
#include "PhysicsTools/Utilities/interface/Polynomial.h"
#include "PhysicsTools/Utilities/interface/Constant.h"
#include "PhysicsTools/Utilities/interface/Convolution.h"
#include "PhysicsTools/Utilities/interface/Operations.h"
#include "PhysicsTools/Utilities/interface/Integral.h"
#include "PhysicsTools/Utilities/interface/MultiHistoChiSquare.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/RootMinuitCommands.h"
#include "PhysicsTools/Utilities/interface/FunctClone.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "TROOT.h"
#include "TH1.h"
#include "TFile.h"
#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include <iostream>
#include <algorithm>
#include <exception>
#include <iterator>
#include <string>
#include <vector>
using namespace std;

// A helper function to simplify the main part.
template<class T>
ostream& operator<<(ostream& os, const vector<T>& v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

//A function that sets istogram contents to 0
//if they are too small
void fix(TH1* histo) {
  for(int i = 1; i <= histo->GetNbinsX(); ++i) {
    if(histo->GetBinContent(i) < 0.1) {
      histo->SetBinContent(i, 0.0);
      histo->SetBinError(i, 0.0);
    }
  }
}

typedef funct::GaussIntegrator IntegratorConv;
typedef funct::GaussIntegrator IntegratorNorm;
//typedef funct::TrapezoidIntegrator IntegratorConv;
//typedef funct::TrapezoidIntegrator IntegratorNorm;

typedef funct::Product<funct::Exponential,
		       funct::Convolution<funct::ZLineShape, funct::Gaussian, IntegratorConv>::type>::type ZPeakNoNorm;

NUMERICAL_FUNCT_INTEGRAL(ZPeakNoNorm, IntegratorNorm);

typedef funct::DefIntegral<ZPeakNoNorm, funct::Constant, funct::Constant, IntegratorNorm> ZPeakNormFactor;
typedef funct::Ratio<ZPeakNoNorm, ZPeakNormFactor>::type ZPeak;

int main(int ac, char *av[]) {
  gROOT->SetStyle("Plain");
  try {
    typedef funct::Power<funct::Parameter, funct::Numerical<2> >::type IsoefficiencytermSQ;
    typedef funct::Master<funct::Product<funct::Parameter, ZPeak>::type> ZMuMuFun;
    typedef funct::Slave<funct::Product<funct::Parameter, ZPeak>::type> ZMuMuFunClone;
    typedef funct::Product<funct::Product<funct::Power<funct::Parameter, funct::Numerical<2> >::type,
                                          funct::Power<funct::Parameter, funct::Numerical<2> >::type >::type,
                           IsoefficiencytermSQ >::type  ZMuMuEfficiencyTerm;
    typedef funct::Product<ZMuMuEfficiencyTerm, ZMuMuFun>::type ZMuMuSig;

    typedef funct::Product<funct::Product<funct::Power<funct::Parameter, funct::Numerical<2> >::type,
                                          funct::Power<funct::Parameter, funct::Numerical<2> >::type >::type,
                          funct::Difference<funct::Numerical<1>, IsoefficiencytermSQ >::type>::type  ZMuMuNoIsoEfficiencyTerm;

    typedef funct::Product<ZMuMuNoIsoEfficiencyTerm, ZMuMuFunClone>::type ZMuMuNoIsoSig;

    typedef funct::Product<funct::Product<funct::Numerical<2>,
                                          funct::Product<funct::Power<funct::Parameter, funct::Numerical<2> >::type,
                                                         funct::Product<funct::Parameter,
                                                                        funct::Difference<funct::Numerical<1>, funct::Parameter>::type
                                                                       >::type
                                                        >::type
                           >::type,  IsoefficiencytermSQ >::type  ZMuTkEfficiencyTerm;

    typedef funct::Product<ZMuTkEfficiencyTerm, ZMuMuFunClone>::type ZMuTkSig;
    typedef funct::Product<funct::Parameter,
                           funct::Product<funct::Exponential, funct::Polynomial<2> >::type >::type ZMuTkBkg;
    typedef funct::Product<funct::Constant,ZMuTkBkg>::type ZMuTkBkgScaled;//bgtrack rescaled
    typedef ZMuTkBkg ZMuMuNoIsoBkg;
    typedef ZMuTkBkgScaled  ZMuMuNoIsoBkgScaled ;//bgZmmNotIso rescaled
    typedef ZMuTkEfficiencyTerm ZMuSaEfficiencyTerm;
    typedef funct::Product<ZMuSaEfficiencyTerm,
                           funct::Product<funct::Parameter, funct::Gaussian>::type>::type ZMuSaSig;
    typedef funct::Product<funct::Parameter, funct::Exponential>::type ZMuSaBkg;

    // typedef ZMuTkBkg ZMuMuNoIsoBkg;
    typedef funct::Product<funct::Constant, funct::Sum<ZMuMuNoIsoSig, ZMuMuNoIsoBkg>::type>::type ZMuMuNoIso;//3
    typedef funct::Product<funct::Constant, ZMuMuSig>::type ZMuMu;
    typedef funct::Product<funct::Constant, funct::Sum<ZMuTkSig, ZMuTkBkg>::type>::type ZMuTk;
    typedef funct::Product<funct::Constant, funct::Sum<ZMuSaSig, ZMuSaBkg>::type>::type ZMuSa;
    typedef fit::MultiHistoChiSquare<ZMuMu, ZMuTk, ZMuSa, ZMuMuNoIso> ChiSquared;

    double fMin, fMax;
    string ext;
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("input-file,i", po::value< vector<string> >(), "input file")
      ("min,m", po::value<double>(&fMin)->default_value(60), "minimum value for fit range")
      ("max,M", po::value<double>(&fMax)->default_value(120), "maximum value for fit range")
      ("plot-format,p", po::value<string>(&ext)->default_value("ps"),
       "output plot format")
      ;

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

    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
	    options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
      }

    fit::RootMinuitCommands<ChiSquared> commands("csa08ZFit.txt");

    const int rebinMuMuNoIso = 2,rebinMuMu = 1, rebinMuTk = 2, rebinMuSa = 8;
    // assume that the bin size is 1 GeV!!!
    funct::Constant rebinMuMuNoIsoConst(rebinMuMuNoIso),rebinMuMuConst(rebinMuMu), rebinMuTkConst(rebinMuTk), rebinMuSaConst(rebinMuSa);

    if (vm.count("input-file")) {
      cout << "Input files are: "
	   << vm["input-file"].as< vector<string> >() << "\n";
      vector<string> v_file = vm["input-file"].as< vector<string> >();
      for(vector<string>::const_iterator it = v_file.begin();
	  it != v_file.end(); ++it) {
	TFile * root_file = new TFile(it->c_str(),"read");

	TH1D * histoZMuMuNoIso = (TH1D*) root_file->Get("nonIsolatedZToMuMuPlots/zMass");
	histoZMuMuNoIso->Rebin(rebinMuMuNoIso);
	fix(histoZMuMuNoIso);

	TH1D * histoZMuMu = (TH1D*) root_file->Get("goodZToMuMuPlots/zMass");
	histoZMuMu->Rebin(rebinMuMu);
	fix(histoZMuMu);

	TH1D * histoZMuTk = (TH1D*) root_file->Get("goodZToMuMuOneTrackPlots/zMass");
	histoZMuTk->Rebin(rebinMuTk);
	fix(histoZMuTk);

	TH1D * histoZMuSa = (TH1D*) root_file->Get("goodZToMuMuOneStandAloneMuonPlots/zMass");
	histoZMuSa->Rebin(rebinMuSa);
	fix(histoZMuSa);

	cout << ">>> histogram loaded\n";
	string f_string = *it;
	replace(f_string.begin(), f_string.end(), '.', '_');
	replace(f_string.begin(), f_string.end(), '/', '_');
	string plot_string = f_string + "." + ext;
	cout << ">>> Input files loaded\n";

	const char * kYieldZMuMu = "YieldZMuMu";
	const char * kEfficiencyTk = "EfficiencyTk";
	const char * kEfficiencySa = "EfficiencySa";
	const char * kEfficiencyIso = "EfficiencyIso";
	const char * kYieldBkgZMuTk = "YieldBkgZMuTk";
	const char * kYieldBkgZMuSa = "YieldBkgZMuSa";
	const char * kYieldBkgZMuMuNotIso = "YieldBkgZMuMuNotIso";
	const char * kLambdaZMuMu = "LambdaZMuMu";
	const char * kMass = "Mass";
	const char * kGamma = "Gamma";
	const char * kPhotonFactorZMuMu = "PhotonFactorZMuMu";
	const char * kInterferenceFactorZMuMu = "InterferenceFactorZMuMu";
	const char * kMeanZMuMu = "MeanZMuMu";
	const char * kSigmaZMuMu = "SigmaZMuMu";
	const char * kAlpha = "Alpha";
	const char * kB0 = "B0";
	const char * kB1 = "B1";
	const char * kB2 = "B2";
	const char * kLambda = "Lambda";
	const char * kA0 = "A0";
	const char * kA1 = "A1";
	const char * kA2 = "A2";
	const char * kBeta = "Beta";
	const char * kSigmaZMuSa = "SigmaZMuSa";

	funct::Parameter lambdaZMuMu(kLambdaZMuMu, commands.par(kLambdaZMuMu));
	funct::Parameter mass(kMass, commands.par(kMass));
	funct::Parameter gamma(kGamma, commands.par(kGamma));
	funct::Parameter photonFactorZMuMu(kPhotonFactorZMuMu, commands.par(kPhotonFactorZMuMu));
	funct::Parameter interferenceFactorZMuMu(kInterferenceFactorZMuMu, commands.par(kInterferenceFactorZMuMu));
	funct::Parameter yieldZMuMu(kYieldZMuMu, commands.par(kYieldZMuMu));
	funct::Parameter efficiencyTk(kEfficiencyTk, commands.par(kEfficiencyTk));
	funct::Parameter efficiencySa(kEfficiencySa, commands.par(kEfficiencySa));
	funct::Parameter efficiencyIso(kEfficiencyIso, commands.par(kEfficiencyIso));
	funct::Parameter yieldBkgZMuTk(kYieldBkgZMuTk, commands.par(kYieldBkgZMuTk));
	funct::Parameter yieldBkgZMuSa(kYieldBkgZMuSa, commands.par(kYieldBkgZMuSa));
	funct::Parameter yieldBkgZMuMuNotIso(kYieldBkgZMuMuNotIso, commands.par(kYieldBkgZMuMuNotIso));
	funct::Parameter meanZMuMu(kMeanZMuMu, commands.par(kMeanZMuMu));
	funct::Parameter sigmaZMuMu(kSigmaZMuMu, commands.par(kSigmaZMuMu));
	funct::Parameter sigmaZMuSa(kSigmaZMuSa, commands.par(kSigmaZMuSa));
	funct::Parameter lambda(kLambda, commands.par(kLambda));
	funct::Parameter alpha(kAlpha, commands.par(kAlpha));
	funct::Parameter beta(kBeta, commands.par(kBeta));
	funct::Parameter b0(kB0, commands.par(kB0));
	funct::Parameter b1(kB1, commands.par(kB1));
	funct::Parameter b2(kB2, commands.par(kB2));
	funct::Parameter a0(kA0, commands.par(kA0));
	funct::Parameter a1(kA1, commands.par(kA1));
	funct::Parameter a2(kA2, commands.par(kA2));
	funct::Constant cFMin(fMin), cFMax(fMax);

	//IntegratorConv integratorConv(20);
	//IntegratorNorm integratorNorm(20);
	IntegratorConv integratorConv(1.e-4);
	IntegratorNorm integratorNorm(1.e-4);

	ZPeakNoNorm zPeakNN = funct::Exponential(lambdaZMuMu) *
	  funct::conv(funct::ZLineShape(mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu),
		      funct::Gaussian(meanZMuMu, sigmaZMuMu),
		      -3*sigmaZMuMu.value(), 3*sigmaZMuMu.value(), integratorConv);
	ZPeak zPeak = zPeakNN / ZPeakNormFactor(zPeakNN, cFMin, cFMax, integratorNorm);
	ZMuMuFun zMuMuFun = funct::master(yieldZMuMu * zPeak);
	ZMuMuFunClone zMuMuFunClone = funct::slave(zMuMuFun);
	IsoefficiencytermSQ efficiencyIsoSquare = (efficiencyIso ^ funct::Numerical<2>(2)); //efficienza Isolamento al quadrato
	ZMuMuEfficiencyTerm zMuMuEfficiencyTerm = ((efficiencyTk ^ funct::Numerical<2>(2)) *
	  (efficiencySa ^ funct::Numerical<2>(2))) * efficiencyIsoSquare;
	ZMuMuNoIsoEfficiencyTerm zMuMuNoIsoEfficiencyTerm = ((efficiencyTk ^ funct::Numerical<2>(2)) *
	  (efficiencySa ^ funct::Numerical<2>(2))) * (funct::Numerical<1>(1) - efficiencyIsoSquare);
	ZMuMu zMuMu = rebinMuMuConst * (zMuMuEfficiencyTerm * zMuMuFun);

	ZMuTkBkg zMuTkBkg = yieldBkgZMuTk * (funct::Exponential(lambda) * funct::Polynomial<2>(a0, a1, a2));
	ZMuTkBkgScaled zMuTkBkgScaled = rebinMuTkConst * zMuTkBkg;
	ZMuTkEfficiencyTerm zMuTkEfficiencyTerm = funct::Numerical<2>(2) *
	  ((efficiencyTk ^ funct::Numerical<2>(2)) * (efficiencySa * (funct::Numerical<1>(1) - efficiencySa))) * efficiencyIsoSquare;
	ZMuTk zMuTk = rebinMuTkConst*(zMuTkEfficiencyTerm * zMuMuFunClone + zMuTkBkg);

	ZMuMuNoIsoBkg zMuMuNoIsoBkg = yieldBkgZMuMuNotIso * (funct::Exponential(alpha) * funct::Polynomial<2>(b0, b1, b2));
	ZMuMuNoIsoBkgScaled  zMuMuNoIsoBkgScaled = rebinMuMuNoIsoConst * zMuMuNoIsoBkg;
	ZMuMuNoIso zMuMuNoIso = rebinMuMuNoIsoConst * ((zMuMuNoIsoEfficiencyTerm * zMuMuFunClone) +  zMuMuNoIsoBkg);

	ZMuSaEfficiencyTerm zMuSaEfficiencyTerm = funct::Numerical<2>(2) *
	  ((efficiencySa ^ funct::Numerical<2>()) * (efficiencyTk * (funct::Numerical<1>() - efficiencyTk)))* efficiencyIsoSquare ;
	ZMuSa zMuSa = rebinMuSaConst *(zMuSaEfficiencyTerm * (yieldZMuMu * funct::Gaussian(mass, sigmaZMuSa))
				       + (yieldBkgZMuSa * funct::Exponential(beta)));

	ChiSquared chi2(zMuMu, histoZMuMu,
			zMuTk, histoZMuTk,
			zMuSa, histoZMuSa,
			zMuMuNoIso,histoZMuMuNoIso,
			fMin, fMax);//WARNING attento all'ordine in cui hai definito il ch2
	cout << "N. deg. of freedom: " << chi2.numberOfBins() << endl;
	fit::RootMinuit<ChiSquared> minuit(chi2, true);
	commands.add(minuit, yieldZMuMu);
	commands.add(minuit, efficiencyTk);
	commands.add(minuit, efficiencySa);
	commands.add(minuit, efficiencyIso);
	commands.add(minuit, yieldBkgZMuTk);
	commands.add(minuit, yieldBkgZMuSa);
	commands.add(minuit, yieldBkgZMuMuNotIso);
	commands.add(minuit, lambdaZMuMu);
	commands.add(minuit, mass);
	commands.add(minuit, gamma);
	commands.add(minuit, photonFactorZMuMu);
	commands.add(minuit, interferenceFactorZMuMu);
	commands.add(minuit, meanZMuMu);
	commands.add(minuit, sigmaZMuMu);
	commands.add(minuit, sigmaZMuSa);
	commands.add(minuit, lambda);
	commands.add(minuit, alpha);
	commands.add(minuit, beta);
	commands.add(minuit, a0);
	commands.add(minuit, a1);
	commands.add(minuit, a2);
	commands.add(minuit, b0);
	commands.add(minuit, b1);
	commands.add(minuit, b2);
	commands.run(minuit);
	const unsigned int nPar = 24;//WARNIG: this must be updated manually for now
	ROOT::Math::SMatrix<double, nPar, nPar, ROOT::Math::MatRepSym<double, nPar> > err;
	minuit.getErrorMatrix(err);
	std::cout << "error matrix:" << std::endl;
	for(unsigned int i = 0; i < nPar; ++i) {
	  for(unsigned int j = 0; j < nPar; ++j) {
	    std::cout << err(i, j) << "\t";
	  }
	  std::cout << std::endl;
	}
	minuit.printFitResults();

	double s;
	s = 0;
	for(int i = 1; i <= histoZMuMuNoIso->GetNbinsX(); ++i)
	  s += histoZMuMuNoIso->GetBinContent(i);
	histoZMuMuNoIso->SetEntries(s);
	for(int i = 1; i <= histoZMuMu->GetNbinsX(); ++i)
	  s += histoZMuMu->GetBinContent(i);
	histoZMuMu->SetEntries(s);
	s = 0;
	for(int i = 1; i <= histoZMuTk->GetNbinsX(); ++i)
	  s += histoZMuTk->GetBinContent(i);
	histoZMuTk->SetEntries(s);
	s = 0;
	for(int i = 1; i <= histoZMuSa->GetNbinsX(); ++i)
	  s += histoZMuSa->GetBinContent(i);
	histoZMuSa->SetEntries(s);
	string ZMuMuPlot = "ZMuMuFit_" + plot_string;
	root::plot<ZMuMu>(ZMuMuPlot.c_str(), *histoZMuMu, zMuMu, fMin, fMax,
			  efficiencyTk, efficiencySa, efficiencyIso,
			  yieldZMuMu, lambdaZMuMu, mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu,
			  meanZMuMu, sigmaZMuMu,
			  kRed, 2, kDashed, 100,
			  "Z -> #mu #mu mass", "#mu #mu invariant mass (GeV/c^{2})",
			  "Events");

	string ZMuMuNoIsoPlot = "ZMuMuNoIsoFit_" + plot_string;
	root::plot<ZMuMuNoIso>(ZMuMuNoIsoPlot.c_str(), *histoZMuMuNoIso, zMuMuNoIso, fMin, fMax,
			       efficiencyTk, efficiencySa, efficiencyIso,
			       yieldZMuMu, lambdaZMuMu, mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu,
			       meanZMuMu, sigmaZMuMu,
			       kRed, 2, kDashed, 100,
			       "Z -> #mu #mu Not Iso mass", "#mu #mu invariant mass (GeV/c^{2})",
			       "Events");

	string ZMuTkPlot = "ZMuTkFit_" + plot_string;
	TF1 funZMuTk = root::tf1<ZMuTk>("ZMuTkFunction", zMuTk, fMin, fMax,
					efficiencyTk, efficiencySa,efficiencyIso,
					yieldZMuMu, lambdaZMuMu, mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu,
					meanZMuMu, sigmaZMuMu,
					yieldBkgZMuTk, lambda, a0, a1, a2);
	funZMuTk.SetLineColor(kRed);
	funZMuTk.SetLineWidth(2);
	funZMuTk.SetLineStyle(kDashed);
	funZMuTk.SetNpx(10000);
	TF1 funZMuTkBkg = root::tf1<ZMuTkBkgScaled>("ZMuTkBack", zMuTkBkgScaled, fMin, fMax,
					      yieldBkgZMuTk, lambda, a0, a1, a2);
	funZMuTkBkg.SetLineColor(kGreen);
	funZMuTkBkg.SetLineWidth(2);
	funZMuTkBkg.SetLineStyle(kDashed);
	funZMuTkBkg.SetNpx(10000);
	histoZMuTk->SetTitle("Z -> #mu + (unmatched) track mass");
	histoZMuTk->SetXTitle("#mu + (unmatched) track invariant mass (GeV/c^{2})");
	histoZMuTk->SetYTitle("Events");
	TCanvas *canvas = new TCanvas("canvas");
	histoZMuTk->Draw("e");
	funZMuTk.Draw("same");
	funZMuTkBkg.Draw("same");
	canvas->SaveAs(ZMuTkPlot.c_str());
	canvas->SetLogy();
	string logZMuTkPlot = "log_" + ZMuTkPlot;
	canvas->SaveAs(logZMuTkPlot.c_str());
	string ZMuSaPlot = "ZMuSaFit_" + plot_string;
	root::plot<ZMuSa>(ZMuSaPlot.c_str(), *histoZMuSa, zMuSa, fMin, fMax,
			  efficiencySa, efficiencyTk, efficiencyIso,
			  yieldZMuMu, mass, sigmaZMuSa, yieldBkgZMuSa,
			  kRed, 2, kDashed, 10000,
			  "Z -> #mu + (unmatched) standalone mass",
			  "#mu + (unmatched) standalone invariant mass (GeV/c^{2})",
			  "Events");
      }
    }

  }
  catch(std::exception& e) {
    cerr << "error: " << e.what() << "\n";
    return 1;
  }
  catch(...) {
    cerr << "Exception of unknown type!\n";
  }
  return 0;
}


#include <mex.h>
#include <igl/C_STR.h>
#include <igl/matlab/mexErrMsgTxt.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) ::mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/fit_cubic_bezier.h>
#include <igl/writeDMAT.h>
#include <Eigen/Core>

void mexFunction(
         int          nlhs,
         mxArray      *plhs[],
         int          nrhs,
         const mxArray *prhs[]
         )
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);
  mexErrMsgTxt(nrhs==2,"nrhs should be == 2");
  Eigen::MatrixXd d;
  parse_rhs_double(prhs+0,d);
  const double error = (double)*mxGetPr(prhs[1]);
  std::vector<Eigen::MatrixXd> cubics;
  igl::fit_cubic_bezier(d,error,cubics);

  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 1:
    {
      prepare_lhs_double(cubics,plhs+0);
      // Fall through
    }
    case 0: break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_WRENCH6D_HPP
#define RW_MATH_WRENCH6D_HPP

/**
 * @file Wrench6D.hpp
 */

#if !defined(SWIG)
#include "EAA.hpp"
#include "Math.hpp"
#include "Transform3D.hpp"
#include "Vector3D.hpp"

#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief Class for representing 6 degrees of freedom wrenches.
     *
     * \f[
     * \mathbf{\nu} =
     * \left[
     *  \begin{array}{c}
     *  f_x\\
     *  f_y\\
     *  f_z\\
     *  \tau_x\\
     *  \tau_y\\
     *  \tau_z
     *  \end{array}
     * \right]
     * \f]
     *
     * A Wrench is the description of a frames linear force and rotational torque
     * with respect to some reference frame.
     *
     */
    template< class T = double > class Wrench6D
    {
      private:
        T _wrench[6];

      public:
        /**
         * @brief Constructs a 6 degrees of freedom velocity screw
         *
         * @param fx [in] @f$ f_x @f$
         * @param fy [in] @f$ f_y @f$
         * @param fz [in] @f$ f_z @f$
         * @param tx [in] @f$ \tau_x @f$
         * @param ty [in] @f$ \tau_y @f$
         * @param tz [in] @f$ \tau_z @f$
         */
        Wrench6D (T fx, T fy, T fz, T tx, T ty, T tz);

        /**
         * @brief Constructs based on Eigen data type
         */
        template< class R > Wrench6D (const Eigen::MatrixBase< R >& v)
        {
            if (v.cols () != 1 || v.rows () != 6)
                RW_THROW ("Unable to initialize VectorND with " << v.rows () << " x " << v.cols ()
                                                                << " matrix");
            for (size_t i = 0; i < 6; i++)
                _wrench[i] = v.row (i) (0);
        }

        /**
         * @brief Default Constructor. Initialized the wrench to 0
         */
        Wrench6D ()
        {
            _wrench[0] = _wrench[1] = _wrench[2] = _wrench[3] = _wrench[4] = _wrench[5] = 0;
        }

        /**
         * @brief Constructs a wrench from a force and torque
         *
         * @param force [in] linear force
         * @param torque [in] angular torque
         */
        Wrench6D (const rw::math::Vector3D< T >& force, const rw::math::Vector3D< T >& torque);

        /**
         * @brief Sets the force component
         *
         * @param force [in] linear force
         */
        void setForce (const rw::math::Vector3D< T >& force)
        {
            _wrench[0] = force (0);
            _wrench[1] = force (1);
            _wrench[2] = force (2);
        }

        /**
         * @brief Sets the torque component
         *
         * @param torque [in] angular torque
         */
        void setTorque (const rw::math::Vector3D< T >& torque)
        {
            _wrench[3] = torque (0);
            _wrench[4] = torque (1);
            _wrench[5] = torque (2);
        }

        /**
         * @brief Extracts the force
         *
         * @return the force
         */
        const rw::math::Vector3D< T > force () const
        {
            return rw::math::Vector3D< T > (_wrench[0], _wrench[1], _wrench[2]);
        }

        /**
         * @brief Extracts the torque and represents it using an Vector3D<T>
         *
         * @return the torque
         */
        const rw::math::Vector3D< T > torque () const
        {
            return rw::math::Vector3D< T > (_wrench[3], _wrench[4], _wrench[5]);
        }
#if !defined(SWIG)
        /**
         * @brief Returns reference to wrench element
         *
         * @param index [in] index in the wrench, index must be @f$ < 6 @f$.
         *
         * @return reference to wrench element
         */
        T& operator() (std::size_t index)
        {
            assert (index < 6);
            return _wrench[index];
        }

        /**
         * @brief Returns const reference to wrench element
         *
         * @param index [in] index in the wrench, index must be @f$ < 6 @f$.
         *
         * @return const reference to wrench element
         */
        const T& operator() (std::size_t index) const
        {
            assert (index < 6);
            return _wrench[index];
        }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param i [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        const T& operator[] (size_t i) const { return (*this) (i); }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param i [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        T& operator[] (size_t i) { return (*this) (i); }
#else
        ARRAYOPERATOR (T);
#endif
        /**
         * @brief Adds the wrench given as a parameter to the wrench.
         *
         * Assumes the wrenches are represented in the same coordinate system.
         *
         * @param wrench [in] Wrench to add
         *
         * @return reference to the Wrench6D to support additional assignments.
         */
        Wrench6D< T >& operator+= (const Wrench6D< T >& wrench)
        {
            for (size_t i = 0; i < 6; i++)
                _wrench[i] += wrench (i);
            return *this;
        }

        /**
         * @brief Subtracts the wrench given as a parameter from the wrench.
         *
         * Assumes the wrenches are represented in the same coordinate system.
         *
         * @param wrench [in] Velocity screw to subtract
         *
         * @return reference to the Wrench6D to support additional
         * assignments.
         */
        Wrench6D< T >& operator-= (const Wrench6D< T >& wrench)
        {
            for (size_t i = 0; i < 6; i++)
                _wrench[i] -= wrench (i);
            return *this;
        }

        /**
         * @brief Scales wrench with s
         *
         * @param s [in] scaling value
         *
         * @return reference to the Wrench6D to support additional
         * assigments
         */
        Wrench6D< T >& operator*= (T s)
        {
            for (size_t i = 0; i < 6; i++)
                _wrench[i] *= s;

            return *this;
        }

        /**
         * @brief Scales wrench and returns scaled version
         * @param s [in] scaling value
         * @return Scaled wrench
         */
        const Wrench6D< T > operator* (T s) const
        {
            Wrench6D result = *this;
            result *= s;
            return result;
        }
#if !defined(SWIG)
        /**
         * @brief Changes frame of reference and referencepoint of
         * wrench: @f$ \robabx{b}{b}{\mathbf{w}}\to
         * \robabx{a}{a}{\mathbf{w}} @f$
         *
         * The frames @f$ \mathcal{F}_a @f$ and @f$ \mathcal{F}_b @f$ are
         * rigidly connected.
         *
         * @param aTb [in] the location of frame @f$ \mathcal{F}_b @f$ wrt.
         * frame @f$ \mathcal{F}_a @f$: @f$ \robabx{a}{b}{\mathbf{T}} @f$
         *
         * @param bV [in] wrench wrt. frame @f$ \mathcal{F}_b @f$: @f$
         * \robabx{b}{b}{\mathbf{\nu}} @f$
         *
         * @return the wrench wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{a}{\mathbf{\nu}} @f$
         *
         * Transformation of both the wrench reference point and of the base to
         * which the wrench is expressed
         *
         * \f[
         * \robabx{a}{a}{\mathbf{w}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{a}{\mathbf{force}} \\
         *  \robabx{a}{a}{\mathbf{torque}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{p}})
         *    \robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{b}{b}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{v}} +
         *    \robabx{a}{b}{\mathbf{p}} \times \robabx{a}{b}{\mathbf{R}}
         *    \robabx{b}{b}{\mathbf{\omega}}\\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         *
         */
        friend const Wrench6D< T > operator* (const Transform3D< T >& aTb, const Wrench6D< T >& bV)
        {
            const rw::math::Vector3D< T >& bv = bV.force ();
            const rw::math::Vector3D< T >& bw = bV.torque ();
            const rw::math::Vector3D< T >& aw = aTb.R () * bw;
            const rw::math::Vector3D< T >& av = aTb.R () * bv + cross (aTb.P (), aw);
            return Wrench6D< T > (av, aw);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief Changes wrench referencepoint of
         * wrench: @f$ \robabx{b}{b}{\mathbf{w}}\to
         * \robabx{a}{a}{\mathbf{w}} @f$
         *
         * The frames @f$ \mathcal{F}_a @f$ and @f$ \mathcal{F}_b @f$ are
         * rigidly connected.
         *
         * @param aPb [in] the location of frame @f$ \mathcal{F}_b @f$ wrt.
         * frame @f$ \mathcal{F}_a @f$: @f$ \robabx{a}{b}{\mathbf{T}} @f$
         *
         * @param bV [in] wrench wrt. frame @f$ \mathcal{F}_b @f$: @f$
         * \robabx{b}{b}{\mathbf{\nu}} @f$
         *
         * @return the wrench wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{a}{\mathbf{\nu}} @f$
         *
         * Transformation of both the velocity reference point and of the base to
         * which the wrench is expressed
         *
         * \f[
         * \robabx{a}{a}{\mathbf{w}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{a}{\mathbf{force}} \\
         *  \robabx{a}{a}{\mathbf{torque}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{p}})
         *    \robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{b}{b}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{v}} +
         *    \robabx{a}{b}{\mathbf{p}} \times \robabx{a}{b}{\mathbf{R}}
         *    \robabx{b}{b}{\mathbf{\omega}}\\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         *
         */
        friend const Wrench6D< T > operator* (const rw::math::Vector3D< T >& aPb, const Wrench6D< T >& bV)
        {
            const rw::math::Vector3D< T >& bv = bV.force ();
            const rw::math::Vector3D< T >& bw = bV.torque ();
            const rw::math::Vector3D< T >& av = bv + cross (aPb, bw);
            return Wrench6D< T > (av, bw);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief Changes frame of reference for wrench: @f$
         * \robabx{b}{i}{\mathbf{w}}\to \robabx{a}{i}{\mathbf{w}}
         * @f$
         *
         * @param aRb [in] the change in orientation between frame
         * @f$ \mathcal{F}_a @f$ and frame
         * @f$ \mathcal{F}_b @f$: @f$ \robabx{a}{b}{\mathbf{R}} @f$
         *
         * @param bV [in] velocity screw wrt. frame
         * @f$ \mathcal{F}_b @f$: @f$ \robabx{b}{i}{\mathbf{\nu}} @f$
         *
         * @return the wrench wrt. frame @f$ \mathcal{F}_a @f$:
         * @f$ \robabx{a}{i}{\mathbf{w}} @f$
         *
         * Transformation of the base to which the wrench is expressed. The wrench
         * reference point is left intact
         *
         * \f[
         * \robabx{a}{i}{\mathbf{w}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{i}{\mathbf{force}} \\
         *  \robabx{a}{i}{\mathbf{torque}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & \mathbf{0}^{3x3} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{b}{i}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{i}{\mathbf{v}} \\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{i}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         */
        friend const Wrench6D< T > operator* (const Rotation3D< T >& aRb, const Wrench6D< T >& bV)
        {
            rw::math::Vector3D< T > bv = bV.force ();
            rw::math::Vector3D< T > bw = bV.torque ();

            return Wrench6D< T > (aRb * bv, aRb * bw);
        }
#endif

        /**
         * @brief Adds two wrenches together @f$
         * \mathbf{w}_{12}=\mathbf{w}_1+\mathbf{w}_2 @f$
         *
         * @param rhs [in] @f$ \mathbf{\nu}_1 @f$
         *
         * @return the wrench @f$ \mathbf{w}_{12} @f$
         */
        const Wrench6D< T > operator+ (const Wrench6D< T >& rhs) const
        {
            return Wrench6D< T > (_wrench[0] + rhs (0),
                                  _wrench[1] + rhs (1),
                                  _wrench[2] + rhs (2),
                                  _wrench[3] + rhs (3),
                                  _wrench[4] + rhs (4),
                                  _wrench[5] + rhs (5));
        }

        /**
         * @brief Subtracts two velocity screws
         * \f$\mathbf{\nu}_{12}=\mathbf{\nu}_1-\mathbf{\nu}_2\f$
         *
         * \param rhs [in] \f$\mathbf{w}_1\f$
         * \return the wrench \f$\mathbf{w}_{12} \f$
         */
        const Wrench6D< T > operator- (const Wrench6D< T >& rhs) const
        {
            return Wrench6D< T > (_wrench[0] - rhs (0),
                                  _wrench[1] - rhs (1),
                                  _wrench[2] - rhs (2),
                                  _wrench[3] - rhs (3),
                                  _wrench[4] - rhs (4),
                                  _wrench[5] - rhs (5));
        }
#if !defined(SWIG)
        /**
         * @brief Ouputs wrench to stream
         *
         * @param os [in/out] stream to use
         * @param wrench [in] the wrench
         * @return the resulting stream
         */
        friend std::ostream& operator<< (std::ostream& os, const Wrench6D< T >& wrench)
        {
            return os << "{{" << wrench (0) << "," << wrench (1) << "," << wrench (2) << "},{"
                      << wrench (3) << "," << wrench (4) << "," << wrench (5) << "}}";
            // return os << wrench.e();
        }
#else
        TOSTRING (rw::math::Wrench6D< T >);
#endif

        /**
         * @brief Takes the 1-norm of the wrench. All elements both
         * force and torque are given the same weight.
         * @return the 1-norm
         */
        T norm1 () const
        {
            return fabs (_wrench[0]) + fabs (_wrench[1]) + fabs (_wrench[2]) + fabs (_wrench[3]) +
                   fabs (_wrench[4]) + fabs (_wrench[5]);
            // return _wrench.template lpNorm<1>();
        }

        /**
         * @brief Takes the 2-norm of the wrench. All elements both
         * force and torque are given the same weight
         * @return the 2-norm
         */
        T norm2 () const
        {
            return std::sqrt (Math::sqr (_wrench[0]) + Math::sqr (_wrench[1]) +
                              Math::sqr (_wrench[2]) + Math::sqr (_wrench[3]) +
                              Math::sqr (_wrench[4]) + Math::sqr (_wrench[5]));
        }

        /**
         * @brief Takes the infinite norm of the wrench. All elements
         * both force and torque are given the same weight.
         *
         * @return the infinite norm
         */
        T normInf () const
        {
            return std::max (
                fabs (_wrench[0]),
                std::max (fabs (_wrench[1]),
                          std::max (fabs (_wrench[2]),
                                    std::max (fabs (_wrench[3]),
                                              std::max (fabs (_wrench[4]), fabs (_wrench[5]))))));
        }

        /**
           @brief Converter to Eigen data type
         */
        Eigen::Matrix< T, 6, 1 > e () const
        {
            Eigen::Matrix< T, 6, 1 > res;
            for (size_t i = 0; i < 6; i++)
                res (i) = _wrench[i];
            return res;
        }

        /**
         * @brief Compares \b a and \b b for equality.
         * @param b [in] other wrench to compare with.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const Wrench6D< T >& b) const
        {
            return _wrench[0] == b[0] && _wrench[1] == b[1] && _wrench[2] == b[2] &&
                   _wrench[3] == b[3] && _wrench[4] == b[4] && _wrench[5] == b[5];
        }

        /**
         * @brief Compares \b a and \b b for inequality.
         * @param b [in] other wrench to compare with.
         * @return True if a and b are different, false otherwise.
         */
        bool operator!= (const Wrench6D< T >& b) const { return !(*this == b); }
    };

    /**
     * @brief Takes the 1-norm of the wrench. All elements both
     * force and torque are given the same weight.
     *
     * @param wrench [in] the wrench
     * @return the 1-norm
     */
    template< class T > T norm1 (const Wrench6D< T >& wrench) { return wrench.norm1 (); }

    /**
     * @brief Takes the 2-norm of the wrench. All elements both
     * force and tporque are given the same weight
     *
     * @param wrench [in] the wrench
     * @return the 2-norm
     */
    template< class T > T norm2 (const Wrench6D< T >& wrench) { return wrench.norm2 (); }

    /**
     * @brief Takes the infinite norm of the wrench. All elements
     * both force and torque are given the same weight.
     *
     * @param wrench [in] the wrench
     *
     * @return the infinite norm
     */
    template< class T > T normInf (const Wrench6D< T >& wrench) { return wrench.normInf (); }

    /**
     * @brief Casts Wrench6D<T> to Wrench6D<Q>
     *
     * @param vs [in] Wrench6D with type T
     *
     * @return Wrench6D with type Q
     */
    template< class Q, class T > const Wrench6D< Q > cast (const Wrench6D< T >& vs)
    {
        return Wrench6D< Q > (static_cast< Q > (vs (0)),
                              static_cast< Q > (vs (1)),
                              static_cast< Q > (vs (2)),
                              static_cast< Q > (vs (3)),
                              static_cast< Q > (vs (4)),
                              static_cast< Q > (vs (5)));
    }
#if !defined(SWIG)
    extern template class rw::math::Wrench6D< double >;
    extern template class rw::math::Wrench6D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Wrench6Dd, rw::math::Wrench6D< double >);
    SWIG_DECLARE_TEMPLATE (Wrench6Df, rw::math::Wrench6D< float >);
#endif

    using Wrench6Dd = Wrench6D< double >;
    using Wrench6Df = Wrench6D< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Wrench6D
         */
        template<>
        void write (const rw::math::Wrench6D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Wrench6D
         */
        template<>
        void write (const rw::math::Wrench6D< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Wrench6D
         */
        template<>
        void read (rw::math::Wrench6D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Wrench6D
         */
        template<>
        void read (rw::math::Wrench6D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard