#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/scope.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/import.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/object.hpp>

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/filtered.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/string.hpp>

#include <string>

#include <scitbx/vec3.h>
#include <boost_adaptbx/boost_range_python.hpp>
#include <boost_adaptbx/exporting.hpp>

#include <mmtbx/geometry/asa.hpp>
#include <mmtbx/geometry/indexing.hpp>
#include <mmtbx/geometry/overlap.hpp>
#include <mmtbx/geometry/containment.hpp>
#include <mmtbx/geometry/sphere_surface_sampling.hpp>
#include <mmtbx/geometry/calculator.hpp>

#include <mmtbx/geometry/boost_python/indexing.hpp>
#include <mmtbx/geometry/boost_python/containment.hpp>

namespace mmtbx
{
namespace geometry
{
namespace asa
{

namespace python
{

template< typename Value >
struct transformed_value_extract
{
public:
  typedef Value value_type;

private:
  boost::python::object transformation_;

public:
  transformed_value_extract(boost::python::object transformation)
  : transformation_( transformation )
  {};

  value_type operator () (boost::python::object obj) const
  {
    return boost::python::extract< value_type >( transformation_( obj ) );
  }
};

template< typename Value >
struct array_adaptor : private transformed_value_extract< Value >
{
public:
  typedef Value value_type;
  typedef std::size_t size_type;

  typedef transformed_value_extract< value_type > transformer_type;

private:
  boost::python::object array_;

public:
  array_adaptor(boost::python::object array, boost::python::object transformation)
  : transformer_type( transformation ), array_( array )
  {};

  value_type operator [](size_type const& index) const
  {
    return transformer_type::operator ()( array_[ index ] );
  }

  size_type size() const
  {
    return boost::python::len( array_ );
  }
};

} // namespace python

namespace
{

template< typename Predicate >
struct filtered_range_type
{
    template< typename Export >
    struct apply
    {
      typedef typename Export::first indexer_type;
      typedef typename indexer_type::range_type range_type;;
      typedef boost::filtered_range< Predicate, range_type > type;
    };
};

template< typename Traits >
struct python_exports
{
  static void wrap()
  {
    using namespace boost::python;
    typedef typename Traits::sphere_type sphere_type;
    typedef typename Traits::sphere_bases_type sphere_bases_type;
    typedef typename Traits::vector_type vector_type;
    typedef typename Traits::value_type value_type;
    typedef typename Traits::discrete_type discrete_type;

    // base module
    class_< sphere_type, sphere_bases_type >( "sphere", no_init )
      .def(
        init< const vector_type&, const value_type&, const size_t& >(
          ( arg( "centre" ), arg( "radius" ), arg( "index" ) )
          )
        )
      .add_property(
        "index",
        make_function(
          &sphere_type::index,
          return_value_policy< copy_const_reference >()
          )
        )
      .add_property( "low", make_function( &sphere_type::low ) )
      .add_property( "high", make_function( &sphere_type::high ) )
      ;

    // indexing module
    object indexing_module(
      handle<>( borrowed( PyImport_AddModule( "asa.indexing" ) ) )
      );
    scope().attr( "indexing" ) = indexing_module;

    { // enter indexing namespace
      scope indexing_scope = indexing_module;

      boost_adaptbx::exporting::class_list< typename Traits::indexers >::process(
        indexing::python::indexer_exports()
        );
    } // exit indexing namespace

    // accessibility module
    object accessibility_module(
      handle<>( borrowed( PyImport_AddModule( "asa.accessibility" ) ) )
      );
    scope().attr( "accessibility" ) = accessibility_module;

    { // enter accessibility namespace
      scope accessibility_scope = accessibility_module;

      typedef typename sphere_surface_sampling::GoldenSpiral< vector_type >
        ::storage_type points_range;
      typedef asa::Transform< vector_type > transformation_type;
      typedef boost::transformed_range< transformation_type, points_range >
        transformed_points_range;

      boost_adaptbx::python::generic_range_wrapper< transformed_points_range >
        ::wrap( "transformed_points_range" );

      class_< transformation_type >( "transformation", no_init )
        .def(
          init< const vector_type&, const typename vector_type::value_type& >(
            ( arg( "centre" ), arg( "radius" ) )
            )
          )
        .def( "__call__", &transformation_type::operator (), arg( "point" ) )
        ;

      boost::transformed_range< transformation_type, points_range >
        (*transformfunc)( points_range&, transformation_type ) =
          &boost::adaptors::transform< transformation_type, points_range >;

      def(
        "transform",
        transformfunc,
        with_custodian_and_ward_postcall< 0, 1 >(),
        ( arg( "range" ), arg( "transformation" ) )
        );

      typedef asa::OverlapEqualityFilter< sphere_type, overlap::BetweenSpheres >
          predicate_type;

      class_< predicate_type >( "overlap_equality_predicate", no_init )
        .def( init< const sphere_type& >( arg( "object" ) ) )
        .def( "__call__", &predicate_type::operator (), arg( "other" ) )
        ;

      boost_adaptbx::exporting::class_list< typename Traits::indexers >::process(
        indexing::python::filter_and_range_export< predicate_type >(
          "overlap_equality_filtered_"
          )
        );

      typedef typename boost::mpl::transform<
        typename Traits::indexers,
        filtered_range_type< predicate_type >
        >::type filtered_ranges_type;
      boost_adaptbx::exporting::class_list< typename Traits::checkers >::process(
        containment::python::checker_export<
          filtered_ranges_type,
          transformed_points_range
          >()
        );
    } // exit accessibility namespace
  }
};

template< typename Vector, typename Discrete = int >
struct python_export_traits
{
  typedef Sphere< Vector > sphere_type;
  typedef boost::python::bases< primitive::Sphere< Vector > > sphere_bases_type;

  typedef typename sphere_type::vector_type vector_type;
  typedef typename vector_type::value_type value_type;
  typedef Discrete discrete_type;

  // Exported indexers - indexing module
  #pragma clang diagnostic push
  #pragma clang diagnostic ignored "-Wmultichar"
  typedef boost::mpl::vector<
    boost::mpl::pair<
      indexing::Linear< sphere_type, vector_type >,
      boost::mpl::string< 'line', 'ar', '_sph', 'eres' >
      >,
    boost::mpl::pair<
      indexing::Hash< sphere_type, vector_type, discrete_type >,
      boost::mpl::string< 'hash', '_sph', 'eres' >
      >
    > indexers;

  // Exported checkers - accessibility module
  typedef boost::mpl::vector<
    boost::mpl::pair<
      containment::Checker< sphere_type, containment::PurePythagorean< false > >,
      boost::mpl::string< 'pyth', 'agor', 'ean' >
      >
    > checkers;
  #pragma clang diagnostic pop
};

struct python_calculator_export
{
  static void wrap()
  {
    using namespace boost::python;
    typedef python::array_adaptor< scitbx::vec3< double > > coordinate_adaptor_type;
    typedef python::array_adaptor< double > radius_adaptor_type;

    class_< coordinate_adaptor_type >( "_coordinate_adaptor", no_init )
      .def( init< object, object >( ( arg( "array" ), arg( "transformation" ) ) ) )
      .def( "__getitem__", &coordinate_adaptor_type::operator [] )
      .def( "__len__", &coordinate_adaptor_type::size )
      ;
    class_< radius_adaptor_type >( "_radius_adaptor", no_init )
      .def( init< object, object >( ( arg( "array" ), arg( "transformation" ) ) ) )
      .def( "__getitem__", &radius_adaptor_type::operator [] )
      .def( "__len__", &radius_adaptor_type::size )
      ;

    typedef calculator::SimpleCalculator<
      coordinate_adaptor_type,
      radius_adaptor_type
      > calculator_type;

    class_< calculator_type >( "calculator", no_init )
      .def(
        init< coordinate_adaptor_type, radius_adaptor_type, double, std::size_t, double, int >(
          (
            arg( "coordinate_adaptor" ),
            arg( "radius_adaptor" ),
            arg( "probe" ) = 1.4,
            arg( "sampling_point_count" ) = 960,
            arg( "cubesize" ) = 7.0,
            arg( "margin" ) = 1
            )
          )
        )
      .def( "accessible_surface_points", &calculator_type::accessible_points, arg( "index" ) )
      .def( "accessible_surface_area", &calculator_type::accessible_surface_area, arg( "index" ) )
      .def(
        "is_overlapping_sphere",
        &calculator_type::is_overlapping_sphere,
        ( arg( "centre" ), arg( "radius" ) )
        )
      ;

    typedef calculator::ConstRadiusCalculator< coordinate_adaptor_type >
      const_radius_calculator_type;

    class_< const_radius_calculator_type >( "const_radius_calculator", no_init )
      .def(
        init< coordinate_adaptor_type, double, double, std::size_t, double, int >(
          (
            arg( "coordinate_adaptor" ),
            arg( "radius" ),
            arg( "probe" ) = 1.4,
            arg( "sampling_point_count" ) = 960,
            arg( "cubesize" ) = 7.0,
            arg( "margin" ) = 1
            )
          )
        )
      .def(
        "accessible_surface_points",
        &const_radius_calculator_type::accessible_points,
        arg( "index" )
        )
      .def(
        "accessible_surface_area",
        &const_radius_calculator_type::accessible_surface_area,
        arg( "index" )
        )
      .def(
        "is_overlapping_sphere",
        &const_radius_calculator_type::is_overlapping_sphere,
        ( arg( "centre" ), arg( "radius" ) )
        )
      ;
  }

};

} // namespace <anonymous>
} // namespace asa
} // namespace geometry
} // namespace mmtbx

BOOST_PYTHON_MODULE(mmtbx_geometry_asa_ext)
{
  // Dependency
  boost::python::object primitives = boost::python::import( "mmtbx_geometry_primitive_ext" );

  mmtbx::geometry::asa::python_exports<
    mmtbx::geometry::asa::python_export_traits< scitbx::vec3< double > >
    >::wrap();
  mmtbx::geometry::asa::python_calculator_export::wrap();
}


#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "newobjectdialog.h"
#include "selectobjectdialog.h"
#include "addobjectdialog.h"
#include "initialmessagedialog.h"
#include "objectsinformation.h"
#include "filtervaluesdialog.h"
#include "newobjectdialog.h"
#include "qsr.h"

