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

#include "ColoredICP.h"

#include <Eigen/Dense>
#include <Core/Geometry/PointCloud.h>
#include <Core/Geometry/KDTreeFlann.h>
#include <Core/Geometry/KDTreeSearchParam.h>
#include <Core/Utility/Eigen.h>

#include <iostream>
#include <Core/Utility/Console.h>

namespace open3d {

namespace {

class PointCloudForColoredICP : public PointCloud {
public:
    std::vector<Eigen::Vector3d> color_gradient_;
};

class TransformationEstimationForColoredICP : public TransformationEstimation {
public:
    TransformationEstimationType GetTransformationEstimationType()
            const override {
        return type_;
    };
    TransformationEstimationForColoredICP(double lambda_geometric = 0.968)
        : lambda_geometric_(lambda_geometric) {
        if (lambda_geometric_ < 0 || lambda_geometric_ > 1.0)
            lambda_geometric_ = 0.968;
    }
    ~TransformationEstimationForColoredICP() override {}

public:
    double ComputeRMSE(const PointCloud &source,
                       const PointCloud &target,
                       const CorrespondenceSet &corres) const override;
    Eigen::Matrix4d ComputeTransformation(
            const PointCloud &source,
            const PointCloud &target,
            const CorrespondenceSet &corres) const override;

public:
    double lambda_geometric_;

private:
    const TransformationEstimationType type_ =
            TransformationEstimationType::ColoredICP;
};

std::shared_ptr<PointCloudForColoredICP> InitializePointCloudForColoredICP(
        const PointCloud &target, const KDTreeSearchParamHybrid &search_param) {
    PrintDebug("InitializePointCloudForColoredICP\n");

    KDTreeFlann tree;
    tree.SetGeometry(target);

    auto output = std::make_shared<PointCloudForColoredICP>();
    output->colors_ = target.colors_;
    output->normals_ = target.normals_;
    output->points_ = target.points_;

    size_t n_points = output->points_.size();
    output->color_gradient_.resize(n_points, Eigen::Vector3d::Zero());

    for (auto k = 0; k < n_points; k++) {
        const Eigen::Vector3d &vt = output->points_[k];
        const Eigen::Vector3d &nt = output->normals_[k];
        double it = (output->colors_[k](0) + output->colors_[k](1) +
                     output->colors_[k](2)) /
                    3.0;

        std::vector<int> point_idx;
        std::vector<double> point_squared_distance;

        if (tree.SearchHybrid(vt, search_param.radius_, search_param.max_nn_,
                              point_idx, point_squared_distance) >= 3) {
            // approximate image gradient of vt's tangential plane
            size_t nn = point_idx.size();
            Eigen::MatrixXd A(nn, 3);
            Eigen::MatrixXd b(nn, 1);
            A.setZero();
            b.setZero();
            for (auto i = 1; i < nn; i++) {
                int P_adj_idx = point_idx[i];
                Eigen::Vector3d vt_adj = output->points_[P_adj_idx];
                Eigen::Vector3d vt_proj = vt_adj - (vt_adj - vt).dot(nt) * nt;
                double it_adj = (output->colors_[P_adj_idx](0) +
                                 output->colors_[P_adj_idx](1) +
                                 output->colors_[P_adj_idx](2)) /
                                3.0;
                A(i - 1, 0) = (vt_proj(0) - vt(0));
                A(i - 1, 1) = (vt_proj(1) - vt(1));
                A(i - 1, 2) = (vt_proj(2) - vt(2));
                b(i - 1, 0) = (it_adj - it);
            }
            // adds orthogonal constraint
            A(nn - 1, 0) = (nn - 1) * nt(0);
            A(nn - 1, 1) = (nn - 1) * nt(1);
            A(nn - 1, 2) = (nn - 1) * nt(2);
            b(nn - 1, 0) = 0;
            // solving linear equation
            bool is_success;
            Eigen::MatrixXd x;
            std::tie(is_success, x) =
                    SolveLinearSystemPSD(A.transpose() * A, A.transpose() * b);
            if (is_success) {
                output->color_gradient_[k] = x;
            }
        }
    }
    return output;
}

Eigen::Matrix4d TransformationEstimationForColoredICP::ComputeTransformation(
        const PointCloud &source,
        const PointCloud &target,
        const CorrespondenceSet &corres) const {
    if (corres.empty() || target.HasNormals() == false ||
        target.HasColors() == false || source.HasColors() == false)
        return Eigen::Matrix4d::Identity();

    double sqrt_lambda_geometric = sqrt(lambda_geometric_);
    double lambda_photometric = 1.0 - lambda_geometric_;
    double sqrt_lambda_photometric = sqrt(lambda_photometric);

    const auto &target_c = (const PointCloudForColoredICP &)target;

    auto compute_jacobian_and_residual =
            [&](int i, std::vector<Eigen::Vector6d, Vector6d_allocator> &J_r,
                std::vector<double> &r) {
                size_t cs = corres[i][0];
                size_t ct = corres[i][1];
                const Eigen::Vector3d &vs = source.points_[cs];
                const Eigen::Vector3d &vt = target.points_[ct];
                const Eigen::Vector3d &nt = target.normals_[ct];

                J_r.resize(2);
                r.resize(2);

                J_r[0].block<3, 1>(0, 0) = sqrt_lambda_geometric * vs.cross(nt);
                J_r[0].block<3, 1>(3, 0) = sqrt_lambda_geometric * nt;
                r[0] = sqrt_lambda_geometric * (vs - vt).dot(nt);

                // project vs into vt's tangential plane
                Eigen::Vector3d vs_proj = vs - (vs - vt).dot(nt) * nt;
                double is = (source.colors_[cs](0) + source.colors_[cs](1) +
                             source.colors_[cs](2)) /
                            3.0;
                double it = (target.colors_[ct](0) + target.colors_[ct](1) +
                             target.colors_[ct](2)) /
                            3.0;
                const Eigen::Vector3d &dit = target_c.color_gradient_[ct];
                double is0_proj = (dit.dot(vs_proj - vt)) + it;

                const Eigen::Matrix3d M =
                        (Eigen::Matrix3d() << 1.0 - nt(0) * nt(0),
                         -nt(0) * nt(1), -nt(0) * nt(2), -nt(0) * nt(1),
                         1.0 - nt(1) * nt(1), -nt(1) * nt(2), -nt(0) * nt(2),
                         -nt(1) * nt(2), 1.0 - nt(2) * nt(2))
                                .finished();

                const Eigen::Vector3d &ditM = -dit.transpose() * M;
                J_r[1].block<3, 1>(0, 0) =
                        sqrt_lambda_photometric * vs.cross(ditM);
                J_r[1].block<3, 1>(3, 0) = sqrt_lambda_photometric * ditM;
                r[1] = sqrt_lambda_photometric * (is - is0_proj);
            };

    Eigen::Matrix6d JTJ;
    Eigen::Vector6d JTr;
    double r2;
    std::tie(JTJ, JTr, r2) = ComputeJTJandJTr<Eigen::Matrix6d, Eigen::Vector6d>(
            compute_jacobian_and_residual, (int)corres.size());

    bool is_success;
    Eigen::Matrix4d extrinsic;
    std::tie(is_success, extrinsic) =
            SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);

    return is_success ? extrinsic : Eigen::Matrix4d::Identity();
}

double TransformationEstimationForColoredICP::ComputeRMSE(
        const PointCloud &source,
        const PointCloud &target,
        const CorrespondenceSet &corres) const {
    double sqrt_lambda_geometric = sqrt(lambda_geometric_);
    double lambda_photometric = 1.0 - lambda_geometric_;
    double sqrt_lambda_photometric = sqrt(lambda_photometric);
    const auto &target_c = (const PointCloudForColoredICP &)target;

    double residual = 0.0;
    for (size_t i = 0; i < corres.size(); i++) {
        size_t cs = corres[i][0];
        size_t ct = corres[i][1];
        const Eigen::Vector3d &vs = source.points_[cs];
        const Eigen::Vector3d &vt = target.points_[ct];
        const Eigen::Vector3d &nt = target.normals_[ct];
        Eigen::Vector3d vs_proj = vs - (vs - vt).dot(nt) * nt;
        double is = (source.colors_[cs](0) + source.colors_[cs](1) +
                     source.colors_[cs](2)) /
                    3.0;
        double it = (target.colors_[ct](0) + target.colors_[ct](1) +
                     target.colors_[ct](2)) /
                    3.0;
        const Eigen::Vector3d &dit = target_c.color_gradient_[ct];
        double is0_proj = (dit.dot(vs_proj - vt)) + it;
        double residual_geometric = sqrt_lambda_geometric * (vs - vt).dot(nt);
        double residual_photometric = sqrt_lambda_photometric * (is - is0_proj);
        residual += residual_geometric * residual_geometric +
                    residual_photometric * residual_photometric;
    }
    return residual;
};

}  // unnamed namespace

RegistrationResult RegistrationColoredICP(
        const PointCloud &source,
        const PointCloud &target,
        double max_distance,
        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
        const ICPConvergenceCriteria &criteria /* = ICPConvergenceCriteria()*/,
        double lambda_geometric /* = 0.968*/) {
    auto target_c = InitializePointCloudForColoredICP(
            target, KDTreeSearchParamHybrid(max_distance * 2.0, 30));
    return RegistrationICP(
            source, *target_c, max_distance, init,
            TransformationEstimationForColoredICP(lambda_geometric), criteria);
}

}  // namespace open3d


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/test/minimal.hpp>

using namespace boost;

template <typename Graph>
void reset_edge_index(Graph& g)
{
  typename property_map<Graph, edge_index_t>::type index = get(edge_index, g);
  typename graph_traits<Graph>::edge_iterator ei, ei_end;
  typename graph_traits<Graph>::edges_size_type cnt = 0;
  for(tie(ei,ei_end) = edges(g); ei != ei_end; ++ei)
    put(index, *ei, cnt++);
}

template <typename Graph>
void reset_vertex_index(Graph& g)
{
  typename property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);
  typename graph_traits<Graph>::vertex_iterator vi, vi_end;
  typename graph_traits<Graph>::vertices_size_type cnt = 0;
  for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    put(index, *vi, cnt++);
}

template <typename Graph>
void make_disconnected_cycles(Graph& g, int num_cycles, int cycle_size)
{
  // This graph will consist of num_cycles cycles, each of which
  // has cycle_size vertices and edges. The entire graph has
  // num_cycles * cycle_size vertices and edges, and requires
  // num_cycles - 1 edges to make it connected

  typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

  for(int i = 0; i < num_cycles; ++i)
    {
      vertex_t first_vertex = add_vertex(g);
      vertex_t prev_vertex;
      vertex_t curr_vertex = first_vertex;
      for(int j = 1; j < cycle_size; ++j)
        {
          prev_vertex = curr_vertex;
          curr_vertex = add_vertex(g);
          add_edge(prev_vertex, curr_vertex, g);
        }
      add_edge(curr_vertex, first_vertex, g);
    }
}

