// ----------------------------------------------------------------------------
// -                        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 "Registration.h"

#include <cstdlib>
#include <ctime>
#include <iostream>

#include <Eigen/Dense>
#include <Core/Utility/Console.h>
#include <Core/Geometry/PointCloud.h>
#include <Core/Geometry/KDTreeFlann.h>
#include <Core/Registration/Feature.h>

namespace open3d {

namespace {

double compute_distance_squared(const Eigen::Vector3d p1, const Eigen::Vector3d p2){
    Eigen::Vector3d diff = p1 - p2;
    return diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2];
}

int compute_closest_vertex(const Eigen::Vector3d best_intpoint, int f, const TriangleMesh target){
    int vertex0 = target.triangles_[f][0];
    int vertex1 = target.triangles_[f][1];
    int vertex2 = target.triangles_[f][2];
    double d0 = compute_distance_squared(best_intpoint, target.vertices_[vertex0]);
    double d1 = compute_distance_squared(best_intpoint, target.vertices_[vertex1]);
    double d2 = compute_distance_squared(best_intpoint, target.vertices_[vertex2]);
    //PrintInfo("d0: %.2f\n", std::sqrt(d0));
    //PrintInfo("d1: %.2f\n", std::sqrt(d1));
    //PrintInfo("d2: %.2f\n", std::sqrt(d2));
    if(d0 <= d1 && d0 <= d2){
        //PrintInfo("picking d0\n");
        return vertex0;
    }
    else{
        if(d1 <= d0 && d1 <= d2){
            //PrintInfo("picking d1\n");
            return vertex1;
        }
        else{
            if(d2 <= d0 && d2 <= d1){
                //PrintInfo("picking d2\n");
                return vertex2;
            }
            else{
                //PrintInfo("Big mistake\n");
                return -1;
            }
        }
    }
}

// Returns true if a ray intersects a triangle.
bool RayIntersectsTriangle(const Eigen::Vector3d rayOrigin,
                        const Eigen::Vector3d rayVector,
                        const Eigen::Vector3d vertex0,
                        const Eigen::Vector3d vertex1,
                        const Eigen::Vector3d vertex2,
                        Eigen::Vector3d& outIntersectionPoint)
{
    const float EPSILON = 0.0000001;
    Eigen::Vector3d edge1, edge2, h, s, q;
    float a,f,u,v;
    edge1 = vertex1 - vertex0;
    edge2 = vertex2 - vertex0;
    h = rayVector.cross(edge2);
    a = edge1.dot(h);
    if (a > -EPSILON && a < EPSILON)
        return false;    // This ray is parallel to this triangle.
    f = 1.0/a;
    s = rayOrigin - vertex0;
    u = f * (s.dot(h));
    if (u < 0.0 || u > 1.0)
        return false;
    q = s.cross(edge1);
    v = f * rayVector.dot(q);
    if (v < 0.0 || u + v > 1.0)
        return false;
    // At this stage we can compute t to find out where the intersection point is on the line.
    float t = f * edge2.dot(q);
    if (t > EPSILON) // ray intersection
    {
        outIntersectionPoint = rayOrigin + rayVector * t;
        return true;
    }
    else // This means that there is a line intersection but not a ray intersection.
        return false;
}

// For each of the neighboring triangles to each point, find which one is intersected by the line.
void find_sc_corr(const TriangleMesh &target, const std::vector<int> &neigh_triangles,\
        const Eigen::Vector3d &source_point, const Eigen::Vector3d &source_normal,\
        double max_correspondence_distance, \
        std::vector<int> &target_corr, std::vector<double> & sc){

    // For each triangle in the set, find if it is intersecting.
    Eigen::Vector3d best_intpoint(0.0, 0.0, 0.0);
    double best_intpoint_dist = 100000000;
    int intpoint_face_ix = -1;
    for (int i = 0; i < (int) neigh_triangles.size(); i++){
        Eigen::Vector3i f = target.triangles_[neigh_triangles[i]];

        Eigen::Vector3d intPoint;
        bool intersect = RayIntersectsTriangle(source_point, source_normal, \
                                                target.vertices_[f[0]], target.vertices_[f[1]],\
                                                target.vertices_[f[2]], intPoint);
        if(!intersect){
            continue;
        }

        double dist2 = compute_distance_squared(intPoint, source_point);
        if(dist2 < best_intpoint_dist){
            best_intpoint_dist = dist2;
            best_intpoint = intPoint;
            intpoint_face_ix = neigh_triangles[i];
        }
    }
    if(intpoint_face_ix >= 0){
        int f = intpoint_face_ix;

        // Pick the vertex closest to the intersection point.
        target_corr[0] = compute_closest_vertex(best_intpoint, f, target);
        double dist2 = compute_distance_squared(target.vertices_[target_corr[0]], best_intpoint);
        Eigen::Vector3d v = best_intpoint;
        //PrintInfo("--------------\n");
        //PrintInfo("Intpoint: %.2f %.2f %.2f\n", v[0], v[1], v[2]);
        v = target.vertices_[target_corr[0]];
        //PrintInfo("vrtpoint: %.2f %.2f %.2f\n", v[0], v[1], v[2]);
        //PrintInfo("vrtpoint to intpoint: %.2f\n", std::sqrt(dist2));
        v = source_point;
        //PrintInfo("Srcpoint: %.2f %.2f %.2f\n", v[0], v[1], v[2]);
        dist2 = compute_distance_squared(target.vertices_[target_corr[0]], source_point);

        // Compute shape complementarity
        sc[0] = target.vertex_normals_[target_corr[0]].dot(-source_normal);
        //PrintInfo("normal mult: %f\n", sc[0]);
        sc[0] = sc[0]*std::exp(-0.5*(dist2));
        //PrintInfo("shape comp: %f \n", sc[0]);
    }
    else{
        target_corr[0] = -1;
        sc[0] = 0.0;
    }
}

    //
// PGC 2019: Get registration results based on shape complementarity
RegistrationResult GetRegistrationResultAndCorrespondencesShapeComplementarity(
        const PointCloud &source,
        const TriangleMesh &target,
        const KDTreeFlann &target_faces_kdtree,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation,
        const Feature &source_feature,
        const Feature &target_feature,
        const int fitness_type
        ) {

    RegistrationResult result(transformation);
    if (max_correspondence_distance <= 0.0) {
        return std::move(result);
    }

    double error2 = 0.0;

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        double error2_private = 0.0;
        CorrespondenceSet correspondence_set_private;
#ifdef _OPENMP
#pragma omp for nowait
#endif
        // Compute the correspondences based on shape complementarity.
        // First find all target points within X of each source point. X=max_correspondence_distance
        // Then get all the triangles on which each target point participates.
        // Find the intersection of the line between source point/normal  and each of the triangles.
        // If the intersection is less than 2, we are good.
        // Find the nearest neighbor to the intersection point.
        for (int i = 0; i < (int)source.points_.size(); i++) {
            std::vector<int> indices(20);
            std::vector<double> dists(20);
            std::vector<int> target_corr(1);
            std::vector<double> target_sc(1);
            const auto &point = source.points_[i];
            const auto &normal= source.normals_[i];
            // Identify all faces within X of the target, up to 20
            if (target_faces_kdtree.SearchHybrid(point, max_correspondence_distance,
                                           20, indices, dists) > 0) {
                // Find the intersection of the line between source point/normal
                //      and each of the triangles.
                find_sc_corr(target, indices, point, normal, max_correspondence_distance, \
                        target_corr, target_sc);

                if(target_sc[0] > 0){
                    //PrintInfo("Adding correspondences: %d %d\n", i, target_corr[0]);
                    error2_private += target_sc[0];
                    correspondence_set_private.push_back(
                        Eigen::Vector2i(i, target_corr[0]));
                }
            }
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            for (int i = 0; i < (int)correspondence_set_private.size(); i++) {
                result.correspondence_set_.push_back(
                        correspondence_set_private[i]);
            }
            error2 += error2_private;
        }
#ifdef _OPENMP
    }
#endif

    if (result.correspondence_set_.empty()) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 1000.0;
    } else {
        size_t corres_number = result.correspondence_set_.size();
        //result.fitness_ = (double)corres_number / (double)source.points_.size();
        if(fitness_type == 1){
            // PGC 2019: fitness: the number of correspondences
            result.fitness_ = (double)corres_number;
        }
        else{
            if(fitness_type == 2){
                // Compute the descriptor distances for all pairs of correspondences.
                double myfitness = 0;
                for (int i = 0; i < (int)result.correspondence_set_.size(); i++) {
                    int s_vix =  result.correspondence_set_[i][0];
                    int t_vix = result.correspondence_set_[i][1];
                    Eigen::VectorXd feat_s = Eigen::VectorXd(source_feature.data_.col(s_vix));
                    Eigen::VectorXd feat_t = Eigen::VectorXd(target_feature.data_.col(t_vix));
                    double desc_dist = 0.0;
//                    std::cout << "Dimension = " << source_feature.Dimension() << std::endl;
//                    std::cout << "Size = " << source_feature.Num() << std::endl;
                    for (int j = 0; j < source_feature.Dimension(); j++){
                        double dist = feat_t[j] - feat_s[j];
                        dist = dist*dist;
                        desc_dist += dist;
                    }

                    myfitness += 1.0/desc_dist;
                }

                result.fitness_ = myfitness;

            }
            else{
                if (fitness_type == 3.0){ // Use shape complementarity (which is in the error).
                    result.fitness_ = error2 / (double)source.points_.size();
                }
                else{
                    result.fitness_ = (double)corres_number / (double)source.points_.size();
                }
            }
        }
        result.inlier_rmse_ = std::sqrt(error2 / (double)corres_number);
    }
    return std::move(result);
}

// PGC 2019: include the fitness type and the features for a custom fitness function.
RegistrationResult GetRegistrationResultAndCorrespondencesCustom(
        const PointCloud &source,
        const PointCloud &target,
        const KDTreeFlann &target_kdtree,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation,
        const Feature &source_feature,
        const Feature &target_feature,
        const int fitness_type
        ) {
    RegistrationResult result(transformation);
    if (max_correspondence_distance <= 0.0) {
        return std::move(result);
    }

    double error2 = 0.0;

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        double error2_private = 0.0;
        CorrespondenceSet correspondence_set_private;
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int i = 0; i < (int)source.points_.size(); i++) {
            std::vector<int> indices(1);
            std::vector<double> dists(1);
            const auto &point = source.points_[i];
            if (target_kdtree.SearchHybrid(point, max_correspondence_distance,
                                           1, indices, dists) > 0) {
                error2_private += dists[0];
                correspondence_set_private.push_back(
                        Eigen::Vector2i(i, indices[0]));
            }
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            for (int i = 0; i < (int)correspondence_set_private.size(); i++) {
                result.correspondence_set_.push_back(
                        correspondence_set_private[i]);
            }
            error2 += error2_private;
        }