#include "QMessageBox"
#include "QCloseEvent"
#include "QFileDialog"
#include "QDesktopServices"
#include "QUrl"
#include "QListWidgetItem"

#include <pcl/filters/project_inliers.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/common/angles.h>

#include <math.h>
#include <eigen3/Eigen/Dense>

#include <iostream>
#include <fstream>

#include <boost/property_tree/xml_parser.hpp>
#include <boost/filesystem/path.hpp>

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    _ui(new Ui::MainWindow),
    _cloud (new pcl::PointCloud<pointT>),
    _cloudUndo (new pcl::PointCloud<pointT>)
{
    init();
}

MainWindow::~MainWindow(){
    delete _ui;
}

///////////////////////////////////////////////////////////////
// The following functions are all the slots' functions used //
// when the user interact with the application               //
///////////////////////////////////////////////////////////////

// Open button action
void MainWindow::on_actionOpen_triggered(){
    // Open the pcd file
    open_pcd_file();

    //////////++++++++++++///////////////////
    // Crear xml parser per fer-ho ///////////
    //////++++++++++/////////////////
    // Save the current directory as a historial
    if(!_fileName.isEmpty()){
        QString info_doc = qApp->applicationDirPath();
        info_doc.remove("bin");
        info_doc.append("/info_app.xml");

        boost::filesystem::path p(_fileName.toStdString());
        std::string path = p.parent_path().string();
        _lastDir = QString::fromStdString(path);

        boost::property_tree::ptree root;
        read_xml(info_doc.toStdString(), root);

        bool showInitial = root.get<bool>("showInitial");

        root.clear();
        root.put("showInitial", showInitial);
        root.put("lastDirectory", path);
        boost::property_tree::xml_writer_settings<char> settings(' ', 3);
        write_xml(info_doc.toStdString(), root, std::locale(), settings);
    }
}

// Exit button action
void MainWindow::on_actionExit_triggered(){
    if(_cloudModified){
        int r = QMessageBox::warning(this,
                                     tr("Changes not saved"),
                                     tr("The point cloud has been changed. Do you want to save the changes?"),
                                     QMessageBox::Yes , QMessageBox::No);
        if (r == QMessageBox::Yes) {
            on_actionSave_PCD_File_triggered();
            //if(!_cloudModified) MainWindow::close();
        }
        else
            MainWindow::close();
    }
    else{
        MainWindow::close();
        }
    }

// Next 6 functions: Slots for the buttons to
// modify the x,y,z coordinate of the pose
void MainWindow::on_buttonXMore_clicked(){
    _objectPose.x += 0.01;
    actualizePose();
}

void MainWindow::on_buttonXLess_clicked(){
    _objectPose.x -= 0.01;
    actualizePose();
}

void MainWindow::on_buttonYMore_clicked(){
    _objectPose.y += 0.01;
    actualizePose();
}

void MainWindow::on_buttonYLess_clicked(){
    _objectPose.y -= 0.01;
    actualizePose();
}

void MainWindow::on_buttonZMore_clicked(){
    _objectPose.z += 0.01;
    actualizePose();
}

void MainWindow::on_buttonZLess_clicked(){
    _objectPose.z -= 0.01;
    actualizePose();
}

// Next 6 functions: Slots for the buttons to modify
// the roll, pitch, yaw angles of the pose
void MainWindow::on_buttonYawMore_clicked(){
    _boxYaw += M_PI/180;
    if (_boxYaw > 2*M_PI) _boxYaw = 0;
    actualizeRotation();
}

void MainWindow::on_buttonYawLess_clicked(){
    _boxYaw -= M_PI/180;
    if (_boxYaw < 0) _boxYaw = 2*M_PI;
    actualizeRotation();
}

void MainWindow::on_buttonPitchMore_clicked(){
    _boxPitch += M_PI/180;
    if (_boxPitch > 2*M_PI) _boxPitch = 0;
    actualizeRotation();
}

void MainWindow::on_buttonPitchLess_clicked(){
    _boxPitch -= M_PI/180;
    if (_boxPitch < 0) _boxPitch = 2*M_PI;
    actualizeRotation();
}

void MainWindow::on_buttonRollMore_clicked(){
    _boxRoll += M_PI/180;
    if (_boxRoll > 2*M_PI) _boxRoll = 0;
    actualizeRotation();
}

void MainWindow::on_buttonRollLess_clicked(){
    _boxRoll -= M_PI/180;
    if (_boxRoll < 0) _boxRoll = 2*M_PI;
    actualizeRotation();
}

// Next 9 functions: Slots to modify the width, length
// and height value of the bounding box
void MainWindow::on_boxWidth_editingFinished(){
    if(_insertingObject){
        _boxWidth = _ui->boxWidth->value()/100;
        actualizeBoxWidth();
    }

    if(!_itemSelected && !_insertingObject){
        _ui->boxWidth->setValue(0);
    }
}

void MainWindow::on_boxWidthMore_clicked(){
    if(_insertingObject){
        _boxWidth += 0.01;
        _ui->boxWidth->setValue(_boxWidth*100);
        actualizeBoxWidth();
    }
}

void MainWindow::on_boxWidthLess_clicked(){
    if(_insertingObject){
        _boxWidth -= 0.01;
        _ui->boxWidth->setValue(_boxWidth*100);
        actualizeBoxWidth();
    }
}

void MainWindow::on_boxLength_editingFinished(){
    if(_insertingObject){
    _boxLength = _ui->boxLength->value()/100;
       actualizeBoxLength();
    }

    if(!_itemSelected && !_insertingObject){
        _ui->boxLength->setValue(0);
    }
}

void MainWindow::on_boxLengthMore_clicked(){
    if(_insertingObject){
        _boxLength += 0.01;
        _ui->boxLength->setValue(_boxLength*100);
        actualizeBoxLength();
    }
}

void MainWindow::on_boxLengthLess_clicked(){
    if(_insertingObject){
        _boxLength -= 0.01;
        _ui->boxLength->setValue(_boxLength*100);
        actualizeBoxLength();
    }
}

void MainWindow::on_boxHeight_editingFinished(){
    if(_insertingObject){
        _boxHeight = _ui->boxHeight->value()/100;
        actualizeBoxHeight();
    }

    if(!_itemSelected && !_insertingObject){
        _ui->boxHeight->setValue(0);
    }
}

void MainWindow::on_boxHeightMore_clicked(){
     if(_insertingObject){
         _boxHeight += 0.01;
         _ui->boxHeight->setValue(_boxHeight*100);
         actualizeBoxHeight();
     }
}

void MainWindow::on_boxHeightLess_clicked(){
    if(_insertingObject){
        _boxHeight -= 0.01;
        _ui->boxHeight->setValue(_boxHeight*100);
        actualizeBoxHeight();
    }
}

// Save pcd file action
void MainWindow::on_actionSave_PCD_File_triggered(){

    if(_pcdLoaded){

    if (_firstsave){
        save_Option(1); // Save
    }
    else
    {
            // Save the .pcd file

            QString fileNamePCD = _fileName;

            std::string fileName = _fileName.toStdString();
            fileName.erase(fileName.find_last_of(".")+1, 3);
            fileName.append("xml");

            QString fileNameXML =  QString::fromStdString(fileName);

            pcl::io::savePCDFileBinary(fileNamePCD.toStdString(), *_cloud);
            _cloudModified = false;

            // Save the objects' information
            if(objectsInfo.getDeskLength() != -1){
                objectsInfo.exportObjectsInformation(fileNameXML, _fileName, _scenario);
            }
            else
                QMessageBox::warning(this,
                                     "XML file not saved",
                                     "The objects' information has not been saved because there is no information.");

        }
    }
    else
        QMessageBox::warning(this,
                             "No PCD file opened",
                             "Open a PCD file and try again..");
}

// Actions to take when an item in the information tree
// widget is selected
void MainWindow::on_treeWidget_itemSelectionChanged(){
    // If the object has been changed without confirm
    // ask if the user want to lose the changes
    if(_insertingObject && _objectModifed){
        int r = QMessageBox::warning(this,
                                     tr("Changes not confirmed"),
                                     tr("You have changed the object without confirm. Do you want to save the changes introduced in the object?"),
                                     QMessageBox::Yes , QMessageBox::No);
        if (r == QMessageBox::Yes) {
            _boxLength = _ui->boxLength->value()/100;
            _boxWidth = _ui->boxWidth->value()/100;
            _boxHeight = _ui->boxHeight->value()/100;
            confirmObjectPosition();
        }
    }

    _insertingObject = false;
    _itemSelected = false;

    QString text = _ui->treeWidget->currentItem()->text(0);

    // If objects is selected, all the objects are shown
    if(text.toStdString() == "Objects"){
        _ui->treeWidget->currentItem()->setExpanded(true);
        viewInteractor.removeBoundingBox();
        std::vector<object> objectList = objectsInfo.getObjectList();
        viewInteractor.highligthAllObjects(_cloud, objectList);
        viewInteractor.render();
        clearPoseInfo();
    }
    // If only one object is selected it is shown with the bounding box
    else if (objectsInfo.getIndex(text) != -1){
        _itemSelected = true;
        // Get the information about the current object selected
        int index = objectsInfo.getIndex(text);
        pcl::PointIndices indices;
        pcl::PointXYZ pose;
        float roll, pitch, yaw, boxLength, boxWidth, boxHeight;
        objectsInfo.getGeometry(index, pose, roll, pitch, yaw, boxLength, boxWidth, boxHeight);
        viewInteractor.defineBoundingBox(pose, roll, pitch, yaw, boxLength, boxWidth, boxHeight);
        objectsInfo.getIndices(index, &indices);

        // Draw the box and highlight the points inside
        drawBoxAndPoints();

        // Actualize the information displayed
        _ui->boxLength->setValue(boxLength*100);
        _ui->boxWidth->setValue(boxWidth*100);
        _ui->boxHeight->setValue(boxHeight*100);
        setPoseInfo(pose, roll, pitch, yaw);

        // Prepare to be modify while is selected
        _objectName = objectsInfo.nameOfObject(index);
        _objectPose = pose;
        _boxRoll = roll;
        _boxPitch = pitch;
        _boxYaw = yaw;
        _insertingObject = true;
        _objectModifed = false;
    }
    else if(text.toStdString() == _scenario.toStdString()){
        _ui->treeWidget->currentItem()->setExpanded(true);
    }

    else {
        // Don't shown any object
        viewInteractor.cleanViewer();
        //clearPoseInfo();
    }
}

