// ======================================================================
/*!
 * \file shapefind
 * \brief A program to find the features closest to the given point
 */
// ======================================================================
/*
 * \page shapefind shapefind
 *
 * shapefind takes as input a single shapefile containing arbitrary
 * shapedata and a coordinate to search for. The program prints out
 * the elements closest to the given point. If the data is polygonal,
 * the polygon containing the point is printed out.
 *
 * The program was originally created for finding the road closest
 * to the given coordinate.
 *
 */
// ======================================================================

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <imagine/NFmiEsriPoint.h>
#include <imagine/NFmiEsriPolyLine.h>
#include <imagine/NFmiEsriPolygon.h>
#include <imagine/NFmiEsriShape.h>
#include <newbase/NFmiArea.h>
#include <newbase/NFmiAreaFactory.h>
#include <newbase/NFmiGeoTools.h>
#include <newbase/NFmiPoint.h>
#include <newbase/NFmiPreProcessor.h>
#include <newbase/NFmiStringTools.h>
#include <cassert>
#include <memory>
#include <stdexcept>
#include <string>

using namespace std;
using namespace Imagine;

// ----------------------------------------------------------------------
/*!
 * \brief Command line options
 */
// ----------------------------------------------------------------------

struct Options
{
  string shapefile;
  string condition;
  string attributes;
  string uniqueattribute;
  string projection;
  string coordinatefile;
  float latitude;
  float longitude;
  float searchradius;
  unsigned int maxcount;
  string delimiter;
  bool verbose;

  Options();
};

// ----------------------------------------------------------------------
/*!
 * \brief Default options
 */
// ----------------------------------------------------------------------

Options::Options()
    : shapefile(),
      condition(),
      attributes(),
      uniqueattribute(),
      projection("latlon"),
      coordinatefile(),
      latitude(),
      longitude(),
      searchradius(10)  // kilometers
      ,
      maxcount(1),
      delimiter("\t"),
      verbose(false)
{
}

// ----------------------------------------------------------------------
/*!
 * \brief Global access to options
 */
// ----------------------------------------------------------------------

Options options;

// Search projection and the projected search coordinate

NFmiAreaFactory::return_type projection;

// ----------------------------------------------------------------------
/*!
 * \brief Parse the command line
 */
// ----------------------------------------------------------------------

bool parse_options(int argc, char *argv[])
{
  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print out help message")(
      "verbose,v", po::bool_switch(&options.verbose), "set verbose mode on")(
      "version,V", "display version number")(
      "attributes,a",
      po::value(&options.attributes),
      "shapefile attributes to be printed (comma separated)")(
      "unique,u",
      po::value(&options.uniqueattribute),
      "force a specific attribute to have unique values")(
      "shapefile,s", po::value(&options.shapefile), "shapefile (without suffix)")(
      "condition,c", po::value(&options.condition), "search condition (for example CLASS>2)")(
      "lat,y", po::value(&options.latitude), "latitude of the searched coordinate")(
      "lon,x", po::value(&options.longitude), "longitude of the searched coordinate")(
      "coordinatefile,l",
      po::value(&options.coordinatefile),
      "file containing lines of form name,lon,lat for processing multiple "
      "coordinates simultaneously")(
      "radius,r", po::value(&options.searchradius), "maximum search radius")(
      "maxcount,n", po::value(&options.maxcount), "maximum number of search results")(
      "delimiter,d", po::value(&options.delimiter), "delimiter string for output columns")(
      "projection,p", po::value(&options.projection), "projection definition (default=latlon)");

  po::positional_options_description p;
  p.add("shapefile", 1);

  po::variables_map opt;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), opt);

  po::notify(opt);

  if (opt.count("version") != 0)
  {
    cout << "shapefind v1.0 (" << __DATE__ << ' ' << __TIME__ << ')' << endl;
  }

  if (opt.count("help"))
  {
    cout << "Usage: shapefind [options] shapefile" << endl
         << endl
         << "shapefind finds the element closest to the given point" << endl
         << endl
         << desc << endl
         << "Note that if the input data consists of polygons, the options" << endl
         << "-u, -r and -n are meaningless since the program finds the first" << endl
         << "polygon containing the input coordinate." << endl
         << endl;
    return false;
  }

  if (opt.count("shapefile") == 0)
    throw runtime_error("shapefile name not specified");

  if ((opt.count("lon") > 0 || opt.count("lat") > 0) && (opt.count("coordinatefile") > 0))
    throw runtime_error("-l and -x,-y options are mutually exclusive");

  // Sanity checks

  if (options.latitude < -90 || options.latitude > 90)
    throw runtime_error("Search latitude outside -90...90");

  if (options.longitude < -180 || options.longitude > 180)
    throw runtime_error("Search longitude outside -180...180");

  if (options.searchradius < 0)
    throw runtime_error("Search radius cannot be negative");

  if (options.maxcount <= 0)
    throw runtime_error("maxcount must be nonnegative");

  return true;
}

// ----------------------------------------------------------------------
/*!
 * \brief Read file of form name,lon,lat
 */
// ----------------------------------------------------------------------

typedef map<string, NFmiPoint> LocationList;

LocationList read_locationlist(const string &theFile)
{
  // this code was copied from qdpoint.cpp

  LocationList ret;

  const bool strip_pound = true;
  NFmiPreProcessor processor(strip_pound);
  if (!processor.ReadAndStripFile(theFile))
    throw runtime_error("Unable to preprocess " + theFile);

  string text = processor.GetString();
  istringstream input(text);
  string line;
  while (getline(input, line))
  {
    if (line.empty())
      continue;
    vector<string> parts = NFmiStringTools::Split(line, ",");
    if (parts.size() != 3)
      cerr << "Warning: Invalid line '" + line + "' in file '" + theFile + "'" << endl;
    else
    {
      try
      {
        string name = parts[0];
        double lon = NFmiStringTools::Convert<double>(parts[1]);
        double lat = NFmiStringTools::Convert<double>(parts[2]);
        ret.insert(make_pair(name, NFmiPoint(lon, lat)));
      }
      catch (...)
      {
        cerr << "Error while reading '" + theFile + "'";
        throw;
      }
    }
  }
  return ret;
}

// ----------------------------------------------------------------------
/*!
 * \brief Parse comparison operation
 */
// ----------------------------------------------------------------------

void parse_condition(string &theVariable, string &theComparison, string &theValue)
{
  if (options.condition.empty())
    return;

  // Possible operations

  list<string> comparisons;
  comparisons.push_back("==");
  comparisons.push_back("<=");
  comparisons.push_back(">=");
  comparisons.push_back("<>");
  comparisons.push_back("<");
  comparisons.push_back(">");
  comparisons.push_back("=");

  // Try to split the condition

  for (list<string>::const_iterator it = comparisons.begin(); it != comparisons.end(); ++it)
  {
    string::size_type pos = options.condition.find(*it);
    if (pos != string::npos)
    {
      theComparison = *it;
      theVariable = options.condition.substr(0, pos);
      theValue = options.condition.substr(pos + it->size());
      break;
    }
  }

  if (theComparison.empty())
    throw runtime_error("Unable to parse search condition");
}

// ----------------------------------------------------------------------
/*!
 * \brief Test the search condition
 */
// ----------------------------------------------------------------------

