#define BOOST_TEST_MODULE "test_global_pair_lennard_jones_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/NaivePairCalculation.hpp>
#include <mjolnir/forcefield/global/GlobalPairLennardJonesInteraction.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(GlobalPairLennardJonesInteraction_numeric_limits)
{
    namespace test = mjolnir::test;
    mjolnir::LoggerManager::set_default_logger("test_global_pair_lennard_jones_interaction.log");
    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type        = traits_type::real_type;
    using coordinate_type  = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using topology_type    = mjolnir::Topology;
    using potential_type   = mjolnir::LennardJonesPotential<real_type>;
    using parameter_list_type = mjolnir::LorentzBerthelotRule<traits_type, potential_type>;
    using parameter_type   = typename parameter_list_type::parameter_type;
    using partition_type   = mjolnir::NaivePairCalculation<traits_type, potential_type>;
    using interaction_type = mjolnir::GlobalPairInteraction<traits_type, potential_type>;

    potential_type potential(2.5);

    parameter_list_type parameter_list(
        std::vector<std::pair<std::size_t, parameter_type>>{
            {0, {/* sigma = */ 1.0, /* epsilon = */1.2}},
            {1, {/* sigma = */ 1.0, /* epsilon = */1.2}}
        }, {}, typename parameter_list_type::ignore_molecule_type("Nothing"),
               typename parameter_list_type::ignore_group_type   ({})
        );

    interaction_type interaction(std::move(potential),
        mjolnir::ParameterList<traits_type, potential_type>(
            mjolnir::make_unique<parameter_list_type>(std::move(parameter_list))),
        mjolnir::SpatialPartition<traits_type, potential_type>(
            mjolnir::make_unique<partition_type>()));

    system_type sys(2, boundary_type{});
    test::clear_everything(sys);

    sys.position(0) = coordinate_type(0.0, 0.0, 0.0);
    sys.position(1) = coordinate_type(0.5, 0.0, 0.0);

    topology_type topol(2);
    topol.construct_molecules();

    interaction.initialize(sys, topol);
    // check if the pair of particles are within the cutoff range.
    BOOST_REQUIRE(interaction.calc_energy(sys) != 0.0);

    std::mt19937 rng(123456789);
    std::normal_distribution<real_type> gauss(0.0, 1.0);

    const real_type r_min = 0.5;
    const real_type r_max = 2.5; // sigma == 1
    const real_type dr    = 1e-3;
    const int max_count = (r_max - r_min) / dr;

    for(int i = 0; i < max_count-1; ++i)
    {
        const real_type dist = r_min + i * dr;

        sys.position(0) = coordinate_type(0,0,0);
        sys.position(1) = coordinate_type(dist,0,0);
        sys.force(0)    = coordinate_type(0,0,0);
        sys.force(1)    = coordinate_type(0,0,0);

        test::apply_random_perturbation(sys, rng, 0.1);
        test::apply_random_rotation(sys, rng);

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;

        test::check_force(sys, interaction, tol, dr);
        test::check_virial(sys, interaction, tol);
        test::check_force_and_virial(sys, interaction, tol);
        test::check_force_and_energy(sys, interaction, tol);
        test::check_force_energy_virial(sys, interaction, tol);
    }
}


#include <ros/ros.h>
#include <Eigen/Dense>
#include "LocalBA.h"
#include "iba_helper.h"
#include "Viewer.h"
#include "tracer.h"

// helper function
template <typename T>
void InitPOD(T& t) {
  memset(&t, 0, sizeof(t));
}

inline bool cmp_by_class_id(const cv::KeyPoint& lhs, const cv::KeyPoint& rhs) {
    return lhs.class_id < rhs.class_id;
}

bool create_iba_frame(const std::vector<cv::KeyPoint>& kps_l,
                      const std::vector<cv::KeyPoint>& kps_r,
                      const std::vector<XP::ImuData>& imu_samples,
                      const double rig_time,
                      IBA::CurrentFrame *ptrCF,
                      IBA::KeyFrame *ptrKF) {
    ROS_ASSERT(std::is_sorted(kps_l.begin(), kps_l.end(), cmp_by_class_id));
    ROS_ASSERT(std::is_sorted(kps_r.begin(), kps_r.end(), cmp_by_class_id));
    ROS_ASSERT(std::includes(kps_l.begin(), kps_l.end(), kps_r.begin(), kps_r.end(), cmp_by_class_id));

    // IBA will handle *unknown* initial depth values
    IBA::Depth kUnknownDepth;
    kUnknownDepth.d = 0.0f;
    kUnknownDepth.s2 = 0.0f;
    static int last_added_point_id = -1;
    static int iba_iFrm = 0;
    auto kp_it_l = kps_l.cbegin(), kp_it_r = kps_r.cbegin();

    IBA::CurrentFrame& CF = *ptrCF;
    IBA::KeyFrame& KF = *ptrKF;

    CF.iFrm = iba_iFrm;
    InitPOD(CF.C); // needed to ensure the dumped frame deterministic even for unused field
    CF.C.C.R[0][0] = CF.C.v[0] = CF.C.ba[0] = CF.C.bw[0] = FLT_MAX;
    // MapPointMeasurement, process in ascending class id, left camera to right
    // Note the right keypoints is a subset of the left ones
    IBA::MapPointMeasurement mp_mea;
    InitPOD(mp_mea);
    mp_mea.x.S[0][0] = mp_mea.x.S[1][1] = 1.f;
    mp_mea.x.S[0][1] = mp_mea.x.S[1][0] = 0.f;
    for (; kp_it_l != kps_l.cend() && kp_it_l->class_id <= last_added_point_id; ++kp_it_l) {
      mp_mea.idx = kp_it_l->class_id;
      mp_mea.x.x[0] = kp_it_l->pt.x;
      mp_mea.x.x[1] = kp_it_l->pt.y;
      mp_mea.right = false;
      CF.zs.push_back(mp_mea);
      if (kp_it_r != kps_r.cend() && kp_it_r->class_id == kp_it_l->class_id) {
        mp_mea.x.x[0] = kp_it_r->pt.x;
        mp_mea.x.x[1] = kp_it_r->pt.y;
        mp_mea.right = true;
        CF.zs.push_back(mp_mea);
        ++kp_it_r;
      }
    }
    std::transform(imu_samples.begin(), imu_samples.end(), std::back_inserter(CF.us), XP::to_iba_imu);
    CF.t = rig_time;
    CF.d = kUnknownDepth;
    bool need_new_kf = std::distance(kp_it_l, kps_l.end()) >= 20 || CF.zs.size() < 20;
    if (std::distance(kp_it_l, kps_l.end()) == 0)
      need_new_kf = false;
    if (!need_new_kf)
        KF.iFrm = -1;
//    else
//      ROS_INFO_STREAM("new keyframe " << CF.iFrm);

    if (!need_new_kf) {
      KF.iFrm = -1;
      //  to make it deterministic
      InitPOD(KF.C);
      InitPOD(KF.d);
    } else {
      KF.iFrm = CF.iFrm;
      KF.C = CF.C.C;
      // MapPointMeasurement, duplication of CF
      KF.zs = CF.zs;
      // MapPoint
      for(; kp_it_l != kps_l.cend(); ++kp_it_l) {
        IBA::MapPoint mp;
        InitPOD(mp.X);
        mp.X.idx = kp_it_l->class_id;
        mp.X.X[0] = FLT_MAX;
        mp_mea.iFrm = iba_iFrm;
        mp_mea.x.x[0] = kp_it_l->pt.x;
        mp_mea.x.x[1] = kp_it_l->pt.y;
        mp_mea.right = false;
        mp.zs.push_back(mp_mea);

        if (kp_it_r != kps_r.cend() && kp_it_r->class_id == kp_it_l->class_id) {
          mp_mea.x.x[0] = kp_it_r->pt.x;
          mp_mea.x.x[1] = kp_it_r->pt.y;
          mp_mea.right = true;
          mp.zs.push_back(mp_mea);
          kp_it_r++;
        } else {
//          ROS_WARN_STREAM("add new feature point " << kp_it_l->class_id << " only found in left image");
        }
        KF.Xs.push_back(mp);
      }
      last_added_point_id = std::max(KF.Xs.back().X.idx, last_added_point_id);
      KF.d = kUnknownDepth;
    }
    ++iba_iFrm;
    return true;
}

LocalBA::LocalBA(const SPtr<XP::DuoCalibParam>& pDuoCalibParam)
    :mpDuoCalibParam(pDuoCalibParam)
{
    mSolver.Create(XP::to_iba_calibration(*mpDuoCalibParam),
                   IBA_SERIAL_LBA | IBA_SERIAL_GBA,
                   IBA_VERBOSE_NONE,
                   IBA_DEBUG_NONE,
                   IBA_HISTORY_LBA | IBA_HISTORY_GBA);

    mSolver.SetCallbackLBA([&](const int iFrm, const float ts) {
        ScopedTrace st("ShowPose");
        IBA::SlidingWindow sliding_window;
        mSolver.GetSlidingWindow(&sliding_window);
        const IBA::CameraIMUState& X = sliding_window.CsLF.back();
        const IBA::CameraPose& C = X.C;
        Eigen::Matrix4f W_vio_T_S = Eigen::Matrix4f::Identity();  // W_vio_T_S
        for (int i = 0; i < 3; ++i) {
          W_vio_T_S(i, 3) = C.p[i];
          for (int j = 0; j < 3; ++j) {
            W_vio_T_S(i, j) = C.R[j][i];  // C.R is actually R_SW
          }
        }

        Eigen::Vector3f speed;
        for(int i = 0; i < 3; ++i)
            speed(i) = X.v[i];

        Viewer::Instance().PubOdometry(W_vio_T_S, speed);
#ifdef DRAW_KEYFRAME_POSE
        std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> vec_p;
        for(int i = 0, n = sliding_window.CsKF.size(); i < n - 1; ++i) {
            Eigen::Vector3f p;
            p(0) = sliding_window.CsKF[i].p[0];
            p(1) = sliding_window.CsKF[i].p[1];
            p(2) = sliding_window.CsKF[i].p[2];
            vec_p.emplace_back(p);
        }

        if(!vec_p.empty())
            Viewer::Instance().PubKeyFramePose(vec_p);
#endif
    });
    mSolver.Start();
    mtLocalBA = std::thread(&LocalBA::Run, this);
}

