#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 "Demos/Visualization/Visualization.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"

// 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 timeStep ();
void buildModel ();
void createMesh();
void render ();
void reset();
void initParameters();
void TW_CALL setBendingMethod(const void *value, void *clientData);
void TW_CALL getBendingMethod(void *value, void *clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);

const int nRows = 50;
const int nCols = 50;
const Real width = 10.0;
const Real height = 10.0;
short simulationMethod = 2;
short bendingMethod = 2;
DemoBase *base;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

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

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

	buildModel();

	initParameters();

	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, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod, " label='Simulation method' enum='0 {None}, 1 {Distance constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics}' group=Simulation");
	TwType enumType3 = TwDefineEnum("BendingMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "BendingMethod", enumType3, setBendingMethod, getBendingMethod, &bendingMethod, " label='Bending method' enum='0 {None}, 1 {Dihedral angle}, 2 {Isometric bending}' group=Bending");

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

	buildModel();
}

void timeStep ()
{
	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);
		STOP_TIMING_AVG;
	}

	for (unsigned int i = 0; i < model->getTriangleModels().size(); i++)
		model->getTriangleModels()[i]->updateMeshNormals(model->getParticles());
}

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

	createMesh();
}

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

/** Create a particle model mesh
*/
void createMesh()
{
	TriangleModel::ParticleMesh::UVs uvs;
	uvs.resize(nRows*nCols);

	const Real dy = width / (Real)(nCols - 1);
	const Real dx = height / (Real)(nRows - 1);

	Vector3r points[nRows*nCols];
	for (int i = 0; i < nRows; i++)
	{
		for (int j = 0; j < nCols; j++)
		{
			const Real y = (Real)dy*j;
			const Real x = (Real)dx*i;
			points[i*nCols + j] = Vector3r(x, 1.0, y);

			uvs[i*nCols + j][0] = x/width;
			uvs[i*nCols + j][1] = y/height;
		}
	}
	const int nIndices = 6 * (nRows - 1)*(nCols - 1);

	TriangleModel::ParticleMesh::UVIndices uvIndices;
	uvIndices.resize(nIndices);

	unsigned int indices[nIndices];
	int index = 0;
	for (int i = 0; i < nRows - 1; i++)
	{
		for (int j = 0; j < nCols - 1; j++)
		{
			int helper = 0;
			if (i % 2 == j % 2)
				helper = 1;

			indices[index] = i*nCols + j;
			indices[index + 1] = i*nCols + j + 1;
			indices[index + 2] = (i + 1)*nCols + j + helper;

			uvIndices[index] = i*nCols + j;
			uvIndices[index + 1] = i*nCols + j + 1;
			uvIndices[index + 2] = (i + 1)*nCols + j + helper;
			index += 3;

			indices[index] = (i + 1)*nCols + j + 1;
			indices[index + 1] = (i + 1)*nCols + j;
			indices[index + 2] = i*nCols + j + 1 - helper;

			uvIndices[index] = (i + 1)*nCols + j + 1;
			uvIndices[index + 1] = (i + 1)*nCols + j;
			uvIndices[index + 2] = i*nCols + j + 1 - helper;
			index += 3;
		}
	}

	SimulationModel *model = Simulation::getCurrent()->getModel();
	model->addTriangleModel(nRows*nCols, nIndices / 3, &points[0], &indices[0], uvIndices, uvs);

	ParticleData &pd = model->getParticles();
	for (unsigned int i = 0; i < pd.getNumberOfParticles(); i++)
	{
		pd.setMass(i, 1.0);
	}

	// Set mass of points to zero => make it static
	pd.setMass(0, 0.0);
	pd.setMass((nRows-1)*nCols, 0.0);

	// init constraints
	for (unsigned int cm = 0; cm < model->getTriangleModels().size(); cm++)
	{
		if (simulationMethod == 1)
		{
			const unsigned int offset = model->getTriangleModels()[cm]->getIndexOffset();
			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 (simulationMethod == 2)
		{
			const unsigned int offset = model->getTriangleModels()[cm]->getIndexOffset();
			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 (simulationMethod == 3)
		{
			const unsigned int offset = model->getTriangleModels()[cm]->getIndexOffset();
			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)
		{
			const unsigned int offset = model->getTriangleModels()[cm]->getIndexOffset();
			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);
					}
				}
			}
		}
	}

	LOG_INFO << "Number of triangles: " << nIndices / 3;
	LOG_INFO << "Number of vertices: " << nRows*nCols;

}

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 setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

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