bool condition_satisfied(const NFmiEsriElement &theElement,
                         const std::string &theVariable,
                         const std::string &theComparison,
                         const std::string &theValue)
{
  if (theComparison.empty())
    return true;

  NFmiEsriAttributeType atype = theElement.GetType(theVariable);

  if (atype == kFmiEsriString)
  {
    if (theComparison == "=" || theComparison == "==")
      return (theElement.GetString(theVariable) == theValue);
    else if (theComparison == "<>")
      return (theElement.GetString(theVariable) != theValue);
    else if (theComparison == "<")
      return (theElement.GetString(theVariable) < theValue);
    else if (theComparison == ">")
      return (theElement.GetString(theVariable) > theValue);
    else if (theComparison == "<=")
      return (theElement.GetString(theVariable) <= theValue);
    else if (theComparison == ">=")
      return (theElement.GetString(theVariable) >= theValue);
  }
  else if (atype == kFmiEsriInteger)
  {
    double value = boost::lexical_cast<double>(theValue);
    if (theComparison == "=" || theComparison == "==")
      return (theElement.GetInteger(theVariable) == value);
    else if (theComparison == "<>")
      return (theElement.GetInteger(theVariable) != value);
    else if (theComparison == "<")
      return (theElement.GetInteger(theVariable) < value);
    else if (theComparison == ">")
      return (theElement.GetInteger(theVariable) > value);
    else if (theComparison == "<=")
      return (theElement.GetInteger(theVariable) <= value);
    else if (theComparison == ">=")
      return (theElement.GetInteger(theVariable) >= value);
  }
  else if (atype == kFmiEsriDouble)
  {
    double value = boost::lexical_cast<double>(theValue);
    if (theComparison == "=" || theComparison == "==")
      return (theElement.GetDouble(theVariable) == value);
    else if (theComparison == "<>")
      return (theElement.GetDouble(theVariable) != value);
    else if (theComparison == "<")
      return (theElement.GetDouble(theVariable) < value);
    else if (theComparison == ">")
      return (theElement.GetDouble(theVariable) > value);
    else if (theComparison == "<=")
      return (theElement.GetDouble(theVariable) <= value);
    else if (theComparison == ">=")
      return (theElement.GetDouble(theVariable) >= value);
  }

  // Should never reach this point
  assert(false);
  return true;
}

// ----------------------------------------------------------------------
/*!
 * \brief Establish projection for distance calculations
 */
// ----------------------------------------------------------------------

void establish_projection()
{
  if (options.projection == "latlon")
    return;

  projection = NFmiAreaFactory::Create(options.projection);
}

// ----------------------------------------------------------------------
/*!
 * \brief Make sure an attribute is selected for output
 */
// ----------------------------------------------------------------------

