#include "midgard/util.h"
#include "midgard/constants.h"
#include "midgard/distanceapproximator.h"
#include "midgard/logging.h"
#include "midgard/point2.h"
#include "midgard/polyline2.h"
#include "midgard/vector2.h"

#include <algorithm>
#include <array>
#include <cctype>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <list>
#include <sstream>
#include <stdlib.h>
#include <sys/stat.h>
#include <vector>

#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>
#include <boost/archive/iterators/transform_width.hpp>

namespace {

constexpr double RAD_PER_METER = 1.0 / 6378160.187;
constexpr double RAD_PER_DEG = valhalla::midgard::kPiDouble / 180.0;
constexpr double DEG_PER_RAD = 180.0 / valhalla::midgard::kPiDouble;

std::vector<valhalla::midgard::PointLL>
resample_at_1hz(const std::vector<valhalla::midgard::gps_segment_t>& segments) {
  std::vector<valhalla::midgard::PointLL> resampled;
  float time_remainder = 0.0;
  for (const auto& segment : segments) {
    // get the speed of this edge
    auto meters = valhalla::midgard::Polyline2<valhalla::midgard::PointLL>::Length(segment.shape);
    // trim the shape to account of the portion of the previous second that bled onto this edge
    auto to_trim = segment.speed * time_remainder;
    auto trimmed = valhalla::midgard::trim_polyline(segment.shape.cbegin(), segment.shape.cend(),
                                                    to_trim / meters, 1.f);
    // resample it at 1 second intervals
    auto second_interval =
        valhalla::midgard::resample_spherical_polyline(trimmed, segment.speed, false);
    resampled.insert(resampled.end(), second_interval.begin(), second_interval.end());
    // figure out how much of the last second will bleed into the next edge
    double intpart;
    time_remainder = std::modf((meters - to_trim) / segment.speed, &intpart);
  }
  return resampled;
}

} // namespace