/**
 * @file reward_clipping_test.hpp
 * @author Shashank Shekhar
 *
 * Test for the reward clipping wrapper for reinforcement learning environments.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core.hpp>

#include <mlpack/methods/reinforcement_learning/environment/mountain_car.hpp>
#include <mlpack/methods/reinforcement_learning/environment/continuous_mountain_car.hpp>
#include <mlpack/methods/reinforcement_learning/environment/cart_pole.hpp>
#include <mlpack/methods/reinforcement_learning/environment/acrobot.hpp>
#include <mlpack/methods/reinforcement_learning/environment/pendulum.hpp>
#include <mlpack/methods/reinforcement_learning/environment/reward_clipping.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/gaussian_init.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <mlpack/methods/reinforcement_learning/q_learning.hpp>
#include <mlpack/methods/reinforcement_learning/policy/greedy_policy.hpp>
#include <mlpack/methods/reinforcement_learning/training_config.hpp>

#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace ens;
using namespace mlpack::rl;

BOOST_AUTO_TEST_SUITE(RewardClippingTest);

// Test checking that reward clipping works with vanilla update.
BOOST_AUTO_TEST_CASE(ClippedRewardTest)
{
  Pendulum task;
  RewardClipping<Pendulum> rewardClipping(task, -2.0, +2.0);

  RewardClipping<Pendulum>::State state = rewardClipping.InitialSample();
  RewardClipping<Pendulum>::Action action;
  action.action[0] = mlpack::math::Random(-1.0, 1.0);
  double reward = rewardClipping.Sample(state, action);

  BOOST_REQUIRE(reward <= 2.0);
  BOOST_REQUIRE(reward >= -2.0);
}

//! Test DQN in Acrobot task.
BOOST_AUTO_TEST_CASE(RewardClippedAcrobotWithDQN)
{
  // We will allow three trials, although it would be very uncommon for the test
  // to use more than one.
  bool converged = false;
  for (size_t trial = 0; trial < 3; ++trial)
  {
    // Set up the network.
    FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
        GaussianInitialization(0, 0.001));
    model.Add<Linear<>>(4, 64);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(64, 32);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(32, 3);

    // Set up the policy and replay method.
    GreedyPolicy<RewardClipping<Acrobot>> policy(1.0, 1000, 0.1, 0.99);
    RandomReplay<RewardClipping<Acrobot>> replayMethod(20, 10000);

    // Set up Acrobot task and reward clipping wrapper.
    Acrobot task;
    RewardClipping<Acrobot> rewardClipping(task, -2.0, +2.0);

    // Set up update rule.
    AdamUpdate update;

    TrainingConfig config;
    config.StepSize() = 0.01;
    config.Discount() = 0.99;
    config.TargetNetworkSyncInterval() = 100;
    config.ExplorationSteps() = 100;
    config.DoubleQLearning() = false;
    config.StepLimit() = 400;

    // Set up DQN agent.
    QLearning<decltype(rewardClipping), decltype(model), AdamUpdate,
              decltype(policy)>
        agent(std::move(config), std::move(model), std::move(policy),
        std::move(replayMethod), std::move(update), std::move(rewardClipping));

    arma::running_stat<double> averageReturn;
    size_t episodes = 0;
    converged = true;
    while (true)
    {
      double episodeReturn = agent.Episode();
      averageReturn(episodeReturn);
      episodes += 1;

      if (episodes > 1000)
      {
        Log::Debug << "Acrobot with DQN failed." << std::endl;
        converged = false;
        break;
      }

      /**
       * I am using a thresold of -380 to check convegence.
       */
      Log::Debug << "Average return: " << averageReturn.mean()
          << " Episode return: " << episodeReturn << std::endl;
      if (averageReturn.mean() > -380.00)
      {
        agent.Deterministic() = true;
        arma::running_stat<double> testReturn;
        for (size_t i = 0; i < 20; ++i)
          testReturn(agent.Episode());

        Log::Debug << "Average return in deterministic test: "
            << testReturn.mean() << std::endl;
        break;
      }
    }

    if (converged)
      break;
  }

  BOOST_REQUIRE(converged);
}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/**
  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_AVX_SIMD_FUNCTION_DEINTERLEAVE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_DEINTERLEAVE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/deinterleave_first.hpp>
#include <boost/simd/function/deinterleave_second.hpp>
#include <boost/simd/function/combine.hpp>
#include <boost/simd/function/slice.hpp>
#include <array>

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

  BOOST_DISPATCH_OVERLOAD ( deinterleave_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::fundamental_<A0>, bs::avx_>
                          , bs::pack_<bd::fundamental_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE std::array<A0,2> operator()(A0 const& a0, A0 const& a1 ) const BOOST_NOEXCEPT
    {
      auto s00 = slice_low(a0), s01 = slice_high(a0);
      auto s10 = slice_low(a1), s11 = slice_high(a1);

      auto f = combine(deinterleave_first (s00,s01), deinterleave_first (s10,s11));
      auto s = combine(deinterleave_second(s00,s01), deinterleave_second(s10,s11));

      std::array<A0,2> that{f,s};
      return that;
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

    @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_FUNCTION_IS_INF_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_INF_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-predicates

    This function object returns @ref True if x is inf or -inf else returns @ref False.

    @par Header <boost/simd/function/is_inf.hpp>

    @par Note

     Using `is_inf(x)` is similar to `(x == Inf) || (x == -Inf)`

    @par Example:

      @snippet is_inf.cpp is_inf

    @par Possible output:

      @snippet is_inf.txt is_inf

  **/
  as_logical_t<Value> is_inf(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_inf.hpp>
#include <boost/simd/function/simd/is_inf.hpp>

#endif


// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "SurgSim/Devices/DeviceFilters/PoseIntegrator.h"

#include <boost/math/special_functions/fpclassify.hpp>

#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/DataStructures/DataGroupCopier.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/Vector.h"

using SurgSim::Math::Vector3d;

namespace SurgSim
{
namespace Devices
{

SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::PoseIntegrator, PoseIntegrator);

PoseIntegrator::PoseIntegrator(const std::string& name) :
	DeviceFilter(name),
	m_poseResult(PoseType::Identity())
{
}

const PoseIntegrator::PoseType& PoseIntegrator::integrate(const PoseType& pose)
{
	// Note: we apply translation and rotation separately.  This is NOT the same as (m_poseResult * pose)!
	m_poseResult.pretranslate(pose.translation());
	m_poseResult.rotate(pose.rotation());
	return m_poseResult;
}

void PoseIntegrator::initializeInput(const std::string& device, const DataStructures::DataGroup& inputData)
{
	if (getInputData().isEmpty())
	{
		if (!inputData.vectors().hasEntry(DataStructures::Names::LINEAR_VELOCITY) ||
			!inputData.vectors().hasEntry(DataStructures::Names::ANGULAR_VELOCITY))
		{
			DataStructures::DataGroupBuilder builder;
			builder.addEntriesFrom(inputData);
			builder.addVector(DataStructures::Names::LINEAR_VELOCITY);
			builder.addVector(DataStructures::Names::ANGULAR_VELOCITY);
			getInputData() = builder.createData();
			m_copier = std::make_shared<DataStructures::DataGroupCopier>(inputData, &getInputData());
		}
	}

	if (m_copier == nullptr)
	{
		getInputData() = inputData;
	}
	else
	{
		m_copier->copy(inputData, &getInputData());
	}

	PoseType pose;
	if (inputData.poses().get(DataStructures::Names::POSE, &pose))
	{
		m_poseResult = pose;
	}
}

void PoseIntegrator::handleInput(const std::string& device, const DataStructures::DataGroup& inputData)
{
	if (m_copier == nullptr)
	{
		getInputData() = inputData;
	}
	else
	{
		m_copier->copy(inputData, &getInputData());
	}

	PoseType pose;
	if (inputData.poses().get(DataStructures::Names::POSE, &pose))
	{
		m_timer.markFrame();
		double rate = m_timer.getAverageFrameRate();
		if (m_timer.getNumberOfClockFails() > 0)
		{
			m_timer.start();
			rate = 0.0;
			SURGSIM_LOG_DEBUG(Framework::Logger::getLogger("Devices/Filters/PoseIntegrator")) <<
				"The Timer used by " << getName() <<
				" had a clock fail.  The calculated velocities will be zero this update.";
		}

		if (!boost::math::isnormal(rate))
		{
			rate = 0.0;
		}

		bool reset = false;
		inputData.booleans().get(m_resetName, &reset);
		if (reset)
		{
			pose.translation() = -m_poseResult.translation();
			pose.linear() = m_poseResult.linear().transpose();
		}

		SurgSim::Math::RigidTransform3d resetPose;
		if (m_resetPose.hasValue())
		{
			m_poseResult.translation() = m_resetPose.getValue().translation();
			m_poseResult.linear() = m_resetPose.getValue().linear();
			m_resetPose.invalidate();
		}

		// Before updating the current pose, use it to calculate the angular velocity.
		Vector3d rotationAxis;
		double angle;
		Math::computeAngleAndAxis(pose.rotation(), &angle, &rotationAxis);
		rotationAxis = m_poseResult.rotation() * rotationAxis; // rotate the axis into global space
		getInputData().vectors().set(DataStructures::Names::ANGULAR_VELOCITY, rotationAxis * angle * rate);
		getInputData().poses().set(DataStructures::Names::POSE, integrate(pose));
		getInputData().vectors().set(DataStructures::Names::LINEAR_VELOCITY, pose.translation() * rate);
	}
	pushInput();
}

void PoseIntegrator::setReset(const std::string& name)
{
	SURGSIM_ASSERT(getInputData().isEmpty()) <<
		"PoseIntegrator::setReset cannot be called after the first call to initializeInput.";
	m_resetName = name;
}

void PoseIntegrator::filterOutput(const std::string& device,
	const SurgSim::DataStructures::DataGroup& dataToFilter,
	SurgSim::DataStructures::DataGroup* result)
{
	SurgSim::Math::RigidTransform3d newPose;
	if (dataToFilter.poses().hasData(SurgSim::DataStructures::Names::POSE))
	{
		dataToFilter.poses().get(SurgSim::DataStructures::Names::POSE, &newPose);
		m_resetPose = newPose;
	}

	*result = dataToFilter;
}

};  // namespace Devices
};  // namespace SurgSim


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_BOX_FDDP_HPP_
#define CROCODDYL_CORE_SOLVERS_BOX_FDDP_HPP_

