#include "stdafx.h"
#include "contest_types.h"
#include "judge.h"
#include "timer.h"
#include "solver_registry.h"

#include <boost/geometry.hpp>
#include <queue>
#include <set>
#include <fmt/core.h>
#include "visual_editor.h"
using vec = std::vector<integer>;
using mat = std::vector<vec>;

integer lower_limit(integer len_given, integer epsilon) {
  auto tmp = len_given * epsilon;
  tmp /= 1000000;
  return len_given - tmp;
}

integer upper_limit(integer len_given, integer epsilon) {
  auto tmp = len_given * epsilon;
  tmp /= 1000000;
  return len_given + tmp;
}

mat create_vertices_distances(SProblemPtr problem) {
  int V = problem->vertices.size();
  mat vertices_distances(V, vec(V, 0));

  for (const auto& e : problem->edges) {
    vertices_distances[e.first][e.second] = distance2(problem->vertices[e.first], problem->vertices[e.second]);
    vertices_distances[e.second][e.first] = vertices_distances[e.first][e.second];
  }
  return vertices_distances;

}
struct vec_holenum {
  std::vector<int> v2h;
  std::vector<int> h2v;
  int H;
  int get_holenum(int x) const { return v2h[x]; }
  int get_vecnum(int x) const { return h2v[x]; }

  vec_holenum(int H_) : H(H_) {
    for (int i = 0; i < H; i++) v2h.push_back(i);
    for (int i = 0; i < H; i++) h2v.push_back(i);
  }

  void Swap(int a, int b) {
    if (a == b) return;
    int A = v2h[a], B = v2h[b];
    v2h[a] = B;
    v2h[b] = A;
    h2v[A] = b;
    h2v[B] = a;
  }

  void Change_V2H(int v, int h) {
    if (v2h[v] == h) return;
    int prev_v = h2v[h];
    Swap(v, prev_v);
  }

  std::vector<int> decided_points(const std::vector<int>& v) const {
    std::vector<int> ret(H);
    for (int i = 0; i < H; i++) ret[v2h[i]] = v[i];
    return ret;
  }
  void shuffle(std::mt19937_64& rng) {
    std::uniform_int_distribution rand(0, H - 1);
    Swap(0, rand(rng));
    Swap(1, rand(rng));
    for (int i = 0; i < 5; i++) Swap(rand(rng), rand(rng));
    //for (int i = 0; i < H; i++) LOG(INFO) << v2h[i] << ' ' << h2v[i];
  }
  void check_valid() const {
    for (int i = 0; i < H; i++) LOG(INFO) << i << ' ' << get_vecnum(i) << ' ' << get_holenum(get_vecnum(i));
  }
};

struct SVOI {
public:
  integer x_min;
  integer x_max;
  integer y_min;
  integer y_max;

  SVOI() {
    x_min = INT64_MAX, x_max = INT64_MIN;
    y_min = INT64_MAX, y_max = INT64_MIN;
  }
  SVOI(const SVOI& rhs) : x_min(rhs.x_min), x_max(rhs.x_max), y_min(rhs.y_min), y_max(rhs.y_max) {}
  SVOI(const std::vector<Point>& points) {
    x_min = INT64_MAX, x_max = INT64_MIN;
    y_min = INT64_MAX, y_max = INT64_MIN;
    for (const auto [x, y] : points) {
      x_min = std::min(x, x_min);
      x_max = std::max(x, x_max);
      y_min = std::min(y, y_min);
      y_max = std::max(y, y_max);
      x_max++;
      y_max++;
    }
  }
  SVOI(const Point& point, integer length) {
    integer len = std::floor(std::sqrt(length)) + 1;
    x_min = get_x(point) - len;
    x_max = get_x(point) + len + 1;
    y_min = get_y(point) - len;
    y_max = get_y(point) + len + 1;

  }
  SVOI operator&(SVOI rhs) const {
    SVOI ret;
    ret.x_min = std::max(x_min, rhs.x_min);
    ret.x_max = std::min(x_max, rhs.x_max);
    ret.y_min = std::max(y_min, rhs.y_min);
    ret.y_max = std::min(y_max, rhs.y_max);
    return ret;
  }
  void operator&=(SVOI rhs) {
    x_min = std::max(x_min, rhs.x_min);
    x_max = std::min(x_max, rhs.x_max);
    y_min = std::max(y_min, rhs.y_min);
    y_max = std::min(y_max, rhs.y_max);
  }
  SVOI operator^(SVOI rhs) const {
    SVOI ret;
    ret.x_min = std::min(x_min, rhs.x_min);
    ret.x_max = std::max(x_max, rhs.x_max);
    ret.y_min = std::min(y_min, rhs.y_min);
    ret.y_max = std::max(y_max, rhs.y_max);
    return ret;
  }
  void operator^=(SVOI rhs) {
    x_min = std::min(x_min, rhs.x_min);
    x_max = std::max(x_max, rhs.x_max);
    y_min = std::min(y_min, rhs.y_min);
    y_max = std::max(y_max, rhs.y_max);
  }

  bool is_valid() {
    if (x_min >= x_max) return false;
    if (y_min >= y_max) return false;
    return true;
  }

  integer size() {
    return this->is_valid() ? (x_max - x_min) * (y_max - y_min) : -1;
  }

  std::vector<Point> const all_points() {
    std::vector<Point> ret;
    for (auto x = x_min; x < x_max; x++) for (auto y = y_min; y < y_max; y++) ret.push_back(std::make_pair(x, y));
    return ret;
  }

};

constexpr integer LARGE = 1e15;
constexpr integer VERYLARGE = 1e17;

struct Striction {
  int vertex;
  std::vector<Point> consider_points;
  std::vector<int> use;
  Striction(int vertex_, int Size) : vertex(vertex_) {
    consider_points.resize(Size, std::make_pair(0, 0));
    use.resize(Size, false);
  }
  Striction(const Striction& rhs) : vertex(rhs.vertex), consider_points(rhs.consider_points), use(rhs.use) {}
  void Add(int x, Point p, const mat& vertices_distances) { if (vertices_distances[x][vertex])consider_points[x] = p; use[x] = 1; }
  void Delete(int x) { consider_points[x] = std::make_pair(0, 0); use[x] = 0; }
  void Set(const std::vector<int> cand_points, SSolutionPtr solution, const mat& vertices_distances) {
    for (auto e : cand_points) if (vertices_distances[e][vertex]) {
      use[e] = true;
      consider_points[e] = solution->vertices[e];
    }
  }
  bool operator<(const Striction& rhs) const {
    if (vertex != rhs.vertex) return vertex < rhs.vertex;
    return consider_points != rhs.consider_points ? consider_points < rhs.consider_points : use < rhs.use;
  }
  bool operator>(const Striction& rhs) const {
    if (vertex != rhs.vertex) return vertex > rhs.vertex;
    return consider_points != rhs.consider_points ? consider_points > rhs.consider_points : use > rhs.use;
  }
  bool operator==(const Striction& rhs) const {
    if (vertex != rhs.vertex) return false;
    return consider_points != rhs.consider_points ? false : use == rhs.use;
  }
};
std::ostream& operator<<(std::ostream& os, const Striction& s) {
  os << std::endl;
  os << "vertex:::" << s.vertex << std::endl;
  for (int i = 0; i < s.use.size(); i++) if (s.use[i]) os << "i is " << i;
  return os;
}

struct MapValue {
  bool available;
  SVOI voi;
  std::vector<Point> cands;
  MapValue() : available(false) {
    cands = {};
  }
  MapValue(const SVOI& voi_, const std::vector<Point> cands_) : available(true), voi(voi_), cands(cands_) {}
  //MapValue(const MapValue& rhs) : available(rhs.available), voi(rhs.voi), cands(rhs.cands) {}

};

using HashMap = std::map<Striction, MapValue>;
bool is_point_valid(SProblemPtr problem, SSolutionPtr solution, const Striction& striction, const mat& vertices_distances, Point& point) {
  int vertex = striction.vertex;
  for (int e = 0; e < striction.consider_points.size(); e++) if (striction.use[e]) {
    auto length = distance2(solution->vertices[e], point);
    if (!tolerate(vertices_distances[e][vertex], length, problem->epsilon)) return false;
  }
  return true;
}

