// Copyright (c) 2013 The claire-common Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file. See the AUTHORS file for names of contributors.

#include <claire/common/strings/StringUtil.h>

#include <stdio.h>
#include <assert.h>

#include <algorithm>

#include <boost/type_traits/is_arithmetic.hpp>

namespace claire {

#pragma GCC diagnostic ignored "-Wtype-limits"
const char digits[] = "9876543210123456789";
const char* zero = digits + 9;
static_assert(sizeof(digits) == 20, "digits size not equal 20");

const char digitsHex[] = "0123456789ABCDEF";
static_assert(sizeof(digitsHex) == 17, "digitsHex size not equal 17");

// Efficient Integer to String Conversions, by Matthew Wilson.
template<typename T>
size_t IntToBuffer(char buffer[], T value)
{
    T i = value;
    char* p = buffer;

    do
    {
        int lsd = static_cast<int>(i % 10);
        i /= 10;
        *p++ = zero[lsd];
    } while (i != 0);

    if (value < 0)
    {
        *p++ = '-';
    }

    *p = '\0';
    std::reverse(buffer, p);

    return p - buffer;
}

size_t HexToBuffer(char buffer[], uintptr_t value)
{
    uintptr_t i = value;
    char* p = buffer;

    do
    {
        int lsd = i % 16;
        i /= 16;
        *p++ = digitsHex[lsd];
    } while (i != 0);

    *p = '\0';
    std::reverse(buffer, p);

    return p - buffer;
}

template size_t IntToBuffer(char [], char);
template size_t IntToBuffer(char [], short);
template size_t IntToBuffer(char [], unsigned short);
template size_t IntToBuffer(char [], int);
template size_t IntToBuffer(char [], unsigned int);
template size_t IntToBuffer(char [], long);
template size_t IntToBuffer(char [], unsigned long);
template size_t IntToBuffer(char [], long long);
template size_t IntToBuffer(char [], unsigned long long);

size_t HexString(char buffer[], size_t buffer_length,
                 const char* data, size_t data_length)
{
    size_t current_length = 0;
    for (size_t i = 0;i < data_length; i++)
    {
        int ret = 0;
        if (i % 16 == 0)
        {
            ret = snprintf(buffer + current_length, buffer_length - current_length, "\n0x%04x:", static_cast<unsigned int>(i));
            if (ret < 0 || ret > static_cast<int>(buffer_length - current_length))
            {
                break;
            }
            current_length += ret;
        }

        if (i % 2 == 0)
        {
            ret = snprintf(buffer + current_length, buffer_length - current_length, " ");
            if (ret < 0 || ret > static_cast<int>(buffer_length - current_length))
            {
                break;
            }
            current_length += ret;
        }

        ret = snprintf(buffer + current_length,  buffer_length - current_length, "%02x", static_cast<uint8_t>(data[i]));
        if (ret < 0 || ret > static_cast<int>(buffer_length - current_length))
        {
            break;
        }
        current_length += ret;
    }

    return current_length;
}

} // namespace claire


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/shuffle.hpp>
#include <boost/simd/detail/shuffle/shuffler.hpp>
#include <boost/simd/arch/x86/sse2/simd/function/topology.hpp>

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

  // -----------------------------------------------------------------------------------------------
  // Unary SSE 2xtype64 shuffle
  BOOST_DISPATCH_OVERLOAD ( shuffle_
                          , (int... Ps, typename A0)
                          , bs::sse2_
                          , bsd::pattern_<Ps...>
                          , bs::pack_<bd::type64_<A0>, bs::sse_>
                         )
  {
    static_assert ( sizeof...(Ps) == 2
                  , "boost::simd::shuffle - Invalid number of permutation indices"
                  );

    BOOST_FORCEINLINE
    A0 operator()(bsd::pattern_<Ps...> const&, const A0 & a0) const
    {
      return detail::shuffler<detail::sse2_shuffle,detail::pattern_<Ps...>>::process(a0);
    }
  };

  // -----------------------------------------------------------------------------------------------
  // Unary SSE 4xint32 shuffle
  BOOST_DISPATCH_OVERLOAD ( shuffle_
                          , (int... Ps, typename A0)
                          , bs::sse2_
                          , bsd::pattern_<Ps...>
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                         )
  {
    static_assert ( sizeof...(Ps) == 4
                  , "boost::simd::shuffle - Invalid number of permutation indices"
                  );

    BOOST_FORCEINLINE A0 operator()(bsd::pattern_<Ps...> const&, const A0 & a0) const
    {
      return detail::shuffler<detail::sse2_shuffle,detail::pattern_<Ps...>>::process(a0);
    }
  };

  // -----------------------------------------------------------------------------------------------
  // Binary SSE 2xtype64 shuffle
  BOOST_DISPATCH_OVERLOAD ( shuffle_
                          , (int... Ps, typename A0)
                          , bs::sse2_
                          , bsd::pattern_<Ps...>
                          , bs::pack_<bd::type64_<A0>, bs::sse_>
                          , bs::pack_<bd::type64_<A0>, bs::sse_>
                         )
  {
    static_assert ( sizeof...(Ps) == 2
                  , "boost::simd::shuffle - Invalid number of permutation indices"
                  );

    BOOST_FORCEINLINE
    A0 operator()(bsd::pattern_<Ps...> const&, const A0 & a0, const A0 & a1) const
    {
      return detail::shuffler<detail::sse2_shuffle,detail::pattern_<Ps...>>::process(a0,a1);
    }
  };

  // -----------------------------------------------------------------------------------------------
  // Binary SSE 4xint32 shuffle
  BOOST_DISPATCH_OVERLOAD ( shuffle_
                          , (int... Ps, typename A0)
                          , bs::sse2_
                          , bsd::pattern_<Ps...>
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                         )
  {
    static_assert ( sizeof...(Ps) == 4
                  , "boost::simd::shuffle - Invalid number of permutation indices"
                  );

    BOOST_FORCEINLINE
    A0 operator()(bsd::pattern_<Ps...> const&, const A0 & a0, const A0 & a1) const
    {
      return detail::shuffler<detail::sse2_shuffle,detail::pattern_<Ps...>>::process(a0,a1);
    }
  };
} } }

#endif


#include "DEMInterpolator.h"

#include <isce3/core/Constants.h>
#include <isce3/io/Raster.h>
#include <Eigen/Dense>
#include <stdexcept>
#include <string>

namespace py = pybind11;

using DI = isce3::geometry::DEMInterpolator;

void addbinding(pybind11::class_<DI> & pyDEMInterpolator)
{
    pyDEMInterpolator
        .def(py::init<double, isce3::core::dataInterpMethod, int>(),
            py::arg("height") = 0.0,
            py::arg("method") = isce3::core::BILINEAR_METHOD,
            py::arg("epsg") = 4326)
        // For convenience allow a string, too.
        .def(py::init([](double h, const std::string & method, int epsg) {
                auto m = parseDataInterpMethod(method);
                return new DI(h, m, epsg);
            }),
            py::arg("height") = 0.0,
            py::arg("method") = "bilinear",
            py::arg("epsg") = 4326)

        .def("load_dem",
            py::overload_cast<isce3::io::Raster&>(&DI::loadDEM))
        .def("load_dem",
            py::overload_cast<isce3::io::Raster&, double, double, double, double>
                (&DI::loadDEM),
            py::arg("raster"), py::arg("min_x"), py::arg("max_x"),
                py::arg("min_y"), py::arg("max_y"))

        .def("interpolate_lonlat", &DI::interpolateLonLat)
        .def("interpolate_xy", &DI::interpolateXY)

        .def_property("ref_height",
            py::overload_cast<>(&DI::refHeight, py::const_),
            py::overload_cast<double>(&DI::refHeight))
        .def_property_readonly("have_raster", &DI::haveRaster)
        .def_property("interp_method",
            py::overload_cast<>(&DI::interpMethod, py::const_),
            py::overload_cast<isce3::core::dataInterpMethod>(&DI::interpMethod))

        // Define all these as readonly even though writable in C++ API.
        // Probably better to just convert your data to a GDAL format than try
        // to build a DEM on the fly.
        .def_property_readonly("data", [](DI & self) { // .data() isn't const
            if (!self.haveRaster()) {
                throw std::out_of_range("Tried to access DEM data but size=0");
            }
            using namespace Eigen;
            using MatF = Eigen::Matrix<float, Dynamic, Dynamic, RowMajor>;
            Map<const MatF> mat(self.data(), self.length(), self.width());
            return mat;
        }, py::return_value_policy::reference_internal)
        .def_property_readonly("x_start", py::overload_cast<>(&DI::xStart, py::const_))
        .def_property_readonly("y_start", py::overload_cast<>(&DI::yStart, py::const_))
        .def_property_readonly("delta_x", py::overload_cast<>(&DI::deltaX, py::const_))
        .def_property_readonly("delta_y", py::overload_cast<>(&DI::deltaY, py::const_))
        .def_property_readonly("width", py::overload_cast<>(&DI::width, py::const_))
        .def_property_readonly("length", py::overload_cast<>(&DI::length, py::const_))
        .def_property_readonly("epsg_code", py::overload_cast<>(&DI::epsgCode, py::const_))
        ;
}


#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 "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Utils/OBJLoader.h"
#include "Demos/Visualization/Visualization.h"
#include "Simulation/DistanceFieldCollisionDetection.h"
#include "Utils/SceneLoader.h"
#include "Utils/TetGenLoader.h"
#include "Simulation/CubicSDFCollisionDetection.h"
#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 "Utils/PDPJConverter.h"
 #include "ShapeOpGL/mesh.h"
 #include "Utils/MyUtil.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 <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 buildModel ();
void readScene(const bool readFile);
void render ();
void reset();
void TW_CALL setContactTolerance(const void *value, void *clientData);
void TW_CALL getContactTolerance(void *value, void *clientData);
void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData);
void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData);
void TW_CALL setBendingMethod(const void *value, void *clientData);
void TW_CALL getBendingMethod(void *value, void *clientData);
void TW_CALL setClothSimulationMethod(const void *value, void *clientData);
void TW_CALL getClothSimulationMethod(void *value, void *clientData);
void TW_CALL setSolidSimulationMethod(const void *value, void *clientData);
void TW_CALL getSolidSimulationMethod(void *value, void *clientData);

DemoBase *base;
Vector3r camPos;
Vector3r camLookat;
CubicSDFCollisionDetection cd;

bool cam_okkake = false;

short clothSimulationMethod = 2;
short solidSimulationMethod = 2;
short bendingMethod = 2;

/////実験設定GLOBAL
std::string idf = "default_collision";
std::string idf_itr = "0";
int max_itr = 1;
int precompute_max_itr = 40;
int precompute_substeps = 1;
int substeps = 40;
double frametime;
float rate = 1.0;
double num_frames  = 1500;
float k = 0.8;
float adap_rate = 0.01;
float coll_max_speed = 2.0;

float custom;

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

bool if_render = true;
bool img_render = if_render;
bool output_position = false;
bool if_dummy = false;
bool if_dummy_3 = false;
bool custom_bool = true;
bool custom_bool2 = true;
bool custom_bool3 = true;
bool custom_bool4 = true;
bool output_obj = false;
bool adap_dummy = false;
bool adams_1 = 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);
// std::make_shared<std::vector<bool>> violated;
int neo_dummy = 1;