#ifdef _OPENMP
    }
#endif

    if (result.correspondence_set_.empty()) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        size_t corres_number = result.correspondence_set_.size();
        //result.fitness_ = (double)corres_number / (double)source.points_.size();
        if(fitness_type == 1){
            // PGC 2019: fitness: the number of correspondences
            result.fitness_ = (double)corres_number;
        }
        else{
            if(fitness_type == 2){
                // Compute the descriptor distances for all pairs of correspondences.
                double myfitness = 0;
                for (int i = 0; i < (int)result.correspondence_set_.size(); i++) {
                    int s_vix =  result.correspondence_set_[i][0];
                    int t_vix = result.correspondence_set_[i][1];
                    Eigen::VectorXd feat_s = Eigen::VectorXd(source_feature.data_.col(s_vix));
                    Eigen::VectorXd feat_t = Eigen::VectorXd(target_feature.data_.col(t_vix));
                    double desc_dist = 0.0;
//                    std::cout << "Dimension = " << source_feature.Dimension() << std::endl;
//                    std::cout << "Size = " << source_feature.Num() << std::endl;
                    for (int j = 0; j < source_feature.Dimension(); j++){
                        double dist = feat_t[j] - feat_s[j];
                        dist = dist*dist;
                        desc_dist += dist;
                    }

                    myfitness += 1.0/desc_dist;
                }

                result.fitness_ = myfitness;

            }
            else{
                result.fitness_ = (double)corres_number / (double)source.points_.size();
            }
        }
        result.inlier_rmse_ = std::sqrt(error2 / (double)corres_number);
    }
    return std::move(result);
}

RegistrationResult GetRegistrationResultAndCorrespondences(
        const PointCloud &source,
        const PointCloud &target,
        const KDTreeFlann &target_kdtree,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    RegistrationResult result(transformation);
    if (max_correspondence_distance <= 0.0) {
        return std::move(result);
    }

    double error2 = 0.0;

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        double error2_private = 0.0;
        CorrespondenceSet correspondence_set_private;
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int i = 0; i < (int)source.points_.size(); i++) {
            std::vector<int> indices(1);
            std::vector<double> dists(1);
            const auto &point = source.points_[i];
            if (target_kdtree.SearchHybrid(point, max_correspondence_distance,
                                           1, indices, dists) > 0) {
                error2_private += dists[0];
                correspondence_set_private.push_back(
                        Eigen::Vector2i(i, indices[0]));
            }
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            for (int i = 0; i < (int)correspondence_set_private.size(); i++) {
                result.correspondence_set_.push_back(
                        correspondence_set_private[i]);
            }
            error2 += error2_private;
        }
#ifdef _OPENMP
    }
#endif

    if (result.correspondence_set_.empty()) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        size_t corres_number = result.correspondence_set_.size();
        //result.fitness_ = (double)corres_number / (double)source.points_.size();
        // PGC 2019: fitness is the number of correspondences, not the fraction.
        result.fitness_ = (double)corres_number;
        result.inlier_rmse_ = std::sqrt(error2 / (double)corres_number);
    }
    return std::move(result);
}

RegistrationResult EvaluateRANSACBasedOnCorrespondence(
        const PointCloud &source,
        const PointCloud &target,
        const CorrespondenceSet &corres,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    RegistrationResult result(transformation);
    double error2 = 0.0;
    int good = 0;
    double max_dis2 = max_correspondence_distance * max_correspondence_distance;
    for (const auto &c : corres) {
        double dis2 =
                (source.points_[c[0]] - target.points_[c[1]]).squaredNorm();
        if (dis2 < max_dis2) {
            good++;
            error2 += dis2;
        }
    }
    if (good == 0) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        result.fitness_ = (double)good / (double)corres.size();
        result.inlier_rmse_ = std::sqrt(error2 / (double)good);
    }
    return result;
}

}  // unnamed namespace

RegistrationResult EvaluateRegistration(
        const PointCloud &source,
        const PointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d
                &transformation /* = Eigen::Matrix4d::Identity()*/) {
    KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
    PointCloud pcd = source;
    if (transformation.isIdentity() == false) {
        pcd.Transform(transformation);
    }
    return GetRegistrationResultAndCorrespondences(
            pcd, target, kdtree, max_correspondence_distance, transformation);
}

RegistrationResult RegistrationICP(
        const PointCloud &source,
        const PointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        const ICPConvergenceCriteria
                &criteria /* = ICPConvergenceCriteria()*/) {
    if (max_correspondence_distance <= 0.0) {
        PrintError("Error: Invalid max_correspondence_distance.\n");
        return RegistrationResult(init);
    }
    if (estimation.GetTransformationEstimationType() ==
                TransformationEstimationType::PointToPlane &&
        (!source.HasNormals() || !target.HasNormals())) {
        PrintError(
                "Error: TransformationEstimationPointToPlane requires "
                "pre-computed normal vectors.\n");
        return RegistrationResult(init);
    }

    Eigen::Matrix4d transformation = init;
    KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
    PointCloud pcd = source;
    if (init.isIdentity() == false) {
        pcd.Transform(init);
    }
    RegistrationResult result;
    result = GetRegistrationResultAndCorrespondences(
            pcd, target, kdtree, max_correspondence_distance, transformation);
    for (int i = 0; i < criteria.max_iteration_; i++) {
        PrintDebug("ICP Iteration #%d: Fitness %.4f, RMSE %.4f\n", i,
                   result.fitness_, result.inlier_rmse_);
        Eigen::Matrix4d update = estimation.ComputeTransformation(
                pcd, target, result.correspondence_set_);
        transformation = update * transformation;
        pcd.Transform(update);
        RegistrationResult backup = result;
        result = GetRegistrationResultAndCorrespondences(
                pcd, target, kdtree, max_correspondence_distance,
                transformation);
        if (std::abs(backup.fitness_ - result.fitness_) <
                    criteria.relative_fitness_ &&
            std::abs(backup.inlier_rmse_ - result.inlier_rmse_) <
                    criteria.relative_rmse_) {
            break;
        }
    }
    return result;
}

RegistrationResult RegistrationRANSACBasedOnCorrespondence(
        const PointCloud &source,
        const PointCloud &target,
        const CorrespondenceSet &corres,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 6*/,
        const RANSACConvergenceCriteria &criteria
        /* = RANSACConvergenceCriteria()*/) {
    if (ransac_n < 3 || (int)corres.size() < ransac_n ||
        max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }
    std::srand((unsigned int)std::time(0));
    Eigen::Matrix4d transformation;
    CorrespondenceSet ransac_corres(ransac_n);
    RegistrationResult result;
    for (int itr = 0;
         itr < criteria.max_iteration_ && itr < criteria.max_validation_;
         itr++) {
        for (int j = 0; j < ransac_n; j++) {
            ransac_corres[j] = corres[std::rand() % (int)corres.size()];
        }
        transformation =
                estimation.ComputeTransformation(source, target, ransac_corres);
        PointCloud pcd = source;
        pcd.Transform(transformation);
        auto this_result = EvaluateRANSACBasedOnCorrespondence(
                pcd, target, corres, max_correspondence_distance,
                transformation);
        if (this_result.fitness_ > result.fitness_ ||
            (this_result.fitness_ == result.fitness_ &&
             this_result.inlier_rmse_ < result.inlier_rmse_)) {
            result = this_result;
        }
    }
    PrintDebug("RANSAC: Fitness %.4f, RMSE %.4f\n", result.fitness_,
               result.inlier_rmse_);
    return result;
}

RegistrationResult RegistrationRANSACBasedOnFeatureMatching(
        const PointCloud &source,
        const PointCloud &target,
        const Feature &source_feature,
        const Feature &target_feature,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 4*/,
        const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
                &checkers /* = {}*/,
        const RANSACConvergenceCriteria &criteria,
        const double ransac_random_seed, /* default: 0*/
        const int fitness_type /* 0: standard ransac fitness function. 1: use the number of inliers (instead of the ration); 2: use 1/d^2 for inliers, where d is descriptor distance.*/

        /* = RANSACConvergenceCriteria()*/) {
    if (ransac_n < 3 || max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }

    RegistrationResult result;
    int total_validation = 0;
    bool finished_validation = false;
    int num_similar_features = 1;
    std::vector<std::vector<int>> similar_features(source.points_.size());

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        CorrespondenceSet ransac_corres(ransac_n);
        KDTreeFlann kdtree(target);
        KDTreeFlann kdtree_feature(target_feature);
        RegistrationResult result_private;
        unsigned int seed_number;
#ifdef _OPENMP
        // each thread has different seed_number
        //seed_number = (unsigned int)std::time(0) * (omp_get_thread_num() + 1);
        seed_number = (unsigned int)ransac_random_seed * (omp_get_thread_num() + 1);
#else
        //seed_number = (unsigned int)std::time(0);
        seed_number = (unsigned int)ransac_random_seed;//std::time(0);
#endif
        std::srand(seed_number);

#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int itr = 0; itr < criteria.max_iteration_; itr++) {
            if (!finished_validation) {
                std::vector<double> dists(num_similar_features);
                Eigen::Matrix4d transformation;
                for (int j = 0; j < ransac_n; j++) {
                    int source_sample_id =
                            std::rand() % (int)source.points_.size();
                    if (similar_features[source_sample_id].empty()) {
                        std::vector<int> indices(num_similar_features);
                        kdtree_feature.SearchKNN(
                                Eigen::VectorXd(source_feature.data_.col(
                                        source_sample_id)),
                                num_similar_features, indices, dists);
#ifdef _OPENMP
#pragma omp critical
#endif
                        { similar_features[source_sample_id] = indices; }
                    }
                    ransac_corres[j](0) = source_sample_id;
                    if (num_similar_features == 1)
                        ransac_corres[j](1) =
                                similar_features[source_sample_id][0];
                    else
                        ransac_corres[j](1) =
                                similar_features[source_sample_id]
                                                [std::rand() %
                                                 num_similar_features];
                }
                bool check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == false &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                transformation = estimation.ComputeTransformation(
                        source, target, ransac_corres);
                check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == true &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                PointCloud pcd = source;
                pcd.Transform(transformation);
                auto this_result = GetRegistrationResultAndCorrespondencesCustom(
                        pcd, target, kdtree, max_correspondence_distance,
                        transformation, source_feature, target_feature, fitness_type);
                if (this_result.fitness_ > result_private.fitness_ ||
                    (this_result.fitness_ == result_private.fitness_ &&
                     this_result.inlier_rmse_ < result_private.inlier_rmse_)) {
                    result_private = this_result;
                }
#ifdef _OPENMP
#pragma omp critical
#endif
                {
                    total_validation = total_validation + 1;
                    if (total_validation >= criteria.max_validation_)
                        finished_validation = true;
                }
            }  // end of if statement
        }      // end of for-loop
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            if (result_private.fitness_ > result.fitness_ ||
                (result_private.fitness_ == result.fitness_ &&
                 result_private.inlier_rmse_ < result.inlier_rmse_)) {
                result = result_private;
            }
        }