MapValue able_points(SProblemPtr problem, SSolutionPtr solution, const Striction& striction, SVOI voi, const mat& vertices_distances, long long& counter) {
  counter++;
  //LOG(INFO) << "arrived able";
  std::vector<Point> ret = {};
  if (counter > LARGE) return MapValue(voi, ret);
  int vertex = striction.vertex;
  int min_e = -1;
  integer min_len = 1e18;
  for (int e = 0; e < striction.consider_points.size(); e++) if (striction.use[e]) {
    SVOI tmp(solution->vertices[e], upper_limit(vertices_distances[e][vertex], problem->epsilon));
    voi &= tmp;
    if (min_len > vertices_distances[e][vertex]) {
      min_e = e;
      min_len = vertices_distances[e][vertex];
    }
  }
  //LOG(INFO) << "voi valid is " << voi.is_valid();
  //LOG(INFO) << "min_e is " << min_e;
  if (!voi.is_valid()) return MapValue(voi, ret);
  //LOG(INFO) << "voi size is " << voi.size();
  if (min_e < 0) return MapValue();
  integer e_max2 = upper_limit(vertices_distances[min_e][vertex], problem->epsilon);
  integer e_min2 = lower_limit(vertices_distances[min_e][vertex], problem->epsilon);
  for (integer y = voi.y_min; y < voi.y_max; y++) {
    integer X = solution->vertices[min_e].first;
    integer len = abs(solution->vertices[min_e].second - y);
    integer len2 = (len * len);
    if (len2 <= e_max2) {
      integer max_len2 = e_max2 - len2;
      integer min_len2 = e_min2 - len2;
      integer max_len = std::ceil(std::sqrt(max_len2));

      if (min_len2 <= 0) {
        integer x_min = std::max(X - max_len, voi.x_min);
        integer x_max = std::min(X + max_len + 1, voi.x_max);
        for (integer x = x_min; x < x_max; x++) {
          Point point = std::make_pair(x, y);
          if (is_point_valid(problem, solution, striction, vertices_distances, point)) ret.push_back(point);
        }
      }

      else {
        integer min_len = std::floor(std::sqrt(min_len2));
        integer x_min = std::max(X - max_len, voi.x_min);
        integer x_max = std::min(X - min_len + 1, voi.x_max);
        for (integer x = x_min; x < x_max; x++) {
          Point point = std::make_pair(x, y);
          if (is_point_valid(problem, solution, striction, vertices_distances, point)) ret.push_back(point);
        }
        x_min = std::max(X + min_len, voi.x_min);
        x_max = std::min(X + max_len + 1, voi.x_max);
        for (integer x = x_min; x < x_max; x++) {
          Point point = std::make_pair(x, y);
          if (is_point_valid(problem, solution, striction, vertices_distances, point)) ret.push_back(point);
        }
      }

    }
  }
  return MapValue(voi, ret);
}

std::vector<Point> able_points_hash(SProblemPtr problem, SSolutionPtr solution, const Striction& str_holes, const Striction& str_inside, HashMap& hash_map_decided, SVOI voi, const mat& vertices_distances, HashMap& hash_map_unrestricted, long long& counter) {
  counter++;
 // LOG(INFO) << "arrived hash able";
  std::vector<Point> ret = {};
  if (counter > LARGE) return ret;
  int vertex = str_holes.vertex;
  if (hash_map_unrestricted.count(str_inside)) return hash_map_unrestricted[str_inside].cands;
  if (!hash_map_decided.count(str_holes)) {
    for (int e = 0; e < str_holes.consider_points.size(); e++) {
      if (str_holes.use[e]) {
        SVOI tmp(solution->vertices[e], upper_limit(vertices_distances[e][vertex], problem->epsilon));
        voi &= tmp;
      }
    }
    if (!voi.is_valid()) {
      hash_map_decided[str_holes] = MapValue(voi, ret);
      return ret;
    }
    hash_map_decided[str_holes] = able_points(problem, solution, str_holes, voi, vertices_distances, counter);
  }

  auto hash_value = hash_map_decided[str_holes];
  if (hash_value.available) {
    auto cand_points = hash_value.cands;
    LOG(INFO) << cand_points.size();
    voi = hash_value.voi;
    if (cand_points.size() == 0) {
      hash_map_unrestricted[str_inside] = MapValue(voi, ret);
      return ret;
    }

    for (auto e : cand_points) {
      if (is_point_valid(problem, solution, str_inside, vertices_distances, e)) ret.push_back(e);
    }
    hash_map_unrestricted[str_inside] = MapValue(voi, ret);
    return ret;
  }

  else {
    MapValue mpv(able_points(problem, solution, str_inside, voi, vertices_distances, counter));
    hash_map_unrestricted[str_inside] = mpv;
    LOG(INFO) << mpv.cands.size();
    return mpv.cands;
  }
}
SSolutionPtr dfs_able_points(SProblemPtr problem, SSolutionPtr solution, const std::vector<Striction>& str_hole_memo, std::vector<int>& decided_points_inside, HashMap& hash_map_decided, std::vector<int> restriction_edges, const mat& vertices_distances, int V, int H, HashMap& hash_map_unrestricted, long long& counter) {
  counter++;
//  int H = problem->hole_polygon.size();
 // LOG(INFO) << "dfs_able_points "<<decided_points_inside.size();
#if 0
  SVisualEditor debug(problem);
  debug.set_pose(solution);
  auto key = debug.show(0);
  if (key == 'x') counter += LARGE;
  if (key == 'q') counter += VERYLARGE;
#endif
  if (counter > LARGE) return nullptr;
  //if (counter % integer(1e4) == 0) LOG(INFO) << "counter is (able_points) " << counter;
  //if (counter > 1e9) return nullptr;
  if (decided_points_inside.size() == V - H) {
    auto judgement = judge(*problem, *solution);
    if (judgement.is_valid()) return solution;
    return nullptr;
  }
  int most_restricted_point = -1;
  int memo = -1;
  for (int i = 0; i < V; i++) if (memo < restriction_edges[i]) {
    memo = restriction_edges[i];
    most_restricted_point = i;
  }
  SVOI voi(problem->hole_polygon);
  Striction str_inside(most_restricted_point, V);
  str_inside.Set(decided_points_inside, solution, vertices_distances);
  auto cands = able_points_hash(problem, solution, str_hole_memo[most_restricted_point], str_inside, hash_map_decided, voi, vertices_distances, hash_map_unrestricted, counter);
  if (cands.size() == 0) return nullptr;
  decided_points_inside.push_back(most_restricted_point);
  for (int i = 0; i < V; i++) if (vertices_distances[most_restricted_point][i] && restriction_edges[i] >= 0) restriction_edges[i]++;
  restriction_edges[most_restricted_point] = -10000000;
  for (auto point : cands) {
    solution->vertices[most_restricted_point] = point;
    auto sol = dfs_able_points(problem, solution, str_hole_memo, decided_points_inside, hash_map_decided, restriction_edges, vertices_distances, V,H, hash_map_unrestricted, counter);
    if (sol) return sol;
    solution->vertices[most_restricted_point] = problem->vertices[most_restricted_point];
  }
  decided_points_inside.pop_back();
  return nullptr;

}

SSolutionPtr Fit_Unstricted_Points(SProblemPtr problem, SSolutionPtr solution, const std::vector<int>& decided_points, const mat& vertices_distances, HashMap& hash_map_decided, long long& counter) {
  int V = problem->vertices.size();
  int H = decided_points.size();
  std::vector<bool> used_vertices(V, false);
  for (auto e : decided_points) used_vertices[e] = true;
  std::vector<int> restriction_edges(V, 0);
  for (auto e : decided_points) for (int i = 0; i < V; i++) if (vertices_distances[i][e]) restriction_edges[i]++;
  for (auto e : decided_points) restriction_edges[e] = -100000;
  std::vector<int> decided_points_inside = {};
  std::vector<Striction> str_hole_memo;
  for (int i = 0; i < V; i++) str_hole_memo.push_back(Striction(i, V));
  //LOG(INFO) << str_hole_memo.size() << "SIZE==============";
  for (int i = 0; i < V; i++) if (!used_vertices[i]) {
    str_hole_memo[i].Set(decided_points, solution, vertices_distances);
  }
  HashMap hash_map_unstricted = {};
  auto ptr_ans = dfs_able_points(problem, solution, str_hole_memo, decided_points_inside, hash_map_decided, restriction_edges, vertices_distances, V, H,  hash_map_unstricted, counter);
  LOG(INFO) << "hash_map_unstricted size is " << hash_map_unstricted.size();
  return ptr_ans;

}

SSolutionPtr FitUnstrictedPoints(SProblemPtr problem, SSolutionPtr solution, const std::vector<int> decided_points) {
  long long counter = 0;
  HashMap hash_map_decided = {};
  return Fit_Unstricted_Points(problem, solution, decided_points, create_vertices_distances(problem), hash_map_decided, counter);
}