namespace valhalla {
namespace midgard {

// scalar * vector operator.
Vector2 operator*(float s, const Vector2& v) {
  return Vector2(v.x() * s, v.y() * s);
}

Vector2d operator*(double s, const Vector2d& v) {
  return Vector2d(v.x() * s, v.y() * s);
}

// Trim the front of a polyline (represented as a list or vector of Point2).
// Returns the trimmed portion of the polyline. The supplied polyline is
// altered (the trimmed part is removed).
template <class container_t> container_t trim_front(container_t& pts, const float dist) {
  // Return if less than 2 points
  if (pts.size() < 2) {
    return {};
  }

  // Walk the polyline and accumulate length until it exceeds dist
  container_t result;
  result.push_back(pts.front());
  double d = 0.0f;
  for (auto p1 = pts.begin(), p2 = std::next(pts.begin()); p2 != pts.end(); ++p1, ++p2) {
    double segdist = p1->Distance(*p2);
    if ((d + segdist) > dist) {
      double frac = (dist - d) / segdist;
      auto midpoint = p1->PointAlongSegment(*p2, frac);
      result.push_back(midpoint);

      // Remove used part of polyline
      pts.erase(pts.begin(), p1);
      pts.front() = midpoint;
      return result;
    } else {
      d += segdist;
      result.push_back(*p2);
    }
  }

  // Used all of the polyline without exceeding dist
  pts.clear();
  return result;
}

void trim_shape(float start,
                PointLL start_vertex, // NOLINT
                float end,
                PointLL end_vertex, // NOLINT
                std::vector<PointLL>& shape) {
  // clip up to the start point if the start_vertex is valid
  float along = 0.f;
  if (start_vertex.IsValid()) {
    // find the spot at which we cross the distance threshold and stop
    auto current = shape.begin();
    for (; !shape.empty() && (current != shape.end() - 1) && along <= start; ++current) {
      along += (current + 1)->Distance(*current);
    }
    // we found the spot to stop for the beginning of the shape so set it to the new beginning
    *(--current) = start_vertex;
    shape.erase(shape.begin(), current);
    along = start;
  }

  // clip after the end point if the end vertex is valid
  if (end_vertex.IsValid()) {
    // find the point at which we cross the distance threshold and stop
    auto current = shape.begin();
    for (; !shape.empty() && (current != shape.end() - 1) && along <= end; ++current) {
      along += (current + 1)->Distance(*current);
    }
    // found the spot to stop for the end of the shape so set it to the new end
    *(current) = end_vertex;
    shape.erase(++current, shape.end());
  }
}

float tangent_angle(size_t index,
                    const PointLL& point,
                    const std::vector<PointLL>& shape,
                    const float sample_distance,
                    bool forward) {
  // depending on if we are going forward or backward we choose a different increment
  auto increment = forward ? -1 : 1;
  auto first_end = forward ? shape.cbegin() : shape.cend() - 1;
  auto second_end = forward ? shape.cend() - 1 : shape.cbegin();

  // u and v will be points we move along the shape until we have enough distance between them or
  // run out of points

  // move backwards until we have enough or run out
  float remaining = sample_distance;
  auto u = point;
  auto i = shape.cbegin() + index + forward;
  while (remaining > 0 && i != first_end) {
    // move along and see how much distance that added
    i += increment;
    auto d = u.Distance(*i);
    // are we done yet?
    if (remaining <= d) {
      auto coef = remaining / d;
      u = u.PointAlongSegment(*i, coef);
      return u.Heading(point);
    }
    // next one
    u = *i;
    remaining -= d;
  }

  // move forwards until we have enough or run out
  auto v = point;
  i = shape.cbegin() + index + !forward;
  while (remaining > 0 && i != second_end) {
    // move along and see how much distance that added
    i -= increment;
    auto d = v.Distance(*i);
    // are we done yet?
    if (remaining <= d) {
      auto coef = remaining / d;
      v = v.PointAlongSegment(*i, coef);
      return u.Heading(v);
    }
    // next one
    v = *i;
    remaining -= d;
  }
  return u.Heading(v);
}

// Explicit instantiations
template std::vector<PointLL> trim_front<std::vector<PointLL>>(std::vector<PointLL>&, const float);
template std::vector<Point2> trim_front<std::vector<Point2>>(std::vector<Point2>&, const float);
template std::list<PointLL> trim_front<std::list<PointLL>>(std::list<PointLL>&, const float);
template std::list<Point2> trim_front<std::list<Point2>>(std::list<Point2>&, const float);

memory_status::memory_status(const std::unordered_set<std::string>& interest) {
  // grab the vm stats from the file
  std::ifstream file("/proc/self/status");
  std::string line;
  while (std::getline(file, line)) {
    // did we find a memory metric
    if (line.find_first_of("Vm") == 0) {
      // grab the name of it and see if we care about it
      std::string name = line.substr(0, line.find_first_of(':'));
      if (interest.size() > 0 && interest.find(name) == interest.end()) {
        continue;
      }
      // try to get the number of bytes
      line.erase(std::remove_if(line.begin(), line.end(),
                                [](const char c) { return !std::isdigit(c); }),
                 line.end());
      if (line.size() == 0) {
        continue;
      }
      auto bytes = std::stod(line) * 1024.0;
      // get the units and scale
      std::pair<double, std::string> metric = std::make_pair(bytes, "b");
      for (auto unit : {"B", "KB", "MB", "GB"}) {
        metric.second = unit;
        if (metric.first > 1024.0) {
          metric.first /= 1024.0;
        } else {
          break;
        }
      }
      metrics.emplace(std::piecewise_construct, std::forward_as_tuple(name),
                      std::forward_as_tuple(metric));
    }
    line.clear();
  }
}

bool memory_status::supported() {
  struct stat s;
  return stat("/proc/self/status", &s) == 0;
}

std::ostream& operator<<(std::ostream& stream, const memory_status& s) {
  for (const auto& metric : s.metrics) {
    stream << metric.first << ": " << metric.second.first << metric.second.second << std::endl;
  }
  return stream;
}

/* This method makes use of several computations explained and demonstrated at:
 *   http://williams.best.vwh.net/avform.htm (reference no longer active)
 * New Reference:
 *   http://www.movable-type.co.uk/scripts/latlong.html
 */
template <class container_t>
container_t
resample_spherical_polyline(const container_t& polyline, double resolution, bool preserve) {
  if (polyline.size() == 0) {
    return {};
  };

  // for each point
  container_t resampled = {polyline.front()};
  resolution *= RAD_PER_METER;
  double remaining = resolution;
  auto last = resampled.back();
  for (auto p = std::next(polyline.cbegin()); p != polyline.cend(); ++p) {
    // radians
    auto lon2 = p->first * -RAD_PER_DEG;
    auto lat2 = p->second * RAD_PER_DEG;
    // how much do we have left on this segment from where we are (in great arc radians)
    // double d = 2.0 * asin(sqrt(pow(sin((resampled.back().second * RAD_PER_DEG - lat2) /
    // 2.0), 2.0) + cos(resampled.back().second * RAD_PER_DEG) * cos(lat2)
    // *pow(sin((resampled.back().first * -RAD_PER_DEG - lon2) / 2.0), 2.0)));
    auto d = (last == *p) ? 0.0
                          : acos(sin(last.second * RAD_PER_DEG) * sin(lat2) +
                                 cos(last.second * RAD_PER_DEG) * cos(lat2) *
                                     cos(last.first * -RAD_PER_DEG - lon2));
    if (std::isnan(d)) {
      // set d to 0, do not skip in case we are preserving coordinates
      d = 0.0;
    }

    // keep placing points while we can fit them
    while (d > remaining) {
      // some precomputed stuff
      auto lon1 = last.first * -RAD_PER_DEG;
      auto lat1 = last.second * RAD_PER_DEG;
      auto sd = sin(d);
      auto a = sin(d - remaining) / sd;
      auto acs1 = a * cos(lat1);
      auto b = sin(remaining) / sd;
      auto bcs2 = b * cos(lat2);
      // find the interpolated point along the arc
      auto x = acs1 * cos(lon1) + bcs2 * cos(lon2);
      auto y = acs1 * sin(lon1) + bcs2 * sin(lon2);
      auto z = a * sin(lat1) + b * sin(lat2);
      last.first = atan2(y, x) * -DEG_PER_RAD;
      last.second = atan2(z, sqrt(x * x + y * y)) * DEG_PER_RAD;
      resampled.push_back(last);
      // we just consumed a bit
      d -= remaining;
      // we need another bit
      remaining = resolution;
    }
    // we're going to the next point so consume whatever's left
    remaining -= d;
    last = *p;
    if (preserve) {
      resampled.push_back(last);
    }
  }

  // TODO: do we want to let them know remaining?

  // hand it back
  return resampled;
}

// explicit instantiations
template std::vector<PointLL>
resample_spherical_polyline<std::vector<PointLL>>(const std::vector<PointLL>&, double, bool);
template std::vector<Point2>
resample_spherical_polyline<std::vector<Point2>>(const std::vector<Point2>&, double, bool);
template std::list<PointLL>
resample_spherical_polyline<std::list<PointLL>>(const std::list<PointLL>&, double, bool);
template std::list<Point2>
resample_spherical_polyline<std::list<Point2>>(const std::list<Point2>&, double, bool);

/* Resample a polyline at uniform intervals using more accurate spherical interpolation between
 * points. The length and number of samples is specified. The interval is computed based on
 * the number of samples and the algorithm guarantees that the secified number of samples
 * is exactly produced.
 * This method makes use of several computations explained and demonstrated at:
 *   http://williams.best.vwh.net/avform.htm (reference no longer active)
 * New Reference:
 *   http://www.movable-type.co.uk/scripts/latlong.html
 */
std::vector<PointLL> uniform_resample_spherical_polyline(const std::vector<PointLL>& polyline,
                                                         const double length,
                                                         const uint32_t n) {
  if (polyline.size() == 0) {
    return {};
  }

  // Compute sample distance that splits the polyline equally to create n vertices.
  // Divisor is n-1 since there is 1 more vertex than edge on the subdivided polyline.
  double sample_distance = length / (n - 1);
  double d0 = sample_distance;

  // for each point
  std::vector<PointLL> resampled = {polyline.front()};
  sample_distance *= RAD_PER_METER;
  double remaining = sample_distance;
  PointLL last = resampled.back();
  for (auto p = std::next(polyline.cbegin()); p != polyline.cend(); ++p) {
    // Distance between this vertex and last (in great arc radians)
    auto lon2 = p->first * -RAD_PER_DEG;
    auto lat2 = p->second * RAD_PER_DEG;
    auto d = (last == *p) ? 0.0
                          : acos(sin(last.second * RAD_PER_DEG) * sin(lat2) +
                                 cos(last.second * RAD_PER_DEG) * cos(lat2) *
                                     cos(last.first * -RAD_PER_DEG - lon2));
    if (std::isnan(d)) {
      continue;
    }

    // Place resampled points on this segment as long as remaining distance is < d
    while (remaining < d) {
      // some precomputed stuff
      auto lon1 = last.first * -RAD_PER_DEG;
      auto lat1 = last.second * RAD_PER_DEG;
      auto sd = sin(d);
      auto a = sin(d - remaining) / sd;
      auto acs1 = a * cos(lat1);
      auto b = sin(sample_distance) / sd;
      auto bcs2 = b * cos(lat2);

      // find the interpolated point along the arc
      auto x = acs1 * cos(lon1) + bcs2 * cos(lon2);
      auto y = acs1 * sin(lon1) + bcs2 * sin(lon2);
      auto z = a * sin(lat1) + b * sin(lat2);
      last.first = atan2(y, x) * -DEG_PER_RAD;
      last.second = atan2(z, sqrt(x * x + y * y)) * DEG_PER_RAD;
      resampled.push_back(last);

      // Update to reduce d and update...
      d -= remaining;
      remaining = sample_distance;
    }
    // we're going to the next point so consume whatever's left
    remaining -= d;
    last = *p;
  }

  if (resampled.size() < n) {
    // Append the last polyline point
    resampled.push_back(std::move(polyline.back()));
  } else if (resampled.size() == n) {
    resampled.back() = polyline.back();
  }

  if (resampled.size() != n) {
    LOG_ERROR("resampled polyline not expected size! n: " + std::to_string(n) +
              " actual: " + std::to_string(resampled.size()) + " length: " + std::to_string(length) +
              " d: " + std::to_string(d0));
  }
  return resampled;
}

// Resample the polyline to the specified resolution. This is a faster and less precise
// method than resample_spherical_polyline.
std::vector<PointLL>
resample_polyline(const std::vector<PointLL>& polyline, const float length, const float resolution) {
  if (polyline.size() == 0) {
    return {};
  }

  // Add the first point
  std::vector<PointLL> resampled = {polyline.front()};

  // Compute sample distance that is near the resolution but splits the polyline equally
  size_t n = std::round(length / resolution);
  float sample_distance = length / n;

  // Iterate through line segments of the polyline
  float accumulated_d = 0.0f;
  auto p0 = polyline.cbegin();
  for (auto p1 = std::next(polyline.cbegin()); p1 != polyline.cend(); ++p0, ++p1) {
    // break if we have sampled enough
    if (resampled.size() == n) {
      break;
    }

    // Find distance (meters) between the 2 points of the input polyline.
    float d = p0->Distance(*p1);

    // Interpolate between the prior polyline point if we exceed the resolution
    // (including distance accumulated so far)
    if (d + accumulated_d > sample_distance) {
      float dlon = p1->first - p0->first;
      float dlat = p1->second - p0->second;

      // Form the first interpolated point
      float p = (sample_distance - accumulated_d) / d;
      resampled.emplace_back(p0->first + p * dlon, p0->second + p * dlat);

      // Continue to interpolate along the segment while accumulated distance is less than resolution
      float dp = sample_distance / d;
      while (p + dp < 1.0f && resampled.size() < n) {
        p += dp;
        resampled.emplace_back(p0->first + p * dlon, p0->second + p * dlat);
      }

      // Set the accumulated distance to the distance remaining on this segment
      accumulated_d = d * (1.0f - p);

    } else {
      // Have not accumulated enough distance. Add d to the accumulated distance
      accumulated_d += d;
    }
  }

  // Append the last polyline point
  resampled.push_back(std::move(polyline.back()));

  return resampled;
}

// Return the intersection of two infinite lines if any
template <class coord_t>
bool intersect(const coord_t& u, const coord_t& v, const coord_t& a, const coord_t& b, coord_t& i) {
  auto uv_xd = u.first - v.first;
  auto uv_yd = u.second - v.second;
  auto ab_xd = a.first - b.first;
  auto ab_yd = a.second - b.second;
  auto d_cross = uv_xd * ab_yd - ab_xd * uv_yd;
  // parallel or very close to it
  if (std::abs(d_cross) < 1e-5) {
    return false;
  }
  auto uv_cross = u.first * v.second - u.second * v.first;
  auto ab_cross = a.first * b.second - a.second * b.first;
  i.first = (uv_cross * ab_xd - uv_xd * ab_cross) / d_cross;
  i.second = (uv_cross * ab_yd - uv_yd * ab_cross) / d_cross;
  return true;
}
template bool intersect<PointLL>(const PointLL& u,
                                 const PointLL& v,
                                 const PointLL& a,
                                 const PointLL& b,
                                 PointLL& i);
template bool
intersect<Point2>(const Point2& u, const Point2& v, const Point2& a, const Point2& b, Point2& i);

// Return the intercept of the line passing through uv with the horizontal line defined by y
template <class coord_t>
typename coord_t::first_type
y_intercept(const coord_t& u, const coord_t& v, const typename coord_t::second_type y) {
  if (std::abs(u.first - v.first) < 1e-5) {
    return u.first;
  }
  if (std::abs(u.second - u.second) < 1e-5) {
    return NAN;
  }
  auto m = (v.second - u.second) / (v.first - u.first);
  auto b = u.second - (u.first * m);
  return (y - b) / m;
}
template PointXY<float>::first_type y_intercept<PointXY<float>>(const PointXY<float>&,
                                                                const PointXY<float>&,
                                                                const PointXY<float>::first_type);
template GeoPoint<float>::first_type y_intercept<GeoPoint<float>>(const GeoPoint<float>&,
                                                                  const GeoPoint<float>&,
                                                                  const GeoPoint<float>::first_type);
template PointXY<double>::first_type y_intercept<PointXY<double>>(const PointXY<double>&,
                                                                  const PointXY<double>&,
                                                                  const PointXY<double>::first_type);
template GeoPoint<double>::first_type
y_intercept<GeoPoint<double>>(const GeoPoint<double>&,
                              const GeoPoint<double>&,
                              const GeoPoint<double>::first_type);

// Return the intercept of the line passing through uv with the vertical line defined by x
template <class coord_t>
typename coord_t::first_type
x_intercept(const coord_t& u, const coord_t& v, const typename coord_t::second_type x) {
  if (std::abs(u.second - v.second) < 1e-5) {
    return u.second;
  }
  if (std::abs(u.first - v.first) < 1e-5) {
    return NAN;
  }
  auto m = (v.second - u.second) / (v.first - u.first);
  auto b = u.second - (u.first * m);
  return x * m + b;
}
template PointXY<float>::first_type x_intercept<PointXY<float>>(const PointXY<float>&,
                                                                const PointXY<float>&,
                                                                const PointXY<float>::first_type);
template GeoPoint<float>::first_type x_intercept<GeoPoint<float>>(const GeoPoint<float>&,
                                                                  const GeoPoint<float>&,
                                                                  const GeoPoint<float>::first_type);
template PointXY<double>::first_type x_intercept<PointXY<double>>(const PointXY<double>&,
                                                                  const PointXY<double>&,
                                                                  const PointXY<double>::first_type);
template GeoPoint<double>::first_type
x_intercept<GeoPoint<double>>(const GeoPoint<double>&,
                              const GeoPoint<double>&,
                              const GeoPoint<double>::first_type);

template <class container_t>
typename container_t::value_type::first_type polygon_area(const container_t& polygon) {
  typename container_t::value_type::first_type area =
      polygon.back() == polygon.front() ? 0.
                                        : (polygon.back().first + polygon.front().first) *
                                              (polygon.back().second + polygon.front().second);
  for (auto p1 = polygon.cbegin(), p2 = std::next(polygon.cbegin()); p2 != polygon.cend();
       ++p1, ++p2) {
    area += (p1->first + p2->first) * (p1->second + p2->second);
  }
  return area * .5;
}

template PointLL::first_type polygon_area(const std::list<PointLL>&);
template PointLL::first_type polygon_area(const std::vector<PointLL>&);
template Point2::first_type polygon_area(const std::list<Point2>&);
template Point2::first_type polygon_area(const std::vector<Point2>&);

std::vector<midgard::PointLL> simulate_gps(const std::vector<gps_segment_t>& segments,
                                           std::vector<float>& accuracies,
                                           float smoothing,
                                           float accuracy,
                                           size_t sample_rate,
                                           unsigned seed) {
  // resample the coords along a given edge at one second intervals
  auto resampled = resample_at_1hz(segments);

  // a way to get noise but only allow for slow change
  std::mt19937 generator(seed);
  std::uniform_real_distribution<float> distribution(-1, 1);
  ring_queue_t<std::pair<float, float>> noises(smoothing);
  auto get_noise = [&]() {
    // we generate a vector whose magnitude is no more than accuracy
    auto lon_adj = distribution(generator);
    auto lat_adj = distribution(generator);
    auto len = std::sqrt((lon_adj * lon_adj) + (lat_adj * lat_adj));
    lon_adj /= len;
    lat_adj /= len; // norm
    auto scale = (distribution(generator) + 1.f) / 2.f;
    lon_adj *= scale * accuracy;
    lat_adj *= scale * accuracy; // random scale <= accuracy
    noises.emplace_back(std::make_pair(lon_adj, lat_adj));
    // average over last n to smooth
    std::pair<float, float> noise{0, 0};
    std::for_each(noises.begin(), noises.end(), [&noise](const std::pair<float, float>& n) {
      noise.first += n.first;
      noise.second += n.second;
    });
    noise.first /= noises.size();
    noise.second /= noises.size();
    return noise;
  };
  // fill up the noise queue so the first points arent unsmoothed
  while (!noises.full()) {
    get_noise();
  }

  // for each point of the 1hz shape
  std::vector<midgard::PointLL> simulated;
  for (size_t i = 0; i < resampled.size(); ++i) {
    const auto& p = resampled[i];
    // is this a harmonic of the desired sampling rate
    if (i % sample_rate == 0) {
      // meters of noise with extremely low likelihood its larger than accuracy
      auto noise = get_noise();
      // use the number of meters per degree in both axis to offset the point by the noise
      auto metersPerDegreeLon = DistanceApproximator<PointLL>::MetersPerLngDegree(p.second);
      simulated.emplace_back(midgard::PointLL(p.first + noise.first / metersPerDegreeLon,
                                              p.second + noise.second / kMetersPerDegreeLat));
      // keep the distance to use for accuracy
      accuracies.emplace_back(simulated.back().Distance(p));
    }
  }
  return simulated;
}

polygon_t to_boundary(const std::unordered_set<uint32_t>& region, const Tiles<PointLL>& tiles) {
  // do we have this tile in this region
  auto member = [&region](int32_t tile) { return region.find(tile) != region.cend(); };
  // get the neighbor tile giving -1 if no neighbor
  auto neighbor = [&tiles](int32_t tile, int side) -> int32_t {
    if (tile == -1) {
      return -1;
    }
    auto rc = tiles.GetRowColumn(tile);
    switch (side) {
      default:
      case 0:
        return rc.second == 0 ? -1 : tile - 1;
      case 1:
        return rc.first == 0 ? -1 : tile - tiles.ncolumns();
      case 2:
        return rc.second == tiles.ncolumns() - 1 ? -1 : tile + 1;
      case 3:
        return rc.first == tiles.nrows() - 1 ? -1 : tile + tiles.ncolumns();
    }
  };
  // get the beginning coord of the counter clockwise winding given edge of the given tile
  auto coord = [&tiles](uint32_t tile, int side) -> PointLL {
    auto box = tiles.TileBounds(tile);
    switch (side) {
      default:
      case 0:
        return PointLL(box.minx(), box.maxy());
      case 1:
        return box.minpt();
      case 2:
        return PointLL(box.maxx(), box.miny());
      case 3:
        return box.maxpt();
    }
  };
  // trace a ring of the polygon
  polygon_t polygon;
  std::array<std::unordered_set<uint32_t>, 4> used;
  auto trace = [&member, &neighbor, &coord, &polygon, &used](uint32_t start_tile, int start_side,
                                                             bool ccw) {
    auto tile = start_tile;
    auto side = start_side;
    polygon.emplace_back();
    auto& ring = polygon.back();
    // walk until you see the starting edge again
    do {
      // add this edges geometry
      if (ccw) {
        ring.push_back(coord(tile, side));
      } else {
        ring.push_front(coord(tile, side));
      }
      auto inserted = used[side].insert(tile);
      if (!inserted.second) {
        throw std::logic_error("Any tile edge can only be used once as part of the geometry");
      }
      // we need to go to the first existing neighbor tile following our winding
      // starting with the one on the other side of the current side
      auto adjc = neighbor(tile, (side + 1) % 4);
      auto diag = neighbor(adjc, side);
      if (member(diag)) {
        tile = diag;
        side = (side + 3) % 4;
      } // next one keep following winding
      else if (member(adjc)) {
        tile = adjc;
      } // if neither of those were there we stay on this tile and go to the next side
      else {
        side = (side + 1) % 4;
      }
    } while (tile != start_tile || side != start_side);
  };

  // the smallest numbered tile has a left edge on the outer ring of the polygon
  auto start_tile = *region.cbegin();
  int start_side = 0;
  for (auto tile : region) {
    if (tile < start_tile) {
      start_tile = tile;
    }
  }

  // trace the outer
  trace(start_tile, start_side, true);

  // trace the inners
  for (auto start_tile : region) {
    // if the neighbor isnt a member and we didnt already use the side between them
    for (start_side = 0; start_side < 4; ++start_side) {
      if (!member(neighbor(start_tile, start_side)) &&
          used[start_side].find(start_tile) == used[start_side].cend()) {
        // build the inner ring
        if (start_side != -1) {
          trace(start_tile, start_side, false);
        }
      }
    }
  }

  // close all the rings
  for (auto& ring : polygon) {
    ring.push_back(ring.front());
  }

  // give it back
  return polygon;
}

constexpr char PADDING_ENCODED = '=';
constexpr char ZERO_ENCODED = 'A';

std::string encode64(const std::string& text) {
  using namespace boost::archive::iterators;
  using Base64Encode = base64_from_binary<transform_width<std::string::const_iterator, 6, 8>>;
  // Encode and add padding to string per octet encoding described here:
  // https://tools.ietf.org/html/rfc4648#section-4
  std::string encoded(Base64Encode(text.begin()), Base64Encode(text.end()));
  size_t num_pad_chars = (3 - text.size() % 3) % 3;
  encoded.append(num_pad_chars, PADDING_ENCODED);
  return encoded;
}

std::string decode64(const std::string& encoded) {
  using namespace boost::archive::iterators;
  using Base64Decode =
      transform_width<binary_from_base64<remove_whitespace<std::string::const_iterator>>, 8, 6>;
  // NOTE(mookerji): Ugh, for more details, see:
  // https://stackoverflow.com/questions/10521581/base64-encode-using-boost-throw-exception
  size_t num_pad_chars = (4 - encoded.size() % 4) % 4;
  std::string padded = encoded;
  padded.append(num_pad_chars, PADDING_ENCODED);
  size_t pad_chars = std::count(padded.begin(), padded.end(), PADDING_ENCODED);
  std::replace(padded.begin(), padded.end(), PADDING_ENCODED, ZERO_ENCODED);
  std::string decoded(Base64Decode(padded.begin()), Base64Decode(padded.end()));
  decoded.erase(decoded.end() - pad_chars, decoded.end());
  return decoded;
}

} // namespace midgard
} // namespace valhalla


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  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_SIMD_FUNCTION_TOUINT_S_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TOUINT_S_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/saturate.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/function/touint.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF( touint_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::saturated_tag
                            , bs::pack_<bd::int_<A0>, X>
                            )
  {
    using result = bd::as_integer_t<A0, unsigned>;
    BOOST_FORCEINLINE result operator()(const saturated_tag &, A0 const& a0) const
    {
      return bitwise_cast<result>(saturate<result>(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD( touint_
                         , (typename A0, typename X)
                         , bd::cpu_
                         , bs::saturated_tag
                         , bs::pack_<bd::uint_<A0>, X>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const saturated_tag &, A0 const& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF( touint_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::saturated_tag
                            , bs::pack_<bd::floating_<A0>, X>
                            )
  {
    using result = bd::as_integer_t<A0, unsigned>;
    BOOST_FORCEINLINE result operator()(const saturated_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using sr_t = bd::scalar_of_t<result>;
      static const A0 Vax = splat<A0>(bs::Valmax<sr_t>());
      return if_zero_else(bs::is_ngez(a0),
                          if_else(bs::is_greater_equal(a0, Vax), Valmax<result>(),
                                  touint(a0)
                                 )
                         );
    }
  };
} } }

#endif


#include <boost/program_options.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <cmath>
#include "ndt_map/ndt_map.h"
#include <fstream>
class map_converter{
  std::string ndt_map_name;
  std::string occ_map_name;
  double ndt_resolution;
  double occ_resolution;
  double lik_tr;
  int width, height;
  perception_oru::NDTMap *ndtMap;
  perception_oru::LazyGrid *mapGrid;
  std::vector<std::vector<int> > map;
public:
  map_converter(int argc, char **argv){
    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
      ("help", "produce help message")
      ("ndt-file", boost::program_options::value<std::string>(&ndt_map_name), "location of the ndt map you want to conver")
      ("ndt-resolution", boost::program_options::value<double>(&ndt_resolution)->default_value(1.), "resolution of the map")
      ("occ-file", boost::program_options::value<std::string>(&occ_map_name), "name of new occupancy map (do not put extension)")
      ("occ-resolution", boost::program_options::value<double>(&occ_resolution)->default_value(1.), "desired resolution")
      ("lik_tr", boost::program_options::value<double>(&lik_tr)->default_value(0.01), "likelihood treshold the lower value the more conservative map")
      ;
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);

    if (!vm.count("ndt-file") || !vm.count("ndt-resolution") || !vm.count("occ-file") || !vm.count("occ-resolution"))
      {
        std::cout << "Missing arguments.\n";
        std::cout << desc << "\n";
        exit(0);
      }
    if (vm.count("help"))
      {
        std::cout << desc << "\n";
        exit(0);
      }
    LoadMap();
    //std::cout << "Map loaded.\n";
    BuildMap();
    //std::cout << "Map built.\n";
    SaveMap();
    //std::cout << "Map saved.\n";
    saveYAMLFile();
    //std::cout << "Yaml created.\n";
  }

private:
  int LoadMap(){
    FILE * jffin;
    jffin = fopen(ndt_map_name.c_str(),"r+b");
    mapGrid=new perception_oru::LazyGrid(ndt_resolution);
    ndtMap=new perception_oru::NDTMap(mapGrid);
    if (ndtMap->loadFromJFF(jffin)<0){
      return -1;
    }
    return 0;
  }

  void BuildMap(){
    double xs, ys, zs;
    double xc, yc, zc;
    double minx, miny, maxx,maxy;
    ndtMap->getCentroid(xc,yc,zc);
    ndtMap->getGridSizeInMeters(xs,ys,zs);
    minx=xc-xs/2.0;
    miny=yc-ys/2.0;
    maxx=xc+xs/2.0;
    maxy=yc+ys/2.0;
    height = round(xs/occ_resolution);
    width = round(ys/occ_resolution);

    //std::cout << "minx: " << minx << "\n";
    //std::cout << "miny: " << miny << "\n";
    //std::cout << "maxx: " << maxx << "\n";
    //std::cout << "maxy: " << maxy << "\n";
    //std::cout << "H:"<<height<< " W:"<<width<<"\n";
    //std::cout << "occ_resolution: " << occ_resolution << "\n";
    //std::cout << "xc: " << xc <<"yc: " << yc << "\n";
    //std::cout << "xs: " << xs <<"ys: " << ys << "\n";

    int h=0;
    map.resize(width, std::vector<int>(height));

    for(double current_x=-xs/2+occ_resolution;current_x<xs/2-occ_resolution;current_x+=occ_resolution){

      //std::cout << "first loop \n";
      int w=0;
      for(double current_y=-ys/2+occ_resolution;current_y<ys/2-occ_resolution;current_y+=occ_resolution){
        //std::cout << "H:"<<h<< " W:"<<w<<"\n";
        perception_oru::NDTCell *check_cell;
        pcl::PointXYZ p;
        p.x=current_x;
        p.y=current_y;
        p.z=0.3;
        if(ndtMap->getCellAtPoint(p,check_cell)){
          //std::cout<<"init"<<std::endl;
          if(check_cell->getOccupancy()<0.0){
            map[w][h]=1;
          }
          else if(check_cell->getOccupancy()>0.0){
            //std::cout<<"lik "<<check_cell->getLikelihood(p)<<std::endl;
            if(check_cell->getLikelihood(p)>lik_tr){
            // Eigen::Vector3d m=check_cell->getMean();
            // Eigen::Vector3d e=check_cell->getEvals();
            // double d=sqrt((m[0]-current_x)*(m[0]-current_x)+(m[1]-current_y)*(m[1]-current_y));
            // double de=sqrt(e[0]*e[0]+e[1]*e[1]);
            // if(d<=de)
              //std::cout << "W:"<<w<< " H:"<<h<<"\n";
              map[w][h]=1;
            }
            else{
              //std::cout << "else\n";
              map[w][h] = 0;
            }
          }
        }
        w++;
      }
      h++;

    }
  }

  void SaveMap(){
    std::cout<<"saving map"<<std::endl;
    std::string png_map_file=occ_map_name;
    png_map_file+=".pgm";
    std::ofstream map_file (png_map_file.c_str());
    if (map_file.is_open())
      {
        map_file<<"P2\n";
        map_file<<width<<" "<<height<<std::endl;
        map_file<<"1\n";
        // 12.10.15 to fit the inverted map
        //for(int w=0;w<width;w++){
        //  for(int h=height-1;h>=0;h--){
        ////////////////////////////////////////////
        for(int w=width-1;w>=0;w--){
            for(int h=0;h<height;h++){
            map_file<<map[w][h]<<" ";
          }
          map_file<<std::endl;
        }
        map_file.close();
      }
    else std::cout << "Unable to open file";
  }

  void saveYAMLFile(){
    double xs, ys, zs;
    double xc, yc, zc;
    double cenx, ceny;
    ndtMap->getCentroid(xc,yc,zc);
    ndtMap->getGridSizeInMeters(xs,ys,zs);
    cenx=xc-xs/2.0;
    ceny=yc-ys/2.0;
    std::cout<<"saving yaml file"<<std::endl;
    std::string yaml_map_file=occ_map_name;
    yaml_map_file+=".yaml";
    std::ofstream map_file (yaml_map_file.c_str());
    if (map_file.is_open()){
      map_file<<"image: "<<occ_map_name<<".pgm\n";
      map_file<<"resolution: "<<occ_resolution<<std::endl;
      map_file<<"origin: ["<<cenx<<", "<<ceny<<", 0.0]\n";
      map_file<<"occupied_thresh: 0.5\n";
      map_file<<"free_thresh: 0.5\n";
      map_file<<"negate: 0\n";
      std::cout<<"YAML FILE SAVED, PLEAS EDIT PATH TO MAP!"<<std::endl;
      map_file.close();
    }
    else std::cout << "Unable to open file";
  }
};

int main(int argc, char **argv){
  map_converter mc(argc,argv);
}


/*******************************************************************************
 * Copyright (c) 2012, Dougal J. Sutherland (dsutherl@cs.cmu.edu).             *
 * All rights reserved.                                                        *
 *                                                                             *
 * Redistribution and use in source and binary forms, with or without          *
 * modification, are permitted provided that the following conditions are met: *
 *                                                                             *
 *     * Redistributions of source code must retain the above copyright        *
 *       notice, this list of conditions and the following disclaimer.         *
 *                                                                             *
 *     * Redistributions in binary form must reproduce the above copyright     *
 *       notice, this list of conditions and the following disclaimer in the   *
 *       documentation and/or other materials provided with the distribution.  *
 *                                                                             *
 *     * Neither the name of Carnegie Mellon University nor the                *
 *       names of the contributors may be used to endorse or promote products  *
 *       derived from this software without specific prior written permission. *
 *                                                                             *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE   *
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR         *
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF        *
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS    *
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN     *
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)     *
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE  *
 * POSSIBILITY OF SUCH DAMAGE.                                                 *
 ******************************************************************************/

// TODO: support regression

/* A MATLAB interface to the C++ SDM class.
 *
 * Don't use this directly; instead, use the SDM.m wrapper class. That class
 * maintains a pointer to an instance of the C++ class, which is passed into
 * this MEX function, which then calls the appropriate C++ methods.
 */

#include <cmath>
#include <cstdio>
#include <string>
#include <vector>

#include <boost/exception/all.hpp>
#include <boost/format.hpp>
#include <boost/ptr_container/ptr_vector.hpp>

#include <mex.h>

#include <svm.h>

#include <flann/flann.hpp>

#include <np-divs/div_params.hpp>
#include <np-divs/matrix_arrays.hpp>
#include <np-divs/div-funcs/from_str.hpp>

#include "sdm/sdm.hpp"
#include "sdm/kernels/gaussian.hpp"
#include "sdm/kernels/linear.hpp"
#include "sdm/kernels/polynomial.hpp"

typedef flann::Matrix<float> MatrixF;
typedef flann::Matrix<double> MatrixD;

using std::string;
using std::vector;

using npdivs::DivParams;

using sdm::SDM;
typedef SDM<float, int> SDMF;

////////////////////////////////////////////////////////////////////////////////
// Helper functions for passing class pointers between MATLAB/MEX
//
// Loosely based on code by Oliver Woodford:
//   http://www.mathworks.com/matlabcentral/newsreader/view_thread/278243

// Define types
#ifdef _MSC_VER
typedef __int32 int32_t;
typedef unsigned __int32 uint32_t;
typedef unsigned __int64 uint64_t;
#else
#include <stdint.h>
#endif

#define CLASS_HANDLE_SIGNATURE 0xa5a50f0f
template<class base>
class class_handle {
    public:
        class_handle(base* pointer) : pointer(pointer) {
            signature = CLASS_HANDLE_SIGNATURE;
        }

        ~class_handle() {
            signature = 0;
            delete pointer;
        }

        bool isValid() { return signature == CLASS_HANDLE_SIGNATURE; }

        base* getPointer() { return pointer; }

    private:
        base* pointer;
        uint32_t signature;
};

template<class base>
inline mxArray *convertPtr2Mat(class_handle<base> *ptr) {
    mxArray *out = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL);
    *((uint64_t *) mxGetData(out)) = reinterpret_cast<uint64_t>(ptr);
    return out;
}