#include <Eigen/Cholesky>
#include <vector>
#include "crocoddyl/core/solvers/fddp.hpp"
#include "crocoddyl/core/solvers/box-qp.hpp"

namespace crocoddyl {

class SolverBoxFDDP : public SolverFDDP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  explicit SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem);
  ~SolverBoxFDDP();

  virtual void allocateData();
  virtual void computeGains(const std::size_t& t);
  virtual void forwardPass(const double& steplength);

  const std::vector<Eigen::MatrixXd>& get_Quu_inv() const;

 protected:
  BoxQP qp_;
  std::vector<Eigen::MatrixXd> Quu_inv_;
  Eigen::VectorXd du_lb_;
  Eigen::VectorXd du_ub_;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_BOX_FDDP_HPP_


#ifndef ASSEMBLER_HPP
#define ASSEMBLER_HPP

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/AssemblyValsCache.hpp>
#include <polyfem/MatrixUtils.hpp>

#include <Eigen/Sparse>
#include <vector>
#include <iostream>
#include <cmath>
#include <memory>

//this casses are instantiated in the cpp, cannot be used with generic assembler
//without adding template instantiation
namespace polyfem
{
	//assemble matrix based on the local assembler
	//local assembler is eg Laplce, LinearElasticy etc
	template <class LocalAssembler>
	class Assembler
	{
	public:
		//assembler stiffness matrix, is the mesh is volumetric, number of bases and bases (FE and geom)
		//gbases and bases can be the same (ie isoparametric)
		void assemble(
			const bool is_volume,
			const int n_basis,
			const std::vector<ElementBases> &bases,
			const std::vector<ElementBases> &gbases,
			const AssemblyValsCache &cache,
			StiffnessMatrix &stiffness) const;

		//references to local assemblers
		inline LocalAssembler &local_assembler() { return local_assembler_; }
		inline const LocalAssembler &local_assembler() const { return local_assembler_; }

	private:
		LocalAssembler local_assembler_;
	};

	//mixed formulation assembler
	template <class LocalAssembler>
	class MixedAssembler
	{
	public:
		//this assembler takes two bases: psi_bases are the scalar ones, phi_bases are the tensor ones
		//both have the same geometric mapping
		void assemble(
			const bool is_volume,
			const int n_psi_basis,
			const int n_phi_basis,
			const std::vector<ElementBases> &psi_bases,
			const std::vector<ElementBases> &phi_bases,
			const std::vector<ElementBases> &gbases,
			const AssemblyValsCache &psi_cache,
			const AssemblyValsCache &phi_cache,
			StiffnessMatrix &stiffness) const;

		inline LocalAssembler &local_assembler() { return local_assembler_; }
		inline const LocalAssembler &local_assembler() const { return local_assembler_; }

	private:
		LocalAssembler local_assembler_;
	};

	//non-linear assembler (eg neohookean elasticity)
	template <class LocalAssembler>
	class NLAssembler
	{
	public:
		//assemble gradient of energy (rhs)
		void assemble_grad(
			const bool is_volume,
			const int n_basis,
			const std::vector<ElementBases> &bases,
			const std::vector<ElementBases> &gbases,
			const AssemblyValsCache &cache,
			const Eigen::MatrixXd &displacement,
			Eigen::MatrixXd &rhs) const;
		//assemble hessian of energy (grad)
		void assemble_hessian(
			const bool is_volume,
			const int n_basis,
			const bool project_to_psd,
			const std::vector<ElementBases> &bases,
			const std::vector<ElementBases> &gbases,
			const AssemblyValsCache &cache,
			const Eigen::MatrixXd &displacement,
			SpareMatrixCache &mat_cache,
			StiffnessMatrix &grad) const;

		//assemble energy
		double assemble(
			const bool is_volume,
			const std::vector<ElementBases> &bases,
			const std::vector<ElementBases> &gbases,
			const AssemblyValsCache &cache,
			const Eigen::MatrixXd &displacement) const;

		inline LocalAssembler &local_assembler() { return local_assembler_; }
		inline const LocalAssembler &local_assembler() const { return local_assembler_; }

	private:
		LocalAssembler local_assembler_;
	};
} // namespace polyfem

#endif //ASSEMBLER_HPP


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  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_FUNCTION_SCALAR_FAST_TRUNC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_FAST_TRUNC_HPP_INCLUDED

#include <boost/simd/function/definition/fast_trunc.hpp>
#include <boost/simd/arch/common/scalar/function/fast_trunc.hpp>

#endif


#include <cstdlib>
#include <cstdint>
#include <string>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/ostream_iterator.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>

#include "LZJD.h"

using namespace std;
namespace bi = boost::archive::iterators;