SSolutionPtr dfs_holes(SProblemPtr problem, SSolutionPtr solution, HashMap& hash_map_decided, const mat& hole_distances, const mat& vertices_distances, const std::vector<std::vector<double>>& upperlimit_distances, std::vector<int>& v, int V, int H, std::vector<int>& used_vertices, const vec_holenum& v_h, long long& counter) {
  constexpr int one_min = 1000 * 60;
  counter++;
  if (/*counter % 50 == 0*/ false) {
    SVisualEditor debug(problem, "full_research", "dfs_holes");
    debug.set_pose(solution);
    auto key = debug.show(3);
    if (key == 'x') counter += LARGE;
    if (key == 'q') counter += VERYLARGE;
  }
  if (counter > LARGE) return nullptr;

  //if(counter % integer(1e4) == 0) LOG(INFO) << "counter is (holes)" << counter;
  //if (counter > 1e9) return nullptr;
  //LOG(INFO) << "v.size is : " << v.size();
  //if (timer.elapsed_ms() > one_min) return nullptr;
  if (v.size() >= H) {
    return Fit_Unstricted_Points(problem, solution, v, vertices_distances, hash_map_decided, counter);
  }
  else {
    for (int x = 0; x < V; x++) if (!used_vertices[x]) {
      if (counter > 1e15) return nullptr;
      v.push_back(x);
      bool okay = true;
      used_vertices[x]++;
      int hole_x = v_h.get_holenum(v.size() - 1);
      for (int i = 0; i < v.size() - 1; i++) {
        int hole_i = v_h.get_holenum(i);
        int y = v[i];
        auto ver_dist = vertices_distances[x][y];
        auto uplimit_dist = upperlimit_distances[x][y];
        auto hole_dist = hole_distances[hole_i][hole_x];
        //LOG(INFO) << fmt::format("Problem  : {}, {}, {}", x,y);
        if (ver_dist && !tolerate(ver_dist, hole_dist, problem->epsilon)) okay = false;
        if (uplimit_dist + 1 < std::sqrt(hole_dist)) okay = false;
        if (!okay) break;
      }
      //counter++;
      if (okay) {
        //LOG(INFO) << "dfs_holes:: v is ";
        //for (auto e : v) LOG(INFO) << e;
        solution->vertices[x] = problem->hole_polygon[hole_x];
        auto sol = dfs_holes(problem, solution, hash_map_decided, hole_distances, vertices_distances, upperlimit_distances, v, V, H, used_vertices, v_h, counter);
        if (sol) return sol;
        solution->vertices[x] = problem->vertices[x];
      }
      v.pop_back();
      used_vertices[x]--;
    }
  }
  return nullptr;
}

#if 1
bool full_research(SProblemPtr problem, SSolutionPtr& solution) {
  LOG(INFO) << "CALLED FULL RESEARCH";
  int V = problem->vertices.size();
  int H = problem->hole_polygon.size();
  LOG(INFO) << "V,H is " << V << ' ' << H;
  unsigned int seed = 20210711;
  std::mt19937_64 rng(seed);

  constexpr double BIGDOUBLE = 1e18;
  vec_holenum v_h(H);

  mat vertices_distances = create_vertices_distances(problem);
  std::vector<std::vector<double>> upperlimit_distances(V, std::vector<double>(V, BIGDOUBLE));

  for (const auto& e : problem->edges) {
    upperlimit_distances[e.first][e.second] = std::sqrt(upper_limit(distance2(problem->vertices[e.first], problem->vertices[e.second]), problem->epsilon));
    upperlimit_distances[e.second][e.first] = upperlimit_distances[e.first][e.second];
  }

  for (int k = 0; k < V; k++) for (int i = 0; i < V; i++) for (int j = 0; j < V; j++) {
    upperlimit_distances[i][j] = std::min(upperlimit_distances[i][j], upperlimit_distances[i][k] + upperlimit_distances[k][j]);
  }

  mat hole_distances(H, vec(H, 0));
  for (int i = 0; i < H; i++) for (int j = 0; j < H; j++) if (i != j) hole_distances[i][j] = distance2(problem->hole_polygon[i], problem->hole_polygon[j]);
  long long counter = 0;
  std::vector<std::vector<int>> ret;
  std::vector<int> used_vertices(V, 0);
  std::vector<int> v = {};
  SSolutionPtr solution_initial(new SSolution(solution->vertices));
  std::vector<int> decided_points_initial = {};
  for (int i = 0; i < V; i++) if (solution->vertices[i] != problem->vertices[i]) {
    decided_points_initial.push_back(i);
    used_vertices[i]++;
  }
  int hole_same_counter = 0;
  for (int i = 0; i < decided_points_initial.size(); i++) {
    auto e = decided_points_initial[i];
    Point point = solution_initial->vertices[e];
    for (int j = 0; j < H; j++) {
      if (point == problem->hole_polygon[j]) {
        v_h.Change_V2H(i, j);
        hole_same_counter++;
        break;
      }
    }
  }
  //  v_h.check_valid();

  std::vector<int> v_init_memo;
  for (auto e : decided_points_initial) v_init_memo.push_back(e);

  LOG(INFO) << "start decided is " << decided_points_initial.size();
  HashMap hash_map_decided = {};
  auto sol = dfs_holes(problem, solution_initial, hash_map_decided, hole_distances, vertices_distances, upperlimit_distances, decided_points_initial, V, H, used_vertices, v_h, counter);
  if (sol) {
    solution = sol;
    LOG(INFO) << "hash_map_size is" << hash_map_decided.size();
    return true;
  }

#if 0

  for (auto e : v_init_memo) {
    LOG(INFO) << "arrived: changing e is " << e;
    SSolutionPtr sol(new SSolution(solution->vertices));
    sol->vertices[e] = problem->vertices[e];
    std::vector<int> dec = {};
    for (auto x : v_init_memo) if (x != e) dec.push_back(x);
    for (int i = 0; i < dec.size(); i++) {
      auto e = dec[i];
      Point point = sol->vertices[e];
      for (int j = 0; j < H; j++) {
        if (point == problem->hole_polygon[j]) {
          v_h.Change_V2H(i, j);
          hole_same_counter++;
          break;
        }
      }
    }
    SVisualEditor debug(problem, "fullresearch", "main");
    debug.set_pose(sol);
    auto key = debug.show(100);
    std::vector<int> used(V, 0);
    for (auto x : dec) if (x != e) used[x]++;
    sol = dfs_holes(problem, sol, hole_distances, vertices_distances, upperlimit_distances, dec, V, H, timer, used, v_h, counter);
    if (sol) {
      solution = sol;
      return true;
    }

  }

#endif

  return false;

}

class FullResearch : public SolverBase {
public:
  FullResearch() { }
  SolverOutputs solve(const SolverArguments& args) override {
    SolverOutputs ret;

    //Timer timer;
    ret.solution = args.problem->create_solution();
    if (args.optional_initial_solution) ret.solution = args.optional_initial_solution;
    full_research(args.problem, ret.solution);
    LOG(INFO) << "finish fullresearch";
    // dummy.
    //std::this_thread::sleep_for(std::chrono::milliseconds(500));

    return ret;
  }
};

REGISTER_SOLVER("FullResearch", FullResearch);
#endif

class FitUnstricted : public SolverBase {
public:
  FitUnstricted() { }
  SolverOutputs solve(const SolverArguments& args) override {

    auto solution = args.problem->create_solution();
    if (args.optional_initial_solution) solution = args.optional_initial_solution;
    std::vector<int> decided_points;
    int V = args.problem->vertices.size();
        std::vector<int> bad_vertex_count(V, 0);
    for (auto e : args.problem->edges) {
      integer a = e.first, b = e.second;
      integer dis_p = distance2(args.problem->vertices[a], args.problem->vertices[b]);
      integer dis_s = distance2(solution->vertices[a], solution->vertices[b]);
      if (!tolerate(dis_p, dis_s, args.problem->epsilon)) {
        bad_vertex_count[a]++;
        bad_vertex_count[b]++;
      }
    }
    std::vector<std::pair<int, int>> baddest;
    for (int i = 0; i < V; i++) baddest.push_back(std::make_pair(bad_vertex_count[i], i));
    std::sort(baddest.begin(), baddest.end());
        for (auto e : baddest) decided_points.push_back(e.second);

    SolverOutputs ret;
    for (int i = 0; i < 90 ; i++) {
      LOG(INFO)<< "CHAllENGE " << i;
      decided_points.pop_back();
      auto sol = FitUnstrictedPoints(args.problem, solution, decided_points);
      if (sol) {
        ret.solution = sol;
        return ret;
      }
    }
    ret.solution = solution;
    return ret;

   // for (int i = 0; i < args.problem->vertices.size(); i++) if (args.problem->vertices[i] != solution->vertices[i]) decided_points.push_back(i);

  //  bool quit = false;

/*

    if(args.visualize){
      SVisualEditor manual_solver(args.problem, "FitUnstricted", "Select Points");
      manual_solver.set_pose(solution);
      manual_solver.set_marked_indices(decided_points);
      while (true) {
        auto key = manual_solver.show(15);
        if (key == 27) break;
      }
      solution = manual_solver.get_pose();
      decided_points = manual_solver.get_marked_indices();
    }
*/
  }

};

REGISTER_SOLVER("FitUnstricted",FitUnstricted);