LocalBA::~LocalBA() {
    mSolver.Stop();
    mSolver.Destroy();
}

void LocalBA::Run() {
    while(1) {
        std::vector<std::pair<IBA::CurrentFrame, IBA::KeyFrame>> measurements;
        std::unique_lock<std::mutex> lock(mMBuffer);
        mCvBuffer.wait(lock, [&] {
            return (measurements = GetMeasurements()).size() != 0;
        });
        lock.unlock();

        for(auto& meas : measurements) {
            ScopedTrace st("BA");
            auto& CF = meas.first;
            auto& KF = meas.second;
            mSolver.PushCurrentFrame(CF, KF.iFrm == -1 ? nullptr : &KF);
        }
    }
}

void LocalBA::PushCurrentFrame(const SPtr<Frame>& curr_frame, float timestamp,
                      const std::vector<XP::ImuData>& imu_meas) {
    std::vector<cv::KeyPoint> kps_l = curr_frame->mvKeys, kps_r = curr_frame->mvKeysRight;
    IBA::CurrentFrame CF;
    IBA::KeyFrame KF;

    std::sort(kps_l.begin(), kps_l.end(), cmp_by_class_id);
    std::sort(kps_r.begin(), kps_r.end(), cmp_by_class_id);

    create_iba_frame(kps_l, kps_r, imu_meas, timestamp, &CF, &KF);

    mMBuffer.lock();
    mqBuffer.emplace(CF, KF);
    mMBuffer.unlock();
    mCvBuffer.notify_one();
}

std::vector<std::pair<IBA::CurrentFrame, IBA::KeyFrame>> LocalBA::GetMeasurements() {
    std::vector<std::pair<IBA::CurrentFrame, IBA::KeyFrame>> measurements;
    while(!mqBuffer.empty()) {
        measurements.emplace_back(mqBuffer.front());
        mqBuffer.pop();
    }
    return measurements;
}


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 * @file   GaussianConditional.cpp
 * @brief  Conditional Gaussian Base class
 * @author Christian Potthast, Frank Dellaert
 */

#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>

#include <boost/format.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif

#include <functional>
#include <list>
#include <string>

using namespace std;

namespace gtsam {

  /* ************************************************************************* */
  GaussianConditional::GaussianConditional(
    Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) :
  BaseFactor(key, R, d, sigmas), BaseConditional(1) {}

  /* ************************************************************************* */
  GaussianConditional::GaussianConditional(
    Key key, const Vector& d, const Matrix& R,
    Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
  BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}

  /* ************************************************************************* */
  GaussianConditional::GaussianConditional(
    Key key, const Vector& d, const Matrix& R,
    Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
  BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}

  /* ************************************************************************* */
  void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
    cout << s << "  Conditional density ";
    for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
      cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
    }
    cout << endl;
    cout << formatMatrixIndented("  R = ", get_R()) << endl;
    for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
      cout << formatMatrixIndented((boost::format("  S[%1%] = ")%(formatter(*it))).str(), getA(it))
        << endl;
    }
    cout << formatMatrixIndented("  d = ", getb(), true) << "\n";
    if (model_)
      model_->print("  Noise model: ");
    else
      cout << "  No noise model" << endl;
  }

  /* ************************************************************************* */
  bool GaussianConditional::equals(const GaussianFactor& f, double tol) const {
    if (const GaussianConditional* c = dynamic_cast<const GaussianConditional*>(&f)) {
      // check if the size of the parents_ map is the same
      if (parents().size() != c->parents().size())
        return false;

      // check if R_ and d_ are linear independent
      for (DenseIndex i = 0; i < Ab_.rows(); i++) {
        list<Vector> rows1, rows2;
        rows1.push_back(Vector(get_R().row(i)));
        rows2.push_back(Vector(c->get_R().row(i)));

        // check if the matrices are the same
        // iterate over the parents_ map
        for (const_iterator it = beginParents(); it != endParents(); ++it) {
          const_iterator it2 = c->beginParents() + (it - beginParents());
          if (*it != *(it2))
            return false;
          rows1.push_back(row(getA(it), i));
          rows2.push_back(row(c->getA(it2), i));
        }

        Vector row1 = concatVectors(rows1);
        Vector row2 = concatVectors(rows2);
        if (!linear_dependent(row1, row2, tol))
          return false;
      }

      // check if sigmas are equal
      if ((model_ && !c->model_) || (!model_ && c->model_)
        || (model_ && c->model_ && !model_->equals(*c->model_, tol)))
        return false;

      return true;
    } else {
      return false;
    }
  }

  /* ************************************************************************* */
  VectorValues GaussianConditional::solve(const VectorValues& x) const {
    // Concatenate all vector values that correspond to parent variables
    const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));

    // Update right-hand-side
    const Vector rhs = get_d() - get_S() * xS;

    // Solve matrix
    const Vector solution = get_R().triangularView<Eigen::Upper>().solve(rhs);

    // Check for indeterminant solution
    if (solution.hasNaN()) {
      throw IndeterminantLinearSystemException(keys().front());
    }

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }

  /* ************************************************************************* */
  VectorValues GaussianConditional::solveOtherRHS(
    const VectorValues& parents, const VectorValues& rhs) const {
    // Concatenate all vector values that correspond to parent variables
    Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));

    // Instead of updating getb(), update the right-hand-side from the given rhs
    const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
    xS = rhsR - get_S() * xS;

    // Solve Matrix
    Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);

    // Scale by sigmas
    if (model_)
      soln.array() *= model_->sigmas().array();

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }

  /* ************************************************************************* */
  void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
    Vector frontalVec = gy.vector(FastVector<Key>(beginFrontals(), endFrontals()));
    frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));

    // Check for indeterminant solution
    if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());

    for (const_iterator it = beginParents(); it!= endParents(); it++)
      gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec;

    // Scale by sigmas
    if (model_)
      frontalVec.array() *= model_->sigmas().array();

    // Write frontal solution into a VectorValues
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal));
      vectorPosition += getDim(frontal);
    }
  }

  /* ************************************************************************* */
  void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array();
      vectorPosition += getDim(frontal);
    }
  }

}  // namespace gtsam


#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <string>
#include <fstream>
#include <stdlib.h>
#include <vector>
#include <sstream>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <boost/format.hpp>
#include <boost/thread/mutex.hpp>
#include <queue>
#include <stack>
#include <TriangleBoxIntersection.h>
#include <stdint.h>

enum cell_state {non_initialized=0, empty=1, occupied=2, outlet=3, edge=4};

struct Point{
    float x; float y; float z;
    Point(){}
    Point(float x, float y, float z){
        this->x = x; this->y =y; this->z=z;
    }
};
struct Triangle{
    Point p1; Point p2; Point p3;
    Triangle(){}
    Triangle(Point p1, Point p2, Point p3){
        this->p1=p1; this->p2=p2; this->p3=p3;
    }
    Point& operator[](int i){
        if(i==0)
            return p1;
        else if (i==1)
            return p2;
        else if(i==2)
            return p3;
        else{
            std::cout<<"Indexing error when accessing the points in triangle! Index must be >= 2";
            return p1;
        }
    }
};

//dimensions of the enviroment [m]
float env_min_x;
float env_min_y;
float env_min_z;
float env_max_x;
float env_max_y;
float env_max_z;
float roundFactor;
//length of the sides of the cell [m]
float cell_size;
float floor_height;

std::vector<std::vector<std::vector<int> > > env;

bool compare_cell(int x, int y, int z, cell_state value){
    if(x<0 || x>=env.size() ||
        y<0 || y>=env[0].size() ||
        z<0 || z>=env[0][0].size()){
            return false;
    }
    else{
        return env[x][y][z]==value;
    }
}

void changeWorldFile(std::string filename){
    std::ifstream input(filename);
    std::stringstream ss;
    std::string line;
    while(getline(input, line)){
        if(line.substr(0,8)=="floorMap"){
            //ignore the floorMap bit, we are replacing it entirely
            while(getline(input, line) && line!=")"){}

            ss<<
            "floorMap                     # load an environment bitmap\n"<<
            "(\n"<<
                "  name \"SimulatedMap\"\n"<<
                "  bitmap \"../../occupancy.pgm\"\n"<<
                "  size ["<<(env_max_x-env_min_x)<<" "<<(env_max_y-env_min_y)<<" "<<(env_max_z-env_min_z) <<"]           #m \n"<<
                "  pose ["<<(env_max_x-env_min_x)/2+env_min_x<<" "<<(env_max_y-env_min_y)/2+env_min_y<<" "<<floor_height<<" 0]    #Coordinates (m) of the Center of the image_map\n"<<
            ")\n";
        }
        else{
            ss<<line<<"\n";
        }
    }
    input.close();
    std::ofstream out(filename);
    out<<ss.rdbuf();
    out.close();
}

void printMap(std::string filename, int scale){
    std::ofstream outfile(filename.c_str());
    outfile << "P2\n"
            << scale *  env[0].size() << " " << scale * env.size() << "\n" <<"1\n";
    //things are repeated to scale them up (the image is too small!)

    int height = (floor_height-env_min_z)/cell_size; //a xy slice of the 3D environment is used as a geometric map for navigation

    for (int row = env.size()-1; row >= 0; row--)
    {
        for (int j = 0; j < scale; j++)
        {
            for (int col = 0; col <env[0].size() ; col++)
            {
                for (int i = 0; i < scale; i++)
                {
                    outfile << (env[row][col][height] == cell_state::empty? 1 : 0) << " ";
                }
            }
            outfile << "\n";
        }
    }
    outfile.close();

}

