#include "PointCloudReader.h"
#include <cassert>
#include "math/mathutils.h"
#include "geo/CoordinateTransformation.h"
#include "math/CloudPoint.h"
#include "string/StringUtils.h"
#include "string/FilenameUtils.h"
#include "io/FileSystem.h"
#include "xml/xml.h"
#include <boost/tokenizer.hpp>

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

PointCloudReader::PointCloudReader()
{
   _line.resize(4096);
   _separators = " ,\t;"; // tokens for value separation in ASCII point cloud
}

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

PointCloudReader::~PointCloudReader()
{

}

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

bool PointCloudReader::Open(const std::string& sFilename, int nSourceEPSG)
{
   if (_ifstream.is_open())
   {
      _ifstream.close();
   }

   _nSourceEPSG = nSourceEPSG;
   _sFilenameA = sFilename;
   _ptsread = 0;

   _ifstream.open(sFilename.c_str());

   return _ifstream.good();
}

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

void PointCloudReader::Close()
{
   _ifstream.close();
}

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

bool PointCloudReader::ReadPoint(CloudPoint& point)
{
   char* line = &_line[0];
   std::vector<double> vOut;

   while (!_ifstream.eof())
   {
      _ifstream.getline(line, 4095);
      std::string string_line(line);
      Tokenize(string_line, _separators, vOut);
      size_t numColumns = vOut.size();

      if (_ptsread == 0) // first point!
      {
         if (numColumns == 3)
         {
            // Reading XYZ Point Cloud
            _pct = PCT_XYZ;
         }
         if (numColumns == 4)
         {
            // Reading XYZI Point Cloud
            _pct = PCT_XYZI;
         }
         else if (numColumns == 6)
         {
            // Reading XYZRGB Point Cloud!
            _pct = PCT_XYZRGB;
         }
         else if (numColumns == 7)
         {
            // Reading XYZIRGB Point Cloud!
            _pct = PCT_XYZIRGB;
         }
         else
         {
             // pc is not valid!
            _pct = PCT_INVALID;
            return false;
         }
      }

      if (vOut.size() >= 3 && vOut.size() == numColumns)
      {
         point.x = vOut[0];
         point.y = vOut[1];
         point.elevation = vOut[2];

         if (_pct == PCT_XYZI)
         {
            point.intensity = (int)vOut[3];
         }
         else if (_pct == PCT_XYZRGB)
         {
            point.r = (unsigned char)vOut[3];
            point.g = (unsigned char)vOut[4];
            point.b = (unsigned char)vOut[5];
         }
         else if (_pct == PCT_XYZIRGB)
         {
            point.intensity = (int)vOut[3];
            point.r = (unsigned char)vOut[4];
            point.g = (unsigned char)vOut[5];
            point.b = (unsigned char)vOut[6];
         }

         _ptsread++;
         return true;
      }
   }

   return false;
}

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


//  Copyright (c) 2021 Robert Ramey
//
// 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)

// test Numeric concept - compile only test

#include <boost/safe_numerics/utility.hpp>
#include <boost/safe_numerics/concept/numeric.hpp>
#include "test_checked_values.hpp"

// test that all intrinsic signed integers are detected as Numeric
using namespace boost::mp11;
static_assert(
    mp_all_of<
        signed_test_types,
        boost::safe_numerics::Numeric
    >(),
    "Numeric concept fails on at least one signed integer type"
);
// test that all intrinsic unigned integers are detected as Numeric
static_assert(
    mp_all_of<
        unsigned_test_types,
        boost::safe_numerics::Numeric
    >(),
    "Numeric concept fails on at least one unsigned integer type"
);

// test types without a std::numeric_limits entry are NOT detected as Numeric
struct X {};

static_assert(
    ! boost::safe_numerics::Numeric<X>(),
    "Type w/o std::numeric_limits entry erroneously detected as Numeric"
);

#include <iostream>
#include <boost/safe_numerics/safe_integer.hpp>

int main(){
    boost::safe_numerics::safe<int> s;
    std::cout << s;
    return 0;
}


// Andrew Naplavkov

#ifndef BARK_GEOMETRY_OSTREAM_HPP
#define BARK_GEOMETRY_OSTREAM_HPP

#include <bark/blob.hpp>
#include <bark/detail/wkb.hpp>
#include <bark/geometry/geometry.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/map.hpp>

namespace bark::geometry {

class ostream {
public:
    blob data;

    template <class T>
    auto operator<<(const T& r)
    {
        apply_tagged(r);
        return *this;
    }

private:
    void apply(const point& r) { data << double(r.x()) << double(r.y()); }

    void apply(const polygon& r)
    {
        data << uint32_t(1 + r.inners().size());
        apply(r.outer());
        for (auto& inner : r.inners())
            apply(inner);
    }

    void apply(const geometry&) {}

    template <class T>
    void apply(const T& r)
    {
        data << uint32_t(r.size());
        for (auto& item : r) {
            if constexpr (std::is_same_v<T, linestring> ||
                          std::is_same_v<T, polygon::ring_type>)
                apply(item);
            else
                apply_tagged(item);
        }
    }

    void apply_tagged(const geometry& r)
    {
        boost::apply_visitor([&](const auto& r) { apply_tagged(r); }, r);
    }

    template <class T>
    void apply_tagged(const T& r)
    {
        using namespace boost::mpl;
        data << wkb::HostEndian
             << uint32_t(
                    at<map<pair<point, int_<wkb::Point>>,
                           pair<linestring, int_<wkb::Linestring>>,
                           pair<polygon, int_<wkb::Polygon>>,
                           pair<multi_point, int_<wkb::MultiPoint>>,
                           pair<multi_linestring, int_<wkb::MultiLinestring>>,
                           pair<multi_polygon, int_<wkb::MultiPolygon>>,
                           pair<geometry_collection,
                                int_<wkb::GeometryCollection>>>,
                       T>::type::value);
        apply(r);
    }
};

}  // namespace bark::geometry

#endif  // BARK_GEOMETRY_OSTREAM_HPP


// Copyright <disenone>

#include <unordered_map>
#include <iostream>
#include <vector>

#define BOOST_TEST_MODULE test_squares
#define BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/timer/timer.hpp>
#include <boost/range/irange.hpp>

#include <common/nuid.hpp>
#include <common/silence_unused.hpp>
#include <squares/squares.hpp>

using namespace aoi;
using namespace aoi::squares;

BOOST_AUTO_TEST_SUITE(test_squares)

class Player;

class SquareAoiTest: public SquareAoi {
 public:
  SquareAoiTest(): SquareAoi(200) {}
friend class Player;
};

class Player {
 public:
  Player() : nuid_(GenNuid()), pos_(0, 0, 0) {}
  Player(Nuid nuid, Pos pos) : nuid_(nuid), pos_(pos) {}

 public:
  inline Nuid AddSensor(float radius, bool log = false) {
    auto sensor_id = GenNuid();
    aoi_->AddSensor(nuid_, sensor_id, radius);
    if (log) {
      printf("Player %lu Add Sensor %lu, radius %f\n", nuid_, sensor_id, radius);
    }
    return sensor_id;
  }

  inline void AddToAoi(SquareAoiTest* aoi, bool log = false) {
    aoi_ = aoi;
    aoi_->AddPlayer(nuid_, pos_.x, pos_.y, pos_.z);
    player_aoi_ = aoi_->player_map_.find(nuid_)->second.get();

    if (log) {
      printf("Add Player: %lu, Pos(%f, %f, %f), square_id(%lu)\n",
            nuid_, pos_.x, pos_.y, pos_.z, player_aoi_->square_id);
    }
  }

  inline void RemoveFromAoi(bool log = false) {
    if (log) {
      printf("Remove Player: %lu, Pos(%f, %f, %f), square_id(%lu)\n",
            nuid_, pos_.x, pos_.y, pos_.z, player_aoi_->square_id);
    }
    aoi_->RemovePlayer(nuid_);
    aoi_ = nullptr;
    player_aoi_ = nullptr;
  }

  inline void MoveDelta(float x, float y, float z) {
    MoveTo(pos_.x + x, pos_.y + y, pos_.z + z);
  }

  inline void MoveTo(float x, float y, float z, bool log = false) {
    pos_.Set(x, y, z);
    SquareId old_square_id = -1, new_square_id = -1;
    if (aoi_) {
      old_square_id = player_aoi_->square_id;
      aoi_->UpdatePos(nuid_, x, y, z);
      new_square_id = player_aoi_->square_id;
    }

    if (log) {
      printf("Player %lu MoveTo (%f, %f, %f), square_id (%lu -> %lu)\n",
            nuid_, x, y, z, old_square_id, new_square_id);
    }

    // BOOST_TEST(new_square_id == PosToId(x, z, aoi_->inverse_square_size_));
  }

 public:
  Nuid nuid_;
  Pos pos_;

  PlayerAoi *player_aoi_;
  SquareAoiTest *aoi_;
};

void PrintAoiUpdateInfos(const AoiUpdateInfos &info) {
  printf("=========================== update_infos\n");
  for (auto &elem : info) {
    auto& update_info = elem.second;
    printf("Player: %lu; update sensors size: %lu\n",
           update_info.nuid, update_info.sensor_update_list.size());
    for (auto &sensor : update_info.sensor_update_list) {
      printf("  Sensor_id: %lu, \n", sensor.sensor_id);

      printf("    enters: (%lu)[", sensor.enters.size());
      for (auto &nuid : sensor.enters) {
        printf("%lu, ", nuid);
      }
      printf("]\n");

      printf("    leaves: (%lu)[", sensor.leaves.size());
      for (auto &nuid : sensor.leaves) {
        printf("%lu,", nuid);
      }
      printf("]\n");
    }
  }
  printf("===========================\n\n");
}

void CheckUpdateInfos(const AoiUpdateInfos &update_infos, const AoiUpdateInfos &require_infos) {
  // BOOST_TEST_REQUIRE((update_infos == require_infos));
  BOOST_TEST_REQUIRE((update_infos.size() == require_infos.size()));
  for (const auto &pair : update_infos) {
    auto &update_info = pair.second;
    BOOST_TEST_REQUIRE((require_infos.find(update_info.nuid) != require_infos.end()));
    auto &require_info = require_infos.find(update_info.nuid)->second;

    BOOST_TEST_REQUIRE(
      (update_info.sensor_update_list.size() == require_info.sensor_update_list.size()));

    for (size_t i =0; i < update_info.sensor_update_list.size(); ++i) {
      auto &sensor_info = update_info.sensor_update_list[i];
      auto &require_sensor_info = require_info.sensor_update_list[i];
      BOOST_TEST_REQUIRE((sensor_info.sensor_id == require_sensor_info.sensor_id));
      BOOST_TEST_REQUIRE((sensor_info.enters == require_sensor_info.enters));
      BOOST_TEST_REQUIRE((sensor_info.leaves == require_sensor_info.leaves));
    }
  }
}

BOOST_AUTO_TEST_CASE(test_suqare_id) {
  Pos pos(0, 0, 0);
  float inverse_square_size = 1/200.;
  BOOST_TEST_REQUIRE((PosToId(pos.x, pos.z, inverse_square_size) == 0UL));
  pos.Set(-1, 0, 0);
  BOOST_TEST_REQUIRE((PosToId(pos.x, pos.z, inverse_square_size) == 18446744069414584320UL));
  pos.Set(0, 0, -1);
  BOOST_TEST_REQUIRE((PosToId(pos.x, pos.z, inverse_square_size) == 4294967295UL));
  pos.Set(-1, 0, -1);
  BOOST_TEST_REQUIRE((PosToId(pos.x, pos.z, inverse_square_size) == 18446744073709551615UL));
}

void TestSimple(bool log = false) {
  SquareAoiTest square_aoi;

  Player player1{GenNuid(), {0, 0, 0}};
  player1.AddToAoi(&square_aoi, log);
  auto sensor_id1 = player1.AddSensor(10, log);

  Player player2{GenNuid(), {0, 0, 0}};
  player2.AddToAoi(&square_aoi, log);
  auto sensor_id2 = player2.AddSensor(5, log);

  auto update_infos = square_aoi.Tick();
  if (log)
    PrintAoiUpdateInfos(update_infos);

  AoiUpdateInfos require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {player2.nuid_}, {}}}}},
      {player2.nuid_, {player2.nuid_, {{sensor_id2, {player1.nuid_}, {}}}}},
  };
  CheckUpdateInfos(update_infos, require_infos);

  // player2 move to (6, 0, 0)
  player2.MoveTo(6, 0, 0, log);

  update_infos = square_aoi.Tick();
  if (log)
    PrintAoiUpdateInfos(update_infos);

  require_infos = {
      {player2.nuid_, {player2.nuid_, {{sensor_id2, {}, {player1.nuid_}}}}},
    };
  CheckUpdateInfos(update_infos, require_infos);

  // player2 move to (600, 0, 100)
  player2.MoveTo(600, 0, 100, log);
  update_infos = square_aoi.Tick();
  if (log)
    PrintAoiUpdateInfos(update_infos);
  require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {}, {player2.nuid_}}}}},
    };
  CheckUpdateInfos(update_infos, require_infos);

  const auto &squares = square_aoi.GetSquares();
  BOOST_TEST_REQUIRE((squares.size() == 2));
  for (const auto &elem : squares) {
    BOOST_TEST_REQUIRE((elem.second.size() == 1));
  }

  player1.MoveTo(601, 100, 101, log);
  update_infos = square_aoi.Tick();
  if (log)
    PrintAoiUpdateInfos(update_infos);
  require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {player2.nuid_}, {}}}}},
      {player2.nuid_, {player2.nuid_, {{sensor_id2, {player1.nuid_}, {}}}}},
  };
  CheckUpdateInfos(update_infos, require_infos);

  player2.RemoveFromAoi(log);
  update_infos = square_aoi.Tick();
  if (log)
    PrintAoiUpdateInfos(update_infos);
  require_infos = {
      {player1.nuid_, {player1.nuid_, {{sensor_id1, {}, {player2.nuid_}}}}},
  };
  CheckUpdateInfos(update_infos, require_infos);
}

