// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "open3d/geometry/PointCloud.h"

#include <Eigen/Dense>
#include <algorithm>
#include <numeric>
#include <random>

#include "open3d/geometry/BoundingVolume.h"
#include "open3d/geometry/KDTreeFlann.h"
#include "open3d/geometry/Qhull.h"
#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Console.h"
#include "open3d/utility/Eigen.h"

namespace open3d {
namespace geometry {

PointCloud &PointCloud::Clear() {
    points_.clear();
    normals_.clear();
    colors_.clear();
    return *this;
}

bool PointCloud::IsEmpty() const { return !HasPoints(); }

Eigen::Vector3d PointCloud::GetMinBound() const {
    return ComputeMinBound(points_);
}

Eigen::Vector3d PointCloud::GetMaxBound() const {
    return ComputeMaxBound(points_);
}

Eigen::Vector3d PointCloud::GetCenter() const { return ComputeCenter(points_); }

AxisAlignedBoundingBox PointCloud::GetAxisAlignedBoundingBox() const {
    return AxisAlignedBoundingBox::CreateFromPoints(points_);
}

OrientedBoundingBox PointCloud::GetOrientedBoundingBox() const {
    return OrientedBoundingBox::CreateFromPoints(points_);
}

PointCloud &PointCloud::Transform(const Eigen::Matrix4d &transformation) {
    TransformPoints(transformation, points_);
    TransformNormals(transformation, normals_);
    return *this;
}

PointCloud &PointCloud::Translate(const Eigen::Vector3d &translation,
                                  bool relative) {
    TranslatePoints(translation, points_, relative);
    return *this;
}

PointCloud &PointCloud::Scale(const double scale,
                              const Eigen::Vector3d &center) {
    ScalePoints(scale, points_, center);
    return *this;
}

PointCloud &PointCloud::Rotate(const Eigen::Matrix3d &R,
                               const Eigen::Vector3d &center) {
    RotatePoints(R, points_, center);
    RotateNormals(R, normals_);
    return *this;
}

PointCloud &PointCloud::operator+=(const PointCloud &cloud) {
    // We do not use std::vector::insert to combine std::vector because it will
    // crash if the pointcloud is added to itself.
    if (cloud.IsEmpty()) return (*this);
    size_t old_vert_num = points_.size();
    size_t add_vert_num = cloud.points_.size();
    size_t new_vert_num = old_vert_num + add_vert_num;
    if ((!HasPoints() || HasNormals()) && cloud.HasNormals()) {
        normals_.resize(new_vert_num);
        for (size_t i = 0; i < add_vert_num; i++)
            normals_[old_vert_num + i] = cloud.normals_[i];
    } else {
        normals_.clear();
    }
    if ((!HasPoints() || HasColors()) && cloud.HasColors()) {
        colors_.resize(new_vert_num);
        for (size_t i = 0; i < add_vert_num; i++)
            colors_[old_vert_num + i] = cloud.colors_[i];
    } else {
        colors_.clear();
    }
    points_.resize(new_vert_num);
    for (size_t i = 0; i < add_vert_num; i++)
        points_[old_vert_num + i] = cloud.points_[i];
    return (*this);
}

PointCloud PointCloud::operator+(const PointCloud &cloud) const {
    return (PointCloud(*this) += cloud);
}

std::vector<double> PointCloud::ComputePointCloudDistance(
        const PointCloud &target) {
    std::vector<double> distances(points_.size());
    KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
#pragma omp parallel for schedule(static)
    for (int i = 0; i < (int)points_.size(); i++) {
        std::vector<int> indices(1);
        std::vector<double> dists(1);
        if (kdtree.SearchKNN(points_[i], 1, indices, dists) == 0) {
            utility::LogDebug(
                    "[ComputePointCloudToPointCloudDistance] Found a point "
                    "without neighbors.");
            distances[i] = 0.0;
        } else {
            distances[i] = std::sqrt(dists[0]);
        }
    }
    return distances;
}

PointCloud &PointCloud::RemoveNonFinitePoints(bool remove_nan,
                                              bool remove_infinite) {
    bool has_normal = HasNormals();
    bool has_color = HasColors();
    size_t old_point_num = points_.size();
    size_t k = 0;                                 // new index
    for (size_t i = 0; i < old_point_num; i++) {  // old index
        bool is_nan = remove_nan &&
                      (std::isnan(points_[i](0)) || std::isnan(points_[i](1)) ||
                       std::isnan(points_[i](2)));
        bool is_infinite = remove_infinite && (std::isinf(points_[i](0)) ||
                                               std::isinf(points_[i](1)) ||
                                               std::isinf(points_[i](2)));
        if (!is_nan && !is_infinite) {
            points_[k] = points_[i];
            if (has_normal) normals_[k] = normals_[i];
            if (has_color) colors_[k] = colors_[i];
            k++;
        }
    }
    points_.resize(k);
    if (has_normal) normals_.resize(k);
    if (has_color) colors_.resize(k);
    utility::LogDebug(
            "[RemoveNonFinitePoints] {:d} nan points have been removed.",
            (int)(old_point_num - k));
    return *this;
}

std::shared_ptr<PointCloud> PointCloud::SelectByIndex(
        const std::vector<size_t> &indices, bool invert /* = false */) const {
    auto output = std::make_shared<PointCloud>();
    bool has_normals = HasNormals();
    bool has_colors = HasColors();

    std::vector<bool> mask = std::vector<bool>(points_.size(), invert);
    for (size_t i : indices) {
        mask[i] = !invert;
    }

    for (size_t i = 0; i < points_.size(); i++) {
        if (mask[i]) {
            output->points_.push_back(points_[i]);
            if (has_normals) output->normals_.push_back(normals_[i]);
            if (has_colors) output->colors_.push_back(colors_[i]);
        }
    }
    utility::LogDebug(
            "Pointcloud down sampled from {:d} points to {:d} points.",
            (int)points_.size(), (int)output->points_.size());
    return output;
}

// helper classes for VoxelDownSample and VoxelDownSampleAndTrace
namespace {
class AccumulatedPoint {
public:
    AccumulatedPoint()
        : num_of_points_(0),
          point_(0.0, 0.0, 0.0),
          normal_(0.0, 0.0, 0.0),
          color_(0.0, 0.0, 0.0) {}

public:
    void AddPoint(const PointCloud &cloud, int index) {
        point_ += cloud.points_[index];
        if (cloud.HasNormals()) {
            if (!std::isnan(cloud.normals_[index](0)) &&
                !std::isnan(cloud.normals_[index](1)) &&
                !std::isnan(cloud.normals_[index](2))) {
                normal_ += cloud.normals_[index];
            }
        }
        if (cloud.HasColors()) {
            color_ += cloud.colors_[index];
        }
        num_of_points_++;
    }

    Eigen::Vector3d GetAveragePoint() const {
        return point_ / double(num_of_points_);
    }

    Eigen::Vector3d GetAverageNormal() const {
        // Call NormalizeNormals() afterwards if necessary
        return normal_ / double(num_of_points_);
    }

    Eigen::Vector3d GetAverageColor() const {
        return color_ / double(num_of_points_);
    }

public:
    int num_of_points_;
    Eigen::Vector3d point_;
    Eigen::Vector3d normal_;
    Eigen::Vector3d color_;
};

class point_cubic_id {
public:
    size_t point_id;
    int cubic_id;
};

class AccumulatedPointForTrace : public AccumulatedPoint {
public:
    void AddPoint(const PointCloud &cloud,
                  size_t index,
                  int cubic_index,
                  bool approximate_class) {
        point_ += cloud.points_[index];
        if (cloud.HasNormals()) {
            if (!std::isnan(cloud.normals_[index](0)) &&
                !std::isnan(cloud.normals_[index](1)) &&
                !std::isnan(cloud.normals_[index](2))) {
                normal_ += cloud.normals_[index];
            }
        }
        if (cloud.HasColors()) {
            if (approximate_class) {
                auto got = classes.find(int(cloud.colors_[index][0]));
                if (got == classes.end())
                    classes[int(cloud.colors_[index][0])] = 1;
                else
                    classes[int(cloud.colors_[index][0])] += 1;
            } else {
                color_ += cloud.colors_[index];
            }
        }
        point_cubic_id new_id;
        new_id.point_id = index;
        new_id.cubic_id = cubic_index;
        original_id.push_back(new_id);
        num_of_points_++;
    }

    Eigen::Vector3d GetMaxClass() {
        int max_class = -1;
        int max_count = -1;
        for (auto it = classes.begin(); it != classes.end(); it++) {
            if (it->second > max_count) {
                max_count = it->second;
                max_class = it->first;
            }
        }
        return Eigen::Vector3d(max_class, max_class, max_class);
    }