int test_main(int, char* [])
{
  typedef adjacency_list
    <vecS,
    vecS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    VVgraph_t;

  typedef adjacency_list
    <vecS,
    listS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    VLgraph_t;

  typedef adjacency_list
    <listS,
    vecS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    LVgraph_t;

  typedef adjacency_list
    <listS,
    listS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    LLgraph_t;

  VVgraph_t gVV;
  std::size_t num_cycles = 10;
  std::size_t cycle_size = 10;
  make_disconnected_cycles(gVV, num_cycles, cycle_size);
  reset_edge_index(gVV);
  std::vector<int> gVV_components(num_vertices(gVV));
  BOOST_CHECK(connected_components(gVV, &gVV_components[0]) ==
              static_cast<int>(num_cycles));
  make_connected(gVV);
  BOOST_CHECK(connected_components(gVV, &gVV_components[0]) == 1);
  BOOST_CHECK(num_edges(gVV) == num_cycles * cycle_size + num_cycles - 1);

  LVgraph_t gLV;
  num_cycles = 20;
  cycle_size = 20;
  make_disconnected_cycles(gLV, num_cycles, cycle_size);
  reset_edge_index(gLV);
  std::vector<int> gLV_components(num_vertices(gLV));
  BOOST_CHECK(connected_components(gLV, &gLV_components[0]) ==
              static_cast<int>(num_cycles));
  make_connected(gLV);
  BOOST_CHECK(connected_components(gLV, &gLV_components[0]) == 1);
  BOOST_CHECK(num_edges(gLV) == num_cycles * cycle_size + num_cycles - 1);

  VLgraph_t gVL;
  num_cycles = 30;
  cycle_size = 30;
  make_disconnected_cycles(gVL, num_cycles, cycle_size);
  reset_edge_index(gVL);
  reset_vertex_index(gVL);
  BOOST_CHECK(connected_components(gVL, make_vector_property_map<int>(get(vertex_index,gVL)))
              == static_cast<int>(num_cycles)
              );
  make_connected(gVL);
  BOOST_CHECK(connected_components(gVL, make_vector_property_map<int>(get(vertex_index,gVL)))
              == 1
              );
  BOOST_CHECK(num_edges(gVL) == num_cycles * cycle_size + num_cycles - 1);

  LLgraph_t gLL;
  num_cycles = 40;
  cycle_size = 40;
  make_disconnected_cycles(gLL, num_cycles, cycle_size);
  reset_edge_index(gLL);
  reset_vertex_index(gLL);
  BOOST_CHECK(connected_components(gLL, make_vector_property_map<int>(get(vertex_index,gLL)))
              == static_cast<int>(num_cycles));
  make_connected(gLL);
  BOOST_CHECK(connected_components(gLL, make_vector_property_map<int>(get(vertex_index,gLL)))
              == 1
              );
  BOOST_CHECK(num_edges(gLL) == num_cycles * cycle_size + num_cycles - 1);

  // Now make sure that no edges are added to an already connected graph
  // when you call make_connected again

  graph_traits<VVgraph_t>::edges_size_type VV_num_edges(num_edges(gVV));
  make_connected(gVV);
  BOOST_CHECK(num_edges(gVV) == VV_num_edges);

  graph_traits<VLgraph_t>::edges_size_type VL_num_edges(num_edges(gVL));
  make_connected(gVL);
  BOOST_CHECK(num_edges(gVL) == VL_num_edges);

  graph_traits<LVgraph_t>::edges_size_type LV_num_edges(num_edges(gLV));
  make_connected(gLV);
  BOOST_CHECK(num_edges(gLV) == LV_num_edges);

  graph_traits<LLgraph_t>::edges_size_type LL_num_edges(num_edges(gLL));
  make_connected(gLL);
  BOOST_CHECK(num_edges(gLL) == LL_num_edges);

  return 0;
}


#include "conex/supernodal_solver.h"
#include "conex/clique_ordering.h"

#include <iostream>
#include <map>

#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::VectorXd;
using Eigen::VectorXi;
using Permutation = Eigen::PermutationMatrix<-1>;
using T = TriangularMatrixOperations;
using std::vector;

namespace {

vector<int> Relabel(const vector<int>& x, const vector<int>& labels) {
  vector<int> y(x.size());
  int i = 0;
  for (auto& xi : x) {
    y.at(i++) = labels.at(xi);
  }
  return y;
}

int GetMax(const std::vector<Clique>& cliques) {
  int max = cliques.at(0).at(0);
  for (const auto& c : cliques) {
    for (const auto ci : c) {
      if (ci > max) {
        max = ci;
      }
    }
  }
  return max;
}

class TriangularMatrixColumnOperations {
 public:
  TriangularMatrixColumnOperations(T::Matrix* mat) : mat_(mat) {
    dense_size = mat_->supernode_size.at(0);
  }
  void NextColumn() {
    supernode_column_++;
    column++;
    dense_size--;
    if (supernode_column_ >= mat_->supernode_size.at(supernode_index_)) {
      supernode_index_++;
      supernode_column_ = 0;
      dense_size = mat_->supernode_size.at(supernode_index_);
    }
  }

  void Rescale(double scale) {
    mat_->supernodes.at(supernode_index_).col(supernode_column_).array() *=
        scale;
    mat_->separator.at(supernode_index_).row(supernode_column_).array() *=
        scale;
  }

  double Diagonal() {
    return mat_->supernodes.at(supernode_index_)(supernode_column_,
                                                 supernode_column_);
  }

  // Applies the operation b = b - L.col(i) * y (i)
  void SubtractWeightedColumn(VectorXd* b, double y) {
    b->segment(column, dense_size) -= y * mat_->supernodes.at(supernode_index_)
                                              .col(supernode_column_)
                                              .tail(dense_size);

    int offset = mat_->supernode_size.at(supernode_index_);
    for (int i = 0; i < mat_->separator.at(supernode_index_).cols(); i++) {
      int row = mat_->path.at(supernode_index_).at(offset + i);
      (*b)(row) -=
          y * mat_->separator.at(supernode_index_)(supernode_column_, i);
    }
  }

  std::vector<int> NonzeroRows() {
    std::vector<int> y;
    for (int i = 1; i < dense_size; i++) {
      y.push_back(column + i);
    }
    int offset = mat_->supernode_size.at(supernode_index_);
    for (int i = 0; i < mat_->separator.at(supernode_index_).cols(); i++) {
      int row = mat_->path.at(supernode_index_).at(offset + i);
      y.push_back(row);
    }
    return y;
  }

 private:
  int supernode_index_ = 0;
  int dense_size = 0;
  int column = 0;
  int supernode_column_ = 0;
  SparseTriangularMatrix* mat_;
};

template <typename T>
int LookupSuperNode(const T& o, int index, int start) {
  for (int j = static_cast<int>(o.snodes.size()) - 1; j >= 0; j--) {
    if (o.snodes.at(j).size() > 0) {
      if (index >= o.snodes.at(j).at(0)) {
        return j;
      }
    }
  }
  throw "Sparse matrix is malformed: invalid supernode partition.";
}

double Get(const SparseTriangularMatrix& o, int i, int j) {
  if (j > i) {
    return 0;
  }
  // Find the supernode that owns the column.
  int node = LookupSuperNode(o, j, 0);

  // Apply offsets.
  int offset_i = i - o.path.at(node).at(0);
  int offset_j = j - o.path.at(node).at(0);
  if ((offset_i < o.supernode_size.at(node)) &&
      (offset_j < o.supernode_size.at(node))) {
    return o.supernodes.at(node)(offset_i, offset_j);
  }
  for (size_t k = o.supernode_size.at(node); k < o.path.at(node).size(); k++) {
    if (i == o.path.at(node).at(k)) {
      return o.separator.at(node)(offset_j, k - o.supernode_size.at(node));
    }
  }
  return 0;
}

class LowerTriangularSuperNodal {
  // We partition matrix into
  //
  // SN1  R1
  // R1   SN2  R2
  // R1   R2   SN3
  //
  // Where R1 is sparse.
 public:
  LowerTriangularSuperNodal(SparseTriangularMatrix* mat)
      : supernodes_(mat->supernodes),
        supernode_size_(mat->supernode_size),
        path_(mat->path),
        separator_(mat->separator) {
    Init();
  }

  void Init() {
    // for (int j = static_cast<int>(path_.size()) - 1; j >= 0; j--) {
    //   supernodes_.at(j).resize(supernode_size_.at(j), supernode_size_.at(j));
    //   separator_.at(j).resize(supernode_size_.at(j),  path_.at(j).size() -
    //   supernode_size_.at(j));
    // }
  }

  // The smallest super-node less than i.
  int LookupSuperNode(int index, int start) const {
    for (int j = static_cast<int>(path_.size()) - 1; j >= 0; j--) {
      if (index >= path_.at(j).at(0)) {
        return j;
      }
    }
    throw "Sparse matrix is malformed: invalid supernode partition.";
  }

  double Get(int i, int j) const {
    if (j > i) {
      return 0;
    }
    // Find the supernode that owns the column.
    int node = LookupSuperNode(j, 0);

    // Apply offsets.
    int offset_i = i - path_.at(node).at(0);
    int offset_j = j - path_.at(node).at(0);
    if ((offset_i < supernode_size_.at(node)) &&
        (offset_j < supernode_size_.at(node))) {
      return supernodes_.at(node)(offset_i, offset_j);
    }
    for (size_t k = supernode_size_.at(node); k < path_.at(node).size(); k++) {
      if (i == path_.at(node).at(k)) {
        return separator_.at(node)(offset_j, k - supernode_size_.at(node));
      }
    }
    return 0;
  }

  void Increment(double val, int i, int j, int supernode_index = -1) {
    if (val == 0) {
      return;
    }
    assert(j <= i);

    int node = supernode_index;
    // Find the supernode that owns storage for column j.
    if (node == -1) {
      node = LookupSuperNode(j, 0);
    }

    // Apply offsets.
    int offset_i = i - path_.at(node).at(0);
    int offset_j = j - path_.at(node).at(0);

    // i is also in the supernode.
    if ((offset_i < supernode_size_.at(node)) &&
        (offset_j < supernode_size_.at(node))) {
      supernodes_.at(node)(offset_i, offset_j) += val;
      return;
    }

    // i is in a separator.
    for (size_t k = supernode_size_.at(node); k < path_.at(node).size(); k++) {
      if (i == path_.at(node).at(k)) {
        separator_.at(node)(offset_j, k - supernode_size_.at(node)) += val;
        return;
      }
    }

    throw "Specified entry of sparse matrix is not accesible.";
  }

 public:
  using MapT = Eigen::Map<Eigen::MatrixXd, Eigen::Aligned>;
  std::vector<MapT>& supernodes_;
  std::vector<int>& supernode_size_;
  std::vector<Clique>& path_;
  std::vector<MapT>& separator_;
};

}  // namespace

void IntersectionOfSorted(const std::vector<int>& v1,
                          const std::vector<int>& v2, std::vector<int>* v3) {
  v3->clear();
  std::set_intersection(v1.begin(), v1.end(), v2.begin(), v2.end(),
                        back_inserter(*v3));
}

std::vector<Clique> Permute(std::vector<Clique>& path,
                            std::vector<int>& permutation) {
  auto y = path;
  for (size_t i = 0; i < path.size(); i++) {
    for (size_t j = 0; j < path.at(i).size(); j++) {
      y.at(i).at(j) = permutation.at(path.at(i).at(j));
    }
  }
  return y;
}

void Sort(std::vector<Clique>* path) {
  for (size_t i = 0; i < path->size(); i++) {
    std::sort(path->at(i).begin(), path->at(i).end());
  }
}

Eigen::MatrixXd TriangularMatrixOperations::ToDense(
    const SparseTriangularMatrix& mat) {
  MatrixXd y(mat.N, mat.N);
  for (int i = 0; i < mat.N; i++) {
    for (int j = 0; j < mat.N; j++) {
      y(i, j) = Get(mat, i, j);
    }
  }
  return y;
}

void TriangularMatrixOperations::SetConstant(SparseTriangularMatrix* mat,
                                             double val) {
  for (auto& n : mat->supernodes) {
    n.array() = val;
  }
  for (auto& n : mat->separator) {
    n.array() = val;
  }
}

void T::CholeskyInPlace(SparseTriangularMatrix* C) {
  TriangularMatrixColumnOperations col(C);
  LowerTriangularSuperNodal mat(C);
  double sqrt_d = std::sqrt(col.Diagonal());
  col.Rescale(1.0 / sqrt_d);

  // i: a supernode
  for (int i = 0; i < C->N - 1; i++) {
    // Substract col(2:n) c(2:n)^T
    auto indices = col.NonzeroRows();
    for (size_t k = 0; k < indices.size(); k++) {
      double weight = -mat.Get(indices.at(k), i);
      // Decrement column k
      for (size_t j = k; j < indices.size(); j++) {
        mat.Increment(weight * mat.Get(indices.at(j), i), indices.at(j),
                      indices.at(k));
      }
    }
    col.NextColumn();
    double sqrt_d = std::sqrt(col.Diagonal());
    col.Rescale(1.0 / sqrt_d);
  }
}

// Apply L  inverse.
// 1
// 1  1
// 1  1  1