BOOST_AUTO_TEST_CASE(test_simple) {
  bool log = false;
  TestSimple(log);
}

std::vector<Player> GenPlayers(const size_t player_num, const float map_size) {
  std::vector<Player> players(player_num);

  boost::random::mt19937 random_generator(std::time(0));
  boost::random::uniform_real_distribution<float> pos_generator(-map_size, map_size);

  for (int i : boost::irange(player_num)) {
    auto &player = players[i];
    player.pos_.Set(pos_generator(random_generator), 0, pos_generator(random_generator));
  }

  BOOST_TEST_REQUIRE((players.size() == player_num));
  return players;
}

std::vector<Pos> GenMovements(const size_t player_num, const float length) {
  std::vector<Pos> movements;
  movements.reserve(player_num);

  boost::random::mt19937 random_generator(std::time(0));
  boost::random::uniform_real_distribution<float> angle_gen(0, 360);
  for (int UNUSED(i) : boost::irange(player_num)) {
    float angle = angle_gen(random_generator);
    float radian = 2 * M_PI * angle / 360;
    movements.emplace_back(std::cos(radian) * length, 0, std::sin(radian) * length);
  }

  return movements;
}

void TestOneMilestone(std::vector<Player> *players, const size_t player_num, const float map_size) {
  printf("\n===Begin Milestore: player_num = %lu, map_size = (%f, %f)\n",
         player_num, -map_size, map_size);

  boost::timer::cpu_timer run_timer;
  int times = 1;
  std::vector<SquareAoiTest> square_aois(times);
  for (auto &square_aoi : square_aois) {
    for (auto &player : *players) {
      player.AddToAoi(&square_aoi);
      player.AddSensor(100);
    }
    BOOST_TEST_REQUIRE((square_aoi.GetPlayerMap().size() == player_num));
  }
  run_timer.stop();
  printf("Add Player (%i times)", times);
  std::cout << run_timer.format();

  for (auto &square_aoi : square_aois) {
    square_aoi.Tick();
  }

  run_timer.start();
  for (auto &square_aoi : square_aois) {
    square_aoi.Tick();
  }
  run_timer.stop();

  printf("Tick (%i times)", times);
  std::cout << run_timer.format();

  float speed = 6;
  float delta_time = 0.1;
  auto movements = GenMovements(player_num, delta_time * speed);
  times = 1 / delta_time;
  run_timer.start();
  for (int UNUSED(t) : boost::irange(times)) {
    for (int i : boost::irange(player_num)) {
      auto &player = players->at(i);
      auto &move = movements[i];
      player.MoveDelta(move.x, move.y, move.z);
    }
  }
  run_timer.stop();
  printf("Update Pos (%i times)", times);
  std::cout << run_timer.format();

  printf("===End Milestore\n");
}