extern "C" {

vector<int32_t> cstring_to_lzjd(char* hash) {
    string line = hash;
    auto first_colon = line.find(":", 0);
    auto second_colon = line.find(":", first_colon + 1);
    string path = line.substr(first_colon + 1, second_colon - first_colon - 1);
    string base64ints = line.substr(second_colon + 1, line.size() - second_colon);
    auto size = base64ints.size();
    while (size > 0 && base64ints[size - 1] == '=')
        size--;
    base64ints = base64ints.substr(0, size);

    //TODO this is not 100% kosher, but C++ is a pain.

    typedef
    bi::transform_width<
            bi::binary_from_base64<bi::remove_whitespace < string::const_iterator>>,
            8, 6
            >
            base64_dec;

    vector<uint8_t> int_parts;

    copy(
            base64_dec(base64ints.cbegin()),
            base64_dec(base64ints.cend()),
            std::back_inserter(int_parts)
            );

    vector<int32_t> decoded_ints(int_parts.size() / 4);
    for (unsigned int i = 0; i < int_parts.size(); i += 4) {
        //big endian extraction of the right value
        int32_t dec_i = (int_parts[i + 0] << 24) | (int_parts[i + 1] << 16) | (int_parts[i + 2] << 8) | (int_parts[i + 3] << 0);
        decoded_ints[i / 4] = dec_i;
        //                cout << dec_i << ", ";
    }
    return decoded_ints;
}

int32_t lzjd_similarity(char *hash1, char *hash2) {
    try {
        vector<int32_t> l1 = cstring_to_lzjd(hash1);
        vector<int32_t> l2 = cstring_to_lzjd(hash2);
        return similarity(l1, l2);
    } catch(...) {
        return 0;
    }
    return 0;
}

} // End Extern C


#include "segment_list.h"
#include <Eigen/Core>
#include <iostream>
#include <mex.h>
#include <igl/C_STR.h>
#include <igl/matlab/mexErrMsgTxt.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) ::mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Bbox_3.h>
#include <CGAL/box_intersection_d.h>

#include <algorithm>

// Inputs:
//   VA  #VA by 2 list of vertex positions
//   EA  #VA by 2 list of segment indices into VA
//   VB  #VB by 2 list of vertex positions
//   EB  #VB by 2 list of segment indices into VB
// Outputs:
//   IF  #IF by 2 list of pairs of indices into EA and EB
//   T  #IF by 2 list of paramteric distances along corresponding edge in EA and
//     EB. [-1 -1] indicates co-linear intersection.
void segment_segment_intersection(
  const Eigen::MatrixXd & VA,
  const Eigen::MatrixXi & EA,
  const Eigen::MatrixXd & VB,
  const Eigen::MatrixXi & EB,
  Eigen::MatrixXi & IF,
  Eigen::MatrixXd & T)
{

  typedef CGAL::Epeck Kernel;
  // 2D Primitives
  typedef CGAL::Point_2<Kernel>    Point_2;
  typedef CGAL::Segment_2<Kernel>  Segment_2;
  std::vector<Segment_2> SA,SB;
  segment_list(VA,EA,SA);
  segment_list(VB,EB,SB);
}

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[]
)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  MatrixXd VA,VB;
  MatrixXi EA,EB;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);
  mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  mexErrMsgTxt(nrhs==4,"nrhs should be == 4");
  parse_rhs_double(prhs,VA);
  parse_rhs_index(prhs+1,EA);
  mexErrMsgTxt(VA.cols()==2,"VA must be #VA by 2");
  mexErrMsgTxt(EA.cols()==2,"EA must be #EA by 2");
  parse_rhs_double(prhs+2,VB);
  parse_rhs_index(prhs+3,EB);
  mexErrMsgTxt(VB.cols()==2,"VB must be #VB by 2");
  mexErrMsgTxt(EB.cols()==2,"EB must be #EB by 2");

  switch(nlhs)
  {
    case 4:
    case 3:
    case 2:
    case 1:
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


// practice 5.1

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>

using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

struct none {};

template<class T=none>
struct wrapAssert : T
{
	typedef typename T type;
	BOOST_STATIC_ASSERT(0);
};

template<class T0, class T1, class T2, class T3>
struct double_half
{
	typedef typename mpl::vector_c<int
		, T0::value*2
		, T1::value*2
		, T2::value
		, T3::value
	> type;
};

template<class Sequence>
struct double_first_half
{
	typedef typename double_half<
		typename mpl::at<Sequence, mpl::int_<0> >::type
		, typename mpl::at<Sequence, mpl::int_<1> >::type
		, typename mpl::at<Sequence, mpl::int_<2> >::type
		, typename mpl::at<Sequence, mpl::int_<3> >::type
	>::type type;

};

struct test1 :
	mpl::eval_if<
		mpl::equal<
			double_first_half< mpl::vector_c<int,1,2,3,4> >::type
			, mpl::vector_c<int,2,4,3,4>
		>
	, none
	, wrapAssert<>
	>
{};

void main()
{
	int a = mpl::at< mpl::vector_c<int, 1,2,3,4>
		, mpl::int_<2> >::type::value;

	int b = mpl::integral_c<int,2>::value;

}


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

CAmount nMoneySupplyPoWEnd = 43199500 * COIN;

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1; nHeight < 1000; nHeight += 1) {
        /* PoW Phase One */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 1000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1001; nHeight <= 10000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 100 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 10001; nHeight <= 20000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 90 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 20001; nHeight <= 30000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 80 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 30001; nHeight <= 40000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 70 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 40001; nHeight <= 50000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 60 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 50001; nHeight <= 60000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 50 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 60001; nHeight <= 70000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 40 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 70001; nHeight <= 80000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 30 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 80001; nHeight <= 90000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 20 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 90001; nHeight <= 100000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10 * COIN);
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
        BOOST_CHECK(nSum > 0 && nSum <= nMoneySupplyPoWEnd);
    }
//	BOOST_CHECK(nSum == 4109975100000000ULL); - RECALCULATE TOTAL LATER
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by kellerberrin on 19/4/21.
//

#include "kol_InformationExclusiveInherited.h"

#include "kol_SetUtilities.h"
#include "kol_Accumulators.h"

#include <utility>
#include <algorithm>

#include <boost/graph/breadth_first_search.hpp>

namespace kol = kellerberrin::ontology;