namespace SimpleMatchFullResearch {

constexpr int kNumTrialsPerComponent = 100000;
constexpr int kMinComponentSize = 10;
constexpr int kMinDegree = 2;

namespace bg = boost::geometry;
using BoostPoint = bg::model::d2::point_xy<double>;
using BoostPolygon = bg::model::polygon<BoostPoint>;
using BoostLinestring = bg::model::linestring<BoostPoint>;

template <typename T>
BoostPoint ToBoostPoint(const T& point) {
  const auto [x, y] = point;
  return BoostPoint(x, y);
}

template <typename T>
BoostPolygon ToBoostPolygon(const std::vector<T>& points) {
  BoostPolygon polygon;
  for (std::size_t i = 0; i <= points.size(); ++i) {
    polygon.outer().push_back(ToBoostPoint(points[i % points.size()]));
  }
  if (bg::area(polygon) < 0.0) {
    bg::reverse(polygon);
  }
  return polygon;
}

template <typename T, typename U>
auto SquaredDistance(const T& vertex0, const U& vertex1) {
  const auto [x0, y0] = vertex0;
  const auto [x1, y1] = vertex1;
  return (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
}

class Solver : public SolverBase {
 public:
  SolverOutputs solve(const SolverArguments& args) override {
    counter_ = 0;
    if (args.visualize) {
      editor_ = std::make_shared<SVisualEditor>(args.problem, "SimpleMatchingSolver", "visualize");
    }

    hole_ = args.problem->hole_polygon;
    vertices_ = args.problem->vertices;
    edges_ = args.problem->edges;
    epsilon_ = args.problem->epsilon;
    const auto hole_polygon = ToBoostPolygon(hole_);

    N_ = vertices_.size();
    queued_.assign(N_, -1);
    assigned_.assign(N_, -1);
    adjacent_.assign(N_, {});
    std::vector<std::vector<double>> distances(
        N_, std::vector<double>(N_, std::numeric_limits<double>::infinity()));
    for (int i = 0; i < N_; ++i) {
      distances[i][i] = 0.0;
    }
    for (const auto& [a, b] : edges_) {
      const auto squared_distance = SquaredDistance(vertices_[a], vertices_[b]);
      const auto margin = epsilon_ * squared_distance / 1'000'000;
      const auto min = squared_distance - margin;
      const auto max = squared_distance + margin;
      adjacent_[a].emplace_back(b, min, max);
      adjacent_[b].emplace_back(a, min, max);
      distances[a][b] = distances[b][a] = std::sqrt(squared_distance) * (1.0 + 1.0e-6 * epsilon_);
    }
    for (int k = 0; k < N_; ++k) {
      for (int i = 0; i < N_; ++i) {
        for (int j = 0; j < N_; ++j) {
          distances[i][j] = std::min(distances[i][j], distances[i][k] + distances[k][j]);
        }
      }
    }
    vertex_max_distances_.resize(N_);
    for (int i = 0; i < N_; ++i) {
      vertex_max_distances_[i].resize(N_);
      for (int j = 0; j < N_; ++j) {
        vertex_max_distances_[i][j] = std::ceil(std::pow(distances[i][j], 2.0));
      }
    }

    M_ = hole_.size();
    hole_candidates_.clear();
    hole_distances_.resize(M_);
    hole_visibilities_.resize(M_);
    for (int i = 0; i < M_; ++i) {
      hole_candidates_.push_back(i);
      hole_distances_[i].resize(M_);
      hole_visibilities_[i].resize(M_);
      for (int j = 0; j < M_; ++j) {
        const BoostLinestring linestring{ToBoostPoint(hole_[i]), ToBoostPoint(hole_[j])};
        hole_visibilities_[i][j] = bg::covered_by(linestring, hole_polygon);
        hole_distances_[i][j] = SquaredDistance(hole_[i], hole_[j]);
      }
    }

    last_updated_ = 0;
    component_sizes_.assign(M_, M_);
    best_sizes_.assign(M_, 0);
    auto pose = vertices_;
    for (int i = 0; i < N_; ++i) {
      queued_[i] = N_;
      vertex_candidates_.push_back(i);
      assigned_counts_.assign(1, 0);
      if (Search()) {
        for (int j = 0; j < N_; ++j) {
          if (assigned_[j] < 0) continue;
          pose[j] = hole_[assigned_[j]];
        }
        break;
      }
      vertex_candidates_.pop_back();
      queued_[i] = -1;
    }

    auto cand_clean_points = Cleanup();

    auto tmp_pose = pose;
    std::vector<int> decided_points;
    for (int i = 0; i < N_; i++) if (assigned_[i] >= 0) decided_points.push_back(i);
    SSolutionPtr solution_simple = std::make_shared<SSolution>(tmp_pose);
    auto solved = FitUnstrictedPoints(args.problem, solution_simple, decided_points);
    if (solved) return  SolverOutputs{ solution_simple };
    return SolverOutputs{ std::make_shared<SSolution>(pose) };

    for (int j = 0; j < N_; j++) if (assigned_[j] >= 0) {
      tmp_pose[j] = args.problem->vertices[j];
      SSolutionPtr solution_simple = std::make_shared<SSolution>(tmp_pose);
      auto solved = full_research(args.problem, solution_simple);
      if (solved) return  SolverOutputs{ solution_simple };
      tmp_pose[j] = pose[j];

    }

  }

  std::vector<std::pair<int,int>> Cleanup(int max_clean = 1000) {
    std::vector<std::pair<int,int>> ret = {};
    if (!max_clean) return ret;
    int counter = 0;
    while (true) {
      bool converged = true;
      for (int i = 0; i < N_; ++i) {
        if (assigned_[i] < 0) continue;
        int degree = 0;
        for (const auto& [next, min, max] : adjacent_[i]) {
          degree += assigned_[next] >= 0;
        }
        if (degree < kMinDegree) {
          ret.push_back(std::make_pair(i,assigned_[i]));
          converged = false;
          assigned_[i] = -1;
          counter++;
          if (counter >= max_clean) return ret;
        }
      }
      if (converged) return ret;
    }
  }
  void Clean(const std::vector<std::pair<int, int>>& rev) {
    for (auto e : rev) assigned_[e.first] = -1;

  }

  void RevertClean(const std::vector<std::pair<int,int>>& rev) {
    for (auto e : rev) assigned_[e.first] = e.second;
  }

  bool Search() {
    if (hole_candidates_.empty()) return true;
    if (++counter_ % 1000 == 0 && editor_) {
      auto pose = vertices_;
      std::vector<int> marked;
      for (int j = 0; j < N_; ++j) {
        if (assigned_[j] < 0) continue;
        pose[j] = hole_[assigned_[j]];
        marked.push_back(j);
      }
      editor_->set_pose(std::make_shared<SSolution>(pose));
      editor_->set_marked_indices(marked);
      editor_->set_persistent_custom_stat(fmt::format("assigned counts = {}", fmt::join(assigned_counts_, ", ")));
      editor_->show(1);
    }
    const int component_index = assigned_counts_.size() - 1;
    best_sizes_[component_index] = std::max(best_sizes_[component_index], assigned_counts_[component_index]);
    if (counter_ > last_updated_ + kNumTrialsPerComponent) {
      last_updated_ = counter_;
      component_sizes_[component_index] = best_sizes_[component_index];
      if (component_sizes_[component_index] < kMinComponentSize) return true;
    }
    for (int i = vertex_candidates_.size() - 1; i >= 0; --i) {
      const int vertex = vertex_candidates_[i];
      vertex_candidates_.erase(vertex_candidates_.begin() + i);
      for (int j = hole_candidates_.size() - 1; j >= 0; --j) {
        const int hole_vertex = hole_candidates_[j];
        bool feasible = true;
        for (const auto& [next, min, max] : adjacent_[vertex]) {
          if (assigned_[next] < 0) continue;
          const auto squared_distance = hole_distances_[assigned_[next]][hole_vertex];
          const auto visible = hole_visibilities_[assigned_[next]][hole_vertex];
          if (!visible || squared_distance < min || squared_distance > max) {
            feasible = false;
            break;
          }
        }
        for (int other = 0; feasible && other < N_; ++other) {
          if (assigned_[other] < 0) continue;
          if (hole_distances_[assigned_[other]][hole_vertex] > vertex_max_distances_[other][vertex]) {
            feasible = false;
          }
        }
        if (feasible) {
          hole_candidates_.erase(hole_candidates_.begin() + j);
          assigned_[vertex] = hole_vertex;
          ++assigned_counts_.back();
          for (const auto& [next, min, max] : adjacent_[vertex]) {
            if (queued_[next] >= 0) continue;
            queued_[next] = vertex;
            vertex_candidates_.push_back(next);
          }
          if (Search()) return true;
          for (const auto& [next, min, max] : adjacent_[vertex]) {
            if (queued_[next] != vertex) continue;
            queued_[next] = -1;
            vertex_candidates_.pop_back();
          }
          --assigned_counts_.back();
          assigned_[vertex] = -1;
          hole_candidates_.insert(hole_candidates_.begin() + j, hole_vertex);
        }
      }
      vertex_candidates_.insert(vertex_candidates_.begin() + i, vertex);
    }
    if (assigned_counts_.back() >= component_sizes_[component_index]) {
      if (assigned_counts_.size() == component_sizes_.size()) return true;
      for (int i = 0; i < N_; ++i) {
        if (queued_[i] >= 0) continue;
        queued_[i] = N_;
        vertex_candidates_.push_back(i);
        assigned_counts_.push_back(0);
        if (Search()) return true;
        assigned_counts_.pop_back();
        vertex_candidates_.pop_back();
        queued_[i] = -1;
      }
    }
    return false;
  }

 private:
  std::vector<Point> hole_;
  std::vector<Point> vertices_;
  std::vector<Edge> edges_;
  integer epsilon_;

  int N_;
  int M_;
  std::vector<int> queued_;
  std::vector<int> assigned_;
  std::vector<int> vertex_candidates_;
  std::vector<int> hole_candidates_;
  std::vector<std::vector<std::tuple<int, integer, integer>>> adjacent_;
  std::vector<std::vector<integer>> hole_distances_;
  std::vector<std::vector<bool>> hole_visibilities_;
  std::vector<std::vector<integer>> vertex_max_distances_;
  std::vector<int> assigned_counts_;
  std::vector<int> component_sizes_;
  std::vector<int> best_sizes_;
  std::size_t last_updated_;
  std::size_t counter_;
  SVisualEditorPtr editor_;
};

}
REGISTER_SOLVER("SimpleMatchFullResearch", SimpleMatchFullResearch::Solver);


/**
 * @file kmeans_test.cpp
 * @author Ryan Curtin
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/kmeans/kmeans.hpp>
#include <mlpack/methods/kmeans/allow_empty_clusters.hpp>
#include <mlpack/methods/kmeans/refined_start.hpp>
#include <mlpack/methods/kmeans/elkan_kmeans.hpp>
#include <mlpack/methods/kmeans/hamerly_kmeans.hpp>
#include <mlpack/methods/kmeans/pelleg_moore_kmeans.hpp>
#include <mlpack/methods/kmeans/dual_tree_kmeans.hpp>
#include <mlpack/methods/kmeans/sample_initialization.hpp>
#include <mlpack/methods/kmeans/random_partition.hpp>

#include <mlpack/core/tree/cover_tree/cover_tree.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::kmeans;
using namespace mlpack::metric;
using namespace mlpack::tree;
using namespace mlpack::neighbor;

BOOST_AUTO_TEST_SUITE(KMeansTest);

// Generate dataset; written transposed because it's easier to read.
arma::mat kMeansData("  0.0   0.0;" // Class 1.
                     "  0.3   0.4;"
                     "  0.1   0.0;"
                     "  0.1   0.3;"
                     " -0.2  -0.2;"
                     " -0.1   0.3;"
                     " -0.4   0.1;"
                     "  0.2  -0.1;"
                     "  0.3   0.0;"
                     " -0.3  -0.3;"
                     "  0.1  -0.1;"
                     "  0.2  -0.3;"
                     " -0.3   0.2;"
                     " 10.0  10.0;" // Class 2.
                     " 10.1   9.9;"
                     "  9.9  10.0;"
                     " 10.2   9.7;"
                     " 10.2   9.8;"
                     "  9.7  10.3;"
                     "  9.9  10.1;"
                     "-10.0   5.0;" // Class 3.
                     " -9.8   5.1;"
                     " -9.9   4.9;"
                     "-10.0   4.9;"
                     "-10.2   5.2;"
                     "-10.1   5.1;"
                     "-10.3   5.3;"
                     "-10.0   4.8;"
                     " -9.6   5.0;"
                     " -9.8   5.1;");

/**
 * 30-point 3-class test case for K-Means.
 */
BOOST_AUTO_TEST_CASE(KMeansSimpleTest)
{
  // This test was originally written to use RandomPartition, and is left that
  // way because RandomPartition gives better initializations here.
  KMeans<EuclideanDistance, RandomPartition> kmeans;

  arma::Row<size_t> assignments;
  kmeans.Cluster((arma::mat) trans(kMeansData), 3, assignments);

  // Now make sure we got it all right.  There is no restriction on how the
  // clusters are ordered, so we have to be careful about that.
  size_t firstClass = assignments(0);

  for (size_t i = 1; i < 13; i++)
    BOOST_REQUIRE_EQUAL(assignments(i), firstClass);

  size_t secondClass = assignments(13);

  // To ensure that class 1 != class 2.
  BOOST_REQUIRE_NE(firstClass, secondClass);

  for (size_t i = 13; i < 20; i++)
    BOOST_REQUIRE_EQUAL(assignments(i), secondClass);

  size_t thirdClass = assignments(20);

  // To ensure that this is the third class which we haven't seen yet.
  BOOST_REQUIRE_NE(firstClass, thirdClass);
  BOOST_REQUIRE_NE(secondClass, thirdClass);

  for (size_t i = 20; i < 30; i++)
    BOOST_REQUIRE_EQUAL(assignments(i), thirdClass);
}

/**
 * Make sure the empty cluster policy class does nothing.
 */
BOOST_AUTO_TEST_CASE(AllowEmptyClusterTest)
{
  arma::Row<size_t> assignments;
  assignments.randu(30);
  arma::Row<size_t> assignmentsOld = assignments;

  arma::mat centroids;
  centroids.randu(30, 3); // This doesn't matter.

  arma::Col<size_t> counts(3);
  counts[0] = accu(assignments == 0);
  counts[1] = accu(assignments == 1);
  counts[2] = 0;
  arma::Col<size_t> countsOld = counts;

  // Make sure the method doesn't modify any points.
  metric::LMetric<2, true> metric;
  BOOST_REQUIRE_EQUAL(AllowEmptyClusters::EmptyCluster(kMeansData, 2, centroids,
      centroids, counts, metric, 0), 0);

  // Make sure no assignments were changed.
  for (size_t i = 0; i < assignments.n_elem; i++)
    BOOST_REQUIRE_EQUAL(assignments[i], assignmentsOld[i]);

  // Make sure no counts were changed.
  for (size_t i = 0; i < 3; i++)
    BOOST_REQUIRE_EQUAL(counts[i], countsOld[i]);
}

/**
 * Make sure the max variance method finds the correct point.
 */
BOOST_AUTO_TEST_CASE(MaxVarianceNewClusterTest)
{
  // Five points.
  arma::mat data("0.4 1.0 5.0 -2.0 -2.5;"
                 "1.0 0.8 0.7  5.1  5.2;");

  // Point 2 is the mis-clustered point we're looking for to be moved.
  arma::Row<size_t> assignments("0 0 0 1 1");

  arma::mat centroids(2, 3);
  centroids.col(0) = (1.0 / 3.0) * (data.col(0) + data.col(1) + data.col(2));
  centroids.col(1) = 0.5 * (data.col(3) + data.col(4));
  centroids(0, 2) = DBL_MAX;
  centroids(1, 2) = DBL_MAX;

  arma::Col<size_t> counts("3 2 0");

  metric::LMetric<2, true> metric;

  // This should only change one point.
  MaxVarianceNewCluster mvnc;
  BOOST_REQUIRE_EQUAL(mvnc.EmptyCluster(data, 2, centroids, centroids, counts,
      metric, 0), 1);

  // Add the variance of each point's distance away from the cluster.  I think
  // this is the sensible thing to do.
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    // Find the closest centroid to this point.
    double minDistance = std::numeric_limits<double>::infinity();
    size_t closestCluster = centroids.n_cols; // Invalid value.

    for (size_t j = 0; j < centroids.n_cols; j++)
    {
      const double distance = metric.Evaluate(data.col(i), centroids.col(j));

      if (distance < minDistance)
      {
        minDistance = distance;
        closestCluster = j;
      }
    }

    assignments[i] = closestCluster;
  }

