#pragma once
#include <Eigen/Dense>
#include <vector>
#include <array>
#include "shape2d.hpp"

using namespace Eigen;
using namespace std;

#ifndef LINE_INTERVAL_HPP
#define LINE_INTERVAL_HPP

class Shape2d;

array<double, 2> polarEquation(Vector2d v0, Vector2d v1);

class LineInterval{
    Vector2d point;
    double angleStart;
    double angleEnd;
    double distLowerBound;
    double distUpperBound;
    array<double, 2> intervalMaxDists;
    vector<unsigned long int> shapeIds;
  public:
    LineInterval(Vector2d _point);
    LineInterval(double _angleStart, double _angleEnd);
    //void calculateTargetShape(Shape2d& shape);
    //void calculateShape(Shape2d& shape);
    void SetAngleEnd(Vector2d _point);
    Vector2d Point();
    array<double, 3> FunctionsAt(array<double, 2> edge, int side);
    double DistAt(array<double, 2> edge, int side);
    bool containsNormal(array<double, 2> edge);
    double ApproxRoot(double distStart, double distEnd, double derivStart, double derivEnd);
    void update(double upperBound, double lowerBound, double distStart, double distEnd, unsigned long int shapeId);
    double UpperBound();
    double LowerBound();
    double MaxWidth();
    bool IntersectsEdge(double v0Angle, double v1Angle);
    //double IntervalAngleStart();
    double IntervalAngleEnd();
    array<double, 3> Divide();
    vector<unsigned long int> ShapeIds();
    array<Vector2d, 2> EndPoints();
};

#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2015 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "alpha_complex_3d"
#include <boost/test/unit_test.hpp>

#include <cmath>  // float comparison
#include <limits>
#include <string>
#include <vector>
#include <random>
#include <cstddef>  // for std::size_t

#include <gudhi/Alpha_complex_3d.h>
#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Unitary_tests_utils.h>
// to construct Alpha_complex from a OFF file of points
#include <gudhi/Points_3D_off_io.h>

#include <CGAL/Random.h>
#include <CGAL/point_generators_3.h>

using Fast_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::FAST, false, false>;
using Safe_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::SAFE, false, false>;
using Exact_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, false, false>;

template <typename Point>
std::vector<Point> get_points() {
  std::vector<Point> points;
  points.push_back(Point(0.0, 0.0, 0.0));
  points.push_back(Point(0.0, 0.0, 0.2));
  points.push_back(Point(0.2, 0.0, 0.2));
  points.push_back(Point(0.6, 0.6, 0.0));
  points.push_back(Point(0.8, 0.8, 0.2));
  points.push_back(Point(0.2, 0.8, 0.6));

  return points;
}

BOOST_AUTO_TEST_CASE(Alpha_complex_3d_from_points) {
  // -----------------
  // Fast version
  // -----------------
  std::cout << "Fast alpha complex 3d" << std::endl;

  std::vector<Fast_alpha_complex_3d::Bare_point_3> points = get_points<Fast_alpha_complex_3d::Bare_point_3>();
  Fast_alpha_complex_3d alpha_complex(points);

  Gudhi::Simplex_tree<> stree;
  alpha_complex.create_complex(stree);

  for (std::size_t index = 0; index < points.size(); index++) {
    bool found = false;
    for (auto point : points) {
      if (point == alpha_complex.get_point(index)) {
        found = true;
        break;
      }
    }
    // Check all points from alpha complex are found in the input point cloud
    BOOST_CHECK(found);
  }
  // Exception if we go out of range
  BOOST_CHECK_THROW(alpha_complex.get_point(points.size()), std::out_of_range);

  // -----------------
  // Exact version
  // -----------------
  std::cout << "Exact alpha complex 3d" << std::endl;

  std::vector<Exact_alpha_complex_3d::Bare_point_3> exact_points = get_points<Exact_alpha_complex_3d::Bare_point_3>();
  Exact_alpha_complex_3d exact_alpha_complex(exact_points);

  Gudhi::Simplex_tree<> exact_stree;
  exact_alpha_complex.create_complex(exact_stree);

  for (std::size_t index = 0; index < exact_points.size(); index++) {
    bool found = false;
    Exact_alpha_complex_3d::Bare_point_3 ap = exact_alpha_complex.get_point(index);
    for (auto point : points) {
      if ((point.x() == ap.x()) && (point.y() == ap.y()) && (point.z() == ap.z())) {
        found = true;
        break;
      }
    }
    // Check all points from alpha complex are found in the input point cloud
    BOOST_CHECK(found);
  }
  // Exception if we go out of range
  BOOST_CHECK_THROW(exact_alpha_complex.get_point(exact_points.size()), std::out_of_range);

  // ---------------------
  // Compare both versions
  // ---------------------
  std::cout << "Exact Alpha complex 3d is of dimension " << exact_stree.dimension() << " - Fast is "
            << stree.dimension() << std::endl;
  BOOST_CHECK(exact_stree.dimension() == stree.dimension());
  std::cout << "Exact Alpha complex 3d num_simplices " << exact_stree.num_simplices() << " - Fast is "
            << stree.num_simplices() << std::endl;
  BOOST_CHECK(exact_stree.num_simplices() == stree.num_simplices());
  std::cout << "Exact Alpha complex 3d num_vertices " << exact_stree.num_vertices() << " - Fast is "
            << stree.num_vertices() << std::endl;
  BOOST_CHECK(exact_stree.num_vertices() == stree.num_vertices());

  auto sh = stree.filtration_simplex_range().begin();
  while (sh != stree.filtration_simplex_range().end()) {
    std::vector<int> simplex;
    std::vector<int> exact_simplex;
    std::cout << "Fast ( ";
    for (auto vertex : stree.simplex_vertex_range(*sh)) {
      simplex.push_back(vertex);
      std::cout << vertex << " ";
    }
    std::cout << ") -> [" << stree.filtration(*sh) << "] ";

    // Find it in the exact structure
    auto sh_exact = exact_stree.find(simplex);
    BOOST_CHECK(sh_exact != exact_stree.null_simplex());

    std::cout << " versus [" << exact_stree.filtration(sh_exact) << "] " << std::endl;
    // Exact and non-exact version is not exactly the same due to float comparison
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(exact_stree.filtration(sh_exact), stree.filtration(*sh));

    ++sh;
  }
  // -----------------
  // Safe version
  // -----------------
  std::cout << "Safe alpha complex 3d" << std::endl;

  std::vector<Safe_alpha_complex_3d::Bare_point_3> safe_points = get_points<Safe_alpha_complex_3d::Bare_point_3>();
  Safe_alpha_complex_3d safe_alpha_complex(safe_points);

  Gudhi::Simplex_tree<> safe_stree;
  safe_alpha_complex.create_complex(safe_stree);

  for (std::size_t index = 0; index < safe_points.size(); index++) {
    bool found = false;
    Safe_alpha_complex_3d::Bare_point_3 ap = safe_alpha_complex.get_point(index);
    for (auto point : points) {
      if ((point.x() == ap.x()) && (point.y() == ap.y()) && (point.z() == ap.z())) {
        found = true;
        break;
      }
    }
    // Check all points from alpha complex are found in the input point cloud
    BOOST_CHECK(found);
  }
  // Exception if we go out of range
  BOOST_CHECK_THROW(safe_alpha_complex.get_point(safe_points.size()), std::out_of_range);

  // ---------------------
  // Compare both versions
  // ---------------------
  std::cout << "Safe Alpha complex 3d is of dimension " << safe_stree.dimension() << " - Fast is "
            << stree.dimension() << std::endl;
  BOOST_CHECK(safe_stree.dimension() == stree.dimension());
  std::cout << "Safe Alpha complex 3d num_simplices " << safe_stree.num_simplices() << " - Fast is "
            << stree.num_simplices() << std::endl;
  BOOST_CHECK(safe_stree.num_simplices() == stree.num_simplices());
  std::cout << "Safe Alpha complex 3d num_vertices " << safe_stree.num_vertices() << " - Fast is "
            << stree.num_vertices() << std::endl;
  BOOST_CHECK(safe_stree.num_vertices() == stree.num_vertices());

  auto safe_sh = stree.filtration_simplex_range().begin();
  while (safe_sh != stree.filtration_simplex_range().end()) {
    std::vector<int> simplex;
    std::vector<int> exact_simplex;
    std::cout << "Fast ( ";
    for (auto vertex : stree.simplex_vertex_range(*safe_sh)) {
      simplex.push_back(vertex);
      std::cout << vertex << " ";
    }
    std::cout << ") -> [" << stree.filtration(*safe_sh) << "] ";

    // Find it in the exact structure
    auto sh_exact = safe_stree.find(simplex);
    BOOST_CHECK(sh_exact != safe_stree.null_simplex());

    std::cout << " versus [" << safe_stree.filtration(sh_exact) << "] " << std::endl;
    // Exact and non-exact version is not exactly the same due to float comparison
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(safe_stree.filtration(sh_exact), stree.filtration(*safe_sh), 1e-15);

    ++safe_sh;
  }
}


#include "Day03-HowYouSliceIt.h"

#include "OverlapGrid.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace
{
const size_t GRID_SIZE = 1001;
}

namespace AdventOfCode
{
namespace Year2018
{
namespace Day03
{

OverlapGrid parseOverlapGridFromAreaLines(const std::vector<std::string>& areaLines)
{
    OverlapGrid overlapGrid{GRID_SIZE};

    for (const auto& line : areaLines)
    {
        std::vector<std::string> tokens;
        boost::split(tokens, line, boost::is_any_of(" @,:x"), boost::token_compress_on);

        if (tokens.size() < 5)
        {
            throw std::runtime_error("Not enough tokens.");
        }

        const unsigned topLeftX = boost::lexical_cast<unsigned>(tokens[1]);
        const unsigned topLeftY = boost::lexical_cast<unsigned>(tokens[2]);
        const unsigned width = boost::lexical_cast<unsigned>(tokens[3]);
        const unsigned height = boost::lexical_cast<unsigned>(tokens[4]);

        Rectangle r{topLeftX, topLeftY, width, height};

        overlapGrid.addRectangle(std::move(r));
    }

    return overlapGrid;
}

unsigned numOverlappingSquares(const std::vector<std::string>& areaLines)
{
    OverlapGrid OverlapGrid = parseOverlapGridFromAreaLines(areaLines);
    return OverlapGrid.getOverlapSize();
}

unsigned findSingleNonOperlappingSquare(const std::vector<std::string>& areaLines)
{
    OverlapGrid OverlapGrid = parseOverlapGridFromAreaLines(areaLines);
    return OverlapGrid.getSingleNonOverlappingIndex() + 1;
}

}
}
}


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

#include "gtest/gtest.h"
#include <traact/util/Logging.h>
#include "spdlog/sinks/stdout_color_sinks.h"

#include <traact/math/perspective.h>
#include <traact/vision.h>
#include <traact/spatial.h>
#include <Eigen/Dense>
#include <math.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

Eigen::Vector3d test_point(const std::vector<Eigen::Affine3d>&  cam_2_world, const std::vector<traact::vision::CameraCalibration>& calibrations, const Eigen::Vector3d& test_position, const double noise=0) {
    using namespace traact::math;
    using namespace traact::vision;
    using namespace Eigen;

    std::vector<Vector2d> image_points;
    for(int i=0;i<cam_2_world.size();++i){
        Vector2d point = reproject_point(cam_2_world[i], calibrations[i], test_position);
        Vector2d pixel_noise;
        pixel_noise.setRandom();
        pixel_noise *= noise;
        point += pixel_noise;

        image_points.push_back(point);
    }

    Vector3d p3_result;

    estimate_3d_point(p3_result,cam_2_world, calibrations, image_points);

    return p3_result;

}