//! A method for determining the common disjunctive ancestors
/*!
  This method returns the common disjunctive ancestors for two terms
*/
kol::OntologySetType<std::string> kol::InformationExclusiveInherited::getCommonDisjointAncestors(const std::string &termC1,
                                                                                const std::string &termC2) const {

  //Z&L: EICommonSet <- <empty_set>
  OntologySetType<std::string> cda;

  //EICA(t,t) = {t}
  if (termC1 == termC2) {

    cda.insert(termC1);
    return cda;

  }

  //Z&L: CommonAnSet <- GetCommonAnSet(t1,t2,AncestorSet)
  OntologySetType<std::string> ancestorsC1 = graph_ptr_->getSelfAncestorTerms(termC1);
  OntologySetType<std::string> ancestorsC2 = graph_ptr_->getSelfAncestorTerms(termC2);
  OntologySetType<std::string> commonAncestors = SetUtilities::setIntersection(ancestorsC1, ancestorsC2);

  //commonDisjointAncestors(c,c) = {c}, by definition
  if (commonAncestors.size() == 1) {

    return commonAncestors;

  }

  //Z&L: UnionAnSet <- GetAnSet(t1,AncestorSet) U GetAnSet(t2,AncestorSet)
  OntologySetType<std::string> unionAncestors = SetUtilities::setUnion(ancestorsC1, ancestorsC2);

  //Z&L: DiffAnSet <- UnionAnSet - CommonAnSet
  OntologySetType<std::string> diffSet = SetUtilities::setDifference(unionAncestors, commonAncestors);

  //get the boost graph
  const GoGraph::Graph &g = graph_ptr_->getGraph();
  //Z&L: for each a in CommonAnSet do ...
  for (auto const &term : commonAncestors) {

    bool isDisj = false;

    GoGraph::GoVertex v = graph_ptr_->getVertexByName(term);
    GoGraph::InEdgeIterator ei, end;
    for (boost::tie(ei, end) = boost::in_edges(v, g); ei != end; ++ei) {

      GoGraph::GoVertex child = boost::source(*ei, g);
      size_t index = graph_ptr_->getVertexIndex(child);
      std::string cTerm = graph_ptr_->getTermStringIdByIndex(index);

      if (diffSet.find(cTerm) != diffSet.end()) {
        //early exit should improve runtime
        isDisj = true;
        break;

      }

    }

    //if the term is a disjoint ancestor add it to the set
    if (isDisj) {

      cda.insert(term);

    }

  }

  return cda;

}

//! An method for returning the shared information of two terms
/*!
  This method returns the mean information content of the frontier ancestors
*/
double kol::InformationExclusiveInherited::sharedInformation(const std::string &termA, const std::string &termB) const {
  // return 0 for any terms not in the datbase
  if (not ic_map_ptr_->validateTerms(termA, termB)) {

    return 0.0;

  }

  Accumulators::MeanAccumulator meanIC;
  OntologySetType<std::string> cda = getCommonDisjointAncestors(termA, termB);

  for (auto const &element : cda) {

    meanIC(ic_map_ptr_->termInformation(element));

  }

  return Accumulators::extractMean(meanIC);

}


#pragma once
#include <Eigen/Dense>

#include "../datastructures/tree.hpp"
#include "triangulation.hpp"

namespace datastructures {
template <>
struct NodeTrait<space::HierarchicalBasisFn> {
  static constexpr size_t N_parents = 2;
  static constexpr size_t N_children = 4;
};
}  // namespace datastructures

namespace space {
class HierarchicalBasisFn : public datastructures::Node<HierarchicalBasisFn> {
 public:
  HierarchicalBasisFn(const std::vector<HierarchicalBasisFn *> &parents,
                      Vertex *vertex)
      : Node(parents), vertex_(vertex) {}

  bool Refine();
  bool is_full() const;
  Vertex *vertex() const { return vertex_; }
  const SmallVector<Element2D *, 4> &support() const { return vertex_->patch; }
  std::pair<double, double> center() const { return {vertex_->x, vertex_->y}; }
  inline bool on_domain_boundary() const { return vertex_->on_domain_boundary; }

  // Whether some element in the support touches the boundary.}
  inline bool TouchesDomainBoundary() const {
    for (auto elem : support())
      if (elem->TouchesDomainBoundary()) return true;
    return false;
  }

  double Volume() const;

  double Eval(double x, double y) const;
  bool Contains(double x, double y) const;
  Eigen::Vector2d EvalGrad(double x, double y) const;

  friend std::ostream &operator<<(std::ostream &os,
                                  const HierarchicalBasisFn &fn) {
    os << "HBF(" << fn.vertex()->x << ", " << fn.vertex()->y << ")";
    return os;
  }

 private:
  Vertex *vertex_;

  // Protected constructor for creating a metaroot.
  HierarchicalBasisFn(Deque<HierarchicalBasisFn> *container, Vertex *vertex)
      : Node(container), vertex_(vertex) {
    assert(vertex->is_metaroot());
  }

  friend datastructures::Tree<HierarchicalBasisFn>;
  friend InitialTriangulation;
  friend Vertex;
};

}  // namespace space


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

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <random>
#include <vector>

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

#include "line_estimator.h"