void printEnv(std::string filename, int scale)
{
    std::ofstream outfile(filename.c_str());

    outfile <<  "#env_min(m) " << env_min_x << " " << env_min_y << " " << env_min_z << "\n";
    outfile <<  "#env_max(m) " << env_max_x << " " << env_max_y << " " << env_max_z << "\n";
    outfile <<  "#num_cells " << env[0].size() << " " << env.size() << " " << env[0][0].size() << "\n";
    outfile <<  "#cell_size(m) " << cell_size << "\n";
    //things are repeated to scale them up (the image is too small!)
    for (int height = 0; height < env[0][0].size(); height++)
    {
        for (int col = 0; col <env[0].size(); col++)
        {
            for (int j = 0; j < scale; j++)
            {
                for (int row = 0; row <env.size(); row++)
                {
                    for (int i = 0; i < scale; i++)
                    {
                        outfile << (env[row][col][height]==cell_state::empty? 0 :
                                (env[row][col][height]==cell_state::outlet? 2 :
                                1))
                                << " ";
                    }
                }
                outfile << "\n";
            }
        }
        outfile << ";\n";
    }
    outfile.close();
}

void printWind(std::vector<double> U,
                std::vector<double> V,
                std::vector<double> W, std::string filename){

    std::ofstream fileU(boost::str(boost::format("%s_U") % filename).c_str());
    std::ofstream fileV(boost::str(boost::format("%s_V") % filename).c_str());
    std::ofstream fileW(boost::str(boost::format("%s_W") % filename).c_str());

    //this code is a header to let the filament_simulator know the file is in binary
    int code=999;

    fileU.write((char*) &code, sizeof(int));
    fileV.write((char*) &code, sizeof(int));
    fileW.write((char*) &code, sizeof(int));

    fileU.write((char*) U.data(), sizeof(double) * U.size());
    fileV.write((char*) V.data(), sizeof(double) * V.size());
    fileW.write((char*) W.data(), sizeof(double) * W.size());

    fileU.close();
    fileV.close();
    fileW.close();
}

void printYaml(std::string output){
    std::ofstream yaml(boost::str(boost::format("%s/occupancy.yaml") % output.c_str()));
    yaml << "image: occupancy.pgm\n"
        << "resolution: " << cell_size/10
        << "\norigin: [" << env_min_x << ", " << env_min_y << ", " << 0 << "]\n"
        << "occupied_thresh: 0.9\n"
        << "free_thresh: 0.1\n"
        << "negate: 0";
    yaml.close();
}

float min_val(float x, float y, float z) {

    float min =x;
    if (y < min)
        min=y;
    if(z < min)
        min=z;

    return min;
}
float max_val(float x, float y, float z) {

    float max= x;
    if (y > max)
        max=y;
    if(z > max)
        max=z;

    return max;
}
bool eq(float x, float y){
    return std::abs(x-y)<0.01;
}

std::vector<Eigen::Vector3d> cubePoints(const Eigen::Vector3d &query_point){
    std::vector<Eigen::Vector3d> points;
    points.push_back(query_point);
    points.push_back(Eigen::Vector3d(query_point.x()-cell_size/2,
                                            query_point.y()-cell_size/2,
                                            query_point.z()-cell_size/2));
    points.push_back(Eigen::Vector3d(query_point.x()-cell_size/2,
                                            query_point.y()-cell_size/2,
                                            query_point.z()+cell_size/2));
    points.push_back(Eigen::Vector3d(query_point.x()-cell_size/2,
                                            query_point.y()+cell_size/2,
                                            query_point.z()-cell_size/2));
    points.push_back(Eigen::Vector3d(query_point.x()-cell_size/2,
                                            query_point.y()+cell_size/2,
                                            query_point.z()+cell_size/2));
    points.push_back(Eigen::Vector3d(query_point.x()+cell_size/2,
                                            query_point.y()-cell_size/2,
                                            query_point.z()-cell_size/2));
    points.push_back(Eigen::Vector3d(query_point.x()+cell_size/2,
                                            query_point.y()-cell_size/2,
                                            query_point.z()+cell_size/2));
    points.push_back(Eigen::Vector3d(query_point.x()+cell_size/2,
                                            query_point.y()+cell_size/2,
                                            query_point.z()-cell_size/2));
    points.push_back(Eigen::Vector3d(query_point.x()+cell_size/2,
                                            query_point.y()+cell_size/2,
                                            query_point.z()+cell_size/2));
    return points;
}

bool pointInTriangle(const Eigen::Vector3d& query_point,
                     const Eigen::Vector3d& triangle_vertex_0,
                     const Eigen::Vector3d& triangle_vertex_1,
                     const Eigen::Vector3d& triangle_vertex_2)
{
    // u=P2−P1
    Eigen::Vector3d u = triangle_vertex_1 - triangle_vertex_0;
    // v=P3−P1
    Eigen::Vector3d v = triangle_vertex_2 - triangle_vertex_0;
    // n=u×v
    Eigen::Vector3d n = u.cross(v);
    bool anyProyectionInTriangle=false;
    std::vector<Eigen::Vector3d> cube= cubePoints(query_point);
    for(const Eigen::Vector3d &vec : cube){
        // w=P−P1
        Eigen::Vector3d w = vec - triangle_vertex_0;
        // Barycentric coordinates of the projection P′of P onto T:
        // γ=[(u×w)⋅n]/n²
        float gamma = u.cross(w).dot(n) / n.dot(n);
        // β=[(w×v)⋅n]/n²
        float beta = w.cross(v).dot(n) / n.dot(n);
        float alpha = 1 - gamma - beta;
        // The point P′ lies inside T if:
        bool proyectionInTriangle= ((0 <= alpha) && (alpha <= 1) &&
                (0 <= beta)  && (beta  <= 1) &&
                (0 <= gamma) && (gamma <= 1));
        anyProyectionInTriangle=anyProyectionInTriangle||proyectionInTriangle;
    }

    n.normalize();

    //we consider that the triangle goes through the cell if the proyection of the center
    //is inside the triangle AND the plane of the triangle intersects the cube of the cell

    return anyProyectionInTriangle;
}

bool parallel (const Point &vec){
    return (eq(vec.y,0)
                &&eq(vec.z,0))||
           (eq(vec.x,0)
                &&eq(vec.z,0))||
           (eq(vec.x,0)
                &&eq(vec.y,0));
}

void occupy(std::vector<Triangle> &triangles,
const std::vector<Point> &normals,
            cell_state value_to_write){

    std::cout<<"Processing the mesh...\n0%\n";
    int numberOfProcessedTriangles=0; //for logging, doesn't actually do anything
    boost::mutex mtx;
    //Let's occupy the enviroment!
    #pragma omp parallel for
    for(int i= 0;i<triangles.size();i++){
        //We try to find all the cells that some triangle goes through
        int x1 = roundf((triangles[i].p1.x-env_min_x)*(roundFactor))/(cell_size*(roundFactor));
        int y1 = roundf((triangles[i].p1.y-env_min_y)*(roundFactor))/(cell_size*(roundFactor));
        int z1 = roundf((triangles[i].p1.z-env_min_z)*(roundFactor))/(cell_size*(roundFactor));
        int x2 = roundf((triangles[i].p2.x-env_min_x)*(roundFactor))/(cell_size*(roundFactor));
        int y2 = roundf((triangles[i].p2.y-env_min_y)*(roundFactor))/(cell_size*(roundFactor));
        int z2 = roundf((triangles[i].p2.z-env_min_z)*(roundFactor))/(cell_size*(roundFactor));
        int x3 = roundf((triangles[i].p3.x-env_min_x)*(roundFactor))/(cell_size*(roundFactor));
        int y3 = roundf((triangles[i].p3.y-env_min_y)*(roundFactor))/(cell_size*(roundFactor));
        int z3 = roundf((triangles[i].p3.z-env_min_z)*(roundFactor))/(cell_size*(roundFactor));

        int min_x = min_val(x1,x2,x3);
        int min_y = min_val(y1,y2,y3);
        int min_z = min_val(z1,z2,z3);

        int max_x = max_val(x1,x2,x3);
        int max_y = max_val(y1,y2,y3);
        int max_z = max_val(z1,z2,z3);

        //is the triangle right at the boundary between two cells (in any axis)?
        bool xLimit = eq(std::fmod(max_val(triangles[i][0].x,triangles[i][1].x,triangles[i][2].x)-env_min_x, cell_size),0)
            ||eq(std::fmod(max_val(triangles[i][0].x,triangles[i][1].x,triangles[i][2].x)-env_min_x, cell_size),cell_size);

        bool yLimit = eq(std::fmod(max_val(triangles[i][0].y,triangles[i][1].y,triangles[i][2].y)-env_min_y, cell_size),0)
            ||eq(std::fmod(max_val(triangles[i][0].y,triangles[i][1].y,triangles[i][2].y)-env_min_y, cell_size),cell_size);

        bool zLimit = eq(std::fmod(max_val(triangles[i][0].z,triangles[i][1].z,triangles[i][2].z)-env_min_z, cell_size),0)
            ||eq(std::fmod(max_val(triangles[i][0].z,triangles[i][1].z,triangles[i][2].z)-env_min_z, cell_size),cell_size);

        bool isParallel =parallel(normals[i]);
        for (int row = min_x; row <= max_x && row < env[0].size(); row++)
        {
            for (int col = min_y; col <= max_y && col < env.size(); col++)
            {
                for (int height = min_z; height <= max_z && height < env[0][0].size(); height++)
                {
                    //check if the triangle goes through this cell
                    //special case for triangles that are parallel to the coordinate axes because the discretization can cause
                    //problems if they fall right on the boundary of two cells
                    if (
                        (isParallel && pointInTriangle(Eigen::Vector3d(row * cell_size + env_min_x+cell_size/2,
                                                        col * cell_size + env_min_y+cell_size/2,
                                                        height * cell_size + env_min_z+cell_size/2),
                                        Eigen::Vector3d(triangles[i][0].x, triangles[i][0].y, triangles[i][0].z),
                                        Eigen::Vector3d(triangles[i][1].x, triangles[i][1].y, triangles[i][1].z),
                                        Eigen::Vector3d(triangles[i][2].x, triangles[i][2].y, triangles[i][2].z)))
                    ||
                    triBoxOverlap(
                            Eigen::Vector3d(row * cell_size + env_min_x+cell_size/2,
                                col * cell_size + env_min_y+cell_size/2,
                                height * cell_size + env_min_z+cell_size/2),
                            Eigen::Vector3d(cell_size/2, cell_size/2, cell_size/2),
                            Eigen::Vector3d(triangles[i][0].x, triangles[i][0].y, triangles[i][0].z),
                                    Eigen::Vector3d(triangles[i][1].x, triangles[i][1].y, triangles[i][1].z),
                                    Eigen::Vector3d(triangles[i][2].x, triangles[i][2].y, triangles[i][2].z)))
                    {
                        mtx.lock();
                        env[col][row][height] = value_to_write;
                        if(value_to_write==cell_state::occupied){
                            //if the "limit" flags are activated, AND we are on the offending cells,
                            //AND the cell has not previously marked as normally occupied by a different triangle
                            //AND the cells are not on the very limit of the environment, mark the cell as "edge" for later cleanup
                            bool limitOfproblematicTriangle=(xLimit&&row==max_x)||
                                (yLimit&&col==max_y)||
                                (zLimit&&height==max_z);

                            bool endOfTheEnvironment = (col>0 || col<env.size() ||
                                row>0 || row<env[0].size() ||
                                height>0 ||  height<env[0][0].size());

                            if( !endOfTheEnvironment &&
                                limitOfproblematicTriangle &&
                                env[col][row][height]!=cell_state::occupied){
                                    env[col][row][height]=cell_state::edge;
                            }
                        }
                        mtx.unlock();

                    }
                }
            }
        }

        //log progress
        if(i>numberOfProcessedTriangles+triangles.size()/10){
            mtx.lock();
            std::cout<<(100*i)/triangles.size()<<"%\n";
            numberOfProcessedTriangles=i;
            mtx.unlock();
        }
    }
}