TEST(TraactVisionTestSuite, Estimate3dPointTest_NoDistortion) {

    using namespace traact::math;
    using namespace traact::vision;
    using namespace Eigen;
    CameraCalibration calibration;
    calibration.width = 640;
    calibration.height = 576;
    calibration.fx = 345.62296;
    calibration.fy = 359.71362;
    calibration.skew = 0;
    calibration.cx = 326.55453;
    calibration.cy = 349.54202;

    {
        Affine3d marker_pose;
        std::vector<Affine3d>  cam_2_world;
        marker_pose = Translation3d(0.1,-0.5,-3);// * AngleAxisd(M_PI, Vector3d::UnitY());

        cam_2_world.push_back(Affine3d::Identity());
        cam_2_world.push_back(Translation3d(-0.1,0,0) * AngleAxisd::Identity());

        std::vector<CameraCalibration> calibrations;
        for(int i=0;i<cam_2_world.size();++i){
            calibrations.push_back(calibration);
        }

        Vector3d p3_result = test_point(cam_2_world, calibrations, marker_pose.translation(), 0);
        EXPECT_NEAR(marker_pose.translation().x(), p3_result.x(), 1e-9);
        EXPECT_NEAR(marker_pose.translation().y(), p3_result.y(), 1e-9);
        EXPECT_NEAR(marker_pose.translation().z(), p3_result.z(), 1e-9);
    }

    {
        Affine3d marker_pose;
        std::vector<Affine3d>  cam_2_world;
        marker_pose = Translation3d(0.1,-0.5,-3);// * AngleAxisd(M_PI, Vector3d::UnitY());

        {
            Affine3d pose_c2w;
            pose_c2w = Translation3d(-2.13208468683971208e+00, -1.20638214730114912e+00, 2.30451254078307244e+00);
            pose_c2w.rotate(Quaterniond(-8.49694826269651537e-01,-3.46199513669736558e-01, 1.29443829434892799e-01, 3.76043739432868451e-01));
            cam_2_world.push_back(pose_c2w);
        }

        {
            Affine3d pose_c2w;
            pose_c2w = Translation3d(-2.24341394331049226e+00, 2.91498703341871979e+00, 2.19838501838564992e+00);
            pose_c2w.rotate(Quaterniond( 4.53584848555412923e-01, 1.66481146382847056e-01, -3.40200132181601056e-01, -8.06727142919858475e-01 ));
            cam_2_world.push_back(pose_c2w);
        }

        std::vector<CameraCalibration> calibrations;
        for(int i=0;i<cam_2_world.size();++i){
            calibrations.push_back(calibration);
        }

        Vector3d p3_result = test_point(cam_2_world, calibrations, marker_pose.translation(), 0);
        EXPECT_NEAR(marker_pose.translation().x(), p3_result.x(), 1e-9);
        EXPECT_NEAR(marker_pose.translation().y(), p3_result.y(), 1e-9);
        EXPECT_NEAR(marker_pose.translation().z(), p3_result.z(), 1e-9);
    }

}

/*
TEST(TraactVisionTestSuite, Estimate3dPointTestNoise_NoDistortion) {
    using namespace traact::math;
    using namespace traact::vision;
    using namespace Eigen;
    CameraCalibration calibration;
    calibration.width = 640;
    calibration.height = 480;
    calibration.fx = 500;
    calibration.fy = 500;
    calibration.skew = 0;
    calibration.cx = calibration.width / 2.0 - 0.5;
    calibration.cy = calibration.height / 2.0 - 0.5;

    Affine3d marker_pose;
    std::vector<Affine3d>  world_2_cam;
    std::vector<Affine3d>  cam_2_world;
    marker_pose = Translation3d(0.1,-0.5,-3);// * AngleAxisd(M_PI, Vector3d::UnitY());

    world_2_cam.push_back(Affine3d::Identity());
    world_2_cam.push_back(Translation3d(-0.1,0,0) * AngleAxisd::Identity());

    std::vector<CameraCalibration> calibrations;
    for(int i=0;i<world_2_cam.size();++i){
        calibrations.push_back(calibration);
        cam_2_world.push_back(world_2_cam[i].inverse());
    }

    typedef Matrix< double, Dynamic, 1, ColMajor > EVector;
    Matrix< double, 100, 1, ColMajor > diff_values;

    for(int i=0;i<11;++i) {
        double noise = 10.0/100.0 * i;

        double sum=0;
        const int test_size = 1000;
        for(int j=0;j<test_size;++j) {
            Vector3d p3_result = test_point(cam_2_world, calibrations, marker_pose.translation(), noise);
            Vector3d diff = p3_result - marker_pose.translation();
            sum += diff.norm()*1000;
        }

        SPDLOG_INFO("2 observations, noise of {0} pixel : error {1}mm", noise, sum/test_size);
    }

    world_2_cam.push_back(Translation3d(0,0,-6) *  AngleAxisd(M_PI, Vector3d::UnitY()));
    world_2_cam.push_back(Translation3d(-0.1,0,-6) *  AngleAxisd(M_PI, Vector3d::UnitY()));

    calibrations.clear();
    cam_2_world.clear();
    for(int i=0;i<world_2_cam.size();++i){
        calibrations.push_back(calibration);
        cam_2_world.push_back(world_2_cam[i].inverse());
    }

    for(int i=0;i<11;++i) {
        double noise = 10.0/100.0 * i;

        double sum=0;
        const int test_size = 1000;
        for(int j=0;j<test_size;++j) {
            Vector3d p3_result = test_point(cam_2_world, calibrations, marker_pose.translation(), noise);
            Vector3d diff = p3_result - marker_pose.translation();
            sum += diff.norm()*1000;
        }

        SPDLOG_INFO("4 observations, noise of {0} pixel : error {1}mm", noise, sum/test_size);
    }

}
 */


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <algorithm>
#include <vector>
#include "gtest/gtest.h"

#include "theia/alignment/alignment.h"
#include "theia/math/util.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/transformation/gdls_similarity_transform.h"
#include "theia/util/random.h"
#include "theia/util/util.h"

namespace theia {
namespace {
using Eigen::AngleAxisd;
using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;

RandomNumberGenerator rng(57);

void TestGdlsSimilarityTransformWithNoise(
    const std::vector<Vector3d>& camera_centers,
    const std::vector<Vector3d>& world_points,
    const double projection_noise_std_dev,
    const Quaterniond& expected_rotation,
    const Vector3d& expected_translation,
    const double expected_scale,
    const double max_reprojection_error,
    const double max_rotation_difference,
    const double max_translation_difference,
    const double max_scale_difference) {
  const int num_points = world_points.size();
  const int num_cameras = camera_centers.size();

  std::vector<Vector3d> camera_rays;
  std::vector<Vector3d> ray_origins;
  camera_rays.reserve(num_points);
  ray_origins.reserve(num_points);
  for (int i = 0; i < num_points; i++) {
    Vector3d ray_origin = (expected_rotation * camera_centers[i % num_cameras] +
                           expected_translation) / expected_scale;

    ray_origins.push_back(ray_origin);

    // Reproject 3D points into camera frame.
    camera_rays.push_back(
        (expected_rotation * world_points[i] + expected_translation -
         expected_scale * ray_origins[i]).normalized());
  }

  if (projection_noise_std_dev) {
    // Adds noise to both of the rays.
    for (int i = 0; i < num_points; i++) {
      AddNoiseToRay(projection_noise_std_dev, &rng, &camera_rays[i]);
    }
  }

  // Run DLS Similarity Transform.
  std::vector<Quaterniond> soln_rotation;
  std::vector<Vector3d> soln_translation;
  std::vector<double> soln_scale;
  GdlsSimilarityTransform(ray_origins, camera_rays, world_points,
                          &soln_rotation, &soln_translation, &soln_scale);

  // Check solutions and verify at least one is close to the actual solution.

  const int num_solutions = soln_rotation.size();
  EXPECT_GT(num_solutions, 0);
  bool matched_transform = false;
  for (int i = 0; i < num_solutions; i++) {
    // Check that reprojection errors are small.
    double max_reproj_err = 0.0;
    for (int j = 0; j < num_points; j++) {
      const Quaterniond unrot =
          Quaterniond::FromTwoVectors(camera_rays[j], Vector3d(0, 0, 1));
      const Vector3d reprojected_point =
          (soln_rotation[i] * world_points[j] + soln_translation[i]) /
              soln_scale[i] - ray_origins[j];

      const Vector3d unrot_cam_ray = unrot * camera_rays[j];
      const Vector3d unrot_reproj_pt = unrot * reprojected_point;

      const double reprojection_error =
          (unrot_cam_ray.hnormalized() - unrot_reproj_pt.hnormalized()).norm();
      if (reprojection_error > max_reproj_err)
        max_reproj_err = reprojection_error;
      EXPECT_LE(reprojection_error, max_reprojection_error)
          << "Reproj error is " << reprojection_error * 512.0;
    }

    // Check that the solution is accurate.
    const double rotation_difference =
        expected_rotation.angularDistance(soln_rotation[i]);
    const bool matched_rotation =
        (rotation_difference < max_rotation_difference);
    const double translation_difference =
        (expected_translation - soln_translation[i]).squaredNorm();
    const bool matched_translation =
        (translation_difference < max_translation_difference);
    const double scale_difference =
        fabs(expected_scale - soln_scale[i]) / expected_scale;
    const bool matched_scale = (scale_difference < max_scale_difference);

    if (matched_translation && matched_rotation && matched_scale) {
      matched_transform = true;
    }
  }
  EXPECT_TRUE(matched_transform);
}

void BasicTest() {
  const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                            Vector3d(1.0, -1.0, 2.0),
                                            Vector3d(-1.0, 1.0, 2.0),
                                            Vector3d(2.0, 1.0, 3.0) };
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double soln_scale = 2.5;
  const double kNoise = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const double kMaxAllowedScaleDifference = 1e-6;

  const std::vector<Vector3d> kImageOrigins = { Vector3d(-1.0, 0.0, 0.0),
                                                Vector3d(0.0, 0.0, 0.0),
                                                Vector3d(2.0, 0.0, 0.0),
                                                Vector3d(3.0, 0.0, 0.0) };

  TestGdlsSimilarityTransformWithNoise(
      kImageOrigins,
      points_3d,
      kNoise,
      soln_rotation,
      soln_translation,
      soln_scale,
      kMaxReprojectionError,
      kMaxAllowedRotationDifference,
      kMaxAllowedTranslationDifference,
      kMaxAllowedScaleDifference);
}

TEST(GdlsSimilarityTransform, Basic) {
  BasicTest();
}

TEST(GdlsSimilarityTransform, NoiseTest) {
  const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                            Vector3d(1.0, -1.0, 2.0),
                                            Vector3d(-1.0, 1.0, 2.0),
                                            Vector3d(2.0, 1.0, 3.0),
                                            Vector3d(-1.0, -3.0, 2.0),
                                            Vector3d(1.0, -2.0, 1.0),
                                            Vector3d(-1.0, 4.0, 2.0),
                                            Vector3d(-2.0, 2.0, 3.0)
  };
  const std::vector<Vector3d> kImageOrigins = { Vector3d(0.0, 1.0, 0.0),
                                                Vector3d(0.0, 0.0, 0.0),
                                                Vector3d(0.0, 2.0, 0.0),
                                                Vector3d(0.0, 3.0, 0.0) };

  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double soln_scale = 2.5;
  const double kNoise = 1.0 / 512.0;
  const double kMaxReprojectionError = 5.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1.0);
  const double kMaxAllowedTranslationDifference = 1e-3;
  const double kMaxAllowedScaleDifference = 1e-2;

  TestGdlsSimilarityTransformWithNoise(
      kImageOrigins,
      points_3d,
      kNoise,
      soln_rotation,
      soln_translation,
      soln_scale,
      kMaxReprojectionError,
      kMaxAllowedRotationDifference,
      kMaxAllowedTranslationDifference,
      kMaxAllowedScaleDifference);
}