namespace ransac_lib {

LineEstimator::LineEstimator(const Eigen::Matrix2Xd& data) {
  data_ = data;
  num_data_ = data_.cols();
}

int LineEstimator::MinimalSolver(const std::vector<int>& sample,
                                 std::vector<Eigen::Vector3d>* lines) const {
  lines->clear();
  if (sample.size() < 2u) return 0;

  lines->resize(1);
  Eigen::Vector3d p1(data_(0, sample[0]), data_(1, sample[0]), 1.0);
  Eigen::Vector3d p2(data_(0, sample[1]), data_(1, sample[1]), 1.0);
  (*lines)[0] = p1.cross(p2);
  // Normalizes the line such that the normal of the line has unit length.
  double normal_norm = (*lines)[0].head<2>().norm();
  if (normal_norm == 0.0) {
    lines->clear();
    return 0;
  }

  (*lines)[0] /= normal_norm;

  return 1;
}

int LineEstimator::NonMinimalSolver(const std::vector<int>& sample,
                                    Eigen::Vector3d* line) const {
  if (sample.size() < 6u) return 0;

  const int kNumSamples = static_cast<int>(sample.size());

  // We fit the line by estimating the eigenvectors of the covariance matrix
  // of the data.
  Eigen::Vector2d mean(0.0, 0.0);
  for (int i = 0; i < kNumSamples; ++i) {
    mean += data_.col(sample[i]);
  }
  mean /= static_cast<double>(kNumSamples);

  // Builds the covariance matrix C.
  Eigen::Matrix2d C = Eigen::Matrix2d::Zero();

  for (int i = 0; i < kNumSamples; ++i) {
    Eigen::Vector2d d = data_.col(sample[i]) - mean;
    C += d * d.transpose();
  }
  C /= static_cast<double>(kNumSamples - 1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig_solver(C);
  if (eig_solver.info() != Eigen::Success) return 0;

  line->head<2>() = eig_solver.eigenvectors().col(1);

  // Re-estimates the translation along the line to account for subtraction
  // of mean.
  (*line)[2] = -line->head<2>().dot(mean);

  return 1;
}

// Evaluates the line on the i-th data point.
double LineEstimator::EvaluateModelOnPoint(const Eigen::Vector3d& line,
                                           int i) const {
  double residual = line.dot(data_.col(i).homogeneous());
  return residual * residual;
}

}  // namespace ransac_lib


/**
 * Copyright (c) 2012 - 2014 TideSDK contributors
 * http://www.tidesdk.org
 * Includes modified sources under the Apache 2 License
 * Copyright (c) 2008 - 2012 Appcelerator Inc
 * Refer to LICENSE for details of distribution and use.
 **/

#ifdef OS_WIN32
#include <tideutils/win/win32_utils.h>
#else
#include <tideutils/posix/posix_utils.h>
#endif
#include <tide/tide.h>
#include "codec_binding.h"

#include <sstream>

#include <Poco/Zip/Zip.h>
#include <Poco/Zip/Compress.h>
#include <Poco/Zip/Decompress.h>
#include <Poco/Base64Encoder.h>
#include <Poco/Base64Decoder.h>
#include <Poco/DigestEngine.h>
#include <Poco/MD2Engine.h>
#include <Poco/MD4Engine.h>
#include <Poco/MD5Engine.h>
#include <Poco/SHA1Engine.h>
#include <Poco/HMACEngine.h>
#include <Poco/HexBinaryEncoder.h>
#include <Poco/HexBinaryDecoder.h>
#include <Poco/Checksum.h>
#include <Poco/File.h>
#include <Poco/Path.h>

#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/insert_linebreaks.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>

using namespace boost::archive::iterators;
using namespace std;

typedef transform_width< binary_from_base64<remove_whitespace<string::const_iterator> >, 8, 6 > it_binary_t;
typedef insert_linebreaks<base64_from_binary<transform_width<string::const_iterator,6,8> >, 72 > it_base64_t;

#define CODEC_MD2       1
#define CODEC_MD4       2
#define CODEC_MD5       3
#define CODEC_SHA1      4
#define CODEC_CRC32     1
#define CODEC_ADLER32   2

namespace ti
{
    CodecBinding::CodecBinding(TiObjectRef global) :
        StaticBoundObject("Codec"),
        global(global)
    {
        /**
         * @tiapi(method=True,name=Codec.encodeBase64,since=0.7) encode a string or Bytes into base64
         * @tiarg(for=Codec.encodeBase64,name=data,type=String|Bytes) data to encode
         * @tiresult(for=Codec.encodeBase64,type=string) returns base64 encoded string
         */
        this->SetMethod("encodeBase64", &CodecBinding::EncodeBase64);

        /**
         * @tiapi(method=True,name=Codec.decodeBase64,since=0.7) decode a string from base64
         * @tiarg(for=Codec.decodeBase64,name=data,type=String) data to decode
         * @tiresult(for=Codec.decodeBase64,type=string) returns base64 decoded string
         */
        this->SetMethod("decodeBase64", &CodecBinding::DecodeBase64);

        /**
         * @tiapi(method=True,name=Codec.digestToHex,since=0.7) encode a string or Bytes using a digest algorithm
         * @tiarg(for=Codec.digestToHex,name=type,type=int) encoding type: currently supports MD2, MD4, MD5, SHA1
         * @tiarg(for=Codec.digestToHex,name=data,type=String|Bytes) data to encode
         * @tiresult(for=Codec.digestToHex,type=string) returns encoded string
         */
        this->SetMethod("digestToHex", &CodecBinding::DigestToHex);

        /**
         * @tiapi(method=True,name=Codec.digestHMACToHex,since=0.7) digest a encoded string in HMAC
         * @tiarg(for=Codec.digestHMACToHex,name=type,type=int) encoding type: currently supports MD2, MD4, MD5, SHA1
         * @tiarg(for=Codec.digestHMACToHex,name=data,type=String) data to encode
         * @tiarg(for=Codec.digestHMACToHex,name=data,type=String) key to us for HMAC
         * @tiresult(for=Codec.digestHMACToHex,type=string) returns base64 decoded string
         */
        this->SetMethod("digestHMACToHex", &CodecBinding::DigestHMACToHex);

        /**
         * @tiapi(method=True,name=Codec.encodeHexBinary,since=0.7) encode a string or Bytes into hex binary
         * @tiarg(for=Codec.encodeHexBinary,name=data,type=String|Bytes) data to encode
         * @tiresult(for=Codec.encodeHexBinary,type=string) returns hex binary encoded string
         */
        this->SetMethod("encodeHexBinary", &CodecBinding::EncodeHexBinary);

        /**
         * @tiapi(method=True,name=Codec.decodeHexBinary,since=0.7) decode a string from hex binary
         * @tiarg(for=Codec.decodeHexBinary,name=data,type=String) data to decode
         * @tiresult(for=Codec.decodeHexBinary,type=string) returns unencoded hex binary string
         */
        this->SetMethod("decodeHexBinary", &CodecBinding::DecodeHexBinary);

        /**
         * @tiapi(method=True,name=Codec.checksum,since=0.7) compute checksum
         * @tiarg(for=Codec.checksum,name=data,type=String) data to checksum (as string)
         * @tiarg(for=Codec.checksum,name=type,type=int,optional=True) checksum type: currently supports CRC32 (default) and ADLER32
         * @tiresult(for=Codec.checksum,type=int) return checksum value
         */
        this->SetMethod("checksum", &CodecBinding::Checksum);

        /**
         * @tiapi(method=True,name=Codec.createZip,since=0.7) Asynchronously write the contents of a directory to a zip file
         * All files will be recursively added from the directory, and the directory will be considered the logical "root" of the zip.
         * @tiarg[Filesystem.File|String, directory] A directory root to write to the zip stream
         * @tiarg[Filesystem.File|String, zipFile] The destination zip file
         * @tiarg[Function, onComplete] A function callback that receives the zip file when writing is finished: function onComplete(destFile) {}
         */
        this->SetMethod("createZip", &CodecBinding::CreateZip);

        this->SetMethod("extractZip", &CodecBinding::ExtractZip);

        /**
         * @tiapi(property=True,name=Codec.MD2,since=0.7) MD2 property
         */
        this->SetInt("MD2", CODEC_MD2);
        /**
         * @tiapi(property=True,name=Codec.MD4,since=0.7) MD4 property
         */
        this->SetInt("MD4", CODEC_MD4);
        /**
         * @tiapi(property=True,name=Codec.MD5,since=0.7) MD5 property
         */
        this->SetInt("MD5", CODEC_MD5);
        /**
         * @tiapi(property=True,name=Codec.SHA1,since=0.7) SHA1 property
         */
        this->SetInt("SHA1", CODEC_SHA1);
        /**
         * @tiapi(property=True,name=Codec.CRC32,since=0.7) CRC32 property
         */
        this->SetInt("CRC32", CODEC_CRC32);
        /**
         * @tiapi(property=True,name=Codec.ADLER32,since=0.7) ADLER32 property
         */
        this->SetInt("ADLER32", CODEC_ADLER32);
    }

    CodecBinding::~CodecBinding()
    {
    }

    static std::string& GetStringFromValue(ValueRef value)
    {
        static std::string data;
        if (value->IsString())
        {
            data = value->ToString();
        }
        else
        {
            AutoPtr<Bytes> bytes(value->ToObject().cast<Bytes>());
            if (!bytes.isNull())
            {
                data = std::string(bytes->Pointer(), bytes->Length());
            }
            else
            {
                data = "";
            }
        }
        return data;
    }