void establish_attribute(const NFmiEsriShape &theShape)
{
  const NFmiEsriShape::attributes_type &attributes = theShape.Attributes();

  if (attributes.size() == 0)
    throw runtime_error("shapefile does not contain any attributes");

  // If no attribute option was given, we accept it as long as
  // the shape contains exactly one attribute

  if (options.attributes.empty())
  {
    if (attributes.size() > 1)
    {
      ostringstream out;
      out << "shapefile contains multiple attributes, choose one: ";
      int pos = 0;
      for (NFmiEsriShape::attributes_type::const_iterator at = attributes.begin();
           at != attributes.end();
           ++at, ++pos)
      {
        if (pos > 0)
          out << ",";
        out << (*at)->Name();
      }

      throw runtime_error(out.str());
    }
    options.attributes = (*attributes.begin())->Name();
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Establish that all elements of shape are of unique type
 */
// ----------------------------------------------------------------------

NFmiEsriElementType establish_type(const NFmiEsriShape &theShape)
{
  if (theShape.Elements().size() == 0)
    throw runtime_error("The shape is empty!");

  NFmiEsriElementType type = kFmiEsriNull;
  bool found_one = false;
  bool multiple_types = false;

  NFmiEsriShape::const_iterator it = theShape.Elements().begin();
  for (; it != theShape.Elements().end(); ++it)
  {
    if (*it == nullptr)
      continue;

    switch ((*it)->Type())
    {
      case kFmiEsriNull:
        break;
      case kFmiEsriMultiPatch:
        throw runtime_error("multipatch elements are not supported");
      case kFmiEsriPoint:
      case kFmiEsriPointM:
      case kFmiEsriPointZ:
      case kFmiEsriMultiPoint:
      case kFmiEsriMultiPointM:
      case kFmiEsriMultiPointZ:
      {
        found_one = true;
        if (type != kFmiEsriNull && type != kFmiEsriPoint)
          multiple_types = true;
        else
          type = kFmiEsriPoint;
        break;
      }
      case kFmiEsriPolyLine:
      case kFmiEsriPolyLineM:
      case kFmiEsriPolyLineZ:
      {
        found_one = true;
        if (type != kFmiEsriNull && type != kFmiEsriPolyLine)
          multiple_types = true;
        else
          type = kFmiEsriPolyLine;
        break;
      }
      case kFmiEsriPolygon:
      case kFmiEsriPolygonM:
      case kFmiEsriPolygonZ:
      {
        found_one = true;
        if (type != kFmiEsriNull && type != kFmiEsriPolygon)
          multiple_types = true;
        else
          type = kFmiEsriPolygon;
        break;
      }
    }

    if (multiple_types)
      throw runtime_error("Shape contains elements of different types");
  }

  if (!found_one)
    throw runtime_error("Shape contains only null elements");

  return type;
}

// ----------------------------------------------------------------------
/*!
 * \brief Latlon distance calculator
 */
// ----------------------------------------------------------------------

float latlon_point_distance(double theX1, double theY1, double theX2, double theY2)
{
  float m = NFmiGeoTools::GeoDistance(theX1, theY1, theX2, theY2);
  return m / 1000;
}

// ----------------------------------------------------------------------
/*!
 * \brief WorldXY Distance calculator
 */
// ----------------------------------------------------------------------

float world_point_distance(double theX1, double theY1, double theX2, double theY2)
{
  float m = NFmiGeoTools::Distance(theX1, theY1, theX2, theY2);
  return m / 1000;
}

// ----------------------------------------------------------------------
/*!
 * \brief Calculate isosegment midpoint
 */
// ----------------------------------------------------------------------

void isosegment_center(
    double theX1, double theY1, double theX2, double theY2, double &theX, double &theY)
{
  double lat1 = FmiRad(theY1);
  double lat2 = FmiRad(theY2);

  double dlon = FmiRad(theX2 - theX1);

  double Bx = cos(lat2) * cos(dlon);
  double By = cos(lat2) * sin(dlon);

  theX = theX1 + atan2(By, cos(lat1) + Bx);
  theY = atan2(sin(lat1) + sin(lat2), sqrt((cos(lat1) + Bx) * (cos(lat1) + Bx) + By * By));
}

// ----------------------------------------------------------------------
/*!
 * \brief Isosegment distance calculator
 */
// ----------------------------------------------------------------------

float isosegment_distance(
    double theX, double theY, double theX1, double theY1, double theX2, double theY2)
{
  float m1 = NFmiGeoTools::GeoDistance(theX, theY, theX1, theY1);

  // Isosegment midpoint

  double xc, yc;
  isosegment_center(theX1, theY1, theX2, theY2, xc, yc);

  float m2 = NFmiGeoTools::GeoDistance(theX, theY, theX2, theY2);

  float m = NFmiGeoTools::GeoDistance(theX, theY, xc, yc);

  if (m > m1 && m > m2)
    return min(m1, m2);

  // Recursion should happen pretty seldom

  double x1 = theX1;
  double y1 = theY1;
  double x2 = theX2;
  double y2 = theY2;

  while (m < m1 && m < m2)
  {
    if (m1 < m2)
    {
      x2 = xc;
      y2 = yc;
      m2 = m;
    }
    else
    {
      x1 = xc;
      y1 = yc;
      m1 = m;
    }

    isosegment_center(x1, y1, x2, y2, xc, yc);
    m = NFmiGeoTools::GeoDistance(theX, theY, xc, yc);
  }

  return m;
}

// ----------------------------------------------------------------------
/*!
 * \brief Latlon line distance calculator
 */
// ----------------------------------------------------------------------

float latlon_line_distance(
    double theX, double theY, double theX1, double theY1, double theX2, double theY2)
{
  float m = isosegment_distance(theX, theY, theX1, theY1, theX2, theY2);
  return m / 1000;
}

// ----------------------------------------------------------------------
/*!
 * \brief WorldXY line distance calculator
 */
// ----------------------------------------------------------------------

float world_line_distance(
    double theX, double theY, double theX1, double theY1, double theX2, double theY2)
{
  float m = NFmiGeoTools::DistanceFromLineSegment(theX, theY, theX1, theY1, theX2, theY2);
  return m / 1000;
}

// ----------------------------------------------------------------------
/*!
 * \brief Establish the attribute type of the given attribute
 */
// ----------------------------------------------------------------------

NFmiEsriAttributeType establish_attribute_type(const NFmiEsriShape &theShape, const string &theName)
{
  const Imagine::NFmiEsriShape::attributes_type &attributes = theShape.Attributes();

  for (Imagine::NFmiEsriShape::attributes_type::const_iterator ait = attributes.begin();
       ait != attributes.end();
       ++ait)
  {
    if ((*ait)->Name() == theName)
      return (*ait)->Type();
  }
  throw runtime_error("No attribute named '" + theName + "' in the shape");
}

// ----------------------------------------------------------------------
/*!
 * \brief Print the desired attribute values
 */
// ----------------------------------------------------------------------

void print_attributes(const NFmiEsriElement &theElement)
{
  vector<string> attribs = NFmiStringTools::Split(options.attributes, ",");

  for (unsigned int i = 0; i < attribs.size(); i++)
  {
    if (i > 0)
      cout << options.delimiter;

    switch (theElement.GetType(attribs[i]))
    {
      case kFmiEsriString:
        cout << theElement.GetString(attribs[i]);
        break;
      case kFmiEsriInteger:
        cout << theElement.GetInteger(attribs[i]);
        break;
      case kFmiEsriDouble:
        cout << theElement.GetDouble(attribs[i]);
        break;
      case kFmiEsriDate:
        cout << theElement.GetDate(attribs[i]).ToStr(kYYYYMMDD);
        break;
    }
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Remove indices which would contain duplicates
 */
// ----------------------------------------------------------------------

void filter_out_duplicates(const NFmiEsriShape &theShape, multimap<float, int> &theData)
{
  if (options.uniqueattribute.empty())
    return;

  multimap<float, int> newdata;

  set<string> unique_strings;
  set<int> unique_ints;
  set<double> unique_doubles;
  set<NFmiMetTime> unique_dates;

  NFmiEsriAttributeType atype = establish_attribute_type(theShape, options.uniqueattribute);

  const NFmiEsriShape::elements_type &elements = theShape.Elements();

  for (multimap<float, int>::const_iterator it = theData.begin(); it != theData.end(); ++it)
  {
    int pos = it->second;
    const NFmiEsriElement *elem = static_cast<const NFmiEsriElement *>(elements[pos]);

    switch (atype)
    {
      case kFmiEsriString:
      {
        string value = elem->GetString(options.uniqueattribute);
        if (unique_strings.find(value) == unique_strings.end())
        {
          newdata.insert(*it);
          unique_strings.insert(value);
        }
        break;
      }
      case kFmiEsriInteger:
      {
        int value = elem->GetInteger(options.uniqueattribute);
        if (unique_ints.find(value) == unique_ints.end())
        {
          newdata.insert(*it);
          unique_ints.insert(value);
        }
        break;
      }
      case kFmiEsriDouble:
      {
        double value = elem->GetDouble(options.uniqueattribute);
        if (unique_doubles.find(value) == unique_doubles.end())
        {
          newdata.insert(*it);
          unique_doubles.insert(value);
        }
        break;
      }
      case kFmiEsriDate:
      {
        NFmiMetTime value = elem->GetDate(options.uniqueattribute);
        if (unique_dates.find(value) == unique_dates.end())
        {
          newdata.insert(*it);
          unique_dates.insert(value);
        }
        break;
      }
    }
  }

  theData = newdata;
}

// ----------------------------------------------------------------------
/*!
 * \brief Find nearest points from point shapefile
 *
 * Algorithm:
 *
 * 1. scan through all points
 * 2. calculate the distance
 * 3. if inside search radius, insert into distance sorted map
 * 4. if map size increases above maxcount, drop last point in the map
 * 5. print out the results
 */
// ----------------------------------------------------------------------

void find_nearest_points(const NFmiEsriShape &theShape,
                         const NFmiPoint &theLatLon,
                         const std::string &theName = "")
{
  // Projected coordinate if needed

  NFmiPoint worldxy;

  if (options.projection != "latlon")
    worldxy = projection->LatLonToWorldXY(theLatLon);

  // Search results in sorted order

  typedef multimap<float, int> DistanceMap;
  DistanceMap distance_map;

  // Search conditions

  string cond_variable, cond_comparison, cond_value;
  parse_condition(cond_variable, cond_comparison, cond_value);

  // Sort nearest elements

  const NFmiEsriShape::elements_type &elements = theShape.Elements();

  for (NFmiEsriShape::elements_type::size_type i = 0; i < elements.size(); i++)
  {
    if (elements[i] == 0)  // null element?
      continue;

    const NFmiEsriPoint *elem = static_cast<const NFmiEsriPoint *>(elements[i]);

    float dist;
    if (options.projection == "latlon")
      dist = latlon_point_distance(elem->X(), elem->Y(), theLatLon.X(), theLatLon.Y());
    else
      dist = world_point_distance(elem->X(), elem->Y(), worldxy.X(), worldxy.Y());

    if (dist <= options.searchradius)
    {
      if (condition_satisfied(*elem, cond_variable, cond_comparison, cond_value))
      {
        distance_map.insert(DistanceMap::value_type(dist, i));
      }
    }
  }

  // Print the results. Note that we print only the shortest distance for
  // each unique attribute, if so requested.

  filter_out_duplicates(theShape, distance_map);

  // Print the results

  unsigned int num = 0;
  for (DistanceMap::const_iterator it = distance_map.begin(); it != distance_map.end(); ++it)
  {
    if (num >= options.maxcount)
      break;

    int pos = it->second;
    const NFmiEsriPoint *elem = static_cast<const NFmiEsriPoint *>(elements[pos]);

    double x = elem->X();
    double y = elem->Y();

    if (options.projection != "latlon")
    {
      NFmiPoint p = projection->WorldXYToLatLon(NFmiPoint(x, y));
      x = p.X();
      y = p.Y();
    }

    if (!theName.empty())
      cout << theName << options.delimiter;

    cout << ++num << options.delimiter << it->first << options.delimiter << x << options.delimiter
         << y << options.delimiter;

    print_attributes(*elem);
    cout << endl;
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Find nearest lines from polyline shapefile
 */
// ----------------------------------------------------------------------

void find_nearest_lines(const NFmiEsriShape &theShape,
                        const NFmiPoint &theLatLon,
                        const std::string &theName = "")
{
  // Projected coordinate if needed

  NFmiPoint worldxy;

  if (options.projection != "latlon")
    worldxy = projection->LatLonToWorldXY(theLatLon);

  // Search results in sorted order

  typedef multimap<float, int> DistanceMap;
  DistanceMap distance_map;

  // Search conditions

  string cond_variable, cond_comparison, cond_value;
  parse_condition(cond_variable, cond_comparison, cond_value);

  // Sort nearest elements

  const NFmiEsriShape::elements_type &elements = theShape.Elements();

  for (NFmiEsriShape::elements_type::size_type i = 0; i < elements.size(); i++)
  {
    if (elements[i] == 0)  // null element?
      continue;

    const NFmiEsriPolyLine *elem = static_cast<const NFmiEsriPolyLine *>(elements[i]);

    float mindist = -1;

    for (int part = 0; part < elem->NumParts(); part++)
    {
      int i1, i2;
      i1 = elem->Parts()[part];  // start of part
      if (part + 1 == elem->NumParts())
        i2 = elem->NumPoints() - 1;  // end of part
      else
        i2 = elem->Parts()[part + 1] - 1;  // end of part

      if (i2 >= i1)
      {
        for (int ii = i1 + 1; ii <= i2; ii++)
        {
          float dist;
          if (options.projection == "latlon")
            dist = latlon_line_distance(theLatLon.X(),
                                        theLatLon.Y(),
                                        elem->Points()[ii - 1].X(),
                                        elem->Points()[ii - 1].Y(),
                                        elem->Points()[ii].X(),
                                        elem->Points()[ii].Y());
          else
            dist = world_line_distance(worldxy.X(),
                                       worldxy.Y(),
                                       elem->Points()[ii - 1].X(),
                                       elem->Points()[ii - 1].Y(),
                                       elem->Points()[ii].X(),
                                       elem->Points()[ii].Y());
          if (mindist < 0 || dist < mindist)
            mindist = dist;
        }
      }
    }

    if (mindist <= options.searchradius)
    {
      if (condition_satisfied(*elem, cond_variable, cond_comparison, cond_value))
      {
        distance_map.insert(DistanceMap::value_type(mindist, i));
      }
    }
  }

  // Print the results. Note that we print only the shortest distance for
  // each unique attribute, if so requested.

  filter_out_duplicates(theShape, distance_map);

  unsigned int num = 0;
  for (DistanceMap::const_iterator it = distance_map.begin(); it != distance_map.end(); ++it)
  {
    if (num >= options.maxcount)
      break;

    int pos = it->second;
    const NFmiEsriElement *elem = static_cast<const NFmiEsriElement *>(elements[pos]);

    if (!theName.empty())
      cout << theName << options.delimiter;

    cout << ++num << options.delimiter << it->first << options.delimiter;
    print_attributes(*elem);
    cout << endl;
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Test if the given point is inside the polygon
 *
 * Copied from NFmiSvgPath::IsInside
 */
// ----------------------------------------------------------------------

bool is_inside(const NFmiEsriPolygon &thePoly, double theX, double theY)
{
  int counter = 0;

  for (int part = 0; part < thePoly.NumParts(); part++)
  {
    int i1 = thePoly.Parts()[part];  // start of part
    int i2;
    if (part + 1 == thePoly.NumParts())
      i2 = thePoly.NumPoints() - 1;  // end of part
    else
      i2 = thePoly.Parts()[part + 1] - 1;  // end of part

    if (i2 >= i1)
    {
      double x1 = thePoly.Points()[i1].X();
      double y1 = thePoly.Points()[i1].Y();

      for (int i = i1 + 1; i <= i2; i++)
      {
        double x2 = thePoly.Points()[i].X();
        double y2 = thePoly.Points()[i].Y();
        if (theY > std::min(y1, y2) && theY <= std::max(y1, y2) && theX <= std::max(x1, x2) &&
            y1 != y2)
        {
          const double xinters = (theY - y1) * (x2 - x1) / (y2 - y1) + x1;
          if (x1 == x2 || theX <= xinters)
            counter++;
        }
        x1 = x2;
        y1 = y2;
      }
    }
  }

  return (counter % 2 != 0);
}

// ----------------------------------------------------------------------
/*!
 * \brief Find polygon surrounding point from polygon shapefile
 */
// ----------------------------------------------------------------------

void find_enclosing_polygons(const NFmiEsriShape &theShape,
                             const NFmiPoint &theLatLon,
                             const std::string &theName = "")
{
  // Projected coordinate if needed

  NFmiPoint worldxy;

  if (options.projection != "latlon")
    worldxy = projection->LatLonToWorldXY(theLatLon);

  // Search conditions

  string cond_variable, cond_comparison, cond_value;
  parse_condition(cond_variable, cond_comparison, cond_value);

  // Find the first match

  const NFmiEsriShape::elements_type &elements = theShape.Elements();

  NFmiEsriShape::elements_type::size_type i;
  for (i = 0; i < elements.size(); i++)
  {
    if (elements[i] == 0)  // null element?
      continue;

    const NFmiEsriPolygon *elem = static_cast<const NFmiEsriPolygon *>(elements[i]);

    bool enclosed = false;

    if (condition_satisfied(*elem, cond_variable, cond_comparison, cond_value))
    {
      if (options.projection == "latlon")
        enclosed = is_inside(*elem, theLatLon.X(), theLatLon.Y());
      else
        enclosed = is_inside(*elem, worldxy.X(), worldxy.Y());
    }

    if (enclosed)
      break;
  }

  // Print the results.

  if (i < elements.size())
  {
    const NFmiEsriPolygon *elem = static_cast<const NFmiEsriPolygon *>(elements[i]);

    if (!theName.empty())
      cout << theName << options.delimiter;

    print_attributes(*elem);
    cout << endl;
  }
  else
  {
    // No match, print name,-,-,-,-,....
    if (!theName.empty())
      cout << theName << options.delimiter << "-";

    vector<string> attribs = NFmiStringTools::Split(options.attributes, ",");
    for (unsigned j = 0; j < attribs.size(); j++)
    {
      if (!theName.empty() || j > 0)
        cout << options.delimiter;
      cout << "-";
    }
    cout << endl;
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief main progran
 */
// ----------------------------------------------------------------------

int domain(int argc, char *argv[])
{
  if (!parse_options(argc, argv))
    return 0;

  // Read the shape

  NFmiEsriShape shape;
  if (!shape.Read(options.shapefile, true))
    throw runtime_error("Failed to read '" + options.shapefile + "'");

  // Validate search attribute

  establish_attribute(shape);

  // Establish projection for distance calculations

  establish_projection();

  // Establish what to do.
  //
  // Polygons: find polygon surrounding the point
  // Polylines: find nearest polyline
  // Points: find nearest point

  NFmiEsriElementType type = establish_type(shape);

  if (options.coordinatefile.empty())
  {
    NFmiPoint latlon(options.longitude, options.latitude);

    if (type == kFmiEsriPoint)
      find_nearest_points(shape, latlon);
    else if (type == kFmiEsriPolyLine)
      find_nearest_lines(shape, latlon);
    else if (type == kFmiEsriPolygon)
      find_enclosing_polygons(shape, latlon);
    else
      throw runtime_error("Internal error while deciding shape type");
  }
  else
  {
    LocationList places = read_locationlist(options.coordinatefile);

    for (LocationList::const_iterator it = places.begin(); it != places.end(); ++it)
    {
      string name = it->first;
      NFmiPoint latlon = it->second;

      if (type == kFmiEsriPoint)
        find_nearest_points(shape, latlon, name);
      else if (type == kFmiEsriPolyLine)
        find_nearest_lines(shape, latlon, name);
      else if (type == kFmiEsriPolygon)
        find_enclosing_polygons(shape, latlon, name);
      else
        throw runtime_error("Internal error while deciding shape type");
    }
  }

  return 1;
}

// ----------------------------------------------------------------------
/*!
 * \brief Main program
 */
// ----------------------------------------------------------------------

int main(int argc, char *argv[])
{
  try
  {
    return domain(argc, argv);
  }
  catch (exception &e)
  {
    cerr << "Error: " << e.what() << endl;
  }
  catch (...)
  {
    cerr << "Error: An unknown exception occurred" << endl;
  }
  return 1;
}


// Copyright 2016-2020 Edwin Schreuder (Falcons)
// SPDX-License-Identifier: Apache-2.0

#ifndef CPERIPHERALSINTERFACETYPES_HPP
#define CPERIPHERALSINTERFACETYPES_HPP

#include <functional>

#include "falconsCommon.hpp"
#include "pose2d.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

struct piVelAcc {
	float m1_vel;
	float m2_vel;
	float m3_vel;
	float m1_acc;
	float m2_acc;
	float m3_acc;
};

struct passBall {
	bool activate;
	float bhLeftSpeed;
	float bhRightSpeed;
};

struct piDisplacement {
	float m1_pos;
	float m2_pos;
	float m3_pos;
};

struct pidSettings {
	float p;
	float i;
	float d;
	float iTh;
	int iMax;
};

struct BallhandlerBoardSettings {
	bool ballhandlerPlotEnabled;
	pidSettings pid;
	pidSettings anglePid;
	size_t maxPwmValue;
	size_t maxPwmStepValue;
};

struct MotionBoardSettings {
	bool motorPlotEnabled;
	pidSettings pid;
	size_t maxPwmValue;
	size_t maxPwmStepValue;
};

struct MotionSettings {
	boost::numeric::ublas::matrix<double> matrix;
	boost::numeric::ublas::matrix<double> inverseMatrix;
};

enum BallhandlerBoardControlMode {
	BALLHANDLER_CONTROL_MODE_OFF,
	BALLHANDLER_CONTROL_MODE_ON
};

struct BallhandlerSettings {
	BallhandlerBoardControlMode controlMode;
};

struct BallhandlerBoardSetpoints {
	float setPointAngle;
	float velocity;
};

struct MotorControllerBoardData {
	bool ledYellow;
	bool ledGreen;
	float pwm;
	float currentChannelA;
	float currentChannelB;
	float measureTime;
	float calculationTime;
	float voltage;
	float boardTemperature;
	float motorTimeout;
	int mode;
	float controllerGain;
	float setpoint;
	float error; //error = setpoint - measured_value
	float integral;//integral = integral + error*dt
	float pidOutput;// output = Kp*error + Ki*integral + Kd*derivative
	pidSettings pid;
	size_t maxPwmValue;
	size_t maxPwmStepValue;
};

struct MotionBoardData {
	double velocity;
	double velocityError;
	double displacementEncTicks;
	double displacementDistance;
	double measuredValue;
	float motorTemperature;
	float pidOutput; // pidOutput from motorControllerBoardData
	float integral; // integral from motorControllerBoardData
	float error; // proportional error from motorControllerBoardData
	float derivative; // proportional error from motorControllerBoardData
};

struct MotionBoardDataOutput {
	MotorControllerBoardData motorController;
	MotionBoardData motion;
};

struct BallhandlerBoardData {
	int tachoZero;
	pidSettings anglePidProperties;
	int tacho;
	int angle;
};

struct BallhandlerBoardDataOutput {
	MotorControllerBoardData motorController;
	BallhandlerBoardData ballhandler;
};

struct BallhandlerSetpoints {
    int angleLeft;
    int angleRight;
    float velocityLeft;
    float velocityRight;
};

struct BallhandlerFeedback {
    int angleLeft;
    int angleRight;
    float velocityLeft;
    float velocityRight;
};

// Update function for PeripheralsInterfaceMotion
typedef std::function<void()> voidFunctionType;

#endif /* CPERIPHERALSINTERFACETYPES_HPP */


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

//#include <iostream>
#include <thread>
//#include <pcl/io/pcd_io.h>
//#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>                // ndt配准文件头
#include <pcl/filters/approximate_voxel_grid.h>  // 滤波文件头
#include <pcl/visualization/pcl_visualizer.h>

#include "study_registration.h"

using namespace std::chrono_literals;

void Study_Registration::test_Icp()
{
    // 定义输入和输出点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

    // 随机填充无序点云
    cloud_in->width = 5;
    cloud_in->height = 1;
    cloud_in->is_dense = false;
    cloud_in->points.resize(cloud_in->width * cloud_in->height);
    for (size_t i = 0; i < cloud_in->points.size(); ++i) {
        cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    std::cout << "Saved " << cloud_in->points.size() << " data points to input:"
              << std::endl;
    for (size_t i = 0; i < cloud_in->points.size(); ++i)
        std::cout << "    " <<
                     cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
                     cloud_in->points[i].z << std::endl;
    *cloud_out = *cloud_in;
    std::cout << "size:" << cloud_out->points.size() << std::endl;

    // 在点云上执行简单的刚性变换，将cloud_out中的x平移0.7f米，然后再次输出数据值。
    for (size_t i = 0; i < cloud_in->points.size(); ++i)
        cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
    // 打印这些点
    std::cout << "Transformed " << cloud_in->points.size() << " data points:"
              << std::endl;
    for (size_t i = 0; i < cloud_out->points.size(); ++i)
        std::cout << "    " << cloud_out->points[i].x << " " <<
                     cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;

    // 创建IterativeClosestPoint的实例
    // setInputSource将cloud_in作为输入点云
    // setInputTarget将平移后的cloud_out作为目标点云
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);

    // 创建一个 pcl::PointCloud<pcl::PointXYZ>实例 Final 对象,存储配准变换后的源点云,
    // 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集,如果这两个点云匹配正确的话
    // （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云）,那么 icp. hasConverged()= 1 (true),
    // 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息。
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);
    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
                 icp.getFitnessScore() << std::endl;
    const pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix = icp.getFinalTransformation();
    std::cout << matrix << std::endl;
}

void Study_Registration::test_ndt(){
    // Loading first scan of room.
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("data-master/tutorials/room_scan1.pcd", *target_cloud) == -1) {
        PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
        return;
    }
    std::cout << "Loaded " << target_cloud->size() << " data points from room_scan1.pcd" << std::endl;

    // Loading second scan of room from new perspective.
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("data-master/tutorials/room_scan2.pcd", *input_cloud) == -1) {
        PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
        return;
    }
    std::cout << "Loaded " << input_cloud->size() << " data points from room_scan2.pcd" << std::endl;

    //将输入扫描过滤到原始尺寸的大约 10% 以提高配准速度。
    // Filtering input scan to roughly 10% of original size to increase speed of registration.
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);//体素大小。小立方体
    approximate_voxel_filter.setInputCloud(input_cloud);
    approximate_voxel_filter.filter(*filtered_cloud);
    std::cout << "Filtered cloud contains " << filtered_cloud->size()
              << " data points from room_scan2.pcd" << std::endl;

    // Initializing Normal Distributions Transform (NDT).
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

    //NDT 算法使用一个体素化数据结构和More-Thuente 线搜索
    // Setting scale dependent NDT parameters
    // Setting minimum transformation difference for termination condition.
    //设置终止条件的最小变化差
    ndt.setTransformationEpsilon(0.01);
    // Setting maximum step size for More-Thuente line search.
    ndt.setStepSize(0.1);
    //Setting Resolution of NDT grid structure (VoxelGridCovariance). 体素网格
    ndt.setResolution(1.0);

    // Setting max number of registration iterations.
    ndt.setMaximumIterations(35);//最大迭代次数

    // Setting point cloud to be aligned.
    ndt.setInputSource(filtered_cloud);
    // Setting point cloud to be aligned to.
    //目标点云target_cloud不需要进行滤波处理，因为NDT算法在目标点云对应的体素Voxel网格数据结构计算时，
    //不使用单个点，而是使用体素的点。即已做了降采样处理
    ndt.setInputTarget(target_cloud);

    //设置使用机器人测距法得到的粗略初始变换矩阵
    // Set initial alignment estimate found using robot odometry.
    Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translation(1.79387, 0, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

    // Calculating required rigid transform to align the input cloud to the target cloud.
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    //求出input_cloud变换到target_cloud的变换矩阵
    ndt.align(*output_cloud, init_guess);

    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
              << " score: " << ndt.getFitnessScore() << std::endl;

    // Transforming unfiltered, input cloud using found transform.
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());

    // Saving transformed input cloud.
    pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud);

    // Initializing point cloud visualizer
    pcl::visualization::PCLVisualizer::Ptr
            viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer_final->setBackgroundColor(0, 0, 0);

    // Coloring and visualizing target cloud (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            target_color(target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                   1, "target cloud");

    // Coloring and visualizing transformed input cloud (green).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            output_color(output_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                   1, "output cloud");

    // Starting visualizer
    viewer_final->addCoordinateSystem(1.0, "global");
    viewer_final->initCameraParameters();

    // Wait until visualizer window is closed.
    while (!viewer_final->wasStopped()) {
        viewer_final->spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }
}

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

// 首先从定义类型开始，以免使代码混乱
// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

//./alignment-prerejective ./data/chef.pcd ./data/rs1.pcd
void Study_Registration::test_robust(int argc, char ** argv){
    // 然后，我们实例化必要的数据容器，检查输入参数并加载对象和场景点云。
    // Point clouds
    PointCloudT::Ptr object (new PointCloudT);
    PointCloudT::Ptr object_aligned (new PointCloudT);
    PointCloudT::Ptr scene (new PointCloudT);
    FeatureCloudT::Ptr object_features (new FeatureCloudT);
    FeatureCloudT::Ptr scene_features (new FeatureCloudT);

    // Get input object and scene
    if (argc != 3)
    {
        pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
        return;
    }

    // Load object and scene
    pcl::console::print_highlight ("Loading point clouds...\n");
    if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
            pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
    {
        pcl::console::print_error ("Error loading object/scene file!\n");
        return;
    }

    // 1, 降采样 Downsample
    // 为了加快处理速度，我们使用PCL的：pcl::VoxelGrid类将对象和场景点云的采样率下采样至5 mm。
    pcl::console::print_highlight ("Downsampling...\n");
    pcl::VoxelGrid<PointNT> grid;
    const float leaf = 0.005f;
    grid.setLeafSize (leaf, leaf, leaf);
    grid.setInputCloud (object);
    grid.filter (*object);
    grid.setInputCloud (scene);
    grid.filter (*scene);

    // Estimate normals for scene
    //2, 估计场景法线 Estimate normals for scene
    pcl::console::print_highlight ("Estimating scene normals...\n");
    pcl::NormalEstimationOMP<PointNT,PointNT> nest;
    nest.setRadiusSearch (0.01);
    nest.setInputCloud (scene);
    nest.compute (*scene);

    // 3, 特征估计 Estimate features
    // 对于下采样点云中的每个点，我们现在使用PCL的pcl::FPFHEstimationOMP<>类来计算用于对齐过程中用于匹配的快速点特征直方图（FPFH）描述符。
    pcl::console::print_highlight ("Estimating features...\n");
    FeatureEstimationT fest;
    fest.setRadiusSearch (0.025);
    fest.setInputCloud (object);
    fest.setInputNormals (object);
    fest.compute (*object_features);
    fest.setInputCloud (scene);
    fest.setInputNormals (scene);
    fest.compute (*scene_features);

    // Perform alignment
    // SampleConsensusPrerejective 实现了有效的RANSAC姿势估计循环
    pcl::console::print_highlight ("Starting alignment...\n");
    pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
    align.setInputSource (object);
    align.setSourceFeatures (object_features);
    align.setInputTarget (scene);
    align.setTargetFeatures (scene_features);
    align.setMaximumIterations (50000); // Number of RANSAC iterations
    align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
    align.setCorrespondenceRandomness (5); // Number of nearest features to use
    align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
    align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
    align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
    {
        pcl::ScopeTime t("Alignment");
        align.align (*object_aligned);
    }

    if (align.hasConverged ())
    {
        // Print results
        printf ("\n");
        Eigen::Matrix4f transformation = align.getFinalTransformation ();
        pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
        pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
        pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
        pcl::console::print_info ("\n");
        pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
        pcl::console::print_info ("\n");
        pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());

        // Show alignment
        pcl::visualization::PCLVisualizer visu("Alignment");
        visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
        visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
        visu.spin ();
    }
    else
    {
        pcl::console::print_error ("Alignment failed!\n");
        return;
    }
}

void Study_Registration::test_robust_default(){
    const char* in[3];
    in[0] = "test_robust.exe";
    in[1] = "D:/Program Files (x86)/PCL 1.9.1/share/doc/pcl-1.9/tutorials/sources/alignment_prerejective/chef.pcd";
    in[2] = "D:/Program Files (x86)/PCL 1.9.1/share/doc/pcl-1.9/tutorials/sources/alignment_prerejective/rs1.pcd";

    char** chs = (char**)in;
    test_robust(3, chs);
}

//-----------------------------------------------------
#include <string>

#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // TicToc

typedef pcl::PointXYZ PointT2;
typedef pcl::PointCloud<PointT2> PointCloudT2;

bool next_iteration = false;

static void
print4x4Matrix (const Eigen::Matrix4d & matrix)
{
    printf ("Rotation matrix :\n");
    printf ("    | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
    printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
    printf ("    | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
    printf ("Translation vector :\n");
    printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}
/**
 * 此函数是查看器的回调。 当查看器窗口位于顶部时，只要按任意键，就会调用此函数。 如果碰到“空格”； 将布尔值设置为true。
 * @param event
 * @param nothing
 */
static void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
                       void*)
{
    if (event.getKeySym () == "space" && event.keyDown ())
        next_iteration = true;
}

//./interactive_icp monkey.ply 1
void Study_Registration::test_interactive_icp(int argc, char** argv)
{
    // The point clouds we will be using
    PointCloudT2::Ptr cloud_in (new PointCloudT2);  // Original point cloud
    PointCloudT2::Ptr cloud_tr (new PointCloudT2);  // Transformed point cloud
    PointCloudT2::Ptr cloud_icp (new PointCloudT2);  // ICP output point cloud

//    我们检查程序的参数，设置初始ICP迭代的次数，然后尝试加载PLY文件。
    // Checking program arguments
    if (argc < 2)
    {
        printf ("Usage :\n");
        printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);
        PCL_ERROR ("Provide one ply file.\n");
        return;
    }

    int iterations = 1;  // Default number of ICP iterations
    if (argc > 2)
    {
        // If the user passed the number of iteration as an argument
        iterations = atoi (argv[2]);
        if (iterations < 1)
        {
            PCL_ERROR ("Number of initial iterations must be >= 1\n");
            return;
        }
    }

    pcl::console::TicToc time;
    time.tic ();
    if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0)
    {
        PCL_ERROR ("Error loading cloud %s.\n", argv[1]);
        return;
    }
    std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;

    // 我们使用刚性矩阵变换来变换原始点云。
    // cloud_in包含原始点云。
    // cloud_tr和cloud_icp包含平移/旋转的点云。
    // cloud_tr是我们将用于显示的备份（绿点云）。

    // Defining a rotation matrix and translation vector
    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();

    // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
    double theta = M_PI / 8;  // The angle of rotation in radians
    transformation_matrix (0, 0) = std::cos (theta);
    transformation_matrix (0, 1) = -sin (theta);
    transformation_matrix (1, 0) = sin (theta);
    transformation_matrix (1, 1) = std::cos (theta);

    // A translation on Z axis (0.4 meters)
    transformation_matrix (2, 3) = 0.4;

    // Display in terminal the transformation matrix
    std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
    print4x4Matrix (transformation_matrix);

    // Executing the transformation
    pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);
    *cloud_tr = *cloud_icp;  // We backup cloud_icp into cloud_tr for later use

    // 这是ICP对象的创建。 我们设置ICP算法的参数。
    // setMaximumIterations（iterations）设置要执行的初始迭代次数（默认值为1）。
    // 然后，我们将点云转换为cloud_icp。 第一次对齐后，我们将在下一次使用该ICP对象时（当用户按下“空格”时）将ICP最大迭代次数设置为1。

    // The Iterative Closest Point algorithm
    time.tic ();
    pcl::IterativeClosestPoint<PointT2, PointT2> icp;
    icp.setMaximumIterations (iterations);
    icp.setInputSource (cloud_icp);
    icp.setInputTarget (cloud_in);
    icp.align (*cloud_icp);
    icp.setMaximumIterations (1);  // We set this variable to 1 for the next time we will call .align () function
    std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;

    // 检查ICP算法是否收敛； 否则退出程序。 如果返回true，我们将转换矩阵存储在4x4矩阵中，然后打印刚性矩阵转换。
    if (icp.hasConverged ())
    {
        std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
        std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
        transformation_matrix = icp.getFinalTransformation ().cast<double>();
        print4x4Matrix (transformation_matrix);
    }
    else
    {
        PCL_ERROR ("\nICP has not converged.\n");
        return;
    }

    // Visualization
    pcl::visualization::PCLVisualizer viewer ("ICP demo");
    // Create two vertically separated viewports
    int v1 (0);
    int v2 (1);
    viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
    viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);

    // The color we will be using
    float bckgr_gray_level = 0.0;  // Black
    float txt_gray_lvl = 1.0 - bckgr_gray_level;

    // Original point cloud is white
    pcl::visualization::PointCloudColorHandlerCustom<PointT2> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,
                                                                               (int) 255 * txt_gray_lvl);
    viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
    viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

    // Transformed point cloud is green
    pcl::visualization::PointCloudColorHandlerCustom<PointT2> cloud_tr_color_h (cloud_tr, 20, 180, 20);
    viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

    // ICP aligned point cloud is red
    pcl::visualization::PointCloudColorHandlerCustom<PointT2> cloud_icp_color_h (cloud_icp, 180, 20, 20);
    viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

    // Adding text descriptions in each viewport
    viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
    viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

    std::stringstream ss;
    ss << iterations;
    std::string iterations_cnt = "ICP iterations = " + ss.str ();
    viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

    // Set background color
    viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
    viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

    // Set camera position and orientation
    viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
    viewer.setSize (1280, 1024);  // Visualiser window size

    // Register keyboard callback :
    viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);

    // Display the visualiser
    while (!viewer.wasStopped ())
    {
        viewer.spinOnce ();

        // The user pressed "space" :
        if (next_iteration)
        {
            // The Iterative Closest Point algorithm
            time.tic ();
            // 如果用户按下键盘上的任意键，则会调用keyboardEventOccurred函数。 此功能检查键是否为“空格”。
            // 如果是，则全局布尔值next_iteration设置为true，从而允许查看器循环输入代码的下一部分：调用ICP对象以进行对齐。
            // 记住，我们已经配置了该对象输入/输出云，并且之前通过setMaximumIterations将最大迭代次数设置为1。

            icp.align (*cloud_icp);
            std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;

            // 和以前一样，我们检查ICP是否收敛，如果不收敛，则退出程序。
            if (icp.hasConverged ())
            {
                // printf（“ 033 [11A”）; 在终端增加11行以覆盖显示的最后一个矩阵是一个小技巧。
                // 简而言之，它允许替换文本而不是编写新行； 使输出更具可读性。 我们增加迭代次数以更新可视化器中的文本值。
                printf ("\033[11A");  // Go up 11 lines in terminal output.
                printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());

                // 这意味着，如果您已经完成了10次迭代，则此函数返回矩阵以将点云从迭代10转换为11。
                std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;

                // 函数getFinalTransformation（）返回在迭代过程中完成的刚性矩阵转换（此处为1次迭代）。
                transformation_matrix *= icp.getFinalTransformation ().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
                print4x4Matrix (transformation_matrix);  // Print the transformation between original pose and current pose

                ss.str ("");
                ss << iterations;
                std::string iterations_cnt = "ICP iterations = " + ss.str ();
                viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
                viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
            }
            else
            {
                PCL_ERROR ("\nICP has not converged.\n");
                return;
            }

            //这不是我们想要的。 如果我们将最后一个矩阵与新矩阵相乘，那么结果就是从开始到当前迭代的转换矩阵。
        }
        next_iteration = false;
    }
}