void parse(std::string filename, cell_state value_to_write){

    bool ascii = false;
    if (FILE *file = fopen(filename.c_str(), "r"))
    {
        //File exists!, keep going!
        char buffer[6];
        fgets(buffer, 6, file);
        if(std::string(buffer).find("solid")!=std::string::npos)
            ascii=true;
        fclose(file);
    }else{
        std::cout<< "File " << filename << " does not exist\n";
        return;
    }

    std::vector<Triangle> triangles;
    std::vector<Point> normals;

    if(ascii){
        //first, we count how many triangles there are (we need to do this before reading the data
        // to create a vector of the right size)
        std::ifstream countfile(filename.c_str());
        std::string line;
        int count = 0;

        while (std::getline(countfile, line)){
            if(line.find("facet normal") != std::string::npos){
                count++;
            }
        }
        countfile.close();
        //each points[i] contains one the three vertices of triangle i
        triangles.resize(count);
        normals.resize(count);
        //let's read the data
        std::ifstream infile(filename.c_str());
        std::getline(infile, line);
        int i =0;
        while (line.find("endsolid")==std::string::npos)
        {
            while (line.find("facet normal") == std::string::npos){std::getline(infile, line);}
            size_t pos = line.find("facet");
            line.erase(0, pos + 12);
            float aux;
            std::stringstream ss(line);
            ss >> std::skipws >>  aux;
            normals[i].x = roundf(aux * roundFactor) / roundFactor;
            ss >> std::skipws >>  aux;
            normals[i].y = roundf(aux * roundFactor) / roundFactor;
            ss >> std::skipws >>  aux;
            normals[i].z = roundf(aux * roundFactor) / roundFactor;
            std::getline(infile, line);

            for(int j=0;j<3;j++){
                std::getline(infile, line);
                size_t pos = line.find("vertex ");
                line.erase(0, pos + 7);
                std::stringstream ss(line);
                ss >> std::skipws >>  aux;
                triangles[i][j].x = roundf(aux * roundFactor) / roundFactor;
                ss >> std::skipws >>  aux;
                triangles[i][j].y = roundf(aux * roundFactor) / roundFactor;
                ss >> std::skipws >>  aux;
                triangles[i][j].z = roundf(aux * roundFactor) / roundFactor;
            }
            i++;
            //skipping lines here makes checking for the end of the file more convenient
            std::getline(infile, line);
            std::getline(infile, line);
            while(std::getline(infile, line)&&line.length()==0);
        }
        infile.close();
    }
    else{
        std::ifstream infile(filename.c_str(), std::ios_base::binary);
        infile.seekg(80 * sizeof(uint8_t), std::ios_base::cur); //skip the header
        uint32_t num_triangles;
        infile.read((char*) &num_triangles, sizeof(uint32_t));
        triangles.resize(num_triangles);
        normals.resize(num_triangles);

        for(int i = 0; i < num_triangles; i++){
            infile.read((char*) &normals[i], 3 * sizeof(float)); //read the normal vector

            for(int j=0; j<3;j++){
                infile.read((char*) &triangles[i][j], 3 * sizeof(float)); //read the point
            }

            infile.seekg(sizeof(uint16_t), std::ios_base::cur); //skip the attribute data
        }
        infile.close();
    }

    //OK, we have read the data, let's do something with it
    occupy(triangles, normals, value_to_write);

}

void findDimensions(std::string filename){
    bool ascii = false;
    if (FILE *file = fopen(filename.c_str(), "r"))
    {
        //File exists!, keep going!
        char buffer[6];
        fgets(buffer, 6, file);
        if(std::string(buffer).find("solid")!=std::string::npos)
            ascii=true;
        fclose(file);
    }else{
        std::cout<< "File " << filename << " does not exist\n";
        return;
    }

    if(ascii){
        //let's read the data
        std::string line;
        std::ifstream infile(filename.c_str());
        std::getline(infile, line);
        int i =0;
        while (line.find("endsolid")==std::string::npos)
        {
            while (std::getline(infile, line) && line.find("outer loop") == std::string::npos);

            for(int j=0;j<3;j++){
                float x, y, z;
                std::getline(infile, line);
                size_t pos = line.find("vertex ");
                line.erase(0, pos + 7);
                std::stringstream ss(line);
                float aux;
                ss >> std::skipws >>  aux;
                x = roundf(aux * roundFactor) / roundFactor;
                ss >> std::skipws >>  aux;
                y = roundf(aux * roundFactor) / roundFactor;
                ss >> std::skipws >>  aux;
                z = roundf(aux * roundFactor) / roundFactor;
                env_max_x = env_max_x>=x?env_max_x:x;
                env_max_y = env_max_y>=y?env_max_y:y;
                env_max_z = env_max_z>=z?env_max_z:z;
                env_min_x = env_min_x<=x?env_min_x:x;
                env_min_y = env_min_y<=y?env_min_y:y;
                env_min_z = env_min_z<=z?env_min_z:z;
            }
            i++;
            //skipping three lines here makes checking for the end of the file more convenient
            std::getline(infile, line);
            std::getline(infile, line);
            while(std::getline(infile, line)&&line.length()==0);
        }
        infile.close();
    }
    else{
        std::ifstream infile(filename.c_str(), std::ios_base::binary);
        infile.seekg(80 * sizeof(uint8_t), std::ios_base::cur); //skip the header
        uint32_t num_triangles;
        infile.read((char*) &num_triangles, sizeof(uint32_t));
        for(int i = 0; i < num_triangles; i++){
            infile.seekg(3 * sizeof(float), std::ios_base::cur); //skip the normal vector
            for(int j=0; j<3;j++){
                float x, y ,z;
                infile.read((char*) &x, sizeof(float));
                infile.read((char*) &y, sizeof(float));
                infile.read((char*) &z, sizeof(float));
                env_max_x = env_max_x>=x?env_max_x:x;
                env_max_y = env_max_y>=y?env_max_y:y;
                env_max_z = env_max_z>=z?env_max_z:z;
                env_min_x = env_min_x<=x?env_min_x:x;
                env_min_y = env_min_y<=y?env_min_y:y;
                env_min_z = env_min_z<=z?env_min_z:z;
            }

            infile.seekg(sizeof(uint16_t), std::ios_base::cur); //skip the attribute data
        }
    }

    std::cout<<"Dimensions are:\n"<<
    "x: ("<<env_min_x<<", "<<env_max_x<<")\n"<<
    "y: ("<<env_min_y<<", "<<env_max_y<<")\n"<<
    "z: ("<<env_min_z<<", "<<env_max_z<<")\n";

}

int indexFrom3D(int x, int y, int z){
	return x + y*env[0].size() + z*env[0].size()*env.size();
}

void openFoam_to_gaden(std::string filename)
{

	//let's parse the file
	std::ifstream infile(filename.c_str());
	std::string line;

	//ignore the first line (column names)
	std::getline(infile, line);
    std::vector<double> U(env[0].size()*env.size()*env[0][0].size());
    std::vector<double> V(env[0].size()*env.size()*env[0][0].size());
    std::vector<double> W(env[0].size()*env.size()*env[0][0].size());
    std::vector<double> v(6);
	int x_idx = 0;
	int y_idx = 0;
	int z_idx = 0;
	while (std::getline(infile, line))
	{
		if (line.length()!=0)
		{
			for (int i = 0; i < 6; i++)
			{
				size_t pos = line.find(",");
				v[i] = atof(line.substr(0, pos).c_str());
				line.erase(0, pos + 1);
			}
			//assign each of the points we have information about to the nearest cell
			x_idx = (int)roundf((v[3] - env_min_x) / cell_size*roundFactor)/roundFactor;
			y_idx = (int)roundf((v[4] - env_min_y) / cell_size*roundFactor)/roundFactor;
			z_idx = (int)roundf((v[5] - env_min_z) / cell_size*roundFactor)/roundFactor;
			U[ indexFrom3D(x_idx, y_idx,z_idx) ] = v[0];
			V[ indexFrom3D(x_idx, y_idx,z_idx) ] = v[1];
			W[ indexFrom3D(x_idx, y_idx,z_idx) ] = v[2];
		}
	}
    infile.close();
    printWind(U,V,W,filename);
}