template<class base>
inline class_handle<base> *convertMat2Ptr(const mxArray *in) {
    if (mxGetNumberOfElements(in) != 1
            || mxGetClassID(in) != mxUINT64_CLASS
            || mxIsComplex(in))
        mexErrMsgTxt("Input must be a real uint64 scalar.");

    class_handle<base> *ptr =
        reinterpret_cast<class_handle<base> *>(*((uint64_t *) mxGetData(in)));

    if (!ptr->isValid())
        mexErrMsgTxt("Handle not valid.");
    return ptr;
}

////////////////////////////////////////////////////////////////////////////////
// Helpers to convert from MATLAB to C++ types

string get_string(const mxArray *thing, const char* err_msg) {
    if (mxIsChar(thing) != 1 || mxGetM(thing) != 1)
        mexErrMsgTxt(err_msg);

    char* c_str = mxArrayToString(thing);
    string str(c_str);
    mxFree(c_str);
    return str;
}

vector<string> get_string_vec(const mxArray *thing, const char* err_msg) {
    if (!mxIsCell(thing))
        mexErrMsgTxt(err_msg);

    mwSize n = mxGetNumberOfElements(thing);
    vector<string> vec;
    vec.reserve(n);

    for (mwSize i = 0; i < n; i++)
        vec.push_back(get_string(mxGetCell(thing, i), err_msg));
    return vec;
}