// Automatic plane detection
void MainWindow::on_actionAutomatic_plane_detection_triggered(){
    if(_pcdLoaded){

        viewInteractor.getPose();

        // Automatic detection of the desk plane and move to the plane x-y
        cloudModifier.automaticTableDetection(_cloud, _cloud);

        visualize();
        _cloudModified = true;

        // If the automatic detection of the plane is not correct, it can be
        // calculated manually by picking three points.
        if(_showInfoMsgs) QMessageBox::information(this,
                                                   "Correct plane?",
                                                   "If the plane is not correct proceed to do the manual plane detection or apply z rotation.");
        _planeDefined=true;
    }
    else{
        QMessageBox::warning(this, "Error", "Firstly, you must load a pcd file.");
    }
}

// Rotate the point cloud 180º in the z axis
void MainWindow::on_actionRotate_z_180_triggered(){
    if(!_pcdLoaded)
    {
        QMessageBox::warning(this, "Error", "Firstly, you must load a pcd file.");
    }
    else{
        cloudModifier.rotate_z_180(_cloud, _cloud);
        visualize();
        _cloudModified = true;
    }
}

// Manual plane definition
void MainWindow::on_actionManual_plane_definition_triggered(){
    std::vector<pointT> tablePoints;

    //  First is checked if the pcd file is loaded.
    if(!_pcdLoaded)
    {
        QMessageBox::warning(this, "Error", "Firstly, you must load a pcd file.");
    }
    else{
        // First pick three points of the table plane
        if(_showInfoMsgs) QMessageBox::information(this,
                                                   "Pick points",
                                                   "Pick three points of the table with shift+left mouse button.");

        viewInteractor.getPointsPicked(3, &tablePoints);

        // Manual plane definition of the desk plane and move to the plane x-y
        cloudModifier.manualTableDetection(_cloud, _cloud, tablePoints);
        _planeDefined = true;
        _ui->actionDesk_segmentation->setEnabled(true);
        visualize();
        _cloudModified = true;
    }
}

// Desk segmentation
void MainWindow::on_actionDesk_segmentation_triggered(){
    if(!_pcdLoaded | !_planeDefined)
    {
        if(!_pcdLoaded)
        {
            QMessageBox::warning(this, "Error", "Firstly, you must load a pcd file.");
        }
        else
        {
            QMessageBox::warning(this, "Error", "Firstly, you must define the plane.");
        }
    }
    else{
        // Ask if it will be a desk, floor, etc
        chooseObjectDialog env(0,1);
        env.exec();
        _scenario = env.getObjectName();
        if(!_scenario.isEmpty()){
            QTreeWidgetItem *itm_scenario = new QTreeWidgetItem(_ui->treeWidget);
            itm_scenario->setText(0, _scenario);
            _ui->treeWidget->insertTopLevelItem(0,itm_scenario);

            // Insert the objects items
            QTreeWidgetItem *itm_objects = new QTreeWidgetItem(_ui->treeWidget);
            itm_objects->setText(0, QString::fromStdString("Objects"));
            _ui->treeWidget->insertTopLevelItem(0,itm_objects);

            pointT pointPicked;
            float table_length, table_width;

            // Copy the point cloud if the user want to undo the segmentation
            pcl::copyPointCloud(*_cloud, *_cloudUndo);

            // Segmentate the table given three points picked by the user

            // Pick the first point needed: lower left corner
            if(_showInfoMsgs) QMessageBox::information(this,
                                                       "Pick a point",
                                                       "Pick the lower left corner of the table with shift+left mouse button.");

            viewInteractor.getPointPicked(&pointPicked);

            // Move the pointcloud to the lower left corner of the table
            cloudModifier.translate_on_plane_x_y(_cloud, _cloud, pointPicked);
            visualize();

            // Pick the second point needed: lower right corner
            if(_showInfoMsgs) QMessageBox::information(this,
                                                       "Pick a point",
                                                       "Pick the lower right corner of the table with shift+left mouse button");
            viewInteractor.getPointPicked(&pointPicked);

            // Align the x axis with the lower edge of the table and calculate the table length
            cloudModifier.align_x_with_edge(_cloud, _cloud, pointPicked);
            pcl::PointXYZ origin(0,0,0);
            table_length = pcl::euclideanDistance(origin, pointPicked);
            visualize();

            // Pick the third point needed: one of the upper edge of the table
            if(_showInfoMsgs) QMessageBox::information(this,
                                                       "Pick a point",
                                                       "Pick a point on the upper edge of the table with shift+left mouse button");
            viewInteractor.getPointPicked(&pointPicked);

            // Eliminate points below the table

            // Min value of 0 remove points from the table -> set to -0.02
            //cloudModifier.filter_axis(_cloud, _cloud, "z", -0.02, 5);

            //Eliminate points at right and left side of the table
             //cloudModifier.filter_axis(_cloud, _cloud, "x", 0, table_length);

            //Eliminate points upper and lower side the table
            table_width = pointPicked.y;
            //cloudModifier.filter_axis(_cloud, _cloud, "y", 0, table_width);

            //Pass the table dimensions
            objectsInfo.setDeskDimensions(table_length, table_width);

            //Insert length information in the QtreeWidget
            displayDeskLengthInfo();

            //Insert width information in the QtreeWidget
            displayDeskWidthInfo();

            visualize();
            _cloudModified = true;
            _planeSegmentated = true;
            _ui->actionUndo->setEnabled(true);

            // Remind to save it
            if(_showInfoMsgs) QMessageBox::information(this,
                                                       "Save the progress",
                                                       "It is recommended to save now the point cloud and the plane information.");

            // Desactivate this action
            _ui->actionDesk_segmentation->setEnabled(false);
        }
    }
}

// Insert new annotation of an object
void MainWindow::on_actionInsert_new_object_triggered(){
    //  First is checked if the pcd file is loaded.
    if(!_pcdLoaded | !_planeSegmentated){

        if(!_pcdLoaded)
            QMessageBox::warning(this, "Error", "Firstly, you must load a pcd file.");
        else
            QMessageBox::warning(this, "Error", "Firstly, you must segmentate the plane.");
    }

    else{

        if(_insertingObject && _objectModifed){
            int r = QMessageBox::warning(this,
                                         tr("Changes not confirmed"),
                                         tr("You have changed the object without confirm. Do you want to save the changes introduced in the object?"),
                                         QMessageBox::Yes , QMessageBox::No);
            if (r == QMessageBox::Yes) {
                confirmObjectPosition();
            }

            else{

               viewInteractor.removeBoundingBox();
            }
        }

        // Select which object is desired to introduce
        AddObject addobj;
        addobj.exec();
        _objectName = addobj.getObjectName();

        if(!_objectName.isEmpty()){
            if(objectsInfo.existsObject(_objectName)){
                QMessageBox::warning(this, "Error", "This object already exists. Insert another name.");
            }
            else{
                _insertingObject = true;

                // Select the points for the initial object position
                if(_showInfoMsgs) QMessageBox::information(this,
                                         "Pick a point",
                                         "Pick the next 4 points of the object with shift+left mouse button:\n - Left corner\n - Right corner\n - Upper left corner\n - Highest part ");
                std::vector<pointT> clickedPoints;
                viewInteractor.getPointsPicked(4, &clickedPoints);

                // First guess is that the object is lying on the table
                _objectPose.x = clickedPoints[0].x;
                _objectPose.y = clickedPoints[0].y;
                _objectPose.z = 0.0;

                // To messure distance I am not using the z component of the picked points
                pcl::PointXYZ point1_x_y, point2_x_y, point3_x_y;
                point1_x_y.x = clickedPoints[0].x; point1_x_y.y = clickedPoints[0].y;
                point2_x_y.x = clickedPoints[1].x; point2_x_y.y = clickedPoints[1].y;
                point3_x_y.x = clickedPoints[2].x; point3_x_y.y = clickedPoints[2].y;

                // Object size
                _boxLength = pcl::euclideanDistance(point1_x_y, point2_x_y);
                _boxWidth = pcl::euclideanDistance(point1_x_y, point3_x_y);
                _boxHeight = clickedPoints[3].z;

                // Calculte the angle between the x axis and the object front edge
                Eigen::Vector4f line_dir(clickedPoints[1].x - clickedPoints[0].x,
                                         clickedPoints[1].y - clickedPoints[0].y,
                                         0, 0);
                Eigen::Vector4f x_axis(1,0,0,0);
                _boxYaw = pcl::getAngle3D (x_axis, line_dir);
                if(clickedPoints[1].y < clickedPoints[0].y){
                    _boxYaw = 2*M_PI-_boxYaw;
                }

                // Define the intial bounding box
                // New feature
                _boxRoll = 0;
                _boxPitch = 0;
                viewInteractor.defineBoundingBox(_objectPose, _boxRoll, _boxPitch, _boxYaw,
                                                 _boxLength, _boxWidth, _boxHeight);
                drawBoxAndPoints();

                // Set the value at the adecuated spin Box
                _ui->boxLength->setValue(_boxLength*100);
                _ui->boxWidth->setValue(_boxWidth*100);
                _ui->boxHeight->setValue(_boxHeight*100);

                // Actualize pose info
                actualizePoseInfo();
             }
        }
    }
}

// Confirm object position
void MainWindow::on_actionConfirm_position_triggered(){
    confirmObjectPosition();
}

// Delete object in the object list
void MainWindow::on_actionDelete_object_triggered(){
    if(objectsInfo.numberOfObjects()>0){
        // Select the object to delete from the annotated objects
        selectObject chooseObjectToDelete(0,2);
        QStringList listOfCurrentObjects = objectsInfo.getListOfObjects();
        chooseObjectToDelete.setObjectList(listOfCurrentObjects);
        chooseObjectToDelete.exec();
        QString objectToDelete = chooseObjectToDelete.getObjectName();

        if(!objectToDelete.isEmpty()){
            // Get the index and delete it
            int index = objectsInfo.getIndex(objectToDelete);
            objectsInfo.deleteObject(objectToDelete);

            // Remove it from the tree widget
            QTreeWidgetItem *child = _ui->treeWidget->topLevelItem(1)->takeChild(index);
            _ui->treeWidget->topLevelItem(1)->removeChild(child);
        }
    }
    else{
        QMessageBox::warning(this, "No objects", "No objects to remove.");
    }
}