TEST(GdlsSimilarityTransform, ManyPoints) {
  // Sets some test rotations and translations.
  static const Vector3d kAxes[] = {
    Vector3d(0.0, 0.0, 1.0).normalized(),
    Vector3d(0.0, 1.0, 0.0).normalized(),
    Vector3d(1.0, 0.0, 0.0).normalized(),
    Vector3d(1.0, 0.0, 1.0).normalized(),
    Vector3d(0.0, 1.0, 1.0).normalized(),
    Vector3d(1.0, 1.0, 1.0).normalized(),
    Vector3d(0.0, 1.0, 1.0).normalized(),
    Vector3d(1.0, 1.0, 1.0).normalized()
  };

  static const double kRotationAngles[THEIA_ARRAYSIZE(kAxes)] = {
    DegToRad(7.0),
    DegToRad(12.0),
    DegToRad(15.0),
    DegToRad(20.0),
    DegToRad(11.0),
    DegToRad(0.0),  // Tests no rotation.
    DegToRad(5.0),
    DegToRad(0.0)  // Tests no rotation and no translation.
  };

  static const Vector3d kTranslations[THEIA_ARRAYSIZE(kAxes)] = {
    Vector3d(1.0, 1.0, 1.0),
    Vector3d(3.0, 2.0, 13.0),
    Vector3d(4.0, 5.0, 11.0),
    Vector3d(1.0, 2.0, 15.0),
    Vector3d(3.0, 1.5, 18.0),
    Vector3d(1.0, 7.0, 11.0),
    Vector3d(0.0, 0.0, 0.0),  // Tests no translation.
    Vector3d(0.0, 0.0, 0.0)  // Tests no translation and no rotation.
  };

  static const double kScales[THEIA_ARRAYSIZE(kAxes)] = {
    0.33,
    1.0,
    4.2,
    10.13,
    3.14,
    7.22,
    0.1,
    10.0
  };

  const std::vector<Vector3d> kImageOrigins = { Vector3d(-1.0, 0.0, 0.0),
                                                Vector3d(0.0, 0.0, 0.0),
                                                Vector3d(2.0, 0.0, 0.0),
                                                Vector3d(3.0, 0.0, 0.0) };

  static const int num_points[3] = { 100, 500, 1000 };
  const double kNoise = 0.5 / 512.0;
  const double kMaxReprojectionError = 10.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(0.3);
  const double kMaxAllowedTranslationDifference = 1e-3;
  const double kMaxAllowedScaleDifference = 1e-2;

  for (int i = 0; i < THEIA_ARRAYSIZE(kAxes); i++) {
    const Quaterniond soln_rotation(AngleAxisd(kRotationAngles[i], kAxes[i]));
    for (int j = 0; j < THEIA_ARRAYSIZE(num_points); j++) {
      std::vector<Vector3d> points_3d;
      points_3d.reserve(num_points[j]);
      for (int k = 0; k < num_points[j]; k++) {
        points_3d.push_back(Vector3d(rng.RandDouble(-5.0, 5.0),
                                     rng.RandDouble(-5.0, 5.0),
                                     rng.RandDouble(2.0, 10.0)));
      }

      TestGdlsSimilarityTransformWithNoise(
          kImageOrigins,
          points_3d,
          kNoise,
          soln_rotation,
          kTranslations[i],
          kScales[i],
          kMaxReprojectionError,
          kMaxAllowedRotationDifference,
          kMaxAllowedTranslationDifference,
          kMaxAllowedScaleDifference);
    }
  }
}

TEST(GdlsSimilarityTransform, NoRotation) {
  const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                            Vector3d(1.0, -1.0, 2.0),
                                            Vector3d(-1.0, 1.0, 2.0),
                                            Vector3d(2.0, 1.0, 3.0),
                                            Vector3d(-1.0, -3.0, 2.0),
                                            Vector3d(1.0, -2.0, 1.0),
                                            Vector3d(-1.0, 4.0, 2.0),
                                            Vector3d(-2.0, 2.0, 3.0)
  };
  const std::vector<Vector3d> kImageOrigins = { Vector3d(-1.0, 0.0, 0.0),
                                                Vector3d(0.0, 0.0, 0.0),
                                                Vector3d(2.0, 0.0, 0.0),
                                                Vector3d(3.0, 0.0, 0.0) };

  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(0.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double soln_scale = 0.77;
  const double kNoise = 0.5 / 512.0;
  const double kMaxReprojectionError = 2.0 / 5.0;
  const double kMaxAllowedRotationDifference = DegToRad(0.2);
  const double kMaxAllowedTranslationDifference = 5e-4;
  const double kMaxAllowedScaleDifference = 1e-2;

  TestGdlsSimilarityTransformWithNoise(
      kImageOrigins,
      points_3d,
      kNoise,
      soln_rotation,
      soln_translation,
      soln_scale,
      kMaxReprojectionError,
      kMaxAllowedRotationDifference,
      kMaxAllowedTranslationDifference,
      kMaxAllowedScaleDifference);
}

TEST(GdlsSimilarityTransform, NoTranslation) {
  const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                            Vector3d(1.0, -1.0, 2.0),
                                            Vector3d(-1.0, 1.0, 2.0),
                                            Vector3d(2.0, 1.0, 3.0),
                                            Vector3d(-1.0, -3.0, 2.0),
                                            Vector3d(1.0, -2.0, 1.0),
                                            Vector3d(-1.0, 4.0, 2.0),
                                            Vector3d(-2.0, 2.0, 3.0)
  };
  const std::vector<Vector3d> kImageOrigins = { Vector3d(-1.0, 0.0, 0.0),
                                                Vector3d(0.0, 0.0, 0.0),
                                                Vector3d(2.0, 0.0, 0.0),
                                                Vector3d(3.0, 0.0, 0.0) };

  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  const double soln_scale = 2.5;
  const double kNoise = 1.0 / 512.0;
  const double kMaxReprojectionError = 4.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(0.5);
  const double kMaxAllowedTranslationDifference = 1e-3;
  const double kMaxAllowedScaleDifference = 1e-2;

  TestGdlsSimilarityTransformWithNoise(
      kImageOrigins,
      points_3d,
      kNoise,
      soln_rotation,
      soln_translation,
      soln_scale,
      kMaxReprojectionError,
      kMaxAllowedRotationDifference,
      kMaxAllowedTranslationDifference,
      kMaxAllowedScaleDifference);
}

}  // namespace
}  // namespace theia


/**
 * @author Andre Anjos <andre.anjos@idiap.ch>
 * @date Sun 27 Oct 09:02:32 2013
 *
 * @brief Normal distributions (with integers or floating point numbers)
 */

#define BOB_CORE_RANDOM_MODULE
#include <bob.core/random_api.h>
#include <bob.blitz/cppapi.h>
#include <bob.blitz/cleanup.h>
#include <bob.extension/documentation.h>
#include <boost/make_shared.hpp>

#include <bob.core/random.h>

static auto normal_doc = bob::extension::ClassDoc(
  BOB_EXT_MODULE_PREFIX ".normal",
  "Models a random normal distribution",
  "This distribution produces random numbers :math:`x` distributed with the probability density function\n\n"
  ".. math::\n\n   p(x) = \\frac{1}{\\sqrt{2\\pi\\sigma}} e^{-\\frac{(x-\\mu)^2}{2\\sigma^2}}\n\n"
  "where the ``mean`` (:math:`\\mu`) and ``sigma`` (:math:`\\sigma`, the standard deviation) are the parameters of this distribution class."
)
.add_constructor(bob::extension::FunctionDoc(
  "normal",
  "Constructs a new normal distribution object"
)
.add_prototype("dtype, [mean], [sigma]", "")
.add_parameter("dtype", ":py:class:`numpy.dtype` or anything that converts to a dtype", "The data type to get the distribution for; only real-valued types are supported")
.add_parameter("mean", "dtype", "[Default: 0.] The mean :math:`\\mu` of the normal distibution")
.add_parameter("sigma", "dtype", "[Default: 1.] The standard deviation :math:`\\sigma` of the normal distributiuon")
);

/* How to create a new PyBoostNormalObject */
static PyObject* PyBoostNormal_New(PyTypeObject* type, PyObject*, PyObject*) {

  /* Allocates the python object itself */
  PyBoostNormalObject* self = (PyBoostNormalObject*)type->tp_alloc(type, 0);
  self->type_num = NPY_NOTYPE;
  self->distro.reset();

  return Py_BuildValue("N", self);
}

/* How to delete a PyBoostNormalObject */
static void PyBoostNormal_Delete (PyBoostNormalObject* o) {
  o->distro.reset();
  Py_TYPE(o)->tp_free((PyObject*)o);
}

template <typename T>
boost::shared_ptr<void> make_normal(PyObject* mean, PyObject* sigma) {
  T cmean = 0.;
  if (mean) cmean = PyBlitzArrayCxx_AsCScalar<T>(mean);
  T csigma = 1.;
  if (sigma) csigma = PyBlitzArrayCxx_AsCScalar<T>(sigma);
  return boost::make_shared<bob::core::random::normal_distribution<T>>(cmean, csigma);
}

PyObject* PyBoostNormal_SimpleNew (int type_num, PyObject* mean, PyObject* sigma) {
BOB_TRY
  PyBoostNormalObject* retval = (PyBoostNormalObject*)PyBoostNormal_New(&PyBoostNormal_Type, 0, 0);
  if (!retval) return 0;
  auto retval_ = make_safe(retval);

  retval->type_num = type_num;

  switch(type_num) {
    case NPY_FLOAT32:
      retval->distro = make_normal<float>(mean, sigma);
      break;
    case NPY_FLOAT64:
      retval->distro = make_normal<double>(mean, sigma);
      break;
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot create %s(T) with T having an unsupported numpy type number of %d (it only supports numpy.float32 or numpy.float64)", Py_TYPE(retval)->tp_name, retval->type_num);
      return 0;
  }

  if (!retval->distro) { // a problem occurred
    return 0;
  }

  return Py_BuildValue("O", retval);
BOB_CATCH_FUNCTION("SimpleNew", 0)
}

/* Implements the __init__(self) function */
static int PyBoostNormal_Init(PyBoostNormalObject* self, PyObject *args, PyObject* kwds) {
BOB_TRY
  char** kwlist = normal_doc.kwlist();

  PyObject* mean = 0;
  PyObject* sigma = 0;

  if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&|OO", kwlist, &PyBlitzArray_TypenumConverter, &self->type_num, &mean, &sigma)) return -1; ///< FAILURE

  switch(self->type_num) {
    case NPY_FLOAT32:
      self->distro = make_normal<float>(mean, sigma);
      break;
    case NPY_FLOAT64:
      self->distro = make_normal<double>(mean, sigma);
      break;
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot create %s(T) with T having an unsupported numpy type number of %d (it only supports numpy.float32 or numpy.float64)", Py_TYPE(self)->tp_name, self->type_num);
      return -1;
  }

  if (!self->distro) { // a problem occurred
    return -1;
  }

  return 0; ///< SUCCESS
BOB_CATCH_MEMBER("constructor", -1)
}

int PyBoostNormal_Check(PyObject* o) {
  if (!o) return 0;
  return PyObject_IsInstance(o, reinterpret_cast<PyObject*>(&PyBoostNormal_Type));
}

int PyBoostNormal_Converter(PyObject* o, PyBoostNormalObject** a) {
  if (!PyBoostNormal_Check(o)) return 0;
  Py_INCREF(o);
  (*a) = reinterpret_cast<PyBoostNormalObject*>(o);
  return 1;
}

static auto mean_doc = bob::extension::VariableDoc(
  "mean",
  "dtype",
  "The mean value the distribution will produce."
);
template <typename T> PyObject* get_mean(PyBoostNormalObject* self) {
  return PyBlitzArrayCxx_FromCScalar(boost::static_pointer_cast<bob::core::random::normal_distribution<T>>(self->distro)->mean());
}

static PyObject* PyBoostNormal_GetMean(PyBoostNormalObject* self) {
BOB_TRY
  switch (self->type_num) {
    case NPY_FLOAT32:
      return get_mean<float>(self);
    case NPY_FLOAT64:
      return get_mean<double>(self);
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot get mean of %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
      return 0;
  }
BOB_CATCH_MEMBER("mean", 0)
}

static auto sigma_doc = bob::extension::VariableDoc(
  "sigma",
  "dtype",
  "The standard deviation the distribution will have"
);
template <typename T> PyObject* get_sigma(PyBoostNormalObject* self) {
  return PyBlitzArrayCxx_FromCScalar(boost::static_pointer_cast<bob::core::random::normal_distribution<T>>(self->distro)->sigma());
}