BOOST_AUTO_TEST_CASE(test_milestone) {
  for (size_t player_num : {100, 1000, 10000}) {
    for (float map_size : {50, 100, 1000, 10000}) {
      auto players = GenPlayers(player_num, map_size);
      TestOneMilestone(&players, player_num, map_size);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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 BOOST_UNITS_SI_IO_HPP
#define BOOST_UNITS_SI_IO_HPP

#include <boost/units/io.hpp>
#include <boost/units/reduce_unit.hpp>

#include <boost/units/systems/si.hpp>

namespace boost {

namespace units {

// gray and sievert are indistinguishable
inline std::string name_string(const reduce_unit<si::absorbed_dose>::type&) { return "gray"; }
inline std::string symbol_string(const reduce_unit<si::absorbed_dose>::type&) { return "Gy"; }

// activity and frequency are indistinguishable - would need a "decays" base unit
//inline std::string name_string(const si::activity&) { return "becquerel"; }
//inline std::string symbol_string(const si::activity&) { return "Bq"; }

inline std::string name_string(const reduce_unit<si::capacitance>::type&)   { return "farad"; }
inline std::string symbol_string(const reduce_unit<si::capacitance>::type&) { return "F"; }

inline std::string name_string(const reduce_unit<si::catalytic_activity>::type&) { return "katal"; }
inline std::string symbol_string(const reduce_unit<si::catalytic_activity>::type&) { return "kat"; }

inline std::string name_string(const reduce_unit<si::conductance>::type&) { return "siemen"; }
inline std::string symbol_string(const reduce_unit<si::conductance>::type&) { return "S"; }

// gray and sievert are indistinguishable
//inline std::string name_string(const si::dose_equivalent&) { return "sievert"; }
//inline std::string symbol_string(const si::dose_equivalent&) { return "Sv"; }

inline std::string name_string(const reduce_unit<si::electric_charge>::type&) { return "coulomb"; }
inline std::string symbol_string(const reduce_unit<si::electric_charge>::type&) { return "C"; }

inline std::string name_string(const reduce_unit<si::electric_potential>::type&) { return "volt"; }
inline std::string symbol_string(const reduce_unit<si::electric_potential>::type&) { return "V"; }

inline std::string name_string(const reduce_unit<si::energy>::type&) { return "joule"; }
inline std::string symbol_string(const reduce_unit<si::energy>::type&) { return "J"; }

inline std::string name_string(const reduce_unit<si::force>::type&) { return "newton"; }
inline std::string symbol_string(const reduce_unit<si::force>::type&) { return "N"; }

inline std::string name_string(const reduce_unit<si::frequency>::type&) { return "hertz"; }
inline std::string symbol_string(const reduce_unit<si::frequency>::type&) { return "Hz"; }

inline std::string name_string(const reduce_unit<si::illuminance>::type&) { return "lux"; }
inline std::string symbol_string(const reduce_unit<si::illuminance>::type&) { return "lx"; }

inline std::string name_string(const reduce_unit<si::inductance>::type&) { return "henry"; }
inline std::string symbol_string(const reduce_unit<si::inductance>::type&) { return "H"; }

inline std::string name_string(const reduce_unit<si::luminous_flux>::type&) { return "lumen"; }
inline std::string symbol_string(const reduce_unit<si::luminous_flux>::type&) { return "lm"; }

inline std::string name_string(const reduce_unit<si::magnetic_flux>::type&) { return "weber"; }
inline std::string symbol_string(const reduce_unit<si::magnetic_flux>::type&) { return "Wb"; }

inline std::string name_string(const reduce_unit<si::magnetic_flux_density>::type&) { return "tesla"; }
inline std::string symbol_string(const reduce_unit<si::magnetic_flux_density>::type&) { return "T"; }

inline std::string name_string(const reduce_unit<si::power>::type&) { return "watt"; }
inline std::string symbol_string(const reduce_unit<si::power>::type&) { return "W"; }

inline std::string name_string(const reduce_unit<si::pressure>::type&) { return "pascal"; }
inline std::string symbol_string(const reduce_unit<si::pressure>::type&) { return "Pa"; }

inline std::string name_string(const reduce_unit<si::resistance>::type&) { return "ohm"; }
inline std::string symbol_string(const reduce_unit<si::resistance>::type&) { return "Ohm"; }

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_SI_IO_HPP


#ifndef GLOBAL_GROUP_SPACE_H
#define GLOBAL_GROUP_SPACE_H

#include <fstream>
#include <vector>

#include <boost/serialization/serialization.hpp>
#include <boost/serialization/split_member.hpp>

#include "group/LocalLengthGroupSpace.hpp"
#include "TimeSeries.hpp"
#include "TimeSeriesSet.hpp"
#include "distance/Distance.hpp"
#include "group/Group.hpp"

namespace genex {

vector<int> generateTraverseOrder(int queryLength, int totalLength);

/**
 *  The set of all groups of equal lengths for a dataset
 */
class GlobalGroupSpace
{
public:

  /**
   *  @brief The constructor for the GlobalGroupSpace
   *
   *  @param dataset the dataset to group
   *  @param threshold the threshold for similarity within a group
   */
  GlobalGroupSpace(const TimeSeriesSet& dataset);

  /**
   *  @brief The deconstructor for the GlobalGroupSpace
   */
  ~GlobalGroupSpace();

  /**
   *  @brief clears the groups
   */
  void reset();

  /**
   *  @brief groups the dataset into groups of equal length
   *    using the metric to determine similarity
   *
   *  @param metric the metric used to group by
   *  @param threshold the threshold to be group with
   *  @param wholeSeriesOnly if set to true group the largest length only
   *  @return the number of groups it creates
   */
  int group(
    const std::string& distance_name, data_t threshold, bool wholeSeriesOnly=false);
  int groupMultiThreaded(
    const std::string& distance_name, data_t threshold, int num_thread, bool wholeSeriesOnly=false);

  int getTotalNumberOfGroups() const;
  std::string getDistanceName() const;
  data_t getThreshold() const;

  /**
   *  @brief gets the most similar sequence in the dataset
   *
   *  @param query gets most similar sequence to the query
   *  @return the best match in the dataset
   */
  candidate_time_series_t getBestMatch(const TimeSeries& query);

  /**
   *  @brief find k similar time series to the query
   *
   *  @param query gets most similar sequence to the query
   *  @param k number of similar time series
   *  @param approx if true, return the approximated distance, otherwise return the exact distance
   *  @return the best match in the dataset
   */
  std::vector<candidate_time_series_t> getKBestMatches(const TimeSeries& query, int k);

  void saveGroupsOld(std::ofstream &fout, bool groupSizeOnly) const;
  int loadGroupsOld(std::ifstream &fin);

private:
  std::string distanceName;
  std::vector<LocalLengthGroupSpace*> localLengthGroupSpace;
  const TimeSeriesSet& dataset;
  dist_t pairwiseDistance;
  dist_t warpedDistance;
  data_t threshold;
  int totalNumberOfGroups = 0;
  bool wholeSeriesOnly = false;
  void _loadDistance(const std::string& distanceName);
  int _group(int i);
  int _getMinLength() const;

  /*************************
   *  Start serialization
   *************************/
  friend class boost::serialization::access;
  template<class A>
  void save(A & ar, unsigned) const
  {
    size_t maxLen = this->localLengthGroupSpace.size();
    size_t minLen = _getMinLength();
    ar << minLen << maxLen << this->distanceName << this->threshold;
    for (auto i = minLen; i < maxLen; i++) {
      ar << *(this->localLengthGroupSpace[i]);
    }
  }

  template<class A>
  void load(A & ar, unsigned)
  {
    size_t maxLen, minLen;
    ar >> minLen >> maxLen >> this->distanceName >> this->threshold;
    this->_loadDistance(this->distanceName);
    this->localLengthGroupSpace.resize(dataset.getMaxLength() + 1, nullptr);
    this->totalNumberOfGroups = 0;
    for (auto i = minLen; i < maxLen; i++) {
      auto llgs = new LocalLengthGroupSpace(dataset, i);
      ar >> *llgs;
      this->localLengthGroupSpace[i] = llgs;
      this->totalNumberOfGroups += llgs->getNumberOfGroups();
    }
  }

  BOOST_SERIALIZATION_SPLIT_MEMBER()

  /*************************
   *  End serialization
   *************************/
};

} // namespace genex
#endif //GLOBAL_GROUP_SPACE_H


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef LIBP2P_SECIO_DIALER_HPP
#define LIBP2P_SECIO_DIALER_HPP

#include <boost/optional.hpp>
#include <libp2p/connection/raw_connection.hpp>
#include <libp2p/crypto/common.hpp>
#include <libp2p/crypto/key.hpp>
#include <libp2p/crypto/key_marshaller.hpp>
#include <libp2p/outcome/outcome.hpp>
#include <libp2p/security/secio/exchange_message.hpp>
#include <libp2p/security/secio/propose_message.hpp>
#include <libp2p/security/secio/propose_message_marshaller.hpp>

namespace libp2p::basic {
  class ProtobufMessageReadWriter;
}

namespace libp2p::security::secio {

  /// Helper class for establishing SECIO connection
  class Dialer {
   public:
    /// protobuf messages read-writer in/to the wire
    const std::shared_ptr<libp2p::basic::ProtobufMessageReadWriter> rw;

    enum class Error {
      INTERNAL_FAILURE = 1,
      PEER_COMMUNICATING_ITSELF,
      NO_COMMON_EC_ALGO,
      NO_COMMON_CIPHER_ALGO,
      NO_COMMON_HASH_ALGO,
    };

    struct Algorithm {
      crypto::common::CurveType curve;
      crypto::common::CipherType cipher;
      crypto::common::HashType hash;
    };

    Dialer(std::shared_ptr<connection::RawConnection> connection);

    /// Stores byte-exact copy of SECIO proposal sent to remote peer
    void storeLocalPeerProposalBytes(
        const std::shared_ptr<std::vector<uint8_t>> &bytes);

    /// Stores byte-exact copy of SECIO proposal received from remote peer
    void storeRemotePeerProposalBytes(
        const std::shared_ptr<std::vector<uint8_t>> &bytes);

    /// Stores ephemeral keypair for further computations
    void storeEphemeralKeypair(crypto::EphemeralKeyPair keypair);

    /**
     * Stores a pair of stretched keys to assign them later to local and remote
     * peers
     */
    void storeStretchedKeys(
        std::pair<crypto::StretchedKey, crypto::StretchedKey> keys);

    /**
     * Produces a corpus to be signed during SECIO keys exchange phase.
     * @param for_local_peer - true if the corpus is going to be signed by local
     * peer, false - remote peer
     * @param ephemeral_public_key - epubkey bytes of the corresponding peer
     * (local or remote)
     * @return byte sequence or an error if happened
     */
    outcome::result<std::vector<uint8_t>> getCorpus(
        bool for_local_peer,
        gsl::span<const uint8_t> ephemeral_public_key) const;

    /**
     * Computes which cipher, hash, and ec-curve to use considering the
     * knowledge which peer set is preferred.
     * @param local - propose message sent to the remote peer
     * @param remote - propose message received from the remote peer
     * @return a structure with chosen algorithms or an error if happened
     */
    outcome::result<Algorithm> determineCommonAlgorithm(
        const ProposeMessage &local, const ProposeMessage &remote);

    /// Returns common ec-curve type if already defined
    outcome::result<crypto::common::CurveType> chosenCurve() const;

    /// Returns common cipher algorithm if already defined
    outcome::result<crypto::common::CipherType> chosenCipher() const;

    /// Returns common hash algorithm if already defined
    outcome::result<crypto::common::HashType> chosenHash() const;

    /**
     * Retrieves public key of a remote peer in an unmarshalled form.
     * @param key_marshaller to unmarshal key bytes
     * @param propose_marshaller to unmarshal remote peer's proposal message
     * bytes
     * @return remote peer public key or an error if happened
     */
    outcome::result<crypto::PublicKey> remotePublicKey(
        const std::shared_ptr<crypto::marshaller::KeyMarshaller>
            &key_marshaller,
        const std::shared_ptr<ProposeMessageMarshaller> &propose_marshaller)
        const;

    /// Computes shared secret via ec-cryptography
    outcome::result<crypto::Buffer> generateSharedSecret(
        crypto::Buffer remote_ephemeral_public_key) const;

    /// Defines which stretched key is for the local peer
    outcome::result<crypto::StretchedKey> localStretchedKey() const;

    /// Defines which stretched key is for the remote peer
    outcome::result<crypto::StretchedKey> remoteStretchedKey() const;

   private:
    /**
     * Computes which peer settings are preferred.
     * @return true if local's peer settings are preferred, false - otherwise
     */
    static outcome::result<bool> determineRoles(const ProposeMessage &local,
                                                const ProposeMessage &remote);

    /**
     * Determine common set of settings considering given peers' preference.
     * @return chosen algorithms structure if successful
     */
    static outcome::result<Algorithm> findCommonAlgo(
        const ProposeMessage &local, const ProposeMessage &remote,
        bool local_peer_is_preferred);

    boost::optional<std::vector<uint8_t>> local_peer_proposal_bytes_;
    boost::optional<std::vector<uint8_t>> remote_peer_proposal_bytes_;
    boost::optional<Algorithm> chosen_algorithm_;
    boost::optional<bool> local_peer_is_preferred_;
    boost::optional<crypto::EphemeralKeyPair> ekey_pair_;
    boost::optional<std::pair<crypto::StretchedKey, crypto::StretchedKey>>
        stretched_keys_;
  };
}  // namespace libp2p::security::secio

OUTCOME_HPP_DECLARE_ERROR(libp2p::security::secio, Dialer::Error);

#endif  // LIBP2P_SECIO_DIALER_HPP


//#pragma once

// Including basic libraries
#include <iostream>
#include <ostream>
#include <functional>
#include <math.h>
#include <cmath>
#include <vector>
#include <memory>
#include <Python.h>
#include <stdexcept>
#include <string>
#include <Python.h>
#include <numeric>
#include <Eigen/Dense>

// Including gtsam stuff
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>

// Including boost python to interface with python
#define BOOST_PYTHON_STATIC_LIB
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/python/stl_iterator.hpp>

// Include Header
#include "LambdaPriorFactor2D.h"

// Function to convert a std::vector vector to gtsam::Vector
gtsam::Vector gtsam_vector_from_vector(std::vector<double> vec_vec)
{
    gtsam::Vector vec = gtsam::Vector::Map(vec_vec.data(), vec_vec.size());
    return vec;
}

// Point 2 from a vector; necessary for the entire thing to work
gtsam::Point2 gtsam_point2_from_vector(std::vector<double> vec_vec)
{
    gtsam::Point2 point_lambda = gtsam::Point2(vec_vec[0], vec_vec[1]);
    return point_lambda;
}

// Convert py objects to vectors
std::vector<double> listTupleToVector_Double(PyObject* incoming) {
	std::vector<double> data;
	if (PyTuple_Check(incoming)) {
		for(Py_ssize_t i = 0; i < PyTuple_Size(incoming); i++) {
			PyObject *value = PyTuple_GetItem(incoming, i);
			data.push_back( PyFloat_AsDouble(value) );
		}
	} else {
		if (PyList_Check(incoming)) {
			for(Py_ssize_t i = 0; i < PyList_Size(incoming); i++) {
				PyObject *value = PyList_GetItem(incoming, i);
				data.push_back( PyFloat_AsDouble(value) );
			}
		} else {
			throw std::logic_error("Passed PyObject pointer was not a list or tuple!");
		}
	}
	return data;
}

// Convert C++ vectors to lists
PyObject* vectorToList_Double(const std::vector<double> &data) {
  PyObject* listObj = PyList_New( data.size() );
	if (!listObj) throw std::logic_error("Unable to allocate memory for Python list");
	for (unsigned int i = 0; i < data.size(); i++) {
		PyObject *num = PyFloat_FromDouble(data[i]);
		if (!num) {
			Py_DECREF(listObj);
			throw std::logic_error("Unable to allocate memory for Python list");
		}
		PyList_SET_ITEM(listObj, i, num);
	}
	return listObj;
}

// Dot multiplication function
double dot_multiplication(std::vector<double> v_1, std::vector<double> v_2)
{
    double product = 0.0;
    int size_v_1 = v_1.size();
    if (v_1.size() != v_2.size())
        throw std::logic_error("Vectors are not at the same size");
    for (int i = 0; i != size_v_1; i++)
    {
        product = product + v_1[i] * v_2[i];
    }
    return product;
}

// Matrix/vector multiplication
std::vector<double> mat_vec_multiplication(std::vector<std::vector<double> > mat, std::vector<double> vec)
{
    int vec_length = vec.size();
    int mat_x_size = mat.size();
    int mat_y_size = mat[0].size();
    if (vec_length != mat_y_size)
        throw std::logic_error("Vector and matrix dimensions do not agree");

    //
    std::vector<double> mult_vector;
    double temp = 0.0;
    for (int i = 0; i != mat_x_size; i++)
    {
        temp = 0.0;
        for (int j = 0; j != mat_y_size; j++)
        {
            temp += mat[i][j] * vec[j];
        }
        mult_vector.push_back(temp);
    }

    return mult_vector;
}

// Fix angles to be between -pi and pi
double angle_fix(double angle)
{
    while (angle > M_PI)
    {
        angle -= 2 * M_PI;
    }
    while (angle < -M_PI)
    {
        angle += 2 * M_PI;
    }
    return angle;
}

// FACTOR CLASS ---------------------------------------------------------------------

// Functions to convert Pose to std::vector. Necessary to talk with Libtorch because it refuses directly to talk with gtsam
std::vector<double> Pose_to_vector(gtsam::Pose2 pose)
{
    return {pose.x(), pose.y(), pose.theta()};
}

std::vector<double> Pose_to_vector(gtsam::Pose3 pose)
{
    return {pose.x(), pose.y(), pose.z(), pose.rotation().yaw(), pose.rotation().pitch(), pose.rotation().roll()};
}

// Function to convert a std::vector<vector> matrix to gtsam::Matrix
gtsam::Matrix gtsam_matrix_from_vector(std::vector<std::vector<double> > vec_mat)
{
    int rows = vec_mat.size();
    int cols = vec_mat[0].size();
    gtsam::Matrix mat; mat.setZero(rows, cols);

    for (int i = 0; i != rows; i++)
    {
        for (int j = 0; j != cols; j++)
        {
            mat(i,j) = vec_mat[i][j];
        }
    }
    return mat;
}

std::vector<double> Reverse_pose2(std::vector<double> pose2)
{
    return {-pose2[0], -pose2[1], pose2[2] + 3.1415};
}

std::vector<double> Reverse_pose3(std::vector<double> pose3)
{
    return {-pose3[0], -pose3[1], -pose3[2], pose3[3] + 3.1415, pose3[4] + 3.1415, pose3[5] + 3.1415};
}

// FACTOR CLASS ---------------------------------------------------------------------

using boost::assign::cref_list_of;
//namespace LambdaPriorFactor

namespace gtsam
{

    //Constructor
LambdaPriorFactor::LambdaPriorFactor(const Key& plambda_key, const Vector& plambda_exp, const SharedNoiseModel& plambda_cov):
    Base(cref_list_of<1>(plambda_key)),
    prior_lambda_vec(plambda_exp),
    noise_model(plambda_cov) {}

//Unwhitened error function;
Vector LambdaPriorFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const
{
    Vector plambda = x.at<Vector>(keys_[0]);

    std::vector<double> err;
//        for (int i=0; i != prior_lambda_vec.size() ; i++ )
//        {
//            err.push_back( -prior_lambda_vec[i] + plambda(i));
//        }
    err.push_back( -prior_lambda_vec[0] + plambda(0));
    err.push_back( -prior_lambda_vec[1] + plambda(1));
    //double R_dim = prior_lambda_vec.size();
    double R_dim = 2;
    Vector err_gtsam = gtsam_vector_from_vector(err);

    Matrix H_1_t;
    H_1_t.setZero(R_dim, R_dim);

    for (int i=0 ; i != R_dim ; i++ )
    {
        H_1_t(i, i) = 1;
    }

    if (H)
    {
        (*H)[0].resize(R_dim, R_dim);
        (*H)[0] << H_1_t;
    }

    return err_gtsam;
}

//Whitened error function;
Vector LambdaPriorFactor::whitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const
{
    Vector plambda = x.at<Vector>(keys_[0]);

    //double R_dim = prior_lambda_vec.size();
    //double R_dim = 2;

    Vector error = unwhitenedError(x, H);

    if (H)
    {
        noise_model->WhitenSystem(*H, error);
    }
    else
    {
        noise_model->whitenInPlace(error);
    }

    return error;
}

//Necessary for some reason
size_t LambdaPriorFactor::dim() const { return 1; }

//Total error
double LambdaPriorFactor::error(const Values& x) const {
    return whitenedError(x).squaredNorm();
}

// Linearization function; (do we need it?)
boost::shared_ptr<GaussianFactor> LambdaPriorFactor::linearize(const Values& x) const
{
// Only linearize if the factor is active
    if (!this->active(x))
        return boost::shared_ptr<gtsam::JacobianFactor>();

    Matrix A1;
    std::vector<Matrix> A(this->size());
    Vector b = -whitenedError(x, A);
    A1 = A[0];

    bool constrained = false;
            if(constrained)
                return GaussianFactor::shared_ptr(
                        new JacobianFactor(keys_[0], A1, b, noiseModel::Constrained::All(1)));
            else
                return GaussianFactor::shared_ptr(
                        new JacobianFactor(keys_[0], A1, b, noiseModel::Unit::Create(b.size())));

}

//Print function
void LambdaPriorFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const
{
    Base::print(s + "LambdaPriorFactor", keyFormatter);
    std::cout << "Prior Lambda: ( ";
    for( unsigned int i = 0 ; i != prior_lambda_vec.size() ; i++ )
        std::cout << prior_lambda_vec[i] << " ";
    std::cout << ")\n";
}

// @return a deep copy of this factor
NonlinearFactor::shared_ptr LambdaPriorFactor::clone() const
{
  return boost::static_pointer_cast<NonlinearFactor>(
      NonlinearFactor::shared_ptr(new This(*this)));
}

}


#include "verticalgazelearner.h"
#include "eyepatcher.h"

#include <boost/lexical_cast.hpp>

using namespace std;

VerticalGazeLearner::VerticalGazeLearner(TrainingParameters& params) : AbstractLearner(params)
{

}

VerticalGazeLearner::~VerticalGazeLearner()
{

}

void VerticalGazeLearner::loadClassifier(const string &filename)
{
    _loadClassifier(filename, learned_function);
}

boost::optional<dlib::matrix<double,0,1>> VerticalGazeLearner::getFeatureVector(GazeHyp& ghyp) {
    auto result = boost::optional<dlib::matrix<double,0,1>>();
    if (ghyp.lidFeatures.size() && ghyp.faceFeatures.size() && ghyp.vertGazeFeatures.size()) {
        switch (trainParams.featureSet.get_value_or(FeatureSetConfig::ALL)) {
        case FeatureSetConfig::ALL:
            result = dlib::join_cols(dlib::join_cols(ghyp.vertGazeFeatures, ghyp.faceFeatures), ghyp.eyeHogFeatures);
            break;
        case FeatureSetConfig::POSREL:
            result = dlib::join_cols(ghyp.vertGazeFeatures, ghyp.faceFeatures);
            break;
        case FeatureSetConfig::RELATIONAL:
            result = ghyp.vertGazeFeatures;
            break;
        case FeatureSetConfig::HOG:
            result = ghyp.eyeHogFeatures;
            break;
        case FeatureSetConfig::HOGREL:
            result = dlib::join_cols(ghyp.vertGazeFeatures, ghyp.eyeHogFeatures);
            break;
        case FeatureSetConfig::HOGPOS:
            result = dlib::join_cols(ghyp.faceFeatures, ghyp.eyeHogFeatures);
            break;
        case FeatureSetConfig::POSITIONAL:
            result = ghyp.faceFeatures;
            break;
        default:
            throw std::runtime_error("feature-set not implemented");
        }
    }
    return result;

}

void VerticalGazeLearner::classify(GazeHyp &ghyp)
{
    _classify(ghyp, learned_function, ghyp.verticalGazeEstimation);
    //cerr << ghyp.verticalGazeEstimation.get() << endl;
}

void VerticalGazeLearner::train(const string &outfilename)
{
    dlib::svr_trainer<kernel_type> trainer;
    _train(outfilename, learned_function, trainer);

}

void VerticalGazeLearner::visualize(GazeHyp &ghyp, double mutualGazeTolerance)
{
    if (!ghyp.verticalGazeEstimation.is_initialized()) return;
    double relativeGazeEst = ghyp.verticalGazeEstimation.get();
    if (!std::isfinite(relativeGazeEst)) return;
    auto fr = ghyp.pupils.faceRect();
    double angle = -relativeGazeEst;
    double length = fr.width/3;
    cv::Point p1 (fr.tl().x + fr.width / 2, fr.tl().y);
    cv::Point p2;
    p2.x = round(p1.x + length * cos(angle * CV_PI / 180.0));
    p2.y = round(p1.y + length * sin(angle * CV_PI / 180.0));
    cv::line(ghyp.parentHyp.frame, p1, p2, cv::Scalar(255, 0, 0), 2, 'A');
    //cv::line(ghyp.parentHyp.frame, p1, p2, cv::Scalar(150, 250, 150), 2, 'A');

    cv::Mat facereg = ghyp.pupils.faceRegion();
    int limita = facereg.rows*(0.5-mutualGazeTolerance/90.0);
    int limitb = facereg.rows*(0.5+mutualGazeTolerance/90.0);
    cv::rectangle(facereg, cv::Rect(cv::Point(0, limita-1), cv::Size(14, 2)), cv::Scalar(255, 100, 100), -1, 'A');
    cv::rectangle(facereg, cv::Rect(cv::Point(0, limitb-1), cv::Size(14, 2)), cv::Scalar(255, 100, 100), -1, 'A');
    int offset = facereg.rows*(0.5-relativeGazeEst/90.0);
    cv::rectangle(facereg, cv::Rect(cv::Point(0, offset-3), cv::Size(10, 6)), cv::Scalar(255, 0, 0), -1, 'A');
}

string VerticalGazeLearner::getId()
{
    return "VerticalGazeLearner";
}


#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/Point32.h"
#include "sensor_msgs/PointCloud.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <Eigen/Dense>
#include <chrono>
#include <iostream>
#include <vector>
using namespace std;
using namespace cv;

/** 参数定义 **/
const float fx = 760.4862674784594; // 相机内参
const float fy = 761.4971958529285;
const float cx = 631.6715834996345;
const float cy = 329.3054436037627;

const float y = 129.5585;   // 固定相机离地面高度，单位mm，通过计算得到，不一定准

float offset;
float thetaX;
float thetaY;
float scale;
int imgWidth;  // 图片大小
int imgHeight;
float up_rate;
float down_rate;
int sizeThres;

ros::Publisher pathpub;
ros::Publisher leftpoint;
ros::Publisher rightpoint;

/** 将图片中的 点uv 转化为 现实坐标中的 点xy **/
cv::Point3f getXYZ(cv::Point2f& point){
    cv::Point3f point3d;
    point3d.x = (fy * y/(point.y-cy))/scale;
    point3d.y = (-(point.x-cx)*point3d.x/fx);
    point3d.z = 0.0;
    // std::cout<<point3d<<std::endl;
    return point3d;
}

/** 将二维边线转化为三维边线 **/
void transTo3D(vector<cv::Point2f>& uv,vector<cv::Point3f>& posearray){
    for(auto p:uv){
        cv::Point3f pose = getXYZ(p);
        posearray.push_back(pose);
    }
}

/** 配合方法一、二：处理单边情况 **/
void pubMiddleline(vector<cv::Point2f>& uv,bool is_left){
    vector<cv::Point3f> xy;
    sensor_msgs::PointCloud path;
    transTo3D(uv,xy);
    // path.points.resize(xy.size());
    int i = 0;
    for(auto p:xy){
        if(is_left){
            geometry_msgs::Point32 point;
            point.x = p.x;
            point.y = p.y - offset;
            point.z = 0.0;
            path.points.push_back(point);
        }
        else{
            geometry_msgs::Point32 point;
            point.x = p.x;
            point.y = p.y + offset;
            point.z = 0.0;
            path.points.push_back(point);

        }

    }
    ROS_INFO("pathpose:%d",path.points.size());
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "camera";
    pathpub.publish(path);
    ROS_INFO("publish successfully");
}

/** 方法四：混合处理不分单双边线 **/
void pubMiddlelineCombine(vector<cv::Point2f>& uvLeft,vector<cv::Point2f>& uvRight){

    // 1.将数据转化为三维坐标系下
    vector<cv::Point3f> xyLeft,xyRight;
    ROS_INFO("int two middle");
    transTo3D(uvLeft,xyLeft);
    transTo3D(uvRight,xyRight);

    // 2.参数定义
    int r = 0;
    int l = 0;
    float max_distance = 0;
    sensor_msgs::PointCloud pc;
    pc.header.stamp = ros::Time::now();
    pc.header.frame_id = "camera";

    float lastparallel = 0; // 记录上一点的横向坐标
    bool init = 0;

    // 计算中线，只要有一边线的点都用完了就退出循环
    while (l<xyLeft.size() && r<xyRight.size())
    {
        // ROS_INFO("l: %d, r:%d",l,r);
        // 1.初始化
        if(!init){
            lastparallel =( xyLeft[l].y + xyRight[r].y )/2;
            init = 1;
            continue;
        }
        // 2.两边线纵向距离相等时
        if(xyLeft[l].x == xyRight[r].x){
            if(xyLeft[l].x > max_distance)
                max_distance = xyLeft[l].x;
            float tmp = ( xyLeft[l].y + xyRight[r].y )/2;
            if(abs(tmp - lastparallel)>0.1){// 与上一点横坐标的偏差
                l++;
                r++;
                continue;
            }
            geometry_msgs::Point32 p;
            p.x = xyLeft[l].x;
            p.y = tmp;
            p.z = 0.0;
            pc.points.push_back(p);
            l++;
            r++;
            lastparallel = tmp;
        }
        // 3.左边线纵向距离大于右边线
        else if(xyLeft[l].x > xyRight[r].x){
            if(xyLeft[l].x > max_distance)
                max_distance = xyLeft[l].x;
            float tmp = xyLeft[l].y - offset;
            // cout<<"LEFT:"<<tmp<<endl;

            if(abs(tmp - lastparallel)>0.1){
                l++;
                continue;
            }
            geometry_msgs::Point32 p;
            p.x = xyLeft[l].x;
            p.y = tmp;
            p.z = 0.0;
            pc.points.push_back(p);
            l++;
            lastparallel = tmp;
        }
        // 4.右边线纵向距离大于左边线
        else{
            if(xyRight[l].x > max_distance)
                max_distance = xyRight[l].x;
            // cout<<"RIGHT:"<<xyRight[r].y<<endl;
            float tmp = xyRight[r].y + offset;
            // cout<<"RIGHT:"<<tmp<<endl;
            if(abs(tmp - lastparallel)>0.1){
                r++;
                continue;
            }
            geometry_msgs::Point32 p;
            p.x = xyRight[r].x;
            p.y = tmp;
            p.z = 0.0;
            pc.points.push_back(p);
            r++;
            lastparallel = tmp;
        }

    }
    // ROS_INFO("OUT1");
    // 将另一边剩下的轨迹点加入
    // while (l<xyLeft.size())
    // {
    //     if(xyLeft[l].x > max_distance)
    //         max_distance = xyLeft[l].x;
    //     float tmp = xyLeft[l].y - offset;

    //     geometry_msgs::Point32 p;
    //     p.x = xyLeft[l].x;
    //     p.y = tmp;
    //     p.z = 0.0;
    //     pc.points.push_back(p);
    //     l++;
    // }

    // while (r<xyRight.size())
    // {
    //     if(xyRight[r].x > max_distance)
    //         max_distance = xyRight[r].x;
    //     // cout<<"RIGHT BEFORT:"<<xyRight[r].y<<endl;
    //     float tmp = xyRight[r].y + offset;
    //     // cout<<"RIGHT AFTER:"<<tmp<<endl;
    //     geometry_msgs::Point32 p;
    //     p.x = xyRight[r].x;
    //     p.y = tmp;
    //     p.z = 0.0;
    //     pc.points.push_back(p);
    //     r++;
    // }
    // ROS_INFO("OUT2");
    ROS_INFO("MAX DISTANCE %f",max_distance);

    pathpub.publish(pc);
}

bool humanline_detection(Mat& binary, Mat& RGB, int imgHeight, int imgWeight) {
    int jump = 1;
    int begin_imgHeight = imgHeight * 3.0 / 5;
    int end_imgHeight = imgHeight * 4.0 / 5;
    int end_imgWeight = imgWeight * 3.0 / 4;
    int all_point = 0;
    int white_point = 0;
    for (int x = begin_imgHeight; x < end_imgHeight; x+=jump) {
        for (int y = 0; y < end_imgWeight; y+=jump) {
            all_point++;
            if (RGB.at<Vec3b>(x, y)[0] > 230 && RGB.at<Vec3b>(x, y)[1] > 230 && RGB.at<Vec3b>(x,y)[2] > 230) {
                white_point++;
            }
        }
    }
    cout<< "rate:"<< (float)white_point / all_point<<"   ";
    //return (float)white_point / all_point;
    if ((float)white_point / all_point > 0.15)
        return true;
    return false;
}
void remove_white(Mat& binary, Mat& RGB, int imgHeight, int imgWeight) {
    int begin_imgHeight = (int)(imgHeight / 2.0);
    for (int x = begin_imgHeight; x < imgHeight; x++) {
        uchar* data = binary.ptr<uchar>(x);
        for (int y = 0; y < imgWeight; y++) {
            if (RGB.at<Vec3b>(x, y)[0] > 230 && RGB.at<Vec3b>(x, y)[1] > 230 && RGB.at<Vec3b>(x, y)[2] > 230) {
                data[y] = 0;
            }
        }
    }
    return;
}

int main(int argc ,char **argv){
    ros::init(argc,argv,"linktracking");
    ros::NodeHandle nh("~");
    pathpub = nh.advertise<sensor_msgs::PointCloud>("path",1);
    leftpoint = nh.advertise<sensor_msgs::PointCloud>("leftpath",1);
    rightpoint = nh.advertise<sensor_msgs::PointCloud>("rightpath",1);
    string video;
    nh.param<string>("video",video,"../dataset/test.mp4");
    nh.param<float>("offset",offset,0.45);
    nh.param<float>("thetaX",thetaX,0.2);
    nh.param<float>("thetaY",thetaY,0.1);
    nh.param<float>("scale",scale,1000.0);
    nh.param<int>("imgWidth",imgWidth,1280);
    nh.param<int>("imgHeight",imgHeight,720);
    nh.param<float>("up_rate",up_rate,0.5);
    nh.param<float>("down_rate",down_rate,0.9);
    nh.param<int>("size_thres",sizeThres,10);

    ROS_INFO("offset : %f",offset);
    ROS_INFO("thetaX : %f",thetaY);
    ROS_INFO("thetaY : %f",thetaX);
    ROS_INFO("scale : %f",scale);
    ROS_INFO("imgWidth : %d",imgWidth);
    ROS_INFO("imgHeight : %d",imgHeight);

    /** 1.获得视频数据 **/
    cv::VideoCapture cap = cv::VideoCapture(video);
    cap.set(3, imgWidth);//CV_CAP_PROP_FRAME_WIDTH
    cap.set(4, imgHeight);//CV_CAP_PROP_FRAME_HEIGHT
    cap.set(cv::CAP_PROP_FPS,30);

    ROS_INFO("%s",video.c_str());
    cv::Mat frame;
    cv::Mat gray;
    cv::Mat binary;
    bool is_find_human_line = true;

    if(!cap.isOpened()){
        ROS_INFO("Don't load the Image!");
        return 1;
    }

    int left_num=1,right_num=1;
    while (ros::ok()&&cap.isOpened()) {
        /** 2.获得帧并二值化 **/
        cap.read(frame);
        if(!frame.data){
            ROS_INFO("Don't get this the frame");
            continue;
        }

        // method 2:自适应二值化
        cv::cvtColor(frame, gray, 7);//CV_RGB2GRAY
        cv::threshold(gray, binary, 150, 255, 0);//CV_THRESH_BINARY
        if(!is_find_human_line)
            remove_white(binary,frame,imgHeight,imgWidth);
        cv::erode(binary, binary, cv::Mat(), cv::Point(-1, -1), 2);
        cv::dilate(binary,binary,11);
        if(is_find_human_line){
            if(humanline_detection(binary,frame,imgHeight,imgWidth)){
                is_find_human_line = false;
            }

        }

        /** 3.获得两边轨迹点 **/

        int vline = binary.cols / 2;
        //动态中线
        int base_vline = frame.cols / 2;
        int float_range = frame.cols / 4;
        float float_rate = ((long)left_num - right_num) / ((long)left_num + right_num + 0.1);
        vline = (int)(base_vline + float_range * float_rate);

        vector<cv::Point2f> uvRight;
        vector<cv::Point2f> uvLeft;
        // right side
        for (int i = int(imgHeight*up_rate); i < int(imgHeight*down_rate)-1; ++i) {
            uchar *data = binary.ptr<uchar>(i);
            for (int j = vline; j < binary.cols - 1; ++j) {
                if (data[j] == 0 && data[j + 1] == 255) {
                    right_num++;
                    for (int tmp = 0; tmp < 10; tmp++) {
                        frame.at<Vec3b>(i, j - tmp)[0] = 0;
                        frame.at<Vec3b>(i, j - tmp)[1] = 0;
                        frame.at<Vec3b>(i, j - tmp)[2] = 255;
                        }
                    uvRight.push_back(cv::Point2f(j, i));
                    break;
                }
            }
       }
        // left side
        for (int i = int(imgHeight*up_rate); i < int(imgHeight*down_rate)-1; ++i) {
            uchar *data = binary.ptr<uchar>(i);
            for (int j = vline; j > 0; --j) {
                if (data[j] == 0 && data[j - 1] == 255) {
                    left_num++;
                    for (int tmp = 0; tmp < 10; tmp++) {
                        frame.at<Vec3b>(i, j + tmp)[0] = 0;
                        frame.at<Vec3b>(i, j + tmp)[1] = 255;
                        frame.at<Vec3b>(i, j + tmp)[2] = 0;
                    }
                    uvLeft.push_back(cv::Point2f(j, i));
                    break;
                }
            }
        }
        //绘制中线
        for (int i = int(imgHeight * up_rate); i < int(imgHeight * down_rate) - 1; ++i) {
            for (int tmp = -1; tmp < 2; tmp++) {
                frame.at<Vec3b>(i, vline+tmp)[0] = 0;
                frame.at<Vec3b>(i, vline+tmp)[1] = 255;
                frame.at<Vec3b>(i, vline+tmp)[2] = 255;
            }

        }
        //绘制基本中线
        for (int i = int(imgHeight * up_rate); i < int(imgHeight * down_rate) - 1; ++i) {
            frame.at<Vec3b>(i, base_vline)[0] = 128;
            frame.at<Vec3b>(i, base_vline)[1] = 128;
            frame.at<Vec3b>(i, base_vline)[2] = 128;
        }

        cv::imshow("frame",binary);
        cv::waitKey(0);

        /** 4.绘制两边线 **/
        ROS_INFO("right.size = %d,left.size = %d",uvRight.size(),uvLeft.size());

        if(uvRight.size()>sizeThres && uvLeft.size()>sizeThres){
            pubMiddlelineCombine(uvLeft,uvRight);
            ROS_INFO("combine");
        }
        else if(uvRight.size()>=uvLeft.size()&&uvRight.size()>sizeThres){
            pubMiddleline(uvRight,0);
            ROS_INFO("right");
        }
        else if(uvLeft.size()>uvRight.size()&&uvLeft.size()>sizeThres){
            pubMiddleline(uvLeft,1);
            ROS_INFO("left");
        }

    }
    cv::destroyAllWindows();
    cap.release();
    return 0;
}


/*******************************************************************************
Copyright(c) 2015-2021 Parker Hannifin Corp. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/Wireless/WirelessNode.h"
#include "mscl/MicroStrain/Wireless/BaseStation.h"
#include "mscl/Utils.h"
#include "mscl/MicroStrain/Wireless/Features/NodeFeatures.h"
#include "mscl/MicroStrain/Wireless/Configuration/WirelessNodeConfig.h"
#include "mscl/MicroStrain/Wireless/Configuration/EepromLocation.h"
#include "mscl/MicroStrain/Wireless/Features/ChannelGroup.h"

#include <boost/test/unit_test.hpp>
#include "mock_BaseStation.h"
#include "mock_WirelessNode.h"
using namespace mscl;

BOOST_AUTO_TEST_SUITE(ChannelGroup_Test)

NodeInfo createInfo(WirelessModels::NodeModel node)
{
    return NodeInfo(Version(9, 9), node, WirelessTypes::region_usa);
}

BOOST_AUTO_TEST_CASE(ChannelGroup_channelGroups)
{
    std::shared_ptr<NodeFeatures> sglink = NodeFeatures::create(createInfo(WirelessModels::node_sgLink));
    mscl::ChannelGroups groups = sglink->channelGroups();

    BOOST_CHECK_EQUAL(groups.size(), 4);
    BOOST_CHECK_EQUAL(groups.at(0).channels().toMask(), 1);    //differential channels
    BOOST_CHECK_EQUAL(groups.at(0).name(), "Differential (ch1)");
    auto diffSettings = groups.at(0).settings();
    BOOST_CHECK_EQUAL(diffSettings.size(), 4);
    BOOST_CHECK_EQUAL(diffSettings.at(0), WirelessTypes::chSetting_inputRange);
    BOOST_CHECK_EQUAL(diffSettings.at(1), WirelessTypes::chSetting_hardwareOffset);
    BOOST_CHECK_EQUAL(diffSettings.at(2), WirelessTypes::chSetting_autoBalance);
    BOOST_CHECK_EQUAL(diffSettings.at(3), WirelessTypes::chSetting_legacyShuntCal);

    BOOST_CHECK_EQUAL(groups.at(1).channels().toMask(), 1);    //cal coefficient ch1
    BOOST_CHECK_EQUAL(groups.at(1).name(), "Differential (ch1)");
    auto ch1Settings = groups.at(1).settings();
    BOOST_CHECK_EQUAL(ch1Settings.at(0), WirelessTypes::chSetting_linearEquation);
    BOOST_CHECK_EQUAL(ch1Settings.at(1), WirelessTypes::chSetting_unit);
    BOOST_CHECK_EQUAL(ch1Settings.at(2), WirelessTypes::chSetting_equationType);

    BOOST_CHECK_EQUAL(groups.at(2).channels().toMask(), 4);    //cal coefficient ch3
    BOOST_CHECK_EQUAL(groups.at(2).name(), "Differential (ch3)");
    auto ch3Settings = groups.at(2).settings();
    BOOST_CHECK_EQUAL(ch3Settings.at(0), WirelessTypes::chSetting_linearEquation);
    BOOST_CHECK_EQUAL(ch3Settings.at(1), WirelessTypes::chSetting_unit);
    BOOST_CHECK_EQUAL(ch3Settings.at(2), WirelessTypes::chSetting_equationType);

    BOOST_CHECK_EQUAL(groups.at(3).channels().toMask(), 8);    //cal coefficient ch4
    BOOST_CHECK_EQUAL(groups.at(3).name(), "Differential (ch4)");
    auto ch4Settings = groups.at(3).settings();
    BOOST_CHECK_EQUAL(ch4Settings.at(0), WirelessTypes::chSetting_linearEquation);
    BOOST_CHECK_EQUAL(ch4Settings.at(1), WirelessTypes::chSetting_unit);
    BOOST_CHECK_EQUAL(ch4Settings.at(2), WirelessTypes::chSetting_equationType);
}

BOOST_AUTO_TEST_CASE(ChannelGroup_channelGroups_multiChannel)
{
    std::shared_ptr<NodeFeatures> node = NodeFeatures::create(createInfo(WirelessModels::node_tcLink200));
    mscl::ChannelGroups groups = node->channelGroups();

    BOOST_CHECK_EQUAL(groups.size(), 16);
    BOOST_CHECK_EQUAL(groups.at(0).channels().toMask(), 1);     //cal coefficients ch1
    BOOST_CHECK_EQUAL(groups.at(0).name(), "Temperature (ch1)");
    auto ch1Settings = groups.at(0).settings();
    BOOST_CHECK_EQUAL(ch1Settings.at(0), WirelessTypes::chSetting_linearEquation);
    BOOST_CHECK_EQUAL(ch1Settings.at(1), WirelessTypes::chSetting_unit);
    BOOST_CHECK_EQUAL(ch1Settings.at(2), WirelessTypes::chSetting_equationType);

    BOOST_CHECK_EQUAL(groups.at(12).channels().toMask(), 4096);    //cal coefficient ch13
    BOOST_CHECK_EQUAL(groups.at(12).name(), "CJC Temperature (ch13)");
    auto ch13Settings = groups.at(12).settings();
    BOOST_CHECK_EQUAL(ch13Settings.at(0), WirelessTypes::chSetting_linearEquation);
    BOOST_CHECK_EQUAL(ch13Settings.at(1), WirelessTypes::chSetting_unit);
    BOOST_CHECK_EQUAL(ch13Settings.at(2), WirelessTypes::chSetting_equationType);

    BOOST_CHECK_EQUAL(groups.at(13).channels().toMask(), 63);   //channels 1-6
    BOOST_CHECK_EQUAL(groups.at(13).name(), "Temperature (ch1-ch6)");
    auto settings1 = groups.at(13).settings();
    BOOST_CHECK_EQUAL(settings1.at(0), WirelessTypes::chSetting_inputRange);
    BOOST_CHECK_EQUAL(settings1.at(1), WirelessTypes::chSetting_tempSensorOptions);

    BOOST_CHECK_EQUAL(groups.at(14).channels().toMask(), 4032); //channels 7-12
    BOOST_CHECK_EQUAL(groups.at(14).name(), "Temperature (ch7-ch12)");
    auto settings2 = groups.at(14).settings();
    BOOST_CHECK_EQUAL(settings2.at(0), WirelessTypes::chSetting_inputRange);
    BOOST_CHECK_EQUAL(settings2.at(1), WirelessTypes::chSetting_tempSensorOptions);

    BOOST_CHECK_EQUAL(groups.at(15).channels().toMask(), 4095); //channels 1-12
    BOOST_CHECK_EQUAL(groups.at(15).name(), "Temperature (ch1-ch12)");
    auto settings3 = groups.at(15).settings();
    BOOST_CHECK_EQUAL(settings3.at(0), WirelessTypes::chSetting_lowPassFilter);
}

BOOST_AUTO_TEST_CASE(ChannelGroup_getGaugeFactor)
{
    std::shared_ptr<mock_WirelessNodeImpl> impl(new mock_WirelessNodeImpl(100));
    BaseStation b = makeBaseStationWithMockImpl();
    WirelessNode node(100, b);
    node.setImpl(impl);

    NodeInfo info(Version(10, 0), WirelessModels::node_shmLink2_cust1, WirelessTypes::region_usa);

    //make the features() function return the NodeFeatures we want
    std::unique_ptr<NodeFeatures> features = NodeFeatures::create(info);
    expectNodeFeatures(features, impl, WirelessModels::node_shmLink2_cust1);

    expectRead(impl, NodeEepromMap::GAUGE_FACTOR_1, Value::FLOAT(1.2345f));
    expectRead(impl, NodeEepromMap::GAUGE_FACTOR_2, Value::FLOAT(713.45101f));
    expectRead(impl, NodeEepromMap::GAUGE_FACTOR_3, Value::FLOAT(0.000152f));

    BOOST_CHECK_THROW(node.getGaugeFactor(ChannelMask(8)), Error_NotSupported);    //ch4 not supported
    BOOST_CHECK_CLOSE(node.getGaugeFactor(ChannelMask(1)), 1.2345, 0.001);        //ch1
    BOOST_CHECK_CLOSE(node.getGaugeFactor(ChannelMask(2)), 713.45101, 0.001);    //ch2
    BOOST_CHECK_CLOSE(node.getGaugeFactor(ChannelMask(4)), 0.000152, 0.001);    //ch3
}

BOOST_AUTO_TEST_CASE(WirelessNode_getLinearEquation)
{
    std::shared_ptr<mock_WirelessNodeImpl> impl(new mock_WirelessNodeImpl(100));
    BaseStation b = makeBaseStationWithMockImpl();
    WirelessNode node(100, b);
    node.setImpl(impl);

    NodeInfo info(Version(9, 9), WirelessModels::node_gLink_2g, WirelessTypes::region_usa);

    //make the features() function return the NodeFeatures we want
    std::unique_ptr<NodeFeatures> features = NodeFeatures::create(info);
    expectNodeFeatures(features, impl, WirelessModels::node_gLink_2g);

    //reading channel 1
    expectRead(impl, NodeEepromMap::CH_ACTION_SLOPE_1, Value::FLOAT(12.789f));        //slope
    expectRead(impl, NodeEepromMap::CH_ACTION_OFFSET_1, Value::FLOAT(-48.001f));    //offset
    expectRead(impl, NodeEepromMap::CH_ACTION_SLOPE_3, Value::FLOAT(6.45f));        //slope
    expectRead(impl, NodeEepromMap::CH_ACTION_OFFSET_3, Value::FLOAT(-0.002f));        //offset
    expectRead(impl, NodeEepromMap::CH_ACTION_ID_1, Value::UINT16(static_cast<uint16>(0x0407)));    //equation and unit

    LinearEquation eq = node.getLinearEquation(ChannelMask(1));//ch1
    LinearEquation eq2 = node.getLinearEquation(ChannelMask(4));//ch3

    BOOST_CHECK_CLOSE(eq.slope(), 12.789, 0.1);
    BOOST_CHECK_CLOSE(eq.offset(), -48.001, 0.1);
    BOOST_CHECK_CLOSE(eq2.slope(), 6.45, 0.1);
    BOOST_CHECK_CLOSE(eq2.offset(), -0.002, 0.1);
    BOOST_CHECK_EQUAL(node.getUnit(ChannelMask(1)), WirelessTypes::unit_volts_millivolts);
    BOOST_CHECK_EQUAL(node.getEquationType(ChannelMask(1)), WirelessTypes::equation_standard);
}

BOOST_AUTO_TEST_CASE(ChannelGroup_getThermocoupleType)
{
    //tests get thermocouple fail (not supported)
    {
        std::shared_ptr<mock_WirelessNodeImpl> impl(new mock_WirelessNodeImpl(100));
        BaseStation b = makeBaseStationWithMockImpl();
        WirelessNode node(100, b);
        node.setImpl(impl);

        NodeInfo info(Version(9, 9), WirelessModels::node_gLink_2g, WirelessTypes::region_usa);

        //make the features() function return the NodeFeatures we want
        std::unique_ptr<NodeFeatures> features = NodeFeatures::create(info);
        expectNodeFeatures(features, impl, WirelessModels::node_gLink_2g);

        BOOST_CHECK_THROW(node.getThermocoupleType(ChannelMask(1)), Error_NotSupported);
    }

    //tests get thermocouple success
    {
        std::shared_ptr<mock_WirelessNodeImpl> impl(new mock_WirelessNodeImpl(100));
        BaseStation b = makeBaseStationWithMockImpl();
        WirelessNode node(100, b);
        node.setImpl(impl);

        std::unique_ptr<NodeFeatures> features;
        expectNodeFeatures(features, impl, WirelessModels::node_tcLink_3ch);

        expectRead(impl, NodeEepromMap::THERMOCPL_TYPE, Value::UINT16(WirelessTypes::tc_N));

        BOOST_CHECK_EQUAL(node.getThermocoupleType(ChannelMask(BOOST_BINARY(00000111))), WirelessTypes::tc_N);
        BOOST_CHECK_THROW(node.getThermocoupleType(ChannelMask(BOOST_BINARY(00000101))), Error_NotSupported);
        BOOST_CHECK_THROW(node.getThermocoupleType(ChannelMask(BOOST_BINARY(00000001))), Error_NotSupported);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*********************************************************************
*
* Author: Noé Pérez Higueras
*********************************************************************/

#include <local_3d_planner/local_3d_planner.h>
//#include <costmap_2d/footprint.h>
#include <string>
#include <sstream>
#include <math.h>
#include <angles/angles.h>

#include <boost/algorithm/string.hpp>

#include <ros/console.h>

//for computing path distance
#include <queue>

using namespace std;
//using namespace costmap_2d;

namespace local_3d_planner{

  /*void UpoPlanner::reconfigure(SimpleLocalPlannerConfig &cfg)
  {
      SimpleLocalPlannerConfig config(cfg);

      boost::mutex::scoped_lock l(configuration_mutex_);

		acc_lim_trans_ = config.max_trans_acc;
		acc_lim_rot_ = config.max_rot_acc;
		max_vel_x_ = config.max_trans_vel;
		min_vel_x_ = config.min_trans_vel;
		max_vel_th_ = config.max_rot_vel;
		min_vel_th_ = config.min_rot_vel;
		min_in_place_vel_th_ = config.min_in_place_rot_vel;
		goal_lin_tolerance_ = config.xy_goal_tolerance;
		goal_ang_tolerance_ = config.yaw_goal_tolerance;
		wp_tolerance_ = config.wp_tolerance;
		sim_time_ = config.sim_time;
		sim_granularity_ = config.sim_granularity;
		angular_sim_granularity_ = config.angular_sim_granularity;
		dwa_ = config.sample_angular_vels;
		//printf("\nPure Planner Reconfigure. new wp_tolerance: %.2f\n", wp_tolerance_);
  }*/

  Local3DPlanner::Local3DPlanner(std::string name, tf::TransformListener* tf,
			local_3d_planner::OdometryHelperRos* oh,
			//std::vector<geometry_msgs::Point> footprint_spec,
			double robot_radius,
			double local_area_radius,
			double controller_freq,
			double max_trans_vel, double min_trans_vel,
			double max_rot_vel, double min_rot_vel,
			double min_in_place_rot_vel,
			double max_trans_acc, double max_rot_acc,
			double yaw_goal_tolerance, double xy_goal_tolerance,
			double wp_tolerance, double sim_time,
			double sim_granularity, double angular_sim_granularity, bool dwa)
  {

		//costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_);

		//printf("\n\n\n---FOOTPRINT----\n");
		//printf("inscribed_radius: %.3f, circumscribed_radius: %.3f\n", inscribed_radius_, circumscribed_radius_);
		//printf("Footprint_specs:\n");
		//for(unsigned int i = 0; i<footprint_spec_.size(); i++)
		//{
		//	printf("point %u: x=%.3f, y=%.3f\n", (i+1), footprint_spec_[i].x, footprint_spec_[i].y);
		//}
		//printf("\n\n");

		controller_freq_ = controller_freq;
		goal_reached_ = false;

    	//For pure-pursuit
    	running_ = false;
		new_plan_ = false;
		wp_index_ = -1;

    	acc_lim_trans_ = max_trans_acc;
		acc_lim_rot_ = max_rot_acc;
		max_vel_x_ = max_trans_vel;
		min_vel_x_ = min_trans_vel;
		max_vel_th_ = max_rot_vel;
		min_vel_th_ = min_rot_vel;
		min_in_place_vel_th_ = min_in_place_rot_vel;
		goal_lin_tolerance_ = xy_goal_tolerance;
		goal_ang_tolerance_ = yaw_goal_tolerance;
		wp_tolerance_ = wp_tolerance;
		sim_time_ = sim_time;
		sim_granularity_ = sim_granularity;
		angular_sim_granularity_ = angular_sim_granularity;

		dwa_ = dwa;
		robot_radius_ = robot_radius;
		local_area_radius_ = local_area_radius;

		collision_detector_ = new CollisionDetection(name, tf, oh, max_vel_x_, max_vel_th_, acc_lim_trans_,
				acc_lim_rot_, sim_time_, robot_radius_, local_area_radius_, sim_granularity_);

  }

  Local3DPlanner::~Local3DPlanner(){
	delete collision_detector_;
  }

  bool Local3DPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan)
  {
		goal_reached_ = false;

		// Copy new plan
		global_plan_.clear();
		global_plan_.resize(new_plan.size());
		for(unsigned int i = 0; i < new_plan.size(); ++i)
		{
			global_plan_[i] = new_plan[i];
		}

		// Check plan size
		if(global_plan_.size() == 0)
		{
			running_ = false;
			wp_index_ = -1;
			ROS_WARN("New local plan size = 0!");
			return true;
		}

		// Set the way-point index to the first point of the path
		wp_index_ = 0;
		running_ = true;
		new_plan_ = true;

		// Set plan goal point
		geometry_msgs::PoseStamped& goal_pose = global_plan_[global_plan_.size()-1];
		goal_x_ = goal_pose.pose.position.x;
		goal_y_ = goal_pose.pose.position.y;
		goal_z_ = goal_pose.pose.position.z;
		goal_t_ = tf::getYaw(goal_pose.pose.orientation);

		// Set the plan starting point
		geometry_msgs::PoseStamped& start_pose = global_plan_[0];
		start_x_ = start_pose.pose.position.x;
		start_y_ = start_pose.pose.position.y;
		start_z_ = start_pose.pose.position.z;
		start_t_ = tf::getYaw(start_pose.pose.orientation);

		return true;
  }

  bool Local3DPlanner::isGoalReached()
  {
		if(goal_reached_) {
			goal_reached_ = false; //we reset the flag
			return true;
		}
		return goal_reached_;
  }

  void Local3DPlanner::resetGoal() {
		goal_reached_ = false;
  }

  //given the current state of the robot, find a good control command
  bool Local3DPlanner::findBestAction(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      geometry_msgs::Twist& cmd_vel)
  {

	//ros::WallTime t1 = ros::WallTime::now();

	//boost::mutex::scoped_lock l(configuration_mutex_);

	goal_reached_ = false;
	double vx, vy = 0.0, vt;

	// Check we have a path and we are running
	if(!running_)
	{
		vx = 0.0;
		vt = 0.0;
		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = 0.0;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		printf("FindBestAction. PurePursuit waiting for a path\n");
		return true;
	}

	// Get current robot position and velocity in X, Y and Theta (odom)
	float rx, ry, rz, rt, rvx, rvy, rvt;
	rx = global_pose.getOrigin().getX();
	ry = global_pose.getOrigin().getY();
	rz = global_pose.getOrigin().getZ();
	rt = tf::getYaw(global_pose.getRotation());
	rvx = global_vel.getOrigin().getX();
	rvy = global_vel.getOrigin().getY();
	rvt = tf::getYaw(global_vel.getRotation());

	// Check if we are close enough to the goal
	double dist_goal = sqrt((rx-goal_x_)*(rx-goal_x_)+(ry-goal_y_)*(ry-goal_y_)); //+(rz-goal_z_)*(rz-goal_z_));
	double dist_start = sqrt((rx-start_x_)*(rx-start_x_)+(ry-start_y_)*(ry-start_y_)); //+(rz-start_z_)*(rz-start_z_));

	if(dist_goal < goal_lin_tolerance_)
	{
		printf("FindBestAction. dist_goal: %.2f. rx:%.2f, ry:%.2f, rz:%.2f, goalx:%.2f, goaly:%.2f, goalz: %.2f\n", dist_goal, rx, ry, rz, goal_x_, goal_y_, goal_z_);
		// Stop the robot
		vx = 0.0;
		vy = 0.0;

		// Rotate at minumin velocity until reaching the goal angle
		if(fabs(goal_t_-rt) < goal_ang_tolerance_)
		{
			vt = 0.0;
			running_ = false;
			goal_reached_ = true;
		}
		/*else if(goal_t_ > rt)
			vt = min_in_place_vel_th_;
		else
			vt = -min_in_place_vel_th_;
		*/
		// Modified by Noé
		else {
			float ang_diff = goal_t_ - rt;
			ang_diff = normalizeAngle(ang_diff, -M_PI, M_PI);
			if(ang_diff > 0.0)
				vt = min_in_place_vel_th_;
			else
				vt = -min_in_place_vel_th_;
		}

		//Added by Noé
		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = vy;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		//ros::WallTime t2 = ros::WallTime::now();
		//double secs = (t2-t1).toSec();
		//printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
		printf("FindBestAction. goal reached, sending zero velocity\n");
		return true;

	}

	// Do we have a new plan? get the closest point into the plan
	if(new_plan_)
	{
		new_plan_ = false;
		double dist;
		wp_index_ = 0;
		for(int i=global_plan_.size()-1; i>=0; i--)
		{
			double wpx = global_plan_[i].pose.position.x;
			double wpy = global_plan_[i].pose.position.y;
			double wpz = global_plan_[i].pose.position.z;
			dist = sqrt((rx-wpx)*(rx-wpx)+(ry-wpy)*(ry-wpy)); //+(rz-wpz)*(rz-wpz));
			if(dist < wp_tolerance_)
			{
				wp_index_ = i;
				break;
			}
		}
	}

	// Get current way-point in the path
	double wpx = global_plan_[wp_index_].pose.position.x;
	double wpy = global_plan_[wp_index_].pose.position.y;
	double wpz = global_plan_[wp_index_].pose.position.z;

	// Is this way-point still valid?
	double dist_swp = sqrt((rx-wpx)*(rx-wpx)+(ry-wpy)*(ry-wpy)); //+(rz-wpz)*(rz-wpz));
	while(dist_swp < wp_tolerance_ && wp_index_ < (int)global_plan_.size()-1)
	{
		wp_index_++;
		wpx = global_plan_[wp_index_].pose.position.x;
		wpy = global_plan_[wp_index_].pose.position.y;
		//wpz = global_plan_[wp_index_].pose.position.z;
		dist_swp = sqrt((rx-wpx)*(rx-wpx)+(ry-wpy)*(ry-wpy)); //+(rz-wpz)*(rz-wpz));
	}

	// Transform way-point into local robot frame and get desired x,y,theta
	double dx = (wpx-rx)*cos(rt) + (wpy-ry)*sin(rt);
	double dy =-(wpx-rx)*sin(rt) + (wpy-ry)*cos(rt);
	double dt = atan2(dy, dx);
	//double dz = (wpz - rz);  //???
	//features_->transformPoseTo(geometry_msgs::PoseStamped pose_in, std::string frame_out, bool usetime);

	double incr = 1/controller_freq_;

	// Check if we need rotation in place before moving the robot to reach the way-point
	if(fabs(dt) > 1.3) // 0.87 0.7~41º  0.79~45º
	{
		//vx = rvx-incr;
		//if(vx < 0.0)
		//	vx = 0.0;
		vx = 0;
		vy = 0.0;
		vt = min_in_place_vel_th_;
		if(dt < 0.0)
			vt = -min_in_place_vel_th_;

		//printf("place_vt: %.2f\n", vt);
	}
	else // Select the linear and angular velocities to reach the way-point
	{
		// Compute actions depending to the distance to the goal and the starting points
		double dist_th = 1.5;
		if(dist_goal < dist_th)
		{
			vx = min_vel_x_ + (max_vel_x_ - min_vel_x_)*dist_goal/dist_th;
			vy = 0.0;
			vt = min_vel_th_ + (max_vel_th_ - min_vel_th_)*dist_goal/dist_th;
			//--added by noe
			if(dt < 0.0)
				vt *= -1;
		}
		/*else if(dist_start < dist_th)
		{
			vx = min_vel_x_ + (max_vel_x_ - min_vel_x_)*dist_start/dist_th;
			vy = 0.0;
			vt = min_vel_th_ + (max_vel_th_ - min_vel_th_)*dist_start/dist_th;
		}*/
		else
		{

			vx = max_vel_x_*(0.1 + exp(-fabs(dt))); // * tanh(4*dist_swp); //max_vel_x_;
			if(vx > max_vel_x_)
				vx = max_vel_x_;
			vy = 0.0;
			vt = max_vel_th_* dt; //max_vel_th_;
		}

		//if(vx > rvx+incr)
		//	vx = rvx+incr;

		// Set the sign of the commanded angular velocity and reset in case of small variations
		//if(dt < 0.0) //commented by Noé
		//	vt *= -1;
		if(fabs(dt) < 0.1)
			vt = 0.0;
	}

	// Check if the action collide with an obstacle
	double px=0.0, py=0.0, pz=0.0, pth=0.0;
	bool valid = true;
	if(vx != 0.0) {
		//valid = checkTrajectory(rx, ry, rt, rvx, rvy, rvt, vx, vy, vt, px, py, pth);
		ros::WallTime t_check1 = ros::WallTime::now();
		valid = collision_detector_->checkTraj(rvx, rvy, rvt, vx, vy, vt, px, py, pz, pth);
		ros::WallTime t_check2 = ros::WallTime::now();
		double secs = (t_check2-t_check1).toSec();
		//printf("\n\nCheckTraj time: %f seconds\n\n", secs);
	}
	if(valid /*|| fabs(vx) < 0.0001*/)
	{
		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = vy;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		//ros::WallTime t2 = ros::WallTime::now();
		//double secs = (t2-t1).toSec();
		//printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
		printf("FindBestAction. validTraj found, vx:%.2f, vy:%.2f, vth:%.2f\n", vx, vy, vt);
		return true;
	}

	// Try to find a valid command by sampling vels
	else if(dwa_)
	{
		//printf("Dentro de DWA!!!!!!!\n");
		float vt_orig = vt;
		float vx_orig = vx;

		float ang_vel_inc = 0.1;
		float lin_vel_dec = 0.1;

		vels_ best_vels;
		best_vels.vel_x = -1.0;
		best_vels.vel_y = vy;

		//Linear vel
		for(unsigned int l=0; l <= 3; l++)
		{

			vx = vx_orig - (lin_vel_dec*l);
			if(vx < 0.1)
				continue;

			best_vels.vel_x = vx;

			//Angular vel
			for(unsigned int v=1; v <= 4; v++)
			{
				//To the right
				vt = vt_orig + (ang_vel_inc * v);
				if(fabs(vt) > max_vel_th_)
					vt = max_vel_th_;

				bool valid1 = collision_detector_->checkTraj(rvx, rvy, rvt, vx, vy, vt, px, py, pz, pth);
				double d1 = 0.0;
				if(valid1) {
					d1 = sqrt((dx-px)*(dx-px) + (dy-py)*(dy-py)); // + (dz-pz)*(dz-pz));
					best_vels.vel_th = vt;
				}

				//to the left
				vt = vt_orig - (ang_vel_inc * v);
				if(fabs(vt) > max_vel_th_)
					vt = -max_vel_th_;

				bool valid2 = collision_detector_->checkTraj(rvx, rvy, rvt, vx, vy, vt, px, py, pz, pth);
				if(valid2) {

					//If both commands are valid,
					//chose the closest to the path.
					if(valid1)
					{
						double d2 = sqrt((dx-px)*(dx-px) + (dy-py)*(dy-py)); // + (dz-pz)*(dz-pz));
						if(d2 < d1)
						{
							best_vels.vel_th = vt;
						}
					}
				}
				if(valid1 || valid2)
				{
					cmd_vel.linear.x = best_vels.vel_x;
					cmd_vel.linear.y = best_vels.vel_y;
					cmd_vel.linear.z = 0.0;
					cmd_vel.angular.x = 0.0;
					cmd_vel.angular.y = 0.0;
					cmd_vel.angular.z = best_vels.vel_th;
					//printf("\nValid cmd found! vx:%.2f, vth:%.2f\n", vx, vt);
					//ros::WallTime t2 = ros::WallTime::now();
					//double secs = (t2-t1).toSec();
					//printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
					printf("FindBestAction. Valid Traj found with DWA. vx:%.2f, vy:%.2f, vth:%.2f\n", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
					return true;
				}

			}
		}
	}

	//If still a valid command is not found, try to rotate in the spot
	if(dt > 0.09 || dt < -0.09) //0.09rad~5º
	{
		//printf("The robot should rotate on the spot\n");
		vx = 0.0;
		vy = 0.0;
		vt = min_in_place_vel_th_;
		if(dt < 0.0)
			vt = -min_in_place_vel_th_;

		cmd_vel.linear.x = vx;
		cmd_vel.linear.y = vy;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = vt;
		//ros::WallTime t2 = ros::WallTime::now();
		//double secs = (t2-t1).toSec();
		//printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
		printf("FindBestAction. Rotating in the spot vx:%.2f, vy:%.2f, vth:%.2f\n", vx, vy, vt);
		return true;
	} else {
		// Stop the robot
		//printf("The robot should stop\n");
		cmd_vel.linear.x = 0.0;
		cmd_vel.linear.y = 0.0;
		cmd_vel.linear.z = 0.0;
		cmd_vel.angular.x = 0.0;
		cmd_vel.angular.y = 0.0;
		cmd_vel.angular.z = 0.0;
		//ros::WallTime t2 = ros::WallTime::now();
		//double secs = (t2-t1).toSec();
		//printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
		printf("FindBestAction. No valid command was found\n");
		return false;
		//return true;
	}

  }

  //we need to take the footprint of the robot into account when we calculate cost to obstacles
  /*double UpoPlanner::footprintCost(double x_i, double y_i, double theta_i){
    //check if the footprint is legal
    return world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_, circumscribed_radius_);
  }*/

};


/**
*   WithinOperation.hpp
*
*   Copyright 2015 The Climate Corporation
*   Copyright 2015 Tony Stone
*
*   Licensed under the Apache License, Version 2.0 (the "License");
*   you may not use this file except in compliance with the License.
*   You may obtain a copy of the License at
*
*   http://www.apache.org/licenses/LICENSE-2.0
*
*   Unless required by applicable law or agreed to in writing, software
*   distributed under the License is distributed on an "AS IS" BASIS,
*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*
*   Created by Tony Stone on 6/11/15.
*
*   MODIFIED 2015 BY Tony Stone. Modifications licensed under Apache License, Version 2.0.
*
*/

#ifndef __WithinOperation_HPP_
#define __WithinOperation_HPP_

#include <boost/variant/variant.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include "GFGeometryVariant.hpp"

namespace geofeatures {
    namespace operators {

        namespace detail {

            /**
             *  within implementation class.
             */
            struct within {

            private:
                /**
                 * Internal visitor class for variant access.
                 */
                class Visitor : public boost::static_visitor<bool> {

                public:

                    /**
                     * Constructor to initialize the visitor members
                     */
                    inline Visitor() : variantToPtrVariantVisitor()  {}

                    // Point
                    //
                    // Note: Point within any Geometry is implented by boost
                    //
                    bool operator()(const Point * lhs, const Point * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Point * lhs, const LineString * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Point * lhs, const Ring * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Point * lhs, const Box * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Point * lhs, const Polygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Point * lhs, const MultiPoint * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Point * lhs, const MultiLineString * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Point * lhs, const MultiPolygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    //
                    // LineString
                    //
                    bool operator()(const LineString * lhs, const LineString * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const LineString * lhs, const Ring * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const LineString * lhs, const Polygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const LineString * lhs, const MultiLineString * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const LineString * lhs, const MultiPolygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    //
                    // MultiLineString
                    //
                    bool operator()(const MultiLineString * lhs, const Ring * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const MultiLineString * lhs, const Polygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const MultiLineString * lhs, const MultiPolygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const MultiLineString * lhs, const LineString * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    //
                    // Ring
                    //
                    bool operator()(const Ring * lhs, const Ring * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Ring * lhs, const Polygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Ring * lhs, const MultiPolygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    //
                    // Polygon
                    //
                    bool operator()(const Polygon * lhs, const Ring * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Polygon * lhs, const Polygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const Polygon * lhs, const MultiPolygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    //
                    // MultiPolygon
                    //
                    bool operator()(const MultiPolygon * lhs, const Ring * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const MultiPolygon * lhs, const Polygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    bool operator()(const MultiPolygon * lhs, const MultiPolygon * rhs) const {
                        return boost::geometry::within(*lhs, *rhs);
                    }

                    /**
                     * Generic default implementation that catches all
                     * non-implemented combinations.
                     */
                    template <typename T1, typename T2>
                    bool operator()(const T1 * lhs, const T2 * rhs) const
                    {
                        return false;
                    }

                    /**
                     * Recursive implementation of GeometryCollection type.
                     */
                    bool operator()(const GeometryCollection<> * t1, const GeometryCollection<> * t2) const {

                        for (auto it = t1->begin(); it != t1->end(); ++it ) {
                            geofeatures::GeometryPtrVariant ptrVariant1 = boost::apply_visitor(variantToPtrVariantVisitor,*it);
                            geofeatures::GeometryPtrVariant ptrVariant2(t2);

                            if (boost::apply_visitor(*this, ptrVariant1, ptrVariant2)) {
                                return true;
                            }
                        }

                        for (auto it = t2->begin(); it != t2->end(); ++it ) {
                            geofeatures::GeometryPtrVariant ptrVariant1(t1);
                            geofeatures::GeometryPtrVariant ptrVariant2  = boost::apply_visitor(variantToPtrVariantVisitor,*it);

                            if (boost::apply_visitor(*this, ptrVariant1, ptrVariant2)) {
                                return true;
                            }
                        }
                        return false;
                    }

                    template <typename T>
                    bool operator()(const GeometryCollection<> * t1, const T * t2) const {
                        geofeatures::GeometryPtrVariant ptrVariant2(t2);

                        for (auto it = t1->begin(); it != t1->end(); ++it ) {
                            geofeatures::GeometryPtrVariant ptrVariant1 = boost::apply_visitor(variantToPtrVariantVisitor,*it);

                            if (boost::apply_visitor(*this, ptrVariant1, ptrVariant2)) {
                                return true;
                            }
                        }
                        return false;
                    }
                    template <typename T>
                    bool operator()(const T * t1, const GeometryCollection<> * t2) const {
                        geofeatures::GeometryPtrVariant ptrVariant1(t1);

                        for (auto it = t2->begin(); it != t2->end(); ++it ) {
                            geofeatures::GeometryPtrVariant ptrVariant2 = boost::apply_visitor(variantToPtrVariantVisitor,*it);

                            if (boost::apply_visitor(*this, ptrVariant1, ptrVariant2)) {
                                return true;
                            }
                        }
                        return false;
                    }

                private:
                    /**
                     * Internal class to convert from GeometryCollection variant
                     * to the GeometryPtrVariant type.
                     */
                    struct VariantToPtrVariant : public boost::static_visitor<GeometryPtrVariant> {
                        template <typename T>
                        GeometryPtrVariant operator()(const T & v) const {
                            return GeometryPtrVariant(&v);
                        }
                    };

                    // Members
                    VariantToPtrVariant variantToPtrVariantVisitor;
                };

            public:

                /**
                     * Apply the detail implementation to the variants passed.
                     */
                template <BOOST_VARIANT_ENUM_PARAMS(typename T1), BOOST_VARIANT_ENUM_PARAMS(typename T2)>
                static inline bool apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T1)> const & variant1,boost::variant<BOOST_VARIANT_ENUM_PARAMS(T2)> const & variant2)
                {
                    return boost::apply_visitor(Visitor(), variant1, variant2);
                }
            };

        }   // namespace detail

        /**
         * within other.
         */
        template <BOOST_VARIANT_ENUM_PARAMS(typename T1), BOOST_VARIANT_ENUM_PARAMS(typename T2)>
        inline bool within(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T1)> const & variant1, boost::variant<BOOST_VARIANT_ENUM_PARAMS(T2)> const & variant2)
        {
            return detail::within::apply(variant1, variant2);
        }

    }   // namespace operators
}   // namespace geofeatures

#endif //__WithinOperation_HPP_


#include <boost/hana/fwd/concept/ring.hpp>


/*
 * This file is a part of the TChecker project.
 *
 * See files AUTHORS and LICENSE for copyright details.
 *
 */

#if BOOST_VERSION <= 106600
#include <boost/functional/hash.hpp>
#else
#include <boost/container_hash/hash.hpp>
#endif

#include "tchecker/zg/state.hh"

namespace tchecker {

namespace zg {

state_t::state_t(tchecker::intrusive_shared_ptr_t<tchecker::shared_vloc_t> const & vloc,
                 tchecker::intrusive_shared_ptr_t<tchecker::shared_intval_t> const & intval,
                 tchecker::intrusive_shared_ptr_t<tchecker::zg::shared_zone_t> const & zone)
    : tchecker::ta::state_t(vloc, intval), _zone(zone)
{
  assert(_zone.ptr() != nullptr);
}

state_t::state_t(tchecker::ta::state_t const & s, tchecker::intrusive_shared_ptr_t<tchecker::shared_vloc_t> const & vloc,
                 tchecker::intrusive_shared_ptr_t<tchecker::shared_intval_t> const & intval,
                 tchecker::intrusive_shared_ptr_t<tchecker::zg::shared_zone_t> const & zone)
    : tchecker::ta::state_t(s, vloc, intval), _zone(zone)
{
  assert(_zone.ptr() != nullptr);
}

bool operator==(tchecker::zg::state_t const & s1, tchecker::zg::state_t const & s2)
{
  return tchecker::ta::operator==(s1, s2) && (s1.zone() == s2.zone());
}

bool operator!=(tchecker::zg::state_t const & s1, tchecker::zg::state_t const & s2) { return !(s1 == s2); }

bool operator<=(tchecker::zg::state_t const & s1, tchecker::zg::state_t const & s2)
{
  return tchecker::ta::operator==(s1, s2) && (s1.zone() <= s2.zone());
}

std::size_t hash_value(tchecker::zg::state_t const & s)
{
  std::size_t h = tchecker::ta::hash_value(s);
  boost::hash_combine(h, s.zone());
  return h;
}

bool alu_le(tchecker::zg::state_t const & s1, tchecker::zg::state_t const & s2,
                 tchecker::clockbounds::map_t const & l, tchecker::clockbounds::map_t const & u)
{
  return tchecker::ta::operator==(s1, s2) && s1.zone().alu_le(s2.zone(), l, u);
}

bool g_le(tchecker::zg::state_t const & s1, tchecker::zg::state_t const & s2,
          std::vector<tchecker::typed_diagonal_clkconstr_expression_t const *>  & G,
          std::vector<tchecker::typed_simple_clkconstr_expression_t const *> const & Gdf)
{
  return tchecker::ta::operator==(s1, s2) && s1.zone().g_le(s2.zone(), G, Gdf);
}

int lexical_cmp(tchecker::zg::state_t const & s1, tchecker::zg::state_t const & s2)
{
  int ta_cmp = tchecker::ta::lexical_cmp(s1, s2);
  if (ta_cmp != 0)
    return ta_cmp;
  return s1.zone().lexical_cmp(s2.zone());
}

} // end of namespace zg

} // end of namespace tchecker


/* Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/defs.h"

#include "bio/environment.h"
#include "bio/hmm_dna.h"
#include "bio/hmm_baum_welch.h"
#include "bio/random.h"
#include "bio/serialisable.h"

#include <boost/iterator/reverse_iterator.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

# ifdef _MSC_VER
# pragma warning( push )
//	warning C4094: untagged 'struct' declared no symbols
# pragma warning( disable : 4094 )
# endif
#include <boost/serialization/export.hpp>
# ifdef _MSC_VER
# pragma warning( pop )
# endif
using namespace boost;

#include <vector>
#include <fstream>
using namespace std;

BIO_NS_START

DnaModel::~DnaModel() { }

DnaModel::ptr_t
create_dna_model(unsigned num_states, unsigned order)
{
	if (0 == num_states)
	{
		throw std::invalid_argument("Need positive number of states");
	}

	switch (order)
	{
	case  0: return DnaModel::ptr_t(new DnaHmm< 0>(num_states));
	case  1: return DnaModel::ptr_t(new DnaHmm< 1>(num_states));
#ifndef _DEBUG //hard to debug in MSVC with too many different instantiations
	case  2: return DnaModel::ptr_t(new DnaHmm< 2>(num_states));
	case  3: return DnaModel::ptr_t(new DnaHmm< 3>(num_states));
	case  4: return DnaModel::ptr_t(new DnaHmm< 4>(num_states));
	case  5: return DnaModel::ptr_t(new DnaHmm< 5>(num_states));
	case  6: return DnaModel::ptr_t(new DnaHmm< 6>(num_states));
	case  7: return DnaModel::ptr_t(new DnaHmm< 7>(num_states));
#if 0
	case  8: return DnaModel::ptr_t(new DnaHmm< 8>(num_states));
	case  9: return DnaModel::ptr_t(new DnaHmm< 9>(num_states));
	case 10: return DnaModel::ptr_t(new DnaHmm<10>(num_states));
	case 11: return DnaModel::ptr_t(new DnaHmm<11>(num_states));
	case 12: return DnaModel::ptr_t(new DnaHmm<12>(num_states));
	case 13: return DnaModel::ptr_t(new DnaHmm<13>(num_states));
	case 14: return DnaModel::ptr_t(new DnaHmm<14>(num_states));
	case 15: return DnaModel::ptr_t(new DnaHmm<15>(num_states));
#endif
#endif
	default:
		throw std::logic_error( "Cannot construct DnaHmm with that large an order" );
	}
}

void DnaHmmOrderNumStateMap::init_singleton()
{
	namespace fs = boost::filesystem;

	try_to_deserialise< false >(
		*this,
		fs::path(
			BioEnvironment::singleton().get_species_hmm_file().c_str()
		)
	);
}

BIO_NS_END

#define BIO_DNA_HMM_EXPORT_GUID(O)															\
	typedef BIO_NS::DnaHmm<O> DnaHmm ## O;		\
	BOOST_CLASS_EXPORT(DnaHmm ## O)

//	BOOST_CLASS_EXPORT_GUID(DnaHmm ## O, "DnaHmm" ## BOOST_PP_STRINGIZE(O))

BIO_DNA_HMM_EXPORT_GUID( 0)
BIO_DNA_HMM_EXPORT_GUID( 1)
BIO_DNA_HMM_EXPORT_GUID( 2)
BIO_DNA_HMM_EXPORT_GUID( 3)
BIO_DNA_HMM_EXPORT_GUID( 4)
BIO_DNA_HMM_EXPORT_GUID( 5)
BIO_DNA_HMM_EXPORT_GUID( 6)
BIO_DNA_HMM_EXPORT_GUID( 7)
BIO_DNA_HMM_EXPORT_GUID( 8)
BIO_DNA_HMM_EXPORT_GUID( 9)
BIO_DNA_HMM_EXPORT_GUID(10)
BIO_DNA_HMM_EXPORT_GUID(11)
BIO_DNA_HMM_EXPORT_GUID(12)
BIO_DNA_HMM_EXPORT_GUID(13)
BIO_DNA_HMM_EXPORT_GUID(14)
BIO_DNA_HMM_EXPORT_GUID(15)


//---------------------------------------------------------------------------//
// Copyright (c) 2022 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2022 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE commitments_type_traits_test

#include <string>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>

//#include <nil/crypto3/zk/commitments/polynomial/kzg.hpp>
#include <nil/crypto3/zk/commitments/polynomial/fri.hpp>
#include <nil/crypto3/zk/commitments/polynomial/batched_fri.hpp>
#include <nil/crypto3/zk/commitments/polynomial/lpc.hpp>
#include <nil/crypto3/zk/commitments/polynomial/batched_lpc.hpp>
#include <nil/crypto3/zk/commitments/polynomial/kimchi_pedersen.hpp>
#include <nil/crypto3/zk/commitments/polynomial/pedersen.hpp>
#include <nil/crypto3/zk/commitments/type_traits.hpp>

using namespace nil::crypto3;

BOOST_AUTO_TEST_SUITE(commitments_type_traits_test_suite)

BOOST_AUTO_TEST_CASE(commitments_type_traits_basic_test) {

    typedef algebra::curves::bls12<381> curve_type;
    typedef curve_type::base_field_type field_type;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

//    static_assert(zk::is_commitment<
//            zk::commitments::kzg<curve_type>>::value);
    static_assert(zk::is_commitment<
            zk::commitments::fri<field_type,
                   merkle_hash_type,
                   transcript_hash_type>>::value);
    static_assert(zk::is_commitment<
            zk::commitments::batched_fri<field_type,
                   merkle_hash_type,
                   transcript_hash_type>>::value);
    static_assert(zk::is_commitment<
            zk::commitments::lpc<field_type,
                   zk::commitments::list_polynomial_commitment_params<
                        merkle_hash_type,
                        transcript_hash_type>>>::value);
    static_assert(zk::is_commitment<
            zk::commitments::batched_lpc<field_type,
                   zk::commitments::list_polynomial_commitment_params<
                        merkle_hash_type,
                        transcript_hash_type>>>::value);
    static_assert(zk::is_commitment<
            zk::commitments::pedersen<curve_type>>::value);
    static_assert(zk::is_commitment<
          zk::commitments::kimchi_pedersen<curve_type>>::value);
}

BOOST_AUTO_TEST_SUITE_END()