double get_double(const mxArray *thing, const char* err_msg) {
    if (mxIsNumeric(thing) != 1 || mxGetNumberOfElements(thing) != 1)
        mexErrMsgTxt(err_msg);
    return mxGetScalar(thing);
}

int get_int(const mxArray *thing, const char* err_msg) {
    double d = get_double(thing, err_msg);
    int i = (int) (d+.1);
    if (std::abs(d - i) > 1e-10)
        mexErrMsgTxt(err_msg);
    return i;
}

size_t get_size_t(const mxArray *thing, const char *err_msg) {
    double d = get_double(thing, err_msg);
    if (d < 0) mexErrMsgTxt(err_msg);
    size_t s = (size_t) (d + .1);
    if (std::abs(d - s) > 1e-10)
        mexErrMsgTxt(err_msg);
    return s;
}

bool get_bool(const mxArray *thing, const char* err_msg) {
    if (mxGetNumberOfElements(thing) == 1) {
        if (mxIsLogical(thing) == 1)
            return mxGetScalar(thing) != 0;

        if (mxIsNumeric(thing) == 1) {
            double d = mxGetScalar(thing);
            if (d == 0)
                return false;
            else if (d == 1)
                return true;
        }
    }
    mexErrMsgTxt(err_msg);
    return false; // to make compilers happy, but this'll never happen
}

template <typename T, typename K>
vector<K> _convert(const void* data_void, mwSize n) {
    vector<K> vec;
    vec.reserve(n);

    T* data = (T*) data_void;
    for (mwSize i = 0; i < n; i++) {
        vec.push_back((K) data[i]);
    }
    return vec;
}

template <typename K>
vector<K> get_vector(const mxArray *thing, const char* err_msg) {
    // this ignores overflow / unsignedness, because that'd be more work
    if (!mxIsNumeric(thing) && !mxIsLogical(thing))
        mexErrMsgTxt(err_msg);

    mwSize n = mxGetNumberOfElements(thing);
    const void* data = mxGetData(thing);

    // switch on the input type, since we can only get a void*
    // ...thanks for being the worst, mex
    switch (mxGetClassID(thing)) {
        case mxINT8_CLASS:   return _convert<int8_T,   K>(data, n);
        case mxUINT8_CLASS:  return _convert<uint8_T,  K>(data, n);
        case mxINT16_CLASS:  return _convert<int16_T,  K>(data, n);
        case mxUINT16_CLASS: return _convert<uint16_T, K>(data, n);
        case mxINT32_CLASS:  return _convert<int32_T,  K>(data, n);
        case mxUINT32_CLASS: return _convert<uint32_T, K>(data, n);
        case mxINT64_CLASS:  return _convert<int64_T,  K>(data, n);
        case mxUINT64_CLASS: return _convert<uint64_T, K>(data, n);
        case mxSINGLE_CLASS: return _convert<float,    K>(data, n);
        case mxDOUBLE_CLASS: return _convert<double,   K>(data, n);
        default:
            mexErrMsgTxt(err_msg);
    }
}

////////////////////////////////////////////////////////////////////////////////
// Helper functions to convert from MATLAB matrices to flann::Matrix

// Copy a MATLAB array of type T into a flann::Matrix<K>.
template <typename T, typename K>
void copyIntoFlann(const mxArray *bag, mwSize rows, mwSize cols,
                   flann::Matrix<K> &target)
{
    const T* bag_data = (T*) mxGetData(bag);

    // copy from column-major source to row-major dest, also cast contents
    for (size_t i = 0; i < rows; i++)
        for (size_t j = 0; j < cols; j++)
            target[i][j] = (K) bag_data[j*rows + i];
}

template <typename K>
flann::Matrix<K> get_matrix(const mxArray *mat, K* data) {
    mwSize r = mxGetM(mat);
    mwSize c = mxGetM(mat);
    flann::Matrix<K> fla(data, r, c);

    switch (mxGetClassID(mat)) {
        case mxINT8_CLASS:   copyIntoFlann<int8_T,   K>(mat, r, c, fla); break;
        case mxUINT8_CLASS:  copyIntoFlann<uint8_T,  K>(mat, r, c, fla); break;
        case mxINT16_CLASS:  copyIntoFlann<int16_T,  K>(mat, r, c, fla); break;
        case mxUINT16_CLASS: copyIntoFlann<uint16_T, K>(mat, r, c, fla); break;
        case mxINT32_CLASS:  copyIntoFlann<int32_T,  K>(mat, r, c, fla); break;
        case mxUINT32_CLASS: copyIntoFlann<uint32_T, K>(mat, r, c, fla); break;
        case mxINT64_CLASS:  copyIntoFlann<int64_T,  K>(mat, r, c, fla); break;
        case mxUINT64_CLASS: copyIntoFlann<uint64_T, K>(mat, r, c, fla); break;
        case mxSINGLE_CLASS: copyIntoFlann<float,    K>(mat, r, c, fla); break;
        case mxDOUBLE_CLASS: copyIntoFlann<double,   K>(mat, r, c, fla); break;
        default: mexErrMsgTxt("unsupported bag type");
    }
    return fla;
}