// L
// B in
VectorXd T::ApplyInverseOfTranspose(SparseTriangularMatrix* mat,
                                    const VectorXd& b) {
  assert(b.rows() == mat->N);
  int n = b.rows();
  VectorXd y(n);
  auto res = b;
  LowerTriangularSuperNodal L(mat);

  y(n - 1) = res(n - 1) / L.Get(n - 1, n - 1);
  for (int i = n - 2; i >= 0; i--) {
    for (int j = 0; j < i + 1; j++) {
      res(j) = res(j) - L.Get(i + 1, j) * y(i + 1);
    }
    y(i) = res(i) / L.Get(i, i);
  }
  return y;
}

VectorXd T::ApplyInverse(SparseTriangularMatrix* mat, const VectorXd& b) {
  assert(b.rows() == mat->N);
  int n = b.rows();
  VectorXd y(n);
  auto res = b;
  LowerTriangularSuperNodal L(mat);

  TriangularMatrixColumnOperations col(mat);
  y(0) = res(0) / col.Diagonal();
  for (int i = 1; i < n; i++) {
    // Iterate over non-zero entries of column.
    // for (int j = i; j < n; j++) {
    //   res(j) = res(j) - L.Get(j, i - 1) * y(i - 1);
    // }
    col.SubtractWeightedColumn(&res, y(i - 1));

    col.NextColumn();
    y(i) = res(i) / col.Diagonal();
  }
  return y;
}

std::vector<int> UnionOfSorted(const std::vector<int>& x1,
                               const std::vector<int>& x2) {
  std::vector<int> y;
  set_union(x1.begin(), x1.end(), x2.begin(), x2.end(), inserter(y, y.end()));
  return y;
}

MatrixData GetData(const vector<vector<int>>& cliques, int root_clique) {
  vector<vector<int>> separators;
  vector<vector<int>> supernodes;
  vector<std::vector<int>> cliques_sorted = cliques;
  Sort(&cliques_sorted);
  vector<int> order;

  PickCliqueOrder(cliques_sorted, root_clique, &order, &supernodes,
                  &separators);

  return SupernodesToData(GetMax(cliques) + 1, order, supernodes, separators);
}

MatrixData GetData(const vector<vector<int>>& cliques,
                   const std::vector<int>& valid_leaf, int root_clique) {
  vector<vector<int>> separators;
  vector<vector<int>> supernodes;
  vector<std::vector<int>> cliques_sorted = cliques;
  Sort(&cliques_sorted);
  vector<int> order;

  PickCliqueOrder(cliques_sorted, valid_leaf, root_clique, &order, &supernodes,
                  &separators);
  return SupernodesToData(GetMax(cliques) + 1, order, supernodes, separators);
}

MatrixData SupernodesToData(int num_vars, const std::vector<int>& order,
                            const std::vector<std::vector<int>>& supernodes,
                            const std::vector<std::vector<int>>& separators) {
  MatrixData d;
  d.clique_order = order;
  d.permutation.resize(num_vars);
  d.permutation_inverse.resize(num_vars);
  int i = 0;
  for (auto& e : order) {
    for (auto& sn_ii : supernodes.at(e)) {
      d.permutation_inverse.at(i) = sn_ii;
      d.permutation.at(sn_ii) = i;
      i++;
    }
  }

  auto& supernode_size = d.supernode_size;
  supernode_size.resize(order.size());
  d.supernodes_original_labels.resize(order.size());
  d.separators_original_labels.resize(order.size());
  d.cliques.resize(supernodes.size());
  i = 0;
  for (auto e : order) {
    d.cliques.at(i) = supernodes.at(e);

    auto temp = Relabel(separators.at(e), d.permutation);
    std::sort(temp.begin(), temp.end());
    auto sep = Relabel(temp, d.permutation_inverse);

    for (auto si : sep) {
      d.cliques.at(i).push_back(si);
    }
    d.cliques.at(i) = Relabel(d.cliques.at(i), d.permutation);
    supernode_size.at(i) = supernodes.at(e).size();

    d.supernodes_original_labels.at(i) = supernodes.at(e);
    d.separators_original_labels.at(i) = sep;

    i++;
  }
  d.N = std::accumulate(supernode_size.begin(), supernode_size.end(), 0);
  return d;
}

}  // namespace conex


#define BOOST_TEST_MODULE algs test
#include <boost/test/auto_unit_test.hpp>

#include "algs.hpp"

std::vector<std::pair<int, int>> input = {
    std::pair<int, int>(3, 4),
    std::pair<int, int>(4, 9),
    std::pair<int, int>(8, 0),
    std::pair<int, int>(2, 3),
    std::pair<int, int>(5, 6),
    std::pair<int, int>(2, 9),
    std::pair<int, int>(5, 9),
    std::pair<int, int>(7, 3),
    std::pair<int, int>(5, 8),
    std::pair<int, int>(5, 6),
    std::pair<int, int>(0, 2),
    std::pair<int, int>(6, 1)
};

std::vector<std::pair<int, int>> expected = {
    std::pair<int, int>(3, 4),
    std::pair<int, int>(4, 9),
    std::pair<int, int>(8, 0),
    std::pair<int, int>(2, 3),
    std::pair<int, int>(5, 6),
    std::pair<int, int>(5, 9),
    std::pair<int, int>(7, 3),
    std::pair<int, int>(5, 8),
    std::pair<int, int>(6, 1)
};

BOOST_AUTO_TEST_CASE(algs_test) {
    BOOST_CHECK(*fast_search(&input) == expected);
    BOOST_CHECK(*fast_merge(&input) == expected);
    BOOST_CHECK(*weighed_fast_merge(&input) == expected);
    BOOST_CHECK(*weighed_fast_merge_with_path_compresion(&input) == expected);
}


/* Greg Anderson
 *
 * Definition of a bounded powerset domain for some underlying domain in the
 * ELINA abstract interpretation library.
 */

#ifndef _POWERSET_H_
#define _POWERSET_H_

#include <elina_abstract0.h>

#include <cstdlib>
#include <vector>
#include <memory>
#include <Eigen/Dense>

/**
 * A thin wrapper class around elina_abstract0_t* which is here to help with
 * memory management.
 */
class Abstract0 {
  public:
    /** The manager associated with this abstract value. */
    elina_manager_t* man;
    /** The wrapped value. */
    elina_abstract0_t* value;
    /** The center of the bounding box of this value. */
    Eigen::VectorXd center;
    /** This bool is true if center is valid. */
    bool center_computed;

    /**
     * Wraps the manager/value pair of m and v into an Abstract0
     *
     * \param m The manager of v.
     * \param v The value to wrap.
     */
    Abstract0(elina_manager_t* m, elina_abstract0_t* v);
    Abstract0(const Abstract0& other);
    Abstract0(Abstract0&& other);

    /**
     * Delete an abstract value. This frees the value but does not free the
     * manager, as it may be shared with other values.
     */
    ~Abstract0();

    Abstract0& operator=(const Abstract0& other);
    Abstract0& operator=(Abstract0&& other);
};

/**
 * Represents a bounded powerset of elements of some underlying abstract
 * domain. For some size N and abstract domain A, a powerset of of size N
 * over A consists of up to N values drawn from A.
 */
class Powerset {
  public:
    /** The number of disjuncts in the domain. */
    int size;
    /** The disjuncts in this powerset. */
    std::vector<std::shared_ptr<Abstract0>> disjuncts;
    /** Copy constructor. */
    Powerset(const Powerset&);
    Powerset(elina_manager_t* man, elina_abstract0_t*, int);
    /** Constructor when we need to compute centers. */
    Powerset(std::vector<std::shared_ptr<Abstract0>>&, int);
    /** Constructor when centers are known. */
    Powerset(std::vector<std::shared_ptr<Abstract0>>&,
        std::vector<Eigen::VectorXd>&, int);

    /**
     * Apply some linear transformation to this powerset.
     *
     * \param dims The dimensions to assign to.
     * \param update The expressions to assign into `dims`
     * \param size The size of `dims` and `update`
     * \param output_dim The dimension of the output abstract value.
     * \return A new powerset representing the modified abstract value.
     */
    Powerset assign_linexpr_array(
        elina_dim_t* dims, elina_linexpr0_t** update,
        unsigned int size,
        unsigned int output_dim) const;

    /**
     * Meet this powerset with some set of linear constraints.
     *
     * \param cons The array of linear constraints to meet with.
     * \return The meet of this value with the constraints.
     */
    Powerset meet_lincons_array(
        elina_lincons0_array_t* cons) const;

    /**
     * Permute the dimensions of this abstract value.
     *
     * \param dp An ELINA dimperm object describing the permutation.
     * \return The permuted abstract value.
     */
    Powerset permute_dimensions(elina_dimperm_t* dp) const;

    /**
     * Remove dimensions from this abstract value.
     *
     * \param dc An ELINA dimchange object describing the dimensions to remove.
     * \return The value with the given dimensions removed.
     */
    Powerset remove_dimensions(elina_dimchange_t* dc) const;

    /**
     * Perform an affine transformation on this abstract value. For each `x` in
     * this abstract value, the result will contain `w*x + b`.
     *
     * \param w The weight matrix of the affine transformation.
     * \param b The bias vector of the affine transformation.
     * \return The transformed abstract value.
     */
    Powerset affine(const Eigen::MatrixXd& w, const Eigen::VectorXd& b) const;

    /**
     * Perform a ReLU on this abstract value. For each dimension i, we split
     * this abstract value around the axis x_i = 0. We assign x_i = 0 in the
     * half where x_i <= 0, then join it with the x_i >= 0 half.
     *
     * \return The abstract value transformed by a ReLU.
     */
    Powerset relu() const;

    /**
     * Join this powerset with another powerset. The size bound of the new
     * powerset will be the greater of the size bounds of this powerset and
     * the other powerset. If the total number of disjuncts in these two
     * powersets is smaller than this bound then the new powerset is
     * initialized with all the disjuncts from both powersets. Otherwise,
     * disjuncts are heuristically chosen and joined until the total number
     * is within the bound.
     *
     * \param other The powerset to join this one with.
     * \return The join of this value and `other`.
     */
    Powerset join(const Powerset& other) const;

    /**
     * Determine whether this powerset's concretization is empty.
     *
     * \return `true` if this abstract value represents bottom.
     */
    bool is_bottom() const;

    /**
     * Get the number of dimensions represented by this value.
     *
     * \return The number of dimensions in the constrained space.
     */
    ssize_t dims() const;

    /**
     * Get a bounding box around this abstract value.
     *
     * \return An ELINA hyperinterval surrounding this abstract value.
     */
    elina_interval_t** bounding_box() const;

    /**
     * Print a bounding box for this powerset.
     */
    void print_bounding_box() const;

    Powerset& operator=(const Powerset& other);
};

#endif


#include "NICS-module.h"
#include <vector>
#include <string>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <math.h>
#include <Eigen/Dense>

//-------------------------------------------------------------
// Program to find the centers of rings fo NICS(0) and NICS(1)
// calculation and write some quazi-.gjf file
//-------------------------------------------------------------

