#include "py_rs_mesh.h"
#include "bs_mesh_grdecl.h"

#include "export_python_wrapper.h"
#include "py_pair_converter.h"
#include <boost/python/tuple.hpp>

#ifdef BSPY_EXPORTING_PLUGIN
using namespace boost::python;

namespace blue_sky { namespace python {

void init_props1(bs_mesh_grdecl& m, t_long nx, t_long ny, t_long nz,
	spv_float dx, spv_float dy, spv_float dz)
{
	m.init_props(nx, ny, nz, dx, dy, dz);
}

void init_props2(bs_mesh_grdecl& m, t_long nx, t_long ny, spv_float coord, spv_float zcorn) {
	m.init_props(nx, ny, coord, zcorn);
}

PY_EXPORTER (mesh_grdecl_exporter, rs_mesh_iface_exporter)
	.def ("get_ext_to_int", &T::get_ext_to_int, args(""), "Return reference to external-to-internal mesh index")
	.def ("get_int_to_ext", &T::get_int_to_ext, args(""), "Return reference to internal-to-external mesh index")
	.def ("get_volumes", &T::get_volumes, args(""), "Return reference to volumes vector")
	.def ("get_cell_volumes", &T::get_cell_volumes, args(""), "Return reference to volumes vector")
	.def ("get_dimensions_range", &T::get_dimensions_range, args("dim1_max, dim1_min, dim2_max, dim2_min, dim3_max, dim3_min"), "Get dimensions ranges")
	.def ("get_element_size", &T::get_element_size, args ("n_elem, dx, dy, dz"), "get elements sizes")
	.def ("get_element_dz", &T::get_element_dim3_size, args ("n_elem"), "get elements dz by internal index")
	.def ("get_element_dz_ext", &T::get_element_dim3_size_ext, args ("i, j, k"), "get elements dz by external index")
	.def ("get_element_ijk_to_int", &T::get_element_ijk_to_int, args ("i, j, k"), "get elements sizes")
	.def ("get_n_active_elements", &T::get_n_active_elements, args (""), "Get elements sizes")
	.def ("calc_element_tops", &T::calc_element_tops, args (""), "Calc element tops")
	.def ("calc_element_center", &T::calc_element_center, args (""), "Calc element center")
	.def ("calc_cells_vertices", &T::calc_cells_vertices)
	.def ("calc_cells_vertices_xyz", &T::calc_cells_vertices_xyz)
	.def ("init_props", &init_props1)
	.def ("init_props", &init_props2)
  .def ("check_data", &T::check_data)
	.def ("get_element_sizes", &T::get_element_sizes)
  .def ("intersect_trajectories", &T::intersect_trajectories)
	;
PY_EXPORTER_END;

}}  // eof blue_sky::python

namespace {
using namespace blue_sky;
using namespace blue_sky::python;

// same as grdecl exporter, but also export gen_coord_zcorn
template <typename T>
struct mesh_grdecl_exporter_plus {
	typedef t_long int_t;
	typedef t_double fp_t;
	typedef spv_float spfp_storarr_t;
	typedef spv_long spi_arr_t;
	typedef typename spi_arr_t::pure_pointed_t int_arr_t;
	typedef std::pair< spv_float, spv_float > coord_zcorn_pair;

	// gen_coord_zcorn overloads
	static coord_zcorn_pair gen_coord_zcorn1(int_t nx, int_t ny, int_t nz, spfp_storarr_t dx, spfp_storarr_t dy, spfp_storarr_t dz,
			fp_t x0, fp_t y0)
	{
		return T::gen_coord_zcorn(nx, ny, nz, dx, dy, dz, x0, y0);
	}
	static coord_zcorn_pair gen_coord_zcorn2(int_t nx, int_t ny, int_t nz, spfp_storarr_t dx, spfp_storarr_t dy, spfp_storarr_t dz,
			fp_t x0)
	{
		return T::gen_coord_zcorn(nx, ny, nz, dx, dy, dz, x0);
	}
	static coord_zcorn_pair gen_coord_zcorn3(int_t nx, int_t ny, int_t nz, spfp_storarr_t dx, spfp_storarr_t dy, spfp_storarr_t dz)
	{
		return T::gen_coord_zcorn(nx, ny, nz, dx, dy, dz);
	}

	// refine_mesh_deltas with overloads
	static tuple refine_mesh_deltas(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t points,
			fp_t m_thresh = DEF_CELL_MERGE_THRESHOLD, fp_t b_thresh = DEF_BAND_THRESHOLD)
	{
		spi_arr_t hit_idx = BS_KERNEL.create_object(int_arr_t::bs_type());
		std::pair< spfp_storarr_t, spfp_storarr_t > r = T::refine_mesh_deltas(nx, ny, coord, points, hit_idx, m_thresh, b_thresh);
		return make_tuple(r.first, r.second, nx, ny, hit_idx);
	}

	static tuple refine_mesh_deltas1(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t points) {
		return refine_mesh_deltas(nx, ny, coord, points);
	}
	static tuple refine_mesh_deltas2(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t points, fp_t m_thresh) {
		return refine_mesh_deltas(nx, ny, coord, points, m_thresh);
	}

	// refine_mesh with overloads
	static tuple refine_mesh(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t zcorn, spfp_storarr_t points,
			fp_t m_thresh = DEF_CELL_MERGE_THRESHOLD, fp_t b_thresh = DEF_BAND_THRESHOLD)
	{
		spi_arr_t hit_idx = BS_KERNEL.create_object(int_arr_t::bs_type());
		std::pair< spfp_storarr_t, spfp_storarr_t > r = T::refine_mesh(nx, ny, coord, zcorn, points, hit_idx, m_thresh, b_thresh);
		return make_tuple(r.first, r.second, nx, ny, hit_idx);
	}

	static tuple refine_mesh1(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t zcorn, spfp_storarr_t points) {
		return refine_mesh(nx, ny, coord, zcorn, points);
	}
	static tuple refine_mesh2(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t zcorn, spfp_storarr_t points, fp_t m_thresh) {
		return refine_mesh(nx, ny, coord, zcorn, points, m_thresh);
	}

	// refine_mesh_deltas with overloads
	// (i,j) points format
	static tuple refine_mesh_deltas_ij(int_t nx, int_t ny, spfp_storarr_t coord,
			spi_arr_t points_pos, spfp_storarr_t points_param,
			fp_t m_thresh = DEF_CELL_MERGE_THRESHOLD, fp_t b_thresh = DEF_BAND_THRESHOLD)
	{
		spi_arr_t hit_idx = BS_KERNEL.create_object(int_arr_t::bs_type());
		std::pair< spfp_storarr_t, spfp_storarr_t > r = T::refine_mesh_deltas(nx, ny, coord, points_pos, points_param,
				hit_idx, m_thresh, b_thresh);
		return make_tuple(r.first, r.second, nx, ny, hit_idx);
	}

	static tuple refine_mesh_deltas_ij1(int_t nx, int_t ny, spfp_storarr_t coord,
			spi_arr_t points_pos, spfp_storarr_t points_param)
	{
		return refine_mesh_deltas_ij(nx, ny, coord, points_pos, points_param);
	}

	static tuple refine_mesh_deltas_ij2(int_t nx, int_t ny, spfp_storarr_t coord,
			spi_arr_t points_pos, spfp_storarr_t points_param, fp_t m_thresh)
	{
		return refine_mesh_deltas_ij(nx, ny, coord, points_pos, points_param, m_thresh);
	}

	// refine_mesh with overloads
	// (i, j) points format
	static tuple refine_mesh_ij(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t zcorn,
			spi_arr_t points_pos, spfp_storarr_t points_param,
			fp_t m_thresh = DEF_CELL_MERGE_THRESHOLD, fp_t b_thresh = DEF_BAND_THRESHOLD)
	{
		spi_arr_t hit_idx = BS_KERNEL.create_object(int_arr_t::bs_type());
		std::pair< spfp_storarr_t, spfp_storarr_t > r = T::refine_mesh(nx, ny, coord, zcorn, points_pos, points_param,
				hit_idx, m_thresh, b_thresh);
		return make_tuple(r.first, r.second, nx, ny, hit_idx);
	}

	static tuple refine_mesh_ij1(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t zcorn,
			spi_arr_t points_pos, spfp_storarr_t points_param)
	{
		return refine_mesh_ij(nx, ny, coord, zcorn, points_pos, points_param);
	}

	static tuple refine_mesh_ij2(int_t nx, int_t ny, spfp_storarr_t coord, spfp_storarr_t zcorn,
			spi_arr_t points_pos, spfp_storarr_t points_param, fp_t m_thresh)
	{
		return refine_mesh_ij(nx, ny, coord, zcorn, points_pos, points_param, m_thresh);
	}