    std::vector<point_cubic_id> GetOriginalID() { return original_id; }

private:
    // original point cloud id in higher resolution + its cubic id
    std::vector<point_cubic_id> original_id;
    std::unordered_map<int, int> classes;
};
}  // namespace

std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
        double voxel_size) const {
    auto output = std::make_shared<PointCloud>();
    if (voxel_size <= 0.0) {
        utility::LogError("[VoxelDownSample] voxel_size <= 0.");
    }
    Eigen::Vector3d voxel_size3 =
            Eigen::Vector3d(voxel_size, voxel_size, voxel_size);
    Eigen::Vector3d voxel_min_bound = GetMinBound() - voxel_size3 * 0.5;
    Eigen::Vector3d voxel_max_bound = GetMaxBound() + voxel_size3 * 0.5;
    if (voxel_size * std::numeric_limits<int>::max() <
        (voxel_max_bound - voxel_min_bound).maxCoeff()) {
        utility::LogError("[VoxelDownSample] voxel_size is too small.");
    }
    std::unordered_map<Eigen::Vector3i, AccumulatedPoint,
                       utility::hash_eigen<Eigen::Vector3i>>
            voxelindex_to_accpoint;

    Eigen::Vector3d ref_coord;
    Eigen::Vector3i voxel_index;
    for (int i = 0; i < (int)points_.size(); i++) {
        ref_coord = (points_[i] - voxel_min_bound) / voxel_size;
        voxel_index << int(floor(ref_coord(0))), int(floor(ref_coord(1))),
                int(floor(ref_coord(2)));
        voxelindex_to_accpoint[voxel_index].AddPoint(*this, i);
    }
    bool has_normals = HasNormals();
    bool has_colors = HasColors();
    for (auto accpoint : voxelindex_to_accpoint) {
        output->points_.push_back(accpoint.second.GetAveragePoint());
        if (has_normals) {
            output->normals_.push_back(accpoint.second.GetAverageNormal());
        }
        if (has_colors) {
            output->colors_.push_back(accpoint.second.GetAverageColor());
        }
    }
    utility::LogDebug(
            "Pointcloud down sampled from {:d} points to {:d} points.",
            (int)points_.size(), (int)output->points_.size());
    return output;
}

std::tuple<std::shared_ptr<PointCloud>,
           Eigen::MatrixXi,
           std::vector<std::vector<int>>>
PointCloud::VoxelDownSampleAndTrace(double voxel_size,
                                    const Eigen::Vector3d &min_bound,
                                    const Eigen::Vector3d &max_bound,
                                    bool approximate_class) const {
    auto output = std::make_shared<PointCloud>();
    Eigen::MatrixXi cubic_id;
    if (voxel_size <= 0.0) {
        utility::LogError("[VoxelDownSample] voxel_size <= 0.");
    }
    // Note: this is different from VoxelDownSample.
    // It is for fixing coordinate for multiscale voxel space
    auto voxel_min_bound = min_bound;
    auto voxel_max_bound = max_bound;
    if (voxel_size * std::numeric_limits<int>::max() <
        (voxel_max_bound - voxel_min_bound).maxCoeff()) {
        utility::LogError("[VoxelDownSample] voxel_size is too small.");
    }
    std::unordered_map<Eigen::Vector3i, AccumulatedPointForTrace,
                       utility::hash_eigen<Eigen::Vector3i>>
            voxelindex_to_accpoint;
    int cid_temp[3] = {1, 2, 4};
    for (size_t i = 0; i < points_.size(); i++) {
        auto ref_coord = (points_[i] - voxel_min_bound) / voxel_size;
        auto voxel_index = Eigen::Vector3i(int(floor(ref_coord(0))),
                                           int(floor(ref_coord(1))),
                                           int(floor(ref_coord(2))));
        int cid = 0;
        for (int c = 0; c < 3; c++) {
            if ((ref_coord(c) - voxel_index(c)) >= 0.5) {
                cid += cid_temp[c];
            }
        }
        voxelindex_to_accpoint[voxel_index].AddPoint(*this, i, cid,
                                                     approximate_class);
    }
    bool has_normals = HasNormals();
    bool has_colors = HasColors();
    int cnt = 0;
    cubic_id.resize(voxelindex_to_accpoint.size(), 8);
    cubic_id.setConstant(-1);
    std::vector<std::vector<int>> original_indices(
            voxelindex_to_accpoint.size());
    for (auto accpoint : voxelindex_to_accpoint) {
        output->points_.push_back(accpoint.second.GetAveragePoint());
        if (has_normals) {
            output->normals_.push_back(accpoint.second.GetAverageNormal());
        }
        if (has_colors) {
            if (approximate_class) {
                output->colors_.push_back(accpoint.second.GetMaxClass());
            } else {
                output->colors_.push_back(accpoint.second.GetAverageColor());
            }
        }
        auto original_id = accpoint.second.GetOriginalID();
        for (int i = 0; i < (int)original_id.size(); i++) {
            size_t pid = original_id[i].point_id;
            int cid = original_id[i].cubic_id;
            cubic_id(cnt, cid) = int(pid);
            original_indices[cnt].push_back(int(pid));
        }
        cnt++;
    }
    utility::LogDebug(
            "Pointcloud down sampled from {:d} points to {:d} points.",
            (int)points_.size(), (int)output->points_.size());
    return std::make_tuple(output, cubic_id, original_indices);
}

std::shared_ptr<PointCloud> PointCloud::UniformDownSample(
        size_t every_k_points) const {
    if (every_k_points == 0) {
        utility::LogError("[UniformDownSample] Illegal sample rate.");
    }
    std::vector<size_t> indices;
    for (size_t i = 0; i < points_.size(); i += every_k_points) {
        indices.push_back(i);
    }
    return SelectByIndex(indices);
}

std::shared_ptr<PointCloud> PointCloud::RandomDownSample(
        double sampling_ratio) const {
    if (sampling_ratio < 0 || sampling_ratio > 1) {
        utility::LogError(
                "[RandomDownSample] Illegal sampling_ratio {}, sampling_ratio "
                "must be between 0 and 1.");
    }
    std::vector<size_t> indices(points_.size());
    std::iota(std::begin(indices), std::end(indices), (size_t)0);
    std::random_device rd;
    std::mt19937 prng(rd());
    std::shuffle(indices.begin(), indices.end(), prng);
    indices.resize((int)(sampling_ratio * points_.size()));
    return SelectByIndex(indices);
}

std::shared_ptr<PointCloud> PointCloud::Crop(
        const AxisAlignedBoundingBox &bbox) const {
    if (bbox.IsEmpty()) {
        utility::LogError(
                "[CropPointCloud] AxisAlignedBoundingBox either has zeros "
                "size, or has wrong bounds.");
    }
    return SelectByIndex(bbox.GetPointIndicesWithinBoundingBox(points_));
}
std::shared_ptr<PointCloud> PointCloud::Crop(
        const OrientedBoundingBox &bbox) const {
    if (bbox.IsEmpty()) {
        utility::LogError(
                "[CropPointCloud] AxisAlignedBoundingBox either has zeros "
                "size, or has wrong bounds.");
    }
    return SelectByIndex(bbox.GetPointIndicesWithinBoundingBox(points_));
}

std::shared_ptr<PointCloud> PointCloud::CropConvexHull(
        const BoundingConvexHull &bhull, bool invert) const {
    Eigen::Vector3d p1;
    Eigen::Vector3d p2;
    Eigen::Vector3d p3;
    Eigen::Vector3d normal;
    Eigen::Vector3d center = bhull.GetCenter();
    Eigen::Vector3i triangle;
    double ccw;
    double cosine;

    std::vector<size_t> out_index;

    for (size_t k = 0; k < points_.size(); k++) {
        const auto &point = points_[k];
        bool valid = true;
        for (size_t j = 0; j < bhull.convex_hull_->triangles_.size(); j++) {
            triangle = bhull.convex_hull_->triangles_[j];
            p1 = bhull.convex_hull_->vertices_[triangle(0)];
            p2 = bhull.convex_hull_->vertices_[triangle(1)];
            p3 = bhull.convex_hull_->vertices_[triangle(2)];
            normal = bhull.convex_hull_->triangle_normals_[j];
            ccw = (p1 - center).dot(normal);
            if (ccw < 0) {
                normal = -normal;
            }
            cosine = (p1 - point).dot(normal);
            if (cosine < 0) {
                valid = false;
                break;
            }
        }
        if (valid) {
            out_index.push_back(k);
        }
    }

    return SelectByIndex(out_index, invert);
}

std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
PointCloud::RemoveRadiusOutliers(size_t nb_points, double search_radius) const {
    if (nb_points < 1 || search_radius <= 0) {
        utility::LogError(
                "[RemoveRadiusOutliers] Illegal input parameters,"
                "number of points and radius must be positive");
    }
    KDTreeFlann kdtree;
    kdtree.SetGeometry(*this);
    std::vector<bool> mask = std::vector<bool>(points_.size());
#pragma omp parallel for schedule(static)
    for (int i = 0; i < int(points_.size()); i++) {
        std::vector<int> tmp_indices;
        std::vector<double> dist;
        size_t nb_neighbors = kdtree.SearchRadius(points_[i], search_radius,
                                                  tmp_indices, dist);
        mask[i] = (nb_neighbors > nb_points);
    }
    std::vector<size_t> indices;
    for (size_t i = 0; i < mask.size(); i++) {
        if (mask[i]) {
            indices.push_back(i);
        }
    }
    return std::make_tuple(SelectByIndex(indices), indices);
}

std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
PointCloud::RemoveStatisticalOutliers(size_t nb_neighbors,
                                      double std_ratio) const {
    if (nb_neighbors < 1 || std_ratio <= 0) {
        utility::LogError(
                "[RemoveStatisticalOutliers] Illegal input parameters, number "
                "of neighbors and standard deviation ratio must be positive");
    }
    if (points_.size() == 0) {
        return std::make_tuple(std::make_shared<PointCloud>(),
                               std::vector<size_t>());
    }
    KDTreeFlann kdtree;
    kdtree.SetGeometry(*this);
    std::vector<double> avg_distances = std::vector<double>(points_.size());
    std::vector<size_t> indices;
    size_t valid_distances = 0;

#pragma omp parallel for schedule(static)
    for (int i = 0; i < int(points_.size()); i++) {
        std::vector<int> tmp_indices;
        std::vector<double> dist;
        kdtree.SearchKNN(points_[i], int(nb_neighbors), tmp_indices, dist);
        double mean = -1.0;
        if (dist.size() > 0u) {
            valid_distances++;
            std::for_each(dist.begin(), dist.end(),
                          [](double &d) { d = std::sqrt(d); });
            mean = std::accumulate(dist.begin(), dist.end(), 0.0) / dist.size();
        }
        avg_distances[i] = mean;
    }
    if (valid_distances == 0) {
        return std::make_tuple(std::make_shared<PointCloud>(),
                               std::vector<size_t>());
    }
    double cloud_mean = std::accumulate(
            avg_distances.begin(), avg_distances.end(), 0.0,
            [](double const &x, double const &y) { return y > 0 ? x + y : x; });
    cloud_mean /= valid_distances;
    double sq_sum = std::inner_product(
            avg_distances.begin(), avg_distances.end(), avg_distances.begin(),
            0.0, [](double const &x, double const &y) { return x + y; },
            [cloud_mean](double const &x, double const &y) {
                return x > 0 ? (x - cloud_mean) * (y - cloud_mean) : 0;
            });
    // Bessel's correction
    double std_dev = std::sqrt(sq_sum / (valid_distances - 1));
    double distance_threshold = cloud_mean + std_ratio * std_dev;
    for (size_t i = 0; i < avg_distances.size(); i++) {
        if (avg_distances[i] > 0 && avg_distances[i] < distance_threshold) {
            indices.push_back(i);
        }
    }
    return std::make_tuple(SelectByIndex(indices), indices);
}

std::tuple<Eigen::Vector3d, Eigen::Matrix3d>
PointCloud::ComputeMeanAndCovariance() const {
    if (IsEmpty()) {
        return std::make_tuple(Eigen::Vector3d::Zero(),
                               Eigen::Matrix3d::Identity());
    }
    std::vector<size_t> all_idx(points_.size());
    std::iota(all_idx.begin(), all_idx.end(), 0);
    return utility::ComputeMeanAndCovariance(points_, all_idx);
}

std::vector<double> PointCloud::ComputeMahalanobisDistance() const {
    std::vector<double> mahalanobis(points_.size());
    Eigen::Vector3d mean;
    Eigen::Matrix3d covariance;
    std::tie(mean, covariance) = ComputeMeanAndCovariance();
    Eigen::Matrix3d cov_inv = covariance.inverse();
#pragma omp parallel for schedule(static)
    for (int i = 0; i < (int)points_.size(); i++) {
        Eigen::Vector3d p = points_[i] - mean;
        mahalanobis[i] = std::sqrt(p.transpose() * cov_inv * p);
    }
    return mahalanobis;
}

std::vector<double> PointCloud::ComputeNearestNeighborDistance() const {
    if (points_.size() < 2) {
        return std::vector<double>(points_.size(), 0);
    }

    std::vector<double> nn_dis(points_.size());
    KDTreeFlann kdtree(*this);
#pragma omp parallel for schedule(static)
    for (int i = 0; i < (int)points_.size(); i++) {
        std::vector<int> indices(2);
        std::vector<double> dists(2);
        if (kdtree.SearchKNN(points_[i], 2, indices, dists) <= 1) {
            utility::LogDebug(
                    "[ComputePointCloudNearestNeighborDistance] Found a point "
                    "without neighbors.");
            nn_dis[i] = 0.0;
        } else {
            nn_dis[i] = std::sqrt(dists[1]);
        }
    }
    return nn_dis;
}

std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
PointCloud::ComputeConvexHull() const {
    return Qhull::ComputeConvexHull(points_);
}

std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
PointCloud::HiddenPointRemoval(const Eigen::Vector3d &camera_location,
                               const double radius) const {
    if (radius <= 0) {
        utility::LogError(
                "[HiddenPointRemoval] radius must be larger than zero.");
    }

    // perform spherical projection
    std::vector<Eigen::Vector3d> spherical_projection;
    for (size_t pidx = 0; pidx < points_.size(); ++pidx) {
        Eigen::Vector3d projected_point = points_[pidx] - camera_location;
        double norm = projected_point.norm();
        spherical_projection.push_back(
                projected_point + 2 * (radius - norm) * projected_point / norm);
    }

    // add origin
    size_t origin_pidx = spherical_projection.size();
    spherical_projection.push_back(Eigen::Vector3d(0, 0, 0));

    // calculate convex hull of spherical projection
    std::shared_ptr<TriangleMesh> visible_mesh;
    std::vector<size_t> pt_map;
    std::tie(visible_mesh, pt_map) =
            Qhull::ComputeConvexHull(spherical_projection);

    // reassign original points to mesh
    size_t origin_vidx = pt_map.size();
    for (size_t vidx = 0; vidx < pt_map.size(); vidx++) {
        size_t pidx = pt_map[vidx];
        if (pidx != origin_pidx) {
            visible_mesh->vertices_[vidx] = points_[pidx];
        } else {
            origin_vidx = vidx;
            visible_mesh->vertices_[vidx] = camera_location;
        }
    }

    // erase origin if part of mesh
    if (origin_vidx < visible_mesh->vertices_.size()) {
        visible_mesh->vertices_.erase(visible_mesh->vertices_.begin() +
                                      origin_vidx);
        pt_map.erase(pt_map.begin() + origin_vidx);
        for (size_t tidx = visible_mesh->triangles_.size(); tidx-- > 0;) {
            if (visible_mesh->triangles_[tidx](0) == (int)origin_vidx ||
                visible_mesh->triangles_[tidx](1) == (int)origin_vidx ||
                visible_mesh->triangles_[tidx](2) == (int)origin_vidx) {
                visible_mesh->triangles_.erase(
                        visible_mesh->triangles_.begin() + tidx);
            } else {
                if (visible_mesh->triangles_[tidx](0) > (int)origin_vidx)
                    visible_mesh->triangles_[tidx](0) -= 1;
                if (visible_mesh->triangles_[tidx](1) > (int)origin_vidx)
                    visible_mesh->triangles_[tidx](1) -= 1;
                if (visible_mesh->triangles_[tidx](2) > (int)origin_vidx)
                    visible_mesh->triangles_[tidx](2) -= 1;
            }
        }
    }
    return std::make_tuple(visible_mesh, pt_map);
}

}  // namespace geometry
}  // namespace open3d


 /**
  * The MIT License (MIT)
  *
  * Copyright (c) 2014 Behrouz Babaki, Tias Guns, Siegfried Nijssen
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
  *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
  *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */

#include <scip/scip.h>
#include <scip/scipdefplugins.h>
#include <cstring>
#include <cstdlib>
#include <fstream>
#include <sstream>
#include <vector>
#include <queue>
#include <string>
#include <iostream>
#include <iomanip>
#include <utility>
#include <boost/concept_check.hpp>
#include <sys/types.h>
#include <dirent.h>

#include "init.h"
#include "cons_mlcl.h"
#include "pricer_optclust.h" // for print_cluster()
using namespace std;

/** public-like **/

SCIP_RETCODE initialize_solution(const char* fileName, SCIP* scip,
                                 const vector<vector<double> > & distances,
                                 vector<SCIP_CONS*> setcover_con, SCIP_CONS* side_con,
                                 vector<pair<unsigned int, unsigned int> > mlVec,
				 vector<pair<unsigned int, unsigned int> > clVec,
				 vector<double>& upper,
				 vector<double>& lower
                                ) {
  vector<vector<bool> > clusters;
  vector<double> costs;

  extract_clusters(fileName, distances, clusters, costs, upper, lower);

  assert (clusters.size() == costs.size());
  int size = costs.size();

  for (int counter = 0; counter < size; counter++) {
    if ( cluster_satisfies_mlcl(clusters[counter], mlVec, clVec) ) {
      SCIP_CALL( add_cluster_variable(scip, distances, setcover_con, side_con, clusters[counter],  costs[counter]) );
    }
    else
      cout << "cluster " << counter << " does not satisfy the constraints." << endl;
  }

  return SCIP_OKAY;
}

SCIP_RETCODE init_sol_dir(const char* dirName, SCIP* scip,
			  const vector<vector<double> > & distances,
			  vector<SCIP_CONS*> setcover_con, SCIP_CONS* side_con,
			  vector<pair<unsigned int, unsigned int> > mlVec,
			  vector<pair<unsigned int, unsigned int> > clVec,
			  vector<double>& upper,
			  vector<double>& lower
			  ){

  priority_queue<struct initClustering, vector<struct initClustering>, compQ> clusteringQ;
  DIR * dir = opendir (dirName);
  struct dirent * entries;

  upper.clear();
  lower.clear();

  while ((entries = readdir(dir)) != NULL)
    if (entries->d_type == 8){
      bool success = false;
      struct initClustering currClustering;
      vector<vector<bool> > clusters;
      vector<double> costs;

      vector<double> upper_bounds;
      vector<double> lower_bounds;

      char fileName [255];
      strcpy(fileName, dirName);
      strcat(fileName, "/");
      strcat(fileName, entries->d_name);
      success = extract_clusters(fileName, distances, clusters, costs, upper_bounds, lower_bounds);

      if (success){
	currClustering.totalCost = 0;
	currClustering.satCount = 0;
	for (int counter = 0, clusteringSize = clusters.size(); counter < clusteringSize; counter++)
	  if (cluster_satisfies_mlcl(clusters[counter], mlVec, clVec)){
	    currClustering.clusters.push_back(clusters[counter]);
	    currClustering.costs.push_back(costs[counter]);
	    currClustering.totalCost += costs[counter];
	    (currClustering.satCount)++;
	  }
	currClustering.upper_bounds = upper_bounds;
	currClustering.lower_bounds = lower_bounds;
	clusteringQ.push(currClustering);
      }

    }
  closedir(dir);

  if (!clusteringQ.empty()){
    vector<vector<bool> > bestClusters = (clusteringQ.top()).clusters;
    vector<double> bClusterCosts = (clusteringQ.top()).costs;
    upper = (clusteringQ.top()).upper_bounds;
    lower = (clusteringQ.top()).lower_bounds;
    for (int counter = 0, clusteringSize = bestClusters.size(); counter < clusteringSize; counter++)
      SCIP_CALL( add_cluster_variable(scip, distances, setcover_con, side_con, bestClusters[counter],  bClusterCosts[counter]) );
  }

  return SCIP_OKAY;

}

SCIP_RETCODE init_MlCl_constraints(const char* consFile, SCIP* scip, vector<pair<unsigned int, unsigned int> >& mlVec, vector<pair<unsigned int, unsigned int> >& clVec){

   extract_constraints(consFile, mlVec, clVec);

   char con_name[255];

   for (auto itr = mlVec.begin(); itr!= mlVec.end(); itr++){
    SCIP_CONS* consML;
    SCIPsnprintf(con_name, 255, "ML-%i-%i", itr->first, itr->second);
    SCIP_CALL( SCIPcreateConsML(scip, &consML, con_name,
                  itr->first,                       /* the first data point to be linked */
                  itr->second                       /* the second data point to be linked */
                  ) );
    SCIP_CALL( SCIPaddCons(scip, consML) );
   }

   for (auto itr = clVec.begin(); itr!= clVec.end(); itr++){
    SCIP_CONS* consCL;
    SCIPsnprintf(con_name, 255, "CL-%i-%i", itr->first, itr->second);
    SCIP_CALL( SCIPcreateConsCL(scip, &consCL, con_name,
                  itr->first,                       /* the first data point to be linked */
                  itr->second                       /* the second data point to be linked */
                  ) );

    SCIP_CALL( SCIPaddCons(scip, consCL) );
   }
   return SCIP_OKAY;
}

/** private-like **/