#include "common.hpp"
#include <boost/date_time/gregorian/gregorian.hpp>
#include <math.h>

po::variables_map parse_introduce_command(po::parsed_options parsed);
std::unordered_map<std::string, std::vector<std::string>> read_two_column (std::string sample_filename);
void record_clade_regions(MAT::Tree* T, std::unordered_map<std::string, std::unordered_map<std::string, float>> region_assignments, std::string filename);
size_t get_monophyletic_cladesize(MAT::Tree* T, std::unordered_map<std::string, float> assignments, MAT::Node* subroot = NULL);
float get_association_index(MAT::Tree* T, std::unordered_map<std::string, float> assignments, bool permute = false, MAT::Node* subroot = NULL);
std::pair<boost::gregorian::date,boost::gregorian::date> get_nearest_date(MAT::Tree* T, MAT::Node* n, std::set<std::string>* in_samples, std::unordered_map<std::string, std::string> datemeta = {});
std::unordered_map<std::string, float> get_assignments(MAT::Tree* T, std::unordered_set<std::string> sample_set, bool eval_uncertainty = false);
void introduce_main(po::parsed_options parsed);


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_COMBINE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_COMBINE_HPP_INCLUDED

#include <boost/simd/pack.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( combine_, tag::simd_
                          , (A0)
                          , ((scalar_<unspecified_<A0>))
                            ((scalar_<unspecified_<A0>>))
                          )
  {
    BOOST_FORCEINLINE bs::pack<A0,2> operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return {a0,a1};
    }
  };
} } }