	template <typename class_t>
	static class_t &
	export_class (class_t &class__) {
		using namespace boost::python;

		mesh_grdecl_exporter<T>::export_class (class__)
			.def("gen_coord_zcorn", &T::gen_coord_zcorn, "Generate COORD & ZCORN from given dimensions")
			.def("gen_coord_zcorn", &gen_coord_zcorn1, "Generate COORD & ZCORN from given dimensions")
			.def("gen_coord_zcorn", &gen_coord_zcorn2, "Generate COORD & ZCORN from given dimensions")
			.def("gen_coord_zcorn", &gen_coord_zcorn3, "Generate COORD & ZCORN from given dimensions")
      .def("gen_coord2", &T::gen_coord2, "Generate COORD")
      .staticmethod("gen_coord2")
			.staticmethod("gen_coord_zcorn")
			.def("refine_mesh_deltas", &refine_mesh_deltas, "Calc dx and dy arrays for refined mesh in given points")
			.def("refine_mesh_deltas", &refine_mesh_deltas1, "Calc dx and dy arrays for refined mesh in given points")
			.def("refine_mesh_deltas", &refine_mesh_deltas2, "Calc dx and dy arrays for refined mesh in given points")
			.def("refine_mesh_deltas", &refine_mesh_deltas_ij, "Calc dx and dy arrays for refined mesh in given points")
			.def("refine_mesh_deltas", &refine_mesh_deltas_ij1, "Calc dx and dy arrays for refined mesh in given points")
			.def("refine_mesh_deltas", &refine_mesh_deltas_ij2, "Calc dx and dy arrays for refined mesh in given points")
			.staticmethod("refine_mesh_deltas")
			.def("refine_mesh", &refine_mesh, "Refine existing mesh in given points")
			.def("refine_mesh", &refine_mesh1, "Refine existing mesh in given points")
			.def("refine_mesh", &refine_mesh2, "Refine existing mesh in given points")
			.def("refine_mesh", &refine_mesh_ij, "Refine existing mesh in given points")
			.def("refine_mesh", &refine_mesh_ij1, "Refine existing mesh in given points")
			.def("refine_mesh", &refine_mesh_ij2, "Refine existing mesh in given points")
			.staticmethod("refine_mesh")
			;
		return class__;
	}
};

template< class fp_type >
void reg_sparray_pair() {
	// register converter from pair of returned arrayes to python list
	typedef std::pair< spv_float, spv_float > array_pair;
	typedef bspy_converter< pair_traits< array_pair > > array_pair_converter;
	array_pair_converter::register_to_py();
	array_pair_converter::register_from_py();
}

} // eof hidden namespace

namespace blue_sky { namespace python {

void py_export_mesh_grdecl () {
	class_exporter< bs_mesh_grdecl, rs_mesh_iface, mesh_grdecl_exporter_plus >::export_class ("mesh_grdecl");
	//reg_sparray_pair< float >();
	reg_sparray_pair< double >();
}

}} // namespace blue_sky::python
#endif


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/dist.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( dist_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      A0 tmp = dist(a0, a1);
      return tmp < Zero<A0>() ? Valmax<A0>() : tmp;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( dist_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::unsigned_<A0> >
                          , bd::scalar_< bd::unsigned_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &
                                    ,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return dist(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( dist_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &
                                    ,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return dist(a0, a1);
    }
  };
} } }

#endif


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include "mitkDiffusionFunctionCollection.h"
#include "mitkNumericTypes.h"

#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/version.hpp>
#include <boost/algorithm/string.hpp>
#include <itkPointShell.h>
#include "itkVectorContainer.h"
#include "vnl/vnl_vector.h"
#include <vtkBox.h>
#include <vtkMath.h>
#include <itksys/SystemTools.hxx>
#include <itkShToOdfImageFilter.h>
#include <itkTensorImageToOdfImageFilter.h>