void fill(int x, int y, int z, cell_state new_value, cell_state value_to_overwrite){
    std::queue<Eigen::Vector3i> q;
    q.push(Eigen::Vector3i(x, y, z));
    env[x][y][z]=new_value;
    while(!q.empty()){
        Eigen::Vector3i point = q.front();
        q.pop();
        if(compare_cell(point.x()+1, point.y(), point.z(), value_to_overwrite)){ // x+1, y, z
            env[point.x()+1][point.y()][point.z()]=new_value;
            q.push(Eigen::Vector3i(point.x()+1, point.y(), point.z()));
        }

        if(compare_cell(point.x()-1, point.y(), point.z(), value_to_overwrite)){ // x-1, y, z
            env[point.x()-1][point.y()][point.z()]=new_value;
            q.push(Eigen::Vector3i(point.x()-1, point.y(), point.z()));
        }

        if(compare_cell(point.x(), point.y()+1, point.z(), value_to_overwrite)){ // x, y+1, z
            env[point.x()][point.y()+1][point.z()]=new_value;
            q.push(Eigen::Vector3i(point.x(), point.y()+1, point.z()));
        }

        if(compare_cell(point.x(), point.y()-1, point.z(), value_to_overwrite)){ // x, y-1, z
            env[point.x()][point.y()-1][point.z()]=new_value;
            q.push(Eigen::Vector3i(point.x(), point.y()-1, point.z()));
        }

        if(compare_cell(point.x(), point.y(), point.z()+1, value_to_overwrite)){ // x, y, z+1
            env[point.x()][point.y()][point.z()+1]=new_value;
            q.push(Eigen::Vector3i(point.x(), point.y(), point.z()+1));
        }

        if(compare_cell(point.x(), point.y(), point.z()-1, value_to_overwrite)){ // x, y, z-1
            env[point.x()][point.y()][point.z()-1]=new_value;
            q.push(Eigen::Vector3i(point.x(), point.y(), point.z()-1));
        }
    }
}

void clean(){
    for(int col=0;col<env.size();col++){
        for(int row=0;row<env[0].size();row++){
            for(int height=0;height<env[0][0].size();height++){

                if(env[col][row][height]==cell_state::edge){

                    if(compare_cell(col+1, row, height, cell_state::empty)||
                        compare_cell(col, row+1, height, cell_state::empty)||
                        compare_cell(col, row, height+1, cell_state::empty)||
                        (compare_cell(col+1, row+1, height, cell_state::empty)
                            &&env[col][row+1][height]==cell_state::edge
                            &&env[col+1][row][height]==cell_state::edge))
                    {
                        env[col][row][height]=cell_state::empty;
                    }else
                    {
                        env[col][row][height]=cell_state::occupied;
                    }

                }

            }
        }
    }
}

int main(int argc, char **argv){
    ros::init(argc, argv, "preprocessing");
    int numModels;
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");
    ros::Publisher pub = nh.advertise<std_msgs::Bool>("preprocessing_done",5,true);

    private_nh.param<float>("cell_size", cell_size, 1); //size of the cells

    roundFactor=100.0/cell_size;
    //stl file with the model of the outlets
    std::string outlet; int numOutletModels;

    //path to the csv file where we want to write the occupancy map
    std::string output;
    private_nh.param<std::string>("output_path", output, "");

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

        //OCCUPANCY

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

    private_nh.param<int>("number_of_models", numModels, 2); // number of CAD models

    std::vector<std::string> CADfiles;
    for(int i = 0; i< numModels; i++){
        std::string paramName = boost::str( boost::format("model_%i") % i); //each of the stl models
        std::string filename;
        private_nh.param<std::string>(paramName, filename, "");
        CADfiles.push_back(filename.c_str());
    }

    for (int i = 0; i < CADfiles.size(); i++)
    {
        findDimensions(CADfiles[i]);
    }

    //x and y are interchanged!!!!!! it goes env[y][x][z]
    //I cannot for the life of me remember why I did that, but there must have been a reason
    env = std::vector<std::vector<std::vector<int> > > (ceil((env_max_y-env_min_y)*(roundFactor)/(cell_size*(roundFactor))),
                                                    std::vector<std::vector<int> >(ceil((env_max_x - env_min_x)*(roundFactor)/(cell_size*(roundFactor))),
                                                                                std::vector<int>(ceil((env_max_z - env_min_z)*(roundFactor)/(cell_size*(roundFactor))), 0)));

    ros::Time start = ros::Time::now();
    for (int i = 0; i < numModels; i++)
    {
        parse(CADfiles[i], cell_state::occupied);
    }

    std::cout <<"Took "<< ros::Time::now().toSec()-start.toSec()<<" seconds \n";
    float empty_point_x;
    private_nh.param<float>("empty_point_x", empty_point_x, 1);
    float empty_point_y;
    private_nh.param<float>("empty_point_y", empty_point_y, 1);
    float empty_point_z;
    private_nh.param<float>("empty_point_z", empty_point_z, 1);

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

        //OUTLETS

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

    private_nh.param<int>("number_of_outlet_models", numOutletModels, 1); // number of CAD models

    std::vector<std::string> outletFiles;
    for(int i = 0; i< numOutletModels; i++){
        std::string paramName = boost::str( boost::format("outlets_model_%i") % i); //each of the stl models
        std::string filename;
        private_nh.param<std::string>(paramName, filename, "");
        outletFiles.push_back(filename.c_str());
    }

    for (int i=0;i<numOutletModels; i++){
        parse(outletFiles[i], cell_state::outlet);
    }

    std::cout<<"Filling...\n";
    //Mark all the empty cells reachable from the empty_point as aux_empty
    //the ones that cannot be reached will be marked as occupied when printing
    fill((empty_point_y-env_min_y)/cell_size,
        (empty_point_x-env_min_x)/cell_size,
        (empty_point_z-env_min_z)/cell_size,
        cell_state::empty, cell_state::non_initialized);

    //get rid of the cells marked as "edge", since those are not truly occupied
    clean();

    private_nh.param<float>("floor_height", floor_height, 0); // number of CAD models
    printMap(boost::str(boost::format("%s/occupancy.pgm") % output.c_str()), 10);

    std::string worldFile;
    private_nh.param<std::string>("worldFile", worldFile, ""); // number of CAD models
    if(worldFile!="")
        changeWorldFile(worldFile);

    //output - path, occupancy vector, scale
    printEnv(boost::str(boost::format("%s/OccupancyGrid3D.csv") % output.c_str()), 1);
    printYaml(output);

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

        //WIND

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

    bool uniformWind;
    private_nh.param<bool>("uniformWind", uniformWind, false);

    //path to the point cloud files with the wind data
    std::string windFileName;
    private_nh.param<std::string>("wind_files", windFileName, "");
    int idx = 0;

    if(uniformWind){

        //let's parse the file
        std::ifstream infile(windFileName);
        std::string line;

        std::vector<double> U(env[0].size()*env.size()*env[0][0].size());
        std::vector<double> V(env[0].size()*env.size()*env[0][0].size());
        std::vector<double> W(env[0].size()*env.size()*env[0][0].size());
        while(std::getline(infile, line)){
            std::vector<double> v;
            for (int i = 0; i < 3; i++)
			{
				size_t pos = line.find(",");
				v.push_back(atof(line.substr(0, pos).c_str()));
				line.erase(0, pos + 1);
			}

            for(int i = 0; i< env[0].size();i++){
                for(int j = 0; j< env.size();j++){
                    for(int k = 0; k< env[0][0].size();k++){
                        if(env[j][i][k]==cell_state::empty){

                            U[ indexFrom3D(i, j, k) ] = v[0];
                            V[ indexFrom3D(i, j, k) ] = v[1];
                            W[ indexFrom3D(i, j, k) ] = v[2];
                        }
                    }
                }
            }
            infile.close();
            printWind(U,V,W, boost::str(boost::format("%s_%i.csv") % windFileName % idx).c_str());
            idx++;
        }
    }else{
        while (FILE *file = fopen(boost::str(boost::format("%s_%i.csv") % windFileName % idx).c_str(), "r"))
        {
            fclose(file);
            openFoam_to_gaden(boost::str(boost::format("%s_%i.csv") % windFileName % idx).c_str());
            idx++;
        }
    }

    ROS_INFO("Preprocessing done");
    std_msgs::Bool b;
    b.data=true;
    pub.publish(b);

}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_TYPES_COMPLEX_HPP
#define BOOST_COMPUTE_TYPES_COMPLEX_HPP

#include <complex>

#include <boost/compute/functional.hpp>
#include <boost/compute/types/fundamental.hpp>
#include <boost/compute/type_traits/make_vector_type.hpp>
#include <boost/compute/type_traits/type_name.hpp>
#include <boost/compute/detail/meta_kernel.hpp>