int main() {

   //---------------------------------------------
   // Get the information out of the input file:
   //---------------------------------------------

   // Variables to get out of input file:
   int nmols;
   std::vector<int> nrings_per_molecule;
   std::vector<std::string> filenames;
   std::vector<std::vector<int>> rings_atoms; // atoms making up rings

   // Read input file:
   read_input_file(nmols, filenames, nrings_per_molecule, rings_atoms);
   std::cout << "nmols: " << nmols << std::endl;
   print_vector_string(filenames);
   std::cout << "nrings_per_molecule: " ;
   print_vector_int(nrings_per_molecule);
   print_matrix_int(rings_atoms);

   //--------------------------------------------------------
   // Get the coordinates of the molecules and rings from
   // the .xyz files, move and reorient them, and print the
   // final coordinates:
   //--------------------------------------------------------

   // Variables:
   std::string in_xyz_file_name, out_ring_xyz_file_name, out_xyz_file_name;
   std::string line, temp;
   std::vector<std::string> lables_ring, lables;
   std::vector<double> mass, xring, yring, zring,  x, y, z;
   double temp_double;
   double xsum, ysum, zsum, xcenter, ycenter, zcenter;
   int i, j, k, kk, line_num, ring_num;
   Eigen::Vector3d vec1, vec2, normal1, normal2, eigenvec_k;
   Eigen::MatrixXd angle_to_normal(3,3);
   Eigen::Matrix3d inert;
   Eigen::MatrixXd ring_coordinates(1,1), coordinates(1,1);
   int min_row, min_col, min_vec;
   std::vector<double> NICS0, NICS1, NICS2;

   ring_num = 0;
   for (i=0; i<nmols; i=i+1) { // for each of the molecules:
      for (j=0; j<nrings_per_molecule[i]; j=j+1) { // for each ring in mol

         // Reset lables and coordinates arrays:
         mass.resize(0);
         lables.resize(0);
         x.resize(0);
         y.resize(0);
         z.resize(0);
         lables_ring.resize(0);
         xring.resize(0);
         yring.resize(0);
         zring.resize(0);

         // Open the input file for that molecule and ring:
         in_xyz_file_name = filenames[i] + ".xyz";
         std::ifstream in_xyz_file;
         in_xyz_file.open(in_xyz_file_name);

         // get the coordinates of the rings and the molecule:
         line_num = 1;
         while (!in_xyz_file.eof()) {
            getline(in_xyz_file,line);
            if (line_num > 2) {
               std::stringstream ssin(line);
               std::vector<std::string> split_line;
               split_line.resize(0);
               while (ssin.good()){
                  ssin >> temp;
                  split_line.push_back(temp);
               }
               if (split_line.size() > 1) {
                  // get lables and coordinates for whole molecule
                  lables.push_back(split_line[0]);
                  temp_double = std::stod(split_line[1]);
                  x.push_back(temp_double);
                  temp_double = std::stod(split_line[2]);
                  y.push_back(temp_double);
                  temp_double = std::stod(split_line[3]);
                  z.push_back(temp_double);
                  for (k=0; k<rings_atoms[ring_num].size(); k=k+1){
                     if (line_num == rings_atoms[ring_num][k] + 2) {
                        // get lables and coordinates for ring:
                        lables_ring.push_back(split_line[0]);
                        temp_double = std::stod(split_line[1]);
                        xring.push_back(temp_double);
                        temp_double = std::stod(split_line[2]);
                        yring.push_back(temp_double);
                        temp_double = std::stod(split_line[3]);
                        zring.push_back(temp_double);
                        continue;
                     }
                  }
               }
            }
            line_num = line_num + 1;
         }

         // Find center of ring:
         xsum = 0.0;
         ysum = 0.0;
         zsum = 0.0;
         for (k=0; k<xring.size(); k=k+1) {
            xsum = xsum + xring[k];
            ysum = ysum + yring[k];
            zsum = zsum + zring[k];
         }
         xcenter = xsum/xring.size();
         ycenter = ysum/yring.size();
         zcenter = zsum/zring.size();

         // Move the coordinates vectors for the ring
         // so that the center of the ring is positioned
         // at (0,0,0):
         for (k=0; k<xring.size(); k=k+1) {
            xring[k] = xring[k] - xcenter;
            yring[k] = yring[k] - ycenter;
            zring[k] = zring[k] - zcenter;
         }

         // Move the coordinates vectors for the
         // entire molecule so that the center of the
         // ring is positioned at (0,0,0):
         for (k=0; k<x.size(); k=k+1) {
            x[k] = x[k] - xcenter;
            y[k] = y[k] - ycenter;
            z[k] = z[k] - zcenter;
         }

         // Define a plane of two vectors on the ring
         // and find a vector normal to these vectors:
         vec1(0) = xring[0] - xring[1];
         vec1(1) = yring[0] - yring[1];
         vec1(2) = zring[0] - zring[1];
         vec2(0) = xring[2] - xring[1];
         vec2(1) = yring[2] - yring[1];
         vec2(2) = zring[2] - zring[1];
         normal1 = vec1.cross(vec2);
         for (k=1; k<normal1.size(); k=k+1) {
            normal1(k) = normal1(k)/normal1.norm();
         }

         mass.resize(lables_ring.size());
         for (k=0; k<mass.size(); k=k+1) {
            periodic_table(lables_ring[k],mass[k]);
         }
         std::cout << "masses: " << std::endl;
         print_vector_double(mass);

         // Determine the inertia tensor:
         inert(0,0) = 0.0;
         inert(0,1) = 0.0;
         inert(0,2) = 0.0;
         inert(1,0) = 0.0;
         inert(1,1) = 0.0;
         inert(1,2) = 0.0;
         inert(2,0) = 0.0;
         inert(2,1) = 0.0;
         inert(2,2) = 0.0;
         for (k=0; k<xring.size(); k=k+1) {
            inert(0,0) = inert(0,0) + mass[k] * (yring[k]*yring[k] +
                  zring[k]*zring[k]) ;
            inert(0,1) = inert(0,1) - mass[k]*yring[k]*xring[k];
            inert(0,2) = inert(0,2) - mass[k]*xring[k]*zring[k];
            inert(1,1) = inert(1,1) + mass[k] * (xring[k]*xring[k] +
                  zring[k]*zring[k]) ;
            inert(1,2) = inert(1,2) - mass[k]*yring[k]*zring[k];
            inert(2,2) = inert(2,2) + mass[k] * (xring[k]*xring[k] +
                  yring[k]*yring[k]) ;
         }
         inert(1,0) = inert(0,1);
         inert(2,0) = inert(0,2);
         inert(2,1) = inert(1,2);

         std::cout << "inertia_tensor" << std::endl;
         std::cout << inert << std::endl;

         // Diagonalize the inertia tensor (identify the
         // principal components):
         Eigen::EigenSolver<Eigen::Matrix3d> principal_components(inert);
         Eigen::Vector3cd eigenvals=principal_components.eigenvalues();
         Eigen::Matrix3cd eigenvecs=principal_components.eigenvectors();

         std::cout << "eigencalues:" << std::endl;
         std::cout << eigenvals << std::endl;
         std::cout << "eigenvectors:" << std::endl;
         std::cout << eigenvecs << std::endl;

         // Which of these eigenvectors is normal to the plane of
         // the ring? (has the smallest angle with the above
         // determined normal vector, normal1):
         for (k=0; k<3; k=k+1) {
            for (kk=0; kk<3; kk=kk+1) {
               eigenvec_k(kk) = eigenvecs(kk,k).real();
            }
            // could be either 0 or pi or 2pi, depending on direction:
            angle_to_normal(k,0) = fabs(acos(eigenvec_k.dot(normal1) /
                     (eigenvec_k.norm() * normal1.norm())) - M_PI);
            angle_to_normal(k,1) = fabs(acos(eigenvec_k.dot(normal1) /
                     (eigenvec_k.norm()  * normal1.norm())));
            angle_to_normal(k,2) = fabs(acos(eigenvec_k.dot(normal1) /
                     (eigenvec_k.norm() * normal1.norm())) - M_PI * 2.0);
         }
         angle_to_normal.minCoeff(&min_row, &min_col);
         min_vec = min_row;
         for (k=1; k<3; k=k+1) {
            normal2(k) = eigenvecs(k,min_vec).real();
         }

         // convert vectors xring, yring, and zring to a
         // ring_coordinates matrix:
         ring_coordinates.resize(xring.size(),3);
         for(k=0; k<xring.size(); k=k+1) {
            ring_coordinates(k,0) = xring[k];
            ring_coordinates(k,1) = yring[k];
            ring_coordinates(k,2) = zring[k];
         }

         // Convert vector x, y, and z to a coordinates matrix
         // and add dummy atoms:
         coordinates.resize(x.size() + 3,3);
         for (k=0; k<x.size(); k=k+1) {
            coordinates(k,0) = x[k];
            coordinates(k,1) = y[k];
            coordinates(k,2) = z[k];
         }
         coordinates(x.size(),0) = normal2(0);
         coordinates(x.size(),1) = normal2(1);
         coordinates(x.size(),2) = normal2(2);
         coordinates(x.size()+1,0) = 0.0;
         coordinates(x.size()+1,1) = 0.0;
         coordinates(x.size()+1,2) = 0.0;
         coordinates(x.size()+2,0) = -normal2(0);
         coordinates(x.size()+2,1) = -normal2(1);
         coordinates(x.size()+2,2) = -normal2(2);

         // Print final coordinates for the ring
         out_ring_xyz_file_name = filenames[i] + "_ring_"
            + std::to_string(j) + ".xyz";
         if_file_exist_delete(out_ring_xyz_file_name);
         std::ofstream out_ring_xyz_file;
         out_ring_xyz_file.open(out_ring_xyz_file_name);

         out_ring_xyz_file << rings_atoms[ring_num].size() + 4 << std::endl;
         out_ring_xyz_file << "sldgjksjg" << std::endl;
         for (kk=0; kk<rings_atoms[ring_num].size(); kk=kk+1) {
            out_ring_xyz_file << lables_ring[kk] << "         "
                         << ring_coordinates(kk,0) << "         "
                         << ring_coordinates(kk,1) << "         "
                         << ring_coordinates(kk,2) << std::endl;
         }
         out_ring_xyz_file << "N          " << normal1(0) << "         "
                           << normal1(1) << "         " << normal1(2)
                           << std::endl;
         for (k=0; k<3; k=k+1) {
            out_ring_xyz_file << "O          " << eigenvecs(k,0).real() << "         "
                              << eigenvecs(k,1).real() << "         " << eigenvecs(k,2).real()
                              << std::endl;
         }

         // Align normal2 to z-axis, so that
         // the ring is in the xy plane"
         align(normal2, coordinates);
         align(normal2, ring_coordinates);

//         // Print final coordinates for the ring
//         out_ring_xyz_file_name = filenames[i] + "_ring_"
//            + std::to_string(j) + ".xyz";
//         if_file_exist_delete(out_ring_xyz_file_name);
//         std::ofstream out_ring_xyz_file;
//         out_ring_xyz_file.open(out_ring_xyz_file_name);
//
//         out_ring_xyz_file << rings_atoms[ring_num].size() + 2 << std::endl;
//         out_ring_xyz_file << "sldgjksjg" << std::endl;
//         for (kk=0; kk<rings_atoms[ring_num].size(); kk=kk+1) {
//            out_ring_xyz_file << lables_ring[kk] << "         "
//                         << ring_coordinates(kk,0) << "         "
//                         << ring_coordinates(kk,1) << "         "
//                         << ring_coordinates(kk,2) << std::endl;
//         }
//         out_ring_xyz_file << "N          " << normal1(0) << "         "
//                           << normal1(1) << "         " << normal1(2)
//                           << std::endl;
//         for (k=0; k<3; k=k+1) {
//            out_ring_xyz_file << "O          " << normal2(0) << "         "
//                              << normal2(1) << "         " << normal2(2)
//                              << std::endl;
//         }

         // Print final coordinates for the ring,
         // adding the dummy atoms:
         out_xyz_file_name = filenames[i] + "_" +
                                  std::to_string(j) + ".xyz";
         if_file_exist_delete(out_xyz_file_name);
         std::ofstream out_xyz_file;
         out_xyz_file.open(out_xyz_file_name);

         // printing:
         out_xyz_file << x.size() + 3 << std::endl;
         out_xyz_file << "0 1" << std::endl;
         for (kk=0; kk<x.size(); kk=kk+1) {
            out_xyz_file << std::left << std::setw(2) << lables[kk]
                         << std::setw(18) << std::right
                         << std::fixed << std::setprecision(7)
                         << coordinates(kk,0)
                         << std::setw(19) << std::right
                         << std::fixed << std::setprecision(7)
                         << coordinates(kk,1)
                         << std::setw(19) << std::right
                         << std::fixed << std::setprecision(7)
                         << coordinates(kk,2) << std::endl;
         }
         for (kk=x.size(); kk<x.size()+3; kk=kk+1) {
         out_xyz_file << std::left << std::setw(2) << "Bq"
                      << std::setw(18) << std::right
                      << std::fixed << std::setprecision(7)
                      << coordinates(kk,0)
                      << std::setw(19) << std::right
                      << std::fixed << std::setprecision(7)
                      << coordinates(kk,1)
                      << std::setw(19) << std::right
                      << std::fixed << std::setprecision(7)
                      << coordinates(kk,2) << std::endl;
         }

         // End of loop, reset for next ring:
         ring_num = ring_num + 1;
         in_xyz_file.close();
         out_ring_xyz_file.close();
         out_xyz_file.close();
      }

   }

   return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef SNPTEST_REGRESSION_INDEPENDENT_LOGF_WEIGHTED_LOGLIKELIHOOD_HPP
#define SNPTEST_REGRESSION_INDEPENDENT_LOGF_WEIGHTED_LOGLIKELIHOOD_HPP

#include <vector>
#include <memory>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "metro/regression/Design.hpp"
#include "metro/regression/LogLikelihood.hpp"
#include "metro/regression/LogPosteriorDensity.hpp"

namespace metro {
	namespace regression {
		// Represents a log-likelihood function weighted by independent
		// log-F distributions on each of a chosen set of parameters.
		struct IndependentLogFWeightedLogLikelihood: public LogPosteriorDensity {
		public:
			typedef std::auto_ptr< IndependentLogFWeightedLogLikelihood > UniquePtr ;
			enum Normalisation { ePDF, eZeroAtMean } ;
			static UniquePtr create(
				LogLikelihood::UniquePtr ll,
				std::vector< int > parameter_indices,
				std::vector< double > nu1,
				std::vector< double > nu2
			) ;
		public:
			IndependentLogFWeightedLogLikelihood(
				LogLikelihood::UniquePtr ll,
				std::vector< int > parameter_indices,
				std::vector< double > nu1,
				std::vector< double > nu2
			) ;

			regression::Design& design() const { return m_ll->design() ; }

			int number_of_parameters() const ;
			std::string get_parameter_name( std::size_t i ) const ;

			int number_of_outcomes() const ;
			// Return a lx2 matrix identifying the l parameters.
			// The row for each parameter contains the outcome level and design matrix column for that parameter.
			IntegerMatrix identify_parameters() const ;

			void evaluate_at( Point const& parameters, int const numberOfDerivatives = 2 ) ;
			void evaluate( int const numberOfDerivatives = 2 ) ;

			Point const& parameters() const {
				return m_ll->parameters() ;
			}

			double get_value_of_function() const {
				return m_ll->get_value_of_function() + m_value_of_function ;
			}

			Vector get_value_of_first_derivative() const {
				return m_ll->get_value_of_first_derivative() + m_value_of_first_derivative ;
			}

			Matrix get_value_of_second_derivative() const {
				return m_ll->get_value_of_second_derivative() + m_value_of_second_derivative ;
			}

			Vector get_prior_mode() const ;

			Matrix get_prior_second_derivative() const {
				return m_value_of_second_derivative ;
			}

			Matrix get_loglikelihood_second_derivative() const {
				return m_ll->get_value_of_second_derivative() ;
			}

			std::string get_summary() const ;

		private:
			LogLikelihood::UniquePtr m_ll ;
			std::vector< int > const m_parameter_indices ;
			std::vector< double > m_alpha ;
			std::vector< double > m_beta ;
			Normalisation const m_normalisation ;
			double m_constant ;
			double m_value_of_function ;
			Vector m_value_of_first_derivative ;
			Matrix m_value_of_second_derivative ;

		private:
			void evaluate_impl( int const numberOfDerivatives = 2 ) ;

		} ;
	}
}
#endif


#include <osmium/handler.hpp>
#include <osmium/io/pbf_input.hpp>
#include <osmium/visitor.hpp>
#include <osmium/osm/item_type.hpp>
#include <string>
#include "rapidjson/writer.h"
#include "rapidjson/stringbuffer.h"
#include <boost/filesystem.hpp>
#include <fstream>
#include <unordered_map>
#include <unordered_set>
#include <cmath>

using namespace rapidjson;
using namespace std;

namespace fs = boost::filesystem;

static const auto decimal_to_radian = M_PI / 180;

struct Tile {
  uint x;
  uint y;
  uint z;
};

class Handler : public osmium::handler::Handler {
  const uint z;
  const string output;
  unordered_map<int,unordered_set<string>> indices;

  public:
    Handler(const uint tileZ, const string & outputFlag) :
      z(tileZ),
      output(outputFlag) {}

    Tile pointToTile(const double lon, const double lat) {
      auto tile = Tile();

      auto latSin = std::sin(lat * decimal_to_radian);
      auto z2 = std::pow(2, z);
      tile.x = std::floor(z2 * (lon / 360 + 0.5));
      tile.y = std::floor(z2 * (0.5 - 0.25 * std::log((1 + latSin) / (1 - latSin)) / M_PI));
      tile.z = z;

      return tile;
    }

    string xy (const Tile & tile) {
      return to_string(tile.x) + "/" + to_string(tile.y);
    }

    void mkdirTile(const Tile & tile) {
      std::ostringstream s;
      s << output << "/" << tile.x << "/" << tile.y;
      fs::create_directories(s.str());
    }

    void appendData(const string & tilePath, const string & data) {
      std::ostringstream path;
      path << output << "/" << tilePath << "/data.json";
      ofstream myfile;
      myfile.open(path.str(), ios::app);
      myfile << data << endl;
      myfile.close();
    }

    void node(const osmium::Node & node) {
      const auto & tags = node.tags();
      auto lon = node.location().lon();
      auto lat = node.location().lat();
      auto id = node.id();

      Tile tile = pointToTile(lon, lat);
      mkdirTile(tile);
      unordered_set<string> tiles;
      tiles.insert(xy(tile));
      pair<int, unordered_set<string>> nodeIndex(id, tiles);

      indices.insert(nodeIndex);

      StringBuffer nodeBuffer;
      Writer<StringBuffer> nodeWriter(nodeBuffer);

      nodeWriter.StartObject();

      nodeWriter.Key("type");
      nodeWriter.String("node");
      nodeWriter.Key("id");
      nodeWriter.Int(id);
      nodeWriter.Key("lon");
      nodeWriter.Double(lon);
      nodeWriter.Key("lat");
      nodeWriter.Double(lat);
      nodeWriter.Key("version");
      nodeWriter.Int(node.version());
      nodeWriter.Key("timestamp");
      nodeWriter.Int(node.timestamp().seconds_since_epoch());
      nodeWriter.Key("user");
      nodeWriter.String(node.user());

      nodeWriter.Key("tags");
      nodeWriter.StartObject();
      for (const auto & tag : tags) {
        nodeWriter.Key(tag.key());
        nodeWriter.String(tag.value());
      }
      nodeWriter.EndObject();
      nodeWriter.EndObject();

      appendData(xy(tile), nodeBuffer.GetString());
    }

    void way(const osmium::Way & way) {
      const auto & tags = way.tags();
      const auto id = way.id();

      StringBuffer wayBuffer;
      Writer<StringBuffer> wayWriter(wayBuffer);

      wayWriter.StartObject();

      wayWriter.Key("type");
      wayWriter.String("way");
      wayWriter.Key("id");
      wayWriter.Int(id);
      wayWriter.Key("version");
      wayWriter.Int(way.version());
      wayWriter.Key("timestamp");
      wayWriter.Int(way.timestamp().seconds_since_epoch());
      wayWriter.Key("user");
      wayWriter.String(way.user());
      wayWriter.Key("closed");
      wayWriter.Bool(way.is_closed());

      wayWriter.Key("node_refs");
      wayWriter.StartArray();
      unordered_set<string> tiles;
      int tileCount = 0;
      for (const auto & node : way.nodes()) {
        int ref = node.ref();
        wayWriter.Int(ref);

        if(indices.count(ref)) {
          const auto & nodeTiles = indices.at(ref);

          for (auto & tile : nodeTiles) {
            tiles.insert(tile);
            tileCount++;
          }
        }
      }
      pair<int,unordered_set<string>> wayIndex(id, tiles);
      indices.insert(wayIndex);
      wayWriter.EndArray();

      wayWriter.Key("tags");
      wayWriter.StartObject();
      for (const auto & tag : tags) {
        wayWriter.Key(tag.key());
        wayWriter.String(tag.value());
      }
      wayWriter.EndObject();
      wayWriter.EndObject();

      for (const auto & tileXy : tiles) {
        appendData(tileXy, wayBuffer.GetString());
      }
    }

    void relation(const osmium::Relation & relation) {
      const auto & tags = relation.tags();
      const auto id = relation.id();

      StringBuffer relationBuffer;
      Writer<StringBuffer> relationWriter(relationBuffer);

      relationWriter.StartObject();

      relationWriter.Key("type");
      relationWriter.String("relation");
      relationWriter.Key("id");
      relationWriter.Int(id);
      relationWriter.Key("version");
      relationWriter.Int(relation.version());
      relationWriter.Key("timestamp");
      relationWriter.Int(relation.timestamp().seconds_since_epoch());
      relationWriter.Key("user");
      relationWriter.String(relation.user());

      relationWriter.Key("members");
      relationWriter.StartArray();
      unordered_set<string> tiles;
      int tileCount = 0;
      for (const auto & member : relation.members()) {
        const auto ref = member.ref();
        relationWriter.StartObject();
        relationWriter.Key("id");
        relationWriter.Int(ref);
        relationWriter.Key("type");
        relationWriter.String(item_type_to_name(member.type()));
        relationWriter.Key("role");
        relationWriter.String(member.role());
        relationWriter.EndObject();

        if(indices.count(ref)) {
          const auto & memberTiles = indices.at(ref);

          for (const auto & tile : memberTiles) {
            tiles.insert(tile);
            tileCount++;
          }
        }
      }
      pair<int,unordered_set<string>> relationIndex(id, tiles);
      indices.insert(relationIndex);
      relationWriter.EndArray();

      relationWriter.Key("tags");
      relationWriter.StartObject();
      for (const auto & tag : tags) {
        relationWriter.Key(tag.key());
        relationWriter.String(tag.value());
      }
      relationWriter.EndObject();
      relationWriter.EndObject();

      for (const auto & tileXy : tiles) {
        appendData(tileXy, relationBuffer.GetString());
      }
    }
};


#include "poli/ContinuousPolicyManager.h"
#include "percepto_msgs/ContinuousAction.h"

#include "argus_utils/utils/MatrixUtils.h"
#include "modprop/utils/Randomization.hpp"

#include "lookup/LookupUtils.hpp"

#include <boost/foreach.hpp>
#include <sstream>

using namespace argus;

namespace percepto
{

ContinuousPolicyManager::ContinuousPolicyManager()
: _infoManager( _lookup ) {}

void ContinuousPolicyManager::Initialize( ContinuousPolicyInterface* interface,
                                          ros::NodeHandle& nh,
                                          ros::NodeHandle& ph )
{
	// Read information from interface
	_interface = interface;
	const VectorType& lowerLimit = _interface->GetLowerLimits();
	const VectorType& upperLimit = _interface->GetUpperLimits();
	unsigned int outputDim = _interface->GetNumOutputs();
	std::vector<std::string> paramNames = _interface->GetParameterNames();

	// Read information from input listener
	ros::NodeHandle subh( ph.resolveName("input_streams") );
	_inputStreams.Initialize( subh );
	unsigned int inputDim = _inputStreams.GetDim();

	// Parse policy info
	ros::NodeHandle neth( ph.resolveName("policy_class") );
	_policy.Initialize( inputDim, outputDim, neth );

	double initialInfo;
	GetParam( neth, "initial_info", initialInfo, 10.0 );

	// Compute initializations
	_policyScales = VectorType( outputDim );
	_policyOffsets = VectorType( outputDim );

	DistributionParameters initParams;
	initParams.mean = VectorType( outputDim );
	initParams.info = MatrixType::Identity( outputDim, outputDim );
	for( unsigned int i = 0; i < paramNames.size(); ++i )
	{
		if( !std::isfinite( upperLimit(i) ) || !std::isfinite( lowerLimit(i) ) )
		{
			_policyScales(i) = 1.0;
			_policyOffsets(i) = 0.0;
		}
		else
		{
			_policyScales(i) = ( upperLimit(i) - lowerLimit(i) ) / 2;
			_policyOffsets(i) = ( upperLimit(i) - lowerLimit(i) ) / 2;
		}
		initParams.mean(i) = 0;
		initParams.info(i,i) = initialInfo;
	}
	_policy.InitializeOutput( initParams );

	ROS_INFO_STREAM( "Network initialized: " << std::endl << *_policy.GetPolicyModule() );
	ROS_INFO_STREAM( "Policy scales: " << _policyScales.transpose() << std::endl <<
	                 "Policy offsets: " << _policyOffsets.transpose() << std::endl );

	// See if any RNG seed was given
	unsigned int seed;
	if( HasParam( ph, "rng_seed" ) )
	{
		GetParamRequired( ph, "rng_seed", seed );
		_mvg = MultivariateGaussian<>( outputDim, seed );
	}
	else
	{
		_mvg = MultivariateGaussian<>( outputDim );
	}
	GetParam( ph, "max_sample_devs", _maxSampleDevs, 3.0 );

	// Create publisher topics
	_actionPub = ph.advertise<percepto_msgs::ContinuousAction>( "action_raw", 0 );
	_normalizedActionPub = ph.advertise<percepto_msgs::ContinuousAction>( "action_normalized", 0 );

	// Advertise parameter services
	std::string getParamTopic = ph.resolveName( "get_params" );
	std::string setParamTopic = ph.resolveName( "set_params" );
	_getParamServer = nh.advertiseService( getParamTopic, &ContinuousPolicyManager::GetParamHandler, this );
	_setParamServer = nh.advertiseService( setParamTopic, &ContinuousPolicyManager::SetParamHandler, this );

	GetParamRequired( ph, "policy_name", _policyName );
	register_lookup_target( ph, _policyName );

	// Register information
	PolicyInfo info;
	info.inputDim = inputDim;
	info.outputDim = outputDim;
	info.paramQueryService = getParamTopic;
	info.paramSetService = setParamTopic;
	GetParamRequired( neth, "", info.networkInfo );
	if( !_infoManager.WriteMemberInfo( _policyName, info, true, ros::Duration( 10.0 ) ) )
	{
		throw std::runtime_error( "Could not write policy info!" );
	}
}

VectorType ContinuousPolicyManager::GetInput( const ros::Time& time )
{
	StampedFeatures input;
	if( !_inputStreams.ReadStream( time, input ) )
	{
		std::stringstream ss;
		ss << "Could not read input streams at time: " << time;
		throw std::out_of_range( ss.str() );
	}
	return input.features;
}

ContinuousPolicyManager::DistributionParameters
ContinuousPolicyManager::GetNormalizedDistribution( const ros::Time& time )
{
	return _policy.GenerateOutput( GetInput( time ) );
}

ContinuousPolicyManager::DistributionParameters
ContinuousPolicyManager::GetDistribution( const ros::Time& time )
{
	DistributionParameters initParams = GetNormalizedDistribution( time );
	initParams.mean = ( _policyScales.array() * initParams.mean.array() ).matrix();
	Eigen::LDLT<MatrixType> infoLDLT( initParams.info );
	MatrixType cov = infoLDLT.solve( MatrixType::Identity( initParams.mean.size(),
	                                                       initParams.mean.size() ) );
	cov = _policyScales.transpose() * cov * _policyScales;
	Eigen::LDLT<MatrixType> covLDLT( cov );
	initParams.info = covLDLT.solve( MatrixType::Identity( initParams.mean.size(),
	                                                   initParams.mean.size() ) );
	return initParams;
}

VectorType ContinuousPolicyManager::GetNormalizedOutput( const ros::Time& time )
{
	ContinuousPolicyManager::DistributionParameters initParams = GetNormalizedDistribution( time );
	_mvg.SetMean( initParams.mean );
	_mvg.SetInformation( initParams.info );
	return _mvg.Sample( _maxSampleDevs );
}

VectorType ContinuousPolicyManager::GetOutput( const ros::Time& time )
{
	VectorType out = GetNormalizedOutput( time );
	return ( out.array() * _policyScales.array() ).matrix() + _policyOffsets;
}

ContinuousAction ContinuousPolicyManager::Execute( const ros::Time& now )
{
	VectorType in = GetInput( now );
	VectorType out = GetOutput( now );
	_interface->SetOutput( out );

	ContinuousAction act( now, in, out );
	_actionPub.publish( act.ToMsg() );
	act.Normalize( _policyScales, _policyOffsets );
	_normalizedActionPub.publish( act.ToMsg() );
	act.Unnormalize( _policyScales, _policyOffsets );
	return act;
}

bool ContinuousPolicyManager::SetParamHandler( percepto_msgs::SetParameters::Request& req,
                                               percepto_msgs::SetParameters::Response& res )
{
	_policy.GetParameters()->SetParamsVec( GetVectorView( req.parameters ) );
	return true;
}

bool ContinuousPolicyManager::GetParamHandler( percepto_msgs::GetParameters::Request& req,
                                               percepto_msgs::GetParameters::Response& res )
{
	VectorType initParams = _policy.GetParameters()->GetParamsVec();
	res.parameters = std::vector<double>( initParams.data(), initParams.data() + initParams.size() );
	return true;
}

}


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/IO/Polyhedron_iostream.h>
//#include <CGAL/Surface_mesh_parameterization/Parameterization_SurfaceMesh_3.h>
#include <CGAL/Surface_mesh_parameterization/parameterize.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_authalic_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Square_border_parameterizer_3.h>
//#include <CGAL/Surface_mesh_parameterization/Parameterization_mesh_patch_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_conformal_map_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Barycentric_mapping_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Mean_value_coordinates_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/LSCM_parameterizer_3.h>
#include <CGAL/Eigen_solver_traits.h>
#include <CGAL/Polygon_mesh_processing/measure.h>
#include <boost/foreach.hpp>
#include <iostream>
#include <fstream>
#include <cstdlib>

#include "angular-map.h"
#include "simple_svg_1.0.0.hpp"

namespace flattener
{

  struct editable_mesh
  {
    struct V
    {
      uint old_vid;
      uint vid;
    };

    struct E{
      uint v[2];
      uint f[2];
    };

    struct F
    {
      uint v[3];
      uint e[3];
      uint nei[3];
    };

    uint add_v(uint vid)
    {
      V v;
      v.old_vid=vid;
      v.vid=last_vid++;
      vs_[v.vid]=v;
      return v.vid;
    }

    uint add_e(uint fid)
    {
      //V v;
      //v.old_vid=vid;
      //v.vid=last_vid++;
      //vs_[v.vid]=v;
      return 0;//v.vid;
    }

    void add_f(masc::triangle& t)
    {
      F f;
      for(int i=0;i<3;i++) f.v[i]=add_v(t.v[i]);
      //for(int i=0;i<3;i++) f.e[i]=add_e(f,i);
    }

    vector<F>   fs_;
    map<uint,V> vs_;
    map<uint,E> es_;
    uint last_vid;
  };

  bool ANGULAR_MAP_CREATOR::cut(masc::model * m)
  {
    return cut(m, 0);
  }

  bool ANGULAR_MAP_CREATOR::cut(masc::model * m, uint seed)
  {
    //set<uint> vids;
    set<uint> fids;
    vector< vector<uint> > neighbors(m->t_size, vector<uint>(3,UINT_MAX)); //one for each triangle

    //construct an mesh by iteratively add facets of m into it
    //starting from the seed facet
    list<uint> open;
    open.push_back(seed);
    fids.insert(seed);
    //for(short i=0;i<3;i++) vids.insert(m->tris[seed].v[i]);
    uint vertex_count=3;
    uint edge_count=3;

    while(open.empty()==false)
    {
      uint fid=open.front();
      open.pop_front();
      auto& tri=m->tris[fid];

      //check neighbors
      // for(short i=0;i<3;i++)
      // {
      //   //get the other triangle
      //   auto& e=tri.edges[i];
      //   if(e.fid.size()>2){ std::cerr<<"! Error: the input mesh is non-manifold"<<std::endl; return false; }
      //   uint otherf = e.other(fid);
      //   if(otherf==UINT_MAX) continue; //no adj triangle via this edge (i.e. a hole edge)
      //   //options that can be apply
      //   if(fids.find(otherf)==fids.end()) //this facet is new
      //   {
      //     //check if we can add the face and edge edge
      //     edge_count+=2;
      //     vertex_count++;
      //   }
      //   else //this face has been added
      //   {
      //     if(neighbors[fid][i]!=otherf) //not a neighbor yet....
      //     {
      //       //check if we can add this face as the neighbor
      //       edge_count--;
      //       vertex_count--; //this is not the only case...
      //     }//end if not a neighbor yet
      //   }//end if this facet is new
      // }
    }

    return true;
  }

  // ----------------------------------------------------------------------------
  // Private types
  // ----------------------------------------------------------------------------
  typedef CGAL::Simple_cartesian<double>       Kernel;
  typedef Kernel::Point_3                      Point_3;
  typedef Kernel::Point_2                      Point_2;
  typedef CGAL::Surface_mesh<Kernel::Point_3>  SurfaceMesh;

  typedef boost::graph_traits<SurfaceMesh>::halfedge_descriptor  halfedge_descriptor;
  typedef boost::graph_traits<SurfaceMesh>::vertex_descriptor    vertex_descriptor;
  typedef boost::graph_traits<SurfaceMesh>::face_descriptor      face_descriptor;

  typedef SurfaceMesh::Property_map<vertex_descriptor, Point_2>   UV_pmap;
  namespace SMP = CGAL::Surface_mesh_parameterization;
  //typedef CGAL::Parameterization_SurfaceMesh_3<Polyhedron> SurfaceMesh;

  //converting masc model to CGAL mesh
  void m2m( masc::model * m_, SurfaceMesh& m)
  {
    assert(m_); //make sure m is at least not null

    //init
    typedef SurfaceMesh::Vertex_index VID;
    typedef SurfaceMesh::Face_index   FID;
    vector<VID> vids;
    vids.reserve(m_->v_size);

    // add the polyhedron vertices
    for( uint i=0; i<m_->v_size; i++ ){
      auto& pos=m_->vertices[i].p;
      VID vid=m.add_vertex( Point_3( pos[0], pos[1], pos[2] ) );
      vids.push_back(vid);
    }

    // add the polyhedron triangles
    for( uint i=0; i<m_->t_size; i++ ){
     auto& tri = m_->tris[i];
     m.add_face( vids[tri.v[0]], vids[tri.v[1]], vids[tri.v[2]] );
    }
  }

  //------------------------------------------------------------------
  //export the parameterized mesh to svg file
  //------------------------------------------------------------------
  void export_to_svg(SurfaceMesh & mesh, UV_pmap uv_map)
  {

    //create a svg file
    //convert the mesh to trianglesß
    char svg_filename[256];
    sprintf(svg_filename, "%s.svg", "cgal-param");

    //------------------------------------------------------------------
    //find dimension
    float min_x=FLT_MAX, min_y=FLT_MAX, max_x=-FLT_MAX, max_y=-FLT_MAX;
    //for (auto pHalfedge = mesh.vertex(); pHalfedge != mesh.halfedges_end(); pHalfedge++)
    //BOOST_FOREACH( vertex_descriptor vd, mesh.vertices())
    for(auto vd : mesh.vertices())
    {
        Point_2 uv=uv_map[vd];
        float x =  uv.x();
        float y =  uv.y();
        if(x<min_x) min_x=x;
        if(x>max_x) max_x=x;
        if(y<min_y) min_y=y;
        if(y>max_y) max_y=y;
    }
    float scale=1.0f;
    int width =(int)ceil(max_x-min_x);
    int height=(int)ceil(max_y-min_y);
    const int desired_width=800;
    if(width<desired_width)
    {
      scale = desired_width*1.0f/width;
      width=(int)ceil(scale*width);
      height=(int)ceil(scale*height);
      min_x*=scale;
      max_x*=scale;
      min_y*=scale;
      max_y*=scale;
    }

    int padding=(int)ceil(min(width,height)*1.0f/20);
    width+=padding;
    height+=padding;
    svg::Dimensions dimensions(width,height);
    svg::Document doc(svg_filename, svg::Layout(dimensions, svg::Layout::TopLeft));

    //------------------------------------------------------------------
    //draw the external boundary
    //for (auto f = mesh.facets_begin(); f != mesh.facets_end(); f++)
    for(auto fd : mesh.faces())
    {
        vector<Point_2> uvs;
        for(auto vd : vertices_around_face(mesh.halfedge(fd), mesh))
        {
          uvs.push_back(uv_map[vd]);
        }

        assert(uvs.size()==3);

        svg::Polygon poly(svg::Fill(svg::Color::Yellow), svg::Stroke(0.5, svg::Color::Black));
        poly << svg::Point(scale * uvs[0].x()+padding-min_x,scale * uvs[0].y()+padding-min_y);
        poly << svg::Point(scale * uvs[1].x()+padding-min_x,scale * uvs[1].y()+padding-min_y);
        poly << svg::Point(scale * uvs[2].x()+padding-min_x,scale * uvs[2].y()+padding-min_y);
        poly.endBoundary();
        doc << poly;
    }

    //------------------------------------------------------------------

    doc.save();
    cout << "- Saved " << svg_filename << endl;
  }

  bool CGAL_AMAP_Creator::create(masc::model * m, flattened_mesh& fm)
  {

    //create mesh
    SurfaceMesh mesh;

    // build a CGAL surface mesh from model *m
    m2m(m,mesh);

    //***************************************
    // Parameterization
    //***************************************
    UV_pmap uv_map = mesh.add_property_map<vertex_descriptor, Point_2>("h:uv").first;

    // Border parameterizer
    typedef SMP::Square_border_arc_length_parameterizer_3<SurfaceMesh> s_arc_bd;
    typedef SMP::Square_border_uniform_parameterizer_3<SurfaceMesh> s_uni_bd;
    typedef SMP::Circular_border_uniform_parameterizer_3<SurfaceMesh> c_uni_bd;
    typedef SMP::Circular_border_arc_length_parameterizer_3<SurfaceMesh> c_arc_bd;

    // Discrete Authalic Parameterization (square border)
    // with Eigen solver
    typedef SMP::Discrete_authalic_parameterizer_3<SurfaceMesh, s_arc_bd> authalic_s_arc_bd;
    typedef SMP::Discrete_authalic_parameterizer_3<SurfaceMesh, s_uni_bd> authalic_s_uni_bd;
    typedef SMP::Discrete_authalic_parameterizer_3<SurfaceMesh, c_arc_bd> authalic_c_arc_bd;
    typedef SMP::Discrete_authalic_parameterizer_3<SurfaceMesh, c_uni_bd> authalic_c_uni_bd;

    typedef SMP::Discrete_conformal_map_parameterizer_3<SurfaceMesh, s_arc_bd> conformal_s_arc_bd;
    typedef SMP::Discrete_conformal_map_parameterizer_3<SurfaceMesh, s_uni_bd> conformal_s_uni_bd;
    typedef SMP::Discrete_conformal_map_parameterizer_3<SurfaceMesh, c_arc_bd> conformal_c_arc_bd;
    typedef SMP::Discrete_conformal_map_parameterizer_3<SurfaceMesh, c_uni_bd> conformal_c_uni_bd;

    typedef SMP::Barycentric_mapping_parameterizer_3<SurfaceMesh, s_arc_bd> barycentric_s_arc_bd;
    typedef SMP::Barycentric_mapping_parameterizer_3<SurfaceMesh, s_uni_bd> barycentric_s_uni_bd;
    typedef SMP::Barycentric_mapping_parameterizer_3<SurfaceMesh, c_arc_bd> barycentric_c_arc_bd;
    typedef SMP::Barycentric_mapping_parameterizer_3<SurfaceMesh, c_uni_bd> barycentric_c_uni_bd;

    typedef SMP::Mean_value_coordinates_parameterizer_3<SurfaceMesh, s_arc_bd> mv_s_arc_bd;
    typedef SMP::Mean_value_coordinates_parameterizer_3<SurfaceMesh, s_uni_bd> mv_s_uni_bd;
    typedef SMP::Mean_value_coordinates_parameterizer_3<SurfaceMesh, c_arc_bd> mv_c_arc_bd;
    typedef SMP::Mean_value_coordinates_parameterizer_3<SurfaceMesh, c_uni_bd> mv_c_uni_bd;

    typedef SMP::LSCM_parameterizer_3<SurfaceMesh, s_arc_bd> lscm_s_arc_bd;
    typedef SMP::LSCM_parameterizer_3<SurfaceMesh, s_uni_bd> lscm_s_uni_bd;
    typedef SMP::LSCM_parameterizer_3<SurfaceMesh, c_arc_bd> lscm_c_arc_bd;
    typedef SMP::LSCM_parameterizer_3<SurfaceMesh, c_uni_bd> lscm_c_uni_bd;

    SMP::Error_code err = SMP::ERROR_EMPTY_MESH;

    // A halfedge on the border
    halfedge_descriptor bhd = CGAL::Polygon_mesh_processing::longest_border(mesh).first;

    if(param_method_==Tutte)
    {
      switch(bd_type_)
      {
        case Square_Arc_Length:   err = SMP::parameterize(mesh, barycentric_s_arc_bd(), bhd, uv_map); break;
        case Square_Uniform:      err = SMP::parameterize(mesh, barycentric_s_uni_bd(), bhd, uv_map); break;
        case Circular_Arc_Length: err = SMP::parameterize(mesh, barycentric_c_arc_bd(), bhd, uv_map); break;
        case Circular_Uniform:    err = SMP::parameterize(mesh, barycentric_c_uni_bd(), bhd, uv_map); break;
        default: break;
      }
    }
    else if(param_method_==Conformal_Map)
    {
      switch(bd_type_)
      {
        case Square_Arc_Length:   err = SMP::parameterize(mesh, conformal_s_arc_bd(), bhd, uv_map); break;
        case Square_Uniform:      err = SMP::parameterize(mesh, conformal_s_uni_bd(), bhd, uv_map); break;
        case Circular_Arc_Length: err = SMP::parameterize(mesh, conformal_c_arc_bd(), bhd, uv_map); break;
        case Circular_Uniform:    err = SMP::parameterize(mesh, conformal_c_uni_bd(), bhd, uv_map); break;
        default: break;
      }
    }
    else if(param_method_==Floater)
    {
      switch(bd_type_)
      {
        case Square_Arc_Length:   err = SMP::parameterize(mesh, mv_s_arc_bd(), bhd, uv_map); break;
        case Square_Uniform:      err = SMP::parameterize(mesh, mv_s_uni_bd(), bhd, uv_map); break;
        case Circular_Arc_Length: err = SMP::parameterize(mesh, mv_c_arc_bd(), bhd, uv_map); break;
        case Circular_Uniform:    err = SMP::parameterize(mesh, mv_c_uni_bd(), bhd, uv_map); break;
        default: break;
      }
    }
    else if(param_method_==Authalic)
    {
      switch(bd_type_)
      {
        case Square_Arc_Length:   err = SMP::parameterize(mesh, authalic_s_arc_bd(), bhd, uv_map); break;
        case Square_Uniform:      err = SMP::parameterize(mesh, authalic_s_uni_bd(), bhd, uv_map); break;
        case Circular_Arc_Length: err = SMP::parameterize(mesh, authalic_c_arc_bd(), bhd, uv_map); break;
        case Circular_Uniform:    err = SMP::parameterize(mesh, authalic_c_uni_bd(), bhd, uv_map); break;
        default: break;
      }
    }
    else if(param_method_==LeastSquares)
    {
      switch(bd_type_)
      {
        case Square_Arc_Length:   err = SMP::parameterize(mesh, lscm_s_arc_bd(), bhd, uv_map); break;
        case Square_Uniform:      err = SMP::parameterize(mesh, lscm_s_uni_bd(), bhd, uv_map); break;
        case Circular_Arc_Length: err = SMP::parameterize(mesh, lscm_c_arc_bd(), bhd, uv_map); break;
        case Circular_Uniform:    err = SMP::parameterize(mesh, lscm_c_uni_bd(), bhd, uv_map); break;
        default: break;
      }
    }

    switch(err) {
      case SMP::OK: // Success
          break;
      case SMP::ERROR_EMPTY_MESH: // Input mesh not supported
      case SMP::ERROR_NON_TRIANGULAR_MESH:
      case SMP::ERROR_NO_TOPOLOGICAL_DISC:
      case SMP::ERROR_BORDER_TOO_SHORT:
          std::cerr << "! Error: Onput mesh not supported: " << SMP::get_error_message(err) << std::endl;
          return false;
          break;
      default: // Error
          std::cerr << "! Error: " << SMP::get_error_message(err) << std::endl;
          return false;
          break;
    };

    //compute the mesh angles in parameterized domain
    uint fid=0;
    // for (auto f = mesh.facets_begin(); f != mesh.facets_end(); f++, fid++)
    // {
    for(auto fd : mesh.faces())
    {
        vector<Point_2> uvs;
        for(auto vd : vertices_around_face(mesh.halfedge(fd), mesh))
        {
          uvs.push_back(uv_map[vd]);
        }

        assert(uvs.size()==3);

        //cout<<"is triangle="<<f->is_triangle()<<endl;
        auto& ff = fm.get_triangle(fid);
        ff.cage[0].set(uvs[0].x(),uvs[0].y());
        ff.cage[1].set(uvs[1].x(),uvs[1].y());
        ff.cage[2].set(uvs[2].x(),uvs[2].y());
        fid++;
    }

    uint vid=0;
    for(auto vd : mesh.vertices())
    {
      auto& vv = fm.get_vertex(vid);
      Point_2 uv=uv_map[vd];
      vv.pos.set(uv.x(), uv.y());
      vid++;
    }

    //export to svg
    export_to_svg(mesh, uv_map);

    return true;
  }

  bool LP_AMAP_Creator::create(masc::model * m, flattened_mesh& fm)
  {
    return true;
  }

}//end namespace flattener


/* This file is part of PyMesh. Copyright (c) 2015 by Qingnan Zhou */
#include "WireProfile.h"
#include <Core/Exception.h>
#include <iostream>
#include <Eigen/Geometry>

using namespace PyMesh;

namespace WireProfileHelper {
    MatrixFr rotate_loop_2D(const MatrixFr& loop, const VectorF& dir) {
        const Float angle = atan2(-dir[0], dir[1]);
        Eigen::Rotation2D<Float> rotation(angle);
        return (rotation.toRotationMatrix() * loop.transpose()).transpose();
    }

    MatrixFr rotate_loop_3D(const MatrixFr& loop, const VectorF& dir) {
        const Vector3F X(1, 0, 0);
        const Vector3F Z(0, 0, 1);

        Eigen::Quaternion<Float> Q;
        Float dir_len = dir.norm();
        if (dir[2] < (-1.0 + 1e-6) * dir_len) {
            // Handle special case: dir ~= -Z
            // Eigen::Quaternion introduce large numeric error for such cases.
            Q = Eigen::Quaternion<Float>(
                    Eigen::AngleAxis<Float>(M_PI, Vector3F::UnitX()));
            Q = Eigen::Quaternion<Float>::FromTwoVectors(-Z, dir) * Q;
        } else {
            Q.setFromTwoVectors(Z, dir);
            if (fabs(dir[0]) > 1e-3 * dir_len && fabs(dir[1]) > 1e-3 * dir_len) {
                // Pre-rotate the profile so all wires has consistent profile
                // orientation.
                Q = Q * Eigen::Quaternion<Float>::FromTwoVectors(
                        X, Vector3F(dir[0], dir[1], 0.0));
            }
        }

        return (Q.toRotationMatrix() * loop.transpose()).transpose();
    }

    void apply_correction(const Vector3F& dir, MatrixFr& loop,
            const Vector3F& rel_correction, const Vector3F& abs_correction,
            const Float correction_cap, const Float spread_const) {
        static const Float tol = 1e-3;
        assert(loop.cols() == abs_correction.size());
        assert(loop.cols() == rel_correction.size());
        assert(correction_cap >= 0.0);

        VectorF centroid = 0.5 *
            (loop.colwise().minCoeff() + loop.colwise().maxCoeff());
        const Vector3F upper_cap = Vector3F::Ones() * correction_cap * 0.5;
        const Vector3F lower_cap =-Vector3F::Ones() * correction_cap * 0.5;
        Float dir_sq_len = dir.squaredNorm();

        const size_t loop_size = loop.rows();
        if (fabs(dir[2]) > tol) {
            for (size_t i=0; i<loop_size; i++) {
                Vector3F v = loop.row(i) - centroid.transpose();
                Float factor = -v[2] / dir[2];
                Vector3F p = v + dir * factor;
                assert(fabs(p[2]) < 1e-12);

                Vector3F sign(1, 1, 1);
                if (v[0] < 0.0) sign[0] = -1;
                if (v[1] < 0.0) sign[1] = -1;
                if (v[2] < 0.0) sign[2] = -1;

                Float ave_XY_abs_offset = 0.5 * (abs_correction[0] +
                        abs_correction[1]);
                Vector3F offset(0, 0, 0);
                offset = p.cwiseProduct(rel_correction) +
                    Vector3F::UnitZ() * v[2] * rel_correction[2] +
                    0.5 * Vector3F::UnitZ() * sign[2] * abs_correction[2] +
                    0.5 * p / p.norm() * ave_XY_abs_offset;
                    //sign.cwiseProduct(abs_correction * 0.5);

                Float dist_to_center = p.norm();
                Float spread = 0.5 * spread_const;
                if (dist_to_center < 2.0 && 2.0 > ave_XY_abs_offset) {
                    spread = 0.5 * spread_const *
                        dist_to_center / (2.0 - ave_XY_abs_offset);
                }
                offset[0] += sign[0] * spread;
                offset[1] += sign[1] * spread;
                offset = offset.cwiseMin(upper_cap);
                offset = offset.cwiseMax(lower_cap);

                p += offset;
                p = p - dir * (dir.dot(p) / dir_sq_len);
                loop.row(i) = (p+centroid).transpose();
            }
        } else {
            for (size_t i=0; i<loop_size; i++) {
                Vector3F v = loop.row(i) - centroid.transpose();
                Vector3F p(v[0], v[1], 0.0);
                Vector3F sign(1, 1, 1);
                if (v[0] < 0.0) sign[0] = -1;
                if (v[1] < 0.0) sign[1] = -1;
                if (v[2] < 0.0) sign[2] = -1;

                Float ave_XY_abs_offset = 0.5 * (abs_correction[0] +
                        abs_correction[1]);
                Vector3F offset(0, 0, 0);
                offset = v.cwiseProduct(rel_correction) +
                    0.5 * Vector3F::UnitZ() * sign[2] * abs_correction[2] +
                    0.5 * p / p.norm() * ave_XY_abs_offset;
                    //sign.cwiseProduct(abs_correction * 0.5);
                Float spread = 0.5 * spread_const;
                offset[0] += sign[0] * spread;
                offset[1] += sign[1] * spread;

                offset = offset.cwiseMin(upper_cap);
                offset = offset.cwiseMax(lower_cap);

                v += offset;
                v = v - dir * (dir.dot(v) / dir_sq_len);
                loop.row(i) = (v+centroid).transpose();
            }
        }
    }
}

using namespace WireProfileHelper;

WireProfile::Ptr WireProfile::create(const std::string& name) {
    if (name == "square") {
        MatrixFr loop(4,3);
        loop << -1, -1, 0,
                 1, -1, 0,
                 1,  1, 0,
                -1,  1, 0;
        Ptr r = Ptr(new WireProfile());
        r->initialize(loop);
        return r;
    } else if (name == "hexagon") {
        return WireProfile::create_isotropic(6);
    } else if (name == "octagon") {
        return WireProfile::create_isotropic(8);
    } else if (name == "triangle") {
        return WireProfile::create_isotropic(3);
    } else {
        std::stringstream err_msg;
        err_msg << "Unknown loop profile: " << name;
        throw NotImplementedError(err_msg.str());
    }
}

WireProfile::Ptr WireProfile::create_isotropic(size_t num_samples) {
    MatrixFr loop = MatrixFr::Zero(num_samples, 3);
    for (size_t i=0; i<num_samples; i++) {
        Float angle = 2 * M_PI * i / num_samples;
        loop.coeffRef(i, 0) = cos(angle);
        loop.coeffRef(i, 1) = sin(angle);
    }
    Ptr r = Ptr(new WireProfile());
    r->initialize(loop);
    return r;
}

WireProfile::Ptr WireProfile::create_2D() {
    MatrixFr loop(2, 2);
    loop << -1, 0, 1, 0;
    Ptr r = Ptr(new WireProfile());
    r->initialize(loop);
    return r;
}

void WireProfile::initialize(const MatrixFr& loop) {
    m_loop = loop;
    m_dim = m_loop.cols();
    if (m_dim == 2) {
        m_offset_dir = Vector2F(0, 1);
    } else if (m_dim == 3) {
        m_offset_dir = Vector3F(0, 0, 1);
    }

    VectorF center = 0.5 * (m_loop.colwise().minCoeff() +
            m_loop.colwise().maxCoeff());
    m_loop.rowwise() -= center.transpose();
    m_radius = m_loop.rowwise().norm().maxCoeff();
}

MatrixFr WireProfile::place(const VectorF& end_1, const VectorF& end_2,
        Float offset, Float thickness,
        const VectorF& rel_correction,
        const VectorF& abs_correction,
        Float correction_cap,
        Float spread_const) {
    VectorF dir = end_2 - end_1;
    MatrixFr loop = m_loop;

    loop *= 0.5 * thickness / m_radius;
    loop.rowwise() += m_offset_dir.transpose() * offset;

    if (m_dim == 2) {
        loop = rotate_loop_2D(loop, dir);
    } else if (m_dim == 3) {
        loop = rotate_loop_3D(loop, dir);
        if (m_correction_table) {
            m_correction_table->apply_correction(dir, loop);
        }
    } else {
        assert(false);
        std::stringstream err_msg;
        err_msg << "Unsupported dimention: " << m_dim;
        throw NotImplementedError(err_msg.str());
    }
    return loop.rowwise() + end_1.transpose();
}


#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <quadrotor_msgs/TRPYCommand.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>
#include <tf/transform_datatypes.h>
#include "snav/snapdragon_navigator.h"
#include <tf_conversions/tf_eigen.h>

class TRPYCmdToSnav : public nodelet::Nodelet
{
 public:
  void onInit(void);

 private:
  void trpy_cmd_callback(const quadrotor_msgs::TRPYCommand::ConstPtr &msg);
  void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);
  void imu_callback(const sensor_msgs::Imu::ConstPtr &pose);
  void trpy_cmd_to_qc_interface(const quadrotor_msgs::TRPYCommand::ConstPtr &msg);
  void motors_on();
  void motors_off();

  //controller state
  SnavCachedData *snav_cached_data_struct;

  bool odom_set_, imu_set_, trpy_cmd_set_;
  Eigen::Quaterniond odom_q_, imu_q_;

  ros::Subscriber trpy_cmd_sub_;
  ros::Subscriber odom_sub_;
  ros::Subscriber imu_sub_;

  int motor_status_;

  double trpy_cmd_timeout_;
  ros::Time last_trpy_cmd_time_;
  quadrotor_msgs::TRPYCommand last_trpy_cmd_;
};