  BOOST_REQUIRE_EQUAL(assignments[0], 0);
  BOOST_REQUIRE_EQUAL(assignments[1], 0);
  BOOST_REQUIRE_EQUAL(assignments[2], 2);
  BOOST_REQUIRE_EQUAL(assignments[3], 1);
  BOOST_REQUIRE_EQUAL(assignments[4], 1);

  // Ensure that the counts are right.
  BOOST_REQUIRE_EQUAL(counts[0], 2);
  BOOST_REQUIRE_EQUAL(counts[1], 2);
  BOOST_REQUIRE_EQUAL(counts[2], 1);
}

/**
 * Make sure the random partitioner seems to return valid results.
 */
BOOST_AUTO_TEST_CASE(RandomPartitionTest)
{
  arma::mat data;
  data.randu(2, 1000); // One thousand points.

  arma::Row<size_t> assignments;

  // We'll ask for 18 clusters (arbitrary).
  RandomPartition::Cluster(data, 18, assignments);

  // Ensure that the right number of assignments were given.
  BOOST_REQUIRE_EQUAL(assignments.n_elem, 1000);

  // Ensure that no value is greater than 17 (the maximum valid cluster).
  for (size_t i = 0; i < 1000; i++)
    BOOST_REQUIRE_LT(assignments[i], 18);
}

