// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cmath>
#include <cstddef>
#include <memory>
#include <pup.h>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/Affine.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.hpp"
#include "Domain/CoordinateMaps/ProductMaps.hpp"
#include "Domain/CoordinateMaps/Rotation.hpp"
#include "Domain/CoordinateMaps/Wedge2D.hpp"
#include "Domain/Direction.hpp"
#include "Domain/OrientationMap.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/MakeWithValue.hpp"
#include "tests/Unit/TestHelpers.hpp"

// IWYU pragma: no_forward_declare Tensor

namespace {
template <typename Map1, typename Map2, typename DataType, size_t Dim>
auto compose_jacobians(const Map1& map1, const Map2& map2,
                       const std::array<DataType, Dim>& point) {
  const auto jac1 = map1.jacobian(point);
  const auto jac2 = map2.jacobian(map1(point));

  auto result =
      make_with_value<Jacobian<DataType, Dim, Frame::Logical, Frame::Grid>>(
          point[0], 0.);
  for (size_t target = 0; target < Dim; ++target) {
    for (size_t source = 0; source < Dim; ++source) {
      for (size_t dummy = 0; dummy < Dim; ++dummy) {
        result.get(target, source) +=
            jac2.get(target, dummy) * jac1.get(dummy, source);
      }
    }
  }
  return result;
}

template <typename Map1, typename Map2, typename DataType, size_t Dim>
auto compose_inv_jacobians(const Map1& map1, const Map2& map2,
                           const std::array<DataType, Dim>& point) {
  const auto inv_jac1 = map1.inv_jacobian(point);
  const auto inv_jac2 = map2.inv_jacobian(map1(point));

  auto result = make_with_value<
      InverseJacobian<DataType, Dim, Frame::Logical, Frame::Grid>>(point[0],
                                                                   0.);
  for (size_t target = 0; target < Dim; ++target) {
    for (size_t source = 0; source < Dim; ++source) {
      for (size_t dummy = 0; dummy < Dim; ++dummy) {
        result.get(source, target) +=
            inv_jac1.get(source, dummy) * inv_jac2.get(dummy, target);
      }
    }
  }
  return result;
}

void test_single_coordinate_map() {
  using affine_map1d = CoordinateMaps::Affine;

  const auto affine1d = make_coordinate_map<Frame::Logical, Frame::Grid>(
      affine_map1d{-1.0, 1.0, 2.0, 8.0});
  const auto affine1d_base =
      make_coordinate_map_base<Frame::Logical, Frame::Grid>(
          affine_map1d{-1.0, 1.0, 2.0, 8.0});
  const auto first_affine1d = affine_map1d{-1.0, 1.0, 2.0, 8.0};

  CHECK(affine1d == *affine1d_base);
  CHECK(*affine1d_base == affine1d);

  std::array<std::array<double, 1>, 4> coords1d{
      {{{0.1}}, {{-8.2}}, {{5.7}}, {{2.9}}}};

  for (const auto& coord : coords1d) {
    CHECK((make_array<double, 1>((*affine1d_base)(
              tnsr::I<double, 1, Frame::Logical>{{{coord[0]}}}))) ==
          first_affine1d(coord));
    CHECK((make_array<double, 1>(
              affine1d_base
                  ->inverse(tnsr::I<double, 1, Frame::Grid>{{{coord[0]}}})
                  .get())) == first_affine1d.inverse(coord).get());

    CHECK((make_array<double, 1>(affine1d(tnsr::I<double, 1, Frame::Logical>{
              {{coord[0]}}}))) == first_affine1d(coord));
    CHECK((make_array<double, 1>(
              affine1d.inverse(tnsr::I<double, 1, Frame::Grid>{{{coord[0]}}})
                  .get())) == first_affine1d.inverse(coord).get());

    const auto jac =
        affine1d.jacobian(tnsr::I<double, 1, Frame::Logical>{{{coord[0]}}});
    const auto expected_jac = first_affine1d.jacobian(coord);
    CHECK(affine1d_base
              ->jacobian(tnsr::I<double, 1, Frame::Logical>{{{coord[0]}}})
              .get(0, 0) == expected_jac.get(0, 0));
    CHECK(jac.get(0, 0) == expected_jac.get(0, 0));

    const auto inv_jac =
        affine1d.inv_jacobian(tnsr::I<double, 1, Frame::Logical>{{{coord[0]}}});
    const auto expected_inv_jac = first_affine1d.inv_jacobian(coord);
    CHECK(affine1d_base
              ->inv_jacobian(tnsr::I<double, 1, Frame::Logical>{{{coord[0]}}})
              .get(0, 0) == expected_inv_jac.get(0, 0));
    CHECK(inv_jac.get(0, 0) == expected_inv_jac.get(0, 0));
  }

  using rotate2d = CoordinateMaps::Rotation<2>;

  const auto first_rotated2d = rotate2d{M_PI_4};
  const auto rotated2d =
      make_coordinate_map<Frame::Logical, Frame::Grid>(first_rotated2d);
  const auto rotated2d_base =
      make_coordinate_map_base<Frame::Logical, Frame::Grid>(first_rotated2d);

  CHECK(rotated2d == *rotated2d_base);
  CHECK(*rotated2d_base == rotated2d);

  std::array<std::array<double, 2>, 4> coords2d{
      {{{0.1, 2.8}}, {{-8.2, 2.8}}, {{5.7, -4.9}}, {{2.9, 3.4}}}};

  for (const auto& coord : coords2d) {
    CHECK((make_array<double, 2>((*rotated2d_base)(
              tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}}))) ==
          first_rotated2d(coord));
    CHECK((make_array<double, 2>(rotated2d_base
                                     ->inverse(tnsr::I<double, 2, Frame::Grid>{
                                         {{coord[0], coord[1]}}})
                                     .get())) ==
          first_rotated2d.inverse(coord).get());

    CHECK((make_array<double, 2>(rotated2d(tnsr::I<double, 2, Frame::Logical>{
              {{coord[0], coord[1]}}}))) == first_rotated2d(coord));
    CHECK((make_array<double, 2>(rotated2d
                                     .inverse(tnsr::I<double, 2, Frame::Grid>{
                                         {{coord[0], coord[1]}}})
                                     .get())) ==
          first_rotated2d.inverse(coord).get());

    const auto jac = rotated2d.jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}});
    const auto jac2 = rotated2d_base->jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}});
    const auto expected_jac = first_rotated2d.jacobian(coord);
    for (size_t j = 0; j < 2; ++j) {
      for (size_t k = 0; k < 2; ++k) {
        CHECK(jac.get(j, k) == expected_jac.get(j, k));
        CHECK(jac2.get(j, k) == expected_jac.get(j, k));
      }
    }

    const auto inv_jac = rotated2d.inv_jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}});
    const auto inv_jac2 = rotated2d_base->inv_jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}});
    const auto expected_inv_jac = first_rotated2d.inv_jacobian(coord);
    for (size_t j = 0; j < 2; ++j) {
      for (size_t k = 0; k < 2; ++k) {
        CHECK(inv_jac.get(j, k) == expected_inv_jac.get(j, k));
        CHECK(inv_jac2.get(j, k) == expected_inv_jac.get(j, k));
      }
    }
  }

  using rotate3d = CoordinateMaps::Rotation<3>;

  const auto first_rotated3d = rotate3d{M_PI_4, M_PI_4, M_PI_2};
  const auto rotated3d =
      make_coordinate_map<Frame::Logical, Frame::Grid>(first_rotated3d);
  const auto rotated3d_base =
      make_coordinate_map_base<Frame::Logical, Frame::Grid>(first_rotated3d);

  CHECK(rotated3d == *rotated3d_base);
  CHECK(*rotated3d_base == rotated3d);

  std::array<std::array<double, 3>, 4> coords3d{{{{0.1, 2.8, 9.3}},
                                                 {{-8.2, 2.8, -9.7}},
                                                 {{5.7, -4.9, 8.1}},
                                                 {{2.9, 3.4, -7.8}}}};

  for (const auto& coord : coords3d) {
    CHECK((make_array<double, 3>((
              *rotated3d_base)(tnsr::I<double, 3, Frame::Logical>{
              {{coord[0], coord[1], coord[2]}}}))) == first_rotated3d(coord));
    CHECK((make_array<double, 3>(rotated3d_base
                                     ->inverse(tnsr::I<double, 3, Frame::Grid>{
                                         {{coord[0], coord[1], coord[2]}}})
                                     .get())) ==
          first_rotated3d.inverse(coord).get());

    CHECK((make_array<double, 3>(rotated3d(tnsr::I<double, 3, Frame::Logical>{
              {{coord[0], coord[1], coord[2]}}}))) == first_rotated3d(coord));
    CHECK((make_array<double, 3>(rotated3d
                                     .inverse(tnsr::I<double, 3, Frame::Grid>{
                                         {{coord[0], coord[1], coord[2]}}})
                                     .get())) ==
          first_rotated3d.inverse(coord).get());

    const auto jac = rotated3d.jacobian(
        tnsr::I<double, 3, Frame::Logical>{{{coord[0], coord[1], coord[2]}}});
    const auto jac2 = rotated3d_base->jacobian(
        tnsr::I<double, 3, Frame::Logical>{{{coord[0], coord[1], coord[2]}}});
    const auto expected_jac = first_rotated3d.jacobian(coord);
    for (size_t j = 0; j < 3; ++j) {
      for (size_t k = 0; k < 3; ++k) {
        CHECK(jac.get(j, k) == expected_jac.get(j, k));
        CHECK(jac2.get(j, k) == expected_jac.get(j, k));
      }
    }

    const auto inv_jac = rotated3d.inv_jacobian(
        tnsr::I<double, 3, Frame::Logical>{{{coord[0], coord[1], coord[2]}}});
    const auto inv_jac2 = rotated3d_base->inv_jacobian(
        tnsr::I<double, 3, Frame::Logical>{{{coord[0], coord[1], coord[2]}}});
    const auto expected_inv_jac = first_rotated3d.inv_jacobian(coord);
    for (size_t j = 0; j < 3; ++j) {
      for (size_t k = 0; k < 3; ++k) {
        CHECK(inv_jac.get(j, k) == expected_inv_jac.get(j, k));
        CHECK(inv_jac2.get(j, k) == expected_inv_jac.get(j, k));
      }
    }
  }
}