int hold_phase = 0;
float hold_phase_inc = 0;
std::vector<std::vector<int>> neighbors;

////ファイル出力
std::ofstream o_pos;
std::ofstream o_vel;
std::ofstream adap_steps;
std::ofstream energy;
std::ofstream precompute_error;
std::ofstream dummy_violate;
std::ofstream cost_log;
// DistanceFieldCollisionDetection cd;

std::vector<std::shared_ptr<ShapeOp::Solver>> solvers_vec;
int num_solver = 0;

ShapeOp::Solver solver_;

std::vector<std::vector<Vector3r>> vat;
std::vector<std::vector<Vector3r>> vat_norm;
float vat_min; float vat_max;
float vat_norm_min; float vat_norm_max;
Matrix3X default_pos;

// main
int main( int argc, char **argv )
{
	//LOCAL Configure
	bool config_file = false;
	double hz = 30.0;
	double damping = 1.0;
	double masses = 0.1;
	double TriangleConstraint = 1000.0;
	double VolumeConstraint = 1000.0;
	double EdgeStrainConstraint = 1000.0;

	//引数回収
	if (argc > 2){//ファイルから設定
		config_file = (bool)atoi(argv[1+1]);
		if (config_file) std::cout << "Configure by File" << std::endl;
	}
	std::cout << "argc: " << argc << std::endl;
	if(argc > 6+1){
		for(int i=2+1; i<argc; i++){
			switch(i-1){
			case 2: substeps = (float)atoi(argv[i]);
			case 3: max_itr =  atoi(argv[i]);
			case 4: hz =  atoi(argv[i]);
			case 5: damping =  atoi(argv[i]);
			case 6: idf =  argv[i];
			case 7: idf_itr =  argv[i];
			case 8: output_position = (bool)atoi(argv[i]);
			case 9: slope = (bool)atoi(argv[i]);
			case 10: ikichi = (float)atof(argv[i]);
			case 11: num_frames = atoi(argv[i]);
			case 12: rate = atof(argv[i]);
			case 13: if_render = (bool)atoi(argv[i]);
			case 14: custom = (float)atof(argv[i]);
			case 15: if_dummy = (bool)atoi(argv[i]);
			case 16: custom_bool = (bool)atoi(argv[i]);
			case 17: custom_bool2 = (bool)atoi(argv[i]);
			case 18: neo_dummy =  atoi(argv[i]);
			case 19: if_dummy_3 = (bool)atoi(argv[i]);
			case 20: custom_bool3 = (bool)atoi(argv[i]);
			case 21: precompute_max_itr = atoi(argv[i]);
			case 22: k = (float)atof(argv[i]);
			case 23: coll_max_speed = (float)atof(argv[i]);
			case 24: output_obj = (bool)atoi(argv[i]);
			case 25: adap_dummy = (bool)atoi(argv[i]);
			case 26: precompute_substeps = (int)atoi(argv[i]);
			case 27: adap_rate = (float)atof(argv[i]);
			case 28: adams_1 = (bool)atoi(argv[i]);
			}
		}
	}
	if_render = img_render || if_render;

	if(config_file){
		std::ifstream ifs("/home/octan/デスクトップ/PositionBasedDynamics/Demos/SceneLoaderDemo/configs/config");
		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());
			if(key == "VolumeConstraint") VolumeConstraint  = std::stod(value.c_str());
			if(key == "EdgeStrainConstraint") EdgeStrainConstraint  = 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(if_dummy){
		if(if_dummy_3){
			if(substeps != 1 && substeps%3 != 0){
				std::cout << "ステップ数が不正です" << std::endl;
				exit(1);
			}

		}
		else{
			if(substeps != 1 && substeps%2 != 0){
				std::cout << "ステップ数が不正です" << std::endl;
				exit(1);
			}

		}
	}

	//フォルダ初期化
	const char *dir = ("../output/"+idf).c_str();
    struct stat statBuf;

    if (stat(("../output/"+idf).c_str(), &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");

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

	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+"/velocities_data");
	o_vel = std::ofstream("../output/"+idf+"/"+idf_itr+"/velocities_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+"/precompute_error");
	precompute_error = std::ofstream("../output/"+idf+"/"+idf_itr+"/precompute_error", std::ios::app);

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

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

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

	REPORT_MEMORY_LEAKS

	std::string exePath = FileSystem::getProgramPath();
	std::string dataPath = exePath + "/" + std::string(PBD_DATA_PATH);

	std::string sceneFileName;
	if (argc > 1)
		sceneFileName = string(argv[1]);
	else
		sceneFileName = FileSystem::normalizePath(dataPath + sceneFileName);

	base = new DemoBase();
	int argc_beta = 2;
	base->init(argc_beta, argv, sceneFileName.c_str());

	SimulationModel *model = new SimulationModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	initParameters();
	base->readParameters();

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

	// OpenGL
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport(40.0f, 0.1f, 500.0f, camPos, camLookat);

	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "ContactTolerance", TW_TYPE_REAL, setContactTolerance, getContactTolerance, &cd, " label='Contact tolerance'  min=0.0 step=0.001 precision=3 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessRigidBody", TW_TYPE_REAL, setContactStiffnessRigidBody, getContactStiffnessRigidBody, model, " label='Contact stiffness RB'  min=0.0 step=0.1 precision=2 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessParticleRigidBody", TW_TYPE_REAL, setContactStiffnessParticleRigidBody, getContactStiffnessParticleRigidBody, model, " label='Contact stiffness Particle-RB'  min=0.0 step=0.1 precision=2 group=Simulation ");
	TwType enumType2 = TwDefineEnum("ClothSimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "ClothSimulationMethod", enumType2, setClothSimulationMethod, getClothSimulationMethod, &clothSimulationMethod, " label='Cloth sim. method' enum='0 {None}, 1 {Distance constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics}' group=Simulation");
	TwType enumType3 = TwDefineEnum("SolidSimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SolidSimulationMethod", enumType3, setSolidSimulationMethod, getSolidSimulationMethod, &solidSimulationMethod,
		" label='Solid sim. method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics (no inversion handling)}, 4 {Shape matching (no inversion handling)}' group=Simulation");
	TwType enumType4 = TwDefineEnum("BendingMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "BendingMethod", enumType4, setBendingMethod, getBendingMethod, &bendingMethod, " label='Bending method' enum='0 {None}, 1 {Dihedral angle}, 2 {Isometric bending}' group=Bending");

	num_solver = model->getTetModels().size();
	ShapeOp::Matrix3X p = r_to_x(model->getParticles());
	solver_.setPoints(p);
	solver_.addForces(std::make_shared<ShapeOp::GravityForce>(ShapeOp::Vector3(0.0, -10.0, 0.0)));
	solvers_vec = std::vector<std::shared_ptr<ShapeOp::Solver>>(num_solver);
	for(int i = 0; i < num_solver; i++){
		auto sol = std::make_shared<ShapeOp::Solver>();
		solvers_vec.push_back(sol);
		Add_EdgeConstraint(solver_, model, EdgeStrainConstraint, i);
		Add_VolumeConstraint(solver_, model,VolumeConstraint, i);
	}
	solver_.initialize(true, masses, damping, frametime / substeps);
	solver_.set_coll_max_speed(coll_max_speed);
	solver_.setPBDmodel(model);
	PrecomputeNeighbor(neighbors, model);

	outputconfig << "/////////////////////////////////////////////////////////" << std::endl;
	outputconfig <<  "substeps: " <<  substeps  << std::endl;
	outputconfig <<  "max_itr: "  <<  max_itr  << std::endl;
	outputconfig <<  "hz: " <<  hz  << std::endl;
	outputconfig <<  "damping: " <<  damping  << std::endl;
	outputconfig <<  "idf: " <<  idf << std::endl;
	outputconfig <<  "idf_itr: " <<  idf_itr  << std::endl;
	outputconfig <<  "output_position: " <<  output_position  << std::endl;
	outputconfig <<  "slope: " <<  slope  << std::endl;
	outputconfig << "ikichi: "  << ikichi  << std::endl;
	outputconfig <<  "num_frames: " <<  num_frames  << std::endl;
	outputconfig <<  "rate: " << rate << std::endl;
	outputconfig <<  "if_render: " <<  if_render  << std::endl;
	outputconfig <<  "custom: " <<  custom << std::endl;
	outputconfig <<  "if_dummy: " <<  if_dummy  << std::endl;
	outputconfig <<  "custom_bool: " <<  custom_bool << std::endl;
	outputconfig <<  "custom_bool2: " <<  custom_bool2  << std::endl;
	outputconfig <<  "neo_dummy: " <<  neo_dummy << std::endl;
	outputconfig <<  "if_dummy_3: " <<  if_dummy_3  << std::endl;
	outputconfig <<  "custom_bool3: " <<  custom_bool3  << std::endl;

	// outputconfig <<  "stretching: " <<  stretching  << std::endl;
	outputconfig <<  "TriangleConstraint: " <<  TriangleConstraint  << std::endl;
	// outputconfig <<  "pin: " <<  pin  << std::endl;
	// outputconfig <<  "nRowss " <<  nRows  << std::endl;

	outputconfig << "/////////////////////////" << std::endl;

	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 << "adams: " << adams_1 << std::endl;
	std::cout << "/////////////////////////" << std::endl;

	int a = model->getTetModels()[0]->getVisVertices().size();
	vat = std::vector<std::vector<Vector3r>>( num_frames+1, std::vector<Vector3r> (model->getTetModels()[0]->getVisVertices().size()));
	// std::cout << "visvert " << (int)model->getTetModels()[0]->getVisVertices().size() << std::end;
	vat_norm = std::vector<std::vector<Vector3r>>( num_frames+1, std::vector<Vector3r>(model->getParticles().size()));
	default_pos.setZero(3, a);
	for(int i=0; i<a; i++){
		default_pos.col(i) = model->getTetModels()[0]->getVisVertices().getPosition(i);
	}
		//NO RENDER SIMULATION
	if(!if_render){
		for(int i=0; i< num_frames; i++){
			timeStep();
		}
		std::cout << "finished..." << std::endl;
		std::cout << std::endl;
		return 0;
	}

	glutMainLoop ();

	base->cleanup();

	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();

	Simulation::getCurrent()->getModel()->cleanup();
	Simulation::getCurrent()->getTimeStep()->getCollisionDetection()->cleanup();

	readScene(false);
}

void timeStep ()
{
	if(image_itr > num_frames) {
		std::cout << "EXIT by num_Frames" << std::endl;
		std::cout << std::endl;
		// record_vat(vat, "/home/octan/Dropbox/vat.exr", num_frames+1, solver_.getPoints().cols(), vat_min, vat_max);
		Utilities::Timing::printAverageTimes();
		Utilities::Timing::printTimeSums();
		Utilities::Timing::printAverageTimes_F(cost_log);
		Utilities::Timing::printTimeSums_F(cost_log);
		// cv::imwrite("/home/octan/Dropbox/vat.exr", VAT);

		exit(1);
	}
	if(cam_okkake){
		camPos(1) = solver_.getPoints().col(0)(1);
		camLookat(1) = solver_.getPoints().col(0)(1);
		MiniGL::setViewport(40.0f, 0.1f, 500.0f, camPos, camLookat);
	}

	START_TIMING("Frame");

	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();

	START_TIMING("PrecomputeError");
	if(custom_bool3){
		solver_.init_alternative();
		for (unsigned int i = 0; i < precompute_substeps; i++){
			solver_.alt_solve(precompute_max_itr, frametime/precompute_substeps);
		}
	}
	STOP_TIMING_AVG;

	int dum_num;
	if(if_dummy_3) dum_num = 3;
	else dum_num = 2;
	int num_violate = 0;
	ParticleData &pd = model->getParticles();

	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	// solver_.advance_collision(max_itr, frametime);
	// for (unsigned int i = 0; i < substeps; i++)
	// {
	// 	START_TIMING("SimStep");
	// 	// Simulation::getCurrent()->getTimeStep()->step(*model);
	// 	solver_.solve(max_itr, frametime/substeps, true, true );
	// 	STOP_TIMING_AVG;
	// }

	START_TIMING("Simulation");
	if(if_dummy){
		if(substeps==1){
			if(if_dummy_3){
				if(image_itr%2 == 0){
					solver_.dummy_solve(frametime/substeps, false, true);
				}
				else if(image_itr%2 == 1){
					solver_.solve(max_itr, (frametime/substeps), false, true);
				}
				else{
					solver_.solve(max_itr, (frametime/substeps), false, true);
				}

			}
			else{
				if(image_itr%2 == 0){
					solver_.initDummy();
					solver_.dummy_solve(frametime/substeps, false, true);
				}
				else if(image_itr%2 == 1){
					solver_.solve(max_itr, (frametime/substeps), false, true);
					if(custom_bool)  num_violate = solver_.closeDummy(neighbors, k);
					dummy_violate << num_violate << ", ";
				}
			}
			record_velocities(o_vel, solver_.getVelocities());
			record_velocities(o_pos, solver_.getPoints());
		}
		else{
			if(adap_dummy&&!if_dummy_3){
				solver_.advance_collision(max_itr, frametime);
				bool phase_d= true;
				int adap_pj = 0;
				for(unsigned int i = 0; i < substeps; i++){
					if(phase_d && i+1 == substeps){
						break;
					}
					START_TIMING("SimStep");
					if(phase_d ){
						solver_.initDummy();

						for(int j=0; j<neo_dummy; j++) solver_.dummy_solve( (frametime/substeps ) / neo_dummy, false, true);
						record_velocities(o_vel, solver_.getVelocities());
						record_velocities(o_pos, solver_.getPoints());
						phase_d = false;
					}
					else{

						solver_.solve(max_itr, (frametime/substeps), false, true);
						adap_pj++;

						if(custom_bool) num_violate = solver_.closeDummy(neighbors, k);

						if( ((double)solver_.closeDummy_pers(k)) / solver_.getPoints().cols() > adap_rate && i+1 != substeps){
							std::cout << i << ": " << ((double)solver_.closeDummy_pers(k)) / solver_.getPoints().cols() << std::endl;
							solver_.solve(max_itr, (frametime/substeps), false, true);
							adap_pj++;
							i++;
						}
						phase_d = true;

						dummy_violate << num_violate << ", ";

					}

					STOP_TIMING_AVG;
					record_velocities(o_vel, solver_.getVelocities());
					record_velocities(o_pos, solver_.getPoints());
				}
				adap_steps << adap_pj << ", ";
			}
			else{
				solver_.advance_collision(max_itr, frametime);
				for(unsigned int i = 0; i < substeps; i++){
					START_TIMING("SimStep");
					if(i%dum_num == 0){
						solver_.initDummy();
						for(int j=0; j<neo_dummy; j++) solver_.dummy_solve( (frametime/substeps ) / neo_dummy, false, true);
					}
					else if(i%dum_num == 1){
						solver_.solve(max_itr, (frametime/substeps), false, true);
						if(custom_bool) num_violate = solver_.closeDummy(neighbors, k);
						dummy_violate << num_violate << ", ";
					}
					else if(i%dum_num == 2) solver_.solve(max_itr, frametime/substeps, false, true);
					STOP_TIMING_AVG;
					record_velocities(o_vel, solver_.getVelocities());
					record_velocities(o_pos, solver_.getPoints());
				}
			}

		}
	}
	else{
		if(adams_1){
			solver_.advance_collision(max_itr, frametime);
			for (unsigned int i = 0; i < substeps; i++)
			{
				START_TIMING("SimStep");
					// s1->process(max_itr, image_itr, frametime/substeps);//ここが一番スタンダード
					solver_.adams_solve_1(max_itr, frametime/substeps, false, true);//ここが一番スタンダード
				STOP_TIMING_AVG;
			}
			record_velocities(o_vel, solver_.getVelocities());
			record_velocities(o_pos, solver_.getPoints());

		}
		else{
			//通常営業
				solver_.advance_collision(max_itr, frametime);
				for (unsigned int i = 0; i < substeps; i++)
				{
					START_TIMING("SimStep");
						solver_.solve(max_itr, frametime/substeps, false, true);	//ここが一番スタンダード
					STOP_TIMING_AVG;
					record_velocities(o_vel, solver_.getVelocities());
					record_velocities(o_pos, solver_.getPoints());
				}
			//通常営業
		}
	}

	STOP_TIMING_AVG;

	START_TIMING("PostProcessing");

	const ShapeOp::Matrix3X p = solver_.getPoints();
	if(custom_bool3){
		const ShapeOp::Matrix3X p2 = solver_.getPoints2();
		float error = 0;
		for (int i = 0; i < (int) pd.size(); i++){
			error += (p2.col(i) - p.col(i)).norm();
		}
		precompute_error << error/(float)p.cols() << ", ";
	}

	#pragma omp for schedule(static)
	for (int i = 0; i < (int) pd.size(); i++) updatePJ(pd.getPosition(i), p, i);
	#pragma omp for schedule(static)
	for (int i = 0; i < (int) pd.size(); i++) updatePJ(pd.getVelocity(i), solver_.getVelocities(), i);

	// Update visualization models

	for (unsigned int i = 0; i < model->getTetModels().size(); i++)
	{
		model->getTetModels()[i]->updateMeshNormals(pd);
		model->getTetModels()[i]->updateVisMesh(pd);
	}
	for (unsigned int i = 0; i < model->getTriangleModels().size(); i++)
	{
		model->getTriangleModels()[i]->updateMeshNormals(pd);
	}
	//base->setValue(DemoBase::PAUSE, true);

	//OpenCV
	if(img_render&&if_render){
		MiniGL::display ();
		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);
		cv::imwrite("../output/"+idf+"/"+idf_itr+"/images/"+std::to_string(image_itr)+".png", image);
	}
	VertexData &vdVis = model->getTetModels()[0]->getVisVertices();
	for(int i=0; i<vdVis.size(); i++){
		vat[image_itr][i] =  vdVis.getPosition(i) - default_pos.col(i);
		if(image_itr == 0 && i==0){
			vat_min = (vdVis.getPosition(i)- default_pos.col(i)).minCoeff();
			vat_max = (vdVis.getPosition(i)- default_pos.col(i)).maxCoeff();
		}
		else{
			float bufmin = (vdVis.getPosition(i)- default_pos.col(i)).minCoeff();
			// std::cout << bufmin << ":" << pd.getPosition(i) << std::endl;
			float bufmax = (vdVis.getPosition(i)- default_pos.col(i)).maxCoeff();
			if(bufmin < vat_min) vat_min = bufmin;
			if(bufmax > vat_max) vat_max = bufmax;
		}
		default_pos.col(i) = vdVis.getPosition(i);
	}

	energy << (float)solver_.compute_energy() << ", ";

	TetModel::SurfaceMesh &VM = model->getTetModels()[0]->getVisMesh();
	// VertexData &vdVis = model->getTetModels()[0]->getVisVertices();

	if(output_obj) record__vismesh_obj(VM, vdVis, "../output/"+idf+"/"+idf_itr+"/objs/meshes"+std::to_string(image_itr)+".obj");
	// record_velocities(o_vel, solver_.getVelocities());
	// record_positions(o_pos, pd);
	image_itr++;

	STOP_TIMING_AVG;//PostProcessing
	STOP_TIMING_AVG;//Frame

}

void render()
{

	base->render();

	// float staticColor[4] = { 1.0f, 0.5f, 0.5f, 1 };
	// float staticColor2[4] = { 0.5f, 0.5f, 1.0f, 1 };
	// Matrix3X vel = solver_.getVelocities();
	// Matrix3X pos = solver_.getPoints();
	// for(int i=0; i<vel.cols(); i++){
	// 	if(!solver_.getViolate()[i]) MiniGL::drawVector(pos.col(i), pos.col(i) - 0.1*vel.col(i), 1, staticColor);
	// 	else MiniGL::drawVector(pos.col(i), pos.col(i) - 0.1*vel.col(i), 1, staticColor2);
	// }

}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (static_cast<Real>(0.005));

	SimulationModel *model = Simulation::getCurrent()->getModel();
	Simulation::getCurrent()->getTimeStep()->setCollisionDetection(*model, &cd);

	readScene(true);
}

void initTriangleModelConstraints()
{
	// init constraints
	SimulationModel *model = Simulation::getCurrent()->getModel();
	for (unsigned int cm = 0; cm < model->getTriangleModels().size(); cm++)
	{
		const unsigned int offset = model->getTriangleModels()[cm]->getIndexOffset();
		if (clothSimulationMethod == 1)
		{
			const unsigned int nEdges = model->getTriangleModels()[cm]->getParticleMesh().numEdges();
			const IndexedFaceMesh::Edge *edges = model->getTriangleModels()[cm]->getParticleMesh().getEdges().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const unsigned int v1 = edges[i].m_vert[0] + offset;
				const unsigned int v2 = edges[i].m_vert[1] + offset;

				model->addDistanceConstraint(v1, v2);
			}
		}
		else if (clothSimulationMethod == 2)
		{

			TriangleModel::ParticleMesh &mesh = model->getTriangleModels()[cm]->getParticleMesh();
			const unsigned int *tris = mesh.getFaces().data();
			const unsigned int nFaces = mesh.numFaces();
			for (unsigned int i = 0; i < nFaces; i++)
			{
				const unsigned int v1 = tris[3 * i] + offset;
				const unsigned int v2 = tris[3 * i + 1] + offset;
				const unsigned int v3 = tris[3 * i + 2] + offset;
				model->addFEMTriangleConstraint(v1, v2, v3);
			}
		}
		else if (clothSimulationMethod == 3)
		{
			TriangleModel::ParticleMesh &mesh = model->getTriangleModels()[cm]->getParticleMesh();
			const unsigned int *tris = mesh.getFaces().data();
			const unsigned int nFaces = mesh.numFaces();
			for (unsigned int i = 0; i < nFaces; i++)
			{
				const unsigned int v1 = tris[3 * i] + offset;
				const unsigned int v2 = tris[3 * i + 1] + offset;
				const unsigned int v3 = tris[3 * i + 2] + offset;
				model->addStrainTriangleConstraint(v1, v2, v3);
			}
		}
		if (bendingMethod != 0)
		{
			TriangleModel::ParticleMesh &mesh = model->getTriangleModels()[cm]->getParticleMesh();
			unsigned int nEdges = mesh.numEdges();
			const TriangleModel::ParticleMesh::Edge *edges = mesh.getEdges().data();
			const unsigned int *tris = mesh.getFaces().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const int tri1 = edges[i].m_face[0];
				const int tri2 = edges[i].m_face[1];
				if ((tri1 != 0xffffffff) && (tri2 != 0xffffffff))
				{
					// Find the triangle points which do not lie on the axis
					const int axisPoint1 = edges[i].m_vert[0];
					const int axisPoint2 = edges[i].m_vert[1];
					int point1 = -1;
					int point2 = -1;
					for (int j = 0; j < 3; j++)
					{
						if ((tris[3 * tri1 + j] != axisPoint1) && (tris[3 * tri1 + j] != axisPoint2))
						{
							point1 = tris[3 * tri1 + j];
							break;
						}
					}
					for (int j = 0; j < 3; j++)
					{
						if ((tris[3 * tri2 + j] != axisPoint1) && (tris[3 * tri2 + j] != axisPoint2))
						{
							point2 = tris[3 * tri2 + j];
							break;
						}
					}
					if ((point1 != -1) && (point2 != -1))
					{
						const unsigned int vertex1 = point1 + offset;
						const unsigned int vertex2 = point2 + offset;
						const unsigned int vertex3 = edges[i].m_vert[0] + offset;
						const unsigned int vertex4 = edges[i].m_vert[1] + offset;
						if (bendingMethod == 1)
							model->addDihedralConstraint(vertex1, vertex2, vertex3, vertex4);
						else if (bendingMethod == 2)
							model->addIsometricBendingConstraint(vertex1, vertex2, vertex3, vertex4);
					}
				}
			}
		}
	}
}