// Import objects' information from a .xml file
void MainWindow::on_actionImport_objects_info_triggered(){
    // Name of the .xml file to import equal as the cloud file loaded
    std::string fileName = _fileName.toStdString();
    fileName.erase(fileName.find_last_of(".")+1, 3);
    fileName.append("xml");

    QString loadFileName = QFileDialog::getOpenFileName(this,
                                                        tr("Open file"),
                                                        QString::fromStdString(fileName),
                                                        tr("Xml Files (*.xml)"));
    if(!loadFileName.isEmpty()){
        // Remove all the information saved in the tree widget
        clearInfoTreeWidget();

        // Remove all the object's information saved
        objectsInfo.clear();

        // Clean the viewer
        viewInteractor.cleanViewer();

        // Import the information
        _scenario = objectsInfo.importObjectsInformation(loadFileName);

        // Insert the items in the tree widget
        QTreeWidgetItem *itm_scenario = new QTreeWidgetItem(_ui->treeWidget);
        itm_scenario->setText(0, _scenario);
        _ui->treeWidget->insertTopLevelItem(0,itm_scenario);

        // Insert the objects items
        QTreeWidgetItem *itm_objects = new QTreeWidgetItem(_ui->treeWidget);
        itm_objects->setText(0, QString::fromStdString("Objects"));
        _ui->treeWidget->insertTopLevelItem(0,itm_objects);

        // Actualize tree widget with the loaded objects and the table size
        actualizeInformationTreeWidget();

        // Allow the user insert new objects
        _planeSegmentated = true;
    }
    else{
        QMessageBox::warning(this, "Error", "File not load.");
    }
}

// Export the current information about the objects annotated
void MainWindow::on_actionExport_objects_info_triggered(){
    if(objectsInfo.getDeskLength() != -1){
        // Set the same name of the cloud file but with the extension xml
        std::string fileName = _fileName.toStdString();
        fileName.erase(fileName.find_last_of(".")+1, 3);
        fileName.append(".xml");

        QString saveFileName = QFileDialog::getSaveFileName(this,
                                                            tr("Export objects' information"),
                                                            QString::fromStdString(fileName),
                                                            tr("Xml Files(*.xml)"));
        if(!saveFileName.isEmpty()){
            objectsInfo.exportObjectsInformation(saveFileName, _fileName, _scenario);
        }
        else{
            QMessageBox::warning(this, "Error", "File not saved.");
        }
    }
    else {
        QMessageBox::warning(this,
                             "Error",
                             "Length and width of the plane not defined. Please, proceed to do the plane segmentation");
    }
}

// Help button. Open the user guide
void MainWindow::on_actionHelp_triggered(){
    QString help_doc = qApp->applicationDirPath();
    help_doc.remove("bin");
    help_doc.append("documentation/user_guide.pdf");
    QDesktopServices::openUrl(QUrl(help_doc,  QUrl::TolerantMode));
}

// Remove or insert coordinate system
void MainWindow::on_actionCoordinate_system_toggled(bool arg1){
    if(arg1)
        viewInteractor.addCoordinateSystem();
    else
        viewInteractor.removeCoordinateSystem();
}

// Next 5 functions: Slots to change the position of
// the camera view
void MainWindow::on_actionUp_triggered(){
    viewInteractor.setCameraPose(0.5, 0.5, 3, 0.5, 0.5, 0, 0, 1, 0);
}

void MainWindow::on_actionFront_triggered(){
    viewInteractor.setCameraPose(0.5, -2, 0, 0.5, 0, 0, 0, 1, 1);
}

void MainWindow::on_actionLeft_triggered(){
    viewInteractor.setCameraPose(-2, 0.5, 0, 0, 0.5, 0, 1, 0, 1);
}

void MainWindow::on_actionRight_triggered(){
    viewInteractor.setCameraPose(2, 0.5, 0, 0, 0.5, 0, -1, 0, 1);
}

void MainWindow::on_actionBack_triggered(){
    viewInteractor.setCameraPose(1, 3, 0, 1, 0, 0, 0, 1, 1);
}

// Action to use when a value on the pose widget has been
// changed manualy
void MainWindow::on_poseInfo_itemChanged(QTreeWidgetItem *item, int column){
    if(_insertingObject){
        bool isNumber;
        float insertedNumber = item->text(1).toFloat(&isNumber);

        if(isNumber){
            if(item->text(0).startsWith(QString::fromStdString("X (m)"))){
                _objectPose.x = insertedNumber;
                actualizePose();
            }
            else if(item->text(0).startsWith(QString::fromStdString("Y (m)"))){
                _objectPose.y = insertedNumber;
                actualizePose();
            }
            else if(item->text(0).startsWith(QString::fromStdString("Z (m)"))){
                _objectPose.z = insertedNumber;
                actualizePose();
            }
            else if(item->text(0).startsWith(QString::fromStdString("Roll"))){
                if(insertedNumber <= 360.0 && insertedNumber >=0.0){
                    _boxRoll = pcl::deg2rad(insertedNumber);
                    actualizeRotation();
                }
                else
                    QMessageBox::warning(this, "Incorrect value", "Insert an angle value between 0 and 360.");
            }
            else if(item->text(0).startsWith(QString::fromStdString("Pitch"))){
                if(insertedNumber <= 360.0 && insertedNumber >=0.0){
                    _boxPitch = pcl::deg2rad(insertedNumber);
                    actualizeRotation();
                }
                else
                    QMessageBox::warning(this, "Incorrect value", "Insert an angle value between 0 and 360.");
            }
            else if(item->text(0).startsWith(QString::fromStdString("Yaw"))){
                if(insertedNumber <= 360.0 && insertedNumber >=0.0){
                    _boxYaw = pcl::deg2rad(insertedNumber);
                    actualizeRotation();
                }
                else
                    QMessageBox::warning(this, "Incorrect value", "Insert an angle value between 0 and 360.");
            }
        }
        else{
            QMessageBox::warning(this, "Format error", "Value incorrect. Please, insert a number.");
            item->setText(1, QString::number(0));
        }
    }
    else if(!_itemSelected)
        item->setText(1, QString::number(0));

    //    if(!_insertingObject && !_itemSelected)
    //        item->setText(1, QString::number(0));
}

// Save the pcd file and the objects' information
void MainWindow::save_Option(int type)
{
    QString fileNamePCD = "";

if(_pcdLoaded){

    if (type == 1){
    fileNamePCD = QFileDialog::getSaveFileName(this,
                                                       tr("Save PCD and XML files"));
                                                       //_fileName.remove(_fileName.size()-4,4),
                                                       //tr(""));
    }

    else{
    fileNamePCD = QFileDialog::getSaveFileName(this,
                                                        tr("Save As PCD and XML files"));
                                                         //_fileName.remove(_fileName.size()-4,4),
                                                         //tr(""));
    }

    if(fileNamePCD != ""){
    _firstsave = false;
    }

    if (!fileNamePCD.contains(".", Qt::CaseInsensitive))
        /*fileNamePCD = _fileName.remove(_fileName.indexOf("."),_fileName.length()-fileName.indexOf("."));
    else*/
    fileNamePCD.append(".pcd");

    std::string fileName = _fileName.remove(_fileName.indexOf("."),_fileName.length()-_fileName.indexOf(".")).toStdString();
    fileName.append(".xml");

    QString fileNameXML =  QString::fromStdString(fileName);

    if(!fileNamePCD.isEmpty()){
        // Save the .pcd file
         pcl::io::savePCDFileBinary(fileNamePCD.toStdString(), *_cloud);
        _fileName = fileNamePCD;
        _cloudModified = false;

        // Save the objects' information
        //fileNameXML.append(".xml");
        if(objectsInfo.getDeskLength() != -1){
            objectsInfo.exportObjectsInformation(fileNameXML, _fileName, _scenario);
        }
        else
            QMessageBox::warning(this,
                                 "XML file not saved",
                                 "The objects' information has not been saved because there is no information.");

    }
    else{
        QMessageBox::warning(this, "Error", "Files not saved.");
    }
}

else
    QMessageBox::warning(this,
                         "No PCD file opened",
                         "Open a PCD file and try again..");
}

// Enable or disable info messages
void MainWindow::on_actionShow_info_messages_toggled(bool arg1)
{
    if(arg1)
        _showInfoMsgs = true;
    else
        _showInfoMsgs = false;
}

// Undo function
void MainWindow::on_actionUndo_triggered()
{
    // Reload the previous point cloud and visualize it
    pcl::copyPointCloud(*_cloudUndo, *_cloud);
    visualize();

    // If the undo is done after the segmentation, delete the information
    // of the tree widget and allow the user to do the segmentation again
    if(!_ui->actionDesk_segmentation->isEnabled()){
        clearInfoTreeWidget();
        _ui->actionDesk_segmentation->setEnabled(true);
        objectsInfo.clear();
    }

    // Disable this action
    _ui->actionUndo->setEnabled(false);
}

// Downsampling function
void MainWindow::on_actionDownsample_point_cloud_triggered()
{
    // Call the dialog to introduce the filter values
    filtervaluesdialog filterDialog;
    filterDialog.exec();
    float leaf = filterDialog.getLeafSize();

    // Copy the filter to Undo function
    pcl::copyPointCloud(*_cloud, *_cloudUndo);

    // Create the filtering object
    pcl::VoxelGrid<pointT> sor;
    sor.setInputCloud (_cloud);
    sor.setLeafSize(leaf, leaf, leaf);
    //    sor.setLeafSize (0.01f, 0.01f, 0.01f);
    sor.filter (*_cloud);
    visualize();

    // Enable Undo function
    _ui->actionUndo->setEnabled(true);
}