void test_coordinate_map_with_affine_map() {
  using affine_map = CoordinateMaps::Affine;
  using affine_map_2d = CoordinateMaps::ProductOf2Maps<affine_map, affine_map>;
  using affine_map_3d =
      CoordinateMaps::ProductOf3Maps<affine_map, affine_map, affine_map>;

  constexpr size_t number_of_points_checked = 10;

  // Test 1D
  const auto map = make_coordinate_map<Frame::Logical, Frame::Grid>(
      affine_map{-1.0, 1.0, 0.0, 2.3}, affine_map{0.0, 2.3, -0.5, 0.5});
  for (size_t i = 1; i < number_of_points_checked + 1; ++i) {
    CHECK((tnsr::I<double, 1, Frame::Grid>(1.0 / i + -0.5))[0] ==
          approx(map(tnsr::I<double, 1, Frame::Logical>{2.0 / i + -1.0})[0]));
    CHECK((tnsr::I<double, 1, Frame::Logical>(2.0 / i + -1.0))[0] ==
          approx(map.inverse(tnsr::I<double, 1, Frame::Grid>{1.0 / i + -0.5})
                     .get()[0]));

    CHECK(approx(map.inv_jacobian(
                        tnsr::I<double, 1, Frame::Logical>{2.0 / i + -1.0})
                     .get(0, 0)) == 2.0);
    CHECK(
        approx(map.jacobian(tnsr::I<double, 1, Frame::Logical>{2.0 / i + -1.0})
                   .get(0, 0)) == 0.5);
  }

  // Test 2D
  const auto prod_map2d = make_coordinate_map<Frame::Logical, Frame::Grid>(
      affine_map_2d{affine_map{-1.0, 1.0, 0.0, 2.0},
                    affine_map{0.0, 2.0, -0.5, 0.5}},
      affine_map_2d{affine_map{0.0, 2.0, 2.0, 6.0},
                    affine_map{-0.5, 0.5, 0.0, 8.0}});
  for (size_t i = 1; i < number_of_points_checked + 1; ++i) {
    const auto mapped_point = prod_map2d(
        tnsr::I<double, 2, Frame::Logical>{{{-1.0 + 2.0 / i, 0.0 + 2.0 / i}}});
    const auto expected_mapped_point =
        tnsr::I<double, 2, Frame::Grid>{{{4.0 / i + 2.0, 8.0 / i + 0.0}}};
    CHECK(get<0>(expected_mapped_point) == approx(get<0>(mapped_point)));
    CHECK(get<1>(expected_mapped_point) == approx(get<1>(mapped_point)));

    const auto inv_mapped_point = prod_map2d
                                      .inverse(tnsr::I<double, 2, Frame::Grid>{
                                          {{4.0 / i + 2.0, 8.0 / i + 0.0}}})
                                      .get();
    const auto expected_inv_mapped_point =
        tnsr::I<double, 2, Frame::Grid>{{{-1.0 + 2.0 / i, 0.0 + 2.0 / i}}};
    CHECK(get<0>(expected_inv_mapped_point) ==
          approx(get<0>(inv_mapped_point)));
    CHECK(get<1>(expected_inv_mapped_point) ==
          approx(get<1>(inv_mapped_point)));

    const auto inv_jac = prod_map2d.inv_jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{-1.0 + 2.0 / i, 0.0 + 2.0 / i}}});
    CHECK(0.5 == approx(get<0, 0>(inv_jac)));
    CHECK(0.0 == approx(get<1, 0>(inv_jac)));
    CHECK(0.0 == approx(get<0, 1>(inv_jac)));
    CHECK(0.25 == approx(get<1, 1>(inv_jac)));

    const auto jac = prod_map2d.jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{-1.0 + 2.0 / i, 0.0 + 2.0 / i}}});
    CHECK(2.0 == approx(get<0, 0>(jac)));
    CHECK(0.0 == approx(get<1, 0>(jac)));
    CHECK(0.0 == approx(get<0, 1>(jac)));
    CHECK(4.0 == approx(get<1, 1>(jac)));
  }

  // Test 3D
  const auto prod_map3d = make_coordinate_map<Frame::Logical, Frame::Grid>(
      affine_map_3d{affine_map{-1.0, 1.0, 0.0, 2.0},
                    affine_map{0.0, 2.0, -0.5, 0.5},
                    affine_map{5.0, 7.0, -7.0, 7.0}},
      affine_map_3d{affine_map{0.0, 2.0, 2.0, 6.0},
                    affine_map{-0.5, 0.5, 0.0, 8.0},
                    affine_map{-7.0, 7.0, 3.0, 23.0}});

  for (size_t i = 1; i < number_of_points_checked + 1; ++i) {
    const auto mapped_point = prod_map3d(tnsr::I<double, 3, Frame::Logical>{
        {{-1.0 + 2.0 / i, 0.0 + 2.0 / i, 5.0 + 2.0 / i}}});
    const auto expected_mapped_point = tnsr::I<double, 3, Frame::Grid>{
        {{4.0 / i + 2.0, 8.0 / i + 0.0, 3.0 + 20.0 / i}}};
    CHECK(get<0>(expected_mapped_point) == approx(get<0>(mapped_point)));
    CHECK(get<1>(expected_mapped_point) == approx(get<1>(mapped_point)));
    CHECK(get<2>(expected_mapped_point) == approx(get<2>(mapped_point)));

    const auto inv_mapped_point =
        prod_map3d
            .inverse(tnsr::I<double, 3, Frame::Grid>{
                {{4.0 / i + 2.0, 8.0 / i + 0.0, 3.0 + 20.0 / i}}})
            .get();
    const auto expected_inv_mapped_point = tnsr::I<double, 3, Frame::Grid>{
        {{-1.0 + 2.0 / i, 0.0 + 2.0 / i, 5.0 + 2.0 / i}}};
    CHECK(get<0>(expected_inv_mapped_point) ==
          approx(get<0>(inv_mapped_point)));
    CHECK(get<1>(expected_inv_mapped_point) ==
          approx(get<1>(inv_mapped_point)));
    CHECK(get<2>(expected_inv_mapped_point) ==
          approx(get<2>(inv_mapped_point)));

    const auto inv_jac =
        prod_map3d.inv_jacobian(tnsr::I<double, 3, Frame::Logical>{
            {{-1.0 + 2.0 / i, 0.0 + 2.0 / i, 5.0 + 2.0 / i}}});
    CHECK(0.5 == approx(get<0, 0>(inv_jac)));
    CHECK(0.0 == approx(get<1, 0>(inv_jac)));
    CHECK(0.0 == approx(get<0, 1>(inv_jac)));
    CHECK(0.25 == approx(get<1, 1>(inv_jac)));
    CHECK(0.0 == approx(get<0, 2>(inv_jac)));
    CHECK(0.0 == approx(get<1, 2>(inv_jac)));
    CHECK(0.0 == approx(get<2, 0>(inv_jac)));
    CHECK(0.0 == approx(get<2, 1>(inv_jac)));
    CHECK(0.1 == approx(get<2, 2>(inv_jac)));

    const auto jac = prod_map3d.jacobian(tnsr::I<double, 3, Frame::Logical>{
        {{-1.0 + 2.0 / i, 0.0 + 2.0 / i, 5.0 + 2.0 / i}}});
    CHECK(2.0 == approx(get<0, 0>(jac)));
    CHECK(0.0 == approx(get<1, 0>(jac)));
    CHECK(0.0 == approx(get<0, 1>(jac)));
    CHECK(4.0 == approx(get<1, 1>(jac)));
    CHECK(0.0 == approx(get<0, 2>(jac)));
    CHECK(0.0 == approx(get<1, 2>(jac)));
    CHECK(0.0 == approx(get<2, 0>(jac)));
    CHECK(0.0 == approx(get<2, 1>(jac)));
    CHECK(10.0 == approx(get<2, 2>(jac)));
  }
}