void initTetModelConstraints()
{
	// init constraints
	SimulationModel *model = Simulation::getCurrent()->getModel();
	for (unsigned int cm = 0; cm < model->getTetModels().size(); cm++)
	{
		const unsigned int offset = model->getTetModels()[cm]->getIndexOffset();
		const unsigned int nTets = model->getTetModels()[cm]->getParticleMesh().numTets();
		const unsigned int *tets = model->getTetModels()[cm]->getParticleMesh().getTets().data();
		const IndexedTetMesh::VertexTets *vTets = model->getTetModels()[cm]->getParticleMesh().getVertexTets().data();
		if (solidSimulationMethod == 1)
		{
			const unsigned int offset = model->getTetModels()[cm]->getIndexOffset();
			const unsigned int nEdges = model->getTetModels()[cm]->getParticleMesh().numEdges();
			const IndexedTetMesh::Edge *edges = model->getTetModels()[cm]->getParticleMesh().getEdges().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const unsigned int v1 = edges[i].m_vert[0] + offset;
				const unsigned int v2 = edges[i].m_vert[1] + offset;

				model->addDistanceConstraint(v1, v2);
			}

			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i] + offset;
				const unsigned int v2 = tets[4 * i + 1] + offset;
				const unsigned int v3 = tets[4 * i + 2] + offset;
				const unsigned int v4 = tets[4 * i + 3] + offset;

				model->addVolumeConstraint(v1, v2, v3, v4);
			}
		}
		else if (solidSimulationMethod == 2)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i] + offset;
				const unsigned int v2 = tets[4 * i + 1] + offset;
				const unsigned int v3 = tets[4 * i + 2] + offset;
				const unsigned int v4 = tets[4 * i + 3] + offset;

				model->addFEMTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (solidSimulationMethod == 3)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i] + offset;
				const unsigned int v2 = tets[4 * i + 1] + offset;
				const unsigned int v3 = tets[4 * i + 2] + offset;
				const unsigned int v4 = tets[4 * i + 3] + offset;

				model->addStrainTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (solidSimulationMethod == 4)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v[4] = { tets[4 * i] + offset,
											tets[4 * i + 1] + offset,
											tets[4 * i + 2] + offset,
											tets[4 * i + 3] + offset };
				// Important: Divide position correction by the number of clusters
				// which contain the vertex.
				const unsigned int nc[4] = { vTets[v[0]].m_numTets, vTets[v[1]].m_numTets, vTets[v[2]].m_numTets, vTets[v[3]].m_numTets };
				model->addShapeMatchingConstraint(4, v, nc);
			}
		}
	}
}