// QSR values displayed in the terminal
void MainWindow::on_actionQSR_values_triggered()
{
    if(objectsInfo.numberOfObjects()>1){
        qsr QSR(objectsInfo.getObjectList());
        QSR.calculateQSRRight();
        QSR.calculateQSRLeft();
        QSR.calculateQSRInFront();
        QSR.calculateQSRBehind();
    }
    else QMessageBox::warning(this,
                              "Error",
                              "It is not possible to calculate the QSR values. Add more objects or load an annotation.");
}

// Obtain a description of the scene using QSR
void MainWindow::on_actionDescription_of_scene_using_QSR_triggered()
{
    if(objectsInfo.numberOfObjects()>1){
        qsr QSR(objectsInfo.getObjectList());
        QString description = QSR.getDescription();

        QMessageBox::information(this,
                                 "Description of the scene",
                                 description);

    }
    else QMessageBox::warning(this,
                              "Error",
                              "It is not possible to obtain a description of the scene. Annotate some objects or load an annotation.");
}

// Save all the QSR values in a .txt file
void MainWindow::on_actionSave_QSR_in_txt_file_triggered()
{
    if(objectsInfo.numberOfObjects()>1){
        qsr QSR(objectsInfo.getObjectList());
        std::string fileName = _fileName.toStdString();
        fileName.erase(fileName.find_last_of(".")+1, 3);
        fileName.append("txt");

        QString file = QFileDialog::getSaveFileName(this,
                                                        tr("Save file"),
                                                        QString::fromStdString(fileName),
                                                        tr("Text File(*.txt)"));
        if(!file.isEmpty()){
            std::ofstream outputFile;
            outputFile.open (file.toAscii(), ios::trunc);
            QString allValues = QSR.getAllValues();
            outputFile << allValues.toStdString();
            outputFile.close();
        }
        else{
            QMessageBox::warning(this, "Error", "Text file not saved.");
        }

    }
    else QMessageBox::warning(this,
                              "Error",
                              "It is not possible to obtain a description of the scene. Annotate some objects or load an annotation.");
}

///////////////////////////////////////////////////////
// The following functions are used inside the above //
// functions (slots)                                 //
///////////////////////////////////////////////////////

// Initialization
void MainWindow::init(){
    //Initialization of the UI
    _ui->setupUi(this);

    // Start the bool variables
    _pcdLoaded = false;
    _pcdLoadError = false;
    _planeDefined = false;
    _planeSegmentated = false;
    _insertingObject = false;
    _objectModifed = false;
    _cloudModified = false;
    _itemSelected = false;
    _showInfoMsgs = true;

    _firstsave = false;

    //Set to black the background color of the QVTKWidget
    QPalette palette = _ui->qvtkWidget->palette();
    palette.setColor(QPalette::Background, QColor("black"));
    _ui->qvtkWidget->setPalette(palette);
    _ui->qvtkWidget->setAutoFillBackground(true);

    //Windows title
    setWindowTitle("3D Annotation Tool");

    // Clear tree widget
    _ui->treeWidget->clear();

    // Read the information left for the user in the previous session
    QString info_doc = qApp->applicationDirPath();
    info_doc.remove("bin");
    info_doc.append("/info_app.xml");;

    boost::property_tree::ptree root;
    read_xml(info_doc.toStdString(), root);

    _showInitialMsg = root.get<bool>("showInitial");
    _lastDir = QString::fromStdString(root.get<std::string>("lastDirectory"));

    // Set the render windowsn setup the interactor and register the callback
    _ui->qvtkWidget->SetRenderWindow(viewInteractor.getRenderWindow(_ui->qvtkWidget->width(), _ui->qvtkWidget->height()));
    viewInteractor.setupInteractor(_ui->qvtkWidget->GetInteractor(), _ui->qvtkWidget->GetRenderWindow());
    viewInteractor.registerCallback();
}

// Used to visualized the point cloud
void MainWindow::visualize(){
    if(_pcdLoaded || _pcdLoadError){
        // Clean the viewer if another pcd has been loaded before
        viewInteractor.cleanAll();
    }
    // Visualize the point cloud and set the render windows
    viewInteractor.visualizePointCloud(_cloud);

    // Add the coordinate system if it's requiered
    if(_ui->actionCoordinate_system->isChecked()){
        viewInteractor.addCoordinateSystem();
    }
}

// Exit
bool MainWindow::okToExit(){
    int r = QMessageBox::warning(this, tr("Exit"),
                                 tr("Are you sure you want to exit?"),
                                 QMessageBox::Yes , QMessageBox::No);
    if (r == QMessageBox::Yes) {
        on_actionExit_triggered();
    } else
        return false;
}

// Open the pcd file
void MainWindow::open_pcd_file(){
    if (_pcdLoaded){
        int r = QMessageBox::warning(this,
                                     tr("PCD open"),
                                     tr("Do you want to open another pcd file?"),
                                     QMessageBox::Yes , QMessageBox::No);
        if (r == QMessageBox::Yes) {
            _fileName = QFileDialog::getOpenFileName(this,
                                                     tr("Open file"),
                                                     _lastDir,
                                                     tr("PCD Files(*.pcd)"));
            if(!_fileName.isEmpty()){
                load_pcd_file(_fileName);
                _firstsave = true;

            }
            else{
                QMessageBox::warning(this, "Error", "PCD file not loaded, do it again.");
                _pcdLoadError=true;
            }
        }
    }
    else{
        _fileName = QFileDialog::getOpenFileName(this,
                                                 tr("Open file"),
                                                 _lastDir,
                                                 tr("PCD Files(*.pcd)"));
        if(!_fileName.isEmpty()){
            load_pcd_file(_fileName);
            _firstsave = true;
        }
        else{
            QMessageBox::warning(this, "Error", "PCD file not loaded, do it again.");
        }
    }
}

void MainWindow::load_pcd_file(QString fileName){
    // Point cloud reader
    pcl::PCDReader reader;

    // Remove all the information saved of the tree widget
    clearInfoTreeWidget();

    // Clear the pose information
    clearPoseInfo();

    // Remove all the objects information saved
    objectsInfo.clear();
    _planeDefined = false;
    _planeSegmentated = false;

    // Read the point cloud
    reader.read(fileName.toStdString(), *_cloud);

    // Remove Nan points
    std::vector<int> index;
    pcl::removeNaNFromPointCloud(*_cloud, *_cloud, index);

    // Visualize the point cloud loaded
    visualize();
    _pcdLoaded=true;
}

// Draws the bounding box and the points inside it. Also actualize the pose info
void MainWindow::drawBoxPointsAndNewInfo(){
    if(_insertingObject){
        drawBoxAndPoints();
        actualizePoseInfo();
    }
}

// Set the values of the pose in the pose info tree widget
void MainWindow::setPoseInfo(pcl::PointXYZ boxPose, float boxRoll, float boxPitch, float boxYaw){
    std::stringstream x, y, z , roll, pitch, yaw;

    x << setprecision(3) << boxPose.x;
    y << setprecision(3) << boxPose.y;
    z << setprecision(3) << boxPose.z;
    roll << setprecision(3) << pcl::rad2deg(boxRoll);
    pitch << setprecision(3) << pcl::rad2deg(boxPitch);
    yaw << setprecision(3) << pcl::rad2deg(boxYaw);

    _ui->poseInfo->topLevelItem(0)->setText(1, QString::fromStdString(x.str()));
    _ui->poseInfo->topLevelItem(1)->setText(1, QString::fromStdString(y.str()));
    _ui->poseInfo->topLevelItem(2)->setText(1, QString::fromStdString(z.str()));
    _ui->poseInfo->topLevelItem(3)->setText(1, QString::fromStdString(roll.str()));
    _ui->poseInfo->topLevelItem(4)->setText(1, QString::fromStdString(pitch.str()));
    _ui->poseInfo->topLevelItem(5)->setText(1, QString::fromStdString(yaw.str()));
}

// Clears the information displayed in the pose info tree widget
void MainWindow::clearPoseInfo(){
    _ui->poseInfo->topLevelItem(0)->setText(1, "0");
    _ui->poseInfo->topLevelItem(1)->setText(1, "0");
    _ui->poseInfo->topLevelItem(2)->setText(1, "0");
    _ui->poseInfo->topLevelItem(3)->setText(1, "0");
    _ui->poseInfo->topLevelItem(4)->setText(1, "0");
    _ui->poseInfo->topLevelItem(5)->setText(1, "0");

    _ui->boxLength->setValue(0);
    _ui->boxWidth->setValue(0);
    _ui->boxHeight->setValue(0);
}

// Actualizes the pose values of the pose info tree widget
void MainWindow::actualizePoseInfo(){
    std::stringstream x, y, z , roll, pitch, yaw;

    x << setprecision(3) << _objectPose.x;
    y << setprecision(3) << _objectPose.y;
    z << setprecision(3) << _objectPose.z;
    roll << setprecision(3) << pcl::rad2deg(_boxRoll);
    pitch << setprecision(3) << pcl::rad2deg(_boxPitch);
    yaw << setprecision(3) << pcl::rad2deg(_boxYaw);

    _ui->poseInfo->topLevelItem(0)->setText(1, QString::fromStdString(x.str()));
    _ui->poseInfo->topLevelItem(1)->setText(1, QString::fromStdString(y.str()));
    _ui->poseInfo->topLevelItem(2)->setText(1, QString::fromStdString(z.str()));
    _ui->poseInfo->topLevelItem(3)->setText(1, QString::fromStdString(roll.str()));
    _ui->poseInfo->topLevelItem(4)->setText(1, QString::fromStdString(pitch.str()));
    _ui->poseInfo->topLevelItem(5)->setText(1, QString::fromStdString(yaw.str()));
}

void MainWindow::actualizePose(){
    viewInteractor.redefinePose(_objectPose);
    if(_insertingObject) _objectModifed = true;
    drawBoxPointsAndNewInfo();
}

void MainWindow::actualizeRotation(){
    viewInteractor.redefineRotation(_boxRoll, _boxPitch, _boxYaw);
    if(_insertingObject) _objectModifed = true;
    drawBoxPointsAndNewInfo();
}

void MainWindow::actualizeBoxWidth(){
    viewInteractor.redefineWidth(_boxWidth);
    if(_insertingObject) _objectModifed = true;
    drawBoxAndPoints();
}

void MainWindow::actualizeBoxLength(){
    viewInteractor.redefineLength(_boxLength);
    if(_insertingObject) _objectModifed = true;
    drawBoxPointsAndNewInfo();
}