void test_coordinate_map_with_rotation_map() {
  using rotate2d = CoordinateMaps::Rotation<2>;
  using rotate3d = CoordinateMaps::Rotation<3>;

  // No 1D test because it would just the be affine map test

  // Test 2D
  const auto double_rotated2d =
      make_coordinate_map<Frame::Logical, Frame::Grid>(rotate2d{M_PI_4},
                                                       rotate2d{M_PI_2});
  const auto first_rotated2d = rotate2d{M_PI_4};
  const auto second_rotated2d = rotate2d{M_PI_2};

  std::array<std::array<double, 2>, 4> coords2d{
      {{{0.1, 2.8}}, {{-8.2, 2.8}}, {{5.7, -4.9}}, {{2.9, 3.4}}}};

  for (size_t i = 0; i < coords2d.size(); ++i) {
    INFO(i);
    const auto coord = gsl::at(coords2d, i);
    CHECK((make_array<double, 2>(double_rotated2d(
              tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}}))) ==
          second_rotated2d(first_rotated2d(coord)));
    CHECK((make_array<double, 2>(double_rotated2d
                                     .inverse(tnsr::I<double, 2, Frame::Grid>{
                                         {{coord[0], coord[1]}}})
                                     .get())) ==
          first_rotated2d.inverse(second_rotated2d.inverse(coord).get()).get());

    const auto jac = double_rotated2d.jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}});
    const auto expected_jac = compose_jacobians(
        first_rotated2d, second_rotated2d, gsl::at(coords2d, i));
    CHECK_ITERABLE_APPROX(jac, expected_jac);

    const auto inv_jac = double_rotated2d.inv_jacobian(
        tnsr::I<double, 2, Frame::Logical>{{{coord[0], coord[1]}}});
    const auto expected_inv_jac = compose_inv_jacobians(
        first_rotated2d, second_rotated2d, gsl::at(coords2d, i));
    CHECK_ITERABLE_APPROX(inv_jac, expected_inv_jac);
  }

  // Test 3D
  const auto double_rotated3d =
      make_coordinate_map<Frame::Logical, Frame::Grid>(
          rotate3d{M_PI_4, M_PI_4, M_PI_2}, rotate3d{M_PI_2, M_PI_4, M_PI_4});
  const auto first_rotated3d = rotate3d{M_PI_4, M_PI_4, M_PI_2};
  const auto second_rotated3d = rotate3d{M_PI_2, M_PI_4, M_PI_4};

  std::array<std::array<double, 3>, 4> coords3d{{{{0.1, 2.8, 9.3}},
                                                 {{-8.2, 2.8, -9.7}},
                                                 {{5.7, -4.9, 8.1}},
                                                 {{2.9, 3.4, -7.8}}}};

  for (size_t i = 0; i < coords3d.size(); ++i) {
    INFO(i);
    const auto coord = gsl::at(coords3d, i);
    CHECK((make_array<double, 3>(
              double_rotated3d(tnsr::I<double, 3, Frame::Logical>{
                  {{coord[0], coord[1], coord[2]}}}))) ==
          second_rotated3d(first_rotated3d(coord)));
    CHECK((make_array<double, 3>(double_rotated3d
                                     .inverse(tnsr::I<double, 3, Frame::Grid>{
                                         {{coord[0], coord[1], coord[2]}}})
                                     .get())) ==
          first_rotated3d.inverse(second_rotated3d.inverse(coord).get()).get());

    const auto jac = double_rotated3d.jacobian(
        tnsr::I<double, 3, Frame::Logical>{{{coord[0], coord[1], coord[2]}}});
    const auto expected_jac = compose_jacobians(
        first_rotated3d, second_rotated3d, gsl::at(coords3d, i));
    CHECK_ITERABLE_APPROX(jac, expected_jac);

    const auto inv_jac = double_rotated3d.inv_jacobian(
        tnsr::I<double, 3, Frame::Logical>{{{coord[0], coord[1], coord[2]}}});
    const auto expected_inv_jac = compose_inv_jacobians(
        first_rotated3d, second_rotated3d, gsl::at(coords3d, i));
    CHECK_ITERABLE_APPROX(inv_jac, expected_inv_jac);
  }

  // Check inequivalence operator
  CHECK_FALSE(double_rotated3d != double_rotated3d);
  test_serialization(double_rotated3d);
}

void test_coordinate_map_with_rotation_map_datavector() {
  using rotate2d = CoordinateMaps::Rotation<2>;
  using rotate3d = CoordinateMaps::Rotation<3>;

  // No 1D test because it would just the be affine map test

  // Test 2D
  {
    const auto double_rotated2d =
        make_coordinate_map<Frame::Logical, Frame::Grid>(rotate2d{M_PI_4},
                                                         rotate2d{M_PI_2});
    const auto first_rotated2d = rotate2d{M_PI_4};
    const auto second_rotated2d = rotate2d{M_PI_2};

    const tnsr::I<DataVector, 2, Frame::Logical> coords2d{
        {{DataVector{0.1, -8.2, 5.7, 2.9}, DataVector{2.8, 2.8, -4.9, 3.4}}}};
    const tnsr::I<DataVector, 2, Frame::Grid> coords2d_grid{
        {{DataVector{0.1, -8.2, 5.7, 2.9}, DataVector{2.8, 2.8, -4.9, 3.4}}}};
    const auto coords2d_array = make_array<DataVector, 2>(coords2d);

    CHECK((make_array<DataVector, 2>(double_rotated2d(coords2d))) ==
          second_rotated2d(first_rotated2d(coords2d_array)));

    const auto jac = double_rotated2d.jacobian(coords2d);
    const auto expected_jac =
        compose_jacobians(first_rotated2d, second_rotated2d, coords2d_array);
    CHECK_ITERABLE_APPROX(jac, expected_jac);

    const auto inv_jac = double_rotated2d.inv_jacobian(coords2d);
    const auto expected_inv_jac = compose_inv_jacobians(
        first_rotated2d, second_rotated2d, coords2d_array);
    CHECK_ITERABLE_APPROX(inv_jac, expected_inv_jac);
  }

  // Test 3D
  {
    const auto first_rotated3d = rotate3d{M_PI_4, M_PI_4, M_PI_2};
    const auto second_rotated3d = rotate3d{M_PI_2, M_PI_4, M_PI_4};
    const auto double_rotated3d_full =
        make_coordinate_map<Frame::Logical, Frame::Grid>(first_rotated3d,
                                                         second_rotated3d);
    const auto double_rotated3d_base =
        make_coordinate_map_base<Frame::Logical, Frame::Grid>(first_rotated3d,
                                                              second_rotated3d);
    const auto& double_rotated3d = *double_rotated3d_base;

    CHECK(double_rotated3d_full == double_rotated3d);

    const auto different_rotated3d_base =
        make_coordinate_map_base<Frame::Logical, Frame::Grid>(second_rotated3d,
                                                              first_rotated3d);
    CHECK(*different_rotated3d_base == *different_rotated3d_base);
    CHECK(*different_rotated3d_base != double_rotated3d);

    const tnsr::I<DataVector, 3, Frame::Logical> coords3d{
        {{DataVector{0.1, -8.2, 5.7, 2.9}, DataVector{2.8, 2.8, -4.9, 3.4},
          DataVector{9.3, -9.7, 8.1, -7.8}}}};
    const tnsr::I<DataVector, 3, Frame::Grid> coords3d_grid{
        {{DataVector{0.1, -8.2, 5.7, 2.9}, DataVector{2.8, 2.8, -4.9, 3.4},
          DataVector{9.3, -9.7, 8.1, -7.8}}}};
    const auto coords3d_array = make_array<DataVector, 3>(coords3d);

    CHECK((make_array<DataVector, 3>(double_rotated3d(coords3d))) ==
          second_rotated3d(first_rotated3d(coords3d_array)));

    const auto jac = double_rotated3d.jacobian(coords3d);
    const auto expected_jac =
        compose_jacobians(first_rotated3d, second_rotated3d, coords3d_array);
    CHECK_ITERABLE_APPROX(jac, expected_jac);

    const auto inv_jac = double_rotated3d.inv_jacobian(coords3d);
    const auto expected_inv_jac = compose_inv_jacobians(
        first_rotated3d, second_rotated3d, coords3d_array);
    CHECK_ITERABLE_APPROX(inv_jac, expected_inv_jac);

    // Check inequivalence operator
    CHECK_FALSE(double_rotated3d_full != double_rotated3d_full);
    test_serialization(double_rotated3d_full);
  }
}