static PyObject* PyBoostNormal_GetSigma(PyBoostNormalObject* self) {
BOB_TRY
  switch (self->type_num) {
    case NPY_FLOAT32:
      return get_sigma<float>(self);
    case NPY_FLOAT64:
      return get_sigma<double>(self);
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot get sigma of %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
      return 0;
  }
BOB_CATCH_MEMBER("sigma", 0)
}

static auto dtype_doc = bob::extension::VariableDoc(
  "dtype",
  ":py:class:`numpy.dtype`",
  "The type of scalars produced by this normal distribution"
);
static PyObject* PyBoostNormal_GetDtype(PyBoostNormalObject* self) {
BOB_TRY
  return Py_BuildValue("N", PyArray_DescrFromType(self->type_num));
BOB_CATCH_MEMBER("dtype", 0)
}

static PyGetSetDef PyBoostNormal_getseters[] = {
    {
      dtype_doc.name(),
      (getter)PyBoostNormal_GetDtype,
      0,
      dtype_doc.doc(),
      0,
    },
    {
      mean_doc.name(),
      (getter)PyBoostNormal_GetMean,
      0,
      mean_doc.doc(),
      0,
    },
    {
      sigma_doc.name(),
      (getter)PyBoostNormal_GetSigma,
      0,
      sigma_doc.doc(),
      0,
    },
    {0}  /* Sentinel */
};

static auto reset_doc = bob::extension::FunctionDoc(
  "reset",
  "Resets this distribution",
  "After calling this method, subsequent uses of the distribution do not depend on values produced by any random number generator prior to invoking reset",
  true
)
.add_prototype("")
;
template <typename T> PyObject* reset(PyBoostNormalObject* self) {
  boost::static_pointer_cast<bob::core::random::normal_distribution<T>>(self->distro)->reset();
  Py_RETURN_NONE;
}

/**
 * Resets the distribution - this is a noop for normal distributions, here
 * only for compatibility reasons
 */
static PyObject* PyBoostNormal_Reset(PyBoostNormalObject* self) {
  switch (self->type_num) {
    case NPY_FLOAT32:
      return reset<float>(self);
    case NPY_FLOAT64:
      return reset<double>(self);
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot reset %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
      return 0;
  }
}

static auto call_doc = bob::extension::FunctionDoc(
  "draw",
  "Draws one random number from this distribution using the given ``rng``",
  ".. note:: The :py:meth:`__call__` function is a synonym for this ``draw``.",
  true
)
.add_prototype("rng", "value")
.add_parameter("rng", ":py:class:`mt19937`", "The random number generator to use")
.add_return("value", "dtype", "A random value that follows the normal distribution")
;
template <typename T> PyObject* call(PyBoostNormalObject* self, PyBoostMt19937Object* rng) {
  typedef bob::core::random::normal_distribution<T> distro_t;
  return PyBlitzArrayCxx_FromCScalar((*boost::static_pointer_cast<distro_t>(self->distro))(*rng->rng));
}

/**
 * Calling a PyBoostNormalObject to generate a random number
 */
static PyObject* PyBoostNormal_Call(PyBoostNormalObject* self, PyObject *args, PyObject* kwds) {
BOB_TRY
  char** kwlist = call_doc.kwlist();

  PyBoostMt19937Object* rng;

  if (!PyArg_ParseTupleAndKeywords(args, kwds, "O!", kwlist, &PyBoostMt19937_Type, &rng)) return 0; ///< FAILURE

  switch(self->type_num) {
    case NPY_FLOAT32:
      return call<float>(self, rng);
      break;
    case NPY_FLOAT64:
      return call<double>(self, rng);
      break;
    default:
      PyErr_Format(PyExc_NotImplementedError, "cannot call %s(T) with T having an unsupported numpy type number of %d (DEBUG ME)", Py_TYPE(self)->tp_name, self->type_num);
  }

  return 0; ///< FAILURE
BOB_CATCH_MEMBER("call", 0)
}

static PyMethodDef PyBoostNormal_methods[] = {
    {
      call_doc.name(),
      (PyCFunction)PyBoostNormal_Call,
      METH_VARARGS|METH_KEYWORDS,
      call_doc.doc(),
    },
    {
      reset_doc.name(),
      (PyCFunction)PyBoostNormal_Reset,
      METH_NOARGS,
      reset_doc.doc(),
    },
    {0}  /* Sentinel */
};

extern PyObject* scalar_to_bytes(PyObject* s);

/**
 * String representation and print out
 */
static PyObject* PyBoostNormal_Repr(PyBoostNormalObject* self) {
BOB_TRY
  PyObject* smean = scalar_to_bytes(PyBoostNormal_GetMean(self));
  if (!smean) return 0;
  auto smean_ = make_safe(smean);
  PyObject* ssigma = scalar_to_bytes(PyBoostNormal_GetSigma(self));
  if (!ssigma) return 0;
  auto ssigma_ = make_safe(ssigma);

  return
    PyString_FromFormat
      (
       "%s(dtype='%s', mean=%s, sigma=%s)",
       Py_TYPE(self)->tp_name, PyBlitzArray_TypenumAsString(self->type_num),
       PyString_AS_STRING(smean), PyString_AS_STRING(ssigma)
      );
BOB_CATCH_MEMBER("repr", 0)
}

PyTypeObject PyBoostNormal_Type = {
  PyVarObject_HEAD_INIT(0,0)
  0
};

bool init_BoostNormal(PyObject* module)
{
  // initialize the type struct
  PyBoostNormal_Type.tp_name = normal_doc.name();
  PyBoostNormal_Type.tp_basicsize = sizeof(PyBoostNormalObject);
  PyBoostNormal_Type.tp_flags = Py_TPFLAGS_DEFAULT;
  PyBoostNormal_Type.tp_doc = normal_doc.doc();
  PyBoostNormal_Type.tp_str = reinterpret_cast<reprfunc>(PyBoostNormal_Repr);
  PyBoostNormal_Type.tp_repr = reinterpret_cast<reprfunc>(PyBoostNormal_Repr);

  // set the functions
  PyBoostNormal_Type.tp_new = PyBoostNormal_New;
  PyBoostNormal_Type.tp_init = reinterpret_cast<initproc>(PyBoostNormal_Init);
  PyBoostNormal_Type.tp_dealloc = reinterpret_cast<destructor>(PyBoostNormal_Delete);
  PyBoostNormal_Type.tp_methods = PyBoostNormal_methods;
  PyBoostNormal_Type.tp_getset = PyBoostNormal_getseters;
  PyBoostNormal_Type.tp_call = reinterpret_cast<ternaryfunc>(PyBoostNormal_Call);

  // check that everything is fine
  if (PyType_Ready(&PyBoostNormal_Type) < 0) return false;

  // add the type to the module
  return PyModule_AddObject(module, "normal", Py_BuildValue("O", &PyBoostNormal_Type)) >= 0;
}


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

//[ guide_custom_minimal_axis

#include <boost/histogram.hpp>
#include <cassert>

int main() {
  using namespace boost::histogram;

  // stateless axis which returns 1 if the input is even and 0 otherwise
  struct even_odd_axis {
    axis::index_type index(int x) const { return x % 2; }
    axis::index_type size() const { return 2; }
  };

  // threshold axis which returns 1 if the input is above threshold
  struct threshold_axis {
    threshold_axis(double x) : thr(x) {}
    axis::index_type index(double x) const { return x >= thr; }
    axis::index_type size() const { return 2; }
    double thr;
  };

  auto h = make_histogram(even_odd_axis(), threshold_axis(3.0));

  h(0, 2.0);
  h(1, 4.0);
  h(2, 4.0);

  assert(h.at(0, 0) == 1); // even, below threshold
  assert(h.at(0, 1) == 1); // even, above threshold
  assert(h.at(1, 0) == 0); // odd, below threshold
  assert(h.at(1, 1) == 1); // odd, above threshold
}

//]


#include <deal.II/base/point.h>

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

#include <gtest/gtest.h>

#include <fstream>

using namespace dealii;

// Declare functions in step-1 and step-2
void first_grid(Triangulation<2> &);
void second_grid(Triangulation<2> &);
void third_grid(Triangulation<2> &);
std::tuple<unsigned int, unsigned int, unsigned int>
get_info(const Triangulation<2> &);

TEST(Step1, Mark1)
{
  Triangulation<2> tria;
  first_grid(tria);
  ASSERT_TRUE(std::ifstream("grid-1.svg"));
}

TEST(Step1, Mark2)
{
  Triangulation<2> tria;
  second_grid(tria);
  ASSERT_TRUE(std::ifstream("grid-2.svg"));
}

TEST(Step1, Mark3)
{
  Triangulation<2> tria;
  third_grid(tria);
  ASSERT_TRUE(std::ifstream("grid-3.vtk"));
}

TEST(Step1, Mark4)
{
  Triangulation<2> tria;
  first_grid(tria);
  auto [levels, cells, active_cells] = get_info(tria);
  EXPECT_EQ(levels, 5u);
  EXPECT_EQ(cells, 341u);
  EXPECT_EQ(active_cells, 256u);
}

TEST(Step1, Mark5)
{
  Triangulation<2> tria;
  second_grid(tria);
  const auto [levels, cells, active_cells] = get_info(tria);
  EXPECT_EQ(levels, 6u);
  EXPECT_EQ(cells, 1250u);
  EXPECT_EQ(active_cells, 940u);
}

TEST(Step1, Mark6)
{
  Triangulation<2> tria;
  third_grid(tria);
  auto [levels, cells, active_cells] = get_info(tria);
  EXPECT_EQ(levels, 5u);
  EXPECT_EQ(cells, 351u);
  EXPECT_EQ(active_cells, 264u);
}

int
main(int argc, char *argv[])
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/**********************************************************************
 *
 * This code is part of the MRcore projec
 * Author:  Alberto Valero Gomez (alberto.valero.gomez@gmail.com)
 *			Julio Valero Gomez
 *
 *
 * MRcore is licenced under the Common Creative License,
 * Attribution-NonCommercial-ShareAlike 3.0
 *
 * You are free:
 *   - to Share - to copy, distribute and transmit the work
 *   - to Remix - to adapt the work
 *
 * Under the following conditions:
 *   - Attribution. You must attribute the work in the manner specified
 *     by the author or licensor (but not in any way that suggests that
 *     they endorse you or your use of the work).
 *   - Noncommercial. You may not use this work for commercial purposes.
 *   - Share Alike. If you alter, transform, or build upon this work,
 *     you may distribute the resulting work only under the same or
 *     similar license to this one.
 *
 * Any of the above conditions can be waived if you get permission
 * from the copyright holder.  Nothing in this license impairs or
 * restricts the author's moral rights.
 *
 * It is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.
 **********************************************************************/

#include <vector>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <algorithm>
#include <numeric>
#include <cmath>
//#include "/usr/local/include/mrcore/mrcore.h"
#include <iostream>
#include <fstream>
#include "tictoc.h"

#include "generation.h"

#include "gnuplot_gui.h"
#include "string.h"

using namespace std;
using namespace Gnuplot;

class Pose{
public:
	Pose(double x, double y){this->x=x;this->y=y;}
	~Pose(){}
	double x,y;
	double distTo(Pose p){
		return sqrt(fabs((x-p.x)*(x-p.x) + (y-p.y)*(y-p.y)));
	}
};

void drawResult(GnuplotGui& gui, vector<Point2d> gplotCities, vector<Point2d> gplotTravellers, Individual* ind, vector<Pose>& poses){
	gui.clear();
	gui.draw_points(gplotTravellers,"Travellers","Travellers");
	gui.draw_points(gplotCities,"Cities","cities");

	int genSize = ind->getGenSize();
	int* gen = ind->getGen();
	int num_travellers= ind->getNumTravellers();
	int robot = gen[0];

//	cout << "Robot = " <<robot << endl;

	vector < Path<Point2d> > paths;
	paths.resize(num_travellers);

	for (int i=0; i<genSize-1; i++){
		if (gen[i+1] < num_travellers){
			paths[robot].push(Point2d(poses[gen[i]].x, poses[gen[i]].y));
			paths[robot].push(Point2d(poses[robot].x, poses[robot].y));
			//next robot
			robot = gen[i+1];
//			cout << "Robot = " <<robot << endl;
			continue;
		}
		paths[robot].push(Point2d(poses[gen[i]].x, poses[gen[i]].y));
	}
	paths[robot].push(Point2d(poses[gen[genSize-1]].x, poses[gen[genSize-1]].y));
	paths[robot].push(Point2d(poses[robot].x, poses[robot].y)); //close the last loop

	//draw the paths
	for (unsigned int i=0;i<paths.size();i++){
		string tag;
		stringstream ss;//create a stringstream
		ss << "Robot " << i;//add number to the stream
		tag = ss.str();//return a string with the contents of the stream
		gui.draw_path(paths[i],tag);
	}

	gui.redraw();
	//sleep(0.2);

}