    void CodecBinding::EncodeBase64(const ValueList& args, ValueRef result)
    {
        args.VerifyException("encodeBase64", "s|o");

        std::string s = GetStringFromValue(args.at(0));

        // Encode
        unsigned int writePaddChars = (3-s.length()%3)%3;
        string base64(it_base64_t(s.begin()),it_base64_t(s.end()));
        base64.append(writePaddChars,'=');
	result->SetString(base64);
    }

    void CodecBinding::DecodeBase64(const ValueList& args, ValueRef result)
    {
        args.VerifyException("decodeBase64", "s");

        std::string base64 = args.at(0)->ToString();

        // Decode
        unsigned int paddChars = std::count(base64.begin(), base64.end(), '=');
        std::replace(base64.begin(),base64.end(),'=','A'); // replace '=' by base64 encoding of '\0'
        std::string decoded(it_binary_t(base64.begin()), it_binary_t(base64.end())); // decode
        decoded.erase(decoded.end()-paddChars, decoded.end());  // erase padding '\0' characters
        result->SetString(decoded);
    }

    void CodecBinding::DigestToHex(const ValueList& args, ValueRef result)
    {
        args.VerifyException("digestToHex", "i s|o");

        int type = args.at(0)->ToInt();

        Poco::DigestEngine *engine = NULL;

        switch(type)
        {
            case CODEC_MD2:
            {
                engine = new Poco::MD2Engine();
                break;
            }
            case CODEC_MD4:
            {
                engine = new Poco::MD4Engine();
                break;
            }
            case CODEC_MD5:
            {
                engine = new Poco::MD5Engine();
                break;
            }
            case CODEC_SHA1:
            {
                engine = new Poco::SHA1Engine();
                break;
            }
            default:
            {
                std::ostringstream msg("Unsupported encoding type: ");
                msg << type;
                throw ValueException::FromString(msg.str());
            }
        }

        if (args.at(1)->IsString())
        {
            engine->update(args.GetString(1));
        }
        else
        {
            AutoPtr<Bytes> bytes(args.GetObject(1).cast<Bytes>());
            if (!bytes.isNull())
            {
                engine->update(bytes->Pointer(), bytes->Length());
            }
        }
        std::string data = Poco::DigestEngine::digestToHex(engine->digest());
        result->SetString(data);
        delete engine;
    }

    void CodecBinding::DigestHMACToHex(const ValueList& args, ValueRef result)
    {
        args.VerifyException("digestHMACToHex", "i s s");

        int type = args.at(0)->ToInt();
        std::string encoded = args.at(1)->ToString();
        std::string key = args.at(2)->ToString();

        switch(type)
        {
            case CODEC_MD2:
            {
                Poco::HMACEngine<Poco::MD2Engine> engine(key);
                engine.update(encoded);
                std::string data = Poco::DigestEngine::digestToHex(engine.digest());
                result->SetString(data);
                break;
            }
            case CODEC_MD4:
            {
                Poco::HMACEngine<Poco::MD4Engine> engine(key);
                engine.update(encoded);
                std::string data = Poco::DigestEngine::digestToHex(engine.digest());
                result->SetString(data);
                break;
            }
            case CODEC_MD5:
            {
                Poco::HMACEngine<Poco::MD5Engine> engine(key);
                engine.update(encoded);
                std::string data = Poco::DigestEngine::digestToHex(engine.digest());
                result->SetString(data);
                break;
            }
            case CODEC_SHA1:
            {
                Poco::HMACEngine<Poco::SHA1Engine> engine(key);
                engine.update(encoded);
                std::string data = Poco::DigestEngine::digestToHex(engine.digest());
                result->SetString(data);
                break;
            }
            default:
            {
                std::ostringstream msg("Unsupported encoding type: ");
                msg << type;
                throw ValueException::FromString(msg.str());
            }
        }
    }

    void CodecBinding::EncodeHexBinary(const ValueList& args, ValueRef result)
    {
        args.VerifyException("encodeHexBinary", "s|o");

        std::stringstream str;
        Poco::HexBinaryEncoder encoder(str);
        encoder << GetStringFromValue(args.at(0));
        encoder.close();
        std::string encoded = str.str();
        result->SetString(encoded);
    }

    void CodecBinding::DecodeHexBinary(const ValueList& args, ValueRef result)
    {
        args.VerifyException("decodeHexBinary", "s");

        std::string encoded = args.at(0)->ToString();
        std::stringstream str;
        str << encoded;
        Poco::HexBinaryDecoder decoder(str);
        std::string decoded;
        decoder >> decoded;
        result->SetString(decoded);
    }

    void CodecBinding::Checksum(const ValueList& args, ValueRef result)
    {
        args.VerifyException("checksum", "s|o ?i");

        int type = CODEC_CRC32;

        if (args.size() == 2)
        {
            type = args.at(1)->ToInt();
        }

        Poco::Checksum *checksum = NULL;

        switch(type)
        {
            case CODEC_CRC32:
            {
                checksum = new Poco::Checksum(Poco::Checksum::TYPE_CRC32);
                break;
            }
            case CODEC_ADLER32:
            {
                checksum = new Poco::Checksum(Poco::Checksum::TYPE_ADLER32);
                break;
            }
            default:
            {
                std::ostringstream msg("Unsupported type: ");
                msg << type;
                throw ValueException::FromString(msg.str());
            }
        }

        if (args.at(0)->IsString())
        {
            std::string encoded = args.at(0)->ToString();
            const char *data = encoded.c_str();
            checksum->update(data,encoded.size());
            result->SetInt(checksum->checksum());
        }
        else if (args.at(0)->IsObject())
        {
            BytesRef bytes = args.at(0)->ToObject().cast<Bytes>();
            if (bytes.isNull())
            {
                delete checksum;
                throw ValueException::FromString("unsupported data type passed as argument 1");
            }
            checksum->update(bytes->Pointer(),bytes->Length());
            result->SetInt(checksum->checksum());
        }
        else
        {
            delete checksum;
            throw ValueException::FromString("unsupported data type passed as argument 1");
        }

        delete checksum;
    }

    static std::string GetPathFromValue(ValueRef value)
    {
        if (value->IsObject())
        {
            return value->ToObject()->DisplayString()->c_str();
        }
        else
        {
            return value->ToString();
        }
    }