#endif


/*
 * Copyright (C) 2017 Love Park Robotics, LLC
 *
 * 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 distribted on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <algorithm>
#include <chrono>
#include <cstdint>
#include <exception>
#include <fstream>
#include <iostream>
#include <memory>
#include <ratio>
#include <string>
#include <vector>
#include <boost/program_options.hpp>
#include <o3d3xx_camera.h>
#include <o3d3xx_oem.h>
#include <opencv2/opencv.hpp>

//
// Quantify framegrabber jitter
//

namespace po = boost::program_options;

template<typename T>
T median2d(const cv::Mat& arr)
{
  T median = 0;
  std::size_t sz = arr.rows*arr.cols;

  std::vector<T> arr_cp(sz);
  std::copy(arr.begin<T>(), arr.end<T>(), arr_cp.begin());
  std::sort(arr_cp.begin(), arr_cp.end());

  if (sz > 0)
    {
      if (sz % 2 == 0)
        {
          median =
            (arr_cp.at(sz/2-1)+arr_cp.at(sz/2))/2.;
        }
      else
        {
          median = arr_cp.at(sz/2);
        }
    }

  return median;
}

template<typename T>
T mad(const cv::Mat& arr, T center)
{
  cv::Mat arr_d;
  cv::absdiff(arr, center, arr_d);
  return median2d<T>(arr_d);
}

int main(int argc, const char** argv)
{
  std::string camera_ip;
  std::uint32_t xmlrpc_port;
  std::string password;
  int nframes;
  std::string outfile;
  cv::Mat arr;

  try
    {
      o3d3xx::CmdLineOpts opts("o3d3xx OEM Frame Grabber Jitter");

      po::options_description jitter_opts("Jitter");
      jitter_opts.add_options()
        ("nframes,f",
         po::value<int>()->default_value(100),
         "Number of frames to capture")

        ("outfile,o",
         po::value<std::string>()->default_value("-"),
         "Raw data output file, if not specified, nothing is written");

      opts.visible.add(jitter_opts);

      if (! opts.Parse(argc, argv, &camera_ip, &xmlrpc_port, &password))
        {
          return 0;
        }

      outfile = opts.vm["outfile"].as<std::string>();
      nframes = opts.vm["nframes"].as<int>();
      nframes = nframes <= 0 ? 100 : nframes;

      auto cam =
        std::make_shared<o3d3xx::Camera>(camera_ip, xmlrpc_port, password);
      auto fg = std::make_shared<o3d3xx::oem::FrameGrabber>(cam);
      auto im = std::make_shared<o3d3xx::oem::ImageBuffer>();
      // get the one-time allocations out of the way, and,
      // make sure it doesn't get optimized away by the compiler
      if (! fg->WaitForFrame(im.get(), 1000))
        {
          std::cerr << "Timeout waiting for first image acquisition!"
                    << std::endl;
          return 1;
        }

      // holds our timing results (millis)
      arr.create(nframes, 1, CV_32FC1);

      for (int i = 0; i < nframes; ++i)
        {
          auto t1 = std::chrono::high_resolution_clock::now();
          if (! fg->WaitForFrame(im.get(), 1000))
            {
              std::cerr << "Timeout waiting for image acquisition!"
                        << std::endl;
              return 1;
            }
          auto t2 = std::chrono::high_resolution_clock::now();

          std::chrono::duration<float, std::milli> fp_ms = t2 - t1;
          arr.at<float>(i, 0) = fp_ms.count();
        }

      cv::Scalar mean, stddev;
      float median = median2d<float>(arr);
      cv::meanStdDev(arr, mean, stddev);
      std::cout << "Mean:   " << mean[0] << " ms" << std::endl;
      std::cout << "Median: " <<  median << " ms" << std::endl;
      std::cout << "Stddev: " << stddev[0] << " ms" << std::endl;
      std::cout << "MAD:    " << mad(arr, median) << " ms" << std::endl;

      if (outfile != "-")
        {
          std::ofstream out;
          out.open(outfile);
          out << cv::format(arr, cv::Formatter::FMT_CSV);
          out.close();
          std::cout << "Raw data has been written to: " << outfile << std::endl;
        }
    }
  catch (const std::exception& ex)
    {
      std::cerr << "Failed to compute jitter statistics: "
                << ex.what() << std::endl;
      return 1;
    }

  return 0;
}


#include <iostream>
#include <fstream>

#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>

#include <json/json.h>

#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <procs/segmentcolour.h>
#include <procs/gridfinder.h>
#include <procs/visitors.h>
#include <procs/colourmatches.h>

void mouseCallbackHandler(int event, int x, int y, int flags, void *userdata)
{
	if (event == CV_EVENT_LBUTTONDOWN)
		std::cout << "(" << x << ", " << y << ")" << std::endl;
}

int main(int argc, char *argv[])
{
	cv::Mat image;
	Json::Value jsonOptions;

	std::vector<cv::Point2i> gridPoints;

	bool isVideo = (argc < 2) || (strlen(argv[1]) < 2);
	std::unique_ptr<cv::VideoCapture> video;
	if (isVideo)
	{
		int videoIndex = 0;
		if (argc > 1)
			videoIndex = boost::lexical_cast<int>(argv[1]);
		video.reset(new cv::VideoCapture(videoIndex));
		video->set(CV_CAP_PROP_FRAME_WIDTH, 320);
		video->set(CV_CAP_PROP_FRAME_HEIGHT, 240);
		video->read(image);
	}
	else
	{
		std::string imagePath(std::string(argv[1]) + ".png");
		std::string jsonPath(std::string(argv[1]) + ".json");

		std::cout << "Loading: '" << imagePath << "'." << std::endl;
		if (boost::filesystem::exists(imagePath))
			image = cv::imread(imagePath);
		else
			std::cerr << "'" << imagePath << "' does not exists." << std::endl;
		Json::Reader reader;
		std::ifstream streamOptions(jsonPath);
		if (streamOptions.good())
			reader.parse(streamOptions, jsonOptions, false);
		else
		{
			std::cerr << "'" << jsonPath << "' does not exists." << std::endl;
		}
	}

	Json::Value &jsonCoords = jsonOptions["coords"];

	cv::namedWindow("Original");
	cv::namedWindow("Converted");
	cv::namedWindow("Debug");

	cv::setMouseCallback("Original", mouseCallbackHandler, nullptr);

	cv::Mat out(image.rows, image.cols, image.type());
	cv::Mat debugMat(image.rows, image.cols, image.type());

	procs::SegmentColour segmentColour(image.rows * image.cols);
	procs::SegmentColourRGWeightGWeightVisitor visitor(cv::Vec2d(0.02, 0.05));
	std::vector<procs::SegmentationColourObject> results;
	std::vector<procs::SegmentationColourObject *> resultsFiltered;

	char key;

	unsigned int seq = 0;

	/*
	cv::Mat dst, cdst;
	std::cout << "1" << std::endl;
	cv::Canny(image, dst, 50, 150);
	std::cout << "2" << std::endl;
	cv::cvtColor(dst, cdst, CV_GRAY2BGR);
	std::vector<cv::Vec4i> lines;
	std::cout << "3" << std::endl;
	dst.convertTo(cdst, CV_8UC1);
	cv::HoughLinesP(cdst, lines, 1, CV_PI/180, 50);
	for( size_t i = 0; i < lines.size(); i++ )
	{
		cv::Vec4i l = lines[i];
		cv::line(image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(0,0,255), 3, CV_AA);
	}
	cv::namedWindow("lines");
	cv::imshow("lines", dst);
	cv::namedWindow("lines2");
	cv::imshow("lines2", cdst);
	 */

	for (unsigned int i = 0; isVideo || (i < 1); ++i)
	{
		if (isVideo)
		{
			results.clear();
			video->read(image);
		}

		out.setTo(cv::Scalar(0));
		debugMat.setTo(cv::Scalar(0));

		segmentColour.action(image, out, 500, 10, 100, 1, visitor, results, debugMat);
		std::cout << results.size() << " results found." << std::endl;
		resultsFiltered.clear();
		for (procs::SegmentationColourObject &obj : results)
		{
			cv::rectangle(out, obj.bBox, cv::Scalar(100, 0, 0));
			// cv::putText(out, std::to_string(std::abs(1.0 - (obj.size / (double)obj.bBox.area()))), obj.center, CV_FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(255, 255, 255));
			if (
				(std::abs(1.0 - (obj.bBox.height / (double) obj.bBox.width)) <= 0.2) // Proportion
				&& (std::abs(1.0 - (obj.size / (double) obj.bBox.area())) <= 0.1) // Area
				)
				resultsFiltered.push_back(&obj);
		}
		std::cout << resultsFiltered.size() << " filtered." << std::endl;
		std::sort(resultsFiltered.begin(), resultsFiltered.end(),
				  [](procs::SegmentationColourObject *a, procs::SegmentationColourObject *b)
				  {
					  return a->bBox.area() > b->bBox.area();
				  });
		unsigned int n = 0;
		for (const procs::SegmentationColourObject *obj : resultsFiltered)
		{
			cv::rectangle(out, obj->bBox, cv::Scalar(0, 255, 0));
			// cv::putText(out, std::to_string(n++), obj.center, CV_FONT_HERSHEY_PLAIN, 0.5, cv::Scalar(255, 255, 255));
		}

		cv::Scalar green(0, 255, 0);
		cv::Vec3b *px;
		std::vector<procs::SegmentationColourObject *> grid;
		std::vector<procs::SegmentationColourObject *> gridTmp;
		for (Json::ValueConstIterator it = jsonCoords.begin(); it != jsonCoords.end(); ++it)
		{
			cv::Point2i pt((*it)["x"].asInt(), (*it)["y"].asInt());
			cv::circle(out, pt, 5, green);
			px = out.ptr<cv::Vec3b>(pt.y, pt.x);
			px->val[0] = 0;
			px->val[1] = 255;
			px->val[2] = 0;
			gridTmp.clear();
			for (
				std::vector<procs::SegmentationColourObject *>::reverse_iterator it = resultsFiltered.rbegin();
				it != resultsFiltered.rend();
				++it
				)
			{
				if ((*it)->bBox.contains(pt))
				{
					grid.push_back(*it);
					break;
				}
			}
		}

		procs::GridFinder finder;
		finder.find(resultsFiltered, grid);
		for (const procs::SegmentationColourObject *obj : grid)
		{
			cv::rectangle(out, obj->bBox, cv::Scalar(0, 255, 0));
			// cv::putText(out, std::to_string(n++), obj->center, CV_FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255));
		}

		procs::ColourMatches colourMatches;
		std::vector<procs::SegmentationColourObjectGroup> groups;
		colourMatches.match(resultsFiltered, groups);
		n = 0;
		for (const procs::SegmentationColourObjectGroup &group : groups)
		{
			for (const procs::SegmentationColourObject* obj : group.objects)
			{
				// cv::putText(image, std::to_string(n), obj->center, CV_FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255));
			}
			++n;
		}

		cv::imshow("Original", image);
		cv::imshow("Converted", out);
		cv::imshow("Debug", debugMat);

		if (isVideo)
		{
			key = cv::waitKey(20);
			if (key == '\n')
			{
				seq++;
				cv::imwrite(std::to_string(seq) + ".png", image);
				cv::imwrite(std::to_string(seq) + "_out.png", out);
				cv::imwrite(std::to_string(seq) + "_debug.png", debugMat);
			}
			else if (key == 'q')
				break;
		}
		else
			cv::waitKey(0);
	}
	return 0;
}