void loadObj(const std::string &filename, VertexData &vd, IndexedFaceMesh &mesh, const Vector3r &scale)
{
	std::vector<OBJLoader::Vec3f> x;
	std::vector<OBJLoader::Vec3f> normals;
	std::vector<OBJLoader::Vec2f> texCoords;
	std::vector<MeshFaceIndices> faces;
	OBJLoader::Vec3f s = { (float)scale[0], (float)scale[1], (float)scale[2] };
	OBJLoader::loadObj(filename, &x, &faces, &normals, &texCoords, s);
	mesh.release();
	const unsigned int nPoints = (unsigned int)x.size();
	const unsigned int nFaces = (unsigned int)faces.size();
	const unsigned int nTexCoords = (unsigned int)texCoords.size();
	mesh.initMesh(nPoints, nFaces * 2, nFaces);
	vd.reserve(nPoints);
	for (unsigned int i = 0; i < nPoints; i++)
	{
		vd.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
	}
	for (unsigned int i = 0; i < nTexCoords; i++)
	{
		mesh.addUV(texCoords[i][0], texCoords[i][1]);
	}
	for (unsigned int i = 0; i < nFaces; i++)
	{
		// Reduce the indices by one
		int posIndices[3];
		int texIndices[3];
		for (int j = 0; j < 3; j++)
		{
			posIndices[j] = faces[i].posIndices[j] - 1;
			if (nTexCoords > 0)
			{
				texIndices[j] = faces[i].texIndices[j] - 1;
				mesh.addUVIndex(texIndices[j]);
			}
		}

		mesh.addFace(&posIndices[0]);
	}
	mesh.buildNeighbors();

	mesh.updateNormals(vd, 0);
	mesh.updateVertexNormals(vd);

	LOG_INFO << "Number of triangles: " << nFaces;
	LOG_INFO << "Number of vertices: " << nPoints;
}

CubicSDFCollisionDetection::GridPtr generateSDF(const std::string &modelFile, const std::string &collisionObjectFileName, const Eigen::Matrix<unsigned int, 3, 1> &resolutionSDF,
	VertexData &vd, IndexedFaceMesh &mesh)
{
	const std::string basePath = FileSystem::getFilePath(base->getSceneFile());
	const string cachePath = basePath + "/Cache";
	const std::string modelFileName = FileSystem::getFileNameWithExt(modelFile);
	CubicSDFCollisionDetection::GridPtr distanceField;

	if (collisionObjectFileName == "")
	{
		std::string md5FileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + ".md5");
		string md5Str = FileSystem::getFileMD5(modelFile);
		bool md5 = false;
		if (FileSystem::fileExists(md5FileName))
			md5 = FileSystem::checkMD5(md5Str, md5FileName);

		// check MD5 if cache file is available
		const string resStr = to_string(resolutionSDF[0]) + "_" + to_string(resolutionSDF[1]) + "_" + to_string(resolutionSDF[2]);
		const string sdfFileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + "_" + resStr + ".csdf");
		bool foundCacheFile = FileSystem::fileExists(sdfFileName);

		if (foundCacheFile && md5)
		{
			LOG_INFO << "Load cached SDF: " << sdfFileName;
			distanceField = std::make_shared<CubicSDFCollisionDetection::Grid>(sdfFileName);
		}
		else
		{
			std::vector<unsigned int> &faces = mesh.getFaces();
			const unsigned int nFaces = mesh.numFaces();

#ifdef USE_DOUBLE
			Discregrid::TriangleMesh sdfMesh(&vd.getPosition(0)[0], faces.data(), vd.size(), nFaces);
#else
			// if type is float, copy vector to double vector
			std::vector<double> doubleVec;
			doubleVec.resize(3 * vd.size());
			for (unsigned int i = 0; i < vd.size(); i++)
				for (unsigned int j = 0; j < 3; j++)
					doubleVec[3 * i + j] = vd.getPosition(i)[j];
			Discregrid::TriangleMesh sdfMesh(&doubleVec[0], faces.data(), vd.size(), nFaces);
#endif
			Discregrid::MeshDistance md(sdfMesh);
			Eigen::AlignedBox3d domain;
			for (auto const& x : sdfMesh.vertices())
			{
				domain.extend(x);
			}
			domain.max() += 0.1 * Eigen::Vector3d::Ones();
			domain.min() -= 0.1 * Eigen::Vector3d::Ones();

			LOG_INFO << "Set SDF resolution: " << resolutionSDF[0] << ", " << resolutionSDF[1] << ", " << resolutionSDF[2];
			distanceField = std::make_shared<CubicSDFCollisionDetection::Grid>(domain, std::array<unsigned int, 3>({ resolutionSDF[0], resolutionSDF[1], resolutionSDF[2] }));
			auto func = Discregrid::DiscreteGrid::ContinuousFunction{};
			func = [&md](Eigen::Vector3d const& xi) {return md.signedDistanceCached(xi); };
			LOG_INFO << "Generate SDF for " << modelFile;
			distanceField->addFunction(func, true);
			if (FileSystem::makeDir(cachePath) == 0)
			{
				LOG_INFO << "Save SDF: " << sdfFileName;
				distanceField->save(sdfFileName);
				FileSystem::writeMD5File(modelFile, md5FileName);
			}
		}
	}
	else
	{
		std::string fileName = collisionObjectFileName;
		if (FileSystem::isRelativePath(fileName))
		{
			fileName = FileSystem::normalizePath(basePath + "/" + fileName);
		}
		LOG_INFO << "Load SDF: " << fileName;
		distanceField = std::make_shared<CubicSDFCollisionDetection::Grid>(fileName);
	}
	return distanceField;
}