/**
 * Make sure that random initialization fails for a corner case dataset.
 */
BOOST_AUTO_TEST_CASE(RandomInitialAssignmentFailureTest)
{
  // This is a very synthetic dataset.  It is one Gaussian with a huge number of
  // points combined with one faraway Gaussian with very few points.  Normally,
  // k-means should not get the correct result -- which is one cluster at each
  // Gaussian.  This is because the random partitioning scheme has very low
  // (virtually zero) likelihood of separating the two Gaussians properly, and
  // then the algorithm will never converge to that result.
  //
  // So we will set the initial assignments appropriately.  Remember, once the
  // initial assignments are done, k-means is deterministic.
  arma::mat dataset(2, 10002);
  dataset.randn();
  // Now move the second Gaussian far away.
  for (size_t i = 0; i < 2; ++i)
    dataset.col(10000 + i) += arma::vec("50 50");

  // Ensure that k-means fails when run with random initialization.  This isn't
  // strictly a necessary test, but it does help let us know that this is a good
  // test.
  size_t successes = 0;
  for (size_t run = 0; run < 15; ++run)
  {
    arma::mat centroids;
    arma::Row<size_t> assignments;
    KMeans<> kmeans;
    kmeans.Cluster(dataset, 2, assignments, centroids);

    // Inspect centroids.  See if one is close to the second Gaussian.
    if ((centroids(0, 0) >= 30.0 && centroids(1, 0) >= 30.0) ||
        (centroids(0, 1) >= 30.0 && centroids(1, 1) >= 30.0))
      ++successes;
  }

  // Only one success allowed.  The probability of two successes should be
  // infinitesimal.
  BOOST_REQUIRE_LT(successes, 2);
}

/**
 * Make sure that specifying initial assignments is successful for a corner case
 * dataset which doesn't usually converge otherwise.
 */
BOOST_AUTO_TEST_CASE(InitialAssignmentTest)
{
  // For a better description of this dataset, see
  // RandomInitialAssignmentFailureTest.
  arma::mat dataset(2, 10002);
  dataset.randn();
  // Now move the second Gaussian far away.
  for (size_t i = 0; i < 2; ++i)
    dataset.col(10000 + i) += arma::vec("50 50");

  // Now, if we specify initial assignments, the algorithm should converge (with
  // zero iterations, actually, because this is the solution).
  arma::Row<size_t> assignments(10002);
  assignments.fill(0);
  assignments[10000] = 1;
  assignments[10001] = 1;

  KMeans<> kmeans;
  kmeans.Cluster(dataset, 2, assignments, true);

  // Check results.
  for (size_t i = 0; i < 10000; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 0);
  for (size_t i = 10000; i < 10002; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 1);

  // Now, slightly harder.  Give it one incorrect assignment in each cluster.
  // The wrong assignment should be quickly fixed.
  assignments[9999] = 1;
  assignments[10000] = 0;

  kmeans.Cluster(dataset, 2, assignments, true);

  // Check results.
  for (size_t i = 0; i < 10000; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 0);
  for (size_t i = 10000; i < 10002; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 1);
}

/**
 * Make sure specifying initial centroids is successful for a corner case which
 * doesn't usually converge otherwise.
 */
BOOST_AUTO_TEST_CASE(InitialCentroidTest)
{
  // For a better description of this dataset, see
  // RandomInitialAssignmentFailureTest.
  arma::mat dataset(2, 10002);
  dataset.randn();
  // Now move the second Gaussian far away.
  for (size_t i = 0; i < 2; ++i)
    dataset.col(10000 + i) += arma::vec("50 50");

  arma::Row<size_t> assignments;
  arma::mat centroids(2, 2);

  centroids.col(0) = arma::vec("0 0");
  centroids.col(1) = arma::vec("50 50");

  // This should converge correctly.
  KMeans<> k;
  k.Cluster(dataset, 2, assignments, centroids, false, true);

  // Check results.
  for (size_t i = 0; i < 10000; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 0);
  for (size_t i = 10000; i < 10002; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 1);

  // Now add a little noise to the initial centroids.
  centroids.col(0) = arma::vec("3 4");
  centroids.col(1) = arma::vec("25 10");

  k.Cluster(dataset, 2, assignments, centroids, false, true);

  // Check results.
  for (size_t i = 0; i < 10000; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 0);
  for (size_t i = 10000; i < 10002; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 1);
}

/**
 * Ensure that initial assignments override initial centroids.
 */
BOOST_AUTO_TEST_CASE(InitialAssignmentOverrideTest)
{
  // For a better description of this dataset, see
  // RandomInitialAssignmentFailureTest.
  arma::mat dataset(2, 10002);
  dataset.randn();
  // Now move the second Gaussian far away.
  for (size_t i = 0; i < 2; ++i)
    dataset.col(10000 + i) += arma::vec("50 50");

  arma::Row<size_t> assignments(10002);
  assignments.fill(0);
  assignments[10000] = 1;
  assignments[10001] = 1;

  // Note that this initial centroid guess is the opposite of the assignments
  // guess!
  arma::mat centroids(2, 2);
  centroids.col(0) = arma::vec("50 50");
  centroids.col(1) = arma::vec("0 0");

  KMeans<> k;
  k.Cluster(dataset, 2, assignments, centroids, true, true);

  // Because the initial assignments guess should take priority, we should get
  // those same results back.
  for (size_t i = 0; i < 10000; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 0);
  for (size_t i = 10000; i < 10002; ++i)
    BOOST_REQUIRE_EQUAL(assignments[i], 1);

  // Make sure the centroids are about right too.
  BOOST_REQUIRE_LT(centroids(0, 0), 10.0);
  BOOST_REQUIRE_LT(centroids(1, 0), 10.0);
  BOOST_REQUIRE_GT(centroids(0, 1), 40.0);
  BOOST_REQUIRE_GT(centroids(1, 1), 40.0);
}

/**
 * Test that the refined starting policy returns decent initial cluster
 * estimates.
 */
BOOST_AUTO_TEST_CASE(RefinedStartTest)
{
  // Our dataset will be five Gaussians of largely varying numbers of points and
  // we expect that the refined starting policy should return good guesses at
  // what these Gaussians are.
  arma::mat data(3, 3000);
  data.randn();

  // First Gaussian: 10000 points, centered at (0, 0, 0).
  // Second Gaussian: 2000 points, centered at (5, 0, -2).
  // Third Gaussian: 5000 points, centered at (-2, -2, -2).
  // Fourth Gaussian: 1000 points, centered at (-6, 8, 8).
  // Fifth Gaussian: 12000 points, centered at (1, 6, 1).
  arma::mat centroids(" 0  5 -2 -6  1;"
                      " 0  0 -2  8  6;"
                      " 0 -2 -2  8  1");

  for (size_t i = 1000; i < 1200; ++i)
    data.col(i) += centroids.col(1);
  for (size_t i = 1200; i < 1700; ++i)
    data.col(i) += centroids.col(2);
  for (size_t i = 1700; i < 1800; ++i)
    data.col(i) += centroids.col(3);
  for (size_t i = 1800; i < 3000; ++i)
    data.col(i) += centroids.col(4);

  // Now run the RefinedStart algorithm and make sure it doesn't deviate too
  // much from the actual solution.
  RefinedStart rs;
  arma::Row<size_t> assignments;
  arma::mat resultingCentroids;
  rs.Cluster(data, 5, assignments);

  // Calculate resulting centroids.
  resultingCentroids.zeros(3, 5);
  arma::Col<size_t> counts(5);
  counts.zeros();
  for (size_t i = 0; i < 3000; ++i)
  {
    resultingCentroids.col(assignments[i]) += data.col(i);
    ++counts[assignments[i]];
  }

  // Normalize centroids.
  for (size_t i = 0; i < 5; ++i)
    if (counts[i] != 0)
      resultingCentroids /= counts[i];

  // Calculate sum of distances from centroid means.
  double distortion = 0;
  for (size_t i = 0; i < 3000; ++i)
    distortion += metric::EuclideanDistance::Evaluate(data.col(i),
        resultingCentroids.col(assignments[i]));

  // Using the refined start, the distance for this dataset is usually around
  // 13500.  Regular k-means is between 10000 and 30000 (I think the 10000
  // figure is a corner case which actually does not give good clusters), and
  // random initial starts give distortion around 22000.  So we'll require that
  // our distortion is less than 14000.
  BOOST_REQUIRE_LT(distortion, 14000.0);
}

#ifdef ARMA_HAS_SPMAT
/**
 * Make sure sparse k-means works okay.
 */