// Intersect a finite line (with end points p0 and p1) with all of the
// cells of a vtkImageData
std::vector< std::pair< itk::Index<3>, double > > mitk::imv::IntersectImage(const itk::Vector<double,3>& spacing, itk::Index<3>& si, itk::Index<3>& ei, itk::ContinuousIndex<float, 3>& sf, itk::ContinuousIndex<float, 3>& ef)
{
  std::vector< std::pair< itk::Index<3>, double > > out;
  if (si == ei)
  {
    double d[3];
    for (int i=0; i<3; ++i)
      d[i] = static_cast<double>(sf[i]-ef[i])*spacing[i];
    double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );
    out.push_back(  std::pair< itk::Index<3>, double >(si, len) );
    return out;
  }

  double bounds[6];

  double entrancePoint[3];
  double exitPoint[3];

  double startPoint[3];
  double endPoint[3];

  double t0, t1;
  for (unsigned int i=0; i<3; ++i)
  {
    startPoint[i] = static_cast<double>(sf[i]);
    endPoint[i] = static_cast<double>(ef[i]);

    if (si[i]>ei[i])
    {
      auto t = si[i];
      si[i] = ei[i];
      ei[i] = t;
    }
  }

  for (auto x = si[0]; x<=ei[0]; ++x)
    for (auto y = si[1]; y<=ei[1]; ++y)
      for (auto z = si[2]; z<=ei[2]; ++z)
      {
        bounds[0] = static_cast<double>(x) - 0.5;
        bounds[1] = static_cast<double>(x) + 0.5;
        bounds[2] = static_cast<double>(y) - 0.5;
        bounds[3] = static_cast<double>(y) + 0.5;
        bounds[4] = static_cast<double>(z) - 0.5;
        bounds[5] = static_cast<double>(z) + 0.5;

        int entryPlane;
        int exitPlane;
        int hit = vtkBox::IntersectWithLine(bounds,
                                            startPoint,
                                            endPoint,
                                            t0,
                                            t1,
                                            entrancePoint,
                                            exitPoint,
                                            entryPlane,
                                            exitPlane);
        if (hit)
        {
          if (entryPlane>=0 && exitPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (exitPoint[i] - entrancePoint[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
          else if (entryPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (static_cast<double>(ef[i]) - entrancePoint[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
          else if (exitPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (exitPoint[i]-static_cast<double>(sf[i]))*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
        }
      }
  return out;
}

//------------------------- SH-function ------------------------------------

double mitk::sh::factorial(int number) {
  if(number <= 1) return 1;
  double result = 1.0;
  for(int i=1; i<=number; i++)
    result *= i;
  return result;
}

void mitk::sh::Cart2Sph(double x, double y, double z, double *spherical)
{
  double phi, th, rad;
  rad = sqrt(x*x+y*y+z*z);
  if( rad < mitk::eps )
  {
    th = itk::Math::pi/2;
    phi = itk::Math::pi/2;
  }
  else
  {
    th = acos(z/rad);
    phi = atan2(y, x);
  }
  spherical[0] = phi;
  spherical[1] = th;
  spherical[2] = rad;
}

vnl_vector_fixed<double, 3> mitk::sh::Sph2Cart(const double& theta, const double& phi, const double& rad)
{
  vnl_vector_fixed<double, 3> dir;
  dir[0] = rad * sin(theta) * cos(phi);
  dir[1] = rad * sin(theta) * sin(phi);
  dir[2] = rad * cos(theta);
  return dir;
}

double mitk::sh::legendre0(int l)
{
  if( l%2 != 0 )
  {
    return 0;
  }
  else
  {
    double prod1 = 1.0;
    for(int i=1;i<l;i+=2) prod1 *= i;
    double prod2 = 1.0;
    for(int i=2;i<=l;i+=2) prod2 *= i;
    return pow(-1.0,l/2.0)*(prod1/prod2);
  }
}

double mitk::sh::Yj(int m, int k, float theta, float phi, bool mrtrix)
{
  if (!mrtrix)
  {
    if (m<0)
      return sqrt(2.0)*static_cast<double>(::boost::math::spherical_harmonic_r(static_cast<unsigned int>(k), -m, theta, phi));
    else if (m==0)
      return static_cast<double>(::boost::math::spherical_harmonic_r(static_cast<unsigned int>(k), m, theta, phi));
    else
      return pow(-1.0,m)*sqrt(2.0)*static_cast<double>(::boost::math::spherical_harmonic_i(static_cast<unsigned int>(k), m, theta, phi));
  }
  else
  {
    double plm = static_cast<double>(::boost::math::legendre_p<float>(k,abs(m),-cos(theta)));
    double mag = sqrt((2.0*k+1.0)/(4.0*itk::Math::pi)*::boost::math::factorial<double>(k-abs(m))/::boost::math::factorial<double>(k+abs(m)))*plm;
    if (m>0)
      return mag*static_cast<double>(cos(m*phi));
    else if (m==0)
      return mag;
    else
      return mag*static_cast<double>(sin(-m*phi));
  }

  return 0;
}

mitk::OdfImage::ItkOdfImageType::Pointer mitk::convert::GetItkOdfFromShImage(mitk::Image::Pointer mitkImage)
{
  mitk::ShImage::Pointer mitkShImage = dynamic_cast<mitk::ShImage*>(mitkImage.GetPointer());
  if (mitkShImage.IsNull())
    mitkThrow() << "Input image is not a SH image!";
  mitk::OdfImage::ItkOdfImageType::Pointer output;
  switch (mitkShImage->ShOrder())
  {
  case 2:
  {
    typedef itk::ShToOdfImageFilter< float, 2 > ShConverterType;
    typename ShConverterType::InputImageType::Pointer itkvol = ShConverterType::InputImageType::New();
    mitk::CastToItkImage(mitkImage, itkvol);
    typename ShConverterType::Pointer converter = ShConverterType::New();
    converter->SetInput(itkvol);
    converter->SetToolkit(mitkShImage->GetShConvention());
    converter->Update();
    output = converter->GetOutput();
    break;
  }
  case 4:
  {
    typedef itk::ShToOdfImageFilter< float, 4 > ShConverterType;
    typename ShConverterType::InputImageType::Pointer itkvol = ShConverterType::InputImageType::New();
    mitk::CastToItkImage(mitkImage, itkvol);
    typename ShConverterType::Pointer converter = ShConverterType::New();
    converter->SetInput(itkvol);
    converter->SetToolkit(mitkShImage->GetShConvention());
    converter->Update();
    output = converter->GetOutput();
    break;
  }
  case 6:
  {
    typedef itk::ShToOdfImageFilter< float, 6 > ShConverterType;
    typename ShConverterType::InputImageType::Pointer itkvol = ShConverterType::InputImageType::New();
    mitk::CastToItkImage(mitkImage, itkvol);
    typename ShConverterType::Pointer converter = ShConverterType::New();
    converter->SetInput(itkvol);
    converter->SetToolkit(mitkShImage->GetShConvention());
    converter->Update();
    output = converter->GetOutput();
    break;
  }
  case 8:
  {
    typedef itk::ShToOdfImageFilter< float, 8 > ShConverterType;
    typename ShConverterType::InputImageType::Pointer itkvol = ShConverterType::InputImageType::New();
    mitk::CastToItkImage(mitkImage, itkvol);
    typename ShConverterType::Pointer converter = ShConverterType::New();
    converter->SetInput(itkvol);
    converter->SetToolkit(mitkShImage->GetShConvention());
    converter->Update();
    output = converter->GetOutput();
    break;
  }
  case 10:
  {
    typedef itk::ShToOdfImageFilter< float, 10 > ShConverterType;
    typename ShConverterType::InputImageType::Pointer itkvol = ShConverterType::InputImageType::New();
    mitk::CastToItkImage(mitkImage, itkvol);
    typename ShConverterType::Pointer converter = ShConverterType::New();
    converter->SetInput(itkvol);
    converter->SetToolkit(mitkShImage->GetShConvention());
    converter->Update();
    output = converter->GetOutput();
    break;
  }
  case 12:
  {
    typedef itk::ShToOdfImageFilter< float, 12 > ShConverterType;
    typename ShConverterType::InputImageType::Pointer itkvol = ShConverterType::InputImageType::New();
    mitk::CastToItkImage(mitkImage, itkvol);
    typename ShConverterType::Pointer converter = ShConverterType::New();
    converter->SetInput(itkvol);
    converter->SetToolkit(mitkShImage->GetShConvention());
    converter->Update();
    output = converter->GetOutput();
    break;
  }
  default:
    mitkThrow() << "SH orders higher than 12 are not supported!";
  }

  return output;
}

mitk::OdfImage::Pointer mitk::convert::GetOdfFromShImage(mitk::Image::Pointer mitkImage)
{
  mitk::OdfImage::Pointer image = mitk::OdfImage::New();
  auto img = GetItkOdfFromShImage(mitkImage);
  image->InitializeByItk( img.GetPointer() );
  image->SetVolume( img->GetBufferPointer() );
  return image;
}

mitk::OdfImage::ItkOdfImageType::Pointer mitk::convert::GetItkOdfFromTensorImage(mitk::Image::Pointer mitkImage)
{
  typedef itk::TensorImageToOdfImageFilter< float, float > FilterType;
  FilterType::Pointer filter = FilterType::New();
  filter->SetInput( GetItkTensorFromTensorImage(mitkImage) );
  filter->Update();
  return filter->GetOutput();
}

mitk::TensorImage::ItkTensorImageType::Pointer mitk::convert::GetItkTensorFromTensorImage(mitk::Image::Pointer mitkImage)
{
  typedef mitk::ImageToItk< mitk::TensorImage::ItkTensorImageType > CasterType;
  CasterType::Pointer caster = CasterType::New();
  caster->SetInput(mitkImage);
  caster->Update();
  return caster->GetOutput();
}

mitk::PeakImage::ItkPeakImageType::Pointer mitk::convert::GetItkPeakFromPeakImage(mitk::Image::Pointer mitkImage)
{
  typedef mitk::ImageToItk< mitk::PeakImage::ItkPeakImageType > CasterType;
  CasterType::Pointer caster = CasterType::New();
  caster->SetInput(mitkImage);
  caster->SetCopyMemFlag(true);
  caster->Update();
  return caster->GetOutput();
}

mitk::OdfImage::Pointer mitk::convert::GetOdfFromTensorImage(mitk::Image::Pointer mitkImage)
{
  mitk::OdfImage::Pointer image = mitk::OdfImage::New();
  auto img = GetItkOdfFromTensorImage(mitkImage);
  image->InitializeByItk( img.GetPointer() );
  image->SetVolume( img->GetBufferPointer() );
  return image;
}

mitk::OdfImage::ItkOdfImageType::Pointer mitk::convert::GetItkOdfFromOdfImage(mitk::Image::Pointer mitkImage)
{
  typedef mitk::ImageToItk< mitk::OdfImage::ItkOdfImageType > CasterType;
  CasterType::Pointer caster = CasterType::New();
  caster->SetInput(mitkImage);
  caster->Update();
  return caster->GetOutput();
}

vnl_matrix<float> mitk::sh::CalcShBasisForDirections(unsigned int sh_order, vnl_matrix<double> U, bool mrtrix)
{
  vnl_matrix<float> sh_basis  = vnl_matrix<float>(U.cols(), (sh_order*sh_order + sh_order + 2)/2 + sh_order );
  for(unsigned int i=0; i<U.cols(); i++)
  {
    double x = U(0,i);
    double y = U(1,i);
    double z = U(2,i);
    double spherical[3];
    mitk::sh::Cart2Sph(x,y,z,spherical);
    U(0,i) = spherical[0];
    U(1,i) = spherical[1];
    U(2,i) = spherical[2];
  }

  for(unsigned int i=0; i<U.cols(); i++)
  {
    for(int k=0; k<=static_cast<int>(sh_order); k+=2)
    {
      for(int m=-k; m<=k; m++)
      {
        int j = (k*k + k + 2)/2 + m - 1;
        double phi = U(0,i);
        double th = U(1,i);
        sh_basis(i,j) = mitk::sh::Yj(m,k,th,phi, mrtrix);
      }
    }
  }

  return sh_basis;
}

unsigned int mitk::sh::ShOrder(int num_coeffs)
{
  int c=3, d=2-2*num_coeffs;
  int D = c*c-4*d;
  if (D>0)
  {
    int s = (-c+static_cast<int>(sqrt(D)))/2;
    if (s<0)
      s = (-c-static_cast<int>(sqrt(D)))/2;
    return static_cast<unsigned int>(s);
  }
  else if (D==0)
    return static_cast<unsigned int>(-c/2);
  return 0;
}

float mitk::sh::GetValue(const vnl_vector<float> &coefficients, const int &sh_order, const double theta, const double phi, const bool mrtrix)
{
  float val = 0;
  for(int k=0; k<=sh_order; k+=2)
  {
    for(int m=-k; m<=k; m++)
    {
      unsigned int j = static_cast<unsigned int>((k*k + k + 2)/2 + m - 1);
      val += coefficients[j] * mitk::sh::Yj(m, k, theta, phi, mrtrix);
    }
  }

  return val;
}

float mitk::sh::GetValue(const vnl_vector<float> &coefficients, const int &sh_order, const vnl_vector_fixed<double, 3> &dir, const bool mrtrix)
{
  double spherical[3];
  mitk::sh::Cart2Sph(dir[0], dir[1], dir[2], spherical);

  float val = 0;
  for(int k=0; k<=sh_order; k+=2)
  {
    for(int m=-k; m<=k; m++)
    {
      int j = (k*k + k + 2)/2 + m - 1;
      val += coefficients[j] * mitk::sh::Yj(m, k, spherical[1], spherical[0], mrtrix);
    }
  }

  return val;
}

//------------------------- gradients-function ------------------------------------

mitk::gradients::GradientDirectionContainerType::ConstPointer mitk::gradients::ReadBvalsBvecs(std::string bvals_file, std::string bvecs_file, double& reference_bval)
{
  mitk::gradients::GradientDirectionContainerType::Pointer directioncontainer = mitk::gradients::GradientDirectionContainerType::New();

  std::vector<float> bvec_entries;
  if (!itksys::SystemTools::FileExists(bvecs_file))
    mitkThrow() << "bvecs file not existing: " << bvecs_file;
  else
  {
    std::string line;
    std::ifstream myfile (bvecs_file.c_str());
    if (myfile.is_open())
    {
      while (std::getline(myfile, line))
      {
        std::vector<std::string> strs;
        boost::split(strs,line,boost::is_any_of("\t \n"));
        for (auto token : strs)
        {
          if (!token.empty())
          {
            try
            {
              bvec_entries.push_back(boost::lexical_cast<float>(token));
            }
            catch(...)
            {
              mitkThrow() << "Encountered invalid bvecs file entry >" << token << "<";
            }
          }
        }
      }
      myfile.close();
    }
    else
    {
      mitkThrow() << "bvecs file could not be opened: " << bvals_file;
    }
  }

  reference_bval = -1;
  std::vector<float> bval_entries;
  if (!itksys::SystemTools::FileExists(bvals_file))
    mitkThrow() << "bvals file not existing: " << bvals_file;
  else
  {
    std::string line;
    std::ifstream myfile (bvals_file.c_str());
    if (myfile.is_open())
    {
      while (std::getline(myfile, line))
      {
        std::vector<std::string> strs;
        boost::split(strs,line,boost::is_any_of("\t \n"));
        for (auto token : strs)
        {
          if (!token.empty())
          {
            try {
              bval_entries.push_back(boost::lexical_cast<float>(token));
              if (bval_entries.back()>reference_bval)
                reference_bval = bval_entries.back();
            }
            catch(...)
            {
              mitkThrow() << "Encountered invalid bvals file entry >" << token << "<";
            }
          }
        }
      }
      myfile.close();
    }
    else
    {
      mitkThrow() << "bvals file could not be opened: " << bvals_file;
    }
  }

  for(unsigned int i=0; i<bval_entries.size(); i++)
  {
    double b_val = bval_entries.at(i);

    mitk::gradients::GradientDirectionType vec;
    vec[0] = bvec_entries.at(i);
    vec[1] = bvec_entries.at(i+bval_entries.size());
    vec[2] = bvec_entries.at(i+2*bval_entries.size());

    // Adjust the vector length to encode gradient strength
    if (reference_bval>0)
    {
      double factor = b_val/reference_bval;
      if(vec.magnitude() > 0)
      {
        vec.normalize();
        vec[0] = sqrt(factor)*vec[0];
        vec[1] = sqrt(factor)*vec[1];
        vec[2] = sqrt(factor)*vec[2];
      }
    }

    directioncontainer->InsertElement(i,vec);
  }

  return GradientDirectionContainerType::ConstPointer(directioncontainer);
}

void mitk::gradients::WriteBvalsBvecs(std::string bvals_file, std::string bvecs_file, GradientDirectionContainerType::ConstPointer gradients, double reference_bval)
{
  std::ofstream myfile;
  myfile.open (bvals_file.c_str());
  for(unsigned int i=0; i<gradients->Size(); i++)
  {
    double twonorm = gradients->ElementAt(i).two_norm();
    myfile << std::round(reference_bval*twonorm*twonorm) << " ";
  }
  myfile.close();

  std::ofstream myfile2;
  myfile2.open (bvecs_file.c_str());
  for(int j=0; j<3; j++)
  {
    for(unsigned int i=0; i<gradients->Size(); i++)
    {
      GradientDirectionType direction = gradients->ElementAt(i);
      direction.normalize();
      myfile2 << direction.get(j) << " ";
    }
    myfile2 << std::endl;
  }
}

std::vector<unsigned int> mitk::gradients::GetAllUniqueDirections(const BValueMap & refBValueMap, GradientDirectionContainerType::ConstPointer refGradientsContainer )
{

  IndiciesVector directioncontainer;
  auto mapIterator = refBValueMap.begin();

  if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
    mapIterator++; //skip bzero Values

  for( ; mapIterator != refBValueMap.end(); mapIterator++){

    IndiciesVector currentShell = mapIterator->second;

    while(currentShell.size()>0)
    {
      unsigned int wntIndex = currentShell.back();
      currentShell.pop_back();

      auto containerIt = directioncontainer.begin();
      bool directionExist = false;
      while(containerIt != directioncontainer.end())
      {
        if (fabs(dot_product(refGradientsContainer->ElementAt(*containerIt), refGradientsContainer->ElementAt(wntIndex)))  > 0.9998)
        {
          directionExist = true;
          break;
        }
        containerIt++;
      }
      if(!directionExist)
      {
        directioncontainer.push_back(wntIndex);
      }
    }
  }

  return directioncontainer;
}

bool mitk::gradients::CheckForDifferingShellDirections(const BValueMap & refBValueMap, GradientDirectionContainerType::ConstPointer refGradientsContainer)
{
  auto mapIterator = refBValueMap.begin();

  if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
    mapIterator++; //skip bzero Values

  for( ; mapIterator != refBValueMap.end(); mapIterator++){

    auto mapIterator_2 = refBValueMap.begin();
    if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
      mapIterator_2++; //skip bzero Values

    for( ; mapIterator_2 != refBValueMap.end(); mapIterator_2++){

      if(mapIterator_2 == mapIterator) continue;

      IndiciesVector currentShell = mapIterator->second;
      IndiciesVector testShell = mapIterator_2->second;
      for (unsigned int i = 0; i< currentShell.size(); i++)
        if (fabs(dot_product(refGradientsContainer->ElementAt(currentShell[i]), refGradientsContainer->ElementAt(testShell[i])))  <= 0.9998) { return true; }

    }
  }
  return false;
}

vnl_matrix<double> mitk::gradients::ComputeSphericalFromCartesian(const IndiciesVector & refShell, const GradientDirectionContainerType * refGradientsContainer)
{

  vnl_matrix<double> Q(3, refShell.size());
  Q.fill(0.0);

  for(unsigned int i = 0; i < refShell.size(); i++)
  {
    GradientDirectionType dir = refGradientsContainer->ElementAt(refShell[i]);
    double x = dir.normalize().get(0);
    double y = dir.normalize().get(1);
    double z = dir.normalize().get(2);
    double cart[3];
    mitk::sh::Cart2Sph(x,y,z,cart);
    Q(0,i) = cart[0];
    Q(1,i) = cart[1];
    Q(2,i) = cart[2];
  }
  return Q;
}

vnl_matrix<double> mitk::gradients::ComputeSphericalHarmonicsBasis(const vnl_matrix<double> & QBallReference, const unsigned int & LOrder)
{
  vnl_matrix<double> SHBasisOutput(QBallReference.cols(), (LOrder+1)*(LOrder+2)*0.5);
  SHBasisOutput.fill(0.0);
  for(unsigned int i=0; i<SHBasisOutput.rows(); i++)
    for(int k = 0; k <= (int)LOrder; k += 2)
      for(int m =- k; m <= k; m++)
      {
        int j = ( k * k + k + 2 ) / 2.0 + m - 1;
        double phi = QBallReference(0,i);
        double th = QBallReference(1,i);
        double val = mitk::sh::Yj(m,k,th,phi);
        SHBasisOutput(i,j) = val;
      }
  return SHBasisOutput;
}

mitk::gradients::GradientDirectionContainerType::Pointer mitk::gradients::CreateNormalizedUniqueGradientDirectionContainer(const mitk::gradients::BValueMap & bValueMap,
                                                                                const GradientDirectionContainerType *origninalGradentcontainer)
{
  mitk::gradients::GradientDirectionContainerType::Pointer directioncontainer = mitk::gradients::GradientDirectionContainerType::New();
  auto mapIterator = bValueMap.begin();

  if(bValueMap.find(0) != bValueMap.end() && bValueMap.size() > 1){
    mapIterator++; //skip bzero Values
    vnl_vector_fixed<double, 3> vec;
    vec.fill(0.0);
    directioncontainer->push_back(vec);
  }

  for( ; mapIterator != bValueMap.end(); mapIterator++){

    IndiciesVector currentShell = mapIterator->second;

    while(currentShell.size()>0)
    {
      unsigned int wntIndex = currentShell.back();
      currentShell.pop_back();

      mitk::gradients::GradientDirectionContainerType::Iterator containerIt = directioncontainer->Begin();
      bool directionExist = false;
      while(containerIt != directioncontainer->End())
      {
        if (fabs(dot_product(containerIt.Value(), origninalGradentcontainer->ElementAt(wntIndex)))  > 0.9998)
        {
          directionExist = true;
          break;
        }
        containerIt++;
      }
      if(!directionExist)
      {
        GradientDirectionType dir(origninalGradentcontainer->ElementAt(wntIndex));
        directioncontainer->push_back(dir.normalize());
      }
    }
  }

  return directioncontainer;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/arch/common/detail/simd/trig_reduction.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/if_allbits_else_zero.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/detail/constant/maxleftshift.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0, class unit_tag, class mode>
    struct trig_base < A0, unit_tag,  tag::simd_type, mode>
    {
      typedef typename bs::as_logical_t<A0>                        bA0; // logical type associated to A0
      typedef trig_reduction<A0,unit_tag, tag::simd_type, mode> redu_t;
      typedef trig_evaluation<A0,tag::simd_type>                eval_t;
      typedef typename bd::scalar_of<A0>::type                     sA0; // scalar version of A0
      typedef typename bd::as_integer<A0, signed>::type       int_type; // signed integer type associated to A0
      typedef typename bs::as_logical<int_type>::type        bint_type; // logical type associated to int_type
      typedef typename bd::scalar_of<int_type>::type         sint_type; // scalar version of the associated type
      typedef typename mode::type                                style;

      // for all functions the scalar algorithm is:
      // * range reduction
      // * computation of sign and evaluation selections flags
      // * evaluations of the two branches and selection using flags
      // * return with flag based corrections and inf and nan or specific invalid cases inputs considerations

      static BOOST_FORCEINLINE A0 cosa(const A0& a0){ return cosa(a0, style()); }
      static BOOST_FORCEINLINE A0 sina(const A0& a0){ return sina(a0, style()); }
      static BOOST_FORCEINLINE A0 tana(const A0& a0){ return tana(a0, style()); }
      static BOOST_FORCEINLINE A0 cota(const A0& a0){ return cota(a0, style()); }
      static BOOST_FORCEINLINE std::pair<A0,A0> sincosa(const A0& a0){ return sincosa(a0,style()); }

    private:
      static BOOST_FORCEINLINE A0 cosa(const A0& a0, const tag::restricted&)
      {
        const A0 x =  scale(a0);
        return  eval_t::cos_eval(sqr(x));
      }

      static BOOST_FORCEINLINE A0 cosa(const A0& a0, const tag::regular&)
      {
        const A0 x = bs::abs(a0);
        A0 xr = Nan<A0>();
        A0 n =  redu_t::reduce(x, xr);
        auto tmp = if_one_else_zero(n >= Two<A0>());
        auto swap_bit = (fma(Mtwo<A0>(), tmp, n));
        auto sign_bit = if_else_zero(bitwise_xor(swap_bit, tmp), Signmask<A0>());
        A0 z = sqr(xr);
        A0 se = eval_t::sin_eval(z, xr);
        A0 ce = eval_t::cos_eval(z);
        A0 z1 = if_else(is_nez(swap_bit), se, ce);
        return bitwise_xor(z1, sign_bit);
      }

      static BOOST_FORCEINLINE A0 sina(const A0& a0_n, const tag::restricted&)
      {
        A0 x =   scale(a0_n);
        A0 se = eval_t::sin_eval(sqr(x), x);
        return se;
      }

      static BOOST_FORCEINLINE A0 sina(const A0& a0, const tag::regular&)
      {
        const A0 x = bs::abs(a0);
        A0 xr = Nan<A0>();
        const A0 n = redu_t::reduce(x, xr);
        auto tmp = if_one_else_zero(n >= Two<A0>());
        auto swap_bit = (fma(Mtwo<A0>(), tmp, n));
        auto sign_bit = bitwise_xor(bitofsign(a0), if_else_zero(tmp, Signmask<A0>()));
        const A0 z = sqr(xr);
        const A0 se = eval_t::sin_eval(z, xr);
        const A0 ce = eval_t::cos_eval(z);
        const A0 z1 = if_else(is_eqz(swap_bit), se, ce);
        return bitwise_xor(z1, sign_bit);
      }

      static BOOST_FORCEINLINE A0 tana(const A0& a0, const tag::restricted&)
      {
        return eval_t::base_tancot_eval(scale(a0));
      }

      static BOOST_FORCEINLINE A0 tana(const A0& a0, const tag::regular&)
      {
        A0 x =  bs::abs(a0);
        A0 xr = Nan<A0>();
        A0 n = redu_t::reduce(x, xr);
        auto tmp = if_one_else_zero( n >= Two<A0>());
        auto swap_bit = (fma(Mtwo<A0>(), tmp, n));
        auto test = is_eqz(swap_bit);
        const A0 y = eval_t::tan_eval(xr, test);
        auto testnan = redu_t::tan_invalid(a0);
        return if_nan_else(testnan, bitwise_xor(y, bitofsign(a0)));
      }

      static BOOST_FORCEINLINE A0 cota(const A0& a0, const tag::restricted&)
      {
        return eval_t::base_tancot_eval(scale(a0));
      }

      static BOOST_FORCEINLINE A0 cota(const A0& a0, const tag::regular&)
      {
        const A0 x = bs::abs(a0);
        A0 xr = Nan<A0>();
        const A0 n = redu_t::reduce(x, xr);
        auto tmp = if_one_else_zero( n >= Two<A0>());
        auto swap_bit = (fma(Mtwo<A0>(), tmp, n));
        auto test = is_eqz(swap_bit);
        const A0 y = eval_t::cot_eval(xr, test);
        const bA0 testnan = redu_t::cot_invalid(a0);
        // this if_else is normally not needed but with clang the zero value if erroneous
        // if not there !
        return if_else(is_nez(a0), if_nan_else(testnan
                                              , bitwise_xor(y, bitofsign(a0))), rec(a0));
      }

      // simultaneous cosa and sina function
      static BOOST_FORCEINLINE std::pair<A0, A0> sincosa(const A0& a0, const tag::restricted&)
      {
        A0 x =  scale(a0);
        A0 z =  sqr(x);
        return {eval_t::sin_eval(z, x), eval_t::cos_eval(z)};
      }

      static BOOST_FORCEINLINE  std::pair<A0, A0> sincosa(const A0& a0, const tag::regular&)
      {
        A0 x =  bs::abs(a0);
        A0 xr = Nan<A0>();
        A0 n = redu_t::reduce(x, xr);
        auto tmp = if_one_else_zero(n >= Two<A0>());
        auto swap_bit = (fma(Mtwo<A0>(), tmp, n));
        auto cos_sign_bit = if_else_zero(bitwise_xor(swap_bit, tmp), Signmask<A0>());
        auto sin_sign_bit = bitwise_xor(bitofsign(a0),if_else_zero(tmp, Signmask<A0>()));

        A0 z = bs::sqr(xr);
        A0 t1 = eval_t::sin_eval(z, xr);
        A0 t2 = eval_t::cos_eval(z);
        auto test = is_nez(swap_bit);
        return { bitwise_xor(if_else(test, t2, t1),sin_sign_bit)
               , bitwise_xor(if_else(test, t1, t2),cos_sign_bit) };
      }

      static BOOST_FORCEINLINE A0 scale(const A0& a0)
      {
        return if_nan_else(is_greater(bs::abs(a0),
                              trig_ranges<A0,unit_tag>::max_range()), a0)
          *trig_ranges<A0,unit_tag>::scale();
      }

    };
  }
} }

#endif


#include <boost/python/module.hpp>
#include <boost/python/def.hpp>

#include <cctbx/covariance/covariance.h>

namespace cctbx { namespace covariance {
  namespace boost_python {

    void wrap_covariance_matrix() {
      using namespace boost::python;

      def("extract_covariance_matrix_for_sites", (
        af::versa<double, af::packed_u_accessor>(*)(
        af::const_ref<std::size_t> const &,
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        extract_covariance_matrix_for_sites,
        (arg("i_seqs"), arg("matrix"), arg("parameter_map")));
      def("extract_covariance_matrix_for_u_aniso", (
        af::versa<double, af::packed_u_accessor>(*)(
        std::size_t,
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        extract_covariance_matrix_for_u_aniso,
        (arg("i_seq"), arg("matrix"), arg("parameter_map")));
      def("variance_for_u_iso", (
        double(*)(
        std::size_t,
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        variance_for_u_iso,
        (arg("i_seq"), arg("matrix"), arg("parameter_map")));
      def("orthogonalize_covariance_matrix", (
        af::versa<double, af::packed_u_accessor>(*)(
        af::const_ref<double, af::packed_u_accessor> const &,
        cctbx::uctbx::unit_cell const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        orthogonalize_covariance_matrix,
        (arg("matrix"), arg("unit_cell"), arg("parameter_map")));
      def("covariance_orthogonalization_matrix", (
       scitbx::sparse::matrix<double>(*)(
        cctbx::uctbx::unit_cell const &,
        cctbx::xray::parameter_map<cctbx::xray::scatterer<double> > const &))
        covariance_orthogonalization_matrix,
        (arg("unit_cell"), arg("parameter_map")));
    }

    void init_module()
    {
      wrap_covariance_matrix();
    }

}}} // namespace cctbx::covariance::boost_python

BOOST_PYTHON_MODULE(cctbx_covariance_ext)
{
  cctbx::covariance::boost_python::init_module();
}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "median_base_quality.hpp"

#include <algorithm>
#include <iterator>

#include <boost/variant.hpp>
#include <boost/optional.hpp>

#include "core/tools/read_assigner.hpp"
#include "core/types/allele.hpp"
#include "io/variant/vcf_record.hpp"
#include "io/variant/vcf_spec.hpp"
#include "utils/maths.hpp"
#include "utils/append.hpp"
#include "../facets/samples.hpp"
#include "../facets/alleles.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string MedianBaseQuality::name_ = "BQ";

std::unique_ptr<Measure> MedianBaseQuality::do_clone() const
{
    return std::make_unique<MedianBaseQuality>(*this);
}

Measure::ResultType MedianBaseQuality::get_default_result() const
{
    return std::vector<boost::optional<int>> {};
}

namespace {

bool is_canonical(const VcfRecord::NucleotideSequence& allele) noexcept
{
    const static VcfRecord::NucleotideSequence deleted_allele {vcfspec::deletedBase};
    return !(allele == vcfspec::missingValue || allele == deleted_allele);
}

bool has_called_alt_allele(const VcfRecord& call, const VcfRecord::SampleName& sample)
{
    if (!call.has_genotypes()) return true;
    const auto& genotype = get_genotype(call, sample);
    return std::any_of(std::cbegin(genotype), std::cend(genotype),
                       [&] (const auto& allele) { return allele != call.ref() && is_canonical(allele); });
}

bool is_evaluable(const VcfRecord& call, const VcfRecord::SampleName& sample)
{
    return has_called_alt_allele(call, sample);
}

auto median_base_quality(const ReadRefSupportSet& reads, const Allele& allele)
{
    boost::optional<AlignedRead::BaseQuality> result {};
    if (!is_indel(allele)) {
        std::vector<AlignedRead::BaseQuality> base_qualities {};
        base_qualities.reserve(reads.size() * sequence_size(allele));
        for (const auto& read : reads) {
            if (overlaps(read.get(), allele)) {
                utils::append(copy_base_qualities(read, mapped_region(allele)), base_qualities);
            }
        }
        if (!base_qualities.empty()) {
            result = maths::median(base_qualities);
        }
    }
    return result;
}

} // namespace

Measure::ResultType MedianBaseQuality::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& alleles = get_value<Alleles>(facets.at("Alleles"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments")).alleles;
    std::vector<boost::optional<int>> result {};
    result.reserve(call.num_alt());
    for (const auto& sample : samples) {
        boost::optional<int> sample_result {};
        if (is_evaluable(call, sample)) {
            const auto& allele_support = assignments.at(sample);
            for (const auto& allele : get_alt(alleles, call, sample)) {
                const auto median_bq = median_base_quality(allele_support.at(allele), allele);
                if (median_bq) {
                    if (sample_result) {
                        sample_result = std::min(*sample_result, static_cast<int>(*median_bq));
                    } else {
                        sample_result = *median_bq;
                    }
                }
            }
        }
        result.push_back(sample_result);
    }
    return result;
}

Measure::ResultCardinality MedianBaseQuality::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& MedianBaseQuality::do_name() const
{
    return name_;
}

std::string MedianBaseQuality::do_describe() const
{
    return "Median base quality of reads supporting each allele";
}

std::vector<std::string> MedianBaseQuality::do_requirements() const
{
    return {"Samples", "Alleles", "ReadAssignments"};
}

} // namespace csr
} // namespace octopus


//Machine-generated by miind.py. Edit at your own risk.

#include <boost/timer/timer.hpp>
#include <GeomLib.hpp>
#include <TwoDLib.hpp>
#include <MPILib/include/MPINetworkCode.hpp>
#include <MPILib/include/RateAlgorithmCode.hpp>
#include <MPILib/include/SimulationRunParameter.hpp>
#include <MPILib/include/report/handler/RootReportHandler.hpp>
#include <MPILib/include/WilsonCowanAlgorithm.hpp>
#include <MPILib/include/PersistantAlgorithm.hpp>
#include <MPILib/include/DelayAlgorithmCode.hpp>
#include <MPILib/include/RateFunctorCode.hpp>
typedef MPILib::MPINetwork<MPILib::DelayedConnection, MPILib::utilities::CircularDistribution> Network;
	// defining variables
	const double omega = 1.0;
int main(int argc, char *argv[]){
	Network network;
	boost::timer::auto_cpu_timer t;

#ifdef ENABLE_MPI
	// initialise the mpi environment this cannot be forwarded to a class
	boost::mpi::environment env(argc, argv);
#endif

	try {	// generating algorithms
	std::vector<std::string> vec_mat_0{"aexp97b7e1e7-b88c-4ff8-8e86-7e1e092f4d9a_-1_0_0_0_.mat","aexp97b7e1e7-b88c-4ff8-8e86-7e1e092f4d9a_1_0_0_0_.mat"};
	TwoDLib::MeshAlgorithm<DelayedConnection,TwoDLib::MasterOdeint> alg_mesh_0("aexp97b7e1e7-b88c-4ff8-8e86-7e1e092f4d9a.model",vec_mat_0,0.00002, 0.0);
	MPILib::Rate RateFunction_1(MPILib::Time);
	MPILib::RateFunctor<DelayedConnection> rate_functor_1(RateFunction_1);
	MPILib::Rate RateFunction_2(MPILib::Time);
	MPILib::RateFunctor<DelayedConnection> rate_functor_2(RateFunction_2);
	// generating nodes
	MPILib::NodeId id_0 = network.addNode(alg_mesh_0,MPILib::EXCITATORY_DIRECT);
	MPILib::NodeId id_1 = network.addNode(rate_functor_1,MPILib::NEUTRAL);
	MPILib::NodeId id_2 = network.addNode(rate_functor_2,MPILib::NEUTRAL);
	// generating connections
	DelayedConnection con_1_0_0(1,1.,0);
	network.makeFirstInputOfSecond(id_1,id_0,con_1_0_0);
	DelayedConnection con_2_0_0(1,-1.,0);
	network.makeFirstInputOfSecond(id_2,id_0,con_2_0_0);
	// generation simulation parameter
	MPILib::report::handler::RootReportHandler handler("single",true);

	SimulationRunParameter par_run( handler,1000000,0,0.4,1e-03,0.0001,"single.log",1e-03);
	network.configureSimulation(par_run);
	network.evolve();
	} catch(std::exception& exc){
		std::cout << exc.what() << std::endl;
#ifdef ENABLE_MPI
	//Abort the MPI environment in the correct way :
	env.abort(1);
#endif
	}

	MPILib::utilities::MPIProxy().barrier();
	t.stop();
	if (MPILib::utilities::MPIProxy().getRank() == 0) {

		std::cout << "Overall time spend\n";
		t.report();
	}
	return 0;
}
MPILib::Rate RateFunction_1(MPILib::Time t){
	return 3000.;
}
MPILib::Rate RateFunction_2(MPILib::Time t){
	return t < 0.1 ? 1000. : 0.;
}


#include <boost/random.hpp>

#ifndef random_hpp_
#define random_hpp_

class O_Random
{
    public:
        typedef boost::mt19937 RandomGeneratorType;

        void set_seed(int seed);
        /* This function gets a random integer in the given range "lower" - "upper".
         *
         * Even though the get_rand_bool could just be a special case of this with "lower" = 0 and "upper"=2
         * it would be more complicated to assign the "prob" weight. Here we just have an unweighted
         * probability distribution.
         * THIS DOES NOT INCLUDE RETURNING THE UPPER BOUND i.e [lower,upper)
         */
        int get_rand_int( int lowerLimit, int upperLimit );

        double random01(void);
    private:
        RandomGeneratorType rg;
};

#endif


//==============================================================================
//         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 BOOST_SIMD_SWAR_FUNCTIONS_CUMSUM_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_CUMSUM_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/simd/operator/functions/plus.hpp>
#include <boost/simd/constant/constants/zero.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief cumsum generic tag

      Represents the cumsum function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct cumsum_ : ext::cumulative_<cumsum_, tag::plus_, tag::Zero>
    {
      /// @brief Parent hierarchy
      typedef ext::cumulative_<cumsum_, tag::plus_, tag::Zero> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_cumsum_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::cumsum_, Site> dispatching_cumsum_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::cumsum_, Site>();
    }
    template<class... Args>
    struct impl_cumsum_;
  }
  /*!
    compute the cumulate sum of the vector elements

    @par Semantic:

    For every parameter of type T0

    @code
    T0 r = cumsum(a0);
    @endcode

    is similar to:

    @code
    T r =x;
    for(int i=0;i < T::static_size; ++i)
      r[i] += r[i-1];
    @endcode

    @param a0

    @return a value of the same type as the second parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cumsum_, cumsum, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cumsum_, cumsum, 2)
} }

#endif


#include <boost/bind.hpp>
#include <boost/threadpool.hpp>

#include <atomic>
/*
 * T expected;
 * atomic(T &expected, T desir){
 *if(atomic == expected){
  atomic= desir;
  expected = atomic;
 retur true;
 }
 * ;
 * expected = atomic;
 * return false;

 */
template<typename T>
class lock_free_stack
{
private:
    struct node
    {
        T data;
        node* next;
        node(T const& data_):
            data(data_)
        {}
    };
 std::atomic<node*> m_head;
public:
    void push(T const& data)
    {
        node *nn = new node(data);
        nn->next = m_head.load();
        while(!m_head.compare_exchange_weak(nn->next, nn)){
            continue;
        }
    }
};
lock_free_stack<std::string> ss;
int main()
{

    ss.push("aaa"); ss.push("b"); ss.push("ccc");

    return 0;
}


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

  @copyright 2016 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_BOOLEAN_HPP_INCLUDED
#define BOOST_SIMD_BOOLEAN_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-boolean Boolean functions

      These functions provide SIMD algorithms for logical operations and branching
      in SIMD mode, their use is possible but not encouraged on scalars entries.

      All these functions can be included individually or all of them just by including <boost/simd/boolean.hpp>

      - **Logical operators**

        These functions return boost::simd::logical values, do not support short-circuiting
        and act in an elementwise way on SIMD vectors.

         |                     |                     |                   |
         |:-------------------:|:-------------------:|:-----------------:|
         | @ref logical_and    | @ref logical_andnot | @ref logical_not  |
         | @ref logical_notand | @ref logical_notor  | @ref logical_or   |
         | @ref logical_ornot  | @ref logical_xor    | @ref mask2logical |

        Masks are pattern of bits all zeros or all ones that can allow masking
        using bitwise operations. @ref mask2logical provides a way to transform a mask vector
        into a logical vector.

        Logical types are architecture dependant,  for instance on intel sse or avx
        logical types are mask valued simd vectors.

      - **Branching utilities**

        Be aware that using SIMD and branching are quite antagonist.

        For instance if a SIMD vector contains more than one element (!) a test on the
        elements will simultaneouly return true for some and false
        for others: so branching in simd may be described as 'compute all branches and select the
        pertinent results...'

        The functors listed above are of interest when one of the branches is much cheaper
        than the other.

        All branching utilities begin by if_ or ifnot_. The first parameter is a SIMD test
        pack which may be of any available type. This test pack is used to determine which branch
        will be selected for each element of the result vector. Non-zero elements are considered as true
        and zero elements as false. The result of each branch is given in the following parameters
        either implicitly or explicitly.

        These functors will generally compute all possible outcomes and then select the
        correct one. If each branch has the same cost (notwithstanding the cost of the test
        and the inner pipelining of the processor) the cost of the branching operation may
        be twice as expensive as the cost of evaluating one of the branches.
        It is for this reason that these functions are of greatest interest when the cardinal of
        the simd vector is greater than 2.

         |                             |                           |                          |                       |
         |:---------------------------:|:-------------------------:|:------------------------:| :--------------------:|
         | @ref if_allbits_else        | @ref if_allbits_else_zero | @ref if_dec              | @ref if_else_allbits  |
         | @ref if_else                | @ref if_else_nan          | @ref if_else_zero        | @ref if_inc           |
         | @ref if_minus               | @ref if_nan_else          | @ref if_neg              | @ref if_one_else_zero |
         | @ref if_plus                | @ref if_zero_else_allbits | @ref if_zero_else        | @ref if_zero_else_one |
         | @ref ifnot_dec              | @ref ifnot_inc            | @ref ifnot_minus         | @ref ifnot_neg        |
         | @ref ifnot_plus             |                           |                          |                       |

  **/

} }

#include <boost/simd/function/if_else.hpp>

#endif


// 生成随机地图，并模拟无人机检测，模拟得到局部地图与全局地图

#include <ros/ros.h>
#include <ros/console.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>
#include <random>
#include <tf/transform_broadcaster.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include "prometheus_msgs/DroneState.h"

using namespace std;

// 初始状态量
bool _map_ok = false;
bool _has_odom = false;
// 无人机初始位置
double _init_x, _init_y;
// 传感器感知半径与频率
double sense_range, sense_rate;
// 地图尺寸
double map_size_x, map_size_y, map_size_z;
// 地图xy范围
double map_x_0, map_x_1, map_y_0, map_y_1;
double _z_limit;
// 地图分辨率
double _resolution;
//  障碍物最小间隔
double _min_dist;
// 障碍物数量
int _pillar_num,_circle_num;
// 柱子形状范围
double  _w_l, _w_h, _h_l, _h_h;
// 圆形形状范围
double radius_l_, radius_h_, z_l_, z_h_;
double theta_;

random_device rd;
default_random_engine eng(rd());
// 产生障碍物随机位置
uniform_real_distribution<double> rand_x;
uniform_real_distribution<double> rand_y;
uniform_real_distribution<double> rand_z;
// 产生障碍物随机形状
uniform_real_distribution<double> rand_w;
uniform_real_distribution<double> rand_h;
uniform_real_distribution<double> rand_inf;
uniform_real_distribution<double> rand_radius;
uniform_real_distribution<double> rand_radius2;
uniform_real_distribution<double> rand_theta;

//无人机状态 [位置、速度、姿态]
ros::Subscriber drone_state_sub;
vector<double> _state;

// kdtree地图，为了索引方便？
pcl::KdTreeFLANN<pcl::PointXYZ> kdtreeLocalMap;
vector<int> pointIdxRadiusSearch;
vector<float> pointRadiusSquaredDistance;

// 障碍物点云地图
pcl::PointCloud<pcl::PointXYZ> cloudMap;
// 用于发布的局部点云和全局点云
sensor_msgs::PointCloud2 localMap_pcd;
sensor_msgs::PointCloud2 globalMap_pcd;

ros::Publisher local_map_pub;
ros::Publisher global_map_pub;

void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
  _has_odom = true;

  _state = { msg->position[0],
                     msg->position[1],
                    msg->position[2],
                    msg->velocity[0],
                     msg->velocity[1],
                    msg->velocity[2],
                    msg->attitude_q.w,
                     msg->attitude_q.x,
                    msg->attitude_q.y,
                    msg->attitude_q.z,
             0.0,
             0.0 };
}

void pubSensedPoints();
void RandomMapGenerate();

//  主函数
int main(int argc, char** argv)
{
  ros::init(argc, argv, "random_map_sensing");
  ros::NodeHandle n("~");

  // 订阅无人机位置信息
  drone_state_sub = n.subscribe("/prometheus/drone_state", 50, drone_state_cb);

  // 发布 模拟的局部点云
  local_map_pub = n.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/local_pcl_sim", 1);

  // 发布 模拟的全局点云
  global_map_pub = n.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/global_pcl_sim", 1);

  // 参数读取
  // 初始位置
  n.param("init_pos_x", _init_x, 0.0);
  n.param("init_pos_y", _init_y, 0.0);

  // 地图尺寸、分辨率
  n.param("map/x_size", map_size_x, 10.0);
  n.param("map/y_size", map_size_y, 10.0);
  n.param("map/z_size", map_size_z, 5.0);
  n.param("map/resolution", _resolution, 0.2);
  // 障碍物 - 两种障碍物：柱子与圆
  n.param("map/circle_num", _circle_num, 30);
  n.param("map/pillar_num", _pillar_num, 30);
  // 障碍物最小距离
  n.param("map/min_distance", _min_dist, 1.0);

  n.param("pillar_shape/min_radius", _w_l, 0.3);
  n.param("pillar_shape/max_radius", _w_h, 0.8);
  n.param("pillar_shape/min_height", _h_l, 3.0);
  n.param("pillar_shape/max_height", _h_h, 7.0);
  n.param("circle_shape/min_radius", radius_l_, 7.0);
  n.param("circle_shape/max_radius", radius_h_, 7.0);
  n.param("circle_shape/min_height", z_l_, 7.0);
  n.param("circle_shape/max_height", z_h_, 7.0);
  n.param("circle_shape/theta", theta_, 7.0);

  // 传感器感知半径与感知频率
  n.param("sensing/sense_range", sense_range, 10.0);
  n.param("sensing/sense_rate", sense_rate, 10.0);

  // 运行频率 = 传感器更新频率
  ros::Rate loop_rate(sense_rate);

  // 地图坐标范围 [map_x_0,  map_x_1] - [map_y_0, map_y_1]
  map_x_0 = -map_size_x / 2.0;
  map_x_1 = +map_size_x / 2.0;

  map_y_0 = -map_size_y / 2.0;
  map_y_1 = +map_size_y / 2.0;

  _z_limit = map_size_z;

  _pillar_num = min(_pillar_num, (int)map_size_x * 10);

  ros::Duration(1.0).sleep();

  //生成随机地图
  RandomMapGenerate();

  while (ros::ok())
  {
    // 更新无人机位置
    ros::spinOnce();

    // 发布感知数据
    pubSensedPoints();

    loop_rate.sleep();
  }
}

void RandomMapGenerate()
{
  pcl::PointXYZ pt_random;
  vector<Eigen::Vector2d> obs_position;

  // uniform_real_distribution函数：
  rand_x = uniform_real_distribution<double>(map_x_0, map_x_1);
  rand_y = uniform_real_distribution<double>(map_y_0, map_y_1);
  rand_w = uniform_real_distribution<double>(_w_l, _w_h);
  rand_h = uniform_real_distribution<double>(_h_l, _h_h);
  // 随机膨胀系数 暂时不使用
  rand_inf = uniform_real_distribution<double>(0.5, 1.5);

  rand_radius = uniform_real_distribution<double>(radius_l_, radius_h_);
  rand_radius2 = uniform_real_distribution<double>(radius_l_, 1.2);
  rand_theta = uniform_real_distribution<double>(-theta_, theta_);
  rand_z = uniform_real_distribution<double>(z_l_, z_h_);

  //  循环生成柱子障碍物
  for (int i = 0; i < _pillar_num; i++)
  {
    // 得到障碍物的随机xy位置和大小，高度
    double x, y, w, h;
    x = rand_x(eng);
    y = rand_y(eng);
    w = rand_w(eng);
    h = rand_h(eng);

    // 如果障碍物距离无人机初始位置 过近，则跳过
    if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < _min_dist)
      continue;

    // 如果障碍物与之前障碍物距离过近，则跳过
    bool flag_continue = false;
    for ( auto p : obs_position )
      if ( (Eigen::Vector2d(x,y) - p).norm() < _min_dist)
      {
        i--;
        flag_continue = true;
        break;
      }
    if ( flag_continue ) continue;

    obs_position.push_back( Eigen::Vector2d(x,y) );

    // 障碍物中心点坐标？
    x = floor(x / _resolution) * _resolution + _resolution / 2.0;
    y = floor(y / _resolution) * _resolution + _resolution / 2.0;

    // 障碍物占据的格子数
    int widNum = ceil(w / _resolution);
    double radius = (w) / 2;

    for (int r = -widNum / 2.0; r < widNum / 2.0; r++)
      for (int s = -widNum / 2.0; s < widNum / 2.0; s++)
      {
        int heiNum = ceil(h / _resolution);
        for (int t = -0; t < heiNum; t++)
        {
          double temp_x = x + (r + 0.5) * _resolution + 1e-2;
          double temp_y = y + (s + 0.5) * _resolution + 1e-2;
          double temp_z = (t + 0.5) * _resolution + 1e-2;
          if ( (Eigen::Vector2d(temp_x,temp_y) - Eigen::Vector2d(x,y)).norm() <= radius )
          {
            pt_random.x = temp_x;
            pt_random.y = temp_y;
            pt_random.z = temp_z;
            // 将障碍物点推入cloudMap
            cloudMap.points.push_back(pt_random);
          }
        }
      }
  }

  //  生成随机圆形障碍物
  for (int i = 0; i < _circle_num; ++i)
  {
    double x, y, z;
    x = rand_x(eng);
    y = rand_y(eng);
    z = rand_z(eng);

    // 坐标
    x = floor(x / _resolution) * _resolution + _resolution / 2.0;
    y = floor(y / _resolution) * _resolution + _resolution / 2.0;
    z = floor(z / _resolution) * _resolution + _resolution / 2.0;

    Eigen::Vector3d translate(x, y, z);

    // 偏转角度？
    double theta = rand_theta(eng);
    Eigen::Matrix3d rotate;
    rotate << cos(theta), -sin(theta), 0.0, sin(theta), cos(theta), 0.0, 0, 0,
        1;

    // 内、外径
    double radius1 = rand_radius(eng);
    double radius2 = rand_radius2(eng);

    // draw a circle centered at (x,y,z)
    Eigen::Vector3d cpt;
    for (double angle = 0.0; angle < 6.282; angle += _resolution / 2)
    {
      cpt(0) = 0.0;
      cpt(1) = radius1 * cos(angle);
      cpt(2) = radius2 * sin(angle);

      // inflate
      Eigen::Vector3d cpt_if;
      for (int ifx = -0; ifx <= 0; ++ifx)
        for (int ify = -0; ify <= 0; ++ify)
          for (int ifz = -0; ifz <= 0; ++ifz) {
            cpt_if = cpt + Eigen::Vector3d(ifx * _resolution, ify * _resolution,
                                           ifz * _resolution);
            cpt_if = rotate * cpt_if + Eigen::Vector3d(x, y, z);
            pt_random.x = cpt_if(0);
            pt_random.y = cpt_if(1);
            pt_random.z = cpt_if(2);
            cloudMap.push_back(pt_random);
          }
    }
  }

  cloudMap.width = cloudMap.points.size();
  cloudMap.height = 1;
  cloudMap.is_dense = true;

  ROS_WARN("Finished generate random map ");

  kdtreeLocalMap.setInputCloud(cloudMap.makeShared());

  _map_ok = true;
}

void pubSensedPoints()
{
  // 定时更新全局点云
  static int num = 0;

  if (num  == 10)
  {
    pcl::toROSMsg(cloudMap, globalMap_pcd);
    globalMap_pcd.header.frame_id = "world";
    global_map_pub.publish(globalMap_pcd);
    num=0;
  }else
  {
    num++;
  }

  if (!_map_ok || !_has_odom)
  {
    printf("[generate random map]: don't have the odom ");
    return;
  }

  pcl::PointCloud<pcl::PointXYZ> localMap;

  pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]);
  pointIdxRadiusSearch.clear();
  pointRadiusSquaredDistance.clear();

  pcl::PointXYZ pt;
  pcl::PointXYZ pt_new;

  if (isnan(searchPoint.x) || isnan(searchPoint.y) || isnan(searchPoint.z)){
    printf("[generate random map]: the odom state is bad, can't generate the local map!\n");
    return;
  }

Eigen::Vector3d p3d;
Eigen::Matrix<double, 3,3> rotaion_mat_global_to_local = Eigen::Quaterniond(_state[6],
                                                                                _state[7],
                                                                                _state[8],
                                                                                _state[9]).toRotationMatrix().transpose();

  if (kdtreeLocalMap.radiusSearch(searchPoint, sense_range, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
  {
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
    {
      pt = cloudMap.points[pointIdxRadiusSearch[i]];
      {
        p3d(0)=pt.x - searchPoint.x;
        p3d(1)=pt.y - searchPoint.y;
        p3d(2)=pt.z - searchPoint.z;
        p3d = rotaion_mat_global_to_local *(p3d);
        pt_new.x = p3d(0);
        pt_new.y = p3d(1);
        pt_new.z = p3d(2);
      }
      localMap.points.push_back(pt_new);
      // localMap.points.push_back(pt);
    }
  }
  else
  {
    ROS_ERROR("[Map server] No obstacles .");
    return;
  }

  localMap.width = localMap.points.size();
  localMap.height = 1;
  localMap.is_dense = true;

  // 发布局部点云,此处需要发布 base_link与 world的关系
  pcl::toROSMsg(localMap, localMap_pcd);
  localMap_pcd.header.frame_id = "base_link";
  local_map_pub.publish(localMap_pcd);

  //发布tf
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(_state[0], _state[1], _state[2]) );
  tf::Quaternion q;
  q.setRPY(0, 0, 0);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));

}


/*
 [auto_generated]
 libs/numeric/odeint/test/integrators_symplectic.cpp

 [begin_description]
 tba.
 [end_description]

 Copyright 2009-2013 Karsten Ahnert
 Copyright 2009-2013 Mario Mulansky

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

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_iterators_symplectic

#define BOOST_FUSION_INVOKE_MAX_ARITY 15
#define BOOST_RESULT_OF_NUM_ARGS 15
#define FUSION_MAX_VECTOR_SIZE 15

#include <cmath>

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/stepper/symplectic_euler.hpp>
#include <boost/numeric/odeint/stepper/symplectic_rkn_sb3a_mclachlan.hpp>
#include <boost/numeric/odeint/stepper/symplectic_rkn_sb3a_m4_mclachlan.hpp>
#include <boost/numeric/odeint/integrate/integrate_const.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;

typedef std::vector<double> container_type;

struct harm_osc {
    void operator()( const container_type &q , container_type &dpdt )
    {
        dpdt[0] = -q[0];
    }
};

class complete_steppers : public mpl::vector<
    symplectic_euler< container_type >
    , symplectic_rkn_sb3a_mclachlan< container_type >
    , symplectic_rkn_sb3a_m4_mclachlan< container_type >
    > {};

BOOST_AUTO_TEST_SUITE( symplectic_steppers_test )

BOOST_AUTO_TEST_CASE_TEMPLATE( test_integrate_const , Stepper , complete_steppers )
{
    container_type q( 1 , 1.0 ) , p( 1 , 0.0 );
    size_t steps = integrate_const( Stepper() , harm_osc() ,
                                    std::make_pair( boost::ref(q) , boost::ref(p) ) ,
                                    0.0 , 1.0 , 0.1 );
    BOOST_CHECK( steps == 10 );
    BOOST_CHECK_CLOSE( q[0] , std::cos(1.0) , 100*std::pow( 0.1 , Stepper::order_value ) );
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright © 2019 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <TensorUtils.hpp>

#include <armnn/Types.hpp>

#include <boost/test/unit_test.hpp>

using namespace armnn;
using namespace armnnUtils;

BOOST_AUTO_TEST_SUITE(TensorUtilsSuite)

BOOST_AUTO_TEST_CASE(ExpandDimsAxis0Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension 0
    armnn::TensorShape outputShape = ExpandDims(inputShape, 0);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 1);
    BOOST_TEST(outputShape[1] == 2);
    BOOST_TEST(outputShape[2] == 3);
    BOOST_TEST(outputShape[3] == 4);
}

BOOST_AUTO_TEST_CASE(ExpandDimsAxis1Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension 1
    armnn::TensorShape outputShape = ExpandDims(inputShape, 1);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 2);
    BOOST_TEST(outputShape[1] == 1);
    BOOST_TEST(outputShape[2] == 3);
    BOOST_TEST(outputShape[3] == 4);
}

BOOST_AUTO_TEST_CASE(ExpandDimsAxis2Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension 2
    armnn::TensorShape outputShape = ExpandDims(inputShape, 2);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 2);
    BOOST_TEST(outputShape[1] == 3);
    BOOST_TEST(outputShape[2] == 1);
    BOOST_TEST(outputShape[3] == 4);
}

BOOST_AUTO_TEST_CASE(ExpandDimsAxis3Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension 3
    armnn::TensorShape outputShape = ExpandDims(inputShape, 3);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 2);
    BOOST_TEST(outputShape[1] == 3);
    BOOST_TEST(outputShape[2] == 4);
    BOOST_TEST(outputShape[3] == 1);
}

BOOST_AUTO_TEST_CASE(ExpandDimsNegativeAxis1Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension -1
    armnn::TensorShape outputShape = ExpandDims(inputShape, -1);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 2);
    BOOST_TEST(outputShape[1] == 3);
    BOOST_TEST(outputShape[2] == 4);
    BOOST_TEST(outputShape[3] == 1);
}

BOOST_AUTO_TEST_CASE(ExpandDimsNegativeAxis2Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension -2
    armnn::TensorShape outputShape = ExpandDims(inputShape, -2);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 2);
    BOOST_TEST(outputShape[1] == 3);
    BOOST_TEST(outputShape[2] == 1);
    BOOST_TEST(outputShape[3] == 4);
}

BOOST_AUTO_TEST_CASE(ExpandDimsNegativeAxis3Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension -3
    armnn::TensorShape outputShape = ExpandDims(inputShape, -3);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 2);
    BOOST_TEST(outputShape[1] == 1);
    BOOST_TEST(outputShape[2] == 3);
    BOOST_TEST(outputShape[3] == 4);
}

BOOST_AUTO_TEST_CASE(ExpandDimsNegativeAxis4Test)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Expand dimension -4
    armnn::TensorShape outputShape = ExpandDims(inputShape, -4);
    BOOST_TEST(outputShape.GetNumDimensions() == 4);
    BOOST_TEST(outputShape[0] == 1);
    BOOST_TEST(outputShape[1] == 2);
    BOOST_TEST(outputShape[2] == 3);
    BOOST_TEST(outputShape[3] == 4);
}

BOOST_AUTO_TEST_CASE(ExpandDimsInvalidAxisTest)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Invalid expand dimension 4
    BOOST_CHECK_THROW(ExpandDims(inputShape, 4), armnn::InvalidArgumentException);
}

BOOST_AUTO_TEST_CASE(ExpandDimsInvalidNegativeAxisTest)
{
    armnn::TensorShape inputShape({ 2, 3, 4 });

    // Invalid expand dimension -5
    BOOST_CHECK_THROW(ExpandDims(inputShape, -5), armnn::InvalidArgumentException);
}

BOOST_AUTO_TEST_SUITE_END()