void TRPYCmdToSnav::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
{
  if(!odom_set_)
    odom_set_ = true;

  odom_q_ = Eigen::Quaterniond(
      odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
      odom->pose.pose.orientation.y, odom->pose.pose.orientation.z);
    if(trpy_cmd_set_ &&
     ((ros::Time::now() - last_trpy_cmd_time_).toSec() >= trpy_cmd_timeout_))
  {
    ROS_INFO("trpy_cmd timeout. %f seconds since last command",
             (ros::Time::now() - last_trpy_cmd_time_).toSec());
    const auto last_trpy_cmd_ptr =
        boost::make_shared<quadrotor_msgs::TRPYCommand>(last_trpy_cmd_);

    trpy_cmd_callback(last_trpy_cmd_ptr);
  }
}

void TRPYCmdToSnav::imu_callback(const sensor_msgs::Imu::ConstPtr &pose)
{
  if(!imu_set_)
    imu_set_ = true;

  imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x,
                              pose->orientation.y, pose->orientation.z);

  if(trpy_cmd_set_ &&
     ((ros::Time::now() - last_trpy_cmd_time_).toSec() >= trpy_cmd_timeout_))
  {
    ROS_INFO("trpy_cmd timeout. %f seconds since last command",
             (ros::Time::now() - last_trpy_cmd_time_).toSec());
    const auto last_trpy_cmd_ptr =
        boost::make_shared<quadrotor_msgs::TRPYCommand>(last_trpy_cmd_);

    trpy_cmd_callback(last_trpy_cmd_ptr);
  }
}