// Copy a MATLAB cell array of distribution samples (with consistent number
// of columns) into a newly-allocated array of flann::Matrix<K>s.
template <typename K>
flann::Matrix<K> *get_matrix_array(const mxArray *bags, mwSize n,
        bool mat_alloc=true)
{
    typedef flann::Matrix<K> Matrix;

    if (!mxIsCell(bags))
        mexErrMsgTxt("get_matrix_array: non-cell argument");

    Matrix *flann_bags;
    if (mat_alloc)
        flann_bags = (Matrix *) mxCalloc(n, sizeof(Matrix));
    else
        flann_bags = new Matrix[n];

    mwSize rows;
    const mwSize cols = mxGetN(mxGetCell(bags, 0));

    const mxArray *bag;

    for (mwSize i = 0; i < n; i++) {
        bag = mxGetCell(bags, i);

        // check dimensions
        if (mxGetNumberOfDimensions(bag) != 2)
            mexErrMsgTxt("bag has too many dimensions");
        rows = mxGetM(bag);
        if (mxGetN(bag) != cols)
            mexErrMsgTxt("inconsistent number of columns in bags");

        // allocate the result matrix
        K* data;
        if (mat_alloc)
            data = (K*) mxCalloc(rows * cols, sizeof(K));
        else
            data = new K[rows * cols];
        flann_bags[i] = Matrix(data, rows, cols);

        switch (mxGetClassID(bag)) {
            case mxINT8_CLASS:
                copyIntoFlann<int8_T,   K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxUINT8_CLASS:
                copyIntoFlann<uint8_T,  K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxINT16_CLASS:
                copyIntoFlann<int16_T,  K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxUINT16_CLASS:
                copyIntoFlann<uint16_T, K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxINT32_CLASS:
                copyIntoFlann<int32_T,  K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxUINT32_CLASS:
                copyIntoFlann<uint32_T, K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxINT64_CLASS:
                copyIntoFlann<int64_T,  K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxUINT64_CLASS:
                copyIntoFlann<uint64_T, K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxSINGLE_CLASS:
                copyIntoFlann<float,    K>(bag, rows, cols, flann_bags[i]);
                break;

            case mxDOUBLE_CLASS:
                copyIntoFlann<double,   K>(bag, rows, cols, flann_bags[i]);
                break;

            default:
                mexErrMsgTxt("unsupported bag type");
        }
    }

    return flann_bags;
}

template <typename T>
flann::Matrix<T>* matalloc_matrix_array(size_t n, size_t rows, size_t cols) {
    typedef flann::Matrix<T> Matrix;
    Matrix* array = (Matrix*) mxCalloc(n, sizeof(Matrix));

    for (size_t i = 0; i < n; i++) {
        array[i] = Matrix((T*) mxCalloc(rows*cols, sizeof(T)), rows, cols);
    }
    return array;
}

void free_matalloced_matrix_array(flann::Matrix_ *bags, mwSize n) {
    for (mwSize i = 0; i < n; i++)
        mxFree(bags[i].ptr());
    mxFree(bags);
}

////////////////////////////////////////////////////////////////////////////////
// Helpers to convert from C++ to MATLAB types

template <typename T> struct matlab_classid {
    mxClassID mxID;
    matlab_classid() : mxID(mxUNKNOWN_CLASS) {}
};
template<> matlab_classid<int8_T>  ::matlab_classid() : mxID(mxINT8_CLASS) {}
template<> matlab_classid<uint8_T> ::matlab_classid() : mxID(mxUINT8_CLASS) {}
template<> matlab_classid<int16_T> ::matlab_classid() : mxID(mxINT16_CLASS) {}
template<> matlab_classid<uint16_T>::matlab_classid() : mxID(mxUINT16_CLASS) {}
template<> matlab_classid<int32_T> ::matlab_classid() : mxID(mxINT32_CLASS) {}
template<> matlab_classid<uint32_T>::matlab_classid() : mxID(mxUINT32_CLASS) {}
template<> matlab_classid<int64_T> ::matlab_classid() : mxID(mxINT64_CLASS) {}
template<> matlab_classid<uint64_T>::matlab_classid() : mxID(mxUINT64_CLASS) {}
template<> matlab_classid<float>   ::matlab_classid() : mxID(mxSINGLE_CLASS) {}
template<> matlab_classid<double>  ::matlab_classid() : mxID(mxDOUBLE_CLASS) {}

// make a MATLAB row vector from a vector<T>
template <typename T>
mxArray *make_vector(const vector<T> vec) {
    mwSize n = vec.size();

    mxArray* mat =
        mxCreateNumericMatrix(1, n, matlab_classid<T>().mxID, mxREAL);
    T* data = (T*) mxGetData(mat);

    for (size_t i = 0; i < n; i++)
        data[i] = vec[i];
    return mat;
}

// make a MATLAB matrix from a vector<vector<T>>
// assumes the inner vectors are of equal length
template <typename T>
mxArray *make_matrix(const vector< vector<T> > vec_matrix) {
    mwSize m = vec_matrix.size();
    mwSize n = m > 0 ? vec_matrix[0].size() : 0;

    mxClassID id = matlab_classid<T>().mxID;
    mxArray* mat = mxCreateNumericMatrix(m, n, id, mxREAL);
    T* data = (T*) mxGetData(mat);

    for (size_t i = 0; i < m; i++)
        for (size_t j = 0; j < n; j++)
            data[i + j*m] = vec_matrix[i][j];
    return mat;
}

// make a MATLAB matrix from a flann::Matrix<T>
template <typename T>
mxArray *make_matrix(const flann::Matrix<T> bag) {
    mxClassID id = matlab_classid<T>().mxID;
    mxArray* mat = mxCreateNumericMatrix(bag.rows, bag.cols, id, mxREAL);
    T* data = (T*) mxGetData(mat);

    for (size_t i = 0; i < bag.rows; i++)
        for (size_t j = 0; j < bag.cols; j++)
            data[i + j*bag.rows] = bag[i][j];
    return mat;
}

// make a MATLAB cell vector of matrices
template <typename T>
mxArray *make_matrix_cells(const flann::Matrix<T> *bags, size_t n) {
    mxArray *cells = mxCreateCellMatrix(1, n);

    for (size_t i = 0; i < n; i++)
        mxSetCell(cells, i, make_matrix(bags[i]));

    return cells;
}

////////////////////////////////////////////////////////////////////////////////
// Constructor / destructor

template <typename Scalar, typename label_type>
void destroy(SDM<Scalar, label_type> *model) {
    model->destroyModelAndProb();
    model->destroyTrainBags();
    delete model;
}

////////////////////////////////////////////////////////////////////////////////
// Training

struct TrainingOptions {
    string div_func;
    string kernel;
    int k;
    size_t tuning_folds;
    bool probability;
    size_t num_threads;
    string index_type;

    TrainingOptions() :
        div_func("l2"), kernel("gaussian"), k(3), tuning_folds(3),
        probability(false), num_threads(0), index_type("kdtree")
    {}

    void parseOpt(string name, mxArray* val) {
        if (name == "div_func") {
            div_func = get_string(val, "div_func must be a string");

        } else if (name == "kernel") {
            kernel = get_string(val, "kernel must be a string");

        } else if (name == "k") {
            k = get_size_t(val, "k must be a positive integer");
            if (k < 1)
                mexErrMsgTxt("k must be a positive integer");

        } else if (name == "tuning_folds") {
            tuning_folds = get_size_t(val,
                            "tuning_folds must be a positive integer");
            if (tuning_folds < 2)
                mexErrMsgTxt("tuning_folds must be at least 2");

        } else if (name == "probability") {
            probability = get_bool(val, "probability must be boolean");

        } else if (name == "num_threads") {
            num_threads = get_size_t(val,
                    "num_threads must be a nonnegative integer");

        } else if (name == "index") {
            index_type = get_string(val, "index must be a string");

        } else {
            mexErrMsgTxt(("unknown training option: " + name).c_str());
        }
    }

    npdivs::DivFunc* getDivFunc() const {
        return npdivs::div_func_from_str(div_func);
    }

    sdm::KernelGroup* getKernelGroup() const {
        if (kernel == "gaussian") {
            return new sdm::GaussianKernelGroup;
        } else if (kernel == "linear") {
            return new sdm::LinearKernelGroup;
        } else if (kernel == "polynomial") {
            return new sdm::PolynomialKernelGroup;
        } else {
            mexErrMsgTxt(("unkown kernel type: " + kernel).c_str());
        }
    }

    svm_parameter getSVMParams() const {
        svm_parameter svm_params = sdm::default_svm_params;
        svm_params.probability = (int) probability;
        return svm_params;
    }

    DivParams getDivParams() const {
        flann::SearchParams search_params(-1);
        return DivParams(k,
                npdivs::index_params_from_str(index_type),
                search_params,
                num_threads);
    }

    vector<double> getCvals() const {
        return sdm::default_c_vals;
    }
};

template <typename Scalar, typename label_type>
SDM<Scalar, label_type> * train(
        const mxArray* bags_m, const mxArray* labels_m,
        const mxArray* opts_m, const mxArray* divs_m)
{
    // first argument: training bags
    mwSize num_train = mxGetNumberOfElements(bags_m);
    MatrixF *bags = get_matrix_array<float>(bags_m, num_train, false);
    // XXX these bags need to live as long as the SDM does, so alloc w/ new

    // second argument: labels
    const vector<label_type> labels = get_vector<label_type>(labels_m,
            "second argument must be an array of integers"); // XXX error string
    if (labels.size() > num_train)
        mexErrMsgTxt("got more labels than bags");
    else if (labels.size() < num_train)
        mexErrMsgTxt("got more bags than labels");

    // third argument: options
    if (!mxIsStruct(opts_m) || mxGetNumberOfElements(opts_m) != 1)
        mexErrMsgTxt("train 3rd argument must be a single struct");
    mwSize nfields = mxGetNumberOfFields(opts_m);

    TrainingOptions opts;
    for (mwSize i = 0; i < nfields; i++) {
        opts.parseOpt(
                string(mxGetFieldNameByNumber(opts_m, i)),
                mxGetFieldByNumber(opts_m, 0, i));
    }

    npdivs::DivFunc* div_func = opts.getDivFunc();
    sdm::KernelGroup* kernel_group = opts.getKernelGroup();

    // fourth argument: precomputed divergences
    double *divs;
    if (divs_m == NULL) {
        divs = NULL;
    } else {
        if (mxGetNumberOfDimensions(divs_m) != 2 ||
                mxGetM(divs_m) != num_train || mxGetN(divs_m) != num_train) {
            mexWarnMsgTxt("precomputed divergences not n x n; ignoring them");
            divs = NULL;
        } else {
            divs = (double*) mxCalloc(num_train * num_train, sizeof(double));
            MatrixD divs_f = get_matrix(divs_m, divs);
        }
    }

    SDMF *model;

    try {
        // train away!
        model = sdm::train_sdm<Scalar, label_type>(
                bags, num_train, labels,
                *div_func, *kernel_group,
                opts.getDivParams(),
                opts.getCvals(),
                opts.getSVMParams(),
                opts.tuning_folds,
                divs);

    } catch (...) {
        delete kernel_group;
        delete div_func;
        if (divs != NULL)
            mxFree(divs);
        throw;
    }
    delete kernel_group;
    delete div_func;
    if (divs != NULL)
        mxFree(divs);

    return model;
}

////////////////////////////////////////////////////////////////////////////////
// Cross-validation

struct CrossValidationOptions : public TrainingOptions {
    typedef TrainingOptions super;

    size_t folds;
    size_t cv_threads;
    bool project_all;

    CrossValidationOptions() :
        super(), folds(10), cv_threads(0), project_all(true)
    {}

    void parseOpt(string name, mxArray* val) {
        if (name == "folds") {
            folds = get_size_t(val, "folds must be a positive integer");
            if (folds < 2)
                mexErrMsgTxt("folds must be at least 2");

        } else if (name == "cv_threads") {
            cv_threads = get_size_t(val,
                    "cv_threads must be a nonnegative integer");
        } else if (name == "project_all") {
            project_all = get_bool(val, "project_all must be a boolean");
        } else {
            super::parseOpt(name, val);
        }
    }
};