bool extract_clusters(const char* fileName, const vector<vector<double> >& distances, vector<vector<bool> >& clusters, vector<double>& costs, vector<double>& upper, vector<double>& lower){

  clusters.clear();
  ifstream inputFile (fileName, ios::in);

  if (inputFile.fail()) {
    cerr << "Error! Can't open cluster file '" << fileName << "', quitting...\n";
    return false;
  }

  int number_of_elements = distances.size();

  int line = -1;
  string s;
  while(getline(inputFile,s)) {
    if (!s.empty()) {
      line += 1;
      int clusIndex = atoi(s.c_str());

      if (clusIndex < 0) {
          cerr << "Error reading '" << fileName << "', found negative cluster index '" << clusIndex << "'\n";
	  return false;
      }

      if (clusIndex > (int)clusters.size()-1) {
          // a new cluster index
          // create empty clusters upto the index+1 (to compensate offset 0)
          clusters.resize(clusIndex+1, vector<bool>(number_of_elements, false));
      }

      clusters[clusIndex][line] = true;
    }
  }
  int number_of_clusters = clusters.size();

  #ifndef NDEBUG
  for (int counter = 0; counter < number_of_clusters; counter++)
    assert (clusters[counter].size() == (unsigned)number_of_elements);
  #endif

  /* computing costs of clusters*/
  costs.clear();
  for (int counter = 0; counter < number_of_clusters; counter++)
    costs.push_back(ObjPricerOptClust::getClusterCost(distances, clusters[counter]));

  /* print the sum of all cluster costs (cost of the whole clustering) */
  double clusteringCost = 0;
  for (auto itr = costs.begin(), itrEnd = costs.end(); itr != itrEnd; itr++)
    clusteringCost += *itr;
  cout << "\ncost of the initial clustering: " << setprecision(15) << clusteringCost << endl;

  /* compute an estimate for first coefficients */
  getFirstCoefs (distances, clusters, costs, upper, lower);

return true;

}

SCIP_RETCODE add_cluster_variable(

   SCIP*                 scip,
   const vector<vector<double> >& distances,
   vector<SCIP_CONS*> setcover_con,
   SCIP_CONS* side_con,
   const vector<bool>&      cluster,
   double cluster_cost
   )
{
   char var_name[255];
   SCIPsnprintf(var_name, 255, "X_%d", SCIPgetNVars(scip));
   SCIPdebugMessage("new variable <%s>\n", var_name);

   /* create the new variable */

   SCIP_VAR* var;
   SCIP_CALL( SCIPcreateVar(scip, &var, var_name,
                            0.0,                     // lower bound
                            1.0,      // upper bound
                            cluster_cost,                       // objective
                            SCIP_VARTYPE_BINARY, // variable type
                            false, false, 0, 0, 0, 0, 0) );

   /* add new variable to the list of variables to price into LP */
   SCIP_CALL( SCIPaddVar(scip, var) );

   /* add the coefficients of this variable to setcovering and side constraints */
   for (unsigned i= 0; i < cluster.size(); ++i)
      if (cluster[i])
         SCIP_CALL( SCIPaddCoefLinear(scip, setcover_con[i], var, 1.0) );

   SCIP_CALL( SCIPaddCoefLinear(scip, side_con, var, 1.0 ) );

   // /* cleanup */
   // SCIP_CALL( SCIPreleaseVar(scip, &var) );

   return SCIP_OKAY;
}

void extract_constraints(const char* fileName, vector<pair<unsigned int, unsigned int> >& mlVec, vector<pair< unsigned int, unsigned int> >& clVec){
   mlVec.clear();
   clVec.clear();
   ifstream inputFile (fileName, ios::in);

  if (inputFile.fail()) {
    cerr << "Error! Can't open constraint file '" << fileName << "', quitting...\n";
    exit(-1);
  }

  string line;
  while(getline(inputFile, line)){
     istringstream ss(line);
     unsigned int first, second;
     int consType;
     ss >> first >> second >> consType;
     if (!ss.fail()){
        pair<unsigned int, unsigned int> p =
           make_pair(first, second);
        if (consType == 1)
           mlVec.push_back(p);
        else if (consType == -1)
           clVec.push_back(p);
     }
  }
}

bool cluster_satisfies_mlcl(std::vector< bool > cluster, vector< std::pair< unsigned int, unsigned int > > mlVec, vector< std::pair< unsigned int, unsigned int > > clVec)
{
    // check ML violation
    for (unsigned i=0; i!=mlVec.size(); i++) {
        unsigned first = mlVec[i].first;
        unsigned second = mlVec[i].second;
        if ( ( cluster[first] == true && cluster[second] == false )
           ||( cluster[first] == false && cluster[second] == true ) )
            return false;
    }
    // check CL violation
    for (unsigned i=0; i!=clVec.size(); i++) {
        if ( cluster[clVec[i].first] == true && cluster[clVec[i].second] == true )
            return false;
    }
    return true;
}

void getFirstCoefs (const vector<vector<double> >& distances, const vector<vector<bool> >& clusters, const vector<double>& costs, vector<double>& upper, vector<double>& lower){

  unsigned int num_instances = distances.size();
  unsigned int num_clusetrs = clusters.size();
  vector<int> cluster_sizes (num_clusetrs , -1);
  vector<int> assignments (num_instances, -1);

  upper.clear();
  upper.resize(num_instances);
  lower.clear();
  lower.resize(num_instances);

  for (size_t i = 0; i < num_clusetrs; i++){
    unsigned int clusterSize = 0;
    for (size_t j = 0; j < num_instances; j++)
      if (clusters[i][j]){
	assignments[j] = i;
	clusterSize++;
      }

    cluster_sizes[i] = clusterSize;
  }

  #ifndef NDEBUG
  for (auto itr = assignments.begin(), iend = assignments.end(); itr!=iend; itr++)
    if (*itr >= num_clusetrs || *itr < 0){
      cout << "something went wrong when assigning instances to clusters" << endl;
      exit (EXIT_FAILURE);
    }

  for (auto itr = cluster_sizes.begin(), iend = cluster_sizes.end(); itr!=iend; itr++)
    if (*itr < 0){
      cout << "cluster with negative size?" << endl;
      exit (EXIT_FAILURE);
    }
  #endif

  for (size_t curr_inst = 0; curr_inst < num_instances; curr_inst++){
    double curr_dist_sum = 0;

    for (size_t other_inst = 0; other_inst < num_instances; other_inst++)
      if (assignments[curr_inst] == assignments[other_inst])
	curr_dist_sum += distances[curr_inst][other_inst];

    unsigned int curr_assigned_cluster = assignments[curr_inst];
    double curr_new_cost = (costs[curr_assigned_cluster] * cluster_sizes[curr_assigned_cluster] -
			    curr_dist_sum ) / (cluster_sizes[curr_assigned_cluster] - 1);
    lower[curr_inst] = costs[curr_assigned_cluster] - curr_new_cost;
  }

  for (size_t curr_inst = 0; curr_inst < num_instances; curr_inst++){
    bool firstRound = true;
    double min_diff = 0;

    for (size_t curr_clust = 0; curr_clust < num_clusetrs; curr_clust++)
      if (curr_clust != static_cast <unsigned int> (assignments[curr_inst])){
	double dist_sum = 0;
	for (size_t other_inst = 0; other_inst < num_instances; other_inst++)
	  if (clusters[curr_clust][other_inst])
	    dist_sum += distances[curr_inst][other_inst];

	double curr_clust_diff = ((costs[curr_clust] * cluster_sizes[curr_clust]
				   + dist_sum) / (cluster_sizes[curr_clust] + 1)) - costs[curr_clust];
	if (firstRound){
	  min_diff = curr_clust_diff;
	  firstRound = false;
	}
	else if (curr_clust_diff < min_diff)
	  min_diff = curr_clust_diff;
      }

    assert (firstRound == false);
    upper[curr_inst] = min_diff;
  }

}


#include <mpi.h>

#include <string>
#include <map>
#include <time.h>
#include <vector>
#include <fstream>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>

#include <arcane/ArcaneVersion.h>
#include <arcane/Timer.h>
#include <arcane/ItemPairGroup.h>
#include <arcane/mesh/ItemFamily.h>
#include <arcane/utils/PlatformUtils.h>
#include <arcane/utils/IMemoryInfo.h>
#include <arcane/utils/OStringStream.h>
#include <arcane/ITimeLoopMng.h>
#include <alien/arcane_tools/accessors/ItemVectorAccessor.h>
#include <alien/core/block/VBlock.h>

#include <alien/arcane_tools/IIndexManager.h>
#include <alien/arcane_tools/indexManager/BasicIndexManager.h>
#include <alien/arcane_tools/indexManager/SimpleAbstractFamily.h>
#include <alien/arcane_tools/distribution/DistributionFabric.h>
#include <alien/arcane_tools/indexSet/IndexSetFabric.h>
#include <alien/arcane_tools/data/Space.h>

#include <alien/kernels/simple_csr/algebra/SimpleCSRLinearAlgebra.h>
#include <alien/kernels/simple_csr/algebra/SimpleCSRInternalLinearAlgebra.h>

#include <alien/ref/AlienRefSemantic.h>

#include <alien/kernels/redistributor/Redistributor.h>
#include <alien/ref/data/scalar/RedistributedVector.h>
#include <alien/ref/data/scalar/RedistributedMatrix.h>
#include <alien/ref/import_export/MatrixMarketSystemWriter.h>

#include <alien/expression/solver/SolverStater.h>

#ifdef ALIEN_USE_PETSC
#include <alien/kernels/petsc/io/AsciiDumper.h>
#include <alien/kernels/petsc/algebra/PETScLinearAlgebra.h>
#endif
#ifdef ALIEN_USE_MTL4
#include <alien/kernels/mtl/algebra/MTLLinearAlgebra.h>
#endif
#ifdef ALIEN_USE_HTSSOLVER
#include <alien/kernels/hts/HTSBackEnd.h>
#include <alien/kernels/hts/data_structure/HTSMatrix.h>
#include <alien/kernels/hts/algebra/HTSLinearAlgebra.h>
#endif
#ifdef ALIEN_USE_TRILINOS
#include <alien/kernels/trilinos/TrilinosBackEnd.h>
#include <alien/kernels/trilinos/data_structure/TrilinosMatrix.h>
#include <alien/kernels/trilinos/algebra/TrilinosLinearAlgebra.h>
#endif

#include <alien/expression/solver/ILinearSolver.h>
#include "AlienCoreSolverOptionTypes.h"

#include "AlienBenchModule.h"

#include <arcane/ItemPairGroup.h>
#include <arcane/IMesh.h>

#include <alien/core/impl/MultiVectorImpl.h>

#include <alien/expression/krylov/AlienKrylov.h>
#include <alien/utils/StdTimer.h>

using namespace Arcane;
using namespace Alien;

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

void
AlienBenchModule::init()
{
  Alien::setTraceMng(traceMng());
  Alien::setVerbosityLevel(Alien::Verbosity::Debug);
  m_parallel_mng = subDomain()->parallelMng();
  m_homogeneous = options()->homogeneous();
  m_diag_coeff = options()->diagonalCoefficient();
  m_lambdax = options()->lambdax();
  m_lambday = options()->lambday();
  m_lambdaz = options()->lambdaz();
  m_alpha = options()->alpha();

  Alien::ILinearSolver* solver = options()->linearSolver();
  solver->init();
}

/*---------------------------------------------------------------------------*/