#ifdef _OPENMP
    }
#endif
    PrintDebug("total_validation : %d\n", total_validation);
    PrintDebug("RANSAC: Fitness %.4f, RMSE %.4f\n", result.fitness_,
               result.inlier_rmse_);
    return result;
}

RegistrationResult RegistrationRANSACBasedOnShapeComplementarity(
        const PointCloud &source,
        const PointCloud &target,
        const TriangleMesh &target_mesh,
        const PointCloud &target_face_centroids_pcd,
        const Feature &source_feature,
        const Feature &target_feature,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 4*/,
        const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
                &checkers /* = {}*/,
        const RANSACConvergenceCriteria &criteria,
        const double ransac_random_seed, /* default: 0*/
        const int fitness_type /* 0: standard ransac fitness function. 1: use the number of inliers (instead of the ration); 2: use 1/d^2 for inliers, where d is descriptor distance.*/

        /* = RANSACConvergenceCriteria()*/) {
    if (ransac_n < 3 || max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }

    RegistrationResult result;
    int total_validation = 0;
    bool finished_validation = false;
    int num_similar_features = 1;
    std::vector<std::vector<int>> similar_features(source.points_.size());

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        CorrespondenceSet ransac_corres(ransac_n);
        // Do a KDTree for the triangles.
        PointCloud target_triangle_centroids();
        KDTreeFlann kdtree(target_face_centroids_pcd);
        KDTreeFlann kdtree_feature(target_feature);
        RegistrationResult result_private;
        unsigned int seed_number;
#ifdef _OPENMP
        // each thread has different seed_number
        //seed_number = (unsigned int)std::time(0) * (omp_get_thread_num() + 1);
        seed_number = (unsigned int)ransac_random_seed * (omp_get_thread_num() + 1);
#else
        //seed_number = (unsigned int)std::time(0);
        seed_number = (unsigned int)ransac_random_seed;//std::time(0);
#endif
        std::srand(seed_number);

#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int itr = 0; itr < criteria.max_iteration_; itr++) {
            if (!finished_validation) {
                std::vector<double> dists(num_similar_features);
                Eigen::Matrix4d transformation;
                for (int j = 0; j < ransac_n; j++) {
                    int source_sample_id =
                            std::rand() % (int)source.points_.size();
                    if (similar_features[source_sample_id].empty()) {
                        std::vector<int> indices(num_similar_features);
                        kdtree_feature.SearchKNN(
                                Eigen::VectorXd(source_feature.data_.col(
                                        source_sample_id)),
                                num_similar_features, indices, dists);
#ifdef _OPENMP
#pragma omp critical
#endif
                        { similar_features[source_sample_id] = indices; }
                    }
                    ransac_corres[j](0) = source_sample_id;
                    if (num_similar_features == 1)
                        ransac_corres[j](1) =
                                similar_features[source_sample_id][0];
                    else
                        ransac_corres[j](1) =
                                similar_features[source_sample_id]
                                                [std::rand() %
                                                 num_similar_features];
                }
                bool check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == false &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                transformation = estimation.ComputeTransformation(
                        source, target, ransac_corres);
                check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == true &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                PointCloud pcd = source;
                pcd.Transform(transformation);
                //PrintInfo("###################\n");
                //PrintInfo("###################\n");
                //PrintInfo("Evaluating transformation\n");
                auto this_result = GetRegistrationResultAndCorrespondencesShapeComplementarity(
                        pcd, target_mesh, kdtree, max_correspondence_distance,
                        transformation, source_feature, target_feature, fitness_type);
                if (this_result.fitness_ > result_private.fitness_ ||
                    (this_result.fitness_ == result_private.fitness_ &&
                     this_result.inlier_rmse_ < result_private.inlier_rmse_)) {
                    result_private = this_result;
                }
#ifdef _OPENMP
#pragma omp critical
#endif
                {
                    total_validation = total_validation + 1;
                    if (total_validation >= criteria.max_validation_)
                        finished_validation = true;
                }
            }  // end of if statement
        }      // end of for-loop
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            if (result_private.fitness_ > result.fitness_ ||
                (result_private.fitness_ == result.fitness_ &&
                 result_private.inlier_rmse_ < result.inlier_rmse_)) {
                result = result_private;
            }
        }
#ifdef _OPENMP
    }
#endif
    PrintDebug("total_validation : %d\n", total_validation);
    PrintDebug("RANSAC: Fitness %.4f, RMSE %.4f\n", result.fitness_,
               result.inlier_rmse_);
    return result;
}

Eigen::Matrix6d GetInformationMatrixFromPointClouds(
        const PointCloud &source,
        const PointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    PointCloud pcd = source;
    if (transformation.isIdentity() == false) {
        pcd.Transform(transformation);
    }
    RegistrationResult result;
    KDTreeFlann target_kdtree(target);
    result = GetRegistrationResultAndCorrespondences(
            pcd, target, target_kdtree, max_correspondence_distance,
            transformation);

    // write q^*
    // see http://redwood-data.org/indoor/registration.html
    // note: I comes first in this implementation
    Eigen::Matrix6d GTG = Eigen::Matrix6d::Identity();
#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        Eigen::Matrix6d GTG_private = Eigen::Matrix6d::Identity();
        Eigen::Vector6d G_r_private = Eigen::Vector6d::Zero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (auto c = 0; c < result.correspondence_set_.size(); c++) {
            int t = result.correspondence_set_[c](1);
            double x = target.points_[t](0);
            double y = target.points_[t](1);
            double z = target.points_[t](2);
            G_r_private.setZero();
            G_r_private(1) = z;
            G_r_private(2) = -y;
            G_r_private(3) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = -z;
            G_r_private(2) = x;
            G_r_private(4) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = y;
            G_r_private(1) = -x;
            G_r_private(5) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        { GTG += GTG_private; }
#ifdef _OPENMP
    }
#endif
    return std::move(GTG);
}

}  // namespace open3d


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_BUNDLE_BUNDLE10_HPP__
#define __OPTIMISATION2_BUNDLE_BUNDLE10_HPP__

#include <libv/lma/ttt/traits/functor_trait.hpp>
#include <libv/lma/ttt/traits/rm_all.hpp>
#include <libv/lma/ttt/traits/unroll1.hpp>
#include "../function/function.hpp"
#include "bundle_utils.hpp"
#include <boost/fusion/include/fold.hpp>
#include <boost/fusion/include/value_at_key.hpp>
#include <boost/fusion/include/make_vector.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/pop_front.hpp>
#include <set>

namespace lma
{
  template<class T> struct SizeInf3 : mpl::if_c< (mpl::size<T>::value < 3), mpl::true_, mpl::false_> {};

  template<class L, class, class Result = mpl::vector<>> struct MakeSP :
    MakeSP<
            typename mpl::pop_front<L>::type,
            typename SizeInf3<L>::type,
            typename mpl::push_back<
                                    Result,
                                    typename br::as_vector<
                                                          typename mpl::transform<
                                                                                typename mpl::pop_front<L>::type,
                                                                                ttt::Indice<bf::pair<mpl::_1,typename mpl::front<L>::type>>
                                                                                >::type
                                                        >::type
                                    >::type
          > {};

  template<class L, class R> struct MakeSP< L, mpl::true_ , R >  { typedef R type;};

  template<class T> struct MakeSP2
  {
    template<class L> struct TTT : mpl::transform<L, br::as_vector<mpl::_1>> {};

    typedef typename MakeSP<T,SizeInf3<T>>::type list;
    typedef typename mpl::transform<
                                      list,
                                      br::as_vector<
                                                     mpl::_1
                                                   >
                                   >::type list2;
    typedef typename br::as_vector<list2>::type type;
  };