void MainWindow::actualizeBoxHeight(){
    viewInteractor.redefineHeight(_boxHeight);
    if(_insertingObject) _objectModifed = true;
    drawBoxPointsAndNewInfo();
}

void MainWindow::drawBoxAndPoints(){
    viewInteractor.drawBoundingBox();
    viewInteractor.highligthPointsinBox(_cloud);
}

void MainWindow::actualizeInformationTreeWidget(){
    // Actualize tree widget with the loaded objects and the table size
    // Insert length information
    displayDeskLengthInfo();

    // Insert width information
    displayDeskWidthInfo();

    // Object's information
    displayObjectsInfo();
}

void MainWindow::displayDeskLengthInfo(){
    QTreeWidgetItem *itm = new QTreeWidgetItem(_ui->treeWidget->topLevelItem(0));
    std::stringstream info;
    info << "Length: " << setprecision(3) << objectsInfo.getDeskLength() << " m";
    itm->setText(0,QString::fromStdString(info.str()));
    _ui->treeWidget->insertTopLevelItem(0,itm);
}

void MainWindow::displayDeskWidthInfo(){
    QTreeWidgetItem *itm = new QTreeWidgetItem(_ui->treeWidget->topLevelItem(0));
    std::stringstream info;
    info << "Width: " << setprecision(3) << objectsInfo.getDeskWidth() << " m";
    itm->setText(0,QString::fromStdString(info.str()));
    _ui->treeWidget->insertTopLevelItem(0,itm);
}

void MainWindow::displayObjectsInfo(){
    for(int i = 0; i < objectsInfo.numberOfObjects(); i++){
        QTreeWidgetItem *itm = new QTreeWidgetItem(_ui->treeWidget->topLevelItem(1));
        itm->setText(0,objectsInfo.nameOfObject(i));
        _ui->treeWidget->addTopLevelItem(itm);
    }
}

void MainWindow::clearInfoTreeWidget(){
    _ui->treeWidget->clear();
}

void MainWindow::confirmObjectPosition(){
    _insertingObject = false;
    _objectModifed = false;

    if(!_objectName.isEmpty()){
        // Get the indices inside the current bounding box
        pcl::PointIndices indices;
        viewInteractor.getPointsInBoundingBox(_cloud, &indices);

        if(objectsInfo.existsObject(_objectName)){
            // If the object already exists, modify it
            objectsInfo.modifyObject(_objectName,
                                     _objectPose,
                                     _boxLength,
                                     _boxWidth,
                                     _boxHeight,
                                     _boxRoll,
                                     _boxPitch,
                                     _boxYaw,
                                     &indices);
        }
        else{
            // Insert the object
            objectsInfo.insertObject(_objectName,
                                     _objectPose,
                                     _boxLength,
                                     _boxWidth,
                                     _boxHeight,
                                     _boxRoll,
                                     _boxPitch,
                                     _boxYaw,
                                     &indices);

            // Insert the object in the Information tree Widget
            QTreeWidgetItem *itm = new QTreeWidgetItem(_ui->treeWidget->topLevelItem(1));
            itm->setText(0,QString::fromStdString(_objectName.toStdString()));
            _ui->treeWidget->addTopLevelItem(itm);
        }

        // Clear the pose information displayed
        clearPoseInfo();

        // Remove the box and the lighted points
        viewInteractor.cleanViewer();
    }
}

void MainWindow::showInitialMessage(){
    if(_showInitialMsg){
        initialmessagedialog msg;
        msg.exec();
        if(msg.getBoolValue()){
            // Write false in the output file
            QString info_doc = qApp->applicationDirPath();
            info_doc.remove("bin");
            info_doc.append("info_app.xml");;

            boost::property_tree::ptree root;

            root.put("showInitial", false);
            root.put("lastDirectory", _lastDir.toStdString());

            boost::property_tree::xml_writer_settings<char> settings(' ', 3);
            write_xml(info_doc.toStdString(), root, std::locale(), settings);
        }
    }
}

void MainWindow::on_actionSaveAs_PCD_xml_triggered()
{
    save_Option(2); //Save As
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    if (okToExit()){
      event->accept();
    } else {
      event->ignore();
    }
}

void MainWindow::on_actionShiftorigin_triggered()
{
    if(_pcdLoaded){

    pointT pointPicked;
    if (objectsInfo.numberOfObjects()==0){
    // Pick the origin
    if(_showInfoMsgs) QMessageBox::information(this,
                                               "Pick the origin point",
                                               "Pick a desired origin point with shift+left mouse button.");
    viewInteractor.getPointPicked(&pointPicked);

    // Move the pointcloud to the new origin
    cloudModifier.translate_on_plane_x_y(_cloud, _cloud, pointPicked);
    visualize();
    }
    else
        QMessageBox::warning(this,
                             "Cannot shift origin",
                             "There are already some objects with the current origin as reference.");
    }

    else
        QMessageBox::warning(this,
                             "No PCD file opened",
                             "Open a PCD file and try again..");
}

void MainWindow::on_actionUndoPoints_triggered()
{
    viewInteractor.undo_points();
}


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
   Contributing author: Mike Parks (SNL)
------------------------------------------------------------------------- */

#include <cmath>
#include <cfloat>
#include <cstdlib>
#include <cstring>
#include <cstdio>
#include <iostream>
#include <Eigen/Eigen>
#include "pair_smd_triangulated_surface.h"
#include "atom.h"
#include "domain.h"
#include "force.h"
#include "update.h"
#include "modify.h"
#include "fix.h"
#include "comm.h"
#include "neighbor.h"
#include "neigh_list.h"
#include "neigh_request.h"
#include "memory.h"
#include "error.h"

using namespace std;
using namespace LAMMPS_NS;
using namespace Eigen;

#define SQRT2 1.414213562e0

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

PairTriSurf::PairTriSurf(LAMMPS *lmp) :
                Pair(lmp) {

        onerad_dynamic = onerad_frozen = maxrad_dynamic = maxrad_frozen = NULL;
        bulkmodulus = NULL;
        kn = NULL;
        scale = 1.0;
}

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

PairTriSurf::~PairTriSurf() {

        if (allocated) {
                memory->destroy(setflag);
                memory->destroy(cutsq);
                memory->destroy(bulkmodulus);
                memory->destroy(kn);

                delete[] onerad_dynamic;
                delete[] onerad_frozen;
                delete[] maxrad_dynamic;
                delete[] maxrad_frozen;
        }
}

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

void PairTriSurf::compute(int eflag, int vflag) {
        int i, j, ii, jj, inum, jnum, itype, jtype;
        double rsq, r, evdwl, fpair;
        int *ilist, *jlist, *numneigh, **firstneigh;
        double rcut, r_geom, delta, r_tri, r_particle, touch_distance, dt_crit;
        int tri, particle;
        Vector3d normal, x1, x2, x3, x4, x13, x23, x43, w, cp, x4cp, vnew, v_old;
        ;
        Vector3d xi, x_center, dx;
        Matrix2d C;
        Vector2d w2d, rhs;

        evdwl = 0.0;
        ev_init(eflag, vflag);

        tagint *mol = atom->molecule;
        double **f = atom->f;
        double **smd_data_9 = atom->smd_data_9;
        double **x = atom->x;
        double **x0 = atom->x0;
        double **v = atom->v;
        double *rmass = atom->rmass;
        int *type = atom->type;
        int nlocal = atom->nlocal;
        double *radius = atom->contact_radius;
        double rcutSq;
        Vector3d offset;

        int newton_pair = force->newton_pair;
        int periodic = (domain->xperiodic || domain->yperiodic || domain->zperiodic);

        inum = list->inum;
        ilist = list->ilist;
        numneigh = list->numneigh;
        firstneigh = list->firstneigh;

        int max_neighs = 0;
        stable_time_increment = 1.0e22;

        // loop over neighbors of my atoms using a half neighbor list
        for (ii = 0; ii < inum; ii++) {
                i = ilist[ii];
                itype = type[i];
                jlist = firstneigh[i];
                jnum = numneigh[i];
                max_neighs = MAX(max_neighs, jnum);

                for (jj = 0; jj < jnum; jj++) {
                        j = jlist[jj];

                        j &= NEIGHMASK;

                        jtype = type[j];

                        /*
                         * decide which one of i, j is triangle and which is particle
                         */
                        if ((mol[i] < 65535) && (mol[j] >= 65535)) {
                                particle = i;
                                tri = j;
                        } else if ((mol[j] < 65535) && (mol[i] >= 65535)) {
                                particle = j;
                                tri = i;
                        } else {
                                error->one(FLERR, "unknown case");
                        }

                        //x_center << x[tri][0], x[tri][1], x[tri][2]; // center of triangle
                        x_center(0) = x[tri][0];
                        x_center(1) = x[tri][1];
                        x_center(2) = x[tri][2];
                        //x4 << x[particle][0], x[particle][1], x[particle][2];
                        x4(0) = x[particle][0];
                        x4(1) = x[particle][1];
                        x4(2) = x[particle][2];
                        dx = x_center - x4; //
                        if (periodic) {
                                domain->minimum_image(dx(0), dx(1), dx(2));
                        }
                        rsq = dx.squaredNorm();

                        r_tri = scale * radius[tri];
                        r_particle = scale * radius[particle];
                        rcut = r_tri + r_particle;
                        rcutSq = rcut * rcut;

                        //printf("type i=%d, type j=%d, r=%f, ri=%f, rj=%f\n", itype, jtype, sqrt(rsq), ri, rj);

                        if (rsq < rcutSq) {

                                /*
                                 * gather triangle information
                                 */
                                normal(0) = x0[tri][0];
                                normal(1) = x0[tri][1];
                                normal(2) = x0[tri][2];

                                /*
                                 * distance check: is particle closer than its radius to the triangle plane?
                                 */
                                if (fabs(dx.dot(normal)) < radius[particle]) {
                                        /*
                                         * get other two triangle vertices
                                         */
                                        x1(0) = smd_data_9[tri][0];
                                        x1(1) = smd_data_9[tri][1];
                                        x1(2) = smd_data_9[tri][2];
                                        x2(0) = smd_data_9[tri][3];
                                        x2(1) = smd_data_9[tri][4];
                                        x2(2) = smd_data_9[tri][5];
                                        x3(0) = smd_data_9[tri][6];
                                        x3(1) = smd_data_9[tri][7];
                                        x3(2) = smd_data_9[tri][8];

                                        PointTriangleDistance(x4, x1, x2, x3, cp, r);

                                        /*
                                         * distance to closest point
                                         */
                                        x4cp = x4 - cp;

                                        /*
                                         * flip normal to point in direction of x4cp
                                         */

                                        if (x4cp.dot(normal) < 0.0) {
                                                normal *= -1.0;
                                        }

                                        /*
                                         * penalty force pushes particle away from triangle
                                         */
                                        if (r < 1.0 * radius[particle]) {

                                                delta = radius[particle] - r; // overlap distance
                                                r_geom = radius[particle];
                                                fpair = 1.066666667e0 * bulkmodulus[itype][jtype] * delta * sqrt(delta * r_geom);
                                                dt_crit = 3.14 * sqrt(rmass[particle] / (fpair / delta));
                                                stable_time_increment = MIN(stable_time_increment, dt_crit);

                                                evdwl = r * fpair * 0.4e0 * delta; // GCG 25 April: this expression conserves total energy

                                                fpair /= (r + 1.0e-2 * radius[particle]); // divide by r + softening and multiply with non-normalized distance vector

                                                if (particle < nlocal) {
                                                        f[particle][0] += x4cp(0) * fpair;
                                                        f[particle][1] += x4cp(1) * fpair;
                                                        f[particle][2] += x4cp(2) * fpair;
                                                }

                                                if (tri < nlocal) {
                                                        f[tri][0] -= x4cp(0) * fpair;
                                                        f[tri][1] -= x4cp(1) * fpair;
                                                        f[tri][2] -= x4cp(2) * fpair;
                                                }

                                                if (evflag) {
                                                        ev_tally(i, j, nlocal, newton_pair, evdwl, 0.0, fpair, x4cp(0), x4cp(1), x4cp(2));
                                                }

                                        }

                                        /*
                                         * if particle comes too close to triangle, reflect its velocity and explicitly move it away
                                         */

                                        touch_distance = 1.0 * radius[particle];
                                        if (r < touch_distance) {

                                                /*
                                                 * reflect velocity if it points toward triangle
                                                 */

                                                normal = x4cp / r;

                                                //v_old << v[particle][0], v[particle][1], v[particle][2];
                                                v_old(0) = v[particle][0];
                                                v_old(1) = v[particle][1];
                                                v_old(2) = v[particle][2];
                                                if (v_old.dot(normal) < 0.0) {
                                                        //printf("flipping velocity\n");
                                                        vnew = 1.0 * (-2.0 * v_old.dot(normal) * normal + v_old);
                                                        v[particle][0] = vnew(0);
                                                        v[particle][1] = vnew(1);
                                                        v[particle][2] = vnew(2);
                                                }

                                                //printf("moving particle on top of triangle\n");
                                                x[particle][0] = cp(0) + touch_distance * normal(0);
                                                x[particle][1] = cp(1) + touch_distance * normal(1);
                                                x[particle][2] = cp(2) + touch_distance * normal(2);
                                        }

                                }
                        }
                }
        }

//      int max_neighs_all = 0;
//      MPI_Allreduce(&max_neighs, &max_neighs_all, 1, MPI_INT, MPI_MAX, world);
//      if (comm->me == 0) {
//              printf("max. neighs in tri pair is %d\n", max_neighs_all);
//      }
//
//              double stable_time_increment_all = 0.0;
//              MPI_Allreduce(&stable_time_increment, &stable_time_increment_all, 1, MPI_DOUBLE, MPI_MIN, world);
//              if (comm->me == 0) {
//                      printf("stable time step tri pair is %f\n", stable_time_increment_all);
//              }
}