void
AlienBenchModule::test()
{
  Timer pbuild_timer(subDomain(), "PBuildPhase", Timer::TimerReal);
  Timer psolve_timer(subDomain(), "PSolvePhase", Timer::TimerReal);
  Timer rbuild_timer(subDomain(), "RBuildPhase", Timer::TimerReal);
  Timer rsolve_timer(subDomain(), "RSolvePhase", Timer::TimerReal);

  ItemGroup areaU = allCells();
  CellCellGroup cell_cell_connection(areaU.own(), areaU, m_stencil_kind);
  CellCellGroup all_cell_cell_connection(areaU, areaU, m_stencil_kind);

  Alien::ArcaneTools::BasicIndexManager index_manager(m_parallel_mng);
  index_manager.setTraceMng(traceMng());

  auto indexSetU = index_manager.buildScalarIndexSet("U", areaU);
  index_manager.prepare();

  ///////////////////////////////////////////////////////////////////////////
  //
  // CREATE Space FROM IndexManger
  // CREATE MATRIX ASSOCIATED TO Space
  // CREATE VECTORS ASSOCIATED TO Space
  //

  // Accès à l'indexation
  Arccore::UniqueArray<Arccore::Integer> allUIndex = index_manager.getIndexes(indexSetU);

  Alien::ArcaneTools::Space space(&index_manager, "TestSpace");
  m_mdist = Alien::ArcaneTools::createMatrixDistribution(space);
  m_vdist = Alien::ArcaneTools::createVectorDistribution(space);

  info() << "GLOBAL SIZE : " << m_vdist.globalSize();

  Alien::Vector vectorB(m_vdist);
  Alien::Vector vectorBB(m_vdist);
  Alien::Vector vectorX(m_vdist);

  Alien::Vector coordX(m_vdist);
  Alien::Vector coordY(m_vdist);
  Alien::Vector coordZ(m_vdist);

  ///////////////////////////////////////////////////////////////////////////
  //
  // VECTOR BUILDING AND FILLING
  //
  info() << "Building & initializing vector b";
  info() << "Space size = " << m_vdist.globalSize()
         << ", local size= " << m_vdist.localSize();

  ENUMERATE_CELL (icell, areaU) {
    Real3 x;
    ENUMERATE_NODE (inode, icell->nodes()) {
      x += m_node_coord[*inode];
    }
    x /= icell->nbNode();
    m_cell_center[icell] = x;
    m_u[icell] = funcn(x);
    m_k[icell] = funck(x);
  }
  {
    // Builder du vecteur
    Alien::VectorWriter writer(vectorX);
    Alien::VectorWriter x(coordX);
    Alien::VectorWriter y(coordY);
    Alien::VectorWriter z(coordZ);

    ENUMERATE_CELL(icell,areaU.own())
    {
      const Integer iIndex = allUIndex[icell->localId()];
      writer[iIndex] = m_u[icell] ;
      x[iIndex] = m_cell_center[icell].x ;
      y[iIndex] = m_cell_center[icell].y ;
      z[iIndex] = m_cell_center[icell].z ;
    }
  }

  Alien::Matrix matrixA(m_mdist); // local matrix for exact measure without side effect
                                  // (however, you can reuse a matrix with several
                                  // builder)

  ///////////////////////////////////////////////////////////////////////////
  //
  // MATRIX BUILDING AND FILLING
  //
  {
    Timer::Sentry ts(&pbuild_timer);
    Alien::MatrixProfiler profiler(matrixA);
    ///////////////////////////////////////////////////////////////////////////
    //
    // DEFINE PROFILE
    //
    ENUMERATE_ITEMPAIR(Cell, Cell, icell, cell_cell_connection)
    {
      const Cell& cell = *icell;
      const Integer iIndex = allUIndex[cell.localId()];
      profiler.addMatrixEntry(iIndex, allUIndex[cell.localId()]);
      ENUMERATE_SUB_ITEM(Cell, isubcell, icell)
      {
        const Cell& subcell = *isubcell;
        profiler.addMatrixEntry(iIndex, allUIndex[subcell.localId()]);
      }
    }
  }
  {
    Timer::Sentry ts(&pbuild_timer);
    Alien::ProfiledMatrixBuilder builder(
        matrixA, Alien::ProfiledMatrixOptions::eResetValues);
    ENUMERATE_ITEMPAIR(Cell, Cell, icell, cell_cell_connection)
    {
      const Cell& cell = *icell;
      double diag = dii(cell);

      Integer i = allUIndex[cell.localId()];
      builder(i, i) += diag;
      ENUMERATE_SUB_ITEM(Cell, isubcell, icell)
      {
        const Cell& subcell = *isubcell;
        double off_diag = fij(cell, subcell);
        builder(i, i) += off_diag;
        Integer j = allUIndex[subcell.localId()];
        builder(i, j) -= off_diag;
      }
    }
    if (options()->sigma() > 0.) {
      m_sigma = options()->sigma();
      auto xCmax = Real3{ 0.25, 0.25, 0.25 };
      auto xCmin = Real3{ 0.75, 0.75, 0.55 };
      ENUMERATE_ITEMPAIR(Cell, Cell, icell, all_cell_cell_connection)
      {
        const Cell& cell = *icell;
        Real3 xC = m_cell_center[icell];
        Real3 xDmax = xC - xCmax;
        Real3 xDmin = xC - xCmin;
        m_s[cell] = 0;
        if (xDmax.abs() < options()->epsilon()) {
          m_s[cell] = 1.;
          Integer i = allUIndex[cell.localId()];
          info() << "MATRIX TRANSFO SIGMAMAX " << i;
          if (cell.isOwn())
            builder(i, i) = m_sigma;
          ENUMERATE_SUB_ITEM(Cell, isubcell, icell)
          {
            const Cell& subcell = *isubcell;
            if (subcell.isOwn()) {
              Integer j = allUIndex[subcell.localId()];
              builder(j, i) = 0.;
            }
          }
        }
        if (xDmin.abs() < options()->epsilon()) {
          m_s[cell] = -1.;
          Integer i = allUIndex[cell.localId()];
          info() << "MATRIX TRANSFO SIGMA MIN" << i;

          if (cell.isOwn())
            builder(i, i) = 1. / m_sigma;
          ENUMERATE_SUB_ITEM(Cell, isubcell, icell)
          {
            const Cell& subcell = *isubcell;
            if (subcell.isOwn()) {
              Integer j = allUIndex[subcell.localId()];
              builder(j, i) = 0.;
            }
          }
        }
      }
    }

    builder.finalize();
  }

  {
    Alien::SimpleCSRLinearAlgebra csrAlg;
    csrAlg.mult(matrixA, vectorX, vectorB);
    csrAlg.mult(matrixA, vectorX, vectorBB);
    Real normeb = csrAlg.norm2(vectorB);
    info() << "||b||=" << normeb;
  }
#ifdef ALIEN_USE_HTSSOLVER
/*{
  info()<<"HTS";
  Alien::HTSLinearAlgebra htsAlg;
  htsAlg.mult(matrixA,vectorX,vectorB);
  htsAlg.mult(matrixA,vectorX,vectorBB);
  Real normeb = htsAlg.norm2(vectorB) ;
  info()<<"||b||="<<normeb;
}*/
#endif
#ifdef ALIEN_USE_TRILINOS
  /*{
    info() << "Trilinos";
    Alien::TrilinosLinearAlgebra tpetraAlg;
    tpetraAlg.mult(matrixA, vectorX, vectorB);
    tpetraAlg.mult(matrixA, vectorX, vectorBB);
    Real normeb = tpetraAlg.norm2(vectorB);
    info() << "||b||=" << normeb;
    // tpetraAlg.dump(matrixA,"MatrixA.txt") ;
    // tpetraAlg.dump(vectorB,"vectorB.txt") ;
    // tpetraAlg.dump(vectorBB,"vectorBB.txt") ;
    // tpetraAlg.dump(vectorX,"vectorX.txt") ;
  }*/
#endif

  if (options()->unitRhs()) {
    Alien::LocalVectorWriter v(vectorBB);
    for (Integer i = 0; i < v.size(); ++i)
      v[i] = 1.;
  }

  if (options()->zeroRhs()) {
    Alien::LocalVectorWriter v(vectorB);
    for (Integer i = 0; i < v.size(); ++i)
      v[i] = 0.;
  }

  ///////////////////////////////////////////////////////////////////////////
  //
  // RESOLUTION
  //
  if(options()->alienCoreSolver.size()>0)
  {
    {
      Alien::LocalVectorReader reader(vectorBB);
      Alien::LocalVectorWriter vb(vectorB);
      Alien::LocalVectorWriter vx(vectorX);
      for (Integer i = 0; i < m_vdist.localSize(); ++i) {
        vx[i] = 0.;
        vb[i] = reader[i];
      }
    }
    typedef Alien::SimpleCSRInternalLinearAlgebra    AlgebraType ;
    typedef typename AlgebraType::BackEndType        BackEndType ;
    typedef Alien::Iteration<AlgebraType>            StopCriteriaType ;
    typedef Alien::CG<AlgebraType>                   SolverType ;

    AlgebraType alg ;
    auto const& true_A = matrixA.impl()->get<Alien::BackEnd::tag::simplecsr>() ;
    auto const& true_b = vectorB.impl()->get<Alien::BackEnd::tag::simplecsr>() ;
    auto&       true_x = vectorX.impl()->get<Alien::BackEnd::tag::simplecsr>(true) ;

    auto const& opt           = options()->alienCoreSolver[0] ;
    int output_level  = opt->outputLevel() ;
    int max_iteration = opt->maxIter() ;
    double tol        = opt->tol() ;

    auto solver_opt   = opt->solver() ;
    auto precond_opt  = opt->preconditioner() ;

    auto backend      = opt->backend() ;
    auto asynch       = opt->asynch() ;

    // clang-format off
    auto run = [&](auto& alg)
              {
                typedef typename
                    boost::remove_reference<decltype(alg)>::type AlgebraType ;
                typedef typename AlgebraType::BackEndType        BackEndType ;
                typedef Alien::Iteration<AlgebraType>            StopCriteriaType ;

                auto const& true_A = matrixA.impl()->get<BackEndType>() ;
                auto const& true_b = vectorB.impl()->get<BackEndType>() ;
                auto&       true_x = vectorX.impl()->get<BackEndType>(true) ;

                StopCriteriaType stop_criteria{alg,true_b,tol,max_iteration,output_level>0?traceMng():nullptr} ;

                switch(solver_opt)
                {
                  case AlienCoreSolverOptionTypes::CG:
                  {
                    typedef Alien::CG<AlgebraType> SolverType ;

                    SolverType solver{alg,traceMng()} ;
                    solver.setOutputLevel(output_level) ;
                    switch(precond_opt)
                    {
                      case AlienCoreSolverOptionTypes::ChebyshevPoly:
                      {
                          info()<<"CHEBYSHEV PRECONDITIONER";
                          double polynom_factor          = opt->polyFactor() ;
                          int    polynom_order           = opt->polyOrder() ;
                          int    polynom_factor_max_iter = opt->polyFactorMaxIter() ;

                          typedef Alien::ChebyshevPreconditioner<AlgebraType,true> PrecondType ;
                          PrecondType      precond{alg,true_A,polynom_factor,polynom_order,polynom_factor_max_iter,traceMng()} ;
                          precond.setOutputLevel(output_level) ;
                          precond.init() ;

                          Timer::Sentry ts(&psolve_timer);
                          if(asynch==0)
                            solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                          else
                            solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                      }
                      break;
                      case AlienCoreSolverOptionTypes::NeumannPoly:
                        {
                          info()<<"NEUMANN PRECONDITIONER";
                          double polynom_factor          = opt->polyFactor() ;
                          int    polynom_order           = opt->polyOrder() ;
                          int    polynom_factor_max_iter = opt->polyFactorMaxIter() ;

                          typedef Alien::NeumannPolyPreconditioner<AlgebraType> PrecondType ;
                          PrecondType precond{alg,true_A,polynom_factor,polynom_order,polynom_factor_max_iter,traceMng()} ;
                          precond.init() ;

                          Timer::Sentry ts(&psolve_timer);
                          if(asynch==0)
                            solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                          else
                            solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                        }
                        case AlienCoreSolverOptionTypes::Diag:
                        default:
                        {
                          info()<<"DIAG PRECONDITIONER";
                          typedef Alien::DiagPreconditioner<AlgebraType> PrecondType ;
                          PrecondType      precond{alg,true_A} ;
                          precond.init() ;

                          Timer::Sentry ts(&psolve_timer);
                          if(asynch==0)
                            solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                          else
                            solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                        }
                        break ;
                    }
                }
                break ;
                case AlienCoreSolverOptionTypes::BCGS:
                {
                  typedef Alien::BiCGStab<AlgebraType> SolverType ;
                  SolverType solver{alg,traceMng()} ;
                  solver.setOutputLevel(output_level) ;
                  switch(precond_opt)
                  {
                  case AlienCoreSolverOptionTypes::ChebyshevPoly:
                   {
                      info()<<"CHEBYSHEV PRECONDITIONER";
                      double polynom_factor          = opt->polyFactor() ;
                      int    polynom_order           = opt->polyOrder() ;
                      int    polynom_factor_max_iter = opt->polyFactorMaxIter() ;

                      typedef Alien::ChebyshevPreconditioner<AlgebraType,true> PrecondType ;
                      PrecondType      precond{alg,true_A,polynom_factor,polynom_order,polynom_factor_max_iter,traceMng()} ;
                      precond.setOutputLevel(output_level) ;
                      precond.init() ;

                      Timer::Sentry ts(&psolve_timer);
                      if(asynch==0)
                        solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                      else
                        solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                    }
                   break ;
                  case AlienCoreSolverOptionTypes::NeumannPoly:
                    {
                      info()<<"NEUMANN PRECONDITIONER";
                      double polynom_factor          = opt->polyFactor() ;
                      int    polynom_order           = opt->polyOrder() ;
                      int    polynom_factor_max_iter = opt->polyFactorMaxIter() ;

                      typedef Alien::NeumannPolyPreconditioner<AlgebraType> PrecondType ;
                      PrecondType precond{alg,true_A,polynom_factor,polynom_order,polynom_factor_max_iter,traceMng()} ;
                      precond.init() ;

                      Timer::Sentry ts(&psolve_timer);
                      if(asynch==0)
                        solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                      else
                        solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                    }
                    break ;
                  case AlienCoreSolverOptionTypes::ILU0:
                    {
                      info()<<"ILU0 PRECONDITIONER";
                      typedef Alien::ILU0Preconditioner<AlgebraType> PrecondType ;
                      PrecondType precond{alg,true_A,traceMng()} ;
                      precond.init() ;

                      Timer::Sentry ts(&psolve_timer);
                      if(asynch==0)
                        solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                      else
                        solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                    }
                    break ;
                  case AlienCoreSolverOptionTypes::FILU0:
                    {
                      info()<<"FILU0 PRECONDITIONER";
                      typedef Alien::FILU0Preconditioner<AlgebraType> PrecondType ;
                      PrecondType precond{alg,true_A,traceMng()} ;
                      precond.setParameter("nb-factor-iter",opt->filuFactorNiter()) ;
                      precond.setParameter("nb-solver-iter",opt->filuSolverNiter()) ;
                      precond.setParameter("tol",           opt->filuTol()) ;
                      precond.init() ;

                      Timer::Sentry ts(&psolve_timer);
                      if(asynch==0)
                        solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                      else
                        solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                    }
                    break ;

                  case AlienCoreSolverOptionTypes::Diag:
                  default:
                    {
                      info()<<"DIAG PRECONDITIONER";
                      typedef Alien::DiagPreconditioner<AlgebraType> PrecondType ;
                      PrecondType      precond{alg,true_A} ;
                      precond.init() ;

                      Timer::Sentry ts(&psolve_timer);
                      if(asynch==0)
                        solver.solve(precond,stop_criteria,true_A,true_b,true_x) ;
                      else
                        solver.solve2(precond,stop_criteria,true_A,true_b,true_x) ;
                    }
                    break ;
                  }
                }
                break ;
                default :
                  fatal()<<"unknown solver";
                }

                if(stop_criteria.getStatus())
                {
                  info()<<"Solver has converged";
                  info()<<"Nb iterations  : "<<stop_criteria();
                  info()<<"Criteria value : "<<stop_criteria.getValue();
                }
                else
                {
                  info()<<"Solver convergence failed";
                }
              } ;
    // clang-format on

    switch(backend)
    {
      case AlienCoreSolverOptionTypes::SimpleCSR:
      {
        Alien::SimpleCSRInternalLinearAlgebra alg;
        run(alg);
      }
      break ;
      case AlienCoreSolverOptionTypes::SYCL:
      {
#ifdef ALIEN_USE_SYCL
        Alien::SYCLInternalLinearAlgebra alg;
        alg.setDotAlgo(vm["dot-algo"].as<int>());
        run(alg);
#else
        fatal() << "SYCL BackEnd not available";
#endif
      }
      break ;
      default:
        fatal() << "SYCL BackEnd not available";
        break ;
    }
  }
  else
  {
    Alien::ILinearSolver* solver = options()->linearSolver();
    solver->init();
#ifdef ALIEN_USE_TRILINOS
      if(solver->getBackEndName().contains("tpetraserial"))
      {
        auto& mat = matrixA.impl()->get<Alien::BackEnd::tag::tpetraserial>(false) ;
        mat.setMatrixCoordinate(coordX,coordY,coordZ) ;
      }
#ifdef KOKKOS_ENABLE_OPENMP
      if(solver->getBackEndName().contains("tpetraomp"))
      {
        auto& mat = matrixA.impl()->get<Alien::BackEnd::tag::tpetraomp>(false) ;
        mat.setMatrixCoordinate(coordX,coordY,coordZ) ;
      }
#endif
#ifdef KOKKOS_ENABLE_THREADS

      if(solver->getBackEndName().contains("tpetrapth"))
      {
        auto& mat = matrixA.impl()->get<Alien::BackEnd::tag::tpetrapth>(false) ;
        mat.setMatrixCoordinate(coordX,coordY,coordZ) ;
      }
#endif
#ifdef KOKKOS_ENABLE_CUDA
      if(solver->getBackEndName().contains("tpetracuda"))
      {
        auto& mat = matrixA.impl()->get<Alien::BackEnd::tag::tpetracuda>(true) ;
        mat->setCoordinate(coordX,coordY,coordZ) ;
      }
#endif
#endif

    if (not solver->hasParallelSupport() and m_parallel_mng->commSize() > 1) {
      info() << "Current solver has not a parallel support for solving linear system : "
                "skip it";
    } else {
      Integer nb_resolutions = options()->nbResolutions();
      for (Integer i = 0; i < nb_resolutions; ++i) {
        if (i > 0) // i=0, matrix allready filled
        {
          Timer::Sentry ts(&pbuild_timer);
          Alien::ProfiledMatrixBuilder builder(
              matrixA, Alien::ProfiledMatrixOptions::eResetValues);
          ENUMERATE_ITEMPAIR(Cell, Cell, icell, cell_cell_connection)
          {
            const Cell& cell = *icell;
            double diag = dii(cell);

            Integer i = allUIndex[cell.localId()];
            builder(i, i) += diag;
            ENUMERATE_SUB_ITEM(Cell, isubcell, icell)
            {
              const Cell& subcell = *isubcell;
              double off_diag = fij(cell, subcell);
              builder(i, i) += off_diag;
              Integer j = allUIndex[subcell.localId()];
              builder(i, j) -= off_diag;
            }
          }
          if (options()->sigma() > 0.) {
            m_sigma = options()->sigma();
            auto xCmax = Real3{ 0.25, 0.25, 0.25 };
            auto xCmin = Real3{ 0.75, 0.75, 0.55 };
            ENUMERATE_ITEMPAIR(Cell, Cell, icell, all_cell_cell_connection)
            {
              const Cell& cell = *icell;
              Real3 xC = m_cell_center[icell];
              Real3 xDmax = xC - xCmax;
              Real3 xDmin = xC - xCmin;
              m_s[cell] = 0;
              if (xDmax.abs() < options()->epsilon()) {
                m_s[cell] = 1.;
                Integer i = allUIndex[cell.localId()];
                info() << "MATRIX TRANSFO SIGMAMAX " << i;
                if (cell.isOwn())
                  builder(i, i) = m_sigma;
                ENUMERATE_SUB_ITEM(Cell, isubcell, icell)
                {
                  const Cell& subcell = *isubcell;
                  if (subcell.isOwn()) {
                    Integer j = allUIndex[subcell.localId()];
                    builder(j, i) = 0.;
                  }
                }
              }
              if (xDmin.abs() < options()->epsilon()) {
                m_s[cell] = -1.;
                Integer i = allUIndex[cell.localId()];
                info() << "MATRIX TRANSFO SIGMA MIN" << i;

                if (cell.isOwn())
                  builder(i, i) = 1. / m_sigma;
                ENUMERATE_SUB_ITEM(Cell, isubcell, icell)
                {
                  const Cell& subcell = *isubcell;
                  if (subcell.isOwn()) {
                    Integer j = allUIndex[subcell.localId()];
                    builder(j, i) = 0.;
                  }
                }
              }
            }
          }

          builder.finalize();
        }

        // Réinitialisation de vectorX
        // if(i>0)  // i=0, vector allready filled
        {
          Alien::LocalVectorReader reader(vectorBB);
          Alien::LocalVectorWriter vb(vectorB);
          Alien::LocalVectorWriter vx(vectorX);
          for (Integer i = 0; i < m_vdist.localSize(); ++i) {
            vx[i] = 0.;
            vb[i] = reader[i];
          }
        }
        if(i==0)
        {
          Alien::MatrixMarketSystemWriter matrix_exporter("AlienBenchMatrixA.mtx",m_parallel_mng->messagePassingMng()) ;
          matrix_exporter.dump(matrixA) ;
          Alien::MatrixMarketSystemWriter vector_exporter("AlienBenchVectorB.mtx",m_parallel_mng->messagePassingMng()) ;
          vector_exporter.dump(vectorB) ;
        }

        Timer::Sentry ts(&psolve_timer);
        solver->solve(matrixA, vectorB, vectorX);
      }
      Alien::SolverStatus status = solver->getStatus();
      if (status.succeeded) {
        info()<<"RESOLUTION SUCCEED";
        Alien::VectorReader reader(vectorX);
        ENUMERATE_CELL (icell, areaU.own()) {
          const Integer iIndex = allUIndex[icell->localId()];
          m_x[icell] = reader[iIndex];
        }

        SimpleCSRLinearAlgebra alg;
        Alien::Vector vectorR(m_vdist);
        alg.mult(matrixA, vectorX, vectorR);
        alg.axpy(-1., vectorB, vectorR);
        Real res = alg.norm2(vectorR);
        info() << "RES : " << res;
      }
      else
        info()<<"SOLVER FAILED";
      solver->getSolverStat().print(Universe().traceMng(), status, "Linear Solver : ");
    }
    solver->end();
  }

  if (options()->redistribution() && m_parallel_mng->commSize() > 1) {
    info() << "Test REDISTRIBUTION";

    {
      Alien::LocalVectorWriter v(vectorX);
      for (Integer i = 0; i < v.size(); ++i)
        v[i] = 0;
    }
    Alien::Vector vectorR(m_vdist);

    bool keep_proc = false;
    if (m_parallel_mng->commRank() == 0)
      keep_proc = true;

    rbuild_timer.start();
    auto small_comm = Arccore::MessagePassing::mpSplit(m_parallel_mng->messagePassingMng(), keep_proc);
    Alien::Redistributor redist(matrixA.distribution().globalRowSize(),
                                m_parallel_mng->messagePassingMng(),
                                small_comm );

    Alien::RedistributedMatrix Aa(matrixA, redist);
    Alien::RedistributedVector bb(vectorB, redist);
    Alien::RedistributedVector xx(vectorX, redist);
    Alien::RedistributedVector rr(vectorR, redist);
    rbuild_timer.stop();
    if (keep_proc) {
      auto solver = options()->linearSolver();
      // solver->updateParallelMng(Aa.distribution().parallelMng());
      solver->init();
      {
        Timer::Sentry ts(&rsolve_timer);
        solver->solve(Aa, bb, xx);
      }

      Alien::SimpleCSRLinearAlgebra alg;

      alg.mult(Aa, xx, rr);
      alg.axpy(-1., bb, rr);
      Real res = alg.norm2(rr);
      info() << "REDISTRIBUTION RES : " << res;
    }
  }
  info() << "===================================================";
  info() << "BENCH INFO :";
  info() << " PBUILD    :" << pbuild_timer.totalTime();
  info() << " PSOLVE    :" << psolve_timer.totalTime();
  info() << " RBUILD    :" << rbuild_timer.totalTime();
  info() << " RSOLVE    :" << rsolve_timer.totalTime();
  info() << "===================================================";

  subDomain()->timeLoopMng()->stopComputeLoop(true);
}