void TRPYCmdToSnav::motors_on()
{
  int ret = sn_spin_props();
  if(ret == -1)
    ROS_ERROR("Not able to send spinning command");
  else
    motor_status_ = 1;

  if(snav_cached_data_struct->general_status.props_state == SN_PROPS_STATE_SPINNING)
    ROS_INFO("All the propellers are spinnig");
  else{
    ROS_ERROR("All the propellers are not spinnig");
    motor_status_ = 0;
  }
}

void TRPYCmdToSnav::motors_off()
{
	do
  {
		//call the update 0 success
		int res_update = sn_update_data();
	  //stop the props 0 success
		int ret = sn_stop_props();
		if(ret == -1)
		  ROS_ERROR("Not able to send switch off propellers");

		//send minimum thrust and identity attitude
		sn_send_thrust_att_ang_vel_command (0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
		motor_status_ = 0;
	}
	while(snav_cached_data_struct->general_status.props_state == SN_PROPS_STATE_SPINNING);

	//check the propellers status
	if(snav_cached_data_struct->general_status.props_state == SN_PROPS_STATE_SPINNING)
	  ROS_ERROR("All the propellers are still spinnig");
	else
	  ROS_INFO("All the propellers are now off");
}

void TRPYCmdToSnav::trpy_cmd_to_qc_interface(
  const quadrotor_msgs::TRPYCommand::ConstPtr &msg)
{
  const tf::Quaternion q_rot = tf::createQuaternionFromRPY(msg->roll, msg->pitch, msg->yaw);
  Eigen::Quaterniond q_des;
  tf::quaternionTFToEigen(q_rot, q_des);

  const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);

  // convert to tf::Quaternion
  tf::Quaternion imu_tf =
    tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w());
  tf::Quaternion odom_tf =
    tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w());

  double throttle = msg->thrust;

  //convert throttle in grams
  throttle = throttle*1000/9.81;

  int res_update = sn_update_data();
  if(res_update ==  -1)
  {
    ROS_ERROR("Flight software non functional");
    return;
  }

  int r = sn_send_thrust_att_ang_vel_command(throttle, q_des.w(), q_des.x(), q_des.y(), q_des.z(), ang_vel(0), ang_vel(1), ang_vel(2));
  if(r == -1)
    ROS_ERROR("Control command not send");
}