void test_coordinate_map_with_rotation_wedge() {
  using Rotate = CoordinateMaps::Rotation<2>;
  using Wedge2D = CoordinateMaps::Wedge2D;

  const auto first_map = Rotate(2.);
  const auto second_map =
      Wedge2D(3., 7., 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::lower_eta(), Direction<2>::lower_xi()}}},
              false);

  const auto composed_map =
      make_coordinate_map<Frame::Logical, Frame::Grid>(first_map, second_map);

  const std::array<double, 2> test_point_array{{0.1, 0.8}};
  const tnsr::I<double, 2, Frame::Logical> test_point_vector(test_point_array);

  const auto mapped_point_array = second_map(first_map(test_point_array));
  const auto mapped_point_vector = composed_map(test_point_vector);
  CHECK((make_array<double, 2>(mapped_point_vector)) == mapped_point_array);

  const auto jac = composed_map.jacobian(test_point_vector);
  const auto expected_jac =
      compose_jacobians(first_map, second_map, test_point_array);
  CHECK_ITERABLE_APPROX(jac, expected_jac);

  const auto inv_jac = composed_map.inv_jacobian(test_point_vector);
  const auto expected_inv_jac =
      compose_inv_jacobians(first_map, second_map, test_point_array);
  CHECK_ITERABLE_APPROX(inv_jac, expected_inv_jac);
}

void test_make_vector_coordinate_map_base() {
  using Affine = CoordinateMaps::Affine;
  using Affine2D = CoordinateMaps::ProductOf2Maps<Affine, Affine>;

  const auto affine1d = make_coordinate_map<Frame::Logical, Frame::Grid>(
      Affine{-1.0, 1.0, 2.0, 8.0});
  const auto affine1d_base =
      make_coordinate_map_base<Frame::Logical, Frame::Grid>(
          Affine{-1.0, 1.0, 2.0, 8.0});
  const auto vector_of_affine1d =
      make_vector_coordinate_map_base<Frame::Logical, Frame::Grid>(
          Affine{-1.0, 1.0, 2.0, 8.0});

  CHECK(affine1d == *affine1d_base);
  CHECK(*affine1d_base == affine1d);
  CHECK(affine1d == *(vector_of_affine1d[0]));
  CHECK(*(vector_of_affine1d[0]) == affine1d);

  using Wedge2DMap = CoordinateMaps::Wedge2D;
  const auto upper_xi_wedge =
      Wedge2DMap{1.0,
                 2.0,
                 0.0,
                 1.0,
                 OrientationMap<2>{std::array<Direction<2>, 2>{
                     {Direction<2>::upper_xi(), Direction<2>::upper_eta()}}},
                 true};
  const auto upper_eta_wedge =
      Wedge2DMap{1.0,
                 2.0,
                 0.0,
                 1.0,
                 OrientationMap<2>{std::array<Direction<2>, 2>{
                     {Direction<2>::upper_eta(), Direction<2>::lower_xi()}}},
                 true};
  const auto lower_xi_wedge =
      Wedge2DMap{1.0,
                 2.0,
                 0.0,
                 1.0,
                 OrientationMap<2>{std::array<Direction<2>, 2>{
                     {Direction<2>::lower_xi(), Direction<2>::lower_eta()}}},
                 true};
  const auto lower_eta_wedge =
      Wedge2DMap{1.0,
                 2.0,
                 0.0,
                 1.0,
                 OrientationMap<2>{std::array<Direction<2>, 2>{
                     {Direction<2>::lower_eta(), Direction<2>::upper_xi()}}},
                 true};
  const auto vector_of_wedges =
      make_vector_coordinate_map_base<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::upper_xi(), Direction<2>::upper_eta()}}},
              true},
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::upper_eta(), Direction<2>::lower_xi()}}},
              true},
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::lower_xi(), Direction<2>::lower_eta()}}},
              true},
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::lower_eta(), Direction<2>::upper_xi()}}},
              true});

  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(upper_xi_wedge) ==
        *(vector_of_wedges[0]));
  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(upper_eta_wedge) ==
        *(vector_of_wedges[1]));
  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(lower_xi_wedge) ==
        *(vector_of_wedges[2]));
  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(lower_eta_wedge) ==
        *(vector_of_wedges[3]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            upper_xi_wedge) == *(vector_of_wedges[0]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            upper_eta_wedge) == *(vector_of_wedges[1]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            lower_xi_wedge) == *(vector_of_wedges[2]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            lower_eta_wedge) == *(vector_of_wedges[3]));

  const auto wedges = std::vector<Wedge2DMap>{upper_xi_wedge, upper_eta_wedge,
                                              lower_xi_wedge, lower_eta_wedge};
  const auto vector_of_wedges2 =
      make_vector_coordinate_map_base<Frame::Logical, Frame::Inertial, 2>(
          wedges);
  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(upper_xi_wedge) ==
        *(vector_of_wedges2[0]));
  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(upper_eta_wedge) ==
        *(vector_of_wedges2[1]));
  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(lower_xi_wedge) ==
        *(vector_of_wedges2[2]));
  CHECK(make_coordinate_map<Frame::Logical, Frame::Inertial>(lower_eta_wedge) ==
        *(vector_of_wedges2[3]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            upper_xi_wedge) == *(vector_of_wedges2[0]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            upper_eta_wedge) == *(vector_of_wedges2[1]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            lower_xi_wedge) == *(vector_of_wedges2[2]));
  CHECK(*make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
            lower_eta_wedge) == *(vector_of_wedges2[3]));

  const auto translation =
      Affine2D{Affine{-1.0, 1.0, -1.0, 1.0}, Affine{-1.0, 1.0, 0.0, 2.0}};
  const auto vector_of_translated_wedges =
      make_vector_coordinate_map_base<Frame::Logical, Frame::Inertial, 2>(
          wedges, translation);

  const auto translated_upper_xi_wedge =
      make_coordinate_map<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::upper_xi(), Direction<2>::upper_eta()}}},
              true},
          translation);
  const auto translated_upper_eta_wedge =
      make_coordinate_map<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::upper_eta(), Direction<2>::lower_xi()}}},
              true},
          translation);
  const auto translated_lower_xi_wedge =
      make_coordinate_map<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::lower_xi(), Direction<2>::lower_eta()}}},
              true},
          translation);
  const auto translated_lower_eta_wedge =
      make_coordinate_map<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::lower_eta(), Direction<2>::upper_xi()}}},
              true},
          translation);
  const auto translated_upper_xi_wedge_base =
      make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::upper_xi(), Direction<2>::upper_eta()}}},
              true},
          translation);
  const auto translated_upper_eta_wedge_base =
      make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::upper_eta(), Direction<2>::lower_xi()}}},
              true},
          translation);
  const auto translated_lower_xi_wedge_base =
      make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::lower_xi(), Direction<2>::lower_eta()}}},
              true},
          translation);
  const auto translated_lower_eta_wedge_base =
      make_coordinate_map_base<Frame::Logical, Frame::Inertial>(
          Wedge2DMap{
              1.0, 2.0, 0.0, 1.0,
              OrientationMap<2>{std::array<Direction<2>, 2>{
                  {Direction<2>::lower_eta(), Direction<2>::upper_xi()}}},
              true},
          translation);

  CHECK(translated_upper_xi_wedge == *(vector_of_translated_wedges[0]));
  CHECK(translated_upper_eta_wedge == *(vector_of_translated_wedges[1]));
  CHECK(translated_lower_xi_wedge == *(vector_of_translated_wedges[2]));
  CHECK(translated_lower_eta_wedge == *(vector_of_translated_wedges[3]));
  CHECK(*translated_upper_xi_wedge_base == *(vector_of_translated_wedges[0]));
  CHECK(*translated_upper_eta_wedge_base == *(vector_of_translated_wedges[1]));
  CHECK(*translated_lower_xi_wedge_base == *(vector_of_translated_wedges[2]));
  CHECK(*translated_lower_eta_wedge_base == *(vector_of_translated_wedges[3]));
}
}  // namespace

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMap", "[Domain][Unit]") {
  test_single_coordinate_map();
  test_coordinate_map_with_affine_map();
  test_coordinate_map_with_rotation_map();
  test_coordinate_map_with_rotation_map_datavector();
  test_coordinate_map_with_rotation_wedge();
  test_make_vector_coordinate_map_base();
}


/**
 * To optimize over the set of a unit 2-sphere
 */