/*---------------------------------------------------------------------------*/

Real
AlienBenchModule::funcn(Real3 p) const
{
  return p.x * p.x * p.y;
}

Real
AlienBenchModule::funck(Real3 p) const
{
#define PI 3.14159265358979323846264

  if (m_homogeneous)
    return m_off_diag_coeff;
  else
    return std::exp(-m_alpha * 0.5 * (1 + std::sin(2 * PI * p.x / m_lambdax))
        * (1 + std::sin(2 * PI * p.y / m_lambday)));
}

Real
AlienBenchModule::dii(const Cell& ci) const
{
  return m_diag_coeff;
}

Real
AlienBenchModule::fij(const Cell& ci, const Cell& cj) const
{
  if (ci == cj)
    return dii(ci);
  else {
    Real3 xi = m_cell_center[ci];
    Real3 xj = m_cell_center[cj];
    Real3 xij = xi + xj;
    xij /= 2.;

    return funck(xij);
  }
}

/*---------------------------------------------------------------------------*/

ARCANE_REGISTER_MODULE_ALIENBENCH(AlienBenchModule);


#ifndef MATHIC_VECTOR_HPP_
#define MATHIC_VECTOR_HPP_

#include <algorithm>
#include <array>
#include <assert.h>
#include <stdexcept>