  template<class ListFunction, class Fonctor> struct MAPZZ2
  {
    typedef typename mpl::transform<ListFunction,CreateListArg<mpl::_1>>::type l0;
    typedef typename mpl::transform<l0, Fonctor>::type l1;

    template<class T> struct ToIndicesVector : br::as_vector<typename mpl::transform<T,ttt::Indice<mpl::_1>>::type> {};
    template<class T> struct ToParamsVector : br::as_vector<typename mpl::transform<T,mpl::_1>::type> {};

    typedef typename mpl::transform<
                                      l1,
                                      ListFunction,
                                      bf::pair<
                                                mpl::_2,
                                                std::vector<
                                                            std::pair<
                                                                        ToIndicesVector<mpl::_1>,
                                                                        MakeSP2<mpl::_1>
                                                                      >
                                                           >
                                              >
                                   >::type MapSparseIndices;

    typedef typename mpl::transform<
                                      l1,
                                      ListFunction,
                                      bf::pair<
                                                mpl::_2,
                                                std::vector<
                                                              ToParamsVector<mpl::_1>
                                                           >
                                              >
                                   >::type MapSparseParameters;

    typedef typename br::as_map<MapSparseIndices>::type Container;

    typedef typename br::as_map<MapSparseParameters>::type ContainerParameters;

    template<class F> struct Second
    {
      typedef typename br::value_at_key<Container,F>::type Vector;
      typedef typename Vector::value_type::second_type type;
    };

    template<class F> struct First
    {
      typedef typename br::value_at_key<Container,F>::type Vector;
      typedef typename Vector::value_type::first_type type;
    };

    Container container;

    ContainerParameters parameters;
    /*
     * container =
     * br::map
     *        <
     *         Key = Function,
     *         Value = std::vector<
     *                             std::pair<
     *                                        first = br::vector< Indice<Function_Parametre1>,...,Indice<Function_ParametreN> >
     *                                        second = br::vector<
     *                                                             br::vector< ttt::Indice<Sparse<Function_Parametre1,Function_Parametre2>>, ttt::Indice<Sparse<Function_Parametre1,Function_Parametre3>>, ..., ttt::Indice<Sparse<Function_Parametre1,Function_ParametreN>> >
     *                                                             br::vector< ttt::Indice<Sparse<Function_Parametre2,Function_Parametre3>>, ttt::Indice<Sparse<Function_Parametre2,Function_Parametre4>>, ..., ttt::Indice<Sparse<Function_Parametre1,Function_ParametreN>> >
     *                                                             ...
     *                                                           >
     *                                      >
     *                            >
     *        >
     */

    template<class Obs> struct ToVector
    {
      typedef typename br::at_key<Container,Obs>::type type;
      typedef typename std::decay<type>::type naked_type;
      typedef typename boost::add_const<naked_type>::type const_type;
      typedef typename boost::add_reference<const_type>::type const_ref_type;
    };

    template<class Obs> struct ToTuple
    {
      typedef typename ToVector<Obs>::type vector_ref;
      typedef typename std::decay<vector_ref>::type vector;
      typedef typename vector::value_type value_type;

      typedef typename boost::add_const<value_type>::type const_type;
      typedef typename boost::add_reference<const_type>::type const_ref_type;

      typedef typename boost::add_reference<value_type>::type type_ref;

    };

    template<class Obs> struct ToTupleElement : ToTuple<Obs>
    {
      typedef typename ToTuple<Obs>::value_type value_type;
      typedef typename value_type::first_type first_type;
      typedef typename value_type::second_type second_type;

      typedef typename boost::add_const<first_type>::type const_first_type;
      typedef typename boost::add_reference<const_first_type>::type first_const_ref_type;

      typedef typename boost::add_reference<first_type>::type type_first_ref;
      typedef typename boost::add_reference<second_type>::type type_second_ref;

    };

    template<class T> struct ForceAddConst
    {
      typedef typename std::decay<T>::type naked_type;
      typedef typename boost::add_const<naked_type>::type const_type;
      typedef typename boost::add_reference<const_type>::type const_ref_type;
    };

    template<class Key>
    typename ToVector<Key>::type at()
    {
      BOOST_MPL_ASSERT((boost::is_same<decltype(bf::at_key<Key>(container)),typename ToVector<Key>::type>));
      return bf::at_key<Key>(container);
    }

    template<class Key>
    typename ToTuple<Key>::type_ref pair(ttt::Indice<Key> indice)
    {
      BOOST_MPL_ASSERT((boost::is_same<decltype(this->at<Key>().at(indice())),typename ToTuple<Key>::type_ref>));
      return this->at<Key>().at(indice());
    }

    template<class Key>
    typename ToTupleElement<Key>::type_first_ref indices(ttt::Indice<Key> indice)
    {
      BOOST_MPL_ASSERT((boost::is_same<decltype(pair(indice).first),typename ToTupleElement<Key>::first_type>));
      return pair(indice).first;
    }

    template<size_t I, class Key>
    typename br::at_c<typename ToTupleElement<Key>::first_type,I>::type indice(ttt::Indice<Key> indice)
    {
      BOOST_MPL_ASSERT((boost::is_same<decltype(bf::at_c<I>(pair(indice).first)),typename br::at_c<typename ToTupleElement<Key>::first_type,I>::type>));
      return bf::at_c<I>(pair(indice).first);
    }

    // const
    template<class Key>
    typename ToVector<Key>::const_ref_type
     xat() const
    { return bf::at_key<Key>(container); }

    template<class Key>
    typename ToTuple<Key>::const_ref_type xpair(const ttt::Indice<Key>& indice)  const
    {
      BOOST_MPL_ASSERT((boost::is_same<typename ToTuple<Key>::const_ref_type,decltype(this->xat<Key>().at(indice()))>));
      return this->xat<Key>().at(indice());
    }

    template<class Key>
    typename ToTupleElement<Key>::first_const_ref_type
    indices(const ttt::Indice<Key>& indice)  const
    { return xpair(indice).first; }

    template<size_t I, class Key>
    typename ForceAddConst<typename br::at_c<typename ToTupleElement<Key>::second_type,I>::type>::const_ref_type
    sparse_indices(const ttt::Indice<Key>& indice)  const
    {
      BOOST_MPL_ASSERT((boost::is_same<typename ForceAddConst<typename br::at_c<typename ToTupleElement<Key>::second_type,I>::type>::const_ref_type,decltype(bf::at_c<I>(xpair(indice).second))>));
      return bf::at_c<I>(xpair(indice).second);
    }

    static void disp()
    {
      std::cout << " =======================\n";
      std::cout << "L0 : " << ttt::name<l0>() << std::endl;
      std::cout << "L1 : " << ttt::name<l1>() << std::endl;
      std::cout << "MapSparseIndices : " << ttt::name<MapSparseIndices>() << std::endl;
      std::cout << "MapSparseParameters : " << ttt::name<MapSparseParameters>() << std::endl;

      std::cout << " =======================\n";
    }
  };

  template<class Set, class Tuple> struct AddSet2
  {
    Set& set;
    Tuple& tuple;
    AddSet2(Set& set_,Tuple& tuple_):set(set_),tuple(tuple_){}

    template<size_t I> void operator()(Int<I> const &)
    {
      // ajoute un parametre à la map correspondante
      //map[KeyParam].add(&param)
      bf::at_key<typename br::value_at_c<Tuple,I>::type>(set)[bf::at_c<I>(tuple)];
    }
  };

  template<class Set, class Parameters> struct AddSet
  {
    Set& set;
    Parameters& parameters;
    AddSet(Set& set_,Parameters& parameters_):set(set_),parameters(parameters_){}

    template<class Obs> void operator()(ttt::wrap<Obs> const &)
    {
      auto& vector = bf::at_key<Obs>(parameters);
      typedef typename ttt::rm_all<decltype(vector)>::type V;
      typedef typename V::value_type Tuple;

      for(auto& tuple : vector)
        // on ajoute chaque paramètre du tuple à la map
        // on utilise unroll plutôt que for_each<Types..> car un tuple peut contenir plusieurs paramètres de mêmes types.
        ttt::unroll<0,mpl::size<typename ttt::rm_all<decltype(tuple)>::type>::value>(AddSet2<Set,Tuple>(set,tuple));
    }
  };

  template<class Set, class Bundle> struct AddParam
  {
    Set& set;
    Bundle& bundle;

    AddParam(Set& set_, Bundle& bundle_):set(set_),bundle(bundle_){}

    template<class Param> void operator()(ttt::wrap<Param> const &)
    {
      // ajout des paramètres au bundle et mise à jour de l'indice
      for(auto& elt : bf::at_key<Param>(set))
        elt.second = bundle.opt_container.add(elt.first);
    }
  };

  template<class Set> struct AdressToIndice
  {
    const Set& set;
    AdressToIndice(const Set& set_):set(set_){}
    template<class A, class B> inline void operator()(A& a, const B& b) const { a = bf::at_key<B>(set).at(b); }
  };

  template<class Set, class Bundle> struct UpdateVab
  {
    const Set& set;
    Bundle& bundle;
    UpdateVab(const Set& set_, Bundle& bundle_):set(set_),bundle(bundle_){}

    template<class Obs> void operator()(ttt::wrap<Obs> const &)
    {
      typename Bundle::MapZZ2::template ToTupleElement<Obs>::first_type tuple;
      auto& vector = bf::at_key<Obs>(bundle.spi2.parameters);
      for(size_t i = 0 ; i < vector.size() ; ++i)
      {
        // tuple d'adresses -> tuple d'indices
        copy_f(tuple,vector[i],AdressToIndice<Set>(set));
        detail::AddTuple::add<Obs>(bundle,tuple);
      }
    }
  };

  template<class Set, class Bundle> struct UpdateVab2
  {
    const Set& set;
    Bundle& bundle;
    UpdateVab2(const Set& set_, Bundle& bundle_):set(set_),bundle(bundle_){}

    template<class Obs> void operator()(ttt::wrap<Obs> const &)
    {
      typename Bundle::MapZZ2::template ToTupleElement<Obs>::first_type tuple;
      auto& vector = bf::at_key<Obs>(bundle.spi2.parameters);
      for(size_t i = 0 ; i < vector.size() ; ++i)
      {
        copy_f(tuple,vector[i],AdressToIndice<Set>(set));
        bundle.spi2.template at<Obs>().emplace_back(tuple,typename Bundle::MapZZ2::template Second<Obs>::type());
        detail::AddTuple::add2<Obs>(bundle,tuple);
      }
    }
  };

  template<class Map> struct ForEachSetMax2
  {
    const Map& map;
    ForEachSetMax2(const Map& map_):map(map_){}
    template<class Pair> void operator()(Pair& pair) const
    {
      auto& sic = pair.second;
      typedef typename ttt::rm_all<decltype(sic)>::type Sic;
      typedef typename Sic::first_type A;
      typedef typename Sic::second_type B;
      sic.set_max(bf::at_key<A>(map).size(),bf::at_key<B>(map).size());
    }
  };

  template<class Map> struct ForEachSetMax
  {
    const Map& map;
    ForEachSetMax(const Map& map_):map(map_){}

    template<class Pair> void operator()(Pair& pair) const
    {
      bf::for_each(pair.second,ForEachSetMax2<Map>(map));
    }
  };

  template<class Map> struct ForEachUpdate2
  {
    const Map& map;
    ForEachUpdate2(const Map& map_):map(map_){}
    template<class Pair> void operator()(Pair& pair) const
    {
      auto& sic = pair.second;
      sic.update();
    }
  };

  template<class Map> struct ForEachUpdate
  {
    const Map& map;
    ForEachUpdate(const Map& map_):map(map_){}

    template<class Pair> void operator()(Pair& pair) const
    {
      bf::for_each(pair.second,ForEachUpdate2<Map>(map));
    }
  };