#ifndef WAVE_SPHERICAL_PARAMETERIZATION_HPP
#define WAVE_SPHERICAL_PARAMETERIZATION_HPP

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/local_parameterization.h>
#include "wave/utils/math.hpp"

namespace wave {

class SphericalParameterization : public ceres::LocalParameterization {
 public:
    virtual ~SphericalParameterization() {}
    virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const;
    virtual bool ComputeJacobian(const double* x, double* jacobian) const;
    virtual int GlobalSize() const { return 3; }
    virtual int LocalSize() const { return 2; }
};

}

#endif //WAVE_SPHERICAL_PARAMETERIZATION_HPP


/**
 * @file dense_inversion.cpp
 * @brief Benchmarks the inversion of 1 random square matrix of increasing size
 *
 * @author Matthew Powelson
 * @date April 1, 2020
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2020, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <benchmark/benchmark.h>

#include <arrayfire.h>
#include <Eigen/Eigen>
#include <torch/torch.h>

auto BM_PYTORCH_INV = [](benchmark::State& state, int size, torch::Device device) {
  torch::TensorOptions options =
      torch::TensorOptions().dtype(torch::kFloat64).layout(torch::kStrided).device(device).requires_grad(true);

  torch::manual_seed(0);
  torch::Tensor tensor0 = torch::rand({ size, size }, options);

  torch::Tensor result = torch::rand({ size, size }, options);
  for (auto _ : state)
  {
    benchmark::DoNotOptimize(result = tensor0.inverse());
  }
};

auto BM_EIGEN_INV = [](benchmark::State& state, int size) {
  Eigen::MatrixXd matrix0 = Eigen::MatrixXd::Random(size, size);
  Eigen::MatrixXd result = Eigen::MatrixXd::Random(size, size);
  for (auto _ : state)
  {
    benchmark::DoNotOptimize(result = matrix0.inverse());
  }
};

// TODO: Make sure the ArrayFire benchmarks are actually executing since they normally use lazy execution
auto BM_ARRAYFIRE_INV = [](benchmark::State& state, int size, auto device) {
  af::setBackend(device);

  af::setSeed(0);
  af::array array0 = af::randu(size, size);

  af::array result = af::randu(size, size);
  for (auto _ : state)
  {
    benchmark::DoNotOptimize(result = af::inverse(array0));
  }
};

int main(int argc, char** argv)
{
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_PYTORCH_CPU_INV_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_PYTORCH_INV, test_input, torch::kCPU)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_PYTORCH_GPU_INV_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_PYTORCH_INV, test_input, torch::kCUDA)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_EIGEN_INV_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_EIGEN_INV, test_input)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_ARRAYFIRE_CPU_INV_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_ARRAYFIRE_INV, test_input, AF_BACKEND_CPU)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_ARRAYFIRE_CUDA_INV_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_ARRAYFIRE_INV, test_input, AF_BACKEND_CUDA)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_ARRAYFIRE_OPENCL_INV_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_ARRAYFIRE_INV, test_input, AF_BACKEND_OPENCL)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  benchmark::Initialize(&argc, argv);
  benchmark::RunSpecifiedBenchmarks();
}


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * 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 project's author 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.
 *
 */

#ifndef __STATIMO_CORE_PCA_MODEL_BUILDER_HXX_
#define __STATIMO_CORE_PCA_MODEL_BUILDER_HXX_

#include "statismo/core/PCAModelBuilder.h"
#include "statismo/core/CommonTypes.h"
#include "statismo/core/Exceptions.h"

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

namespace statismo
{

template <typename T>
UniquePtrType<typename PCAModelBuilder<T>::StatisticalModelType>
PCAModelBuilder<T>::BuildNewModel(const DataItemListType & sampleDataList,
                                  double                   noiseVariance,
                                  bool                     computeScores,
                                  EigenValueMethod         method) const
{

  STATISMO_LOG_INFO("Building new model");
  STATISMO_LOG_INFO("Noise variance: " + std::to_string(noiseVariance));

  auto n = sampleDataList.size();
  STATISMO_LOG_INFO("Sample count: " + std::to_string(n));

  if (n <= 0)
  {
    throw StatisticalModelException("Provided empty sample set. Cannot build the sample matrix",
                                    Status::BAD_INPUT_ERROR);
  }

  unsigned     p = sampleDataList.front()->GetSampleVector().rows();
  const auto * representer = sampleDataList.front()->GetRepresenter();

  // Compute the mean vector mu
  VectorType mu = VectorType::Zero(p);

  for (const auto & item : sampleDataList)
  {
    assert(item->GetSampleVector().rows() == p);   // all samples must have same number of rows
    assert(item->GetRepresenter() == representer); // all samples have the same representer
    mu += item->GetSampleVector();
  }

  mu /= n;

  // Build the mean free sample matrix X0
  MatrixType matX0(n, p);
  unsigned   i{ 0 };
  for (const auto & item : sampleDataList)
  {
    matX0.row(i++) = item->GetSampleVector() - mu;
  }

  // build the model
  auto model = BuildNewModelInternal(representer, matX0, mu, noiseVariance, method);

  // compute the scores if requested
  MatrixType scores;
  if (computeScores)
  {
    scores = this->ComputeScores(sampleDataList, model.get());
  }

  typename BuilderInfo::ParameterInfoList bi;
  bi.emplace_back(BuilderInfo::KeyValuePair("NoiseVariance ", std::to_string(noiseVariance)));

  typename BuilderInfo::DataInfoList dataInfo;
  i = 0;
  for (const auto & item : sampleDataList)
  {
    STATISMO_LOG_INFO("Adding info about dataset with uri " + item->GetDatasetURI());
    std::ostringstream os;
    os << "URI_" << i++;
    dataInfo.emplace_back(os.str().c_str(), item->GetDatasetURI());
  }

  // finally add meta data to the model info
  ModelInfo::BuilderInfoList biList;
  biList.emplace_back("PCAModelBuilder", dataInfo, bi);

  model->SetModelInfo(ModelInfo{ scores, biList });

  return model;
}

template <typename T>
UniquePtrType<typename PCAModelBuilder<T>::StatisticalModelType>
PCAModelBuilder<T>::BuildNewModelInternal(const Representer<T> * representer,
                                          const MatrixType &     matX0,
                                          const VectorType &     mu,
                                          double                 noiseVariance,
                                          EigenValueMethod       method) const
{
  unsigned n = matX0.rows();
  unsigned p = matX0.cols();

  STATISMO_LOG_INFO("n: " + std::to_string(n));
  STATISMO_LOG_INFO("p: " + std::to_string(p));

  switch (method)
  {
    case EigenValueMethod::JACOBI_SVD:

      STATISMO_LOG_INFO("Using JACOBI_SVD method");

      using SVDType = Eigen::JacobiSVD<MatrixType>;
      using SVDDoublePrecisionType = Eigen::JacobiSVD<MatrixTypeDoublePrecision>;

      // We destinguish the case where we have more variables than samples and
      // the case where we have more samples than variable.
      // In the first case we compute the (smaller) inner product matrix instead of the full covariance matrix.
      // It is known that this has the same non-zero singular values as the covariance matrix.
      // Furthermore, it is possible to compute the corresponding eigenvectors of the covariance matrix from the
      // decomposition.

      if (n < p)
      {
        // we compute the eigenvectors of the covariance matrix by computing an SVD of the
        // n x n inner product matrix 1/(n-1) X0X0^T
        MatrixType             cov = matX0 * matX0.transpose() * 1.0 / (n - 1);
        SVDDoublePrecisionType svd(cov.cast<double>(), Eigen::ComputeThinV);
        VectorType             singularValues = svd.singularValues().cast<ScalarType>();
        MatrixType             matV = svd.matrixV().cast<ScalarType>();

        unsigned numComponentsAboveTolerance =
          ((singularValues.array() - noiseVariance - Superclass::sk_tolerance) > 0).count();

        // there can be at most n-1 nonzero singular values in this case. Everything else must be due to numerical
        // inaccuracies
        unsigned numComponentsToKeep = std::min(numComponentsAboveTolerance, n - 1);
        // compute the pseudo inverse of the square root of the singular values
        // which is then needed to recompute the PCA basis
        VectorType singSqrt = singularValues.array().sqrt();
        VectorType singSqrtInv = VectorType::Zero(singSqrt.rows());
        for (unsigned i = 0; i < numComponentsToKeep; i++)
        {
          assert(singSqrt(i) > Superclass::sk_tolerance);
          singSqrtInv(i) = 1.0 / singSqrt(i);
        }

        if (numComponentsToKeep == 0)
        {
          STATISMO_LOG_ERROR("No component to keep");
          throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
        }

        // we recover the eigenvectors U of the full covariance matrix from the eigenvectors V of the inner product
        // matrix. We use the fact that if we decompose X as X=UDV^T, then we get X^TX = UD^2U^T and XX^T = VD^2V^T
        // (exploiting the orthogonormality of the matrix U and V from the SVD). The additional factor sqrt(n-1) is to
        // compensate for the 1/sqrt(n-1) in the formula for the covariance matrix.

        MatrixType pcaBasis = matX0.transpose() * matV * singSqrtInv.asDiagonal();
        pcaBasis /= sqrt(n - 1.0);
        pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

        VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
        VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);

        auto model = StatisticalModelType::SafeCreate(representer, mu, pcaBasis, pcaVariance, noiseVariance);

        return model;
      }
      else // NOLINT
      {
        // we compute an SVD of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly
        SVDType    svd(matX0.transpose() * matX0, Eigen::ComputeThinU);
        VectorType singularValues = svd.singularValues();
        singularValues /= (n - 1.0);
        unsigned numComponentsToKeep =
          ((singularValues.array() - noiseVariance - Superclass::sk_tolerance) > 0).count();
        MatrixType pcaBasis = svd.matrixU();

        pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

        if (numComponentsToKeep == 0)
        {
          STATISMO_LOG_ERROR("No component to keep");
          throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
        }

        VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
        VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
        auto       model = StatisticalModelType::SafeCreate(representer, mu, pcaBasis, pcaVariance, noiseVariance);
        return model;
      }
      break;

    case EigenValueMethod::SELF_ADJOINT_EIGEN_SOLVER:
    {
      STATISMO_LOG_INFO("Using SELF_ADJOINT_EIGEN_SOLVER method");
      // we compute the eigenvalues/eigenvectors of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly

      using SelfAdjointEigenSolver = Eigen::SelfAdjointEigenSolver<MatrixType>;
      SelfAdjointEigenSolver es;
      es.compute(matX0.transpose() * matX0);
      VectorType eigenValues =
        es.eigenvalues().reverse(); // SelfAdjointEigenSolver orders the eigenvalues in increasing order
      eigenValues /= (n - 1.0);

      unsigned   numComponentsToKeep = ((eigenValues.array() - noiseVariance - Superclass::sk_tolerance) > 0).count();
      MatrixType pcaBasis = es.eigenvectors().rowwise().reverse();
      pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

      if (numComponentsToKeep == 0)
      {
        throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
      }

      VectorType sampleVarianceVector = eigenValues.topRows(numComponentsToKeep);
      VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
      auto       model = StatisticalModelType::SafeCreate(representer, mu, pcaBasis, pcaVariance, noiseVariance);
      return model;
    }
    break;

    default:
      throw StatisticalModelException("Unrecognized decomposition/eigenvalue solver method.");
  }
  return nullptr;
}

} // namespace statismo