int main(){

	GnuplotGui gui_poses;

	gui_poses.set_xrange(0,500);
	gui_poses.set_yrange(0,250);

	vector<Point2d> gplotCities;
	vector<Point2d> gplotTravellers;

	//vector<Pose> poses;

	int num_cities, num_travellers, steady_number, population_size;
	cout << "Please introduce the following data:" << endl;
	cout << "Num Cities: "; cin >> num_cities;
	cout << "Num Travellers: "; cin >> num_travellers;
	cout << "Number of steady iterations for terminate: "; cin >> steady_number;

	population_size=100;

	/********* CREATE GRAPH*********/
	cout << "Creating Graph" << endl;
	srand(time(0));
	vector<Pose> poses;
	double** costs;
	int num_points = num_cities + num_travellers;

	for (int i=0;i<num_travellers;i++){
		double x= double(rand()%500);
		double y= double(rand()%250);
		gplotTravellers.push_back(Point2d(x,y));
		poses.push_back(Pose(x,y));
	}

	gui_poses.draw_points(gplotTravellers,"Travellers","Travellers"); gui_poses.redraw();

	for (int i=0;i<num_cities;i++){
		double x= double(rand()%500);
		double y= double(rand()%250);
		gplotCities.push_back(Point2d(x,y));
		poses.push_back(Pose(x,y));
	}

	// draw poses
	gui_poses.draw_points(gplotCities,"Cities","cities"); gui_poses.redraw();

	sleep(2);

	costs = new double*[num_points];

	for (int i=0;i<num_points;i++){
		costs[i]=new double[num_points];
	}

	for (int i=0;i<num_points;i++){
		costs[i][i]=0;
		for (int j=i+1;j<num_points;j++){
			costs[j][i]=costs[i][j]=(poses[i]).distTo(poses[j]);
		}
	}

	cout << "Graph Created" << endl;
	/************ END CREATE GRAPH *******************/

	/************** EVOLVE **************************/
	cout << "Evolving" << endl;

	Generation* generation = new Generation(population_size,num_cities,num_travellers,costs,20,1.05);
	int numberGenerations = 0;

	double bestCost,prevBestCost;

	tictoc* timer = new tictoc();
	tic(timer);

	generation->generateInitGeneration(5,1000);

	Individual* ind = generation->getBestIndividual();
	bestCost=ind->getCost();
	prevBestCost=bestCost;

	int steadyCounter=0;
	int steadyCounter2=0;

	while(true){
		numberGenerations ++;

		if (steadyCounter>steady_number){
			break; //end the evolution
		}
		float mutationPerc = 100.f*float(steadyCounter)/500;
		if (mutationPerc>50) mutationPerc=50;
		generation->setMutationPerc(mutationPerc);
		generation->generateNewGeneration();
		Individual* ind=generation->getBestIndividual();
		bestCost=ind->getCost();
		if (fabs(bestCost-prevBestCost)<0.1){
			steadyCounter++;steadyCounter2++;
		}
		else{
			steadyCounter=steadyCounter2=0;
			drawResult(gui_poses,gplotCities,gplotTravellers,ind,poses);
			//cout << "Improvement" << endl;
		}
		prevBestCost=bestCost;

		if (steadyCounter2>=steady_number/3){
			generation->inmigration(1,10000,25);
			steadyCounter2=0;
		}
	}

	drawResult(gui_poses,gplotCities,gplotTravellers,generation->getBestIndividual(),poses);

	cout << "Finish of Evolution" << endl;
	/**********END EVOLUTION*******************/

	/** FOR LOGGING **/

	double totalTime = toc(timer);
	delete timer;

	ind = generation->getBestIndividual();

	double totalCost = bestCost;
	numberGenerations = numberGenerations - steady_number;
	double* individualCosts = new double[num_travellers];

	for (int i=0;i<num_travellers;i++) individualCosts[i]=0;

	int * gen = ind->getGen();
	int genSize = ind->getGenSize();

	int robot = gen[0];

	for (int i=0; i<genSize-1; i++){
		if (gen[i+1] < num_travellers){
			individualCosts[robot]+=costs[gen[i]][robot]; //close the path
			//next robot
			robot = gen[i+1];
			continue;
		}
		individualCosts[robot]+=costs[gen[i]][gen[i+1]];
	}

	individualCosts[robot]+=costs[gen[genSize-1]][robot]; //close the last loop

	cout << population_size << "\t " << steady_number << "\t" << num_travellers << "\t" << num_cities << "\t" << numberGenerations << "\t" << totalTime << "\t" << totalCost <<"\t";
	for (int i=0; i < num_travellers;i++){
		cout << individualCosts[i] << "\t";
	}
	cout <<endl;

	/** END LOGGING OPERATIONS**/
	delete [] individualCosts;
	delete generation;

	delete [] costs;
	cout << "Done!" << endl;
	return 1;
}


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <iostream>
#include <algorithm>
#include <vector>
#include <boost/numeric/ublas/tensor/algorithms.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>
#include "utility.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE ( test_tensor_algorithms/*,
                      * boost::unit_test::depends_on("test_shape_dynamic") * boost::unit_test::depends_on("test_strides")*/
                      )

// BOOST_AUTO_TEST_SUITE ( test_tensor_algorithms)

using test_types  = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;
using test_types2 = std::tuple<std::int32_t,std::int64_t,float,double,std::complex<float>>;