  template<class FL, class ParamFonctor_ = DoNothing<mpl::_1> > struct Bundle
  {
    typedef ParamFonctor_ ParamFonctor;
    typedef Bundle<FL,ParamFonctor> self;

    typedef typename FindOrder<FL,ParamFonctor>::type::list_function ListFunction;
    typedef typename FindOrder<FL,ParamFonctor>::type::list_parameter ListeParam;

    static const std::size_t NbClass = mpl::size<ListeParam>::value;

    typedef MultiContainer<ListeParam,detail::FonctorPairTic<mpl::_1>> MCA;
    typedef MultiContainer<ListFunction,detail::FonctorPairTic<mpl::_1>> MCFonction;
    typedef typename MakeSparseIndiceContainer1<ListeParam>::type VABMap;

    typedef typename mpl::transform<ListFunction,CreateListArg<mpl::_1>>::type ListListArg_;
    typedef typename mpl::transform<ListListArg_,ParamFonctor>::type ListListArg;
    typedef typename mpl::transform<ListListArg,TransformToPair<mpl::_1>>::type VectorVectorParametre;

    typedef typename MapMapMaker<ListFunction,VectorVectorParametre,FunctorPairKeyMap>::type MapMapParametre;// use in get_map2

    typedef MAPZZ2<ListFunction,ParamFonctor> MapZZ2;
    MapZZ2 spi2;

    public:
    // Container des paramètres (MapOfTIC : bf::map< bf::pair< A,TIC< A > >, .., bp::pair< X, TIC< Z > > >)
    MCA opt_container;

    // Container des foncteurs
    MCFonction fonction_container;

    // Conteneur d'indices
    VABMap vab_map;

    Bundle(){
//       MapZZ2::disp();
//       std::cout << " VAB : " << ttt::name<VABMap>() << std::endl;
//       abort();
    }

    template<class MContainer, class Key> struct GetValueType
    {
      typedef typename MContainer::template AtKey<Key>::type ContainerRef;
      typedef typename std::decay<ContainerRef>::type Container;
      typedef typename Container::Type type_;
      typedef typename boost::add_reference<typename boost::add_const<type_>::type>::type type;
    };

    template<class MContainer, class Key> struct GetValueType2
    {
      typedef typename br::at_key<MContainer,Key>::type ContainerRef;
      typedef typename std::decay<ContainerRef>::type Vector;
      typedef typename Vector::value_type type_;
      typedef typename boost::add_reference<typename boost::add_const<type_>::type>::type type;
    };

    //! at_key< Key>() -> TIC< Key >
    template<class Key> typename MCA::template AtKey<Key>::type& at_opt()       { return opt_container.template at_key<Key>(); }
    template<class Key> typename MCA::template AtKey<Key>::const_ref_type& at_opt() const { return opt_container.template at_key<Key>(); }
    template<class Key> typename MCFonction::template AtKey<Key>::type& at_obs() { return fonction_container.template at_key<Key>(); }
    template<class Key> typename MCFonction::template AtKey<Key>::const_ref_type at_obs() const {return fonction_container.template at_key<Key>(); }
    template<class Key> typename GetValueType<MCA,Key>::type opt(const ttt::Indice<Key>& indice) const { return at_opt<Key>()(indice); }
    template<class Key> typename GetValueType<MCFonction,Key>::type obs(const ttt::Indice<Key>& indice) const { return at_obs<Key>()(indice); }

    //! operator< Key1, Key2 >() -> SIC< Key1, Key2 >
    template<class Key1, class Key2> typename BinaryAt<VABMap,Key1,Key2>::type_ref indices()
    {
      BOOST_MPL_ASSERT((boost::is_reference<typename BinaryAt<VABMap,Key1,Key2>::type_ref>));
      return bf::at_key<Key1>(bf::at_key<Key2>(vab_map));
    }

    template<class Key1, class Key2> typename BinaryAt<VABMap,Key1,Key2>::const_ref_type indices() const
    {
      return bf::at_key<Key1>(bf::at_key<Key2>(vab_map));
    }

    //! Map of parameters from an observation indice
    //template<class Obs> typename br::value_at_key<MapMapParametre,Obs>::type map(const ttt::Indice<Obs>& indice) const

    template<class Obs> typename GetValueType2<typename MapZZ2::ContainerParameters,Obs>::type map(const ttt::Indice<Obs>& indice) const
    {
      assert(size_t(indice()) < bf::at_key<Obs>(spi2.parameters).size());
      return bf::at_key<Obs>(spi2.parameters)[indice()];
    }

    template<class Obs, class ... Params> void add(const Obs& f, const Params& ... params)
    {
      // stocke le tuple d'adresse dans spi2.container
      bf::at_key<Obs>(spi2.parameters).emplace_back(params...);
      fonction_container.add(f);
    }

    bool is_updated = false;
    void update()
    {
      if (is_updated) return ;
      is_updated = true;
      typedef typename br::as_map<
                                  typename mpl::transform<
                                                          ListeParam,
                                                          bf::pair<
                                                                    mpl::_1,
                                                                    std::map<mpl::_1,ttt::Indice<mpl::_1>>
                                                                  >
                                                        >::type
                                >::type MapAdressParameters;
      MapAdressParameters map;
      // liste et ordonne les parameters dans map
      mpl::for_each<ListFunction,ttt::wrap<mpl::_1>>(AddSet<MapAdressParameters,typename MapZZ2::ContainerParameters>(map,spi2.parameters));
      // ajoute les parametres dans le bundle et met à jour map avec les indices correspondants
      mpl::for_each<ListeParam,  ttt::wrap<mpl::_1>>(AddParam<MapAdressParameters,self>(map,*this));
      // update indices relations and sparse_indices

      bf::for_each(vab_map,ForEachSetMax<MapAdressParameters>(map));
      /*
      bf::for_each(vab_map,[&map](auto& m)
      {
        bf::for_each(m.second,[&map](auto& pair)
        {
          auto& sic = pair.second;
          typedef typename ttt::rm_all<decltype(sic)>::type Sic;
          typedef typename Sic::first_type A;
          typedef typename Sic::second_type B;
          sic.set_max(bf::at_key<A>(map).size(),bf::at_key<B>(map).size());
        });
      });
      */

      mpl::for_each<ListFunction,ttt::wrap<mpl::_1>>(UpdateVab<MapAdressParameters,self>(map,*this));

      bf::for_each(vab_map,ForEachUpdate<MapAdressParameters>(map));
      /*bf::for_each(vab_map,[&map](auto& m)
      {
        bf::for_each(m.second,[&map](auto& pair)
        {
          auto& sic = pair.second;
          sic.update();
        });
      });*/

      mpl::for_each<ListFunction,ttt::wrap<mpl::_1>>(UpdateVab2<MapAdressParameters,self>(map,*this));

    }
    size_t nb_obs() const { return bf::fold(fonction_container.map(),0,TicCounter()); }

    std::ostream& disp(std::ostream& o) const
    {
      detail::DispName<false> dn(o);
      bf::for_each(opt_container.map(), dn);
      detail::DispName<true> dn2(o);
      bf::for_each(fonction_container.map(), dn2);
      return o;
    }

    const typename self::MCA& clone_opt_container() const{return this->opt_container;}
    void restore_container(const typename self::MCA& mca){this->opt_container = mca;}
  };

}// eon lma

template<class X, class _>
std::ostream& operator<<(std::ostream& o, const lma::Bundle<X,_>& bundle)
{
  bundle.disp(o);
  lma::detail::Print6 p1(o);
  boost::fusion::for_each(bundle.opt_container().map(), p1);
  return o;
}

#endif


//
// Created by cheyulin on 7/15/16.
//

#include "../MetaFunctionTool.h"
#include <boost/type_traits.hpp>

using namespace boost;

//MetaFunction, 使用了元函数转发
mp_arglist<mp_arg T>
mp_function demo_func : conditional<
        is_pointer<T>::value, typename add_const<typename remove_pointer<T>::type>::type,
        typename add_pointer<typename add_const<T>::type>::type> {
};

//另一种方式
mp_arglist<mp_arg T>
mp_function demo_func2 :
        mpl::eval_if<
                is_pointer<T>,
                add_const<typename remove_pointer<T>::type>,
                add_pointer<typename add_const<T>::type>
        > {
};

int main() {
    assert((boost::is_same<mp_exec(demo_func<int>), const int *>::value));
    assert((boost::is_same<mp_exec(demo_func<int *>), const int>::value));

    assert((boost::is_same<mp_exec(demo_func2<int>), const int *>::value));
    assert((boost::is_same<mp_exec(demo_func2<int *>), const int>::value));
}


#include <string>
#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

using namespace boost::filesystem;
namespace po = boost::program_options;

int main(int ac, char** av)
{
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("file,F", po::value<std::string>(), "kp list file")
  ;
  po::variables_map vm;
  po::store(po::parse_command_line(ac, av, desc), vm);
  po::notify(vm);
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 1;
  }
  std::string file;
  if (vm.count("file")) {
    file = vm["file"].as<std::string>();
    std::cout << "file: " << file <<std::endl;
  }
  else {
    std::cerr<<"file not set"<<std::endl;
    std::cerr << desc << std::endl;
    return -1;
  }
  std::string output_file = file+".weighted";
  std::ifstream ifs(file.c_str());
  std::string line;
  std::vector<std::pair<double, std::string> > items;
  while(getline(ifs, line))
  {
    std::vector<std::string> vec;
    boost::algorithm::split( vec, line, boost::algorithm::is_any_of(",") );
    if(vec.size()<3)
    {
      std::cout<<"invalid line : "<<line<<std::endl;
      continue;
    }
    std::string text = vec[0];
    uint32_t freq = boost::lexical_cast<uint32_t>(vec[1]);
    uint32_t df = boost::lexical_cast<uint32_t>(vec[2]);
    double value = (double)freq/std::log((double)(df+1));
    items.push_back(std::make_pair(value, text));
  }
  ifs.close();
  std::sort(items.begin(), items.end(), std::greater<std::pair<double, std::string> >());
  std::ofstream ofs(output_file.c_str());
  for(uint32_t i=0;i<items.size();i++)
  {
    ofs<<items[i].second<<"\t"<<items[i].first<<std::endl;
  }
  ofs.close();
  return 0;

}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/iterator/counting_iterator.hpp>

#include <boost/iterator/filter_iterator.hpp>
using namespace boost;

///////////////////////////////////////

bool is_prime(int x)
{
    for (int i = 2; i < x / 2; ++i)
    {
        if (x % i == 0)
        {   return false;}
    }
    return true;
}

#include "step_iterator.hpp"

void case1()
{
    typedef counting_iterator<int> ci_t;
    ci_t c1(11), c2(101);

    typedef step_iterator<ci_t> si_t;
    si_t si1(c1), si2(c2);

    //typedef filter_iterator<bool(*)(int), si_t> fi_t;
    typedef filter_iterator<decltype(&is_prime), si_t> fi_t;

    fi_t first(&is_prime, si1, si2);
    fi_t last(&is_prime, si2, si2);

    //std::copy(first, last,
    //    ostream_iterator<int>(cout, " "));

    std::copy(
            make_filter_iterator(&is_prime, si1, si2),
            make_filter_iterator(&is_prime, si2, si2),
            ostream_iterator<int>(cout, " "));

}

///////////////////////////////////////