#endif


#include <iostream> //std::cout
#include <chrono> //std::chrono::high_resolution_clock, std::chrono::duration_cast
#include <fstream> // std::ofstream
#include <filesystem> //fs::is_directory, fs::create_directory
#include <boost/progress.hpp> //boost::progress_display, boost::progress_timer
#include <numeric> //std::accumulate

#include "histogram.h"
#include "init.h"
#include "vars.h"
#include "evolve.h"

namespace fs = std::filesystem;
void print_parameters();

//three function declarations below are no longer in use
std::vector<std::vector<double>> evolve_and_hist(float time, std::vector <std::vector<double>> init_vel_matrix, std::vector <std::vector<double>> init_pos_matrix, std::list<double> bin_list);
void sample_xv_in_csv(std::string folder, std::string file, float time, std::vector <std::vector<double>> init_vel_matrix, std::vector <std::vector<double>> init_pos_matrix, bool exporting_vel);
void old_main();
/*
 * Runs hard rod gas simulation a number of times given by no_samples for each sampling_time, up until max_time. Set ALL parameters in vars.h
 * Saves the averaged density and current distribution time_series of each simulation run in two csv files.
 */
int main() {

    print_parameters();

    //Create a directory if there isn't one
    if (fs::is_directory(folder_name)) { std::cout << "N.B. Directory with this name already exists.\n\n"; }
    else { fs::create_directory(folder_name); }

    //Initialise csv files for exporting
    //Set file names
    std::string file_name = "T" + std::to_string(int(max_time)) + "_ST" + std::to_string(int(sampling_time)) + \
        "_V" + std::to_string(float(V)) + "_kickW" + std::to_string(kick_width) + "_temp" + std::to_string(float(T)) + \
        "_samples" + std::to_string(no_samples) \
        + "_rods" + std::to_string(N) + "_sys_length" + std::to_string(int(L)) + "_rodlength" + std::to_string(float(rod_length));

    std::ofstream rho_bar_csvfile;
    rho_bar_csvfile.open(".\\" + folder_name + "\\D_" + file_name + ".csv"); //Create csv file in which density data is stored

    std::ofstream j_bar_csvfile;
    j_bar_csvfile.open(".\\" + folder_name + "\\J_" + file_name + ".csv"); //Create csv file in which current data is stored

    if (fs::exists(".\\" + folder_name + "\\" + file_name)) {
        std::cout << "File name already exists - please rename. exiting";
        return 0;
    }
    else {

        //Initialise matrices of density + current data for reading to csv

        int no_time_samples = std::round(max_time / sampling_time + 1);
        int no_cells = std::round(L / cell_length);

        std::vector<std::vector<double>> rho_accumulator(no_time_samples, std::vector<double>(no_cells)); //density matrix size no_time_samples x no_cells
        std::vector<std::vector<double>> j_accumulator(no_time_samples, std::vector<double>(no_cells)); //current matrix size no_time_samples x no_cells

        //Initialise histogram bins

        std::list<double> bins_list = init_bins(std::round(L / cell_length));

        //Initalise progress bar
        std::cout << "Running simulation:" << "\n";
        unsigned int sample_size = no_samples;
        boost::progress_display show_progress(sample_size);

        //Start timing

        boost::progress_timer t;

        for (unsigned int i = 0; i < sample_size; i++) {

            //initialise sample positions + velocities from init.cpp

            std::vector<double> x_0 = init_uniform_positions();

            std::vector<double> v_0 = init_velocities();

            if (implementing_v_kick) { v_0 = velocity_kick(kick_centre, V, kick_width, v_0, x_0); }

            for (int j = 0; j < static_cast<int> (max_time / sampling_time) + 1; j++) {

                std::vector<std::vector<double>> xv_evolved = evolve_to(sampling_time * j, x_0, v_0); // call evolve.cpp function

                std::vector<std::vector<double>> rho_j_dist_matrix = rho_j_dist(xv_evolved[0], xv_evolved[1], bins_list); // call histogram.cpp function

                //std::cout << std::accumulate(rho_j_dist_matrix[0].begin(), rho_j_dist_matrix[0].end(), 0); //print out sum of the bins

                //add to density and current containers
                std::transform(rho_accumulator[j].begin(), rho_accumulator[j].end(), rho_j_dist_matrix[0].begin(), rho_accumulator[j].begin(), std::plus<double>());
                std::transform(j_accumulator[j].begin(), j_accumulator[j].end(), rho_j_dist_matrix[1].begin(), j_accumulator[j].begin(), std::plus<double>());

            }

            ++show_progress;

        }
        int sample_no = no_samples; //no_samples is a const so can't be used in a lambda

        for (int i = 0; i < no_time_samples; i++) {
            //normalise rho and j by sample_no
            std::transform(rho_accumulator[i].begin(), rho_accumulator[i].end(), rho_accumulator[i].begin(), [&sample_no](auto& c) {return c / sample_no;});
            std::transform(j_accumulator[i].begin(), j_accumulator[i].end(), j_accumulator[i].begin(), [&sample_no](auto& c) {return c / sample_no;});
        }
        //write rho_accumulator and j_accumulator matrices to csv files

        if (rho_bar_csvfile.is_open()) {

            for (unsigned i = 0; i < rho_accumulator.size(); i++) {
                for (unsigned j = 0; j < rho_accumulator[i].size() - 1; j++) {
                    rho_bar_csvfile << rho_accumulator[i][j] << ",";
                }
                rho_bar_csvfile << rho_accumulator[i].back() << "\n"; //last entry on row doesn't require a comma
            }
        }
        else {
            std::cout << "\nError opening file.";
        }

        if (j_bar_csvfile.is_open()) {

            for (unsigned i = 0; i < j_accumulator.size(); i++) {
                for (unsigned j = 0; j < j_accumulator[i].size() - 1; j++) {
                    j_bar_csvfile << j_accumulator[i][j] << ",";
                }
                j_bar_csvfile << j_accumulator[i].back() << "\n"; //last entry on row doesn't require a comma
            }
        }
        else {
            std::cout << "\nError opening file.";
        }
        //End of simulation
        std::cout << "\n\nHard rod simulation has completed with runtime of ";
    }
}