#include <algorithm>
#include <array>
#include <assert.h>
#include <boost/type_traits/add_const.hpp>
#include <boost/type_traits/add_lvalue_reference.hpp>
#include <boost/type_traits/add_reference.hpp>
#include <boost/type_traits/remove_cv.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/utility/enable_if.hpp>
#include <stdexcept>

namespace mathic {
  namespace detail {
    template <size_t Index, typename T>
    struct data_holder {
      enum {
        index = Index
      };
      typedef T type;
      type value;
      data_holder() : value() {
      }
      data_holder(type const &value) : value(value) {
      }
      template <typename Y>
      data_holder(Y const &value) : value(value) {
      }
      void
      swap(data_holder<Index, T> &rhs) {
        using std::swap;
        swap(value, rhs.value);
      }
      template <size_t idx1, typename Y>
      data_holder &
      operator=(const data_holder<idx1, Y> &rhs) {
        value = rhs.value;
        return *this;
      }
    };
    template <size_t... Indexes>
    struct indexes_tuple {
      enum {
        size = sizeof...(Indexes)
      };
    };
    template <size_t num, typename tp = indexes_tuple<>>
    struct index_builder;
    template <size_t num, size_t... Indexes>
    struct index_builder<num, indexes_tuple<Indexes...>> : index_builder<
                                                               num - 1, indexes_tuple<Indexes..., sizeof...(Indexes)>> {
    };
    template <size_t... Indexes>
    struct index_builder<0, indexes_tuple<Indexes...>> {
      typedef indexes_tuple<Indexes...> type;
      enum {
        size = sizeof...(Indexes)
      };
    };
    template <typename IndexTuple, typename T>
    struct vector_builder;
    template <size_t... Indexes, typename T>
    struct vector_builder<indexes_tuple<Indexes...>, T> : data_holder<Indexes, T>... {
      typedef indexes_tuple<Indexes...> indexes_tuple_type;
      typedef vector_builder<indexes_tuple_type, T> this_type;
      typedef T element_type;
      typedef typename boost::remove_reference<
          typename boost::remove_cv<element_type>::type>::type value_type;
      typedef typename boost::add_lvalue_reference<value_type>::type lvalue_reference;
      typedef typename boost::add_reference<
          typename boost::add_const<value_type>::type>::type const_reference;
      typedef value_type *pointer;
      typedef value_type const *const_pointer;
      typedef value_type *iterator;
      typedef value_type const *const_iterator;
      vector_builder() : data_holder<Indexes, T>()... {
      }
      //		vector_builder(vector_builder const& rhs) :
      //			data_holder< Indexes, T >( rhs.template at< Indexes >() ) ...
      //		{
      //		}
      /**
		 * Construct vector from a vector of larger size
		 * @param rhs
		 * @param
		 */
      template <size_t... IndexesR, typename U>
      vector_builder(
          vector_builder<indexes_tuple<IndexesR...>, U> const &rhs,
          typename boost::enable_if_c<sizeof...(Indexes) <= sizeof...(IndexesR)>::type * = 0) : data_holder<Indexes, T>(rhs.template at<Indexes>())... {
      }
      template <typename... E>
      vector_builder(E const &... args,
                     typename boost::enable_if_c<sizeof...(Indexes) == sizeof...(E)>::type * = 0) : data_holder<Indexes, T>(args)... {
      }
      template <typename... E>
      vector_builder(E &&... args,
                     typename boost::enable_if_c<sizeof...(Indexes) == sizeof...(E)>::type * = 0) : data_holder<Indexes, T>(std::forward<E>(args))... {
      }
      vector_builder(std::initializer_list<value_type> const &args) : data_holder<Indexes, T>(*(args.begin() + Indexes))... {
      }
      vector_builder(const_pointer p) : data_holder<Indexes, T>(*(p + Indexes))... {
      }
      pointer
      data() {
        return &this->data_holder<0, T>::value;
      }
      const_pointer
      data() const {
        return &this->data_holder<0, T>::value;
      }
      template <size_t N>
      lvalue_reference
      at() {
        return this->data_holder<N, T>::value;
      }
      template <size_t N>
      const_reference
      at() const {
        return this->data_holder<N, T>::value;
      }
      iterator
      begin() {
        return &this->data_holder<0, T>::value;
      }
      const_iterator
      begin() const {
        return &this->data_holder<0, T>::value;
      }
      iterator
      end() {
        return begin() + indexes_tuple_type::size;
      }
      const_iterator
      end() const {
        return begin() + indexes_tuple_type::size;
      }
      lvalue_reference
      operator[](size_t idx) {
        assert(idx < indexes_tuple_type::size);
        return begin()[idx];
      }
      const_reference
      operator[](size_t idx) const {
        assert(idx < indexes_tuple_type::size);
        return begin()[idx];
      }
    };
  } // namespace detail
  template <typename T, size_t Size>
  struct vector;
  namespace detail {
    template <size_t N, typename T>
    struct dot_product;
    template <size_t N, typename T, size_t Size>
    struct dot_product<N, vector<T, Size>> {
      typename vector<T, Size>::value_type
      operator()(vector<T, Size> const &lhs, vector<T, Size> const &rhs) {
        return dot_product<N - 1, vector<T, Size>>()(lhs, rhs) +
               lhs.template at<N>() * rhs.template at<N>();
      }
    };
    template <typename T, size_t Size>
    struct dot_product<0, vector<T, Size>> {
      typename vector<T, Size>::value_type
      operator()(vector<T, Size> const &lhs, vector<T, Size> const &rhs) {
        return lhs.template at<0>() * rhs.template at<0>();
      }
    };
    template <size_t N, typename T>
    struct scalar_multiplication;
    template <size_t N, typename T, size_t Size>
    struct scalar_multiplication<N, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &v, U s) {
        scalar_multiplication<N - 1, vector<T, Size>>()(v, s);
        v.template at<N>() *= s;
      }
    };
    template <typename T, size_t Size>
    struct scalar_multiplication<0, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &v, U s) {
        v.template at<0>() *= s;
      }
    };
    template <size_t N, typename T>
    struct scalar_division;
    template <size_t N, typename T, size_t Size>
    struct scalar_division<N, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &v, U s) {
        scalar_division<N - 1, vector<T, Size>>()(v, s);
        v.template at<N>() /= s;
      }
    };
    template <typename T, size_t Size>
    struct scalar_division<0, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &v, U s) {
        v.template at<0>() /= s;
      }
    };
    template <size_t N, typename T>
    struct vector_addition;
    template <size_t N, typename T, size_t Size>
    struct vector_addition<N, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &lhs, vector<U, Size> const &rhs) {
        vector_addition<N - 1, vector<T, Size>>()(lhs, rhs);
        lhs.template at<N>() += rhs.template at<N>();
      }
    };
    template <typename T, size_t Size>
    struct vector_addition<0, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &lhs, vector<U, Size> const &rhs) {
        lhs.template at<0>() += rhs.template at<0>();
      }
    };
    template <size_t N, typename T>
    struct vector_substraction;
    template <size_t N, typename T, size_t Size>
    struct vector_substraction<N, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &lhs, vector<U, Size> const &rhs) {
        vector_substraction<N - 1, vector<T, Size>>()(lhs, rhs);
        lhs.template at<N>() -= rhs.template at<N>();
      }
    };
    template <typename T, size_t Size>
    struct vector_substraction<0, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &lhs, vector<U, Size> const &rhs) {
        lhs.template at<0>() -= rhs.template at<0>();
      }
    };
    template <size_t N, typename T>
    struct set_all_elements;
    template <size_t N, typename T, size_t Size>
    struct set_all_elements<N, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &v, U s) {
        set_all_elements<N - 1, vector<T, Size>>()(v, s);
        v.template at<N>() = s;
      }
    };
    template <typename T, size_t Size>
    struct set_all_elements<0, vector<T, Size>> {
      template <typename U>
      void
      operator()(vector<T, Size> &v, U s) {
        v.template at<0>() = s;
      }
    };
  } // namespace detail
  template <typename T, size_t Size>
  struct vector : detail::vector_builder<
                      typename detail::index_builder<Size>::type, T> {
    typedef detail::vector_builder<
        typename detail::index_builder<Size>::type, T>
        super_type;
    typedef vector<T, Size> this_type;
    typedef typename super_type::value_type value_type;
    typedef typename super_type::lvalue_reference lvalue_reference;
    typedef typename super_type::const_reference const_reference;
    typedef typename super_type::pointer pointer;
    typedef typename super_type::const_pointer const_pointer;
    vector() : super_type() {
    }
    template <typename... E>
    vector(E const &... args) : super_type(args...) {
    }
    vector(std::initializer_list<value_type> const &args) : super_type(args) {
    }
    vector(const_pointer p) : super_type(p) {
    }
    template <typename U, size_t SizeR>
    vector(vector<U, SizeR> const &rhs) : super_type(rhs) {
    }
    this_type
    operator-() {
      this_type res(*this);
      res *= -1;
      return res;
    }
    template <typename U>
    this_type &
    operator*=(U val) {
      detail::scalar_multiplication<Size - 1, this_type>()(*this, val);
      return *this;
    }
    template <typename U>
    this_type &
    operator/=(U val) {
      detail::scalar_division<Size - 1, this_type>()(*this, val);
      return *this;
    }
    template <typename U>
    this_type &
    operator+=(vector<U, Size> const &rhs) {
      detail::vector_addition<Size - 1, this_type>()(*this, rhs);
      return *this;
    }
    template <typename U>
    this_type &
    operator-=(vector<U, Size> const &rhs) {
      detail::vector_substraction<Size - 1, this_type>()(*this, rhs);
      return *this;
    }
    value_type
    magnitude_square() const {
      return detail::dot_product<Size - 1, this_type>()(*this, *this);
    }
    value_type
    magnitude() const {
      return sqrt(magnitude_square());
    }
    bool
    is_zero() const {
      return magnitude_square() == 0;
    }
    bool
    is_unit() const {
      return magnitude_square() == 1;
    }
    this_type &
    zero() {
      detail::set_all_elements<Size - 1, this_type>()(*this, 0);
      return *this;
    }
    this_type &
    normalize() {
      value_type m = magnitude();
      if (m != 0) {
        if (m != 1)
          (*this) /= m;
      } else {
        throw std::runtime_error("Cannot normalize a zero vector");
      }
      return *this;
    }
    this_type
    normalize() const {
      this_type v(*this);
      v.normalize();
      return v;
    }
  };
  template <typename T, typename U, size_t Size>
  bool
  operator==(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    return std::equal(lhs.begin(), lhs.end(), rhs.begin());
  }
  template <typename T, typename U, size_t Size>
  bool
  operator!=(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    return !(lhs == rhs);
  }
  template <typename T, typename U, size_t Size>
  bool
  operator<(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    return std::lexicographical_compare(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
  }
  template <typename T, typename U, size_t Size>
  bool
  operator>(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    return rhs < lhs;
  }
  template <typename T, typename U, size_t Size>
  bool
  operator<=(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    return !(rhs < lhs);
  }
  template <typename T, typename U, size_t Size>
  bool
  operator>=(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    return !(lhs < rhs);
  }
  template <typename T, size_t Size>
  typename vector<T, Size>::value_type
  operator*(vector<T, Size> const &lhs, vector<T, Size> const &rhs) {
    return detail::dot_product<Size - 1, vector<T, Size>>()(lhs, rhs);
  }
  template <typename T, size_t Size, typename U>
  vector<T, Size>
  operator*(vector<T, Size> const &v, U s) {
    vector<T, Size> res(v);
    res *= s;
    return res;
  }
  template <typename T, size_t Size, typename U>
  vector<T, Size>
  operator*(U s, vector<T, Size> const &v) {
    vector<T, Size> res(v);
    res *= s;
    return res;
  }
  template <typename T, size_t Size, typename U>
  vector<T, Size>
  operator/(vector<T, Size> const &v, U s) {
    vector<T, Size> res(v);
    res /= s;
    return res;
  }
  template <typename T, typename U, size_t Size>
  vector<T, Size>
  operator+(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    vector<T, Size> res(lhs);
    res += rhs;
    return res;
  }
  template <typename T, typename U, size_t Size>
  vector<T, Size>
  operator-(vector<T, Size> const &lhs, vector<U, Size> const &rhs) {
    vector<T, Size> res(lhs);
    res -= rhs;
    return res;
  }
  template <typename T, size_t Size>
  typename vector<T, Size>::value_type
  magnitude_square(vector<T, Size> const &v) {
    return v.magnitude_square();
  }
  template <typename T, size_t Size>
  typename vector<T, Size>::value_type
  magnitude(vector<T, Size> const &v) {
    return v.magnitude();
  }
  template <typename T, typename U, size_t Size>
  typename vector<T, Size>::value_type
  distance_square(vector<T, Size> const &a, vector<U, Size> const &b) {
    return magnitude_square(a - b);
  }
  template <typename T, typename U, size_t Size>
  typename vector<T, Size>::value_type
  distance(vector<T, Size> const &a, vector<U, Size> const &b) {
    return magnitude(a - b);
  }
  template <typename T, size_t Size>
  vector<typename vector<T, Size>::value_type, Size>
  normalize(vector<T, Size> const &v) {
    return v.normalize();
  }
  /**
 * Cross product for 3D vectors
 * @param a
 * @param b
 * @return
 */
  template <typename T, typename U>
  vector<typename vector<T, 3>::value_type, 3>
  cross(vector<T, 3> const &a, vector<U, 3> const &b) {
    vector<T, 3> res{
        a.template at<1>() * b.template at<2>() - a.template at<2>() * b.template at<1>(),
        a.template at<2>() * b.template at<0>() - a.template at<0>() * b.template at<2>(),
        a.template at<0>() * b.template at<1>() - a.template at<1>() * b.template at<0>()};
    return res;
  }
  /**
 * Cross product for homogenous 3D vectors
 * @param a
 * @param b
 * @return
 */
  template <typename T, typename U>
  vector<typename vector<T, 4>::value_type, 4>
  cross(vector<T, 4> const &a, vector<U, 4> const &b) {
    vector<T, 4> res{
        a.template at<1>() * b.template at<2>() - a.template at<2>() * b.template at<1>(),
        a.template at<2>() * b.template at<0>() - a.template at<0>() * b.template at<2>(),
        a.template at<0>() * b.template at<1>() - a.template at<1>() * b.template at<0>(),
        typename vector<T, 4>::value_type(1)};
    return res;
  }
  /**
 * Projection of vector v onto vector n
 * @param n Target vector
 * @param v Source vector
 * @return Vector that is parallel to n
 */
  template <typename T, size_t Size>
  vector<typename vector<T, Size>::value_type, Size>
  projection(vector<T, Size> const &n, vector<T, Size> const &v) {
    return n * (v * n / n.magnitude_square());
  }
  /**
 * Vector that is perpendicular to n, such as vǁ + vⱶ = v
 * @param n
 * @param v
 * @return
 */
  template <typename T, size_t Size>
  vector<typename vector<T, Size>::value_type, Size>
  perpendicular(vector<T, Size> const &n, vector<T, Size> const &v) {
    return v - projection(n, v);
  }
  /**
 * Project vector v onto vector n
 * @param n
 * @param v
 * @return Pair of vectors vǁ, vⱶ. vǁ is parallel to n, vǁ + vⱶ = v
 */
  template <typename T, size_t Size>
  std::pair<
      vector<typename vector<T, Size>::value_type, Size>,
      vector<typename vector<T, Size>::value_type, Size>>
  project(vector<T, Size> const &n, vector<T, Size> const &v) {
    auto p = projection(n, v);
    return std::make_pair(p, v - p);
  }
} // namespace mathic
#endif