namespace boost {
namespace compute {
namespace detail {

template<class T>
meta_kernel& operator<<(meta_kernel &kernel, const std::complex<T> &x)
{
    typedef typename std::complex<T> value_type;

    kernel << "(" << type_name<value_type>() << ")"
           << "(" << x.real() << ", " << x.imag() << ")";

    return kernel;
}

// get<N>() result type specialization for std::complex<>
template<size_t N, class T>
struct get_result_type<N, std::complex<T> >
{
    typedef T type;
};

// get<N>() specialization for std::complex<>
template<size_t N, class Arg, class T>
inline meta_kernel& operator<<(meta_kernel &kernel,
                               const invoked_get<N, Arg, std::complex<T> > &expr)
{
    BOOST_STATIC_ASSERT(N < 2);

    return kernel << expr.m_arg << (N == 0 ? ".x" : ".y");
}

} // end detail namespace

// returns the real component of a complex<T>
template<class T>
struct real
{
    typedef T result_type;

    template<class Arg>
    detail::invoked_get<0, Arg, std::complex<T> >
    operator()(const Arg &x) const
    {
        return detail::invoked_get<0, Arg, std::complex<T> >(x);
    }
};

// returns the imaginary component of a complex<T>
template<class T>
struct imag
{
    typedef T result_type;

    template<class Arg>
    detail::invoked_get<1, Arg, std::complex<T> >
    operator()(const Arg &x) const
    {
        return detail::invoked_get<1, Arg, std::complex<T> >(x);
    }
};

namespace detail {

template<class Arg1, class Arg2, class T>
struct invoked_complex_multiplies
{
    typedef typename std::complex<T> result_type;

    invoked_complex_multiplies(const Arg1 &x, const Arg2 &y)
        : m_x(x),
          m_y(y)
    {
    }

    Arg1 m_x;
    Arg2 m_y;
};

template<class Arg1, class Arg2, class T>
inline meta_kernel& operator<<(meta_kernel &kernel,
                               const invoked_complex_multiplies<Arg1, Arg2, T> &expr)
{
    typedef typename std::complex<T> value_type;

    kernel << "(" << type_name<value_type>() << ")"
           << "(" << expr.m_x << ".x*" << expr.m_y << ".x-"
                  << expr.m_x << ".y*" << expr.m_y << ".y,"
                  << expr.m_x << ".y*" << expr.m_y << ".x+"
                  << expr.m_x << ".x*" << expr.m_y << ".y" << ")";

    return kernel;
}

template<class Arg, class T>
struct invoked_complex_conj
{
    typedef typename std::complex<T> result_type;

    invoked_complex_conj(const Arg &arg)
        : m_arg(arg)
    {
    }

    Arg m_arg;
};

template<class Arg, class T>
inline meta_kernel& operator<<(meta_kernel &kernel,
                               const invoked_complex_conj<Arg, T> &expr)
{
    typedef typename std::complex<T> value_type;

    kernel << "(" << type_name<value_type>() << ")"
           << "(" << expr.m_arg << ".x" << ", -" << expr.m_arg << ".y" << ")";

    return kernel;
}

} // end detail namespace

// specialization for multiplies<T>
template<class T>
class multiplies<std::complex<T> > :
    public function<std::complex<T> (std::complex<T>, std::complex<T>)>
{
public:
    multiplies() :
        function<
            std::complex<T> (std::complex<T>, std::complex<T>)
        >("complex_multiplies")
    {
    }

    template<class Arg1, class Arg2>
    detail::invoked_complex_multiplies<Arg1, Arg2, T>
    operator()(const Arg1 &x, const Arg2 &y) const
    {
        return detail::invoked_complex_multiplies<Arg1, Arg2, T>(x, y);
    }
};

// returns the complex conjugate of a complex<T>
template<class T>
struct conj
{
    typedef typename std::complex<T> result_type;