template <typename Scalar>
double crossvalidate(
        const mxArray* bags_m, const mxArray* labels_m, const mxArray* opts_m,
        const mxArray* divs_m)
{
    // first argument: bags
    mwSize num = mxGetNumberOfElements(bags_m);
    MatrixF *bags = get_matrix_array<float>(bags_m, num, true);
    // XXX these bags can (and should) die when we exit the function

    // second argument: labels
    const vector<int> labels = get_vector<int>(labels_m,
            "second argument must be an array of integers");
    if (labels.size() > num)
        mexErrMsgTxt("got more labels than bags");
    else if (labels.size() < num)
        mexErrMsgTxt("got more bags than labels");

    // third argument: options
    if (!mxIsStruct(opts_m) || mxGetNumberOfElements(opts_m) != 1)
        mexErrMsgTxt("crossvalidate options must be a single struct");
    mwSize nfields = mxGetNumberOfFields(opts_m);

    CrossValidationOptions opts;
    for (mwSize i = 0; i < nfields; i++) {
        opts.parseOpt(
                string(mxGetFieldNameByNumber(opts_m, i)),
                mxGetFieldByNumber(opts_m, 0, i));
    }

    npdivs::DivFunc* div_func = opts.getDivFunc();
    sdm::KernelGroup* kernel_group = opts.getKernelGroup();

    if (opts.folds > num)
        opts.folds = num;

    // fourth argument: precomputed divergences
    double *divs;
    if (divs_m == NULL) {
        divs = NULL;
    } else {
        if (mxGetNumberOfDimensions(divs_m) != 2 ||
                mxGetM(divs_m) != num || mxGetN(divs_m) != num) {
            mexWarnMsgTxt("precomputed divergences not n x n; ignoring them");
            divs = NULL;
        } else {
            divs = (double*) mxCalloc(num * num, sizeof(double));
            MatrixD divs_f = get_matrix(divs_m, divs);
        }
    }

    double acc;
    try {
        // train away!
        acc = sdm::crossvalidate<Scalar>(
                bags, num, labels,
                *div_func, *kernel_group,
                opts.getDivParams(),
                opts.folds,
                opts.project_all,
                false,
                opts.cv_threads,
                opts.getCvals(),
                opts.getSVMParams(),
                opts.tuning_folds,
                divs);

    } catch (...) {
        delete kernel_group; delete div_func;
        free_matalloced_matrix_array(bags, num);
        if (divs != NULL)
            mxFree(divs);
        throw;
    }
    delete kernel_group;
    delete div_func;
    free_matalloced_matrix_array(bags, num);
    if (divs != NULL)
        mxFree(divs);

    return acc;
}

////////////////////////////////////////////////////////////////////////////////
// Function to compute divergences
// ...yeah, it's not SDM, but the helpers are here and it's easier for now

struct DivOptions {
    vector<string> div_funcs;
    int k;
    size_t num_threads;
    string index_type;

    DivOptions() :
        k(3), num_threads(0), index_type("kdtree")
    {}

    void parseOpt(string name, mxArray* val) {
        if (name == "div_funcs") {
            div_funcs.push_back(get_string(val,
                    "div_funcs must be cell array of strings"));

        } else if (name == "k") {
            k = get_size_t(val, "k must be a positive integer");
            if (k < 1)
                mexErrMsgTxt("k must be a positive integer");

        } else if (name == "num_threads") {
            num_threads = get_size_t(val,
                    "num_threads must be a nonnegative integer");

        } else if (name == "index") {
            index_type = get_string(val, "index must be a string");

        } else {
            mexErrMsgTxt(("unknown training option: " + name).c_str());
        }
    }

    // even though this looks like object slicing, it's not, i promise
    flann::IndexParams getIndexParams() const {
        if (index_type == "linear" || index_type == "brute") {
            flann::LinearIndexParams ps;
            return ps;
        } else if (index_type == "kdtree" || index_type == "kd") {
            flann::KDTreeSingleIndexParams ps;
            return ps;
        } else {
            mexErrMsgTxt(("unknown index type: " + index_type).c_str());
        }
    }

    DivParams getDivParams() const {
        flann::SearchParams search_params(-1);
        return DivParams(k, getIndexParams(), search_params, num_threads);
    }
};

mxArray* do_divs(const mxArray *x_bags_m, const mxArray *y_bags_m,
        const mxArray *opts_m) {
    // x_bags; alloc with matlab, because they shouldn't persist
    mwSize num_x = mxGetNumberOfElements(x_bags_m);
    MatrixF *x_bags = get_matrix_array<float>(x_bags_m, num_x, true);

    // y_bags
    mwSize num_y = num_x;
    MatrixF *y_bags = NULL;

    if (y_bags_m != NULL) {
        num_y = mxGetNumberOfElements(y_bags_m);
        if (num_y == 0) {
            num_y = num_x;
        } else {
            y_bags = get_matrix_array<float>(y_bags_m, num_y, true);
        }
    }

    // third argument: options
    if (!mxIsStruct(opts_m) || mxIsEmpty(opts_m))
        mexErrMsgTxt("crossvalidate options must be a struct");
    mwSize nfields = mxGetNumberOfFields(opts_m);
    mwSize nels = mxGetNumberOfElements(opts_m);

    DivOptions opts;
    // elements other than cell arrays will be set repeatedly...
    for (mwSize outer = 0; outer < nels; outer++) {
        for (mwSize i = 0; i < nfields; i++) {
            opts.parseOpt(
                    string(mxGetFieldNameByNumber(opts_m, i)),
                    mxGetFieldByNumber(opts_m, outer, i));
        }
    }

    size_t num_df = opts.div_funcs.size();
    if (num_df == 0) {
        opts.div_funcs.push_back("l2");
        num_df = 1;
    }

    boost::ptr_vector<npdivs::DivFunc> dfs;
    for (size_t i = 0; i < num_df; i++)
        dfs.push_back(npdivs::div_func_from_str(opts.div_funcs[i]));

    // allocate space for results
    MatrixD *divs = matalloc_matrix_array<double>(num_df, num_x, num_y);

    // run it!
    npdivs::np_divs(x_bags, num_x, y_bags, num_y,
            dfs, divs, opts.getDivParams());

    // copy into output
    mxArray* divs_cell = make_matrix_cells(divs, num_df);

    // kill temp vars
    free_matalloced_matrix_array(divs, num_df);

    return divs_cell;
}

////////////////////////////////////////////////////////////////////////////////
// Dispatch function

void dispatch(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs) {
    // first arg is a string saying what the desired operation is
    string op = get_string(prhs[0], "first input must be a string");

    // TODO: configure logging
    FILELog::ReportingLevel() = logDEBUG2;

    if (op == "predict") {
        if (nrhs < 3 || nrhs > 4) mexErrMsgTxt("predict takes 2-3 arguments");
        if (nlhs < 1 || nlhs > 2) mexErrMsgTxt("predict returns 1-2 values");

        SDMF *model = convertMat2Ptr<SDMF>(prhs[1])->getPointer();

        mwSize n = mxGetNumberOfElements(prhs[2]);
        MatrixF *test_bags = get_matrix_array<float>(prhs[2], n);
        // these are allocated by matlab, so will die on mex exit

        size_t num_train = model->getNumTrain();

        double *divs = NULL;
        if (nrhs >= 4) {
            if (mxGetNumberOfDimensions(prhs[3]) != 2 ||
                    mxGetM(prhs[3]) != n ||
                    mxGetN(prhs[3]) != num_train) {
                mexWarnMsgTxt("precomputed divergences not n_test x n_train; "
                              "ignoring them");
            } else {
                divs = (double*) mxCalloc(n * num_train, sizeof(double));
                get_matrix(prhs[3], divs);
            }
        }

        // TODO: if predict becomes more efficient without vals, check here
        vector<int> preds;
        vector< vector<double> > vals;
        if (divs == NULL) {
            preds = model->predict(test_bags, n, vals);
        } else {
            model->getKernel()->transformDivergences(divs, n, num_train);
            model->predict_from_kerns(divs, n, preds, vals);
        }

        plhs[0] = make_vector(preds);
        if (nlhs >= 2)
            plhs[1] = make_matrix(vals);

        free_matalloced_matrix_array(test_bags, n);
        if (divs != NULL) mxFree(divs);

    } else if (op == "train") {
        if (nrhs < 4 || nrhs > 5) mexErrMsgTxt("train takes 3-4 arguments");
        if (nlhs != 1) mexErrMsgTxt("train returns exactly 1 output");

        const mxArray* divs = (nrhs >= 5 && !mxIsEmpty(prhs[4])) ?
                              prhs[4] : NULL;
        SDMF *model = train<float, int>(prhs[1], prhs[2], prhs[3], divs);

        class_handle<SDMF> * ptr = new class_handle<SDMF>(model);
        plhs[0] = convertPtr2Mat<SDMF>(ptr);

    } else if (op == "delete") {
        if (nrhs != 2) mexErrMsgTxt("delete needs exactly one argument");
        if (nlhs > 0) mexErrMsgTxt("delete doesn't return anything");

        destroy(convertMat2Ptr<SDMF>(prhs[1])->getPointer());

    } else if (op == "train_bags") {
        if (nrhs != 2) mexErrMsgTxt("train_bags takes exactly one argument");
        if (nlhs != 1) mexErrMsgTxt("train_bags returns exactly 1 output");

        SDMF *model = convertMat2Ptr<SDMF>(prhs[1])->getPointer();
        plhs[0] = make_matrix_cells(model->getTrainBags(),
                                    model->getNumTrain());

    } else if (op == "info") {
        if (nrhs != 2) mexErrMsgTxt("info takes exactly one argument");
        if (nlhs != 4) mexErrMsgTxt("info returns exactly 4 outputs");

        SDMF *model = convertMat2Ptr<SDMF>(prhs[1])->getPointer();
        plhs[0] = mxCreateString(model->name().c_str());
        plhs[1] = mxCreateString(model->getKernel()->name().c_str());
        plhs[2] = mxCreateString(model->getDivFunc()->name().c_str());
        plhs[3] = mxCreateDoubleScalar(model->getSVM()->param.C);

    } else if (op == "crossvalidate") {
        if (nrhs < 4 || nrhs > 5)
            mexErrMsgTxt("crossvalidate takes 3-4 arguments");
        if (nlhs != 1) mexErrMsgTxt("crossvalidate returns exactly 1 output");

        const mxArray* divs = (nrhs >= 5 && !mxIsEmpty(prhs[4])) ?
                              prhs[4] : NULL;
        double acc = crossvalidate<float>(prhs[1], prhs[2], prhs[3], divs);
        plhs[0] = mxCreateDoubleScalar(acc);

    } else if (op == "npdivs") {
        if (nrhs != 4) mexErrMsgTxt("npdivs takes exactly three arguments");
        if (nlhs != 1) mexErrMsgTxt("npdivs returns exactly 1 outputs");

        plhs[0] = do_divs(prhs[1], prhs[2], prhs[3]);

    }  else {
        mexErrMsgTxt(("Unknown operation '" + op + "'.").c_str());
    }
}

void mexFunction(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs) {
    FILE* log_fd = std::fopen("logfile.txt", "a");
    Output2FILE::Stream() = log_fd;

    try {
        dispatch(nlhs, plhs, nrhs, prhs);
    } catch (boost::exception &e) {
        FILE_LOG(logERROR) << boost::diagnostic_information(e);
        std::fclose(log_fd);
        throw;
    } catch (std::exception &e) {
        FILE_LOG(logERROR) << e.what();
        std::fclose(log_fd);
        throw;
    } catch (...) {
        std::fclose(log_fd);
        throw;
    }
    std::fclose(log_fd);
}


//
// Created by denn nevera on 2019-09-01.
//

#pragma once