BOOST_AUTO_TEST_CASE(SparseKMeansTest)
{
  // Huge dimensionality, few points.
  arma::SpMat<double> data(5000, 12);
  data(14, 0) = 6.4;
  data(14, 1) = 6.3;
  data(14, 2) = 6.5;
  data(14, 3) = 6.2;
  data(14, 4) = 6.1;
  data(14, 5) = 6.6;
  data(1402, 6) = -3.2;
  data(1402, 7) = -3.3;
  data(1402, 8) = -3.1;
  data(1402, 9) = -3.4;
  data(1402, 10) = -3.5;
  data(1402, 11) = -3.0;

  arma::Row<size_t> assignments;

  KMeans<metric::EuclideanDistance, RandomPartition, MaxVarianceNewCluster,
         NaiveKMeans, arma::sp_mat> kmeans; // Default options.

  kmeans.Cluster(data, 2, assignments);

  size_t clusterOne = assignments[0];
  size_t clusterTwo = assignments[6];

  BOOST_REQUIRE_EQUAL(assignments[0], clusterOne);
  BOOST_REQUIRE_EQUAL(assignments[1], clusterOne);
  BOOST_REQUIRE_EQUAL(assignments[2], clusterOne);
  BOOST_REQUIRE_EQUAL(assignments[3], clusterOne);
  BOOST_REQUIRE_EQUAL(assignments[4], clusterOne);
  BOOST_REQUIRE_EQUAL(assignments[5], clusterOne);
  BOOST_REQUIRE_EQUAL(assignments[6], clusterTwo);
  BOOST_REQUIRE_EQUAL(assignments[7], clusterTwo);
  BOOST_REQUIRE_EQUAL(assignments[8], clusterTwo);
  BOOST_REQUIRE_EQUAL(assignments[9], clusterTwo);
  BOOST_REQUIRE_EQUAL(assignments[10], clusterTwo);
  BOOST_REQUIRE_EQUAL(assignments[11], clusterTwo);
}

#endif // ARMA_HAS_SPMAT