/** Create the rigid body model
*/
void readScene(const bool readFile)
{
	SimulationModel *model = Simulation::getCurrent()->getModel();
	SimulationModel::RigidBodyVector &rb = model->getRigidBodies();
	SimulationModel::TriangleModelVector &triModels = model->getTriangleModels();
	SimulationModel::TetModelVector &tetModels = model->getTetModels();
	SimulationModel::ConstraintVector &constraints = model->getConstraints();

	if (readFile)
	{
		base->cleanup();
		base->readScene();
	}
	SceneLoader::SceneData &data = base->getSceneData();

	camPos = data.m_camPosition;
	camLookat = data.m_camLookat;

	TimeManager::getCurrent()->setTimeStepSize(data.m_timeStepSize);

	if (readFile && (data.m_triangleModelSimulationMethod != -1))
		clothSimulationMethod = data.m_triangleModelSimulationMethod;
	if (readFile && (data.m_tetModelSimulationMethod != -1))
		solidSimulationMethod = data.m_tetModelSimulationMethod;
	if (readFile && (data.m_triangleModelBendingMethod != -1))
		bendingMethod = data.m_triangleModelBendingMethod;
	cd.setTolerance(data.m_contactTolerance);
	model->setContactStiffnessRigidBody(data.m_contactStiffnessRigidBody);
	model->setContactStiffnessParticleRigidBody(data.m_contactStiffnessParticleRigidBody);

	model->setValue<Real>(SimulationModel::CLOTH_BENDING_STIFFNESS, data.m_cloth_bendingStiffness);
	model->setValue<Real>(SimulationModel::CLOTH_STIFFNESS_XX, data.m_cloth_xxStiffness);
	model->setValue<Real>(SimulationModel::CLOTH_STIFFNESS_YY, data.m_cloth_yyStiffness);
	model->setValue<Real>(SimulationModel::CLOTH_STIFFNESS_XY, data.m_cloth_xyStiffness);
	model->setValue<Real>(SimulationModel::CLOTH_POISSON_RATIO_XY, data.m_cloth_xyPoissonRatio);
	model->setValue<Real>(SimulationModel::CLOTH_POISSON_RATIO_YX, data.m_cloth_yxPoissonRatio);
	model->setValue<bool>(SimulationModel::CLOTH_NORMALIZE_STRETCH, data.m_cloth_normalizeStretch);
	model->setValue<bool>(SimulationModel::CLOTH_NORMALIZE_SHEAR, data.m_cloth_normalizeShear);

	model->setValue<Real>(SimulationModel::SOLID_STIFFNESS, data.m_solid_stiffness);
	model->setValue<Real>(SimulationModel::SOLID_POISSON_RATIO, data.m_solid_poissonRatio);
	model->setValue<bool>(SimulationModel::SOLID_NORMALIZE_STRETCH, data.m_solid_normalizeStretch);
	model->setValue<bool>(SimulationModel::SOLID_NORMALIZE_SHEAR, data.m_solid_normalizeShear);

	//////////////////////////////////////////////////////////////////////////
	// rigid bodies
	//////////////////////////////////////////////////////////////////////////

	// map file names to loaded geometry to prevent multiple imports of same files
	std::map<std::string, pair<VertexData, IndexedFaceMesh>> objFiles;
	std::map<std::string, CubicSDFCollisionDetection::GridPtr> distanceFields;
	for (unsigned int i = 0; i < data.m_rigidBodyData.size(); i++)
	{
		SceneLoader::RigidBodyData &rbd = data.m_rigidBodyData[i];

		// Check if already loaded
		rbd.m_modelFile = FileSystem::normalizePath(rbd.m_modelFile);
		if (objFiles.find(rbd.m_modelFile) == objFiles.end())
		{
			IndexedFaceMesh mesh;
			VertexData vd;
			loadObj(rbd.m_modelFile, vd, mesh, Vector3r::Ones());
			objFiles[rbd.m_modelFile] = { vd, mesh };
		}

		const std::string basePath = FileSystem::getFilePath(base->getSceneFile());
		const string cachePath = basePath + "/Cache";
		const string resStr = to_string(rbd.m_resolutionSDF[0]) + "_" + to_string(rbd.m_resolutionSDF[1]) + "_" + to_string(rbd.m_resolutionSDF[2]);
		const std::string modelFileName = FileSystem::getFileNameWithExt(rbd.m_modelFile);
		const string sdfFileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + "_" + resStr + ".csdf");

		std::string sdfKey = rbd.m_collisionObjectFileName;
		if (sdfKey == "")
		{
			sdfKey = sdfFileName;
		}
		if (distanceFields.find(sdfKey) == distanceFields.end())
		{
			// Generate SDF
			if (rbd.m_collisionObjectType == SceneLoader::SDF)
			{
				if (rbd.m_collisionObjectFileName == "")
				{
					std::string md5FileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + ".md5");
					string md5Str = FileSystem::getFileMD5(rbd.m_modelFile);
					bool md5 = false;
					if (FileSystem::fileExists(md5FileName))
						md5 = FileSystem::checkMD5(md5Str, md5FileName);

					// check MD5 if cache file is available
					const string resStr = to_string(rbd.m_resolutionSDF[0]) + "_" + to_string(rbd.m_resolutionSDF[1]) + "_" + to_string(rbd.m_resolutionSDF[2]);
					const string sdfFileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + "_" + resStr + ".csdf");
					bool foundCacheFile = FileSystem::fileExists(sdfFileName);

					if (foundCacheFile && md5)
					{
						LOG_INFO << "Load cached SDF: " << sdfFileName;
						distanceFields[sdfFileName] = std::make_shared<CubicSDFCollisionDetection::Grid>(sdfFileName);
					}
					else
					{
						VertexData &vd = objFiles[rbd.m_modelFile].first;
						IndexedFaceMesh &mesh = objFiles[rbd.m_modelFile].second;

						std::vector<unsigned int> &faces = mesh.getFaces();
						const unsigned int nFaces = mesh.numFaces();

#ifdef USE_DOUBLE
						Discregrid::TriangleMesh sdfMesh(&vd.getPosition(0)[0], faces.data(), vd.size(), nFaces);
#else
						// if type is float, copy vector to double vector
						std::vector<double> doubleVec;
						doubleVec.resize(3 * vd.size());
						for (unsigned int i = 0; i < vd.size(); i++)
							for (unsigned int j = 0; j < 3; j++)
								doubleVec[3 * i + j] = vd.getPosition(i)[j];
						Discregrid::TriangleMesh sdfMesh(&doubleVec[0], faces.data(), vd.size(), nFaces);
#endif
						Discregrid::MeshDistance md(sdfMesh);
						Eigen::AlignedBox3d domain;
						for (auto const& x : sdfMesh.vertices())
						{
							domain.extend(x);
						}
						domain.max() += 0.1 * Eigen::Vector3d::Ones();
						domain.min() -= 0.1 * Eigen::Vector3d::Ones();

						LOG_INFO << "Set SDF resolution: " << rbd.m_resolutionSDF[0] << ", " << rbd.m_resolutionSDF[1] << ", " << rbd.m_resolutionSDF[2];
						distanceFields[sdfFileName] = std::make_shared<CubicSDFCollisionDetection::Grid>(domain, std::array<unsigned int, 3>({ rbd.m_resolutionSDF[0], rbd.m_resolutionSDF[1], rbd.m_resolutionSDF[2] }));
						auto func = Discregrid::DiscreteGrid::ContinuousFunction{};
						func = [&md](Eigen::Vector3d const& xi) {return md.signedDistanceCached(xi); };
						LOG_INFO << "Generate SDF for " << rbd.m_modelFile;
						distanceFields[sdfFileName]->addFunction(func, true);
						if (FileSystem::makeDir(cachePath) == 0)
						{
							LOG_INFO << "Save SDF: " << sdfFileName;
							distanceFields[sdfFileName]->save(sdfFileName);
							FileSystem::writeMD5File(rbd.m_modelFile, md5FileName);
						}
					}
				}
				else
				{
					std::string fileName = rbd.m_collisionObjectFileName;
					if (FileSystem::isRelativePath(fileName))
					{
						fileName = FileSystem::normalizePath(basePath + "/" + fileName);
					}
					LOG_INFO << "Load SDF: " << fileName;
					distanceFields[rbd.m_collisionObjectFileName] = std::make_shared<CubicSDFCollisionDetection::Grid>(fileName);
				}
			}
		}
	}
	std::cout << "before Tet" << std::endl;
	for (unsigned int i = 0; i < data.m_tetModelData.size(); i++)
	{
		const SceneLoader::TetModelData &tmd = data.m_tetModelData[i];

		// Check if already loaded
		if ((tmd.m_modelFileVis != "") &&
			(objFiles.find(tmd.m_modelFileVis) == objFiles.end()))
		{
			IndexedFaceMesh mesh;
			VertexData vd;
			std::cout << "b" << std::endl;
			loadObj(FileSystem::normalizePath(tmd.m_modelFileVis), vd, mesh, Vector3r::Ones());
			std::cout << "b" << std::endl;
			objFiles[tmd.m_modelFileVis] = { vd, mesh };
		}

		const std::string basePath = FileSystem::getFilePath(base->getSceneFile());
		const string cachePath = basePath + "/Cache";
		const string resStr = to_string(tmd.m_resolutionSDF[0]) + "_" + to_string(tmd.m_resolutionSDF[1]) + "_" + to_string(tmd.m_resolutionSDF[2]);
		const std::string modelFileName = FileSystem::getFileNameWithExt(tmd.m_modelFileVis);
		const string sdfFileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + "_" + resStr + ".csdf");

		std::string sdfKey = tmd.m_collisionObjectFileName;
		if (sdfKey == "")
		{
			sdfKey = sdfFileName;
		}
		if (distanceFields.find(sdfKey) == distanceFields.end())
		{
			// Generate SDF
			if (tmd.m_collisionObjectType == SceneLoader::SDF)
			{
				VertexData &vd = objFiles[tmd.m_modelFileVis].first;
				IndexedFaceMesh &mesh = objFiles[tmd.m_modelFileVis].second;

				CubicSDFCollisionDetection::GridPtr distanceField = generateSDF(tmd.m_modelFileVis, tmd.m_collisionObjectFileName, tmd.m_resolutionSDF, vd, mesh);

				if (tmd.m_collisionObjectFileName == "")
					distanceFields[sdfFileName] = distanceField;
				else
					distanceFields[tmd.m_collisionObjectFileName] = distanceField;
			}
		}
	}

	rb.resize(data.m_rigidBodyData.size());
	std::map<unsigned int, unsigned int> id_index;
	for (unsigned int i = 0; i < data.m_rigidBodyData.size(); i++)
	{
		const SceneLoader::RigidBodyData &rbd = data.m_rigidBodyData[i];

		if (objFiles.find(rbd.m_modelFile) == objFiles.end())
			continue;

		id_index[rbd.m_id] = i;

		VertexData &vd = objFiles[rbd.m_modelFile].first;
		IndexedFaceMesh &mesh = objFiles[rbd.m_modelFile].second;

		rb[i] = new RigidBody();
		rb[i]->initBody(rbd.m_density,
			rbd.m_x,
			rbd.m_q,
			vd, mesh,
			rbd.m_scale);

		if (!rbd.m_isDynamic)
			rb[i]->setMass(0.0);
		else
		{
			rb[i]->setVelocity(rbd.m_v);
			rb[i]->setAngularVelocity(rbd.m_omega);
		}
		rb[i]->setRestitutionCoeff(rbd.m_restitutionCoeff);
		rb[i]->setFrictionCoeff(rbd.m_frictionCoeff);

		const std::vector<Vector3r> *vertices = rb[i]->getGeometry().getVertexDataLocal().getVertices();
		const unsigned int nVert = static_cast<unsigned int>(vertices->size());

		switch (rbd.m_collisionObjectType)
		{
			case SceneLoader::No_Collision_Object: break;
			case SceneLoader::Sphere:
				cd.addCollisionSphere(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, rbd.m_collisionObjectScale[0], rbd.m_testMesh, rbd.m_invertSDF);
				break;
			case SceneLoader::Box:
				cd.addCollisionBox(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, rbd.m_collisionObjectScale, rbd.m_testMesh, rbd.m_invertSDF);
				break;
			case SceneLoader::Cylinder:
				cd.addCollisionCylinder(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, rbd.m_collisionObjectScale.head<2>(), rbd.m_testMesh, rbd.m_invertSDF);
				break;
			case SceneLoader::Torus:
				cd.addCollisionTorus(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, rbd.m_collisionObjectScale.head<2>(), rbd.m_testMesh, rbd.m_invertSDF);
				break;
			case SceneLoader::HollowSphere:
				cd.addCollisionHollowSphere(i, PBD::CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, rbd.m_collisionObjectScale[0], rbd.m_thicknessSDF, rbd.m_testMesh, rbd.m_invertSDF);
				break;
			case SceneLoader::HollowBox:
				cd.addCollisionHollowBox(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, rbd.m_collisionObjectScale, rbd.m_thicknessSDF, rbd.m_testMesh, rbd.m_invertSDF);
				break;
			case SceneLoader::SDF:
			{
				if (rbd.m_collisionObjectFileName == "")
				{
					const std::string basePath = FileSystem::getFilePath(base->getSceneFile());
					const string cachePath = basePath + "/Cache";
					const string resStr = to_string(rbd.m_resolutionSDF[0]) + "_" + to_string(rbd.m_resolutionSDF[1]) + "_" + to_string(rbd.m_resolutionSDF[2]);
					const std::string modelFileName = FileSystem::getFileNameWithExt(rbd.m_modelFile);
					const string sdfFileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + "_" + resStr + ".csdf");
					cd.addCubicSDFCollisionObject(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, distanceFields[sdfFileName], rbd.m_collisionObjectScale, rbd.m_testMesh, rbd.m_invertSDF);
				}
				else
					cd.addCubicSDFCollisionObject(i, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, distanceFields[rbd.m_collisionObjectFileName], rbd.m_collisionObjectScale, rbd.m_testMesh, rbd.m_invertSDF);
				break;
			}
		}
	}

	//////////////////////////////////////////////////////////////////////////
	// triangle models
	//////////////////////////////////////////////////////////////////////////

	// map file names to loaded geometry to prevent multiple imports of same files
	std::map<std::string, pair<VertexData, IndexedFaceMesh>> triFiles;
	for (unsigned int i = 0; i < data.m_triangleModelData.size(); i++)
	{
		const SceneLoader::TriangleModelData &tmd = data.m_triangleModelData[i];

		// Check if already loaded
		if (triFiles.find(tmd.m_modelFile) == triFiles.end())
		{
			IndexedFaceMesh mesh;
			VertexData vd;
			loadObj(FileSystem::normalizePath(tmd.m_modelFile), vd, mesh, Vector3r::Ones());
			triFiles[tmd.m_modelFile] = { vd, mesh };
		}
	}

	triModels.reserve(data.m_triangleModelData.size());
	std::map<unsigned int, unsigned int> tm_id_index;
	for (unsigned int i = 0; i < data.m_triangleModelData.size(); i++)
	{
		const SceneLoader::TriangleModelData &tmd = data.m_triangleModelData[i];

		if (triFiles.find(tmd.m_modelFile) == triFiles.end())
			continue;

		tm_id_index[tmd.m_id] = i;

		VertexData vd = triFiles[tmd.m_modelFile].first;
		IndexedFaceMesh &mesh = triFiles[tmd.m_modelFile].second;

		const Matrix3r R = tmd.m_q.matrix();
		for (unsigned int j = 0; j < vd.size(); j++)
		{
			vd.getPosition(j) = R * (vd.getPosition(j).cwiseProduct(tmd.m_scale)) + tmd.m_x;
		}

		model->addTriangleModel(vd.size(), mesh.numFaces(), &vd.getPosition(0), mesh.getFaces().data(), mesh.getUVIndices(), mesh.getUVs());

		TriangleModel *tm = triModels[triModels.size() - 1];
		ParticleData &pd = model->getParticles();
		unsigned int offset = tm->getIndexOffset();

		for (unsigned int j = 0; j < tmd.m_staticParticles.size(); j++)
		{
			const unsigned int index = tmd.m_staticParticles[j] + offset;
			pd.setMass(index, 0.0);
		}

		tm->setRestitutionCoeff(tmd.m_restitutionCoeff);
		tm->setFrictionCoeff(tmd.m_frictionCoeff);
	}

	initTriangleModelConstraints();

	//////////////////////////////////////////////////////////////////////////
	// tet models
	//////////////////////////////////////////////////////////////////////////

	// map file names to loaded geometry to prevent multiple imports of same files
	std::map<pair<string, string>, pair<vector<Vector3r>, vector<unsigned int>>> tetFiles;
	for (unsigned int i = 0; i < data.m_tetModelData.size(); i++)
	{
		const SceneLoader::TetModelData &tmd = data.m_tetModelData[i];

		// Check if already loaded
		pair<string, string> fileNames = { tmd.m_modelFileNodes, tmd.m_modelFileElements };
		if (tetFiles.find(fileNames) == tetFiles.end())
		{
			vector<Vector3r> vertices;
			vector<unsigned int> tets;
			TetGenLoader::loadTetgenModel(FileSystem::normalizePath(tmd.m_modelFileNodes), FileSystem::normalizePath(tmd.m_modelFileElements), vertices, tets);
			tetFiles[fileNames] = { vertices, tets };
		}
	}

	tetModels.reserve(data.m_tetModelData.size());
	std::map<unsigned int, unsigned int> tm_id_index2;
	for (unsigned int i = 0; i < data.m_tetModelData.size(); i++)
	{
		const SceneLoader::TetModelData &tmd = data.m_tetModelData[i];

		pair<string, string> fileNames = { tmd.m_modelFileNodes, tmd.m_modelFileElements };
		auto geo = tetFiles.find(fileNames);
		if (geo == tetFiles.end())
			continue;

		tm_id_index2[tmd.m_id] = i;

		vector<Vector3r> vertices = geo->second.first;
		vector<unsigned int> &tets = geo->second.second;

		const Matrix3r R = tmd.m_q.matrix();
		for (unsigned int j = 0; j < vertices.size(); j++)
		{
			vertices[j] = R * (vertices[j].cwiseProduct(tmd.m_scale)) + tmd.m_x;
		}

		model->addTetModel((unsigned int)vertices.size(), (unsigned int)tets.size() / 4, vertices.data(), tets.data());

		TetModel *tm = tetModels[tetModels.size() - 1];
		ParticleData &pd = model->getParticles();
		unsigned int offset = tm->getIndexOffset();

		tm->setInitialX(tmd.m_x);
		tm->setInitialR(R);
		tm->setInitialScale(tmd.m_scale);

		for (unsigned int j = 0; j < tmd.m_staticParticles.size(); j++)
		{
			const unsigned int index = tmd.m_staticParticles[j] + offset;
			pd.setMass(index, 0.0);
		}

		// read visualization mesh
		if (tmd.m_modelFileVis != "")
		{
			if (objFiles.find(tmd.m_modelFileVis) != objFiles.end())
			{
				IndexedFaceMesh &visMesh = tm->getVisMesh();
				VertexData &vdVis = tm->getVisVertices();
				vdVis = objFiles[tmd.m_modelFileVis].first;
				visMesh = objFiles[tmd.m_modelFileVis].second;

				for (unsigned int j = 0; j < vdVis.size(); j++)
					vdVis.getPosition(j) = R * (vdVis.getPosition(j).cwiseProduct(tmd.m_scale)) + tmd.m_x;

				tm->updateMeshNormals(pd);
				tm->attachVisMesh(pd);
				tm->updateVisMesh(pd);
			}
		}

		tm->setRestitutionCoeff(tmd.m_restitutionCoeff);
		tm->setFrictionCoeff(tmd.m_frictionCoeff);

		tm->updateMeshNormals(pd);
	}

	initTetModelConstraints();

	// init collision objects for deformable models
	ParticleData &pd = model->getParticles();
	for (unsigned int i = 0; i < data.m_triangleModelData.size(); i++)
	{
		TriangleModel *tm = triModels[i];
		unsigned int offset = tm->getIndexOffset();
		const unsigned int nVert = tm->getParticleMesh().numVertices();
		cd.addCollisionObjectWithoutGeometry(i, CollisionDetection::CollisionObject::TriangleModelCollisionObjectType, &pd.getPosition(offset), nVert, true);

	}
	for (unsigned int i = 0; i < data.m_tetModelData.size(); i++)
	{
		TetModel *tm = tetModels[i];
		unsigned int offset = tm->getIndexOffset();
		const unsigned int nVert = tm->getParticleMesh().numVertices();
		const IndexedTetMesh &tetMesh = tm->getParticleMesh();

		const SceneLoader::TetModelData &tmd = data.m_tetModelData[i];

		switch (tmd.m_collisionObjectType)
		{
		case SceneLoader::No_Collision_Object:
			cd.addCollisionObjectWithoutGeometry(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, true);
			break;
		case SceneLoader::Sphere:
			cd.addCollisionSphere(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, tmd.m_collisionObjectScale[0], tmd.m_testMesh, tmd.m_invertSDF);
			break;
		case SceneLoader::Box:
			cd.addCollisionBox(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, tmd.m_collisionObjectScale, tmd.m_testMesh, tmd.m_invertSDF);
			break;
		case SceneLoader::Cylinder:
			cd.addCollisionCylinder(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, tmd.m_collisionObjectScale.head<2>(), tmd.m_testMesh, tmd.m_invertSDF);
			break;
		case SceneLoader::Torus:
			cd.addCollisionTorus(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, tmd.m_collisionObjectScale.head<2>(), tmd.m_testMesh, tmd.m_invertSDF);
			break;
		case SceneLoader::HollowSphere:
			cd.addCollisionHollowSphere(i, PBD::CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, tmd.m_collisionObjectScale[0], tmd.m_thicknessSDF, tmd.m_testMesh, tmd.m_invertSDF);
			break;
		case SceneLoader::HollowBox:
			cd.addCollisionHollowBox(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, tmd.m_collisionObjectScale, tmd.m_thicknessSDF, tmd.m_testMesh, tmd.m_invertSDF);
			break;
		case SceneLoader::SDF:
		{
			if (tmd.m_collisionObjectFileName == "")
			{
				const std::string basePath = FileSystem::getFilePath(base->getSceneFile());
				const string cachePath = basePath + "/Cache";
				const string resStr = to_string(tmd.m_resolutionSDF[0]) + "_" + to_string(tmd.m_resolutionSDF[1]) + "_" + to_string(tmd.m_resolutionSDF[2]);
				const std::string modelFileName = FileSystem::getFileNameWithExt(tmd.m_modelFileVis);
				const string sdfFileName = FileSystem::normalizePath(cachePath + "/" + modelFileName + "_" + resStr + ".csdf");
				cd.addCubicSDFCollisionObject(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, distanceFields[sdfFileName], tmd.m_collisionObjectScale, tmd.m_testMesh, tmd.m_invertSDF);
			}
			else
				cd.addCubicSDFCollisionObject(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, distanceFields[tmd.m_collisionObjectFileName], tmd.m_collisionObjectScale, tmd.m_testMesh, tmd.m_invertSDF);
			break;
		}
		}
	}

	// init tet BVH
	std::vector<CollisionDetection::CollisionObject*> &collisionObjects = cd.getCollisionObjects();
	for (unsigned int k = 0; k < collisionObjects.size(); k++)
	{
		if (cd.isDistanceFieldCollisionObject(collisionObjects[k]) &&
			(collisionObjects[k]->m_bodyType == CollisionDetection::CollisionObject::TetModelCollisionObjectType))
		{
			const unsigned int modelIndex = collisionObjects[k]->m_bodyIndex;
			TetModel *tm = tetModels[modelIndex];
			const unsigned int offset = tm->getIndexOffset();
			const IndexedTetMesh &mesh = tm->getParticleMesh();

			((DistanceFieldCollisionDetection::DistanceFieldCollisionObject*) collisionObjects[k])->initTetBVH(&pd.getPosition(offset), mesh.numVertices(), mesh.getTets().data(), mesh.numTets(), data.m_contactTolerance);
		}
	}

	//////////////////////////////////////////////////////////////////////////
	// joints
	//////////////////////////////////////////////////////////////////////////

	for (unsigned int i = 0; i < data.m_ballJointData.size(); i++)
	{
		const SceneLoader::BallJointData &jd = data.m_ballJointData[i];
		model->addBallJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position);
	}

	for (unsigned int i = 0; i < data.m_ballOnLineJointData.size(); i++)
	{
		const SceneLoader::BallOnLineJointData &jd = data.m_ballOnLineJointData[i];
		model->addBallOnLineJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
	}

	for (unsigned int i = 0; i < data.m_hingeJointData.size(); i++)
	{
		const SceneLoader::HingeJointData &jd = data.m_hingeJointData[i];
		model->addHingeJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
	}

	for (unsigned int i = 0; i < data.m_universalJointData.size(); i++)
	{
		const SceneLoader::UniversalJointData &jd = data.m_universalJointData[i];
		model->addUniversalJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis[0], jd.m_axis[1]);
	}

	for (unsigned int i = 0; i < data.m_sliderJointData.size(); i++)
	{
		const SceneLoader::SliderJointData &jd = data.m_sliderJointData[i];
		model->addSliderJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_axis);
	}

	for (unsigned int i = 0; i < data.m_rigidBodyParticleBallJointData.size(); i++)
	{
		const SceneLoader::RigidBodyParticleBallJointData &jd = data.m_rigidBodyParticleBallJointData[i];
		model->addRigidBodyParticleBallJoint(id_index[jd.m_bodyID[0]], jd.m_bodyID[1]);
	}

	for (unsigned int i = 0; i < data.m_targetAngleMotorHingeJointData.size(); i++)
	{
		const SceneLoader::TargetAngleMotorHingeJointData &jd = data.m_targetAngleMotorHingeJointData[i];
		model->addTargetAngleMotorHingeJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
		((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
		((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
		((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
	}

	for (unsigned int i = 0; i < data.m_targetVelocityMotorHingeJointData.size(); i++)
	{
		const SceneLoader::TargetVelocityMotorHingeJointData &jd = data.m_targetVelocityMotorHingeJointData[i];
		model->addTargetVelocityMotorHingeJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position, jd.m_axis);
		((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
		((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
		((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
	}

	for (unsigned int i = 0; i < data.m_targetPositionMotorSliderJointData.size(); i++)
	{
		const SceneLoader::TargetPositionMotorSliderJointData &jd = data.m_targetPositionMotorSliderJointData[i];
		model->addTargetPositionMotorSliderJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_axis);
		((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
		((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
		((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
	}

	for (unsigned int i = 0; i < data.m_targetVelocityMotorSliderJointData.size(); i++)
	{
		const SceneLoader::TargetVelocityMotorSliderJointData &jd = data.m_targetVelocityMotorSliderJointData[i];
		model->addTargetVelocityMotorSliderJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_axis);
		((MotorJoint*)constraints[constraints.size() - 1])->setTarget(jd.m_target);
		((MotorJoint*)constraints[constraints.size() - 1])->setTargetSequence(jd.m_targetSequence);
		((MotorJoint*)constraints[constraints.size() - 1])->setRepeatSequence(jd.m_repeat);
	}

	for (unsigned int i = 0; i < data.m_damperJointData.size(); i++)
	{
		const SceneLoader::DamperJointData &jd = data.m_damperJointData[i];
		model->addDamperJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_axis, jd.m_stiffness);
	}

	for (unsigned int i = 0; i < data.m_rigidBodySpringData.size(); i++)
	{
		const SceneLoader::RigidBodySpringData &jd = data.m_rigidBodySpringData[i];
		model->addRigidBodySpring(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position1, jd.m_position2, jd.m_stiffness);
	}

	for (unsigned int i = 0; i < data.m_distanceJointData.size(); i++)
	{
		const SceneLoader::DistanceJointData &jd = data.m_distanceJointData[i];
		model->addDistanceJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position1, jd.m_position2);
	}
}

void TW_CALL setContactTolerance(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((DistanceFieldCollisionDetection*)clientData)->setTolerance(val);
}

void TW_CALL getContactTolerance(void *value, void *clientData)
{
	*(Real *)(value) = ((DistanceFieldCollisionDetection*)clientData)->getTolerance();
}

void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessRigidBody(val);
}

void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessRigidBody();
}

void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessParticleRigidBody(val);
}

void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessParticleRigidBody();
}

void TW_CALL setBendingMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getBendingMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setClothSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getClothSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setSolidSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;

	reset();
}

void TW_CALL getSolidSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}


// This file is part of ViViA, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/vivia/blob/master/LICENSE for details.

#include "vpSuperResDepthWarper.h"

#include "vpProjectParser.h"
#include "vpSuperResDepthViewer.h"

#include <super3d/depth/super_config.h>
#include <super3d/depth/flow_manip.h>
#include <super3d/depth/world_frustum.h>
#include <super3d/depth/cost_volume.h>
#include <super3d/depth/multiscale.h>
#include <super3d/depth/tv_refine_search.h>
#include <super3d/depth/weighted_dbw.h>

#include <tracking/refine_homography.h>
#include <video_transforms/super_res_utils.h>
#include <video_transforms/adjoint_image_op.h>
#include <video_transforms/warp_image.h>

#include <vil/vil_load.h>
#include <vil/vil_image_view.h>
#include <vil/vil_convert.h>
#include <vil/vil_crop.h>
#include <vil/vil_resample_bilin.h>
#include <vil/vil_save.h>
#include <vnl/vnl_inverse.h>

#include <vtkPoints.h>

#include <qtStlUtil.h>

#include <QDebug>

#include <boost/scoped_ptr.hpp>

//-----------------------------------------------------------------------------
vpSuperResDepthWarper::vpSuperResDepthWarper(
  const std::vector<vpgl_perspective_camera<double> >& cameras,
  boost::shared_ptr<super3d::config> cfg, std::string depthConfigFile,
  int cropi0, int cropj0, int cropni, int cropnj, int ref, double scaleFactor,
  double sensorSigma, double lambda, const std::string& cropId,
  vtkSmartPointer<vtkPolyData> polydata, boost::shared_ptr<bool> interrupt) :
    vpSuperResWarper(cropi0, cropj0, cropni, cropnj,
                     ref, scaleFactor, sensorSigma, interrupt)
{
  this->Cameras = cameras;
  this->Drm = 0;
  this->WorldSpace = 0;
  this->Config = cfg;
  this->DepthConfigFile = depthConfigFile;
  this->Iterations = cfg->get_value<unsigned int>("depth_iterations");
  this->PolyData = polydata;
  this->Lambda = lambda;
}

//-----------------------------------------------------------------------------
vpSuperResDepthWarper::~vpSuperResDepthWarper()
{
  if (this->Drm)
    {
    delete this->Drm;
    }

  if (this->WorldSpace)
    {
    delete this->WorldSpace;
    }
}

//-----------------------------------------------------------------------------
void vpSuperResDepthWarper::setPointsFromDepth(
  const vil_image_view<double>& depth)
{
  vtkPoints* pts = this->PolyData->GetPoints();
  vtkIdType index = 0;
  for (unsigned int j = 0; j < depth.nj(); ++j)
    {
    for (unsigned int i = 0; i < depth.ni(); ++i)
      {
      vnl_double_3 pt3d = this->WorldSpace->point_at_depth(i, j, depth(i, j));
      pts->SetPoint(index++, pt3d.data_block());
      }
    }
}

//-----------------------------------------------------------------------------
void vpSuperResDepthWarper::receiveData(
  super3d::depth_refinement_monitor::update_data d)
{
  emit progressUpdated(
    static_cast<int>((100.0 * d.num_iterations) / this->Iterations));
  this->setPointsFromDepth(d.current_result);
  emit depthRenderRequested();
}

//-----------------------------------------------------------------------------
bool vpSuperResDepthWarper::
process(std::vector<vil_image_view<double> >& frames,
        std::vector<vidtk::adjoint_image_ops_func<double> >& warps)
{
  try
    {
    // Normalize intensities between 0 and 1
    std::vector<vil_image_view<double> > greyFrames(frames.size());
    const double normalizer = 1.0 / 255.0;
    for (unsigned int i = 0; i < frames.size(); ++i)
      {
      vil_math_scale_values(frames[i], normalizer);
      if (frames[i].nplanes() == 1)
        {
        greyFrames[i].deep_copy(frames[i]);
        }
      else
        {
        vil_convert_planes_to_grey(frames[i], greyFrames[i]);
        }
      }

    std::vector<vpgl_perspective_camera<double> > scaledCameras;
    for (unsigned int i = 0; i < this->Cameras.size(); ++i)
      {
      scaledCameras.push_back(super3d::scale_camera(this->Cameras[i],
                                                    this->ScaleFactor));
      }

    vil_image_view<double> depth;
    if (!this->computeDepth(greyFrames, depth))
      {
      return false;
      }

    unsigned int dni =
      static_cast<unsigned int>(this->ScaleFactor * depth.ni());
    unsigned int dnj =
      static_cast<unsigned int>(this->ScaleFactor * depth.nj()) ;
    vil_image_view<double> temp;
    vil_resample_bilin(depth, temp, dni, dnj);
    depth = temp;

    int i0, ni, j0, nj;

    vpgl_perspective_camera<double> refCamera = scaledCameras[this->RefFrame];

    i0 = static_cast<int>(this->I0 * this->ScaleFactor);
    j0 = static_cast<int>(this->J0 * this->ScaleFactor);
    ni = static_cast<int>(this->Ni * this->ScaleFactor);
    nj = static_cast<int>(this->Nj * this->ScaleFactor);

    refCamera = super3d::crop_camera(refCamera, i0, j0);

    std::vector<vil_image_view<double> > weights(scaledCameras.size());

    for (unsigned int i = 0; i < scaledCameras.size(); ++i)
      {
      weights[i].set_size(dni, dnj);
      weights[i].fill(1.0);
      }

    std::vector<vil_image_view<double> > flows;
    super3d::compute_occluded_flows_from_depth(scaledCameras, refCamera, depth,
                                               flows);
    vidtk::crop_frames_and_flows(flows, frames, this->ScaleFactor, 3);
    this->createWarpsFromFlows(flows, frames, weights, warps);
    }
  catch (const super3d::config::cfg_exception& e)
    {
    emit statusChanged(e.what());
    emit progressUpdated(0);
    return false;
    }

  return true;
}

//-----------------------------------------------------------------------------
bool vpSuperResDepthWarper::computeDepth(
  std::vector<vil_image_view<double> >& frames, vil_image_view<double>& depth)
{
  try
    {
    emit statusChanged(tr("Estimating Depth Costs"));
    emit progressUpdated(0);

    frames[this->RefFrame] = vil_crop(frames[this->RefFrame],
                                      this->I0, this->Ni, this->J0, this->Nj);
    this->Cameras[this->RefFrame] =
      super3d::crop_camera(this->Cameras[this->RefFrame], this->I0, this->J0);

    double depthMin, depthMax;
    if (this->Config->is_set("depth_landmarks_path"))
      {
      std::string dir = this->DepthConfigFile.substr(0,
        this->DepthConfigFile.find_last_of("/\\"));
      dir = dir.append("/");
      std::string landmark_file =
        this->Config->get_value<std::string>("depth_landmarks_path");

      vpProjectParser::ConstructCompletePath(landmark_file, dir);
      qDebug() << "Using landmarks from" << qtString(landmark_file);
      if (!super3d::compute_depth_range(this->Cameras[this->RefFrame], 0,
                                        this->Ni, 0, this->Nj, landmark_file,
                                        depthMin, depthMax))
        {
        emit statusChanged("Not enough landmarks in selected region"
                           " to estimate depth range."
                           " Please consider a larger region.");
        emit progressUpdated(0);
        return false;
        }
      qDebug() << "Depth range:" << depthMin << depthMax;
      }
    else
      {
      depthMin = this->Config->get_value<double>("depth_min");
      depthMax = this->Config->get_value<double>("depth_max");
      }

    this->WorldSpace =
      new super3d::world_frustum(this->Cameras[this->RefFrame], depthMin,
                                 depthMax, this->Ni, this->Nj);

    unsigned int S = this->Config->get_value<unsigned int>("depth_num_slices");
    double theta0 = this->Config->get_value<double>("depth_theta_start");
    double theta_end = this->Config->get_value<double>("depth_theta_end");
    double beta = this->Config->get_value<double>("depth_beta");
    double gw_alpha = this->Config->get_value<double>("depth_gw_alpha");
    double epsilon = this->Config->get_value<double>("depth_epsilon");

    vil_image_view<double> g;
    vil_image_view<double> cost_volume;

    double iw = 0.0, gw = 0.0, cw = 0.0;
    gw = this->Config->get_value<double>("depth_gradient_weight");
    iw = this->Config->get_value<double>("depth_intensity_weight");

    super3d::compute_world_cost_volume(frames, this->Cameras, this->WorldSpace,
                                       this->RefFrame, S, cost_volume, iw, gw, cw);
    this->WorldSpace->compute_g(frames[this->RefFrame], g, gw_alpha, 1.0);
    emit progressUpdated(100);

    emit statusChanged(tr("Estimating Depth"));
    emit progressUpdated(0);

    depth.set_size(cost_volume.ni(), cost_volume.nj(), 1);

    boost::function<void (super3d::depth_refinement_monitor::update_data)> f;
    f = std::bind1st(std::mem_fun(&vpSuperResDepthWarper::receiveData), this);
    this->Drm = new super3d::depth_refinement_monitor(f, 20, this->Interrupted);

    super3d::refine_depth(cost_volume, g, depth, this->Iterations, theta0,
                          theta_end, this->Lambda, epsilon, this->Drm);
    if (*this->Interrupted)
      {
      return false;
      }

    this->setPointsFromDepth(depth);
    emit depthCompleted();
    double depthScale = depthMax - depthMin;
    vil_math_scale_and_offset_values(depth, depthScale, depthMin);
    }
  catch (const super3d::config::cfg_exception& e)
    {
    emit statusChanged(e.what());
    emit progressUpdated(0);
    return false;
    }

  return true;
}

//-----------------------------------------------------------------------------
void vpSuperResDepthWarper::createWarpsFromFlows(
  const std::vector<vil_image_view<double> >& flows,
  const std::vector<vil_image_view<double> >& frames,
  const std::vector<vil_image_view<double> >& weights,
  std::vector<vidtk::adjoint_image_ops_func<double> >& warps)
{
  assert(flows.size() == frames.size());
  bool downSampleAveraging =
    this->Config->get_value<bool>("down_sample_averaging");
  bool bicubicWarping = this->Config->get_value<bool>("bicubic_warping");

  warps.clear();
  warps.reserve(flows.size());
  for (unsigned int i=0; i<flows.size(); ++i)
    {
    warps.push_back(super3d::create_dbw_from_flow(flows[i],
                                                  weights[i],
                                                  frames[i].ni(),
                                                  frames[i].nj(),
                                                  frames[i].nplanes(),
                                                  this->ScaleFactor,
                                                  this->SensorSigma,
                                                  downSampleAveraging,
                                                  bicubicWarping));
    }
}


#include "MapDumper.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include <pcl/console/print.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/radius_outlier_removal.h>

#include <boost/thread.hpp>
#include <boost/filesystem.hpp>

void MapDumper::dumpTo(const string &dump_dir)
{
    if (m_cameraPosFile.length())
        initPoses();

    m_dumpDir = dump_dir;

    if (boost::filesystem::exists(m_dumpDir) && !boost::filesystem::is_directory(m_dumpDir)) {
            boost::filesystem::remove(m_dumpDir);
    }
    boost::filesystem::create_directories(m_dumpDir);

    using namespace boost::filesystem;

    int map_index = m_dumpStart;
    int cloud_index = 0;
    size_t total = m_cameraPoses.size() ? m_cameraPoses.size() : std::count_if(
        directory_iterator(m_mapDir),
        directory_iterator(),
        boost::bind(static_cast<bool(*)(const path&)>(is_regular_file), boost::bind(&directory_entry::path, _1))) / 2;

    boost::thread_group group;

    int cpu_cores = std::max(boost::thread::hardware_concurrency(), (unsigned)2);
    int count = 0;
    for (; map_index + m_dumpStep < total; map_index += m_dumpStep) {
        if (count == cpu_cores) {
            count = 0;
            group.join_all();
        }
        group.create_thread(boost::bind(&MapDumper::forEachMap, this, map_index));
        ++count;
    }
    group.join_all();
}

CloudTypePtr MapDumper::mapToCloud(std::vector<float> &vmap, std::vector<float> &nmap)
{
    assert(vmap.size() == nmap.size());

    CloudTypePtr cloud(new CloudType);
    cloud->height = 1;
    cloud->is_dense = true;
    cloud->points.reserve(vmap.size());

    int nan_count = 0;
    int invalid_norm_count = 0;
    int invalid_depth_count = 0;
    for (int row = 0; row < 480; ++row) {
        for (int col = 0; col < 640; ++col) {
            Eigen::Vector3f norm;
            PointType point;
            point.normal_x = nmap[640 * (0 * 480 + row) + col];
            if (pcl_isnan(point.normal_x)) {
                ++nan_count;
                continue;
            }
            point.normal_y = nmap[640 * (1 * 480 + row) + col];
            point.normal_z = nmap[640 * (2 * 480 + row) + col];

            point.x  = vmap[640 * (0 * 480 + row) + col];

            if (pcl_isnan(point.x)) {
                ++nan_count;
                continue;
            }
            point.y = vmap[640 * (1 * 480 + row) + col];
            point.z = vmap[640 * (2 * 480 + row) + col];
            if (point.z < m_minDepth || point.z > m_maxDepth) {
                ++invalid_depth_count;
                continue;
            }
            float thre_cos = cosf(m_normAngleThres * M_PI / 180);
            if (fabsf(point.normal_z) < thre_cos) {
                ++invalid_norm_count;
                continue;
            }
            if (point.normal_z > 0) {
                point.normal_x = -point.normal_x;
                point.normal_y = -point.normal_y;
                point.normal_z = -point.normal_z;
            }
            cloud->points.push_back(point);
        }
    }

    {
        boost::mutex::scoped_lock lock(m_outputMutex);
        PCL_INFO("\n\tCloud made with %d NaN, %d invalid depth, %d invalid norm points skipped!\n", nan_count, invalid_depth_count, invalid_norm_count);
    }

    cloud->width = cloud->points.size();
    return cloud;
}

void MapDumper::forEachMap(int index)
{
    char filename[1024] = { 0 };
    std::ifstream vertex_file;
    sprintf(filename, "%s/cloud_%d.vmap", m_mapDir.c_str(), index);
    vertex_file.open(filename, ios::in | ios::binary);
    if (!vertex_file.is_open()) {
        PCL_WARN("Can't open vertex map file : %s, skipped!", filename);
        return;
    }
    std::vector<float> point_data;
    point_data.resize(640 * 480 * 3);
    vertex_file.read((char *)&point_data[0], point_data.size() * sizeof(float));
    vertex_file.close();

    std::ifstream norm_file;
    sprintf(filename, "%s/cloud_%d.nmap", m_mapDir.c_str(), index);
    norm_file.open(filename, ios::in | ios::binary);
    if (!norm_file.is_open()) {
        PCL_WARN("Can't open normal map file : %s, skipped!", filename);
        return;
    }
    std::vector<float> norm_data;
    norm_data.resize(640 * 480 * 3);
    norm_file.read((char *)&norm_data[0], norm_data.size() * sizeof(float));
    norm_file.close();

    CloudTypePtr cloud = mapToCloud(point_data, norm_data);
    if (m_shouldFilter) {
        cloud = filterByRemoveOutlier(cloud);
    }
    CloudTypePtr transformed = cloud;
    if (m_cameraPoses.size() > index) {
        transformed.reset(new CloudType);
        pcl::transformPointCloudWithNormals(*cloud, *transformed, m_cameraPoses[index]);
    }
    sprintf(filename, "%s/cloud_%d.%s", m_dumpDir.c_str(), index++, m_dumpFormat.c_str());
    if (m_dumpFormat == "pcd") {
        pcl::io::savePCDFileBinary(filename, *transformed);
    }
    else if (m_dumpFormat == "ply") {
        pcl::io::savePLYFileBinary(filename, *transformed);
    }
}

void MapDumper::initPoses()
{
    m_cameraPoses.clear();

    FILE *fp = fopen(m_cameraPosFile.c_str(), "r");
    if (!fp) {
        PCL_ERROR("Can't open file : %s\n", m_cameraPosFile.c_str());
        exit(-1);
    }
    while (!feof(fp)) {
        float tx, ty, tz, w, x, y, z;
        fscanf(fp, "%f, %f, %f, %f, %f, %f, %f", &tx, &ty, &tz, &w, &x, &y, &z);
        Eigen::Vector3f trans(tx, ty, tz);
        Eigen::Quaternionf rot(w, x, y, z);
        Eigen::Affine3f affine;
        affine.linear() = rot.toRotationMatrix();
        affine.translation() = trans;
        m_cameraPoses.push_back(affine);
    }
    fclose(fp);
}

CloudTypePtr MapDumper::filterByRemoveOutlier(CloudTypePtr &incloud)
{
    // point cloud instance for the result
    CloudTypePtr cleaned(new CloudType);
    // create the radius outlier removal filter
    pcl::RadiusOutlierRemoval<PointType> radius_outlier_removal;
    // set input cloud
    radius_outlier_removal.setInputCloud(incloud);
    // set radius for neighbor search
    radius_outlier_removal.setRadiusSearch(0.05);
    // set threshold for minimum required neighbors neighbors
    radius_outlier_removal.setMinNeighborsInRadius(200);
    // do filtering
    radius_outlier_removal.filter(*cleaned);
    return cleaned;
}