#include <armadillo>
#include "dehancer/properties.hpp"
#include "dehancer/details/observable_array.hpp"

namespace dehancer {

    namespace math {

        /**
         * N-dimensions column float vector
         * @tparam N
         */
        template<size_t N>
        class float_vector: public arma::fvec::fixed<N> {
            using armfloatN = arma::fvec::fixed<N>;
        public:
            using armfloatN::armfloatN;
            float_vector& operator=(const observable::Aproxy<float_vector> &a) {
                return *this = a.get_data();
            }
        };

        /**
         * 2-dimensions column float vector
         */
        class float2: public float_vector<2> {

        public:

            using float_vector::float_vector;

            float& x() { return (*this)[0]; };
            const float& x() const { return (*this)[0]; };

            float& y() { return (*this)[1]; };
            const float& y() const { return (*this)[1]; };

            float2(const observable::Aproxy<float2> &a):float_vector(a.get_data()){}
        };

        /**
        * 3-dimensions column float vector
        */
        class float3: public float_vector<3> {

        public:
            using float_vector::float_vector;

            float& x() { return (*this)[0]; };
            const float& x() const { return (*this)[0]; };

            float& y() { return (*this)[1]; };
            const float& y() const { return (*this)[1]; };

            float& z() { return (*this)[2]; };
            const float& z() const { return (*this)[2]; };

            float3(const observable::Aproxy<float3> &a):float_vector(a.get_data()){}

        };

        /**
        * 4-dimensions column float vector
        */
        class float4: public float_vector<4> {

        public:
            using float_vector::float_vector;

            float& x() { return (*this)[0]; };
            const float& x() const { return (*this)[0]; };

            float& y() { return (*this)[1]; };
            const float& y() const { return (*this)[1]; };

            float& z() { return (*this)[2]; };
            const float& z() const { return (*this)[2]; };

            float& w() { return (*this)[3]; };
            const float& w() const { return (*this)[3]; };

            float4(const observable::Aproxy<float4> &a):float_vector(a.get_data()){}
        };
    }
}


// File: autd_types.hpp
// Project: include
// Created Date: 26/12/2020
// Author: Shun Suzuki
// -----
// Last Modified: 27/12/2020
// Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
// -----
// Copyright (c) 2020 Hapis Lab. All rights reserved.
//

#pragma once

#if WIN32
#include <codeanalysis/warnings.h>  // NOLINT
#pragma warning(push)
#pragma warning(disable : ALL_CODE_ANALYSIS_WARNINGS)
#endif
#include <Eigen/Dense>
#if WIN32
#pragma warning(pop)
#endif

namespace autd {
#ifdef USE_DOUBLE_AUTD
using Float = double;
constexpr Float ToFloat(const double f) { return f; }
#else
using Float = float;
constexpr Float ToFloat(const double f) { return static_cast<Float>(f); }
#endif

using Vector3 = Eigen::Matrix<Float, 3, 1>;
using Quaternion = Eigen::Quaternion<Float>;
}  // namespace autd


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_INTERVAL_HPP_JOFA_101014
#define BOOST_ICL_INTERVAL_HPP_JOFA_101014

#include <boost/icl/type_traits/interval_type_default.hpp>

namespace boost{ namespace icl
{

    template <class IntervalT, bool IsDiscrete, bound_type PretendedBounds, bound_type RepresentedBounds>
    struct static_interval;

    template <class DomainT, ICL_COMPARE Compare = ICL_COMPARE_INSTANCE(std::less, DomainT)>
    struct interval
    {
        typedef typename interval_type_default<DomainT,Compare>::type interval_type;
        typedef interval_type type;

#ifdef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS

        static inline interval_type open(const DomainT& low, const DomainT& up)
        {
            return
                static_interval
                < interval_type                // if the domain_type is discrete ...
                , is_discrete<typename interval_traits<interval_type>::domain_type>::value
                , interval_bounds::static_open // 'pretended' bounds will be transformed to
                , interval_bound_type<interval_type>::value // the represented bounds
                >
                ::construct(low, up);
        }

        static inline interval_type left_open(const DomainT& low, const DomainT& up)
        {
            return
                static_interval
                < interval_type
                , is_discrete<typename interval_traits<interval_type>::domain_type>::value
                , interval_bounds::static_left_open
                , interval_bound_type<interval_type>::value
                >
                ::construct(low, up);
        }

        static inline interval_type right_open(const DomainT& low, const DomainT& up)
        {
            return
                static_interval
                < interval_type
                , is_discrete<typename interval_traits<interval_type>::domain_type>::value
                , interval_bounds::static_right_open
                , interval_bound_type<interval_type>::value
                >
                ::construct(low, up);
        }

        static inline interval_type closed(const DomainT& low, const DomainT& up)
        {
            return
                static_interval
                < interval_type
                , is_discrete<typename interval_traits<interval_type>::domain_type>::value
                , interval_bounds::static_closed
                , interval_bound_type<interval_type>::value
                >
                ::construct(low, up);
        }

        static inline interval_type construct(const DomainT& low, const DomainT& up)
        { return icl::construct<interval_type>(low, up); }

#else // ICL_USE_DYNAMIC_INTERVAL_BORDER_DEFAULTS
        static inline interval_type right_open(const DomainT& low, const DomainT& up)
        { return icl::construct<interval_type>(low, up, interval_bounds::right_open()); }

        static inline interval_type left_open(const DomainT& low, const DomainT& up)
        { return icl::construct<interval_type>(low, up, interval_bounds::left_open()); }

        static inline interval_type open(const DomainT& low, const DomainT& up)
        { return icl::construct<interval_type>(low, up, interval_bounds::open()); }

        static inline interval_type closed(const DomainT& low, const DomainT& up)
        { return icl::construct<interval_type>(low, up, interval_bounds::closed()); }

        static inline interval_type construct(const DomainT& low, const DomainT& up)
        { return icl::construct<interval_type>(low, up); }

#endif
    };

    template <class IntervalT, bound_type PretendedBounds, bound_type RepresentedBounds>
    struct static_interval<IntervalT, true, PretendedBounds, RepresentedBounds>
    {// is_discrete<domain_type<IntervalT>>
        typedef typename interval_traits<IntervalT>::domain_type domain_type;

        static inline IntervalT construct(const domain_type& low, const domain_type& up)
        {
            return icl::construct<IntervalT>(
                  shift_lower(interval_bounds(PretendedBounds), interval_bounds(RepresentedBounds), low)
                , shift_upper(interval_bounds(PretendedBounds), interval_bounds(RepresentedBounds), up )
                );
        }
    };

    template <class IntervalT, bound_type PretendedBounds, bound_type RepresentedBounds>
    struct static_interval<IntervalT, false, PretendedBounds, RepresentedBounds>
    {// !is_discrete<domain_type<IntervalT>>
        typedef typename interval_traits<IntervalT>::domain_type domain_type;

        static inline IntervalT construct(const domain_type& low, const domain_type& up)
        {
            BOOST_STATIC_ASSERT((is_discrete<domain_type>::value || PretendedBounds==RepresentedBounds));
            // For domain_types that are not discrete, e.g. interval<float>
            // one of the following must hold: If you call
            // interval<T>::right_open(x,y) then interval<T>::type must be static_right_open
            // interval<T>::left_open(x,y)  then interval<T>::type must be static_left_open
            // interval<T>::open(x,y)       then interval<T>::type must be static_open
            // interval<T>::closed(x,y)     then interval<T>::type must be static_closed
            // Conversion between 'PretendedBounds' and 'RepresentedBounds' is only possible
            // for discrete domain_types.
            return icl::construct<IntervalT>(low, up);
        }
    };

}} // namespace boost icl

#endif // BOOST_ICL_INTERVAL_HPP_JOFA_101014