/* ----------------------------------------------------------------------
 allocate all arrays
 ------------------------------------------------------------------------- */

void PairTriSurf::allocate() {
        allocated = 1;
        int n = atom->ntypes;

        memory->create(setflag, n + 1, n + 1, "pair:setflag");
        for (int i = 1; i <= n; i++)
                for (int j = i; j <= n; j++)
                        setflag[i][j] = 0;

        memory->create(bulkmodulus, n + 1, n + 1, "pair:kspring");
        memory->create(kn, n + 1, n + 1, "pair:kn");

        memory->create(cutsq, n + 1, n + 1, "pair:cutsq"); // always needs to be allocated, even with granular neighborlist

        onerad_dynamic = new double[n + 1];
        onerad_frozen = new double[n + 1];
        maxrad_dynamic = new double[n + 1];
        maxrad_frozen = new double[n + 1];
}

/* ----------------------------------------------------------------------
 global settings
 ------------------------------------------------------------------------- */

void PairTriSurf::settings(int narg, char **arg) {
        if (narg != 1)
                error->all(FLERR, "Illegal number of args for pair_style smd/tri_surface");

        scale = force->numeric(FLERR, arg[0]);
        if (comm->me == 0) {
                printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
                printf("SMD/TRI_SURFACE CONTACT SETTINGS:\n");
                printf("... effective contact radius is scaled by %f\n", scale);
                printf(">>========>>========>>========>>========>>========>>========>>========>>========\n");
        }

}

/* ----------------------------------------------------------------------
 set coeffs for one or more type pairs
 ------------------------------------------------------------------------- */

void PairTriSurf::coeff(int narg, char **arg) {
        if (narg != 3)
                error->all(FLERR, "Incorrect args for pair coefficients");
        if (!allocated)
                allocate();

        int ilo, ihi, jlo, jhi;
        force->bounds(FLERR,arg[0], atom->ntypes, ilo, ihi);
        force->bounds(FLERR,arg[1], atom->ntypes, jlo, jhi);

        double bulkmodulus_one = atof(arg[2]);

        // set short-range force constant
        double kn_one = 0.0;
        if (domain->dimension == 3) {
                kn_one = (16. / 15.) * bulkmodulus_one; //assuming poisson ratio = 1/4 for 3d
        } else {
                kn_one = 0.251856195 * (2. / 3.) * bulkmodulus_one; //assuming poisson ratio = 1/3 for 2d
        }

        int count = 0;
        for (int i = ilo; i <= ihi; i++) {
                for (int j = MAX(jlo, i); j <= jhi; j++) {
                        bulkmodulus[i][j] = bulkmodulus_one;
                        kn[i][j] = kn_one;
                        setflag[i][j] = 1;
                        count++;
                }
        }

        if (count == 0)
                error->all(FLERR, "Incorrect args for pair coefficients");
}

/* ----------------------------------------------------------------------
 init for one type pair i,j and corresponding j,i
 ------------------------------------------------------------------------- */

double PairTriSurf::init_one(int i, int j) {

        if (!allocated)
                allocate();

        if (setflag[i][j] == 0)
                error->all(FLERR, "All pair coeffs are not set");

        bulkmodulus[j][i] = bulkmodulus[i][j];
        kn[j][i] = kn[i][j];

        // cutoff = sum of max I,J radii for
        // dynamic/dynamic & dynamic/frozen interactions, but not frozen/frozen

        double cutoff = maxrad_dynamic[i] + maxrad_dynamic[j];
        cutoff = MAX(cutoff, maxrad_frozen[i] + maxrad_dynamic[j]);
        cutoff = MAX(cutoff, maxrad_dynamic[i] + maxrad_frozen[j]);

        if (comm->me == 0) {
                printf("cutoff for pair smd/smd/tri_surface = %f\n", cutoff);
        }
        return cutoff;
}

/* ----------------------------------------------------------------------
 init specific to this pair style
 ------------------------------------------------------------------------- */