    void CodecBinding::CreateZip(const ValueList& args, ValueRef result)
    {
        args.VerifyException("createZip", "s|o s|o ?m");

        std::string directory = GetPathFromValue(args.at(0));
        std::string zipFile = GetPathFromValue(args.at(1));

        if (directory.size() <= 0)
        {
            throw ValueException::FromString("Error: Directory name in createZip is empty");
        }
        if (zipFile.size() <= 0)
        {
            throw ValueException::FromString("Error: Destination file name in createZip is empty");
        }

        Poco::Path path(directory);
        path.makeDirectory();
        if (!Poco::File(path).exists())
        {
            throw ValueException::FromFormat("Error: Directory %s doesn't exist in createZip", directory.c_str());
        }

        TiMethodRef zipAsyncMethod = new FunctionPtrMethod(&CodecBinding::CreateZipAsync);
        ValueList zipArgs;
        zipArgs.push_back(Value::NewString(directory));
        zipArgs.push_back(Value::NewString(zipFile));

        AutoPtr<AsyncJob> zipJob = new AsyncJob(zipAsyncMethod);
        zipArgs.push_back(Value::NewObject(zipJob));
        if (args.size() > 2 && args.at(2)->IsMethod())
        {
            zipArgs.push_back(args.at(2));
        }

        zipJob->SetArguments(zipArgs);
        zipJob->RunAsynchronously();
        result->SetObject(zipJob);
    }

    void CodecBinding::ExtractZip(const ValueList& args, ValueRef result)
    {
        args.VerifyException("extractZip", "s|o s|o ?m");

        std::string zipFile = GetPathFromValue(args.at(0));
        std::string directory = GetPathFromValue(args.at(1));

        if (zipFile.size() <= 0)
        {
            throw ValueException::FromString("Error: Zip file name in extractZip is empty");
        }
        if (zipFile.size() <= 0)
        {
            throw ValueException::FromString("Error: Destination directory name in extractZip is empty");
        }

        TiMethodRef extractAsyncMethod = new FunctionPtrMethod(&CodecBinding::ExtractZipAsync);
        ValueList extractArgs;
        extractArgs.push_back(Value::NewString(zipFile));
        extractArgs.push_back(Value::NewString(directory));

        AutoPtr<AsyncJob> extractJob = new AsyncJob(extractAsyncMethod);
        extractArgs.push_back(Value::NewObject(extractJob));
        if (args.size() > 2)
        {
            extractArgs.push_back(args.at(2));
        }

        extractJob->SetArguments(extractArgs);
        extractJob->RunAsynchronously();
        result->SetObject(extractJob);
    }

    /*static*/
    ValueRef CodecBinding::CreateZipAsync(const ValueList& args)
    {
        std::string directory = args.GetString(0);
        std::string zipFile = args.GetString(1);
        AutoPtr<AsyncJob> job = args.GetObject(2).cast<AsyncJob>();
        TiMethodRef callback = 0;
        if (args.size() > 3)
        {
            callback = args.GetMethod(3);
        }

        Poco::Path path(directory);
        path.makeDirectory();

        std::ofstream stream(UTF8ToSystem(zipFile).c_str(),
            std::ios::binary | std::ios::trunc);
        Poco::Zip::Compress compressor(stream, true);
        try
        {
            compressor.addRecursive(path);
        }
        catch (std::exception& e)
        {
            Logger::Get("Codec")->Error("exception compressing: %s", e.what());
            throw ValueException::FromFormat("Exception during zip: %s", e.what());
        }

        compressor.close();
        stream.close();

        if (!callback.isNull())
        {
            ValueList args;
            args.push_back(Value::NewString(zipFile));
            RunOnMainThread(callback, args, true);
        }

        return Value::Undefined;
    }

    /*static*/
    ValueRef CodecBinding::ExtractZipAsync(const ValueList& args)
    {
        std::string zipFile = args.GetString(0);
        std::string directory = args.GetString(1);
        AutoPtr<AsyncJob> job = args.GetObject(2).cast<AsyncJob>();
        TiMethodRef callback = 0;
        if (args.size() > 3)
        {
            callback = args.GetMethod(3);
        }

        std::ifstream stream(UTF8ToSystem(zipFile).c_str(), std::ios::binary);
        Poco::Zip::Decompress decompressor(stream, directory);
        try
        {
            decompressor.decompressAllFiles();
        }
        catch (std::exception& e)
        {
            Logger::Get("Codec")->Error("exception decompressing: %s", e.what());
            throw ValueException::FromFormat("Exception during extraction: %s", e.what());
        }

        stream.close();

        if (!callback.isNull())
        {
            ValueList args;
            args.push_back(Value::NewString(directory));
            RunOnMainThread(callback, args, true);
        }

        return Value::Undefined;
    }
}


#include "pclwindow.h"
#include "ui_pclwindow.h"
#include <QFileDialog>
#include <QMessageBox>

#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include <vtkRenderWindow.h>
#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

pcl::visualization::PCLVisualizer pviz ("test_viz", false);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);

Pclwindow::Pclwindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::Pclwindow)
{
    ui->setupUi(this);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
    for (float y = -0.5f; y <= 0.5f; y += 0.01f)
    {
      for (float z = -0.5f; z <= 0.5f; z += 0.01f)
      {
        pcl::PointXYZ point;
        point.x = 2.0f - y;
        point.y = y;
        point.z = z;
        cloud_xyz->points.push_back (point);
      }
    }
    cloud_xyz->width = cloud_xyz->points.size ();
    cloud_xyz->height = 1;

       vtkSmartPointer<vtkRenderWindow> renderWindow = pviz.getRenderWindow();
       ui->widget->SetRenderWindow (renderWindow);

       pviz.setupInteractor (ui->widget->GetInteractor (), ui->widget->GetRenderWindow ());
       pviz.getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);

       pviz.addPointCloud<pcl::PointXYZ>(cloud_xyz);
       pviz.setBackgroundColor(0, 0, 0.1);

   ui->widget->show();

}

Pclwindow::~Pclwindow()
{
    delete ui;
}

void Pclwindow::on_action_triggered()
{
    this->close();
}

void Pclwindow::on_action_2_triggered()
{
    QString fileName = QFileDialog::getOpenFileName(this, tr("Open File"), "", tr("Files (*.pcd)"));
    if (fileName != "")
    {
        pviz.removeAllPointClouds();
        ui->widget->setDisabled(true);
        ui->menuBar->setDisabled(true);
        boost::thread t(boost::bind(pcl::io::loadPCDFile<pcl::PointXYZ>, fileName.toStdString(), boost::ref(*cloud_xyz)));
        //for (int i = 0; i <= (int) sec; i++)
        //{
        //    ui->progressBar->setValue((int) ((i/sec)*100));
        //    ui->label->setText(QString::fromStdString("Wait for "+boost::lexical_cast<std::string>(((int)sec)-i)+" sec"));
            //boost::this_thread::sleep(boost::posix_time::seconds(1)); // wait 1 sec
        //}
        t.join();
        ui->progressBar->setValue(100);
        ui->widget->setEnabled(true);
        ui->menuBar->setEnabled(true);
        if (cloud_xyz->size() > 0)
        {
            pviz.addPointCloud<pcl::PointXYZ>(cloud_xyz);
            ui->progressBar->setValue(0);
        }
    };
}