#pragma once
#include <Eigen/Dense>
#include "../../ConnectedComponent/ConnectedComponent/CPU/ConnectedComponent_CPU.h"
#include "../../MeshVoxelizer/MeshVoxelizer.h"
void loadCloud2Array(std::vector<float> *vertices, std::vector<unsigned int> *pointLabels,
        std::vector<unsigned int> *pointInstances, std::vector<int> *objIndices,
        ORUtils::MemoryBlock<ORUtils::Vector3<float>> *points, ORUtils::MemoryBlock<unsigned short> *labels, ORUtils::MemoryBlock<unsigned int> *instances,
        float voxelSize, bool OccupancyOnly = false, bool Fill = true, const std::string &pth_cam_extrinsic = ""){
    assert(vertices->size() / 9 == pointLabels->size());
    assert(vertices->size() / 9 == pointInstances->size());

    TICK("Rasterization");
    std::vector<float3> points_;
    std::vector<unsigned int> labels_, instances_;
    DEBUG("Voxelize Mesh\n");
    DEBUG("\tThere are %zu objects.\n", objIndices->size());
//    for(size_t i=0;i<objIndices->size();++i)
    for(int i=objIndices->size()-1;i>=0;--i)
    {
        DEBUG("\n\t [Object %d]\n", i);

        int vertice_start, vertice_end;
        if(i==0){
            vertice_start = 0;
            vertice_end = objIndices->at(i);
        } else {
            vertice_start = objIndices->at(i-1);
            vertice_end = objIndices->at(i);
        }
        assert(vertice_start%3==0);
        DEBUG("\tProcess start index (%d) to end index(%d).\n", vertice_start, vertice_end);

        Eigen::Vector3f boundary_min(0,0,0), boundary_max(0,0,0);
        for(int j=vertice_start; j < vertice_end; ++j) {
            auto check=[](const Eigen::Map<const Eigen::Vector3f> &value, Eigen::Vector3f &low, Eigen::Vector3f &height){
                for(size_t i=0;i<3;++i)
                    if(std::isinf(value.data()[i])) assert(false) ;
                for(size_t i=0;i<3;++i)
                    if(value.data()[i] < low.data()[i]) low.data()[i] = value.data()[i];
                for(size_t i=0;i<3;++i)
                    if(value.data()[i] > height.data()[i]) height.data()[i] = value.data()[i];
            };
            auto eigenMap = Eigen::Map<const Eigen::Vector3f> (&vertices->at(j * 3));
            check(eigenMap, boundary_min, boundary_max);
        }
        float3 bbox_min = make_float3(boundary_min.x(),boundary_min.y(),boundary_min.z()),bbox_max = make_float3(boundary_max.x(),boundary_max.y(),boundary_max.z());
        DEBUG("\tObject boundary found from min (%f,%f,%f) to max (%f,%f,%f).\n",
              boundary_min.x(),boundary_min.y(),boundary_min.z(), boundary_max.x(),boundary_max.y(),boundary_max.z());
        {
            bool allZero=true;
            for(size_t i=0;i<3;++i) if(boundary_min.data()[i] != 0) allZero = false;
            for(size_t i=0;i<3;++i) if(boundary_max.data()[i] != 0) allZero = false;
            if(allZero) {
                DEBUG("\tBounding box was invalid (all zeros)! Skip.\n");
                continue;
            }
            auto totalSize = (int)std::floor((bbox_max.x-bbox_min.x)*(bbox_max.y-bbox_min.y)*(bbox_max.z-bbox_min.z)/voxelSize);
            DEBUG("totalSize: %d (%f MB)\n", totalSize, float(totalSize)/1024.f/1024.f);
            if(totalSize>1e6){
                DEBUG("Object size too big. Skip!\n");
                continue;
            }
        }

        MeshVoxelizer meshVoxelizer;
        meshVoxelizer.compute(vertices->data()+vertice_start*3, pointLabels->data()+vertice_start/3,
                pointInstances->data()+vertice_start/3, (vertice_end-vertice_start)*3,bbox_min,bbox_max,voxelSize);
        std::vector<float3> points_tmp;
        std::vector<unsigned int> labels_tmp, instances_tmp;
        meshVoxelizer.getOutput(points_tmp, labels_tmp, instances_tmp);

        /// Fill objects by using connected component
        if(Fill){
            std::map<uint,uint> labelCounter, instanceCounter;
            for(size_t l=0; l < labels_tmp.size(); ++l){
                if(labelCounter.find(labels_tmp[l]) == labelCounter.end())
                    labelCounter[labels_tmp[l]] = 1;
                else
                    labelCounter[labels_tmp[l]]++;
                if (instanceCounter.find(instances_tmp[l]) == instanceCounter.end())
                    instanceCounter[instances_tmp[l]] = 1;
                else
                    instanceCounter[instances_tmp[l]]++;
            }
#ifndef NDEBUG
            for(auto l :labelCounter) printf("Before CC:[%d][%d]\n",l.first,l.second);
//        DEBUG("\tbefore CC there are %zu labels\n", labelCounter.size());
#endif

            if(labelCounter.size() == 1 && instanceCounter.size() == 1) {
                DEBUG("\tPerforming CC to fill out voxels\n");
                unsigned int *volumeTable = meshVoxelizer.getVolumeTable();
                unsigned int *labelTable = meshVoxelizer.getLabelTable();
                unsigned int *instanceTable = meshVoxelizer.getInstanceTable();
                VoxelInfo *voxelInfo = meshVoxelizer.getVoxelInfo();
                uint size = voxelInfo->gridSize.x * voxelInfo->gridSize.y *
                            voxelInfo->gridSize.z;
                auto *volume_out = new unsigned int[size];
                uint min_size = std::min(voxelInfo->gridSize.x,std::min(voxelInfo->gridSize.y,voxelInfo->gridSize.z));
                uint3 blockDims = {min_size, min_size, min_size};
                SCFUSION::ConnectedComponent_CPU cc;
                cc.process(volumeTable, volume_out, &voxelInfo->gridSize.x, &blockDims.x, 3);

                // calculate size of each segment
                std::map<uint,uint> ccMap;
                for(uint l=0;l<size;++l) {
                    if (ccMap.find(volume_out[l]) == ccMap.end())
                        ccMap[volume_out[l]] = 0;
                    ccMap[volume_out[l]]++;
                }

                if(ccMap.size() > 2) {
                    // locate the target segment
                    size_t idx_target_segment=0, idx_max_segment = 0,max_segment_count = 0;
                    for(const auto &segment:ccMap) {
                        if (segment.second == labelCounter.begin()->second)
                            idx_target_segment = segment.first;
                        if (segment.second > max_segment_count) {
                            idx_max_segment = segment.first;
                            max_segment_count = segment.second;
                        }
                    }
                    DEBUG("target_segment: %zu\n", idx_target_segment);
                    DEBUG("max_segment: %zu\n", idx_max_segment);

                    for(uint l=0;l<size;++l) {
                        if(volume_out[l] != idx_target_segment && volume_out[l] != idx_max_segment) {
                            volumeTable[l]=1;
                            labelTable[l] = labelCounter.begin()->first;
                            instanceTable[l]=instanceCounter.begin()->first;
                        }
                    }

#ifndef NDEBUG
                    printf("[before] cc: \n");
                    for(auto l : ccMap)
                        printf("\t[%d]:[%d]\n",l.first,l.second);

                    ccMap.clear();
                    cc.process(volumeTable, volume_out, &voxelInfo->gridSize.x, &blockDims.x, 3);
                    for(uint l=0;l<size;++l) {
                        if(ccMap.find(volume_out[l]) == ccMap.end())
                            ccMap[volume_out[l]] = 1;
                        else
                            ccMap[volume_out[l]]++;
                    }
                    printf("[after] cc: \n");
                    for(auto l : ccMap)
                        printf("\t[%d]:[%d]\n",l.first,l.second);
#endif
                }

                delete []volume_out;
            } else
                DEBUG("\tMore than one label were found. Skip CC\n");

            meshVoxelizer.getOutput(points_tmp, labels_tmp, instances_tmp);

#ifndef NDEBUG
            labelCounter.clear();instanceCounter.clear();
            for(size_t l=0; l < labels_tmp.size(); ++l){
                if(labelCounter.find(labels_tmp[l]) == labelCounter.end())
                    labelCounter[labels_tmp[l]] = 1;
                else
                    labelCounter[labels_tmp[l]]++;
                if (instanceCounter.find(instances_tmp[l]) == instanceCounter.end())
                    instanceCounter[instances_tmp[l]] = 1;
                else
                    instanceCounter[instances_tmp[l]]++;
            }
            for(auto l :labelCounter) printf("After CC:[%d][%d]\n",l.first,l.second);
//        DEBUG("\tAfter CC there are %zu labels\n", labelCounter.size());
#endif
        }

        points_.insert(points_.end(), make_move_iterator(points_tmp.begin()), make_move_iterator(points_tmp.end()));
        labels_.insert(labels_.end(), make_move_iterator(labels_tmp.begin()), make_move_iterator(labels_tmp.end()));
        instances_.insert(instances_.end(), make_move_iterator(instances_tmp.begin()), make_move_iterator(instances_tmp.end()));
    }
    TOCK("Rasterization");

    TICK("Copy Container to Vector3f");
    points->Resize(points_.size());
    labels->Resize(points_.size());
    instances->Resize(points_.size());

    auto points_cpu = points->GetData(MEMORYDEVICE_CPU);
    auto labels_cpu = labels->GetData(MEMORYDEVICE_CPU);
    auto instances_cpu = instances->GetData(MEMORYDEVICE_CPU);
    for(size_t i=0; i < points_.size(); ++i){
        points_cpu[i].x = points_[i].x;
        points_cpu[i].y = points_[i].y;
        points_cpu[i].z = points_[i].z;

        if(OccupancyOnly) {
            labels_cpu[i] = 1;
            instances_cpu[i]=0;
        } else {
            labels_cpu[i] = labels_[i];
            instances_cpu[i] = instances_[i];
        }

    }
    points->UpdateDeviceFromHost();
    labels->UpdateDeviceFromHost();
    instances->UpdateDeviceFromHost();
    TOCK("Copy Container to Vector3f");
}


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#ifdef BOOST_TEST_MAIN
#  undef BOOST_TEST_MAIN
#endif
#include <mc_control/api.h>
#include <mc_control/mc_controller.h>
#include <mc_rtc/logging.h>
#include <mc_tasks/CoMTask.h>
#include <mc_tasks/OrientationTask.h>

#include <boost/test/unit_test.hpp>

#ifndef M_PI
#  include <boost/math/constants/constants.hpp>
#  define M_PI boost::math::constants::pi<double>()
#endif

namespace mc_control
{

struct MC_CONTROL_DLLAPI TestOrientationTaskController : public MCController
{
public:
  TestOrientationTaskController(std::shared_ptr<mc_rbdyn::RobotModule> rm, double dt) : MCController(rm, dt)
  {
    // Check that the default constructor loads the robot + ground environment
    BOOST_CHECK_EQUAL(robots().robots().size(), 2);
    // Check that JVRC-1 was loaded
    BOOST_CHECK_EQUAL(robot().name(), "jvrc1");
    solver().addConstraintSet(contactConstraint);
    solver().addConstraintSet(dynamicsConstraint);
    postureTask->stiffness(1);
    postureTask->weight(1);
    solver().addTask(postureTask.get());
    solver().setContacts(
        {mc_rbdyn::Contact(robots(), "LeftFoot", "AllGround"), mc_rbdyn::Contact(robots(), "RightFoot", "AllGround")});

    /* Create and add the position task with the default stiffness/weight */
    oriTask = std::make_shared<mc_tasks::OrientationTask>("R_WRIST_Y_S", robots(), 0);
    solver().addTask(oriTask);

    comTask = std::make_shared<mc_tasks::CoMTask>(robots(), 0);
    solver().addTask(comTask);

    mc_rtc::log::success("Created TestOrientationTaskController");
  }

  virtual bool run() override
  {
    bool ret = MCController::run();
    BOOST_CHECK(ret);
    nrIter++;
    if(nrIter == 1500)
    {
      /* Check that the task is "finished" */
      BOOST_CHECK_SMALL(oriTask->eval().norm(), 1e-3);
      BOOST_CHECK_SMALL(oriTask->speed().norm(), 1e-3);

      /* Apply dimWeight to ignore rotation around z */
      oriTask->dimWeight(Eigen::Vector3d(1., 1., 0.));
      oriTask->orientation(oriTask->orientation() * sva::RotZ<double>(-M_PI / 2));
    }
    if(nrIter == 3000)
    {
      /* At this point the task error on z should be significant */
      BOOST_CHECK(fabs(oriTask->eval().z() - M_PI / 2) > 0.1);
      /* But the task speed on z should be small */
      BOOST_CHECK_SMALL(fabs(oriTask->speed().z()), 1e-2);

      /* Reset the task and ask to raise the hand by 15 cm using only the
       * right arm joints */
      oriTask->reset();
      oriTask->dimWeight(Eigen::Vector3d(1., 1., 1.));
      oriTask->selectActiveJoints(solver(), active_joints);
      oriTask->orientation(oriTask->orientation() * sva::RotZ<double>(-M_PI / 2));
    }
    if(nrIter == 4500)
    {
      /* Check that the task is "finished" */
      BOOST_CHECK_SMALL(oriTask->eval().norm(), 1e-2);
      BOOST_CHECK_SMALL(oriTask->speed().norm(), 1e-2);

      /* Now move the hand down again, forbid elbow pitch movement */
      oriTask->reset();
      oriTask->selectUnactiveJoints(solver(), {"R_ELBOW_P"});
      orig_rep = robot().mbc().q[robot().jointIndexByName("R_ELBOW_P")][0];
      oriTask->orientation(oriTask->orientation() * sva::RotZ<double>(M_PI / 2));

      comTask->selectUnactiveJoints(solver(), {"R_ELBOW_P"});

      /* Also reset the joint target in posture task */
      auto p = postureTask->posture();
      p[robot().jointIndexByName("R_ELBOW_P")][0] = orig_rep;
      postureTask->posture(p);
    }
    if(nrIter == 6000)
    {
      /* Check that the task is "finished" */
      BOOST_CHECK_SMALL(oriTask->eval().norm(), 1e-2);
      BOOST_CHECK_SMALL(oriTask->speed().norm(), 1e-2);

      /* And that R_ELBOW_P didn't move. Note that the error is not so
       * small because of other tasks' interaction */
      double current_rep = robot().mbc().q[robot().jointIndexByName("R_ELBOW_P")][0];
      BOOST_CHECK_SMALL(fabs(orig_rep - current_rep), 5e-2);
    }
    return ret;
  }

  virtual void reset(const ControllerResetData & reset_data) override
  {
    MCController::reset(reset_data);
    /* Reset the task to the current end-effector position */
    oriTask->reset();
    comTask->reset();
    /* Align the wrist X axis with gravity */
    oriTask->orientation(sva::RotY(-M_PI / 2));
  }

private:
  unsigned int nrIter = 0;
  std::shared_ptr<mc_tasks::OrientationTask> oriTask = nullptr;
  std::shared_ptr<mc_tasks::CoMTask> comTask = nullptr;
  std::vector<std::string> active_joints = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_P",
                                            "R_ELBOW_Y",    "R_WRIST_R",    "R_WRIST_Y"};
  double orig_rep = 0;
};

} // namespace mc_control

SIMPLE_CONTROLLER_CONSTRUCTOR("TestOrientationTaskController", mc_control::TestOrientationTaskController)