void PairTriSurf::init_style() {
        int i;

        // error checks

        if (!atom->contact_radius_flag)
                error->all(FLERR, "Pair style smd/smd/tri_surface requires atom style with contact_radius");

        // old: half list
        int irequest = neighbor->request(this);
        neighbor->requests[irequest]->size = 1;

        // need a full neighbor list
//      int irequest = neighbor->request(this);
//      neighbor->requests[irequest]->half = 0;
//      neighbor->requests[irequest]->full = 1;

        // set maxrad_dynamic and maxrad_frozen for each type
        // include future Fix pour particles as dynamic

        for (i = 1; i <= atom->ntypes; i++)
                onerad_dynamic[i] = onerad_frozen[i] = 0.0;

        double *radius = atom->radius;
        int *type = atom->type;
        int nlocal = atom->nlocal;

        for (i = 0; i < nlocal; i++) {
                onerad_dynamic[type[i]] = MAX(onerad_dynamic[type[i]], radius[i]);
        }

        MPI_Allreduce(&onerad_dynamic[1], &maxrad_dynamic[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);
        MPI_Allreduce(&onerad_frozen[1], &maxrad_frozen[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);
}

/* ----------------------------------------------------------------------
 neighbor callback to inform pair style of neighbor list to use
 optional granular history list
 ------------------------------------------------------------------------- */

void PairTriSurf::init_list(int id, NeighList *ptr) {
        if (id == 0)
                list = ptr;
}

/* ----------------------------------------------------------------------
 memory usage of local atom-based arrays
 ------------------------------------------------------------------------- */

double PairTriSurf::memory_usage() {

        return 0.0;
}

/*
 * distance between triangle and point
 */
/*
 function [dist,PP0] = pointTriangleDistance(TRI,P)
 % calculate distance between a point and a triangle in 3D
 % SYNTAX
 %   dist = pointTriangleDistance(TRI,P)
 %   [dist,PP0] = pointTriangleDistance(TRI,P)
 %
 % DESCRIPTION
 %   Calculate the distance of a given point P from a triangle TRI.
 %   Point P is a row vector of the form 1x3. The triangle is a matrix
 %   formed by three rows of points TRI = [P1;P2;P3] each of size 1x3.
 %   dist = pointTriangleDistance(TRI,P) returns the distance of the point P
 %   to the triangle TRI.
 %   [dist,PP0] = pointTriangleDistance(TRI,P) additionally returns the
 %   closest point PP0 to P on the triangle TRI.
 %
 % Author: Gwendolyn Fischer
 % Release: 1.0
 % Release date: 09/02/02
 % Release: 1.1 Fixed Bug because of normalization
 % Release: 1.2 Fixed Bug because of typo in region 5 20101013
 % Release: 1.3 Fixed Bug because of typo in region 2 20101014

 % Possible extention could be a version tailored not to return the distance
 % and additionally the closest point, but instead return only the closest
 % point. Could lead to a small speed gain.

 % Example:
 % %% The Problem
 % P0 = [0.5 -0.3 0.5];
 %
 % P1 = [0 -1 0];
 % P2 = [1  0 0];
 % P3 = [0  0 0];
 %
 % vertices = [P1; P2; P3];
 % faces = [1 2 3];
 %
 % %% The Engine
 % [dist,PP0] = pointTriangleDistance([P1;P2;P3],P0);
 %
 % %% Visualization
 % [x,y,z] = sphere(20);
 % x = dist*x+P0(1);
 % y = dist*y+P0(2);
 % z = dist*z+P0(3);
 %
 % figure
 % hold all
 % patch('Vertices',vertices,'Faces',faces,'FaceColor','r','FaceAlpha',0.8);
 % plot3(P0(1),P0(2),P0(3),'b*');
 % plot3(PP0(1),PP0(2),PP0(3),'*g')
 % surf(x,y,z,'FaceColor','b','FaceAlpha',0.3)
 % view(3)

 % The algorithm is based on
 % "David Eberly, 'Distance Between Point and Triangle in 3D',
 % Geometric Tools, LLC, (1999)"
 % http:\\www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
 %
 %        ^t
 %  \     |
 %   \reg2|
 %    \   |
 %     \  |
 %      \ |
 %       \|
 %        *P2
 %        |\
%        | \
%  reg3  |  \ reg1
 %        |   \
%        |reg0\
%        |     \
%        |      \ P1
 % -------*-------*------->s
 %        |P0      \
%  reg4  | reg5    \ reg6
 */

//void PairTriSurf::PointTriangleDistance(const Vector3d P, const Vector3d TRI1, const Vector3d TRI2, const Vector3d TRI3,
//              Vector3d &CP, double &dist) {
//
//      Vector3d B, E0, E1, D;
//      double a, b, c, d, e, f;
//      double det, s, t, sqrDistance, tmp0, tmp1, numer, denom, invDet;
//
//      // rewrite triangle in normal form
//      B = TRI1;
//      E0 = TRI2 - B;
//      E1 = TRI3 - B;
//
//      D = B - P;
//      a = E0.dot(E0);
//      b = E0.dot(E1);
//      c = E1.dot(E1);
//      d = E0.dot(D);
//      e = E1.dot(D);
//      f = D.dot(D);
//
//      det = a * c - b * b;
//      //% do we have to use abs here?
//      s = b * e - c * d;
//      t = b * d - a * e;
//
//      //% Terible tree of conditionals to determine in which region of the diagram
//      //% shown above the projection of the point into the triangle-plane lies.
//      if ((s + t) <= det) {
//              if (s < 0) {
//                      if (t < 0) {
//                              // %region4
//                              if (d < 0) {
//                                      t = 0;
//                                      if (-d >= a) {
//                                              s = 1;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              s = -d / a;
//                                              sqrDistance = d * s + f;
//                                      }
//                              } else {
//                                      s = 0;
//                                      if (e >= 0) {
//                                              t = 0;
//                                              sqrDistance = f;
//                                      } else {
//                                              if (-e >= c) {
//                                                      t = 1;
//                                                      sqrDistance = c + 2 * e + f;
//                                              } else {
//                                                      t = -e / c;
//                                                      sqrDistance = e * t + f;
//                                              }
//                                      }
//                              }
//                              // end % of region 4
//                      } else {
//                              // % region 3
//                              s = 0;
//                              if (e >= 0) {
//                                      t = 0;
//                                      sqrDistance = f;
//                              } else {
//                                      if (-e >= c) {
//                                              t = 1;
//                                              sqrDistance = c + 2 * e + f;
//                                      } else {
//                                              t = -e / c;
//                                              sqrDistance = e * t + f;
//                                      }
//                              }
//                      }
//                      // end of region 3
//              } else {
//                      if (t < 0) {
//                              //% region 5
//                              t = 0;
//                              if (d >= 0) {
//                                      s = 0;
//                                      sqrDistance = f;
//                              } else {
//                                      if (-d >= a) {
//                                              s = 1;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              s = -d / a;
//                                              sqrDistance = d * s + f;
//                                      }
//                              }
//                      } else {
//                              // region 0
//                              invDet = 1 / det;
//                              s = s * invDet;
//                              t = t * invDet;
//                              sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                      }
//              }
//      } else {
//              if (s < 0) {
//                      // % region 2
//                      tmp0 = b + d;
//                      tmp1 = c + e;
//                      if (tmp1 > tmp0) { //% minimum on edge s+t=1
//                              numer = tmp1 - tmp0;
//                              denom = a - 2 * b + c;
//                              if (numer >= denom) {
//                                      s = 1;
//                                      t = 0;
//                                      sqrDistance = a + 2 * d + f;
//                              } else {
//                                      s = numer / denom;
//                                      t = 1 - s;
//                                      sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                              }
//                      } else
//                              // % minimum on edge s=0
//                              s = 0;
//                      if (tmp1 <= 0) {
//                              t = 1;
//                              sqrDistance = c + 2 * e + f;
//                      } else {
//                              if (e >= 0) {
//                                      t = 0;
//                                      sqrDistance = f;
//                              } else {
//                                      t = -e / c;
//                                      sqrDistance = e * t + f;
//                              }
//                      }
//              } //end % of region     2
//              else {
//                      if (t < 0) {
//                              // %region6
//                              tmp0 = b + e;
//                              tmp1 = a + d;
//                              if (tmp1 > tmp0) {
//                                      numer = tmp1 - tmp0;
//                                      denom = a - 2 * b + c;
//                                      if (numer >= denom) {
//                                              t = 1;
//                                              s = 0;
//                                              sqrDistance = c + 2 * e + f;
//                                      } else {
//                                              t = numer / denom;
//                                              s = 1 - t;
//                                              sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                                      }
//                              } else {
//                                      t = 0;
//                                      if (tmp1 <= 0) {
//                                              s = 1;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              if (d >= 0) {
//                                                      s = 0;
//                                                      sqrDistance = f;
//                                              } else {
//                                                      s = -d / a;
//                                                      sqrDistance = d * s + f;
//                                              }
//                                      }
//                              } // % end region 6
//                      } else {
//                              //% region 1
//                              numer = c + e - b - d;
//                              if (numer <= 0) {
//                                      s = 0;
//                                      t = 1;
//                                      sqrDistance = c + 2 * e + f;
//                              } else {
//                                      denom = a - 2 * b + c;
//                                      if (numer >= denom) {
//                                              s = 1;
//                                              t = 0;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              s = numer / denom;
//                                              t = 1 - s;
//                                              sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                                      }
//                              } //% end of region 1
//                      }
//              }
//      }
//
//      // % account for numerical round-off error
//      if (sqrDistance < 0) {
//              sqrDistance = 0;
//      }
//
//      dist = sqrt(sqrDistance);
//
//      // closest point
//      CP = B + s * E0 + t * E1;
//
//}
/*
 * % The algorithm is based on
 % "David Eberly, 'Distance Between Point and Triangle in 3D',
 % Geometric Tools, LLC, (1999)"
 % http:\\www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
 */

void PairTriSurf::PointTriangleDistance(const Vector3d sourcePosition, const Vector3d TRI0, const Vector3d TRI1,
                const Vector3d TRI2, Vector3d &CP, double &dist) {

        Vector3d edge0 = TRI1 - TRI0;
        Vector3d edge1 = TRI2 - TRI0;
        Vector3d v0 = TRI0 - sourcePosition;

        double a = edge0.dot(edge0);
        double b = edge0.dot(edge1);
        double c = edge1.dot(edge1);
        double d = edge0.dot(v0);
        double e = edge1.dot(v0);

        double det = a * c - b * b;
        double s = b * e - c * d;
        double t = b * d - a * e;

        if (s + t < det) {
                if (s < 0.f) {
                        if (t < 0.f) {
                                if (d < 0.f) {
                                        s = clamp(-d / a, 0.f, 1.f);
                                        t = 0.f;
                                } else {
                                        s = 0.f;
                                        t = clamp(-e / c, 0.f, 1.f);
                                }
                        } else {
                                s = 0.f;
                                t = clamp(-e / c, 0.f, 1.f);
                        }
                } else if (t < 0.f) {
                        s = clamp(-d / a, 0.f, 1.f);
                        t = 0.f;
                } else {
                        float invDet = 1.f / det;
                        s *= invDet;
                        t *= invDet;
                }
        } else {
                if (s < 0.f) {
                        float tmp0 = b + d;
                        float tmp1 = c + e;
                        if (tmp1 > tmp0) {
                                float numer = tmp1 - tmp0;
                                float denom = a - 2 * b + c;
                                s = clamp(numer / denom, 0.f, 1.f);
                                t = 1 - s;
                        } else {
                                t = clamp(-e / c, 0.f, 1.f);
                                s = 0.f;
                        }
                } else if (t < 0.f) {
                        if (a + d > b + e) {
                                float numer = c + e - b - d;
                                float denom = a - 2 * b + c;
                                s = clamp(numer / denom, 0.f, 1.f);
                                t = 1 - s;
                        } else {
                                s = clamp(-e / c, 0.f, 1.f);
                                t = 0.f;
                        }
                } else {
                        float numer = c + e - b - d;
                        float denom = a - 2 * b + c;
                        s = clamp(numer / denom, 0.f, 1.f);
                        t = 1.f - s;
                }
        }

        CP = TRI0 + s * edge0 + t * edge1;
        dist = (CP - sourcePosition).norm();

}

double PairTriSurf::clamp(const double a, const double min, const double max) {
        if (a < min) {
                return min;
        } else if (a > max) {
                return max;
        } else {
                return a;
        }
}

void *PairTriSurf::extract(const char *str, int &/*i*/) {
        //printf("in PairTriSurf::extract\n");
        if (strcmp(str, "smd/tri_surface/stable_time_increment_ptr") == 0) {
                return (void *) &stable_time_increment;
        }

        return NULL;

}