struct fixture
{
  using extents_t = boost::numeric::ublas::extents<>;
  const std::vector<extents_t> extents =
  {
      extents_t{1,1}, // 1
      extents_t{1,2}, // 2
      extents_t{2,1}, // 3
      extents_t{2,3}, // 4
      extents_t{2,3,1}, // 5
      extents_t{4,1,3}, // 6
      extents_t{1,2,3}, // 7
      extents_t{4,2,3}, // 8
      extents_t{4,2,3,5}
  };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_copy, value,  test_types2, fixture )
{
  namespace ublas    = boost::numeric::ublas;
  using value_type   = value;
  using vector_t     = std::vector<value_type>;

  constexpr auto first_order = ublas::layout::first_order{};
  constexpr auto last_order  = ublas::layout::last_order {};

  for(auto const& n : extents) {

    auto a  = vector_t(product(n));
    auto b  = vector_t(product(n));
    auto c  = vector_t(product(n));

    auto wa = ublas::to_strides(n,first_order);
    auto wb = ublas::to_strides(n,last_order );
    auto wc = ublas::to_strides(n,first_order);

    auto v = value_type{};
    for(auto i = 0ul; i < a.size(); ++i, v+=1){
      a[i]=v;
    }

    ublas::copy( ublas::size(n), n.data(), b.data(), wb.data(), a.data(), wa.data() );
    ublas::copy( ublas::size(n), n.data(), c.data(), wc.data(), b.data(), wb.data() );

    for(auto i = 1ul; i < c.size(); ++i)
      BOOST_CHECK_EQUAL( c[i], a[i] );

    std::size_t const*const p0 = nullptr;
    value_type* c0 = nullptr;

    BOOST_CHECK_THROW( ublas::copy( ublas::size(n), p0,      c.data(), wc.data(), b.data(), wb.data() ), std::runtime_error );
    BOOST_CHECK_THROW( ublas::copy( ublas::size(n), n.data(), c.data(), p0,        b.data(), wb.data() ), std::runtime_error );
    BOOST_CHECK_THROW( ublas::copy( ublas::size(n), n.data(), c.data(), wc.data(), b.data(), p0        ), std::runtime_error );
    BOOST_CHECK_THROW( ublas::copy( ublas::size(n), n.data(), c0,       wc.data(), b.data(), wb.data() ), std::runtime_error );
  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_copy_exceptions, value,  test_types2, fixture )
{
  namespace ublas    = boost::numeric::ublas;
  using value_type   = value;
  using vector_t     = std::vector<value_type>;
  constexpr auto first_order = ublas::layout::first_order{};

  for(auto const& n : extents) {

    value_type* a  = nullptr;
    auto c  = vector_t(ublas::product(n));

    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::copy( ublas::size(n), n.data(), c.data(), wc.data(), a, wa.data() ), std::runtime_error );

  }

  for(auto const& n : extents) {

    value_type* a  = nullptr;
    value_type* c  = nullptr;

    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::copy( ublas::size(n), n.data(), c, wc.data(), a, wa.data() ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(product(n));
    value_type* c  = nullptr;

    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::copy( ublas::size(n), n.data(), c, wc.data(), a.data(), wa.data() ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(product(n));
    auto c  = vector_t(product(n));

    size_t* wa = nullptr;
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::copy( ublas::size(n), n.data(), c.data(), wc.data(), a.data(), wa ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(product(n));
    auto c  = vector_t(product(n));

    size_t* wc = nullptr;
    auto wa = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::copy( ublas::size(n), n.data(), c.data(), wc, a.data(), wa.data() ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(product(n));
    auto c  = vector_t(product(n));

    size_t* m = nullptr;
    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::copy( ublas::size(n), m, c.data(), wc.data(), a.data(), wa.data() ), std::runtime_error );

  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_transform, value,  test_types2, fixture )
{
  namespace ublas    = boost::numeric::ublas;
  using value_type   = value;
  using vector_t  = std::vector<value_type>;

  constexpr auto first_order = ublas::layout::first_order{};
  constexpr auto last_order  = ublas::layout::last_order {};

  for(auto const& n : extents) {

    auto a  = vector_t(ublas::product(n));
    auto b  = vector_t(ublas::product(n));
    auto c  = vector_t(ublas::product(n));

    auto wa = ublas::to_strides(n,first_order);
    auto wb = ublas::to_strides(n,last_order );
    auto wc = ublas::to_strides(n,first_order);

    auto v = value_type{};
    for(auto i = 0ul; i < a.size(); ++i, v+=1){
      a[i]=v;
    }

    ublas::transform( ublas::size(n), n.data(), b.data(), wb.data(), a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} );
    ublas::transform( ublas::size(n), n.data(), c.data(), wc.data(), b.data(), wb.data(), [](value_type const& a){ return a - value_type(1);} );

    auto zero = std::size_t{0};
    ublas::transform(zero, n.data(), c.data(), wc.data(), b.data(), wb.data(), [](value_type const& a){ return a + value_type(1);} );

    value_type* c0 = nullptr;
    const std::size_t* s0 = nullptr;
    std::size_t const*const p0 = nullptr;

    BOOST_CHECK_THROW(ublas::transform( ublas::size(n), n.data(), c0, wb.data(), a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error);
    BOOST_CHECK_THROW(ublas::transform( ublas::size(n), n.data(), b.data(), s0, a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error);
    BOOST_CHECK_THROW(ublas::transform( ublas::size(n), p0, b.data(), wb.data(), a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error);

    for(auto i = 1ul; i < c.size(); ++i)
      BOOST_CHECK_EQUAL( c[i], a[i] );

  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_transform_exceptions, value,  test_types2, fixture )
{
  namespace ublas    = boost::numeric::ublas;
  using value_type   = value;
  using vector_t  = std::vector<value_type>;

  constexpr auto first_order = ublas::layout::first_order{};

  for(auto const& n : extents) {

    value_type* a  = nullptr;
    auto c  = vector_t(ublas::product(n));

    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::transform( ublas::size(n), n.data(), c.data(), wc.data(), a, wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error );

  }

  for(auto const& n : extents) {

    value_type* a  = nullptr;
    value_type* c  = nullptr;

    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::transform( ublas::size(n), n.data(), c, wc.data(), a, wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(ublas::product(n));
    value_type* c  = nullptr;

    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::transform( ublas::size(n), n.data(), c, wc.data(), a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(product(n));
    auto c  = vector_t(product(n));

    size_t* wa = nullptr;
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::transform( ublas::size(n), n.data(), c.data(), wc.data(), a.data(), wa, [](value_type const& a){ return a + value_type(1);} ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(ublas::product(n));
    auto c  = vector_t(ublas::product(n));

    size_t* wc = nullptr;
    auto wa = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::transform( ublas::size(n), n.data(), c.data(), wc, a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error );

  }

  for(auto const& n : extents) {

    auto a  = vector_t(product(n));
    auto c  = vector_t(product(n));

    size_t* m = nullptr;
    auto wa = ublas::to_strides(n,first_order);
    auto wc = ublas::to_strides(n,first_order);

    BOOST_REQUIRE_THROW( ublas::transform( ublas::size(n), m, c.data(), wc.data(), a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} ), std::runtime_error );

  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_accumulate, value,  test_types2, fixture )
{
  namespace ublas    = boost::numeric::ublas;
  using value_type   = value;
  using vector_t     = std::vector<value_type>;

  constexpr auto first_order = ublas::layout::first_order{};

  for(auto const& n : extents) {

    auto const s = ublas::product(n);

    auto a  = vector_t(ublas::product(n));
    auto wa = ublas::to_strides(n,first_order);

    auto v = value_type{};
    for(auto i = 0ul; i < a.size(); ++i, v+=value_type(1)){
      a[i]=v;
    }

    auto acc = ublas::accumulate( ublas::size(n), n.data(), a.data(), wa.data(), v);

    auto sum = std::div(s*(s+1),2).quot;

    BOOST_CHECK_EQUAL( acc, value_type( static_cast< inner_type_t<value_type> >( sum ) )  );

    auto zero = std::size_t{0};
    (void)ublas::accumulate(zero, n.data(), a.data(), wa.data(),v);

    value_type* c0 = nullptr;
    std::size_t const*const p0 = nullptr;

    BOOST_CHECK_THROW((void)ublas::accumulate( ublas::size(n), n.data(), c0, wa.data(), v), std::runtime_error);
    BOOST_CHECK_THROW((void)ublas::accumulate( ublas::size(n), n.data(), a.data(), p0, v), std::runtime_error);
    BOOST_CHECK_THROW((void)ublas::accumulate( ublas::size(n), p0, a.data(), wa.data(), v), std::runtime_error);

    auto acc2 = ublas::accumulate( ublas::size(n), n.data(), a.data(), wa.data(), v,
                                  [](auto const& l, auto const& r){return l + r; });

    BOOST_CHECK_EQUAL( acc2, value_type( static_cast< inner_type_t<value_type> >( sum ) )  );

    (void)ublas::accumulate(zero, n.data(), a.data(), wa.data(), v, [](auto const& l, auto const& r){return l + r; });

    BOOST_CHECK_THROW((void)ublas::accumulate( ublas::size(n), n.data(), c0, wa.data(), v,[](auto const& l, auto const& r){return l + r; }), std::runtime_error);
    BOOST_CHECK_THROW((void)ublas::accumulate( ublas::size(n), n.data(), a.data(), p0, v, [](auto const& l, auto const& r){return l + r; }), std::runtime_error);
    BOOST_CHECK_THROW((void)ublas::accumulate( ublas::size(n), p0, a.data(), wa.data(),v, [](auto const& l, auto const& r){return l + r; }), std::runtime_error);

  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_accumulate_exceptions, value,  test_types2, fixture )
{
  namespace ublas    = boost::numeric::ublas;
  using value_type   = value;
  using vector_t  = std::vector<value_type>;
  constexpr auto first_order = ublas::layout::first_order{};

  for(auto const& n : extents) {
    value_type* a  = nullptr;
    auto wa = ublas::to_strides(n,first_order);
    BOOST_REQUIRE_THROW( (void)ublas::accumulate( ublas::size(n), n.data(), a, wa.data(), value_type{0} ), std::runtime_error );

  }

  for(auto const& n : extents) {
    value_type* a  = nullptr;
    auto wa = ublas::to_strides(n,first_order);
    BOOST_REQUIRE_THROW( (void)ublas::accumulate( ublas::size(n), n.data(), a, wa.data(), value_type{0},[](value_type const& a,value_type const& b){ return a + b;} ), std::runtime_error );
  }

  for(auto const& n : extents) {
    auto a  = vector_t(product(n));
    auto wa = ublas::to_strides(n,first_order);
    size_t p = 0u;
    BOOST_CHECK_EQUAL ( ublas::accumulate( p, n.data(), a.data(), wa.data(), value_type{0} ), value_type{0} );
  }

  for(auto const& n : extents) {
    auto a  = vector_t(product(n));
    auto wa = ublas::to_strides(n,first_order);
    size_t p = 0u;
    BOOST_CHECK_EQUAL( ublas::accumulate( p, n.data(), a.data(), wa.data(), value_type{0}, [](value_type const& a,value_type const& b){ return a + b;} ), value_type{0} );
  }

  for(auto const& n : extents) {
    auto a  = vector_t(product(n));
    size_t* wa = nullptr;
    BOOST_REQUIRE_THROW( (void)ublas::accumulate( ublas::size(n), n.data(), a.data(), wa, value_type{0} ), std::runtime_error );
  }

  for(auto const& n : extents) {
    auto a  = vector_t(product(n));
    auto wa = ublas::to_strides(n,first_order);
    size_t* m = nullptr;
    BOOST_REQUIRE_THROW( (void)ublas::accumulate( ublas::size(n), m, a.data(), wa.data(), value_type{0}, [](value_type const& a,value_type const& b){ return a + b;} ), std::runtime_error );
  }

}

template<class V>
void init(std::vector<V>& a)
{
  auto v = V(1);
  for(auto i = 0u; i < a.size(); ++i, ++v){
    a[i] = v;
  }
}

template<class V>
void init(std::vector<std::complex<V>>& a)
{
  auto v = std::complex<V>(1,1);
  for(auto i = 0u; i < a.size(); ++i){
    a[i] = v;
    v.real(v.real()+1);
    v.imag(v.imag()+1);
  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_trans, value,  test_types, fixture )
{
  namespace ublas   = boost::numeric::ublas;
  using value_type  = typename value::first_type;
//  using layout_t    = typename value::second_type;
  using vector_t    = std::vector<value_type>;
  using base_t      = typename extents_t::base_type;
  using permutation_type = std::vector<std::size_t>;

  constexpr auto first_order = ublas::layout::first_order{};

  for(auto const& n : extents) {

    auto p   = ublas::size(n);
    auto s   = ublas::product(n);

    auto pi  = permutation_type(p);
    auto a   = vector_t(s);
    auto b1  = vector_t(s);
    auto b2  = vector_t(s);
    auto c1  = vector_t(s);
    auto c2  = vector_t(s);

    auto wa = ublas::to_strides(n,first_order);

    init(a);

    // so wie last-order.
    for(auto i = std::size_t{0}, j = p; i < ublas::size(n); ++i, --j)
      pi[i] = j;

    auto nc_base = base_t(p);
    for(auto i = 0u; i < p; ++i)
      nc_base[pi[i]-1] = n[i];

    auto nc = extents_t(std::move(nc_base));

    auto wc    = ublas::to_strides(nc,first_order);
    auto wc_pi = base_t(p);
    for(auto i = 0u; i < p; ++i)
      wc_pi[pi[i]-1] = wc[i];

    ublas::copy ( p, n.data(),            c1.data(), wc_pi.data(), a.data(), wa.data());
    ublas::trans( p, n.data(), pi.data(), c2.data(), wc.data(),    a.data(), wa.data() );

    if(!std::is_compound_v<value_type>)
      for(auto i = 0ul; i < s; ++i)
        BOOST_CHECK_EQUAL( c1[i], c2[i] );

    auto nb_base = base_t(p);
    for(auto i = 0u; i < p; ++i)
      nb_base[pi[i]-1] = nc[i];

    auto nb = extents_t(std::move(nb_base));

    auto wb    = ublas::to_strides(nb,first_order);
    auto wb_pi = base_t(p);
    for(auto i = 0u; i < p; ++i)
      wb_pi[pi[i]-1] = wb[i];

    ublas::copy ( p, nc.data(),            b1.data(), wb_pi.data(), c1.data(), wc.data());
    ublas::trans( p, nc.data(), pi.data(), b2.data(), wb.data(),    c2.data(), wc.data() );

    if(!std::is_compound_v<value_type>)
      for(auto i = 0ul; i < s; ++i)
        BOOST_CHECK_EQUAL( b1[i], b2[i] );

    for(auto i = 0ul; i < s; ++i)
      BOOST_CHECK_EQUAL( a[i], b2[i] );

    auto zero = std::size_t{0};
    ublas::trans( zero, n.data(), pi.data(), c2.data(), wc.data(), a.data(), wa.data() );
    ublas::trans( zero, nc.data(), pi.data(), b2.data(), wb.data(), c2.data(), wc.data() );

    value_type *c0 = nullptr;
    std::size_t const*const s0 = nullptr;

    BOOST_CHECK_THROW(ublas::trans( p, n.data(), pi.data(), c0, wc.data(),  a.data(), wa.data()), std::runtime_error);
    BOOST_CHECK_THROW(ublas::trans( p, s0, pi.data(), c2.data(),wc.data(),  a.data(), wa.data()), std::runtime_error);
    BOOST_CHECK_THROW(ublas::trans( p, n.data(), pi.data(), c2.data(), s0,  a.data(), wa.data()), std::runtime_error);
    BOOST_CHECK_THROW(ublas::trans( p, n.data(), s0, c2.data(), wc.data(),  a.data(), wa.data()), std::runtime_error);

  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_trans_exceptions, value,  test_types, fixture )
{
  namespace ublas        = boost::numeric::ublas;
  using value_type       = typename value::first_type;
  using layout_t         = typename value::second_type;
  using vector_t         = std::vector<value_type>;
  using permutation_type = std::vector<std::size_t>;

  constexpr auto layout = layout_t{};

  std::size_t* nnullptr = nullptr;
  value_type * anullptr = nullptr;

  for(auto const& n : extents) {
    auto p  = ublas::size(n);
    auto s  = ublas::product(n);
    auto pi = permutation_type(p);
    auto a  = vector_t(s);
    auto c  = vector_t(s);
    auto wa = ublas::to_strides(n,layout);
    auto wc = ublas::to_strides(n,layout);
    if(p>1){
      BOOST_REQUIRE_THROW( ublas::trans( p, nnullptr, pi.data(), c.data(), wc.data(),    a.data(), wa.data() ), std::runtime_error );
      BOOST_REQUIRE_THROW( ublas::trans( p, n.data() , nnullptr , c.data(), wc.data(),    a.data(), wa.data() ), std::runtime_error );
      BOOST_REQUIRE_THROW( ublas::trans( p, n.data() , pi.data(), c.data(), nnullptr ,    a.data(), nnullptr  ), std::runtime_error );
      BOOST_REQUIRE_THROW( ublas::trans( p, n.data() , pi.data(), c.data(), wc.data(),    a.data(), nnullptr  ), std::runtime_error );
      BOOST_REQUIRE_THROW( ublas::trans( p, n.data() , pi.data(), c.data(), nnullptr ,    a.data(), wa.data() ), std::runtime_error );
      BOOST_REQUIRE_THROW( ublas::trans( p, n.data() , pi.data(), anullptr, wc.data(),    anullptr, wa.data() ), std::runtime_error );
      BOOST_REQUIRE_THROW( ublas::trans( p, n.data() , pi.data(), c.data(), wc.data(),    anullptr, wa.data() ), std::runtime_error );
      BOOST_REQUIRE_THROW( ublas::trans( p, n.data() , pi.data(), anullptr, wc.data(),    a.data(), wa.data() ), std::runtime_error );
    }

    // ublas::trans( p, n.data(), pi.data(), c.data(), wc.data(),    a.data(), wa.data() );
  }

}

BOOST_AUTO_TEST_SUITE_END()


#ifndef RANDOM_UTIL_HPP
#define RANDOM_UTIL_HPP
/**
 * @file random_util.hpp
 *
 * @brief some functions commonly used in MTToolBox.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2011 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <inttypes.h>
#include <stdint.h>
#include <stdexcept>
#include <openssl/sha.h>
#include <tr1/memory>

#include <NTL/GF2X.h>

#define bit_size(tp) (static_cast<int>(sizeof(tp) * 8))

namespace MTToolBox {
    inline static int count_bit(uint16_t x);
    inline static int count_bit(uint32_t x);
    inline static int count_bit(uint64_t x);
    inline static uint32_t reverse_bit(uint32_t x);
    inline static uint64_t reverse_bit(uint64_t x);

    /**
     * calculate the largest number which is 2^m and does not exceed n.
     * @tparam T type of integer
     * @param n number
     * @returns the largest number which is 2^m and does not exceed n.
     */
    template<typename T>
    T floor2p(T n) {
	if (n == 1) {
	    return 1;
	} else {
	    return 2 * floor2p<T>(n / 2);
	}
    }

    /**
     * print polynomial in binary form. The coefficient of the smallest degree
     * is printed first.
     * @param os output-stream
     * @param poly polynomial to be printed
     * @param breakline if true, break line every 32 outputs.
     */
    inline static void print_binary(std::ostream& os,
				    NTL::GF2X& poly,
				    bool breakline = true) {
	using namespace NTL;
	if (deg(poly) < 0) {
	    os << "0deg=-1" << std::endl;
	    return;
	}
	for(int i = 0; i <= deg(poly); i++) {
	    if(rep(coeff(poly, i)) == 1) {
		os << '1';
	    } else {
		os << '0';
	    }
	    if (breakline && ((i % 32) == 31)) {
		os << std::endl;
	    }
	}
	os << "deg=" << deg(poly) << std::endl;
    }

    /**
     * change \b input to the number between \b start and \b end
     * @param input input number
     * @param start start of the range
     * @param end end of the range
     * @return the number r such that \b start <= \b r <= \b end.
     */
    inline static uint32_t get_range(uint32_t input, int start, int end) {
	if (end < start) {
	    printf("get_range:%d, %d\n", start, end);
	    exit(0);
	}
	return input % (end - start + 1) + start;
    }

    /**
     * change \b input to the number between \b start and \b end
     * @param input input number
     * @param start start of the range
     * @param end end of the range
     * @return the number r such that \b start <= \b r <= \b end.
     */
    inline static uint64_t get_range(uint64_t input, int start, int end) {
	if (end < start) {
	    printf("get_range:%u, %u\n", start, end);
	    exit(0);
	}
	return input % (end - start + 1) + start;
    }

    /**
     * change the small F2 table to the fast and redundant table.
     * @tparam T type of table member.
     * @param dist_tbl new redundant table
     * @param src_tbl source table
     * @param size size of \b dist_table
     */
    template<typename T>
    void fill_table(T dist_tbl[], T src_tbl[], int size) {
    	for(int i = 1; i < size; i++) {
	    for(int j = 1, k = 0; j <= i; j <<= 1, k++) {
		if (i & j) {
		    dist_tbl[i] ^= src_tbl[k];
		}
	    }
	}
    }

    /**
     * calculate the SHA1 digest of F2 polynomial. The coefficients of
     * the polynomial are changed to the string of "0" and "1", which
     * starts with the coefficient of the lowest degree, and the SHA1
     * hash of the string is calculated. The result hash is returned
     * by hexadecimal string.
     *
     * @param str output string
     * @param poly F2 polynomial
     */
    inline static void poly_sha1(std::string& str, const NTL::GF2X& poly) {
	using namespace NTL;
	using namespace std;
	SHA_CTX ctx;
	SHA1_Init(&ctx);
	if (deg(poly) < 0) {
	    SHA1_Update(&ctx, "-1", 2);
	}
	for(int i = 0; i <= deg(poly); i++) {
	    if(rep(coeff(poly, i)) == 1) {
		SHA1_Update(&ctx, "1", 1);
	    } else {
		SHA1_Update(&ctx, "0", 1);
	    }
	}
	unsigned char md[SHA_DIGEST_LENGTH];
	SHA1_Final(md, &ctx);
	stringstream ss;
	for (int i = 0; i < SHA_DIGEST_LENGTH; i++) {
	    ss << setfill('0') << setw(2) << hex
	       << static_cast<int>(md[i]);
	}
	ss >> str;
    }

    /**
     * calculate the position of most right 1, or
     * least significant 1. The position of the MSB is
     * 0. returns -1 when \b v is zero.
     * citing from a website http://aggregate.org/MAGIC/#Trailing Zero Count
     *
     * @param x input
     * @return the position of most right 1.
     */
    int calc_1pos(uint16_t x)
    {
	if (x == 0) {
	    return -1;
	}
	int16_t y = (int16_t)x;
	y = count_bit((uint16_t)((y & -y) - 1));
	return 15 - y;
    }

    /**
     * calculate the position of most right 1, or
     * least significant 1. The position of the MSB is
     * 0. returns -1 when \b v is zero.
     * citing from a website http://aggregate.org/MAGIC/#Trailing Zero Count
     *
     * @param x input
     * @return the position of most right 1.
     */
    int calc_1pos(uint32_t x)
    {
	if (x == 0) {
	    return -1;
	}
	int32_t y = (int32_t)x;
	y = count_bit((uint32_t)(y & -y) - 1);
	return 31 - y;
    }

    /**
     * calculate the position of most right 1, or
     * least significant 1. The position of the MSB is
     * 0. returns -1 when \b v is zero.
     * citing from a website http://aggregate.org/MAGIC/#Trailing Zero Count
     *
     * @param x input
     * @return the position of most right 1.
     */
    int calc_1pos(uint64_t x)
    {
	if (x == 0) {
	    return -1;
	}
	int64_t y = (int64_t)x;
	y = count_bit((uint64_t)(y & -y) - 1);
	return 63 - y;
    }

    /**
     * check if \b array is all zero or not.
     * @tparam type of members of \b array
     * @param array checked array
     * @param size size of \b array
     * @return true if all elements of \b array are zero.
     */
    template<typename T>
    bool is_zero_array(T *array, int size) {
	if (array[0] != 0) {
	    return false;
	} else {
	    return (memcmp(array, array + 1, sizeof(T) * (size - 1)) == 0);
	}
    }

    /**
     * calculate the minimal polynomial of the generated sequence.
     * @returns the minimal polynomial
     */
    template<typename G>
    std::tr1::shared_ptr<NTL::GF2X> get_minpoly(G& generator, int length) {
	using namespace std;
	using namespace NTL;
	using namespace std::tr1;

	vec_GF2 vec;
	vec.SetLength(length * 2);
	for (int i = 0; i < length * 2; i++) {
	    vec[i] = generator() & 1;
	}
	shared_ptr<GF2X> minpoly(new GF2X());
	MinPolySeq(*minpoly, vec, length);
	return minpoly;
    }

    /**
     * count the number of 1
     * SIMD within a Register algorithm
     * citing from a website http://aggregate.org/MAGIC/
     */
    inline static int count_bit(uint16_t x) {
        x -= (x >> 1) & UINT16_C(0x5555);
        x = ((x >> 2) & UINT16_C(0x3333)) + (x & UINT16_C(0x3333));
        x = ((x >> 4) + x) & UINT16_C(0x0f0f);
        x += (x >> 8);
        return (int)(x & 0x1f);
    }

    inline static int count_bit(uint32_t x) {
        x -= (x >> 1) & UINT32_C(0x55555555);
        x = ((x >> 2) & UINT32_C(0x33333333)) + (x & UINT32_C(0x33333333));
        x = ((x >> 4) + x) & UINT32_C(0x0f0f0f0f);
        x += (x >> 8);
        x += (x >> 16);
        return (int)(x & 0x3f);
    }

    inline static int count_bit(uint64_t x) {
	x -= (x >> 1) & UINT64_C(0x5555555555555555);
	x = ((x >> 2) & UINT64_C(0x3333333333333333))
	    + (x & UINT64_C(0x3333333333333333));
	x = ((x >> 4) + x) & UINT64_C(0x0f0f0f0f0f0f0f0f);
	x += (x >> 8);
	x += (x >> 16);
	x += (x >> 32);
	return (int)(x & 0x7f);
    }

    inline static uint32_t reverse_bit(uint32_t x)
    {
	uint32_t y = 0x55555555;
	x = (((x >> 1) & y) | ((x & y) << 1));
	y = 0x33333333;
	x = (((x >> 2) & y) | ((x & y) << 2));
	y = 0x0f0f0f0f;
	x = (((x >> 4) & y) | ((x & y) << 4));
	y = 0x00ff00ff;
	x = (((x >> 8) & y) | ((x & y) << 8));
	return((x >> 16) | (x << 16));
    }

    inline static uint64_t reverse_bit(uint64_t x)
    {
	uint64_t y = UINT64_C(0x5555555555555555);
	x = (((x >> 1) & y) | ((x & y) << 1));
	y = UINT64_C(0x3333333333333333);
	x = (((x >> 2) & y) | ((x & y) << 2));
	y = UINT64_C(0x0f0f0f0f0f0f0f0f);
	x = (((x >> 4) & y) | ((x & y) << 4));
	y = UINT64_C(0x00ff00ff00ff00ff);
	x = (((x >> 8) & y) | ((x & y) << 8));
	y = UINT64_C(0x0000ffff0000ffff);
	x = (((x >> 16) & y) | ((x & y) << 16));
	return((x >> 32) | (x << 32));
    }

    /**
     * divide and ceil
     */
    inline static int div_ceil(int x, int y) {
	if (x % y == 0) {
	    return x / y;
	} else {
	    return x / y + 1;
	}
    }

    /**
     * polynomial to string
     */
    inline static void to_str(uint8_t * str, int size, NTL::GF2X& poly) {
	if (deg(poly) >= size * 8) {
	    std::cerr << "str size too small" << std::endl;
	    throw new std::out_of_range("str size too small");
	}
	for (int i = 0; i < size; i++) {
	    str[i] = 0;
	}
	int idx = 0;
	for (int i = 0; i < size; i++) {
	    uint8_t mask = 1;
	    for (int j = 0; j < 8; j++) {
		if (IsOne(coeff(poly, idx++))) {
		    str[i] |= mask;
		}
		mask <<= 1;
	    }
	}
    }
}
#endif


#pragma once

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <array>
#include <boost/assert.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multi_array.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

#include "base/array_buffer.hpp"
#include "base/exceptions.hpp"
#include "base/timer.hpp"
#include "filtered_range.hpp"
#ifndef NOHDF5
#include "base/eigen2hdf.hpp"
#endif
#include "quadrature/qhermitew.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/laguerren_ks.hpp"
#include "spectral/eval_handlers.hpp"
#include "spectral/pl_radial_eval.hpp"

namespace boltzmann {
namespace detail {

/**
 * @brief sort *polar* elements by `k`
 */
struct CMP
{
  template <typename E>
  bool operator()(const E &e1, const E &e2) const
  {
    // typedef typename boost::mpl::at_c<typename E::types_t,0>::type fa_type;
    typedef typename boost::mpl::at_c<typename E::types_t, 1>::type fr_type;

    typename E::Acc::template get<fr_type> fr_accessor;
    // typename E::Acc::template get<fa_type> fa_accessor;

    const auto &idR1 = fr_accessor(e1).get_id();
    const auto &idR2 = fr_accessor(e2).get_id();

    return std::tie(idR1.k, idR1.j) < std::tie(idR2.k, idR2.j);
  }
};

}  // end namespace detail

template <typename PolarBasis, typename HermiteBasis>
class Polar2Hermite
{
 public:
  Polar2Hermite(const PolarBasis &polar_basis, const HermiteBasis &hermite_basis);
#ifndef NOHDF5
  void exportmat(const std::string &fname) const;
#endif

  void to_hermite(std::vector<double> &dst, const std::vector<double> &src) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_hermite(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_hermite_T(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const;

  void to_polar(std::vector<double> &dst, const std::vector<double> &src) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_polar(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const;

  void to_polar(double *dst, const double *src) const;

  const Eigen::MatrixXd &get_mat(int k) const;

 private:
  /// Transformation matrices for each polynomial degree k
  std::vector<Eigen::MatrixXd> tmatrices_;
  std::vector<unsigned int> offsets_;
  /// Permutation matrix for polar degrees of freedom
  Eigen::SparseMatrix<double> P_;

  Eigen::VectorXd polar_mass_m_;
  thread_local static ::ArrayBuffer<> buf_;
};

template <typename PolarBasis, typename HermiteBasis>
thread_local ::ArrayBuffer<> Polar2Hermite<PolarBasis, HermiteBasis>::buf_;

template <typename PolarBasis, typename HermiteBasis>
Polar2Hermite<PolarBasis, HermiteBasis>::Polar2Hermite(const PolarBasis &polar_basis,
                                                       const HermiteBasis &hermite_basis)
{
  BOOST_VERIFY(polar_basis.n_dofs() == hermite_basis.n_dofs());
  int K = spectral::get_max_k(polar_basis);
  const unsigned int qpts1d = std::max(40, 2 * (K + 1));
  // initialize quadrature
  QHermiteW quad(1.0, qpts1d);

  // typedef std::array<double, 2> point_t;
  typedef boost::multi_array<double, 2> array2d;
  // typedef boost::multi_array<point_t, 2> array2dpts;
  array2d W(boost::extents[qpts1d][qpts1d]);
  // quad. nodes in cartesian coordinates
  array2d X(boost::extents[qpts1d][qpts1d]);
  array2d Y(boost::extents[qpts1d][qpts1d]);
  array2d R(boost::extents[qpts1d][qpts1d]);
  array2d PHI(boost::extents[qpts1d][qpts1d]);

  for (unsigned int i = 0; i < qpts1d; ++i) {
    const double x = quad.pts(i);
    for (unsigned int j = 0; j < qpts1d; ++j) {
      const double y = quad.pts(j);
      X[i][j] = x;
      Y[i][j] = y;
      W[i][j] = quad.wts(i) * quad.wts(j);
      PHI[i][j] = std::atan2(y, x);
      R[i][j] = std::sqrt(y * y + x * x);
    }
  }
  const unsigned int nq = qpts1d * qpts1d;

  //  ...
  PolarBasis basis_k(polar_basis);
  basis_k.sort(detail::CMP());

  tmatrices_.resize(K + 1);
  P_.resize(polar_basis.n_dofs(), polar_basis.n_dofs());

  // evaluate basis functions at quadrature points
  RDTSCTimer timer;
  timer.start();
  LaguerreNKS<double> L(K);
  unsigned int Rlen = R.shape()[0] * R.shape()[1];
  L.compute(R.data(), Rlen, 0.5);

  typedef typename PolarBasis::elem_t polar_elem_t;
  typedef typename boost::mpl::at_c<typename polar_elem_t::types_t, 1>::type radial_elem_t;
  typedef typename boost::mpl::at_c<typename polar_elem_t::types_t, 0>::type ang_elem_t;
  typename polar_elem_t::Acc::template get<radial_elem_t> get_rad;
  typename polar_elem_t::Acc::template get<ang_elem_t> get_ang;

  typedef typename HermiteBasis::elem_t herm_elem_t;
  typedef typename boost::mpl::at_c<typename herm_elem_t::types_t, 0>::type hx_t;
  typedef typename boost::mpl::at_c<typename herm_elem_t::types_t, 1>::type hy_t;

  typename herm_elem_t::Acc::template get<hx_t> get_hx;
  typename herm_elem_t::Acc::template get<hy_t> get_hy;

  // HermiteEvalHandler2d<HermiteNW<double>, herm_elem_t> heval(H);
  auto heval = hermite_evaluator2d<>::make(hermite_basis, quad.pts());
  PLRadialEval<LaguerreNKS<double>, polar_elem_t> leval(L);
  // permutation matrix entries
  std::vector<unsigned int> p_loc(polar_basis.n_dofs());
  // update offset vector
  unsigned int offset = 0;
  offsets_.resize(K + 2);
  // ----------------------------------------------------
  // loop over block-diagonal matrix and compute offsets
  // ----------------------------------------------------
  offsets_[0] = 0;
  for (int k = 0; k <= K; ++k) {
    std::function<bool(const polar_elem_t &)> pred = [&](const polar_elem_t &e) {
      return (get_rad(e).get_id().k == k);
    };
    auto range = filtered_range(basis_k.begin(), basis_k.end(), pred);

    // collect polar elements of polynomial degree k
    std::vector<polar_elem_t> polar_elems;
    for (auto it = std::get<0>(range); it != std::get<1>(range); ++it) {
      polar_elems.push_back(*it);
    }
    offset += polar_elems.size();
    offsets_[k + 1] = offset;
  }

  // --------------------------------------
  // Compute transformation matrix entries
  // --------------------------------------
  timer.start();
#pragma omp parallel for
  // (* load-balancing will be rather poor *)
  for (int k = 0; k <= K; ++k) {
    std::function<bool(const polar_elem_t &)> pred = [&](const polar_elem_t &e) {
      return (get_rad(e).get_id().k == k);
    };
    auto range = filtered_range(basis_k.begin(), basis_k.end(), pred);

    // collect polar elements of polynomial degree k
    std::vector<polar_elem_t> polar_elems;
    for (auto it = std::get<0>(range); it != std::get<1>(range); ++it) {
      polar_elems.push_back(*it);
    }

    // collect hermite elements of polynomial degree k
    std::vector<herm_elem_t> herm_elems;
    std::function<bool(const herm_elem_t &)> pred2 = [&](const herm_elem_t &e) {
      return (get_hx(e).get_id().k + get_hy(e).get_id().k == k);
    };
    auto range_herm = filtered_range(hermite_basis.begin(), hermite_basis.end(), pred2);
    for (auto it = std::get<0>(range_herm); it != std::get<1>(range_herm); ++it) {
      herm_elems.push_back(*it);
    }

    assert(herm_elems.size() == polar_elems.size());

    // init matrix
    auto &Mk = tmatrices_[k];
    unsigned int nk = herm_elems.size();
    Mk.resize(nk, nk);

    // quadrature nodes and weights
    const double *w = W.origin();
    // const double* x = X.origin();
    // const double* y = Y.origin();
    // const double* r = R.origin();
    const double *phi = PHI.origin();

    for (auto itp = polar_elems.begin(); itp != polar_elems.end(); ++itp) {
      // create permutation matrix
      unsigned int dofx = polar_basis.get_dof_index(itp->get_id());
      unsigned int dofy = basis_k.get_dof_index(itp->get_id());
      p_loc[dofy] = dofx;  // permutation
      //      const unsigned int j = get_rad(*itp).get_id().j;
      const unsigned int k = get_rad(*itp).get_id().k;

      for (auto ith = herm_elems.begin(); ith != herm_elems.end(); ++ith) {
        unsigned int dofy2 = hermite_basis.get_dof_index(ith->get_id());  // index
        // quadrature
        double val = 0;
        for (unsigned int q = 0; q < nq; ++q) {
          // const double rloc = r[q];
          val += (w[q] * leval(*itp, q) * get_ang(*itp).evaluate(phi[q]) *
                  heval(*ith, q / qpts1d, q % qpts1d));
        }
        Mk(dofy2 - offsets_[k], dofy - offsets_[k]) = val;
      }
    }
  }

  for (unsigned int dofy = 0; dofy < p_loc.size(); ++dofy) {
    P_.insert(dofy, p_loc[dofy]) = 1;
  }
  P_.makeCompressed();

  // prepare diagonal entries of mass matrix
  const double PI = boost::math::constants::pi<double>();
  // initialize polar mass m
  const unsigned int N = basis_k.n_dofs();
  polar_mass_m_ = Eigen::VectorXd(N);
  for (unsigned int i = 0; i < N; ++i) {
    const auto &elem = basis_k.get_elem(i);
    if (get_ang(elem).get_id().l == 0)
      polar_mass_m_[i] = PI;
    else
      polar_mass_m_[i] = PI / 2;
  }
}

// ---------------------------------------------------------------------
template <typename PolarBasis, typename HermiteBasis>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_hermite(std::vector<double> &dst,
                                                    const std::vector<double> &src) const
{
  assert(dst.size() == src.size());
  assert(src.size() == (unsigned int)P_.cols());

  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> cvec_t;

  cvec_t vsrc(src.data(), src.size());
  vec_t vdst(dst.data(), dst.size());

  this->to_hermite(vdst, vsrc);
}

// ----------------------------------------------------------------------
template <typename PolarBasis, typename HermiteBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_hermite(Eigen::DenseBase<DERIVED1> &dst,
                                                    const Eigen::DenseBase<DERIVED2> &src) const
{
  const int K = tmatrices_.size();
  const int N = offsets_[K];

  BOOST_ASSERT(dst.size() == N);
  BOOST_ASSERT(src.size() == N);
  auto vpsrc = buf_.get<Eigen::VectorXd>(N);

  // TODO, this is slow
  vpsrc = P_ * src.derived();

  for (int k = 0; k < tmatrices_.size(); ++k) {
    const unsigned int blocksize = offsets_[k + 1] - offsets_[k];
    dst.segment(offsets_[k], blocksize) = tmatrices_[k] * vpsrc.segment(offsets_[k], blocksize);
  }
}

template <typename PolarBasis, typename HermiteBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_hermite_T(Eigen::DenseBase<DERIVED1> &dst,
                                                      const Eigen::DenseBase<DERIVED2> &src) const
{
  assert(dst.size() == src.size());
  assert(src.size() == (unsigned int)P_.cols());
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> cvec_t;

  const int K = tmatrices_.size();
  const int N = offsets_[K];

  auto vpsrc = buf_.get<Eigen::VectorXd>(N);

  for (int k = 0; k < tmatrices_.size(); ++k) {
    const unsigned int blocksize = offsets_[k + 1] - offsets_[k];
    vpsrc.segment(offsets_[k], blocksize) =
        tmatrices_[k].transpose() * src.segment(offsets_[k], blocksize);
  }

  dst = P_.transpose() * vpsrc;
}

template <typename PolarBasis, typename HermiteBasis>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_polar(std::vector<double> &dst,
                                                  const std::vector<double> &src) const
{
  assert(dst.size() == src.size());
  assert(src.size() == (unsigned int)P_.cols());
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> cvec_t;
  vec_t vdst(dst.data(), dst.size());
  cvec_t vsrc(src.data(), src.size());

  this->to_polar(vdst, vsrc);
}

template <typename PolarBasis, typename HermiteBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_polar(Eigen::DenseBase<DERIVED1> &dst,
                                                  const Eigen::DenseBase<DERIVED2> &src) const
{
  const int K = tmatrices_.size();
  const int N = offsets_[K];

  BOOST_ASSERT(dst.size() == N);
  BOOST_ASSERT(src.size() == N);

  // TODO: use buffer memory instead
  auto vpsrc = buf_.get<Eigen::VectorXd>(N);
  for (int k = 0; k < tmatrices_.size(); ++k) {
    const unsigned int blocksize = offsets_[k + 1] - offsets_[k];
    vpsrc.segment(offsets_[k], blocksize) =
        tmatrices_[k].transpose() * src.segment(offsets_[k], blocksize);
  }
  vpsrc = vpsrc.array() / polar_mass_m_.array();

  dst = P_.transpose() * vpsrc;
}

template <typename PolarBasis, typename HermiteBasis>
const Eigen::MatrixXd &
Polar2Hermite<PolarBasis, HermiteBasis>::get_mat(int k) const
{
  BOOST_ASSERT(k < tmatrices_.size());
  return tmatrices_[k];
}

#ifndef NOHDF5
template <typename PolarBasis, typename HermiteBasis>
void
Polar2Hermite<PolarBasis, HermiteBasis>::exportmat(const std::string &fname) const
{
  hid_t h5f = H5Fcreate(fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  eigen2hdf::save_sparse(h5f, "P", P_);
  for (unsigned int k = 0; k < tmatrices_.size(); ++k) {
    eigen2hdf::save(h5f, std::to_string(k), tmatrices_[k]);
  }
  H5Fclose(h5f);
}
#endif

}  // end namespace boltzmann