int main()
{
    std::cout << "hello filter_iterator" << std::endl;

    case1();
}


#pragma once

#include <data_model.hpp>

#include <iostream>
#include <string>
#include <vector>
#include <stdexcept>
#include <map>
#include <sstream>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace verilog
{
  namespace graph {

    struct Node {
      LogicValue value = LogicValue::Undefined;
      std::string identifier;
      Node(const std::string & i): identifier(i) {}
      Node(): identifier("<extra>") {}
    };

    typedef boost::adjacency_list<
      boost::setS,
      boost::vecS,
      boost::bidirectionalS,
      Node, NegP> GD;

    struct G {
      GD::vertex_descriptor zero, one;
      GD graph;

      G() {
        zero = add_vertex("1'b0");
        one  = add_vertex("1'b1");
      }

      int add_vertex(const std::string & name) {
        return boost::add_vertex(Node(name), graph);
      }

      int new_vertex() {
        return boost::add_vertex(graph);
      }

      void add_edge(int a, int b, NegP p) {
        boost::add_edge(a, b, p, graph);
      }

    };

    struct G_builder  {
      G g;
      std::map<const std::string, int> name_map;
      std::vector<std::string> inputs, outputs;

      G_builder() {
        name_map[g.graph[g.zero].identifier] = g.zero;
        name_map[g.graph[g.one].identifier] = g.one;
      }

      int get_vertex(std::string name) {
        auto it = name_map.find(name);
        if (it != name_map.end()) {
          return it->second;
        }
        else {
          int x = g.add_vertex(name);
          name_map[name] = x;
          return x;
        }
      }

      /*
      std::string new_named_vertex() {
        int v = g.new_vertex();
        std::stringstream ss;
        ss << "extra_vertex_" << v;
        name_map[ss.str()] = v;
        return ss.str();
      }
      */

      void add_edge(int v1, std::string name2, NegP p) {
        int v2 = get_vertex(name2);
        g.add_edge(v1, v2, p);
      }

      void add_edge(std::string name1, std::string name2, NegP p) {
        int v1 = get_vertex(name1);
        int v2 = get_vertex(name2);
        g.add_edge(v1, v2, p);
      }

      void add_edge(std::string name1, int v2, NegP p) {
        int v1 = get_vertex(name1);
        g.add_edge(v1, v2, p);
      }

      void add_edge(int v1, int v2, NegP p) {
        g.add_edge(v1, v2, p);
      }

      void add_vertex(std::string name) {
        get_vertex(name);
      }

    };

    void write_graph(std::ostream & out, const G_builder & b) {
      out << "// inputs:";
      for(auto i : b.inputs) out << " <" << i << ">";
      out << "\n";
      out << "// outputs:";
      for(auto i : b.outputs) out << " <" << i << ">";
      out << "\n";

      boost::write_graphviz(out, b.g.graph
          , [&](std::ostream& o, const GD::vertex_descriptor& v)
          { o << "[label=\""
              << b.g.graph[v].identifier
              << ": "
              << b.g.graph[v].value<< "\"]";}
          , [&](std::ostream& o, const GD::edge_descriptor& e)
          { if (b.g.graph[e] == NegP::Negative)
              o << "[style=dotted]"; }
          );
    }
  }
}


/*
 * DynamicalSystem.hpp
 *
 *  Created on: July 7, 2019
 *      Author: Quincy Jones
 *
 * Copyright (c) <2019> <Quincy Jones - quincy@implementedrobotics.com/>
 * 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#endif // NOMAD_CORE_NOMAD_NOMAD_H,
 * 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.
 */

#ifndef NOMAD_CORE_SYSTEMS_DYNAMICALSYSTEM_H_
#define NOMAD_CORE_SYSTEMS_DYNAMICALSYSTEM_H_

#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>

// TODO: Need to split all of this into Discrete Time and Continuous Time Systems
// TODO: Also Maybe a SS Container?
class DynamicalSystem
{

public:
    DynamicalSystem(const int& num_states, const int& num_inputs, const double& Ts = 1e-1);

    // Step System
    virtual void Step(const Eigen::VectorXd& u) = 0;

    // Update
    virtual void Update() = 0;

    // Get Current State
    Eigen::VectorXd GetState() const { return x_; }

    // Set Current State
    void SetState(const Eigen::VectorXd &x) { x_ = x; }

    int NumStates() const { return num_states_; }
    int NumInputs() const { return num_inputs_; }

protected:

    Eigen::VectorXd x_;  // State Vector

    int num_states_; // Number of System States
    int num_inputs_;     // Number of inputs

    double T_s_; // Sample Time
    double t_; // Current Time
};

class NonLinearDynamicalSystem : public DynamicalSystem
{

protected:

    virtual void f(const Eigen::MatrixXd& x, const Eigen::MatrixXd& u) = 0;

};

class LinearDynamicalSystem : public DynamicalSystem
{

public:

    LinearDynamicalSystem(const int& num_states, const int& num_inputs, const double& T_s = 1e-1);

    // Model Matrices
    Eigen::MatrixXd A() const { return A_; };
    Eigen::MatrixXd B() const { return B_; };

    // Discrete Time Model Matrices
    Eigen::MatrixXd A_d() const { return A_d_; };
    Eigen::MatrixXd B_d() const { return B_d_; };

protected:

    Eigen::MatrixXd A_;  // State Transition Mat rix
    Eigen::MatrixXd B_;  // Control Input Matrix

    Eigen::MatrixXd A_d_;  // Discrete Time State Transition Mat rix
    Eigen::MatrixXd B_d_;  // Discrete Time Input Matrix

};

class LinearTimeVaryingDynamicalSystem : public LinearDynamicalSystem
{

public:

    // Constructor
    LinearTimeVaryingDynamicalSystem(const int& num_states, const int& num_inputs, const double& T_s = 1e-1);

    // Get the Time-Varying Matrices from Model
    virtual void GetModelMatrices(const int& N, Eigen::MatrixXd& A, Eigen::MatrixXd& B) = 0;

    // Model Matrices (Time Varying)
    std::vector<Eigen::MatrixXd> A_TV() const { return A_tv_; };
    std::vector<Eigen::MatrixXd> B_TV() const { return B_tv_; };

    // Discrete Time Model Matrices (Time Varying)
    std::vector<Eigen::MatrixXd> A_d_TV() const { return A_d_tv_; };
    std::vector<Eigen::MatrixXd> B_d_TV() const { return B_d_tv_; };

protected:

    // Model Matrices (Time Varying)
    std::vector<Eigen::MatrixXd> A_tv_;
    std::vector<Eigen::MatrixXd> B_tv_;

    // Discrete Time Model Matrices (Time Varying)
    std::vector<Eigen::MatrixXd> A_d_tv_;
    std::vector<Eigen::MatrixXd> B_d_tv_;
};

#endif // NOMAD_CORE_SYSTEMS_DYNAMICALSYSTEM_H_


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

#include <boost/hana/group/minus_mcd.hpp>
#include <boost/hana/group/negate_mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/group.hpp>
using namespace boost::hana;

template <template <typename ...> class mcd>
void test() {
    constexpr auto g = detail::minimal::group<mcd>;
    BOOST_HANA_CONSTEXPR_ASSERT(negate(g(2)) == g(-2));
}

int main() {
    test<Group::negate_mcd>();
    test<Group::minus_mcd>();
}


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "PositionBasedElasticRodsModel.h"
#include "PositionBasedElasticRodsConstraints.h"
#include "PositionBasedElasticRodsTSC.h"
#include <iostream>
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"
 #include "ShapeOpGL/mesh.h"
 #include "Utils/PDPJConverter.h"
 #include <memory>
 #include <opencv2/opencv.hpp>
 #include <unistd.h>
#include <string>
#include <jsoncpp/json/json.h>
#include <sys/stat.h>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem/exception.hpp>
#include <boost/filesystem/convenience.hpp>
#include "Utils/MyUtil.h"
#include <stdlib.h>

#define _USE_MATH_DEFINES
#include "math.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void initParameters();
void timeStep ();
void timeStep_time ();
void timeStep_itr ();
void buildModel ();
void createRod();
void render ();
void reset();

void TW_CALL setRestDarbouxX(const void *value, void *clientData);
void TW_CALL setRestDarbouxY(const void *value, void *clientData);
void TW_CALL setRestDarbouxZ(const void *value, void *clientData);

void TW_CALL getRestDarbouxX(void *value, void *clientData);
void TW_CALL getRestDarbouxY(void *value, void *clientData);
void TW_CALL getRestDarbouxZ(void *value, void *clientData);

void TW_CALL getBendingAndTwistingStiffnessX(void *value, void *clientData);
void TW_CALL getBendingAndTwistingStiffnessY(void *value, void *clientData);
void TW_CALL getBendingAndTwistingStiffnessZ(void *value, void *clientData);

void TW_CALL setBendingAndTwistingStiffnessX(const void *value, void *clientData);
void TW_CALL setBendingAndTwistingStiffnessY(const void *value, void *clientData);
void TW_CALL setBendingAndTwistingStiffnessZ(const void *value, void *clientData);

/////シミュレーション設定
DemoBase *base;
PositionBasedElasticRodsTSC sim;
const int numberOfPoints = 20;

/////実験設定GLOBAL
std::string idf = "default";
std::string idf_itr = "0";
int max_itr = 40;
int substeps = 1.0;
double frametime;
float rate = 1.0;
double num_frames  = 1500;

float custom;

//SLOPE
bool slope = false;
float ikichi = -1;

bool if_render = false;
bool img_render = if_render;
bool output_position = false;
bool if_dummy = false;

/////GLOBAL変数
float old_energy = -1;
float old_inc = -1;
int image_itr = 0;
std::shared_ptr<ShapeOpSurfaceMesh> s1;
MAverage a(3);
MAverage amp(30);
float hold_force;

int hold_phase = 0;
//0: neutral N
//1: up to zero S - L
//2: up from zero L - S
//3: down to zero S - L
//4: down from zero L - S
float hold_phase_inc = 0;

////ファイル出力
std::ofstream o_pos;

std::ofstream energy;
std::ofstream energy_inc;
std::ofstream inc_inc;

std::ofstream neo_substep;
std::ofstream sub_force;

std::ofstream estimate_force;
std::ofstream precompute_error;

std::ofstream slope_state;

std::ofstream est_amp;
std::ofstream out_offset;
std::ofstream chain_data;