/*
 * Prints parameters in vars.h
 */
void print_parameters() {
    std::cout << "Parameters chosen:\n";
    std::cout << "Max_Time: " << max_time << " Sampling_Time: " << sampling_time << " No_Samples: " << no_samples << "\n";
    std::cout << "No_Rods: " << N << " Temp: " << T << " Sys_Length: " << L << " Rod_Length: " << std::to_string(rod_length) << " Cell_Length: " << std::to_string(cell_length) << "\n\n";
    std::cout << "Position sampling method: " << posn_init_method << "\n";

    if (posn_init_method == "init_densityjump") {
        std::cout << "Left_weight: " << l_weight << " Right_weight: " << r_weight << "\n";
    }

    if (implementing_v_kick) {
        std::cout << "Implementing_v_kick: Yes" << "\n";
        std:: cout << "Kick_Centre: " << kick_centre << " Kick_Width: " << kick_width << " Kick_Strength: " << V << "\n\n";
    }
    else {
        std::cout << "Implementing_v_kick: No" << "\n";
    }
}

/*
* Calls evolve.cpp and hist.cpp functions - returns averaged density and current distributions for each time.
*/
std::vector<std::vector<double>> evolve_and_hist(float time, std::vector <std::vector<double>> init_vel_matrix, std::vector <std::vector<double>> init_pos_matrix, std::list<double> bin_list) {

    std::vector<double> rho_accumulator(std::round(L / cell_length)); //initialise vector which adds the density distributions for each sample
    std::vector<double> j_accumulator(std::round(L / cell_length)); //initialise vector which adds the current distributions for each sample

    for (size_t i = 0; i < init_vel_matrix.size(); i++) {

        auto start = std::chrono::high_resolution_clock::now();

        //std::cout << "\nRun " << i << " is in progress...";
        auto iter_start = std::chrono::high_resolution_clock::now();

        //Call evolve.cpp function
        auto xv_matrix = evolve_to(time, init_pos_matrix[i], init_vel_matrix[i]); // x_evolved = xv_matrix[0]; v_evolved = xv_matrix[1]

        //histogram call
        auto rho_j_dist_matrix = rho_j_dist(xv_matrix[0], xv_matrix[1],bin_list);

        //add to density and current containers
        std::transform(rho_accumulator.begin(), rho_accumulator.end(), rho_j_dist_matrix[0].begin(), rho_accumulator.begin(), std::plus<double>());
        std::transform(j_accumulator.begin(), j_accumulator.end(), rho_j_dist_matrix[1].begin(), j_accumulator.begin(), std::plus<double>());

        // Print time data
        auto iter_stop = std::chrono::high_resolution_clock::now();
        auto iter_duration = std::chrono::duration_cast<std::chrono::microseconds>(iter_stop - iter_start);
        auto time_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(iter_stop - start);
    }
    //normalise density and current containers by no_samples
    int sample_no = no_samples;
    std::transform(rho_accumulator.begin(), rho_accumulator.end(), rho_accumulator.begin(), [&sample_no](auto& c) {return c / sample_no;});
    std::transform(j_accumulator.begin(), j_accumulator.end(), j_accumulator.begin(), [& sample_no](auto& c) {return c / sample_no;});

    return { rho_accumulator, j_accumulator };
}

/*
 * Creates a .csv file for both x[N] and v[N] after the system is evolved to a particular time. Both .csv files have 'no_rods' columns and 'no_samples' rows.
 *
 * @param std::string folder is the existing/new folder in which you want to store the .csv file
 * @param std::string file contains the simulation parameter data of the .csv file
 * @param float time is the duration for which evolve_to runs (evolve_to takes 'time' as an argument)
 * @param init_vel_matrix and init_pos_matrix are the matrices containing 'no_samples' intial velocity and position vectors respectively
 * @param bool export_vel exports velocities to a csv file, default value is False.
 */
void sample_xv_in_csv(std::string folder, std::string file, float time, std::vector <std::vector<double>> init_vel_matrix, std::vector <std::vector<double>> init_pos_matrix, bool exporting_vel) {

    std::ofstream positions_csv_file;
    positions_csv_file.open(".\\" + folder + "\\P_" + file + ".csv"); //Create csv file in which position data is stored

    std::ofstream velocities_csv_file;
    velocities_csv_file.open(".\\" + folder + "\\V_" + file + ".csv"); //Create csv file in which velocity data is stored

    //std::cout << "Beginning sample_run:" << "\n";

    auto start = std::chrono::high_resolution_clock::now();

    //Call evolve functions 'sample_no' times

    for (size_t i = 0; i < init_vel_matrix.size(); i++) {

        //std::cout << "\nRun " << i << " is in progress...";
        auto iter_start = std::chrono::high_resolution_clock::now();

        //Call evolve.cpp function
        auto xv_matrix = evolve_to(time, init_pos_matrix[i], init_vel_matrix[i]); // x_evolved = xv_matrix[0]; v_evolved = xv_matrix[1]

        //HISTOGRAM CALL

        // Print time data
        auto iter_stop = std::chrono::high_resolution_clock::now();
        auto iter_duration = std::chrono::duration_cast<std::chrono::microseconds>(iter_stop - iter_start);
        auto time_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(iter_stop - start);

        //std::cout << " and has completed after " << iter_duration.count() << "mu_s. Total runtime is " << time_elapsed.count() << "ms.";

        //Entering position data to csv file
        if (positions_csv_file.is_open()) {

            for (int j = 0; j < N - 1; j++) {
                //positions_csv_file << xv_matrix[0][j] << ",";
            }
            //positions_csv_file << xv_matrix[0][N - 1] << "\n"; //last entry on row doesn't require a comma
        }
        else {
            std::cout << "\nError opening file.";
        }

        if (exporting_vel) {
            if (velocities_csv_file.is_open()) {

                for (int j = 0; j < N - 1; j++) {
                    //velocities_csv_file << xv_matrix[1][j] << ",";
                }
                //velocities_csv_file << xv_matrix[1][N - 1] << "\n"; //last entry on row doesn't require a comma
            }
            else {
                std::cout << "\nError opening file.";
            }
        }

    }
    //Close csv files
    positions_csv_file.close();
    if (exporting_vel) {
        velocities_csv_file.close();
    }
    else {
        velocities_csv_file.close();
        fs::remove(".\\" + folder + "\\V_" + file + ".csv");
    }
}

void old_main() {
    print_parameters();

    //Run initialisation
    //Create a directory if there isn't one
    if (fs::is_directory(folder_name)) { std::cout << "N.B. Directory with this name already exists.\n\n"; }
    else { fs::create_directory(folder_name); }

    //Start clock
    std::cout << "Beginning simulation:" << "\n";

    auto start = std::chrono::high_resolution_clock::now();

    //Call init.cpp functions
    std::cout << "Initialisation in progress...";
    std::vector <std::vector<double>> init_vel_matrix = init_velocities_matrix();
    std::vector <std::vector<double>> init_posn_matrix = init_positions_matrix(posn_init_method);

    if (implementing_v_kick) { init_vel_matrix = kick_velocity_matrix(init_vel_matrix, init_posn_matrix); }

    auto init_stop = std::chrono::high_resolution_clock::now();
    auto init_duration = std::chrono::duration_cast<std::chrono::milliseconds>(init_stop - start);
    std::cout << " and has completed after " << init_duration.count() << "ms. total runtime is " << init_duration.count() / 1000 << "s.";

    std::cout << "\nBegin evolving + sampling:" << "\n";

    //set file_name
    std::string file_name = "T" + std::to_string(std::round(max_time)) + "ST" + std::to_string(std::round(sampling_time)) + \
        "_temp" + std::to_string(std::round(T)) + "_samples" + std::to_string(no_samples) \
        + "_rods" + std::to_string(N) + "_sys_length" + std::to_string(std::round(L)) + "_rodlength" + std::to_string(rod_length);

    //check if file_name doesn't already exist inside folder_name
    if (fs::exists(".\\" + folder_name + "\\" + file_name)) {
        std::cout << "File name already exists - please rename. exiting";
        //return 0;
    }
    else {
        std::ofstream rho_bar_csvfile;
        rho_bar_csvfile.open(".\\" + folder_name + "\\D_" + file_name + ".csv"); //Create csv file in which density data is stored

        std::ofstream j_bar_csvfile;
        j_bar_csvfile.open(".\\" + folder_name + "\\J_" + file_name + ".csv"); //Create csv file in which current data is stored

        int no_time_samples = std::round(max_time / sampling_time) + 1;

        std::vector<std::vector<double>> rho_t_series(no_time_samples);
        std::vector<std::vector<double>> j_t_series(no_time_samples);

        std::list<double> bins_list = init_bins(std::round(L / cell_length));

        //for loop to run evolve_and_hist every 'sampling_time' up until 'max_time'
        for (int i = 0; i < static_cast<int> (max_time / sampling_time) + 1; i++) {

            std::cout << "\nSampling at time t = " << std::to_string(sampling_time * i) << " is in progress...";
            auto iter_start = std::chrono::high_resolution_clock::now();

            //call evolve_and_hist

            std::vector<std::vector<double>> averaged_rho_j_dists = evolve_and_hist(sampling_time * i, init_vel_matrix, init_posn_matrix, bins_list);
            rho_t_series[i] = averaged_rho_j_dists[0];
            j_t_series[i] = averaged_rho_j_dists[1];

            auto iter_stop = std::chrono::high_resolution_clock::now();
            auto iter_duration = std::chrono::duration_cast<std::chrono::milliseconds>(iter_stop - iter_start);
            auto time_elapsed = std::chrono::duration_cast<std::chrono::seconds>(iter_stop - start);
            std::cout << " and has completed after " << iter_duration.count() << "ms. Total runtime is " << time_elapsed.count() << "s.";
        }
        //exporting to csv

        if (rho_bar_csvfile.is_open()) {

            for (unsigned i = 0; i < rho_t_series.size(); i++) {
                for (unsigned j = 0; j < rho_t_series[i].size() - 1; j++) {
                    rho_bar_csvfile << rho_t_series[i][j] << ",";
                }
                rho_bar_csvfile << rho_t_series[i].back() << "\n"; //last entry on row doesn't require a comma
            }
        }
        else {
            std::cout << "\nError opening file.";
        }

        if (j_bar_csvfile.is_open()) {

            for (unsigned i = 0; i < j_t_series.size(); i++) {
                for (unsigned j = 0; j < j_t_series[i].size() - 1; j++) {
                    j_bar_csvfile << j_t_series[i][j] << ",";
                }
                j_bar_csvfile << j_t_series[i].back() << "\n"; //last entry on row doesn't require a comma
            }
        }
        else {
            std::cout << "\nError opening file.";
        }

    }
}


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MIQP_COMMON_MAP_CONVEXIFIED_MAP_HPP_
#define MIQP_COMMON_MAP_CONVEXIFIED_MAP_HPP_