    template<class Arg>
    detail::invoked_complex_conj<Arg, T>
    operator()(const Arg &x) const
    {
        return detail::invoked_complex_conj<Arg, T>(x);
    }
};

namespace detail {

// type_name() specialization for std::complex
template<class T>
struct type_name_trait<std::complex<T> >
{
    static const char* value()
    {
        typedef typename make_vector_type<T, 2>::type vector_type;

        return type_name<vector_type>();
    }
};

} // end detail namespace
} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_TYPES_COMPLEX_HPP


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2012 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   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 <boost/timer.hpp>
#include <boost/foreach.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/regex.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <boost/make_shared.hpp>

#include <iostream>
#include <fstream>
#include <streambuf>

#include <Core/Algorithms/DataIO/EigenMatrixFromScirunAsciiFormatConverter.h>
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Algorithms/Base/AlgorithmBase.h>
#include <Core/Utils/FileUtil.h>
#include <Core/Utils/StringUtil.h>

using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Utility;
using namespace SCIRun::Core::Algorithms::DataIO::internal;

EigenMatrixFromScirunAsciiFormatConverter::EigenMatrixFromScirunAsciiFormatConverter(const ProgressReporter* reporter) : reporter_(reporter)
{
}

MatrixHandle EigenMatrixFromScirunAsciiFormatConverter::make(const std::string& matFile)
{
  if (reporter_)
    reporter_->update_progress(0.01);
  if (fileContainsString(matFile, "DenseMatrix"))
    return makeDense(matFile);
  if (fileContainsString(matFile, "SparseRowMatrix"))
    return makeSparse(matFile);
  if (fileContainsString(matFile, "ColumnMatrix"))
    return makeColumn(matFile);

  /// @todo: no access to error(), need alternative for logging this exception
  BOOST_THROW_EXCEPTION(AlgorithmInputException() << ErrorMessage("Unknown SCIRun matrix format"));
}

SparseRowMatrixHandle EigenMatrixFromScirunAsciiFormatConverter::makeSparse(const std::string& matFile)
{
  SparseData data = convertRaw(parseSparseMatrixString(getMatrixContentsLine(readFile(matFile)).get()).get());
  if (reporter_)
    reporter_->update_progress(0.7);
  SparseRowMatrixHandle mat(boost::make_shared<SparseRowMatrix>(data.get<0>(), data.get<1>()));

  typedef Eigen::Triplet<double> T;
  std::vector<T> tripletList;
  {
    const Indices& rowAcc = data.get<3>();
    const Indices& cols = data.get<4>();
    const Data& values = data.get<5>();
    size_t estimation_of_entries = values.size();
    tripletList.reserve(estimation_of_entries);

    int count = 0;
    int nextRow;
    for (int r = 0; r < mat->rows(); ++r)
    {
      nextRow = rowAcc[r+1];
      while (count < nextRow)
      {
        tripletList.push_back(T(r, cols[count], values[count]));
        count++;
      }
    }
  }

  mat->setFromTriplets(tripletList.begin(), tripletList.end());
  mat->makeCompressed();
  if (reporter_)
    reporter_->update_progress(1);
  return mat;
}

boost::optional<std::string> EigenMatrixFromScirunAsciiFormatConverter::getMatrixContentsLine(const std::string& matStr)
{
  if (reporter_)
    reporter_->update_progress(0.2);

  std::istringstream reader(matStr);
  std::string line;
  while(std::getline(reader, line))
  {
    if (line.length() > 2 && isdigit(line[0]))
      return line;
  }
  return boost::optional<std::string>();
}

std::string EigenMatrixFromScirunAsciiFormatConverter::readFile(const std::string& filename)
{
  std::ifstream matFile(filename.c_str());
  std::string matStr((std::istreambuf_iterator<char>(matFile)),
    std::istreambuf_iterator<char>());
  if (reporter_)
    reporter_->update_progress(0.1);
  return matStr;
}

DenseMatrixHandle EigenMatrixFromScirunAsciiFormatConverter::makeDense(const std::string& matFile)
{
  DenseData data = convertRaw(parseDenseMatrixString(getMatrixContentsLine(readFile(matFile)).get()).get());
  DenseMatrixHandle mat(boost::make_shared<DenseMatrix>(data.get<0>(), data.get<1>()));

  auto values = data.get<2>().begin();
  for (int i = 0; i < mat->rows(); ++i)
    for (int j = 0; j < mat->cols(); ++j)
      (*mat)(i,j) = *values++;

  return mat;
}

DenseColumnMatrixHandle EigenMatrixFromScirunAsciiFormatConverter::makeColumn(const std::string& matFile)
{
  DenseData data = convertRaw(parseColumnMatrixString(getMatrixContentsLine(readFile(matFile)).get()).get());
  DenseColumnMatrixHandle mat(boost::make_shared<DenseColumnMatrix>(data.get<0>()));

  auto values = data.get<2>().begin();
  for (int i = 0; i < mat->rows(); ++i)
      (*mat)(i) = *values++;

  return mat;
}

boost::optional<EigenMatrixFromScirunAsciiFormatConverter::RawDenseData> EigenMatrixFromScirunAsciiFormatConverter::parseDenseMatrixString(const std::string& matString)
{
  boost::regex r("(\\d+) (\\d+) \\{0 (.*)\\}\\}");
  boost::smatch what;
  regex_match(matString, what, r);
  if (what.size() == 4)
  {
    return boost::make_tuple(what[1].str(), what[2].str(), what[3].str());
  }
  return boost::optional<RawDenseData>();
}

boost::optional<EigenMatrixFromScirunAsciiFormatConverter::RawDenseData> EigenMatrixFromScirunAsciiFormatConverter::parseColumnMatrixString(const std::string& matString)
{
  boost::regex r("(\\d+) (.*)\\}");
  boost::smatch what;
  regex_match(matString, what, r);
  if (what.size() == 3)
  {
    return boost::make_tuple(what[1].str(), std::string("1"), what[2].str());
  }
  return boost::optional<RawDenseData>();
}

EigenMatrixFromScirunAsciiFormatConverter::DenseData EigenMatrixFromScirunAsciiFormatConverter::convertRaw(const RawDenseData& data)
{
  return boost::make_tuple(
    boost::lexical_cast<int>(data.get<0>()),
    boost::lexical_cast<int>(data.get<1>()),
    parseLineOfNumbers<double>(data.get<2>()));
}

boost::optional<EigenMatrixFromScirunAsciiFormatConverter::RawSparseData> EigenMatrixFromScirunAsciiFormatConverter::parseSparseMatrixString(const std::string& matString)
{
  boost::regex r("(\\d+) (\\d+) (\\d+) \\{8 (.*)\\}\\{8 (.*)\\}\\{(.*)\\}\\}\\h*");
  boost::smatch what;
  regex_match(matString, what, r);
  if (what.size() == 7)
  {
    if (reporter_)
      reporter_->update_progress(0.4);
    return boost::make_tuple(what[1].str(), what[2].str(), what[3].str(), what[4].str(), what[5].str(), what[6].str());
  }
  return boost::optional<RawSparseData>();
}

EigenMatrixFromScirunAsciiFormatConverter::SparseData EigenMatrixFromScirunAsciiFormatConverter::convertRaw(const RawSparseData& data)
{
  if (reporter_)
    reporter_->update_progress(0.5);
  return boost::make_tuple(
    boost::lexical_cast<int>(data.get<0>()),
    boost::lexical_cast<int>(data.get<1>()),
    boost::lexical_cast<int>(data.get<2>()),
    parseLineOfNumbers<int>(data.get<3>()),
    parseLineOfNumbers<int>(data.get<4>()),
    parseLineOfNumbers<double>(data.get<5>()));
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"

#include <boost/python.hpp>

#include "graph.hh"
#include "histogram.hh"
#include "graph_selectors.hh"
#include "graph_properties.hh"

#include "graph_corr_hist.hh"

#include <iostream>

using namespace std;
using namespace boost;
using namespace graph_tool;

// implementations spread across different compile units to minimize memory
// usage during compilation
void graph_correlations_imp1(GraphInterface& g, boost::python::object& hist,
                             boost::python::object& ret_bins,
                             boost::any deg1, boost::any deg2,
                             boost::any weight,
                             const std::array<vector<long double>,2>& bins);

typedef ConstantPropertyMap<int,GraphInterface::edge_t> cweight_map_t;

boost::python::object
get_vertex_correlation_histogram(GraphInterface& gi,
                                 GraphInterface::deg_t deg1,
                                 GraphInterface::deg_t deg2,
                                 boost::any weight,
                                 const vector<long double>& xbin,
                                 const vector<long double>& ybin)
{
    boost::python::object hist;
    boost::python::object ret_bins;

    std::array<vector<long double>,2> bins;
    bins[0] = xbin;
    bins[1] = ybin;

    any weight_prop;
    typedef DynamicPropertyMapWrap<long double, GraphInterface::edge_t>
        wrapped_weight_t;

    if (!weight.empty())
    {
        weight_prop = wrapped_weight_t(weight, edge_scalar_properties());
    }
    else
        weight_prop = cweight_map_t(1);

    try
    {
        run_action<>()(gi, get_correlation_histogram<GetNeighboursPairs>
                       (hist, bins, ret_bins),
                       scalar_selectors(), scalar_selectors(),
                       boost::mpl::vector<cweight_map_t>())
            (degree_selector(deg1), degree_selector(deg2), weight_prop);
    }
    catch (ActionNotFound&)
    {
        graph_correlations_imp1(gi, hist, ret_bins, degree_selector(deg1),
                                degree_selector(deg2), weight_prop, bins);
    }

    return boost::python::make_tuple(hist, ret_bins);
}

using namespace boost::python;

void export_vertex_correlations()
{
    def("vertex_correlation_histogram", &get_vertex_correlation_histogram);
}


//
// Copyright © 2021 Arm Ltd and Contributors. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersSerializeFixture.hpp"
#include "../Deserializer.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(DeserializeParser)

struct ArgMinMaxFixture : public ParserFlatbuffersSerializeFixture
{
    explicit ArgMinMaxFixture(const std::string& inputShape,
                              const std::string& outputShape,
                              const std::string& axis,
                              const std::string& argMinMaxFunction)
    {
        m_JsonString = R"(
        {
          layers: [
            {
              layer_type: "InputLayer",
              layer: {
                base: {
                  base: {
                    layerName: "InputLayer",
                    layerType: "Input",
                    inputSlots: [

                    ],
                    outputSlots: [
                      {
                        tensorInfo: {
                          dimensions: )" + inputShape + R"(,
                          dataType: "Float32",
                          quantizationScale: 0.0
                        }
                      }
                    ]
                  }
                }
              }
            },
            {
              layer_type: "ArgMinMaxLayer",
              layer: {
                base: {
                  index: 1,
                  layerName: "ArgMinMaxLayer",
                  layerType: "ArgMinMax",
                  inputSlots: [
                    {
                      connection: {
                        sourceLayerIndex: 0,
                        outputSlotIndex: 0
                      }
                    }
                  ],
                  outputSlots: [
                    {
                      tensorInfo: {
                        dimensions: )" + outputShape + R"(,
                        dataType: "Signed64",
                        quantizationScale: 0.0
                      }
                    }
                  ]
                },
                descriptor: {
                  axis: )" + axis + R"(,
                  argMinMaxFunction: )" + argMinMaxFunction + R"(
                }
              }
            },
            {
              layer_type: "OutputLayer",
              layer: {
                base: {
                  base: {
                    index: 2,
                    layerName: "OutputLayer",
                    layerType: "Output",
                    inputSlots: [
                      {
                        connection: {
                          sourceLayerIndex: 1,
                          outputSlotIndex: 0
                        }
                      }
                    ],
                    outputSlots: [

                    ]
                  }
                }
              }
            }
          ],
          inputIds: [
            0
          ],
          outputIds: [
            0
          ],
          featureVersions: {
            bindingIdsScheme: 1
          }
        }
    )";
        Setup();
    }
};

struct SimpleArgMinMaxFixture : public ArgMinMaxFixture
{
    SimpleArgMinMaxFixture() : ArgMinMaxFixture("[ 1, 1, 1, 5 ]",
                                                "[ 1, 1, 1 ]",
                                                "-1",
                                                "Max") {}
};

BOOST_FIXTURE_TEST_CASE(ArgMinMax, SimpleArgMinMaxFixture)
{
    RunTest<3, armnn::DataType::Float32, armnn::DataType::Signed64>(
            0,
            {{"InputLayer", { 6.0f, 2.0f, 8.0f, 10.0f, 9.0f}}},
            {{"OutputLayer",{ 3l }}});
}

BOOST_AUTO_TEST_SUITE_END()


// SISTEMAS //
#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <fstream>
#include <math.h>
#include <algorithm>
#include <ctime>
#include <string.h>
#include <sys/stat.h>

#define LOGNAME_FORMAT "%d-%b-%y_%H:%M:%S"
#define LOGNAME_SIZE 20
//////////////

/// ROS //////
#include <ros/ros.h>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <tf/transform_listener.h>
//////////////
#include <vant/vaant.h>
#include <vant/vision.h>

// #### MATRICIAL ### //
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>

// ========================================================================================================================================= //
// Segunda versao do multiTagsLanding. O metodo chooseTag2 foi Modificado
// O centro de pouso e o marcador 15 !!! VERSAO TAG = VERSAO ANTIGA MAS COM OUTROS MARCADORES AO REDOR
// ========================================================================================================================================= //

using namespace std;

int main(int argc, char **argv)
{
    int flagID ;
    double tempX, tempY, tempZ;

    ros::init(argc, argv, "multiTagsLandingV2");
    ros::NodeHandle n;
    vision visao(n,100,vision::VISION_ARGUMENTS::opt.single,vision::VISION_ARGUMENTS::v.V3);

    ros::Publisher chatter_pub = n.advertise<ar_track_alvar_msgs::AlvarMarker>("/vision/main_tag_pose",1);
    ar_track_alvar_msgs::AlvarMarker toPub;
    ros::Rate loop_rate(10);

    vision::flagList tempFlag;

    static tf::TransformBroadcaster br;
    tf::TransformListener listener;
    tf::StampedTransform TH_M_C, TH_CENTER_C, TH_M0_M34, TH_M1_M34,
            TH_M2_M34, TH_M4_M34, TH_M5_M34, TH_M6_M34, TH_M8_M34, TH_M9_M34, TH_M11_M34, TH_M12_M34,TH_M13_M34,
            TH_M14_M34,TH_M15_M34, TH_M16_M34, TH_M18_M34, TH_M25_M34, TH_M27_M34, TH_M31_M34, TH_M32_M34,
            TH_M34_M34, TH_M38_M34, TH_M40_M34, TH_M43_M34, TH_M45_M34, TH_M54_M34, TH_M59_M34, TH_M60_M34,
            TH_M61_M34, TH_M63_M34, TH_M64_M34, TH_M68_M34, TH_M73_M34, TH_M75_M34, TH_M79_M34, TH_M80_M34,
            TH_M91_M34, TH_M95_M34, TH_M96_M34, TH_M98_M34, TH_M100_M34;

    TH_M0_M34  = vaant::createHT_Matrix(-0.000,-0.865,0,0,0,0);       /// Cria a Matriz Marker 0 em relacao ao Marker 34.
    TH_M1_M34 = vaant::createHT_Matrix( 0.112,-0.866,0,0,0,0);   /// Cria a Matriz Marker 1 em relacao ao Marker 34.
    TH_M2_M34 = vaant::createHT_Matrix( -0.530,0.801,0,0,0,0);   /// Cria a Matriz Marker 2 em relacao ao Marker 34.
    TH_M4_M34 = vaant::createHT_Matrix( -0.265,0.801,0,0,0,0);   /// Cria a Matriz Marker 4 em relacao ao Marker 34.
    TH_M5_M34 = vaant::createHT_Matrix(  0.000,0.801,0,0,0,0);
    TH_M6_M34 = vaant::createHT_Matrix(  0.265,0.801,0,0,0,0);
    TH_M8_M34 = vaant::createHT_Matrix(  0.530,0.803,0,0,0,0);
    TH_M9_M34 = vaant::createHT_Matrix( -0.530,0.535,0,0,0,0);
    TH_M11_M34 = vaant::createHT_Matrix( -0.265,0.535,0,0,0,0);
    TH_M12_M34 = vaant::createHT_Matrix(  0.000,0.535,0,0,0,0);
    TH_M13_M34 = vaant::createHT_Matrix( 0.265,0.535,0,0,0,0);
    TH_M14_M34 = vaant::createHT_Matrix( 0.530,-0.535,0,0,0,0);
    TH_M15_M34 = vaant::createHT_Matrix( -0.530,0.269,0,0,0,0);
    TH_M16_M34 = vaant::createHT_Matrix( -0.265,0.269,0,0,0,0);
    TH_M18_M34 = vaant::createHT_Matrix(  0.0,0.269,0,0,0,0);
    TH_M25_M34 = vaant::createHT_Matrix(  0.265,0.269,0,0,0,0);
    TH_M27_M34 = vaant::createHT_Matrix(  0.530,0.271,0,0,0,0);
    TH_M31_M34 = vaant::createHT_Matrix( -0.530,0.0,0,0,0,0);
    TH_M32_M34 = vaant::createHT_Matrix( -0.265,0.0,0,0,0,0);
    TH_M34_M34 = vaant::createHT_Matrix( 0.,0.0,0,0,0,0);
    TH_M38_M34 = vaant::createHT_Matrix( 0.265,0.0,0,0,0,0);
    TH_M40_M34 = vaant::createHT_Matrix( 0.530,0.0,0,0,0,0);
    TH_M43_M34 = vaant::createHT_Matrix( -0.530,-0.265,0,0,0,0);
    TH_M45_M34 = vaant::createHT_Matrix( -0.265,-0.265,0,0,0,0);
    TH_M54_M34 = vaant::createHT_Matrix(  0.000,-0.265,0,0,0,0);
    TH_M59_M34 = vaant::createHT_Matrix(  0.265,-0.265,0,0,0,0);
    TH_M60_M34 = vaant::createHT_Matrix(  0.530, -0.264,0,0,0,0);
    TH_M61_M34 = vaant::createHT_Matrix( -0.625,-0.515,0,0,0,0);
    TH_M63_M34 = vaant::createHT_Matrix(  0.632,-0.515,0,0,0,0);
    TH_M64_M34 = vaant::createHT_Matrix( -0.625,-0.775,0,0,0,0);   /// Cria a Matriz Marker 64 em relacao ao Marker 34.
    TH_M68_M34 = vaant::createHT_Matrix( -0.632,-0.775,0,0,0,0);   /// Cria a Matriz Marker 68 em relacao ao Marker 34.
    TH_M73_M34 = vaant::createHT_Matrix( -0.625,-1.035,0,0,0,0);   /// Cria a Matriz Marker 73 em relacao ao Marker 34.
    TH_M75_M34 = vaant::createHT_Matrix(  0.632,-1.035,0,0,0,0);   /// Cria a Matriz Marker 75 em relacao ao Marker 34.
    TH_M79_M34 = vaant::createHT_Matrix( -0.625,-1.295,0,0,0,0);   /// Cria a Matriz Marker 79 em relacao ao Marker 34.
    TH_M80_M34 = vaant::createHT_Matrix(  0.632,-1.295,0,0,0,0);   /// Cria a Matriz Marker 80 em relacao ao Marker 34.
    TH_M91_M34 = vaant::createHT_Matrix( -0.530,-1.465,0,0,0,0);   /// Cria a Matriz Marker 91 em relacao ao Marker 34.
    TH_M95_M34 = vaant::createHT_Matrix( -0.265,-1.465,0,0,0,0);   /// Cria a Matriz Marker 95 em relacao ao Marker 34.
    TH_M96_M34 = vaant::createHT_Matrix(  0.000,-1.465,0,0,0,0);   /// Cria a Matriz Marker 96 em relacao ao Marker 34.
    TH_M98_M34 = vaant::createHT_Matrix(  0.265,-1.465,0,0,0,0);   /// Cria a Matriz Marker 98 em relacao ao Marker 34.
    TH_M100_M34 = vaant::createHT_Matrix( 0.530,-1.465,0,0,0,0);   /// Cria a Matriz Marker 100 em relacao ao Marker 34.

    while(ros::ok()){

        tempFlag = visao.getFlags(); // Para verificar se viu algum marcador e assim não repetir TFs antigos
        cout<< tempFlag.flagEmpty << endl;
        if(tempFlag.flagEmpty == 1 ){
            cout << "Nao vejo nada" << endl;
            toPub.id = 0; // Nao tem marcador
            toPub.pose.pose.position.x = 0;
            toPub.pose.pose.position.y = 0;
            toPub.pose.pose.position.z = 0;
            toPub.pose.pose.orientation.w = 0;
            toPub.pose.pose.orientation.x = 0;
            toPub.pose.pose.orientation.y = 0;
            toPub.pose.pose.orientation.z = 0;

        }
        else{
            TH_M_C = visao.getPoseAlvarHTmatrix(); //monta mat trans. homogenea do currentTag (ou marcador visto) em relacao a camera
            tempFlag = visao.getFlags();      // NUNCA ISNERIR UM SPIN ENTRE ESSA INSTRUCAO E ANTERIOR
            br.sendTransform(tf::StampedTransform(TH_M_C,ros::Time::now(),"camera","currentTag"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame

            flagID = tempFlag.flagChooseID;
            cout <<"marcador visto " <<flagID <<endl;
            switch (flagID){ // Switch necessario para determinar quem que o currentTag na arvore de TF !

            case 0:
                br.sendTransform(tf::StampedTransform(TH_M0_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 1:
                br.sendTransform(tf::StampedTransform(TH_M1_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 2:
                br.sendTransform(tf::StampedTransform(TH_M2_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 4:
                br.sendTransform(tf::StampedTransform(TH_M4_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 5:
                br.sendTransform(tf::StampedTransform(TH_M5_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 6:
                br.sendTransform(tf::StampedTransform(TH_M6_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 8:
                br.sendTransform(tf::StampedTransform(TH_M8_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 9:
                br.sendTransform(tf::StampedTransform(TH_M9_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 11:
                br.sendTransform(tf::StampedTransform(TH_M11_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 12:
                br.sendTransform(tf::StampedTransform(TH_M12_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 13:
                br.sendTransform(tf::StampedTransform(TH_M13_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 14:
                br.sendTransform(tf::StampedTransform(TH_M14_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 15:
                br.sendTransform(tf::StampedTransform(TH_M15_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 16:
                br.sendTransform(tf::StampedTransform(TH_M16_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 18:
                br.sendTransform(tf::StampedTransform(TH_M18_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 25:
                br.sendTransform(tf::StampedTransform(TH_M25_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 27:
                br.sendTransform(tf::StampedTransform(TH_M27_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 31:
                br.sendTransform(tf::StampedTransform(TH_M31_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 32:
                br.sendTransform(tf::StampedTransform(TH_M32_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 34:
                br.sendTransform(tf::StampedTransform(TH_M34_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 38:
                br.sendTransform(tf::StampedTransform(TH_M38_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 40:
                br.sendTransform(tf::StampedTransform(TH_M40_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 43:
                br.sendTransform(tf::StampedTransform(TH_M43_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 45:
                br.sendTransform(tf::StampedTransform(TH_M45_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 54:
                br.sendTransform(tf::StampedTransform(TH_M54_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 59:
                br.sendTransform(tf::StampedTransform(TH_M59_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 60:
                br.sendTransform(tf::StampedTransform(TH_M60_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 61:
                br.sendTransform(tf::StampedTransform(TH_M61_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 63:
                br.sendTransform(tf::StampedTransform(TH_M63_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 64:
                br.sendTransform(tf::StampedTransform(TH_M64_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 68:
                br.sendTransform(tf::StampedTransform(TH_M68_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 73:
                br.sendTransform(tf::StampedTransform(TH_M73_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 75:
                br.sendTransform(tf::StampedTransform(TH_M75_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 79:
                br.sendTransform(tf::StampedTransform(TH_M79_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 80:
                br.sendTransform(tf::StampedTransform(TH_M80_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 91:
                br.sendTransform(tf::StampedTransform(TH_M91_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 95:
                br.sendTransform(tf::StampedTransform(TH_M95_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 96:
                br.sendTransform(tf::StampedTransform(TH_M96_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 98:
                br.sendTransform(tf::StampedTransform(TH_M98_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;

            case 100:
                br.sendTransform(tf::StampedTransform(TH_M100_M34.inverse(),ros::Time::now(),"currentTag","center"));//envia transformacao para o TF. Camera eh o parent frame e marcador child frame
                break;
            }

            try{
                listener.lookupTransform("/camera","/center",ros::Time(0),TH_CENTER_C); //obtem matriz de transf. homogena do marcador0 em relacao a camera
            }
            catch (tf::TransformException &ex) {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
                continue;
            }

            tempX = TH_CENTER_C.getOrigin().getX();
            tempY = TH_CENTER_C.getOrigin().getY();
            tempZ = TH_CENTER_C.getOrigin().getZ();

            toPub.pose.pose.position.x = tempX;
            toPub.pose.pose.position.y = tempY;
            toPub.pose.pose.position.z = tempZ;
            toPub.pose.pose.orientation.w = TH_CENTER_C.getRotation().getW();
            toPub.pose.pose.orientation.x = TH_CENTER_C.getRotation().getX();
            toPub.pose.pose.orientation.y = TH_CENTER_C.getRotation().getY();
            toPub.pose.pose.orientation.z = TH_CENTER_C.getRotation().getZ();

            toPub.id = 1; // Tem marcador

            cout <<" Posicao Central: "<< setiosflags(ios::fixed) << setprecision(3)<< setw(6) <<tempX << setw(8) << tempY <<setw(8) << tempZ <<endl;

        }

        chatter_pub.publish(toPub);

        loop_rate.sleep();

        ros::spinOnce();
    }
    ros::spin();
    return 0;
}