// main
int main( int argc, char **argv )
{
	bool config_file = false;
	substeps = 1.0;
	max_itr = 40;
	double hz = 10.0;
	double damping = 1.0;
	double masses = 10;
	double TriangleConstraint = 1000.0;

	if (argc > 1){
		config_file = (bool)atoi(argv[1]);
		if (config_file) std::cout << "Configure by File" << std::endl;
	}
	if(argc == 4){
		substeps = (float)atoi(argv[2]);
		max_itr =  atoi(argv[3]);
	}
	else if(argc == 7){
		substeps = (float)atoi(argv[2]);
		max_itr =  atoi(argv[3]);
		hz =  atoi(argv[4]);
		damping =  atoi(argv[5]);
		idf =  argv[6];
		std::cout << "6!" << std::endl;
	}
	else if(argc == 8){
		substeps = (float)atoi(argv[2]);
		max_itr =  atoi(argv[3]);
		hz =  atoi(argv[4]);
		damping =  atoi(argv[5]);
		idf =  argv[6];
		output_position = (bool)atoi(argv[7]);
		std::cout << "7!" << std::endl;
	}
	else if(argc == 9){
		substeps = (float)atoi(argv[2]);
		max_itr =  atoi(argv[3]);
		hz =  atoi(argv[4]);
		damping =  atoi(argv[5]);
		idf =  argv[6];
		output_position = (bool)atoi(argv[7]);
		slope = (bool)atoi(argv[8]);
		std::cout << "8!" << std::endl;
	}
	else if(argc == 10){
		substeps = (float)atoi(argv[2]);
		max_itr =  atoi(argv[3]);
		hz =  atoi(argv[4]);
		damping =  atoi(argv[5]);
		idf =  argv[6];
		output_position = (bool)atoi(argv[7]);
		slope = (bool)atoi(argv[8]);
		rate = 		(float)atof(argv[9]);
		std::cout << "9!" << std::endl;
	}
	else if(argc == 16){
		substeps = (float)atoi(argv[2]);
		max_itr =  atoi(argv[3]);
		hz =  atoi(argv[4]);
		damping =  atoi(argv[5]);
		idf =  argv[6];
		idf_itr =  argv[7];
		output_position = (bool)atoi(argv[8]);
		slope = (bool)atoi(argv[9]);
		ikichi = (float)atof(argv[10]);
		num_frames = atoi(argv[11]);
		rate = atof(argv[12]);
		if_render = (bool)atoi(argv[13]);
		custom = (float)atof(argv[14]);
		if_dummy = (bool)atoi(argv[15]);
		std::cout << "15!" << std::endl;
	}
	if_render = img_render || if_render;

	boost::filesystem::remove_all("../output_2/"+idf+"/image");

	if(config_file){
		std::ifstream ifs("/home/octan/デスクトップ/PositionBasedDynamics/Demos/ClothDemo/ClothConfig");
		std::string str;

		if (ifs.fail()) {
			std::cerr << "Failed to open file." << std::endl;
			return -1;
		}
		while (getline(ifs, str)) {
			int num = str.find_first_of(":");
			std::string key = str.substr(0, num);
			std::string value = str.substr(num+1, str.size());
			if(key == "damping") damping = std::stod(value.c_str());
			if(key == "substeps") substeps = std::stod(value.c_str());
			if(key == "max_itr") max_itr = std::stod(value.c_str());
			if(key == "TriangleConstraint") TriangleConstraint  = std::stod(value.c_str());
			if(key == "hz") hz  = std::stod(value.c_str());
			if(key == "masses") masses  = std::stod(value.c_str());

		}
	}
	img_render = if_render;

	frametime = 1.0/hz;
	std::cout << "frametime: " << frametime << "substeps: " << substeps << " , max_itr: " << max_itr << std::endl;
	if((substeps%2 != 0) && if_dummy){
		std::cout << "ダミーステップを用いるときはsubstep数を偶数にしてください" << std::endl;
	}

	const char *dir = ("../output/"+idf).c_str();
    struct stat statBuf;

    if (stat(dir, &statBuf) == 0)
        printf("ディレクトリ%sは存在します。\n", dir);
    else{
		mkdir(("../output/"+idf).c_str(),  0777);
        printf("ディレクトリ%sは存在しません。\n。", dir);
	}

	mkdir(("../output/"+idf+"/"+idf_itr).c_str(),  0777);
	mkdir(("../output/"+idf+"/"+idf_itr+"/images").c_str(),  0777);
	boost::filesystem::remove_all("../output/"+idf+"/"+idf_itr+"/image");

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/enrgy_data");
	energy = std::ofstream("../output/"+idf+"/"+idf_itr+"/enrgy_data", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/positions_data");
	o_pos = std::ofstream("../output/"+idf+"/"+idf_itr+"/positions_data", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/energy_inc");
	energy_inc = std::ofstream("../output/"+idf+"/"+idf_itr+"/energy_inc", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/neo_substep");
	neo_substep = std::ofstream("../output/"+idf+"/"+idf_itr+"/neo_substep", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/sub_force");
	sub_force = std::ofstream("../output/"+idf+"/"+idf_itr+"/sub_force", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/estimate_force");
	estimate_force = std::ofstream("../output/"+idf+"/"+idf_itr+"/estimate_force", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/precompute_error");
	precompute_error = std::ofstream("../output/"+idf+"/"+idf_itr+"/precompute_error", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/inc_inc");
	inc_inc = std::ofstream("../output/"+idf+"/"+idf_itr+"/inc_inc", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/slope_state");
	slope_state = std::ofstream("../output/"+idf+"/"+idf_itr+"/slope_state", std::ios::app);

    std::ofstream outputfile_energy("../output/"+idf+"/"+idf_itr+"/enrgy_data+", std::ios::app);
	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/outputconfig");
	std::ofstream outputconfig("../output/"+idf+"/"+idf_itr+"/outputconfig", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/est_amp");
	est_amp = std::ofstream("../output/"+idf+"/"+idf_itr+"/est_amp", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/out_offset");
	out_offset = std::ofstream("../output/"+idf+"/"+idf_itr+"/out_offset", std::ios::app);

	boost::filesystem::remove("../output/"+idf+"/"+idf_itr+"/chain_data");
	chain_data = std::ofstream("../output/"+idf+"/"+idf_itr+"/chain_data", std::ios::app);

	outputconfig << damping << " " << substeps <<" " <<  max_itr << " " << TriangleConstraint << " " << hz << " " << masses;

	REPORT_MEMORY_LEAKS

	base = new DemoBase();
	base->init(argc, argv, "Elastic rod demo");

	PositionBasedElasticRodsModel *model = new PositionBasedElasticRodsModel();
	model->init();
	Simulation::getCurrent()->setModel(model);
	PositionBasedElasticRodsTSC *tsc = new PositionBasedElasticRodsTSC();
	tsc->init();
	delete Simulation::getCurrent()->getTimeStep();
	Simulation::getCurrent()->setTimeStep(tsc);

	buildModel();

	initParameters();

	Simulation::getCurrent()->setSimulationMethodChangedCallback([&]() { reset(); initParameters(); base->getSceneLoader()->readParameterObject(Simulation::getCurrent()->getTimeStep()); });

	// OpenGL
	MiniGL::setClientIdleFunc (5, timeStep);
	// MiniGL::setKeyFunc(0, 'r', reset);
	// MiniGL::setKeyFunc(1, 'e', timeStep);
	// MiniGL::setKeyFunc(2, 't', timeStep_time);
	// MiniGL::setKeyFunc(3, 'i', timeStep_itr);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 5.0, 10.0), Vector3r (5.0, 0.0, 0.0));

	// TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxX", TW_TYPE_REAL, setRestDarbouxX, getRestDarbouxX, model, " label='Rest Darboux X'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	// TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxY", TW_TYPE_REAL, setRestDarbouxY, getRestDarbouxY, model, " label='Rest Darboux Y'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	// TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxZ", TW_TYPE_REAL, setRestDarbouxZ, getRestDarbouxZ, model, " label='Rest Darboux Z'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");

	// TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessX", TW_TYPE_REAL, setBendingAndTwistingStiffnessX, getBendingAndTwistingStiffnessX, model, " label='Bending X stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	// TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessY", TW_TYPE_REAL, setBendingAndTwistingStiffnessY, getBendingAndTwistingStiffnessY, model, " label='Bending Y stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	// TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessZ", TW_TYPE_REAL, setBendingAndTwistingStiffnessZ, getBendingAndTwistingStiffnessZ, model, " label='Twisting stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");

	auto n1 = std::make_shared<GLSurfaceMeshObject>("buf_chain.obj", Eigen::Vector3f(1.0f, 0.5f, 0.5f));
    Eigen::Matrix4f mat = Eigen::Matrix4f::Identity(); mat(0, 3) = -1.4;

	s1 = std::make_shared<ShapeOpSurfaceMesh>(n1, outputfile_energy, frametime / substeps, damping, masses, TriangleConstraint);
	s1->addPin(20000,0);
    // s1->addPin(1000,380);
    s1->addChain(1000);
    // s1->addTriangleStrain(TriangleConstraint);
	s1->initsolver(outputfile_energy, true, masses, damping, frametime / substeps);//kokoppoi

	old_energy = s1->compute_energy();
	hold_force = 0;

	std::cout << "/////////////////////////////////////////////////////////" << std::endl;
    std::cout << "damping: " << damping << std::endl;
    std::cout << "timestep: " << frametime / substeps << std::endl;
	std::cout << "substeps: " << substeps << std::endl;
	std::cout << "frametime: " << frametime << std::endl;
    std::cout << "masses: " << masses << std::endl;
	std::cout << "ikichi: " << ikichi << std::endl;
	std::cout << "slope: " << slope << std::endl;
	std::cout << "dummy: " << if_dummy << std::endl;
	std::cout << "num_frames: " << num_frames << std::endl;
	std::cout << "/////////////////////////" << std::endl;

	//シミュレーション準備
	old_energy = s1->compute_energy();
	old_inc = 0.0;

	if(!if_render){
		for(int i=0; i<10000; i++){
			timeStep();
		}
		return 0;
	}

	glutMainLoop ();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void initParameters()
{
	TwRemoveAllVars(MiniGL::getTweakBar());
	TweakBarParameters::cleanup();

	MiniGL::initTweakBarParameters();

	TweakBarParameters::createParameterGUI();
	TweakBarParameters::createParameterObjectGUI(base);
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent());
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getModel());
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getTimeStep());
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();
}

void buildModel()
{
	TimeManager::getCurrent()->setTimeStepSize(0.002f);
	PositionBasedElasticRodsModel *model = (PositionBasedElasticRodsModel*)Simulation::getCurrent()->getModel();
	model->setBendingAndTwistingStiffness(Vector3r(0.5, 0.5, 0.5));

	sim.setDamping(0.001f);

	createRod();
}

void timeStep ()
{
	if(image_itr > num_frames) {
		std::cout << "EXIT by num_Frames" << std::endl;
		std::cout << std::endl;
		exit(1);
	}

	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();

										std::vector<float> ens;
										// // s1->pre_process(max_itr, frametime/2.0, ens);
										// s1->pre_process(max_itr, frametime/substeps, ens);
										// estimate_force << ens[1] - ens[0] << ", " <<  ens[2] - ens[1] << ", " <<ens[3] - ens[2] << ", " ;
										// int pattern = judge_preprocess(ens, old_energy, old_inc/2);

										std::vector<float> steps;
										// // inclined_substeps(substeps, frametime, steps, (float)old_inc );
										// inclined_substeps(substeps, frametime, steps, pattern, rate);
										// // const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	if(if_dummy){
		for (unsigned int i = 0; i < substeps/2; i++){
			START_TIMING("SimStep");
			s1->comp_process(max_itr, image_itr, (frametime/substeps) );
			s1->process(max_itr, image_itr, (frametime/substeps));
			STOP_TIMING_AVG;
		}

	}
	else{
		for (unsigned int i = 0; i < substeps; i++)
		{
			START_TIMING("SimStep");
			if(slope){
				s1->process(max_itr, image_itr, steps[i]);
				neo_substep << steps[i] << ", ";
				sub_force << (float)s1->compute_energy() << ", ";
			}
			else{
			// Simulation::getCurrent()->getTimeStep()->step(*model);
				s1->process(max_itr, image_itr);
			}

			STOP_TIMING_AVG;
		}
	}

	ParticleData &pd = model->getParticles();
	const ShapeOp::Matrix3X p = s1->getSolver();
	#pragma omp for schedule(static)
	for (int i = 0; i < (int) pd.size(); i++)
	{
		updatePJ(pd.getPosition(i), p, i);
	}
	int w = 1024;    int h = 768;
    cv::Mat_<cv::Vec3b> image(h,w);
	glReadPixels(0, 0, w, h, GL_BGR, GL_UNSIGNED_BYTE, image.data);
	cv::flip(image, image, 0);
	if(img_render)	cv::imwrite("../output_2/"+idf+"/images/"+std::to_string(image_itr)+".png", image);
	image_itr++;
	chain_data <<  (float)model->getParticles().getPosition((int) model->getParticles().size() - 1)(1) << ", ";
	energy << (float)s1->compute_energy() << ", ";
	hold_force = (float)s1->compute_energy() - old_energy;
	energy_inc << hold_force << ", ";
	inc_inc << ((float)s1->compute_energy() - old_energy) - old_inc << ", ";

	old_inc = (float)s1->compute_energy() - old_energy;
	old_energy = (float)s1->compute_energy();
}

void timeStep_time ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		// Simulation::getCurrent()->getTimeStep()->step(*model);
		s1->process(false);
		ParticleData &pd = model->getParticles();
		const ShapeOp::Matrix3X p = s1->getSolver();
		#pragma omp for schedule(static)
		for (int i = 0; i < (int) pd.size(); i++)
		{
			updatePJ(pd.getPosition(i), p, i);
		}
		STOP_TIMING_AVG;
	}
	int w = 1024;    int h = 768;
    cv::Mat_<cv::Vec3b> image(h,w);
	glReadPixels(0, 0, w, h, GL_BGR, GL_UNSIGNED_BYTE, image.data);
	cv::flip(image, image, 0);
	if(img_render)	cv::imwrite("../output_2/"+idf+"/images/"+std::to_string(image_itr)+".png", image);
	image_itr++;
	chain_data <<  (float)model->getParticles().getPosition((int) model->getParticles().size() - 1)(1) << ", ";
}
void timeStep_itr ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		// Simulation::getCurrent()->getTimeStep()->step(*model);
		s1->process(true);
		ParticleData &pd = model->getParticles();
		const ShapeOp::Matrix3X p = s1->getSolver();
		#pragma omp for schedule(static)
		for (int i = 0; i < (int) pd.size(); i++)
		{
			updatePJ(pd.getPosition(i), p, i);
		}
		STOP_TIMING_AVG;
	}
	int w = 1024;    int h = 768;
    cv::Mat_<cv::Vec3b> image(h,w);
	glReadPixels(0, 0, w, h, GL_BGR, GL_UNSIGNED_BYTE, image.data);
	cv::flip(image, image, 0);
	if(img_render)	cv::imwrite("../output_2/"+idf+"/images/"+std::to_string(image_itr)+".png", image);
	image_itr++;
	chain_data <<  (float)model->getParticles().getPosition((int) model->getParticles().size() - 1)(1) << ", ";
	energy << (float)s1->compute_energy() << ", ";
}

void render()
{
	base->render();

	// Draw sim model
	PositionBasedElasticRodsModel *model = (PositionBasedElasticRodsModel*) Simulation::getCurrent()->getModel();
	ParticleData &pd = model->getParticles();
	// ParticleData &ghostParticles = model->getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model->getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float pointColor[4] = { 0.1f, 0.2f, 0.6f, 1 };
	float ghostPointColor[4] = { 0.1f, 0.1f, 0.1f, 0.5f };
	float edgeColor[4] = { 0.0f, 0.6f, 0.2f, 1 };

	for (unsigned int i = 0; i < numberOfPoints; i++)
	{
		MiniGL::drawSphere(pd.getPosition(i), 0.07f, pointColor);
	}

	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		// MiniGL::drawSphere(ghostParticles.getPosition(i), 0.07f, ghostPointColor);
		MiniGL::drawVector(pd.getPosition(i), pd.getPosition(i + 1), 0.2f, edgeColor);
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

/** Create the elastic rod model
*/
void createRod()
{
	PositionBasedElasticRodsModel *model = (PositionBasedElasticRodsModel*)Simulation::getCurrent()->getModel();
	ParticleData &particles = model->getParticles();
	// ParticleData &ghostParticles = model->getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model->getConstraints();
	std::cout << "a" << std::endl;
	Vector3r points[numberOfPoints];
	//centreline points
	for (unsigned int i = 0; i < numberOfPoints; i++)
	{
		points[i] = Vector3r(0.0, static_cast<Real>(0.01 * (numberOfPoints - i - 1)), 0.0);
		particles.addVertex(points[i]);

	}
	std::cout << "a" << std::endl;
	std::cout <<  (float)model->getParticles().getPosition((int) model->getParticles().size() - 1)(1) << std::endl;
	//edge ghost points
	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		// ghostParticles.addVertex(Vector3r(static_cast<Real>(0.25 * i) + static_cast<Real>(0.125), static_cast<Real>(0.25), 0.0));
	}
	std::cout << "a" << std::endl;
	//lock two first particles and first ghost point
	particles.setMass(0, 0.0f);
	particles.setMass(1, 0.0f);
	// ghostParticles.setMass(0, 0.0f);

	for (unsigned int i = 0; i < numberOfPoints - 1; i++)
	{
		// model->addDistanceConstraint(i, i + 1);
		// model->addPerpendiculaBisectorConstraint(i, i + 1, i);
		// model->addGhostPointEdgeDistanceConstraint(i, i + 1, i);

		// if (i < numberOfPoints - 2)
		// {
		// 	//  Single rod element:
		// 	//      D   E		//ghost points
		// 	//		|	|
		// 	//  --A---B---C--	// rod points
		// 	int pA = i;
		// 	int pB = i + 1;
		// 	int pC = i + 2;
		// 	int pD = i;
		// 	int pE = i + 1;
		// 	model->addDarbouxVectorConstraint(pA, pB, pC, pD, pE);
		// }
	}
	std::cout << "a" << std::endl;
	write_mymesh(numberOfPoints, &points[0], "buf_chain.obj");
}

void TW_CALL setRestDarbouxX(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[0] = *(const Real *)(value);
}

void TW_CALL setRestDarbouxY(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[1] = *(const Real *)(value);
}

void TW_CALL setRestDarbouxZ(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[2] = *(const Real *)(value);
}

void TW_CALL getRestDarbouxX(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[0];
}

void TW_CALL getRestDarbouxY(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[1];
}

void TW_CALL getRestDarbouxZ(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[2];
}

void TW_CALL setBendingAndTwistingStiffnessX(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[0] = *(const Real *)(value);
}

void TW_CALL setBendingAndTwistingStiffnessY(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[1] = *(const Real *)(value);
}

void TW_CALL setBendingAndTwistingStiffnessZ(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[2] = *(const Real *)(value);
}

void TW_CALL getBendingAndTwistingStiffnessX(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[0];
}

void TW_CALL getBendingAndTwistingStiffnessY(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[1];
}

void TW_CALL getBendingAndTwistingStiffnessZ(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[2];
}


#define BOOST_TEST_MODULE pcraster model_engine averagemap
#include <boost/test/unit_test.hpp>

#define private public
#include "calc_averagemap.h"

// NOTE use string failureExpected in files expected to fail, see style guide

BOOST_AUTO_TEST_CASE(test)
{
  using namespace calc;

  {
   AverageMap am;
   UINT1 id[1]  = { 1 };
   REAL4 val[1] = { 8 };
   am.apply(id,1,val,1);
   BOOST_CHECK(am.d_map.size()==1);
   double res[1]={-1};
   am.setResults(res,1);
   BOOST_CHECK(res[0]==8);
  }
  {
   AverageMap am;
   UINT1 id[4]  = { 1, 1, MV_UINT1, 2 };
   REAL4 val[1] = { 8 };
   am.apply(id,4,val,1);
   BOOST_CHECK(am.d_map.size()==2);
   BOOST_CHECK(am.d_map[1].nr()==2);
   BOOST_CHECK(am.d_map[2].nr()==1);
   double res[2]={-1,-1};
   am.setResults(res,2);
   BOOST_CHECK(res[0]==8);
   BOOST_CHECK(res[1]==8);
  }
  {
   AverageMap am;
   UINT1 id[4]  = { 1, 1, MV_UINT1, 2 };
   REAL4 val[4] = { 8, 4, 4,        1 };
   am.apply(id,4,val,4);
   BOOST_CHECK(am.d_map.size()==2);
   BOOST_CHECK(am.d_map[1].nr()==2);
   BOOST_CHECK(am.d_map[2].nr()==1);
   double res[2]={-1,-1};
   am.setResults(res,2);
   BOOST_CHECK(res[0]==6);
   BOOST_CHECK(res[1]==1);
  }
}