#include <vector>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/polygon/voronoi.hpp>
#include "bark/commons/base_type.hpp"
#include "bark/commons/util/util.hpp"
#include "bark/geometry/polygon.hpp"
#include "bark/models/dynamic/dynamic_model.hpp"
#include "common/geometry/geometry.hpp"

namespace miqp {
namespace common {
namespace map {

typedef boost::polygon::voronoi_diagram<double> boost_voronoi_diagram;
typedef boost::polygon::point_data<double> point_data_t;

class ConvexifiedMap : public bark::commons::BaseType {
 public:
  explicit ConvexifiedMap(const bark::commons::ParamsPtr& params,
                          const bark::geometry::Polygon& map_polygon,
                          const double buffer_radius,
                          const double max_simplify_dist,
                          const double buffer_reference,
                          const double buffer_for_merging_tolerance);

  virtual ~ConvexifiedMap() {}

  /**
   * @brief shrinks and converts nonconvex polygon to multiple convex ones
   *
   * @return true ... if conversion was successfull
   */
  bool Convert();

  /**
   * @brief Get the Map Non Convex Polygon object
   *
   * @return bark::geometry::Polygon
   */
  bark::geometry::Polygon GetMapNonConvexPolygon() const {
    return nonconvex_input_polygon_;
  }

  /**
   * @brief Get the Map Convex Polygons object
   *
   * @return miqp::common::geometry::PolygonMap
   */
  miqp::common::geometry::PolygonMap GetMapConvexPolygons() const {
    return convex_output_polygon_map_;
  }

  /**
   * @brief returns convex polygons of street that intersect with reference.
   * reference is bufferd to make sure that enough environment polygons are
   * available for a maneuver.
   *
   * @param referenceTraj ... reference trajectory
   * @return PolygonMap ... convex polygons
   */
  miqp::common::geometry::PolygonMap GetIntersectingConvexPolygons(
      const bark::models::dynamic::Trajectory& referenceTraj) const;

  /**
   * @brief checks if input polygon is valid
   *
   * @return true if polygon is valid
   * @return false if polygon is not valid
   */
  bool HasValidPolygon() { return nonconvex_input_polygon_.Valid(); }

  /**
   * @brief Set the Map Polygon object
   *
   * @param map_polygon
   */
  void SetMapPolygon(const bark::geometry::Polygon& map_polygon);

 private:
  /**
   * @brief function checks if decomposition of initial polygon was successfull
   * or not
   *
   * @param shrinked_input_polygon ... shrinked nonconvex input polygon
   * @param convex_output_polygons ... shrinked decomposed output polygons
   * @return true ... if decomposition successfull
   */
  bool CheckDecomposition(const bark::geometry::Polygon& shrinked_input_polygon,
                          const miqp::common::geometry::BoostMultiPolygon&
                              convex_output_polygons) const;

  /**
   * @brief converts cells of voronoi diagram to multipolygon
   *
   * @param vd ... voronoi diagram
   * @return BoostMultiPolygon ... cells from voronoi diagram
   */
  miqp::common::geometry::BoostMultiPolygon VoronoiCellsToBoostMultiPolygon(
      const boost::polygon::voronoi_diagram<double>& vd);

  /**
   * @brief construct voronoi diagram from input polygon
   *
   * @param input_polygon ... input geometry as polygon
   * @param input_points ... input geometry as points vector
   * @param vd ... voronoi diagram
   */
  void ConstructVoronoiDiagram(
      const bark::geometry::Polygon& buffered_simplified_input_polygon,
      std::vector<point_data_t>& input_points, boost_voronoi_diagram& vd) const;

  /**
   * @brief converts possibly nonconvex cells to triangles
   *
   * @param input_points ... points vector of input geometry
   * @param cells_inside_map ... possibly nonconvex cells
   * @param boost_polygon_convex_out ... triangles
   */
  void TriangulateCells(
      const std::vector<point_data_t>& input_points,
      miqp::common::geometry::BoostMultiPolygon& cells_inside_map,
      miqp::common::geometry::BoostMultiPolygon& boost_polygon_convex_out);

  /**
   * @brief create polygon from three points
   *
   * @param p1 ... input point
   * @param p2 ... input point
   * @param p3 ... input point
   * @param triangle ... output polygon
   * @return true ... if polygon could be constructed
   */
  bool CreateTriangle(const miqp::common::geometry::PointXY& p1,
                      const miqp::common::geometry::PointXY& p2,
                      const miqp::common::geometry::PointXY& p3,
                      miqp::common::geometry::BoostPolygon& triangle) const;

  /**
   * @brief simplifying and buffering input polygon
   *
   * @return Polygon ... simplified and buffered polygon
   */
  bark::geometry::Polygon PreprocessPolygon() const;

  /**
   * @brief thakes the convex_output_polygon_map_ member and simplifies the
   * linestrings in the polygons again to avoid too short segments. Afterwards
   * inflate the polygons again to avoid holes in between.
   */
  void PostprocessPolygonVector();

  bark::geometry::Polygon nonconvex_input_polygon_;
  miqp::common::geometry::PolygonMap convex_output_polygon_map_;
  const double envelope_offset_;
  bool decomposed_;
  double merging_tolerance_;
  const double buffer_radius_;
  const double buffer_reference_;
  const double buffer_for_merging_tolerance_;
  const double max_simplify_dist_;
  const double postprocess_simplification_dist_;
  const int num_merging_runs_;
};

}  // namespace map
}  // namespace common
}  // namespace miqp

#endif  // MIQP_COMMON_MAP_CONVEXIFIED_MAP_HPP_


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

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

namespace bs = boost::simd;
using pack_ft = bs::pack <float, 4>;

int main()
{
  pack_ft pf = bs::enumerate<pack_ft>(-1, 2);
  pack_ft qf = bs::enumerate<pack_ft>( 0, 3);
  pack_ft rf = bs::enumerate<pack_ft>( 2, 5);

  std::cout
    << "---- simd" << '\n'
    << " <- pf =                   " << pf << '\n'
    << " <- qf =                   " << qf << '\n'
    << " <- rf =                   " << rf << '\n'
    << " -> bs::fnma(pf, qf, rf) = " << bs::fnma(pf, qf, rf) << '\n';

  float xf = 3.0f, yf = -3.0f, zf = 2.0f;

  std::cout
    << "---- scalar" << '\n'
    << " <- xf =                   " << xf << '\n'
    << " <- yf =                   " << yf << '\n'
    << " <- yf =                   " << zf << '\n'
    << " -> bs::fnma(xf, yf, rf) = " << bs::fnma(xf, yf, zf) << '\n';
  return 0;
}
//! [fnma]