BOOST_AUTO_TEST_CASE(ElkanTest)
{
  const size_t trials = 5;

  for (size_t t = 0; t < trials; ++t)
  {
    arma::mat dataset(10, 1000);
    dataset.randu();

    const size_t k = 5 * (t + 1);
    arma::mat centroids(10, k);
    centroids.randu();

    // Make sure Elkan's algorithm and the naive method return the same
    // clusters.
    arma::mat naiveCentroids(centroids);
    KMeans<> km;
    arma::Row<size_t> assignments;
    km.Cluster(dataset, k, assignments, naiveCentroids, false, true);

    KMeans<metric::EuclideanDistance, RandomPartition, MaxVarianceNewCluster,
         ElkanKMeans> elkan;
    arma::Row<size_t> elkanAssignments;
    arma::mat elkanCentroids(centroids);
    elkan.Cluster(dataset, k, elkanAssignments, elkanCentroids, false, true);

    for (size_t i = 0; i < dataset.n_cols; ++i)
      BOOST_REQUIRE_EQUAL(assignments[i], elkanAssignments[i]);

    for (size_t i = 0; i < centroids.n_elem; ++i)
      BOOST_REQUIRE_CLOSE(naiveCentroids[i], elkanCentroids[i], 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(HamerlyTest)
{
  const size_t trials = 5;

  for (size_t t = 0; t < trials; ++t)
  {
    arma::mat dataset(10, 1000);
    dataset.randu();

    const size_t k = 5 * (t + 1);
    arma::mat centroids(10, k);
    centroids.randu();

    // Make sure Hamerly's algorithm and the naive method return the same
    // clusters.
    arma::mat naiveCentroids(centroids);
    KMeans<> km;
    arma::Row<size_t> assignments;
    km.Cluster(dataset, k, assignments, naiveCentroids, false, true);

    KMeans<metric::EuclideanDistance, RandomPartition, MaxVarianceNewCluster,
        HamerlyKMeans> hamerly;
    arma::Row<size_t> hamerlyAssignments;
    arma::mat hamerlyCentroids(centroids);
    hamerly.Cluster(dataset, k, hamerlyAssignments, hamerlyCentroids, false,
        true);

    for (size_t i = 0; i < dataset.n_cols; ++i)
      BOOST_REQUIRE_EQUAL(assignments[i], hamerlyAssignments[i]);

    for (size_t i = 0; i < centroids.n_elem; ++i)
      BOOST_REQUIRE_CLOSE(naiveCentroids[i], hamerlyCentroids[i], 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(PellegMooreTest)
{
  const size_t trials = 5;

  for (size_t t = 0; t < trials; ++t)
  {
    arma::mat dataset(10, 1000);
    dataset.randu();

    const size_t k = 5 * (t + 1);
    arma::mat centroids(10, k);
    centroids.randu();

    // Make sure the Pelleg-Moore algorithm and the naive method return the same
    // clusters.
    arma::mat naiveCentroids(centroids);
    KMeans<> km;
    arma::Row<size_t> assignments;
    km.Cluster(dataset, k, assignments, naiveCentroids, false, true);

    KMeans<metric::EuclideanDistance, RandomPartition, MaxVarianceNewCluster,
        PellegMooreKMeans> pellegMoore;
    arma::Row<size_t> pmAssignments;
    arma::mat pmCentroids(centroids);
    pellegMoore.Cluster(dataset, k, pmAssignments, pmCentroids, false, true);

    for (size_t i = 0; i < dataset.n_cols; ++i)
      BOOST_REQUIRE_EQUAL(assignments[i], pmAssignments[i]);

    for (size_t i = 0; i < centroids.n_elem; ++i)
      BOOST_REQUIRE_CLOSE(naiveCentroids[i], pmCentroids[i], 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(DTNNTest)
{
  const size_t trials = 5;

  for (size_t t = 0; t < trials; ++t)
  {
    arma::mat dataset(10, 1000);
    dataset.randu();

    const size_t k = 5 * (t + 1);
    arma::mat centroids(10, k);
    centroids.randu();

    arma::mat naiveCentroids(centroids);
    KMeans<> km;
    arma::Row<size_t> assignments;
    km.Cluster(dataset, k, assignments, naiveCentroids, false, true);

    KMeans<metric::EuclideanDistance, RandomPartition, MaxVarianceNewCluster,
        DefaultDualTreeKMeans> dtnn;
    arma::Row<size_t> dtnnAssignments;
    arma::mat dtnnCentroids(centroids);
    dtnn.Cluster(dataset, k, dtnnAssignments, dtnnCentroids, false, true);

    for (size_t i = 0; i < dataset.n_cols; ++i)
      BOOST_REQUIRE_EQUAL(assignments[i], dtnnAssignments[i]);

    for (size_t i = 0; i < centroids.n_elem; ++i)
      BOOST_REQUIRE_CLOSE(naiveCentroids[i], dtnnCentroids[i], 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(DTNNCoverTreeTest)
{
  const size_t trials = 5;

  for (size_t t = 0; t < trials; ++t)
  {
    arma::mat dataset(10, 1000);
    dataset.randu();

    const size_t k = 5;
    arma::mat centroids(10, k);
    centroids.randu();

    arma::mat naiveCentroids(centroids);
    KMeans<> km;
    arma::Row<size_t> assignments;
    km.Cluster(dataset, k, assignments, naiveCentroids, false, true);

    KMeans<metric::EuclideanDistance, RandomPartition, MaxVarianceNewCluster,
        CoverTreeDualTreeKMeans> dtnn;
    arma::Row<size_t> dtnnAssignments;
    arma::mat dtnnCentroids(centroids);
    dtnn.Cluster(dataset, k, dtnnAssignments, dtnnCentroids, false, true);

    for (size_t i = 0; i < dataset.n_cols; ++i)
      BOOST_REQUIRE_EQUAL(assignments[i], dtnnAssignments[i]);

    for (size_t i = 0; i < centroids.n_elem; ++i)
      BOOST_REQUIRE_CLOSE(naiveCentroids[i], dtnnCentroids[i], 1e-5);
  }
}

/**
 * Make sure that the sample initialization strategy successfully samples points
 * from the dataset.
 */
BOOST_AUTO_TEST_CASE(SampleInitializationTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 100);
  const size_t clusters = 10;
  arma::mat centroids;

  SampleInitialization::Cluster(dataset, clusters, centroids);

  // Check that the size of the matrix is correct.
  BOOST_REQUIRE_EQUAL(centroids.n_cols, 10);
  BOOST_REQUIRE_EQUAL(centroids.n_rows, 5);

  // Check that each entry in the matrix is some sample from the dataset.
  for (size_t i = 0; i < clusters; ++i)
  {
    // If the loop successfully terminates, j will be equal to dataset.n_cols.
    // If not then we have found a match.
    size_t j;
    for (j = 0; j < dataset.n_cols; ++j)
    {
      const double distance = metric::EuclideanDistance::Evaluate(
          centroids.col(i), dataset.col(j));
      if (distance < 1e-10)
        break;
    }

    BOOST_REQUIRE_LT(j, dataset.n_cols);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef NOBODY_ORTHONORMAL_FRAME_H_
#define NOBODY_ORTHONORMAL_FRAME_H_

#include <Eigen/Dense>
#include <cmath>

namespace nobody {

class Orthonormal_frame {
  using Vector3f = Eigen::Vector3f;

 public:
  Orthonormal_frame();
  Orthonormal_frame(const Vector3f& origin, const Vector3f& up,
                    const Vector3f& back);

  const Vector3f& origin() const { return origin_; }
  const Vector3f& up() const { return up_; }
  const Vector3f& right() const { return right_; }
  const Vector3f& back() const { return back_; }

  void set_origin(const Vector3f& origin) { origin_ = origin; }

 private:
  void compute_basis_from_up_and_back();
  void compute_basis_from_back_and_up();

 private:
  Vector3f origin_;
  Vector3f back_;
  Vector3f up_;
  Vector3f right_;
};

inline Orthonormal_frame blender_orthonormal_frame(
    const Eigen::Vector3f& origin) {
  return Orthonormal_frame(origin, Eigen::Vector3f(0.0, -1.0f, 0.0f),
                           Eigen::Vector3f(0.0f, 0.0f, 1.0f));
}

inline Orthonormal_frame opengl_orthonormal_frame(
    const Eigen::Vector3f& origin) {
  return Orthonormal_frame(origin, Eigen::Vector3f(0.0f, 0.0f, 1.0f),
                           Eigen::Vector3f(0.0f, 1.0f, 0.0f));
}

Eigen::Vector3f horizontal_coordinates(const Orthonormal_frame& base_system,
                                       float radius, float altitude,
                                       float azimuth);

}  // namespace nobody

#endif  // NOBODY_ORTHONORMAL_FRAME_H_


// CHECKLIST:
// 1º) Use your editor search/replace to replace GREENDP with the method name
//    (all in caps).
// 2º) Use your editor search/replace to replace greendp with the method name.
// 3º) Implement your method (replace METHOD CODE with your code).
// 4º) Search every TODO, follow it, and delete it.
// 5º) Make a copy and change the relevant bits on a run_*.cpp and maybe the
//    test_*.cpp (basically execute steps 1 and 2 on them, including the
//    filename).
// 6º) Change the Makefile to compile your run_*.cpp and test_*.cpp too.
// 7º) Delete this comment.
// Notes: if you want to pass commandline flags you can remove the wrapper_t
//  and make your own function that receives argv/argc.
#ifndef HBM_GREENDP_HPP
#define HBM_GREENDP_HPP

#include "ukp_common.hpp"
#include "wrapper.hpp"
#include "type_name.hpp"

#include <chrono>
#include <boost/rational.hpp>

#ifndef HBM_PROFILE_PRECISION
  #define HBM_PROFILE_PRECISION 5
#endif

#ifndef HBM_PRINT_VAR
  #define HBM_PRINT_VAR(var) out << #var ": " << var << std::endl
#endif

namespace hbm {
  template <typename W, typename P, typename I>
  struct greendp_extra_info_t : extra_info_t {
    // TODO: check if this struct is adequated or not. If you want to have
    // those profiling times you will need to put HBM_START_TIMER and
    // HBM_STOP_TIMER on your code (at METHOD CODE). Check the other codes
    // to see how it works. Also, the variables n and c need to be set.
    std::string algorithm_name{"greendp"};
    #ifdef HBM_PROFILE
    double sort_time{0};        ///< Time used sorting items.
    double vector_alloc_time{0};///< Time used allocating vectors for DP.
    double linear_comp_time{0}; ///< Time used by linear time preprocessing.
    double dp_time{0};     ///< Time used creating partial solutions.
    double sol_time{0};    ///< Time used to assemble solution.
    double total_time{0};  ///< Time used by all the algorithm.
    #endif //HBM_PROFILE
    I n{0};      ///< Instance number of items.
    W c{0};      ///< Instance capacity.

    virtual std::string gen_info(void) {
      std::stringstream out("");

      HBM_PRINT_VAR(algorithm_name);
      out <<  "git_head_at_compilation: "
          << HBM_GIT_HEAD_AT_COMPILATION << std::endl;
      out << "type_W: " << hbm::type_name<W>::get() << std::endl;
      out << "type_P: " << hbm::type_name<P>::get() << std::endl;
      out << "type_I: " << hbm::type_name<I>::get() << std::endl;
      HBM_PRINT_VAR(c);
      HBM_PRINT_VAR(n);

      #ifdef HBM_PROFILE
      out << "HBM_PROFILE: defined" << std::endl;
      const double sum_time = sort_time + vector_alloc_time +
        linear_comp_time + dp_time + sol_time;

      std::streamsize old_precision = out.precision(HBM_PROFILE_PRECISION);
      const int two_first_digits_and_period = 3;
      const int percent_size = two_first_digits_and_period+HBM_PROFILE_PRECISION;
      std::ios_base::fmtflags old_flags = out.setf(std::ios::fixed, std:: ios::floatfield);
      char old_fill = out.fill(' ');

      #ifndef HBM_PRINT_TIME
        #define HBM_PRINT_TIME(name, var)\
          out << name << " time: " << var << "s (";\
          out << std::setw(percent_size) << (var/total_time)*100.0;\
          out << "%)" << std::endl
      #endif

      HBM_PRINT_TIME("Sort", sort_time);
      HBM_PRINT_TIME("Vect", vector_alloc_time);
      HBM_PRINT_TIME("O(n)", linear_comp_time);
      HBM_PRINT_TIME("DP  ", dp_time);
      HBM_PRINT_TIME("sol ", sol_time);
      HBM_PRINT_TIME("Sum ", sum_time);
      HBM_PRINT_TIME("All ", total_time);

      out.fill(old_fill);
      out.setf(old_flags);
      out.precision(old_precision);
      #else //HBM_PROFILE
      out << "HBM_PROFILE: undefined" << std::endl;
      #endif //HBM_PROFILE

      return out.str();
    }
  };

  namespace hbm_greendp_impl {
    using namespace std;
    using namespace std::chrono;
    using namespace boost::math;

    // TODO: quick comment on the method
    template<typename W, typename P, typename I>
    void greendp(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted) {
      // Extra Info Pointer == eip
      greendp_extra_info_t<W, P, I>* eip = new greendp_extra_info_t<W, P, I>();
      extra_info_t* upcast_ptr = dynamic_cast<extra_info_t*>(eip);
      sol.extra_info = std::shared_ptr<extra_info_t>(upcast_ptr);

      #ifdef HBM_PROFILE
      // Used to compute the time all the execution algorithm.
      steady_clock::time_point all_greendp_begin = steady_clock::now();

      // Used by HBM_START_TIMER and HBM_STOP_TIMER if HBM_PROFILE is defined.
      steady_clock::time_point begin;
      #endif

      // METHOD CODE

      #ifdef HBM_PROFILE
      eip->total_time = difftime_between_now_and(all_greendp_begin);
      #endif
    }

    // -------------------- WRAPPERS --------------------
    template<typename W, typename P, typename I>
    struct greendp_wrap : wrapper_t<W, P, I> {
      virtual void operator()(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted) const {
        // Calls the overloaded version with the third argument as a bool
        hbm_greendp_impl::greendp(ukpi, sol, already_sorted);

        return;
      }

      virtual const std::string& name(void) const {
        static const std::string name = "greendp";
        return name;
      }
    };

    template<typename W, typename P, typename I = size_t>
    void greendp(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol,
                int argc, argv_t argv) {
      simple_wrapper(greendp_wrap<W, P, I>(), ukpi, sol, argc, argv);
    }
  }
  // -------------------- EXTERNAL FUNCTIONS --------------------

  /// Solves an UKP instance by the
  /// TODO: ALGORITH_NAME presented at PAPER_NAME
  /// , and stores the results at sol.
  ///
  /// @note TODO: it only work with integers? there's some other caveat?
  /// @param ukpi The UKP instance to be solved.
  /// @param sol The object where the results will be written.
  /// @param already_sorted If the ukpi.items vector needs to be sorted by
  ///   TODO: what kind of ordering?
  template<typename W, typename P, typename I>
  void greendp(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted) {
    hbm_greendp_impl::greendp(ukpi, sol, already_sorted);
  }

  /// An overloaded function, it's used as argument to test_common functions.
  ///
  /// The only parameter recognized is "--already-sorted". If this parameter is
  /// given the ukpi.items will NOT be sorted by TODO: WHAT ORDERING?
  /// @see main_take_path
  /// @see greendp(instance_t<W, P> &, solution_t<W, P, I> &, bool)
  template<typename W, typename P, typename I>
  void greendp(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, int argc, argv_t argv) {
    hbm_greendp_impl::greendp(ukpi, sol, argc, argv);
  }
}

#endif //HBM_GREENDP_HPP