void TRPYCmdToSnav::trpy_cmd_callback(
    const quadrotor_msgs::TRPYCommand::ConstPtr &msg)
{
  if(!trpy_cmd_set_)
    trpy_cmd_set_ = true;

  //switch on motors
  if(msg->aux.enable_motors && !motor_status_)
  	motors_on();
  else if(!msg->aux.enable_motors)
  	motors_off();

  trpy_cmd_to_qc_interface(msg);

  // save last trpy_cmd
  last_trpy_cmd_ = *msg;
  last_trpy_cmd_time_ = ros::Time::now();
}

void TRPYCmdToSnav::onInit(void)
{
  ros::NodeHandle priv_nh(getPrivateNodeHandle());

  // get param for trpy command timeout duration
  priv_nh.param("trpy_cmd_timeout", trpy_cmd_timeout_, 0.25);

  odom_set_ = false;
  imu_set_ = false;
  trpy_cmd_set_ = false;
  motor_status_ = 0;
	snav_cached_data_struct = NULL;
  if (sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_cached_data_struct) != 0)
  {
    ROS_ERROR("\nFailed to get flight data pointer!\n");
    return;
  }

  trpy_cmd_sub_ =
      priv_nh.subscribe("trpy_cmd", 10, &TRPYCmdToSnav::trpy_cmd_callback, this,
                        ros::TransportHints().tcpNoDelay());

  odom_sub_ = priv_nh.subscribe("odom", 10, &TRPYCmdToSnav::odom_callback,
                                this, ros::TransportHints().tcpNoDelay());

  imu_sub_ = priv_nh.subscribe("imu", 10, &TRPYCmdToSnav::imu_callback, this,
                               ros::TransportHints().tcpNoDelay());
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(TRPYCmdToSnav, nodelet::Nodelet);