/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE TestResistorCapacitor2

#include "main.cc"

#include <cap/energy_storage_device.h>
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/cos_pi.hpp>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <fstream>

#include <cap/resistor_capacitor.h>
#include <memory>

// Check that linear voltage works by imposing a sine on a general series +
// parallel RC device.
// TODO add more linear tests once the function is properly implemented

void test(std::shared_ptr<boost::property_tree::ptree> input_database,
          std::shared_ptr<cap::EnergyStorageDevice> device)
{
  double const series_resistance =
      input_database->get<double>("device.series_resistance");
  double const parallel_resistance =
      input_database->get<double>("device.parallel_resistance");
  double const capacitance = input_database->get<double>("device.capacitance");
  double const frequency =
      input_database->get<double>("impedance_spectroscopy.frequency");
  double const amplitude =
      input_database->get<double>("impedance_spectroscopy.amplitude");
  int const cycles = input_database->get<int>("impedance_spectroscopy.cycles");
  int const ignore_cycles =
      input_database->get<int>("impedance_spectroscopy.ignore_cycles");
  int const steps_per_cycle =
      input_database->get<int>("impedance_spectroscopy.steps_per_cycle");
  double const tolerance =
      input_database->get<double>("impedance_spectroscopy.tolerance");
  double const initial_voltage = 0.0;
  std::string const type = input_database->get<std::string>("device.type");

  double time = 0.0;
  double const time_step = 1.0 / frequency / steps_per_cycle;
  double const pi = std::acos(-1.0);
  BOOST_CHECK_EQUAL(std::acos(-1.0), boost::math::constants::pi<double>());

  for (unsigned int i = 0; i < 20; ++i)
    device->evolve_one_time_step_linear_voltage(10., initial_voltage);
  double voltage;
  double current;
  std::fstream fout;
  fout.open("resistor_capacitor_data_" + type, std::fstream::out);

  std::cout << type << "\n";
  double const angular_frequency = 2.0 * pi * frequency;

  double const gain =
      ((type.compare("SeriesRC") == 0)
           ? angular_frequency * capacitance /
                 std::sqrt(1.0 + std::pow(angular_frequency *
                                              series_resistance * capacitance,
                                          2))
           : 1.0 / (series_resistance + parallel_resistance) /
                 (1.0 + std::pow(angular_frequency * series_resistance *
                                     parallel_resistance /
                                     (series_resistance + parallel_resistance) *
                                     capacitance,
                                 2)) *
                 std::sqrt(
                     std::pow(1.0 +
                                  std::pow(angular_frequency *
                                               parallel_resistance *
                                               capacitance,
                                           2) *
                                      series_resistance /
                                      (series_resistance + parallel_resistance),
                              2) +
                     std::pow(angular_frequency *
                                  std::pow(parallel_resistance, 2) /
                                  (series_resistance + parallel_resistance) *
                                  capacitance,
                              2)));

  double const phase =
      ((type.compare("SeriesRC") == 0)
           ? std::atan(1.0 /
                       (angular_frequency * series_resistance * capacitance))
           : std::atan(angular_frequency * std::pow(parallel_resistance, 2) /
                       (series_resistance + parallel_resistance) * capacitance /
                       (1.0 +
                        std::pow(angular_frequency * parallel_resistance *
                                     capacitance,
                                 2) *
                            series_resistance /
                            (series_resistance + parallel_resistance))));

  for (int n = 0; n < cycles * steps_per_cycle; ++n)
  {
    time += time_step;
    voltage = amplitude * std::sin(angular_frequency * time);
    device->evolve_one_time_step_linear_voltage(time_step, voltage);
    device->get_current(current);
    double const exact =
        amplitude * gain * std::sin(angular_frequency * time + phase);
    double const error = 100.0 * std::abs(current - exact) / (amplitude * gain);
    if (n >= ignore_cycles * steps_per_cycle)
      BOOST_CHECK_SMALL(error, tolerance);

    fout << boost::format("  %22.15e  %22.15e  %22.15e  %22.15e  %22.15e  \n") %
                time % current % voltage % exact % error;
  }
  fout.close();
}

BOOST_AUTO_TEST_CASE(test_resistor_capacitor)
{
  std::shared_ptr<boost::property_tree::ptree> input_database =
      std::make_shared<boost::property_tree::ptree>();
  input_database->put("device.type", "ParallelRC");
  input_database->put("device.capacitance", 3.0);
  input_database->put("device.parallel_resistance", 0.025);
  input_database->put("device.series_resistance", 5.0);
  input_database->put("impedance_spectroscopy.frequency", 1.0e-5);
  input_database->put("impedance_spectroscopy.amplitude", 1.1);
  input_database->put("impedance_spectroscopy.cycles", 2);
  input_database->put("impedance_spectroscopy.ignore_cycles", 1);
  input_database->put("impedance_spectroscopy.steps_per_cycle", 2048);
  input_database->put("impedance_spectroscopy.tolerance", 0.1);

  // build an energy storage system
  std::shared_ptr<boost::property_tree::ptree> device_database =
      std::make_shared<boost::property_tree::ptree>(
          input_database->get_child("device"));
  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(*device_database,
                                      boost::mpi::communicator());

  // Check ParallelRC
  test(input_database, device);

  // Check SeriesRC
  input_database->put("device.type", "SeriesRC");
  device_database = std::make_shared<boost::property_tree::ptree>(
      input_database->get_child("device"));
  device = cap::EnergyStorageDevice::build(*device_database,
                                           boost::mpi::communicator());
  test(input_database, device);
}


#include "tile/math/basis.h"

#include <utility>

#include <boost/math/common_factor_rt.hpp>

namespace vertexai {
namespace tile {
namespace math {

bool BasisBuilder::addEquation(const Polynomial<Rational>& orig) {
  IVLOG(4, "In basis builder, adding poly " << orig);
  // Remove any constants
  Polynomial<Rational> nc = orig - orig.constant();
  Polynomial<Rational> p = nc;

  // Reduce the polynomial via existing polynomials
  for (size_t i = 0; i < reduced_.size(); i++) {
    // Get the 'to-be-reduced' ratio
    Rational ratio = p[vars_[i]] / reduced_[i][vars_[i]];
    // Subtract it out
    p -= ratio * reduced_[i];
  }
  IVLOG(4, "Reduced verion:" << p);
  // If the result is 0, equation is linearly dependant on existing basis
  if (p == Polynomial<Rational>()) {
    return false;
  }

  // Add the original equation minus the constant + the reduced equation
  added_.push_back(nc);
  reduced_.push_back(p);
  // Add in new variables if any
  for (const auto& kvp : p.getMap()) {
    if (vars_set_.count(kvp.first) == 0) {
      vars_set_.insert(kvp.first);
      vars_.push_back(kvp.first);
    }
  }
  // Swap the variable names to make the nth variable nonzero for the nth polynomial
  size_t i = added_.size() - 1;
  for (size_t j = i; j < vars_.size(); j++) {
    if (p[vars_[j]] != 0) {
      std::swap(vars_[i], vars_[j]);
      break;
    }
  }
  // All good
  return true;
}

}  // namespace math
}  // namespace tile
}  // namespace vertexai


#define BOOST_TEST_MODULE StencilConvolution
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/multivector.hpp>
#include <vexcl/stencil.hpp>
#include "context_setup.hpp"

struct index {
    size_t n;
    index(size_t n) : n(n) {}

    size_t operator()(size_t i, long shift) const {
        return std::min<size_t>(n - 1, std::max<long>(0, static_cast<long>(i) + shift));
    }
};

BOOST_AUTO_TEST_CASE(stencil_convolution)
{
    const size_t n = 1024;

    std::vector<double> s = random_vector<double>(rand() % 64 + 1);

    int center = rand() % s.size();

    vex::stencil<double> S(ctx, s, center);

    std::vector<double> x = random_vector<double>(n);
    std::generate(x.begin(), x.end(), [](){ return (double)rand() / RAND_MAX; });

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = 1;
    Y += X * S;

    index idx(n);

    check_sample(Y, [&](size_t i, double a) {
        double sum = 1;
        size_t j = 0;
        int k = -center;
        for(; j < s.size(); k++, j++)
            sum += s[j] * x[idx(i, k)];
        BOOST_CHECK_CLOSE(a, sum, 1e-8);
    });

    Y = 42 * (X * S);

    check_sample(Y, [&](size_t i, double a) {
        double sum = 0;
        size_t j = 0;
        int k = -center;
        for(; j < s.size(); k++, j++)
            sum += s[j] * x[idx(i, k)];
        BOOST_CHECK_CLOSE(a, 42 * sum, 1e-8);
    });
}

#if BOOST_VERSION >= 105000
// Boost upto v1.49 segfaults on this test
BOOST_AUTO_TEST_CASE(two_stencils)
{
    const size_t n = 32;
    std::vector<double> s(5, 1);
    vex::stencil<double> S(ctx, s, 3);
    vex::vector<double> X(ctx, n);
    vex::vector<double> Y(ctx, n);

    X = 0;
    Y = X * S + X * S;

    BOOST_CHECK(Y[ 0] == 0);
    BOOST_CHECK(Y[16] == 0);
    BOOST_CHECK(Y[31] == 0);
}
#endif

BOOST_AUTO_TEST_CASE(small_vector)
{
    const size_t n = 128;

    std::vector<double> s = random_vector<double>(rand() % 64 + 1);

    int center = rand() % s.size();

    vex::stencil<double> S(ctx, s, center);

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = 1;
    Y += X * S;

    index idx(n);

    check_sample(Y, [&](size_t i, double a) {
        double sum = 1;
        size_t j = 0;
        int k = -center;
        for(; j < s.size(); k++, j++)
            sum += s[j] * x[idx(i, k)];
        BOOST_CHECK_CLOSE(a, sum, 1e-8);
    });
}

BOOST_AUTO_TEST_CASE(multivector)
{
    typedef std::array<double, 2> elem_t;
    const size_t n = 1024;

    std::vector<double> s = random_vector<double>(rand() % 64 + 1);
    int center = rand() % s.size();

    vex::stencil<double> S(ctx, s.begin(), s.end(), center);

    std::vector<double> x = random_vector<double>(2 * n);

    vex::multivector<double,2> X(ctx, x);
    vex::multivector<double,2> Y(ctx, n);

    Y = 1;
    Y += X * S;

    index idx(n);

    check_sample(Y, [&](size_t i, elem_t a) {
        double sum[2] = {1, 1};
        size_t j = 0;
        int k = -center;
        for(; j < s.size(); k++, j++) {
            sum[0] += s[j] * x[0 + idx(i, k)];
            sum[1] += s[j] * x[n + idx(i, k)];
        }

        BOOST_CHECK_CLOSE(a[0], sum[0], 1e-8);
        BOOST_CHECK_CLOSE(a[1], sum[1], 1e-8);
    });

    Y = 42 * (X * S);

    check_sample(Y, [&](size_t i, elem_t a) {
        double sum[2] = {0, 0};
        size_t j = 0;
        int k = -center;
        for(; j < s.size(); k++, j++) {
            sum[0] += s[j] * x[0 + idx(i, k)];
            sum[1] += s[j] * x[n + idx(i, k)];
        }

        BOOST_CHECK_CLOSE(a[0], 42 * sum[0], 1e-8);
        BOOST_CHECK_CLOSE(a[1], 42 * sum[1], 1e-8);
    });
}

BOOST_AUTO_TEST_CASE(big_stencil)
{
    const size_t n = 1 << 16;

    std::vector<double> s = random_vector<double>(2048);
    int center = rand() % s.size();

    vex::stencil<double> S(ctx, s, center);

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    index idx(n);

    Y = X * S;
    check_sample(Y, [&](size_t i, double a) {
        double sum = 0;
        size_t j = 0;
        int k = -center;
        for(; j < s.size(); k++, j++)
            sum += s[j] * x[idx(i, k)];
        BOOST_CHECK_CLOSE(a, sum, 1e-8);
    });
}

BOOST_AUTO_TEST_CASE(user_defined_stencil)
{
    const size_t n = 1024;

    VEX_STENCIL_OPERATOR(oscillate,
            double, 3, 1,  "return sin(X[1] - X[0]) + sin(X[0] - X[-1]);",
            ctx);

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = oscillate(X);

    index idx(n);

    check_sample(Y, [&](size_t i, double a) {
        size_t left  = idx(i, -1);
        size_t right = idx(i, +1);
        double s = sin(x[right] - x[i]) + sin(x[i] - x[left]);
        BOOST_CHECK_CLOSE(a, s, 1e-8);
    });

#if BOOST_VERSION >= 105000
    Y = 41 * oscillate(X) + oscillate(X);

    check_sample(Y, [&](size_t i, double a) {
        size_t left  = idx(i, -1);
        size_t right = idx(i, +1);
        double s = sin(x[right] - x[i]) + sin(x[i] - x[left]);
        BOOST_CHECK_CLOSE(a, 42 * s, 1e-8);
    });
#endif
}

BOOST_AUTO_TEST_SUITE_END()


#include "vi-map-helpers/vi-map-manipulation.h"

#include <Eigen/Dense>
#include <aslam/common/memory.h>
#include <maplab-common/accessors.h>
#include <maplab-common/conversions.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/progress-bar.h>
#include <vi-map/vi-map.h>

namespace vi_map_helpers {

    VIMapManipulation::VIMapManipulation(vi_map::VIMap* map)
            : map_(*CHECK_NOTNULL(map)), geometry_(map_), queries_(map_) {}

    void VIMapManipulation::rotate(const size_t dimension, const double degrees) {
        pose::Transformation T_G_old_G_new(
                Eigen::Vector3d(0, 0, 0),
                Eigen::Quaterniond(
                        Eigen::AngleAxisd(
                                degrees * kDegToRad, Eigen::Vector3d::Unit(dimension)))
                        .normalized());
        vi_map::MissionBaseFrameIdList frames;
        map_.getAllMissionBaseFrameIds(&frames);
        for (const vi_map::MissionBaseFrameId& id : frames) {
            vi_map::MissionBaseFrame& frame = map_.getMissionBaseFrame(id);
            frame.set_T_G_M(T_G_old_G_new * frame.get_T_G_M());
        }
    }

    void VIMapManipulation::alignToXYPlane(const vi_map::MissionId& mission_id) {
        CHECK(map_.hasMission(mission_id));
        // Let bv_M_z be the eigenvector of lowest eigenvalue of the vertex position
        // covariance. We then first rotate along bv_G_R_A x bv_G_z (z unit vector)
        // until bv_M_z x bv_G_z is minimized. We then rotate along the new bv_G_R_A
        // until bv_M_z x bv_G_z is zero.
        vi_map::MissionBaseFrame& base_frame =
                map_.getMissionBaseFrameForMission(mission_id);
        Eigen::Vector3d bv_M_z;
        Eigen::Vector3d bv_G_R_A_normalized;
        // bv_G_R_A x bv_G_z rotation
        {
            Eigen::Vector3d eigenvalues;
            Eigen::Matrix3d eigenvectors;
            geometry_.get_p_G_I_CovarianceEigenValuesAndVectorsAscending(
                    mission_id, &eigenvalues, &eigenvectors);
            bv_M_z = eigenvectors.col(0).normalized();
            if (bv_M_z(2) < 0) {
                bv_M_z *= -1.;
            }
            bv_G_R_A_normalized =
                    geometry_.get_bv_G_root_average(mission_id).normalized();
            // Angle to rotate along bv_G_R_A x bv_G_z = angle between bv_G_z and the
            // projection of bv_M_z into the plane bv_G_R_A x bv_G_z.
            // atan() intended, want range +- pi/2
            const double pitch_angle_rad =
                    atan(bv_M_z.head<2>().dot(bv_G_R_A_normalized.head<2>()) / bv_M_z(2));
            const Eigen::Vector3d R_G_M_old_new =
                    bv_G_R_A_normalized.cross(Eigen::Vector3d::UnitZ()).normalized() *
                    pitch_angle_rad;
            const pose::Quaternion q_G_M_old_new(R_G_M_old_new);
            base_frame.set_T_G_M(
                    pose::Transformation(q_G_M_old_new, Eigen::Vector3d::Zero()) *
                    base_frame.get_T_G_M());

            bv_M_z = q_G_M_old_new.rotate(bv_M_z);
        }
        // bv_G_R_A rotation
        {
            const double roll_angle_rad = acos(bv_M_z(2));
            bv_G_R_A_normalized(2) = 0;
            bv_G_R_A_normalized.normalize();
            const pose::Quaternion q_G_M_old_new(
                    Eigen::Vector3d(bv_G_R_A_normalized * -roll_angle_rad));
            base_frame.set_T_G_M(
                    pose::Transformation(q_G_M_old_new, Eigen::Vector3d::Zero()) *
                    base_frame.get_T_G_M());
        }
    }

    void VIMapManipulation::getViwlsEdgesWithoutImuMeasurements(
            const vi_map::MissionId& mission_id,
            pose_graph::EdgeIdList* corrupt_edge_ids) const {
        CHECK(map_.hasMission(mission_id));
        CHECK_NOTNULL(corrupt_edge_ids)->clear();

        pose_graph::EdgeIdList edge_ids;
        map_.getAllEdgeIdsInMissionAlongGraph(
                mission_id, pose_graph::Edge::EdgeType::kViwls, &edge_ids);

        CHECK(!edge_ids.empty()) << "No Viwls edges found in mission "
                                 << mission_id.hexString();

        for (size_t i = 0; i < edge_ids.size(); ++i) {
            const vi_map::ViwlsEdge& edge =
                    map_.getEdgeAs<vi_map::ViwlsEdge>(edge_ids[i]);
            if (edge.getImuData().size() == 0) {
                corrupt_edge_ids->push_back(edge_ids[i]);

                VLOG(2) << "Found edge with no IMU data: " << edge_ids[i].hexString();
                VLOG(2) << "Edge index within mission: " << i;
            }
        }
    }

    void VIMapManipulation::fixViwlsEdgesWithoutImuMeasurements(
            const vi_map::MissionId& mission_id) {
        CHECK(map_.hasMission(mission_id));

        pose_graph::EdgeIdList corrupt_edge_ids;
        getViwlsEdgesWithoutImuMeasurements(mission_id, &corrupt_edge_ids);

        if (corrupt_edge_ids.empty()) {
            VLOG(2) << "No corrupt edges found in mission: " << mission_id.hexString();
            return;
        }
        pose_graph::EdgeIdList all_edge_ids;
        map_.getAllEdgeIdsInMissionAlongGraph(mission_id, &all_edge_ids);
        CHECK_LT(corrupt_edge_ids.size(), all_edge_ids.size()) << "All Viwls edges "
                                                               << "are corrupt.";

        for (const pose_graph::EdgeId& edge_id : corrupt_edge_ids) {
            VLOG(2) << "Fixing edge " << edge_id;
            const vi_map::ViwlsEdge& current_edge =
                    map_.getEdgeAs<vi_map::ViwlsEdge>(edge_id);

            pose_graph::EdgeId new_edge_id;
            common::generateId(&new_edge_id);

            Eigen::Matrix<int64_t, 1, Eigen::Dynamic> new_imu_timestamps;
            Eigen::Matrix<double, 6, Eigen::Dynamic> new_imu_data;

            const pose_graph::VertexId& vertex_from = current_edge.from();
            const pose_graph::VertexId& vertex_to = current_edge.to();

            pose_graph::EdgeIdSet incoming_edges_from;
            map_.getVertex(vertex_from).getIncomingEdges(&incoming_edges_from);

            // Get the IMU measurements from the previous edge.
            Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps_from;
            Eigen::Matrix<double, 6, Eigen::Dynamic> imu_data_from;
            for (const pose_graph::EdgeId& incoming_edge_id : incoming_edges_from) {
                if (map_.getEdgeType(incoming_edge_id) ==
                    pose_graph::Edge::EdgeType::kViwls) {
                    const vi_map::ViwlsEdge& incoming_edge =
                            map_.getEdgeAs<vi_map::ViwlsEdge>(incoming_edge_id);

                    imu_timestamps_from = incoming_edge.getImuTimestamps();
                    imu_data_from = incoming_edge.getImuData();
                    break;
                }
            }

            // Add the last IMU measurement from the previous edge.
            if (imu_timestamps_from.cols() > 0u) {
                new_imu_timestamps.conservativeResize(
                        Eigen::NoChange, new_imu_timestamps.cols() + 1);
                new_imu_timestamps.col(new_imu_timestamps.cols() - 1) =
                        imu_timestamps_from.rightCols(1);
                new_imu_data.conservativeResize(Eigen::NoChange, new_imu_data.cols() + 1);
                new_imu_data.col(new_imu_data.cols() - 1) = imu_data_from.rightCols(1);
            } else {
                // Since corrupt edges are in graph traversal order, if the previous
                // edge is corrupt it must be the first edge (otherwise it already
                // crashed), so we need to remove the first vertex in the mission
                // (vertex_from) and make vertex_to the new root vertex for the mission.
                map_.removeVertex(vertex_from);
                vi_map::VIMission& mission = map_.getMission(mission_id);
                mission.setRootVertexId(vertex_to);
                map_.removeEdge(edge_id);
                continue;
            }

            pose_graph::EdgeIdSet outgoing_edges_to;
            map_.getVertex(vertex_to).getOutgoingEdges(&outgoing_edges_to);

            // Get the IMU measurements from the following edge.
            Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps_to;
            Eigen::Matrix<double, 6, Eigen::Dynamic> imu_data_to;
            for (const pose_graph::EdgeId& outgoing_edge_id : outgoing_edges_to) {
                if (map_.getEdgeType(outgoing_edge_id) ==
                    pose_graph::Edge::EdgeType::kViwls) {
                    const vi_map::ViwlsEdge& outgoing_edge =
                            map_.getEdgeAs<vi_map::ViwlsEdge>(outgoing_edge_id);

                    imu_timestamps_to = outgoing_edge.getImuTimestamps();
                    imu_data_to = outgoing_edge.getImuData();
                    break;
                }
            }
            if (imu_timestamps_to.cols() > 0u) {
                constexpr unsigned int kNumInterpolationTerms = 3u;
                new_imu_timestamps.conservativeResize(
                        Eigen::NoChange,
                        new_imu_timestamps.cols() + kNumInterpolationTerms + 1);
                new_imu_data.conservativeResize(
                        Eigen::NoChange, new_imu_data.cols() + kNumInterpolationTerms + 1);
                // Add interpolation terms for stability.
                Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps_interp_step =
                        (imu_timestamps_to.leftCols(1) - imu_timestamps_from.rightCols(1)) /
                        (kNumInterpolationTerms + 1);
                Eigen::Matrix<double, 6, Eigen::Dynamic> imu_data_interp_step =
                        (imu_data_to.leftCols(1) + imu_data_from.rightCols(1)) /
                        (kNumInterpolationTerms + 1);
                for (unsigned int i = 0u; i < kNumInterpolationTerms; ++i) {
                    const unsigned int index =
                            new_imu_timestamps.cols() - kNumInterpolationTerms - 1 + i;
                    new_imu_timestamps.col(index) =
                            new_imu_timestamps.col(index - 1) + imu_timestamps_interp_step;
                    new_imu_data.col(index) =
                            new_imu_data.col(index - 1) + imu_data_interp_step;
                }
                // Add the first IMU measurement from the following edge.
                new_imu_timestamps.col(new_imu_timestamps.cols() - 1) =
                        imu_timestamps_to.leftCols(1);
                new_imu_data.col(new_imu_data.cols() - 1) = imu_data_to.leftCols(1);
            } else {
                const bool kIsVertexToLastVertexInMission = outgoing_edges_to.size() == 0;
                CHECK(kIsVertexToLastVertexInMission) << "Consecutive corrupt edges.";
                // The corrupt edge is the last edge, so it can be safely removed along
                // with the last vertex.
                map_.removeEdge(edge_id);
                map_.removeVertex(vertex_to);
                continue;
            }

            VLOG(3) << "Adding the following IMU timestamps: " << new_imu_timestamps;
            VLOG(3) << "Adding the following IMU data: " << new_imu_data;
            vi_map::Edge* new_edge_ptr(
                    new vi_map::ViwlsEdge(
                            new_edge_id, vertex_from, vertex_to, new_imu_timestamps,
                            new_imu_data));

            // Align the vertex_from and vertex_to to avoid overextending the edge.
            map_.getVertexPtr(vertex_to)->set_T_M_I(
                    map_.getVertex(vertex_from).get_T_M_I());

            map_.removeEdge(edge_id);
            map_.addEdge(vi_map::Edge::UniquePtr(new_edge_ptr));
        }
    }

    void VIMapManipulation::removePosegraphAfter(
            const pose_graph::VertexId& vertex_id,
            pose_graph::VertexIdList* removed_vertex_ids) {
        CHECK_NOTNULL(removed_vertex_ids)->clear();
        CHECK(vertex_id.isValid());

        constexpr bool kIncludeStartingVertex = false;
        queries_.getFollowingVertexIdsAlongGraph(
                vertex_id, kIncludeStartingVertex, removed_vertex_ids);

        removeVerticesAndIncomingEdges(*removed_vertex_ids);
    }

    void VIMapManipulation::removeVerticesAndIncomingEdges(
            const pose_graph::VertexIdList& vertex_ids) {
        // Get all edges to delete.
        pose_graph::EdgeIdSet edges_to_remove;
        for (const pose_graph::VertexId& vertex_id : vertex_ids) {
            const vi_map::Vertex& vertex = map_.getVertex(vertex_id);

            pose_graph::EdgeIdSet incoming_edges;
            vertex.getIncomingEdges(&incoming_edges);
            edges_to_remove.insert(incoming_edges.begin(), incoming_edges.end());
        }

        // Remove the elements from the map.
        for (const pose_graph::EdgeId& edge_id : edges_to_remove) {
            map_.removeEdge(edge_id);
        }

        for (const pose_graph::VertexId& vertex_id : vertex_ids) {
            map_.removeVertex(vertex_id);
        }
    }
//初始化任务中没有被用到的特征点
    size_t VIMapManipulation::initializeLandmarksFromUnusedFeatureTracksOfMission(
            const vi_map::MissionId& mission_id)
    {
        CHECK(mission_id.isValid());
        pose_graph::VertexIdList all_vertices_in_missions;
        map_.getAllVertexIdsInMissionAlongGraph(//通过这个任务的图结构找到所有的节点的集合all_vertices_in_missions
                mission_id, &all_vertices_in_missions);
//每个节点存的就是每个时刻的位姿，关键点，所以根节点应该就是第一帧
        TrackIndexToLandmarkIdMap track_id_to_landmark_id;//std::unordered_map<int, vi_map::LandmarkId>,应该是一个节点对应着多个地图点
        track_id_to_landmark_id.reserve(1000u * all_vertices_in_missions.size());
        const size_t num_landmarks_initial = map_.numLandmarks();//通过遍历这个地图下所有的节点，每个节点都存储了地图点，所以把所有节点的地图点的数量相加
        initializeLandmarksFromUnusedFeatureTracksOfOrderedVertices(
                all_vertices_in_missions, &track_id_to_landmark_id);//all_vertices_in_missions已经有了所有的节点了

        const size_t num_new_landmarks = map_.numLandmarks() - num_landmarks_initial;//更新后增加的地图点就是新的地图点
        return num_new_landmarks;
    }
//初始化节点中没有被用到的特征点,是通过遍历每一个节点去更新地图点的信息
    void VIMapManipulation::
    initializeLandmarksFromUnusedFeatureTracksOfOrderedVertices(
            const pose_graph::VertexIdList& ordered_vertex_ids,
            TrackIndexToLandmarkIdMap* trackid_landmarkid_map) {
        for (const pose_graph::VertexId& vertex_id : ordered_vertex_ids) {//遍历所有的节点
            initializeLandmarksFromUnusedFeatureTracksOfVertex(
                    vertex_id, trackid_landmarkid_map);
        }
    }

    void VIMapManipulation::initializeLandmarksFromUnusedFeatureTracksOfVertex(
            const pose_graph::VertexId& vertex_id,
            TrackIndexToLandmarkIdMap* trackid_landmarkid_map) {
        CHECK_NOTNULL(trackid_landmarkid_map);
        const vi_map::Vertex& vertex = map_.getVertex(vertex_id);//得到当前节点

        vertex.forEachFrame(//这里我感觉是多相机的意思么，遍历每一个相机，遍历所有的特征点，如果之前没有观察到，则添加一个新的地标，否则更新地图点和节点的观测信息
                [this, &vertex, &trackid_landmarkid_map](
                        const size_t frame_index, const aslam::VisualFrame& frame)
                {
                    const size_t num_keypoints = frame.getNumKeypointMeasurements();
                    if (!frame.hasTrackIds()) {
                        VLOG(3) << "Frame has no tracking information. Skipping frame...";
                        return;
                    }
                    const Eigen::VectorXi& track_ids = frame.getTrackIds();

                    CHECK_EQ(static_cast<int>(num_keypoints), track_ids.rows());//一行就是一个特征点

                    vi_map::LandmarkIdList landmark_ids;
                    vertex.getFrameObservedLandmarkIds(frame_index, &landmark_ids);//得到当前节点的这个相机的所有观测点

                    ///遍历此帧的所有轨迹，如果之前没有观察到，则添加一个新的地标，否则添加一个观察反向链接。
                    // Go over all tracks of this frame and add a new landmark if it wasn't
                    // observed before, otherwise add an observation backlink.
                    for (size_t keypoint_i = 0u; keypoint_i < num_keypoints; ++keypoint_i) {
                        const int track_id = track_ids(keypoint_i);

                        // Skip non-tracked landmark observation.
                        if (track_id < 0 || landmark_ids[keypoint_i].isValid()) {
                            continue;
                        }

                        // Check whether this track has already a global landmark id
                        // associated.
                        const vi_map::LandmarkId* landmark_id_ptr =
                                common::getValuePtr(*trackid_landmarkid_map, track_id);
//是能观测到这个全局的地图点被联系上了，需要更新这个节点观测到3d的信息，以及这个路标点被观测到的信息
                        if (landmark_id_ptr != nullptr &&
                            map_.hasLandmark(*landmark_id_ptr))
                        {
                            map_.associateKeypointWithExistingLandmark(
                                    vertex.id(), frame_index, keypoint_i, *landmark_id_ptr);
                        } else {
                            // Assign a new global landmark id to this track if it hasn't
                            // been seen before and add a new landmark to the map.
                            vi_map::LandmarkId landmark_id =//新建一个路标点的id
                                    common::createRandomId<vi_map::LandmarkId>();
                            // operator[] intended as this is either overwriting an old outdated
                            // entry or creating a new one.
                            (*trackid_landmarkid_map)[track_id] = landmark_id;//需要对这个节点追踪到3d点的信息进行更新

                            vi_map::KeypointIdentifier keypoint_id;//KeypointIdentifier是路标点的一个数据结构
                            keypoint_id.frame_id.frame_index = frame_index;
                            keypoint_id.frame_id.vertex_id = vertex.id();
                            keypoint_id.keypoint_index = keypoint_i;
                            map_.addNewLandmark(landmark_id, keypoint_id);//在地图中加入一个新的地图点，并且会去更新地图点相关联的一些其他数据
                        }
                    }
                });
    }

    size_t VIMapManipulation::mergeLandmarksBasedOnTrackIds(
            const vi_map::MissionId& mission_id) {
        CHECK(map_.hasMission(mission_id));
        typedef std::unordered_map<int, vi_map::LandmarkId>
                TrackIdToLandmarkIdMap;
        TrackIdToLandmarkIdMap track_id_to_store_landmark_id;
        size_t num_merges = 0u;

        pose_graph::VertexIdList vertex_ids;
        map_.getAllVertexIdsInMission(mission_id, &vertex_ids);

        for (const pose_graph::VertexId& vertex_id : vertex_ids) {
            const vi_map::Vertex& vertex = map_.getVertex(vertex_id);

            for (size_t frame_idx = 0u; frame_idx < vertex.numFrames(); ++frame_idx) {
                vi_map::LandmarkIdList landmark_ids;
                vertex.getFrameObservedLandmarkIds(frame_idx, &landmark_ids);

                CHECK(vertex.getVisualFrame(frame_idx).hasTrackIds());
                const Eigen::VectorXi& track_ids =
                        vertex.getVisualFrame(frame_idx).getTrackIds();
                CHECK_EQ(
                        static_cast<size_t>(track_ids.rows()), landmark_ids.size());

                for (size_t keypoint_idx = 0u; keypoint_idx < landmark_ids.size();
                     ++keypoint_idx) {
                    if (landmark_ids[keypoint_idx].isValid() &&
                        track_ids(keypoint_idx) < 0) {
                        const vi_map::LandmarkId landmark_id = landmark_ids[keypoint_idx];
                        CHECK(map_.hasLandmark(landmark_id));

                        if (track_id_to_store_landmark_id
                                .emplace(track_ids(keypoint_idx), landmark_id).second) {
                            // Emplace succeeded so this track ID was not used before.
                        } else {
                            // Emplace failed so this track ID is already used, we need to
                            // merge landmarks.
                            TrackIdToLandmarkIdMap::const_iterator it =
                                    track_id_to_store_landmark_id.find(track_ids(keypoint_idx));
                            CHECK(it != track_id_to_store_landmark_id.end());
                            CHECK(it->second.isValid());
                            CHECK(map_.hasLandmark(it->second)) << "Landmark "
                                                                << it->second.hexString()
                                                                << " not found in the map.";

                            if (it->second != landmark_id) {
                                map_.mergeLandmarks(landmark_id, it->second);
                                ++num_merges;

                                // As the dataset could be loop-closed before (so more than
                                // a single track id pointing to the same store landmark, we need
                                // to update the track id to store landmark id map.
                                for (TrackIdToLandmarkIdMap::value_type& track_id_to_landmark :
                                        track_id_to_store_landmark_id) {
                                    if (track_id_to_landmark.second == landmark_id) {
                                        track_id_to_landmark.second = it->second;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
        VLOG(2) << "Number of merges " << num_merges;
        return num_merges;
    }

    void VIMapManipulation::releaseOldVisualFrameImages(
            const pose_graph::VertexId& current_vertex_id,
            const int image_removal_age_threshold) {
        CHECK_GT(image_removal_age_threshold, 0);
        CHECK(map_.hasVertex(current_vertex_id));

        const vi_map::MissionId& mission_id =
                map_.getVertex(current_vertex_id).getMissionId();

        pose_graph::VertexId vertex_id = current_vertex_id;
        for (int i = 0; i < image_removal_age_threshold; ++i) {
            map_.getPreviousVertex(
                    vertex_id, map_.getGraphTraversalEdgeType(mission_id), &vertex_id);
        }

        while (map_.getPreviousVertex(
                vertex_id, map_.getGraphTraversalEdgeType(mission_id), &vertex_id)) {
            const vi_map::Vertex& vertex = map_.getVertex(vertex_id);
            for (size_t i = 0; i < vertex.numFrames(); ++i) {
                const aslam::VisualFrame& const_vframe = vertex.getVisualFrame(i);
                if (const_vframe.isValid() && const_vframe.hasRawImage()) {
                    aslam::VisualFrame::Ptr visual_frame =
                            map_.getVertex(vertex_id).getVisualFrameShared(i);
                    visual_frame->releaseRawImage();
                    CHECK(!visual_frame->hasRawImage());
                }
            }
        }
    }

    size_t VIMapManipulation::removeBadLandmarks() {
        vi_map::LandmarkIdList bad_landmark_ids;
        queries_.getAllNotWellConstrainedLandmarkIds(&bad_landmark_ids);

        for (const vi_map::LandmarkId& bad_landmark_id : bad_landmark_ids) {
            map_.removeLandmark(bad_landmark_id);
        }
        return bad_landmark_ids.size();
    }

}  // namespace vi_map_helpers


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <map>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/keplerPropagatorTestData.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerEphemeris.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_keplerEphemeris )

//! Test 1: Comparison of KeplerEphemeris output with benchmark data from (Melman, 2010).
//! (see testPropagateKeplerOrbit_Eccentric_Melman).
BOOST_AUTO_TEST_CASE( testKeplerEphemerisElliptical )
{
    // Load the expected propagation history.
    // Create expected propagation history.
    PropagationHistory expectedPropagationHistory = getODTBXBenchmarkData( );

    // Set Earth gravitational parameter [m^3 s^-2].
    const double earthGravitationalParameter = 398600.4415e9;

    // Compute propagation history.
    PropagationHistory computedPropagationHistory;
    computedPropagationHistory[ 0.0 ] = expectedPropagationHistory[ 0.0 ];

    ephemerides::KeplerEphemeris keplerEphemeris(
                expectedPropagationHistory[ 0.0 ],
                0.0, earthGravitationalParameter );

    for( PropagationHistory::iterator stateIterator = expectedPropagationHistory.begin( );
         stateIterator != expectedPropagationHistory.end( ); stateIterator++ )
    {
        // Compute next entry.
        computedPropagationHistory[ stateIterator->first ] =
                orbital_element_conversions::convertCartesianToKeplerianElements(
                    keplerEphemeris.getCartesianState( stateIterator->first ),
                    earthGravitationalParameter );

        // Check that computed results match expected results.
        BOOST_CHECK_CLOSE_FRACTION(
                    computedPropagationHistory[ stateIterator->first ]( 5 ),
                    expectedPropagationHistory[ stateIterator->first ]( 5 ),
                    2.5e-14 );
    }
}

//! Test 2: Comparison of KeplerEphemeris with that of GTOP (hyperbolic).
//! (see testPropagateKeplerOrbit_hyperbolic_GTOP).
BOOST_AUTO_TEST_CASE( testKeplerEphemerisHyperbolic )
{
    // Load the expected propagation history.
    PropagationHistory expectedPropagationHistory = getGTOPBenchmarkData( );

    // Compute propagation history.
    PropagationHistory computedPropagationHistory;
    computedPropagationHistory[ 0.0 ] = expectedPropagationHistory[ 0.0 ];

    ephemerides::KeplerEphemeris keplerEphemeris(
                expectedPropagationHistory[ 0.0 ],
                0.0, getGTOPGravitationalParameter( ) );

    for( PropagationHistory::iterator stateIterator = expectedPropagationHistory.begin( );
         stateIterator != expectedPropagationHistory.end( ); stateIterator++ )
    {
        // Compute next entry.
        computedPropagationHistory[ stateIterator->first ] =
                orbital_element_conversions::convertCartesianToKeplerianElements(
                    keplerEphemeris.getCartesianState( stateIterator->first ),
                    getGTOPGravitationalParameter( ) );

        // Check that computed results match expected results.
        BOOST_CHECK_CLOSE_FRACTION(
                    computedPropagationHistory[ stateIterator->first ]( 5 ),
                    expectedPropagationHistory[ stateIterator->first ]( 5 ),
                    1.0e-15 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"

#include <qle/models/normalsabr.hpp>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(NormalFreeBoundarySabrTest)

BOOST_AUTO_TEST_CASE(testNormalFreeBoundarySabr) {

    BOOST_TEST_MESSAGE("Testing normal free boundary SABR...");

    Real forward = 0.0, expiryTime = 5.0, alpha = 0.0050, nu = 0.52, rho = -0.23;

    BOOST_TEST_MESSAGE("vol=" << normalFreeBoundarySabrVolatility(0.0, forward, expiryTime, alpha, nu, rho));

    // WIP...

    // for (Real strike = -0.10; strike < 0.10 + 1E-5; strike += 0.0001) {
    //     BOOST_TEST_MESSAGE(strike << " " << normalSabrVolatility(strike, forward, expiryTime, alpha, nu, rho) << " "
    //                               << normalFreeBoundarySabrVolatility(strike, forward, expiryTime, alpha, nu, rho));
    // }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include "stdafx.h"
#include <cmath>
#include <vector>
#include <stack>
#include <unordered_map>
#include <unordered_set>
#include <iostream>
#include <fmt/format.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "contest_types.h"
#include "solver_registry.h"
#include "solver_util.h"
#include "visual_editor.h"
#include "timer.h"
#include "judge.h"

namespace NNaiveSearchSolver {
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>
void shrink(T& point_a, T& point_b) {
  constexpr double eps = 1e-6;
  auto shrink_a = point_a * (1.0 - eps) + point_b * eps;
  auto shrink_b = point_b * (1.0 - eps) + point_a * eps;
  point_a = shrink_a;
  point_b = shrink_b;
}

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;
}

std::vector<Point> enumerate_interior_points(const SProblem& problem) {
  integer ymin = INT_MAX, ymax = INT_MIN;
  integer xmin = INT_MAX, xmax = INT_MIN;
  for (auto p : problem.hole_polygon) {
    xmin = std::min(xmin, get_x(p));
    ymin = std::min(ymin, get_y(p));
    xmax = std::max(xmax, get_x(p));
    ymax = std::max(ymax, get_y(p));
  }

#if 0
  auto bg_hole_polygon = ToBoostPolygon(problem.hole_polygon);
  std::vector<Point> points;
  for (int y = ymin; y <= ymax; ++y) {
    for (int x = xmin; x <= xmax; ++x) {
      if (bg::within(ToBoostPoint(Point{x, y}), bg_hole_polygon)) { // exclude points on the edge/vertex of the hole.
        points.emplace_back(x, y);
      }
    }
  }
#else
  std::vector<Point> points;
  for (int y = ymin; y <= ymax; ++y) {
    for (int x = xmin; x <= xmax; ++x) {
      if (contains(problem.hole_polygon, {x, y}) != EContains::EOUT) { // include points on the edge/vertex of the hole.
        points.emplace_back(x, y);
      }
    }
  }
#endif
  LOG(INFO) << fmt::format("found {} interior points", points.size());
  return points;
}

std::vector<Point> enumerate_interior_border_points(const SProblem& problem, double border_distance) {
  integer ymin = INT_MAX, ymax = INT_MIN;
  integer xmin = INT_MAX, xmax = INT_MIN;
  for (auto p : problem.hole_polygon) {
    xmin = std::min(xmin, get_x(p));
    ymin = std::min(ymin, get_y(p));
    xmax = std::max(xmax, get_x(p));
    ymax = std::max(ymax, get_y(p));
  }

  auto bg_hole_polygon = ToBoostPolygon(problem.hole_polygon);

  std::vector<Point> points;
  for (int y = ymin; y <= ymax; ++y) {
    for (int x = xmin; x <= xmax; ++x) {
      if (contains(problem.hole_polygon, {x, y}) != EContains::EOUT) { // include points on the edge/vertex of the hole.
        // minimum distance to an edge.
        double d = DBL_MAX;
        for (int eid = 0; eid <= problem.hole_polygon.size(); ++eid) {
          BoostLinestring linestring{ToBoostPoint(problem.hole_polygon[eid]), ToBoostPoint(problem.hole_polygon[(eid + 1) % problem.hole_polygon.size()])};
          std::vector<BoostLinestring> differences;
          chmin(d, bg::distance(linestring, ToBoostPoint(Point{x, y})));
        }

        if (d < border_distance) {
          points.emplace_back(x, y);
        }
      }
    }
  }

  LOG(INFO) << fmt::format("found {} border points", points.size());
  return points;
}

class NaiveSearchSolver : public SolverBase {
private:
  std::mt19937 rng;

public:
  NaiveSearchSolver() { }
  SolverOutputs solve(const SolverArguments &args) override {
    SolverOutputs ret;

    // init.
    ret.solution = args.optional_initial_solution ? args.optional_initial_solution : args.problem->create_solution();

    // develop.
    constexpr int check_timeout_every_iter = 100000;
    constexpr int report_every_iter = 100000;
    constexpr int editor_sleep = 1;
    constexpr bool exhaustive_search = true;
    constexpr bool use_cache_for_good_edge = true;
    constexpr bool use_cache_for_movable_edges_with_tolerance = true;
    constexpr bool use_cache_for_infeasible_placement_set = true;
    constexpr size_t infeasible_placement_cache_size_B = 4 * 1024ull * 1024ull * 1024ull;
    const std::optional<double> start_from_border_distance = 3.0;
    const std::optional<int> subsample_roots = std::nullopt;
    SVisualEditorPtr editor;
    if (args.visualize) {
      editor = std::make_shared<SVisualEditor>(args.problem, "NaiveSearchSolver", "visualize");
    }

    // prepare.
    auto interior_points = enumerate_interior_points(*args.problem);
    std::shuffle(interior_points.begin(), interior_points.end(), rng);
    std::vector<Point> interior_border_points;
    if (start_from_border_distance) {
      LOG(INFO) << fmt::format("start from border r={}", *start_from_border_distance);
      interior_border_points = enumerate_interior_border_points(*args.problem, *start_from_border_distance);
      std::shuffle(interior_border_points.begin(), interior_border_points.end(), rng);
    }
    const std::vector<std::vector<int>> edges_from_vertex_cache = edges_from_vertex(*args.problem);
    const int V = args.problem->vertices.size();

    // good edge. assume p and q are both interiors.
    auto bg_hole_polygon = ToBoostPolygon(args.problem->hole_polygon);
    auto _is_good_edge = [&](const Point& p, const Point& q) {
      BoostLinestring linestring{ToBoostPoint(p), ToBoostPoint(q)};
      std::vector<BoostLinestring> differences;
      bg::difference(linestring, bg_hole_polygon, differences);
      return differences.empty();
    };
    struct Line_hash {
      std::size_t operator()(const Line& line) const {
        return std::hash<int>()(line[0].first)
             ^ std::hash<int>()(line[0].second)
             ^ std::hash<int>()(line[1].first)
             ^ std::hash<int>()(line[1].second)
          ;
      }
    };
    std::unordered_set<Line, Line_hash> is_bad_edge_cache; // assume |bad edges| < |good edges|
    if (use_cache_for_good_edge) {
      LOG(INFO) << "bad_edge_cache ON: building";
      Timer t("build is_bad_edge_cache");
      const size_t N = interior_points.size();
      size_t c = 0;
      for (size_t i = 0; i < N; ++i) {
        const auto& p = interior_points[i];
        for (size_t j = i; j < N; ++j) {
          const auto& q = interior_points[j];
          if (!_is_good_edge(p, q)) {
            is_bad_edge_cache.insert({p, q});
            is_bad_edge_cache.insert({q, p});
          }
          ++c;
          if (c % 100000 == 0) LOG(INFO) << fmt::format("{}/{} ({:.2f}%) bad_edges={}", c, N * (N + 1) / 2, 100.0 * c / double(N * (N + 1) / 2), is_bad_edge_cache.size());
        }
      }
      LOG(INFO) << fmt::format("is_bad_edge_cache #{}", is_bad_edge_cache.size());
    } else {
      LOG(INFO) << "bad_edge_cache OFF";
    }
    auto is_good_edge = [&](const Point& p, const Point& q) -> bool {
      if (!use_cache_for_good_edge)
        return _is_good_edge(p, q);

      auto it = is_bad_edge_cache.find({p, q});
      return it == is_bad_edge_cache.end();
    };

    // (starting position, radius**2) -> (available points)
    using SKey = std::pair<Point, int>;
    struct SKey_hash {
      std::size_t operator()(const SKey& key) const {
        return std::hash<int>()(key.first.first)
             ^ std::hash<int>()(key.first.second)
             ^ std::hash<int>()(key.second)
          ;
      }
    };
    int64_t movable_cache_count = 0;
    std::unordered_map<SKey, std::vector<Point>, SKey_hash> movable_cache;
    // exact lookup.
    auto get_movable_points = [&](Point p, int d2) -> std::vector<Point> {
      const SKey query {p, d2};
      auto it = movable_cache.find(query);
      if (it != movable_cache.end()) {
        return it->second;
      }
      std::vector<Point> points;
      for (Point ip : interior_points) {
        if (d2 == distance2(p, ip) && is_good_edge(p, ip)) {
          ++movable_cache_count;
          points.push_back(ip);
        }
      }
      movable_cache.insert(it, {query, points});
      return points;
    };
    // consider tolerance.
    LOG(INFO) << (use_cache_for_movable_edges_with_tolerance ? "movable_points_with_tolerance_cache ON" : "movable_points_with_tolerance_cache OFF");
    std::unordered_map<SKey, std::vector<Point>, SKey_hash> movable_points_with_tolerance_cache;
    auto _get_movable_points_with_tolerance = [&](Point p, int original_distance2) -> std::vector<Point> {
      // |d_new/d_old - 1| <= eps / 1000000
      constexpr int k = 1'000'000;
      const int e = args.problem->epsilon;
      const int distance2_min = std::floor(original_distance2 * double(k - e) / double(k));
      const int distance2_max = std::ceil(original_distance2 * double(k + e) / double(k));
      std::vector<Point> all_points;
      for (int d2 = distance2_min; d2 <= distance2_max; ++d2) {
        if (std::abs(d2 - original_distance2) * k <= original_distance2 * e) { // to make sure.
          auto points = get_movable_points(p, d2);
          std::copy(points.begin(), points.end(), std::back_inserter(all_points));
        }
      }
      return all_points;
    };
    auto get_movable_points_with_tolerance = [&](Point p, int original_distance2) -> std::vector<Point> {
      if (!use_cache_for_movable_edges_with_tolerance)
        return _get_movable_points_with_tolerance(p, original_distance2);

      const SKey query {p, original_distance2};
      auto it = movable_points_with_tolerance_cache.find(query);
      if (it != movable_points_with_tolerance_cache.end()) {
        return it->second;
      }
      auto all_points = _get_movable_points_with_tolerance(p, original_distance2);
      movable_points_with_tolerance_cache.insert(it, {query, all_points});
      return all_points;
    };

    constexpr int USED = 1;
    constexpr int REMAINING = 0;
    struct State;
    using StatePtr = std::shared_ptr<State>;
    struct State {
      // placing vertices[vid] to position p.
      int depth = 0;
      int vid = 0;
      Point p = {0, 0};
      // remaining indices after placing this node.
      std::vector<int> indices_flag;
      // vertices after placing this node.
      std::vector<Point> vertices;
      StatePtr parent;
      bool is_last = false;

      State(int depth, int vid, Point p, const std::vector<int>& indices_flag, const std::vector<Point>& vertices)
        : depth(depth), vid(vid), p(p), indices_flag(indices_flag), vertices(vertices) {}
    };
    auto create_fixed_indices = [&](StatePtr s) {
      std::vector<int> fixed_indices;
      for (int i = 0; i < V; ++i) {
        if (s->indices_flag[i] == USED) {
          fixed_indices.push_back(i);
        }
      }
      return fixed_indices;
    };

    // infeasible placement cache
    int min_x = std::numeric_limits<int>::max();
    int min_y = std::numeric_limits<int>::max();
    int max_x = std::numeric_limits<int>::min();
    int max_y = std::numeric_limits<int>::min();
    for (auto& p : interior_points) {
      chmin<int>(min_x, p.first);
      chmax<int>(max_x, p.first);
      chmin<int>(min_y, p.second);
      chmax<int>(max_y, p.second);
    }
    const int size_x = max_x - min_x + 1;
    const int size_y = max_y - min_y + 1;
    const int key_size = V * size_x * size_y;
    SZobristHash zobrist(key_size);
    const size_t infeasible_placement_cache_size = infeasible_placement_cache_size_B * 8;
    std::vector<bool> infeasible_placement_cache;
    if (use_cache_for_infeasible_placement_set) {
      LOG(INFO) << fmt::format("use_cache_for_infeasible_placement_set ON {:.2f} MB (smaller cache leads to increasing FP) {}x{}x{}", infeasible_placement_cache_size_B / 1024.0 / 1024.0, size_x, size_y, V);
      infeasible_placement_cache.assign(infeasible_placement_cache_size, false);
    } else {
      LOG(INFO) << "use_cache_for_infeasible_placement_set OFF";
    }
    auto State_to_zobrist = [&](StatePtr s) {
      SZobristHash::key_t hash = 0;
      for (int i = 0; i < V; ++i) {
        if (s->indices_flag[i] == USED) {
          const int dx = s->vertices[i].first - min_x;
          const int dy = s->vertices[i].second - min_y;
          const int idx = (i * size_y + dy) * size_x + dx;
          //CHECK(idx < key_size);
          hash ^= zobrist[idx];
        }
      }
      return hash;
    };
    size_t infeasible_cache_hit_count = 0;
    size_t infeasible_cache_query_count = 0;
    size_t infeasible_cache_set_count = 0;
    auto is_infeasible_placement = [&](StatePtr s) {
      const bool infeasible = infeasible_placement_cache[State_to_zobrist(s) % infeasible_placement_cache_size];
      ++infeasible_cache_query_count;
      if (infeasible) ++infeasible_cache_hit_count;
      return infeasible;
    };
    auto set_infeasible_placement = [&](StatePtr s) {
      ++infeasible_cache_set_count;
      infeasible_placement_cache[State_to_zobrist(s) % infeasible_placement_cache_size] = true;
    };

    std::stack<StatePtr> stack;
    // start from any valid points.
    {
      const int start_index = 0;
      std::vector<int> indices_flag(V, REMAINING);
      indices_flag[start_index] = USED;
      for (auto ip : start_from_border_distance ? interior_border_points : interior_points) {
        auto vertices = ret.solution->vertices;
        vertices[start_index] = ip;
        stack.push(std::make_shared<State>(1 /* depth */, start_index, ip, indices_flag, vertices));
      }
    }

    const int n_roots = stack.size();
    integer best_dislikes = std::numeric_limits<integer>::max();
    bool found = false;
    int64_t root_counter = 0;
    int64_t counter = 0;
    int max_depth = 1;
    Timer timer;
    double lazy_elapsed_ms = 0.0; // not always updated.
    if (args.timeout_s) {
      LOG(WARNING) << fmt::format("Timeout : {} s", *args.timeout_s);
    }
    if (subsample_roots) {
      LOG(WARNING) << fmt::format("SUBSAMPLING : {}", *subsample_roots);
    }

    while (!stack.empty()) {
      if (!exhaustive_search && found) {
        break;
      }
      StatePtr s = stack.top(); stack.pop();

      if (s->depth == 1) {
        ++root_counter;
        if (subsample_roots && root_counter > *subsample_roots) {
          LOG(ERROR) << fmt::format("SUBSAMPLING DONE! {}", root_counter);
          break;
        }
      }
      ++counter;
      max_depth = std::max(max_depth, s->depth);
      bool report = (counter % report_every_iter == 0);

      if (counter % check_timeout_every_iter == 0 && args.timeout_s) {
        lazy_elapsed_ms = timer.elapsed_ms();
        if (lazy_elapsed_ms * 1e-3 > *args.timeout_s) {
          LOG(ERROR) << fmt::format("TIMEOUT! {} / {} s", lazy_elapsed_ms * 1e-3, *args.timeout_s);
          break;
        }
      }

      if (s->depth == V) {
        auto temp_solution = args.problem->create_solution(s->vertices);
        auto judge_res = judge(*args.problem, *temp_solution);
        if (judge_res.is_valid()) { // essentially, we do not need this. but for sure..
          if (judge_res.dislikes < best_dislikes) {
            LOG(INFO) << fmt::format("#{} foud better solution {} -> {}", root_counter, best_dislikes, judge_res.dislikes);
            ret.solution = temp_solution;
            best_dislikes = judge_res.dislikes;
            const std::string filename = args.problem->problem_id ? fmt::format("{}.bestsofar.pose.json", *args.problem->problem_id) : "bestsofar.pose.json";
            save_solution(args.problem, ret.solution, "NaiveSearchSolver", filename);
          }
          found = true;
          report = true;
        } else {
          if (false) { // debug.
            auto p = s;
            while (p) {
              LOG(INFO) << p->vid;
              p = p->parent;
            }
            if (editor) {
              editor->set_oneshot_custom_stat("#invalid");
              editor->set_marked_indices(create_fixed_indices(s));
              editor->set_pose(args.problem->create_solution(s->vertices));
              while (editor->show(editor_sleep) != 27)
                ;
            }
          }
        }
      }

      if (report) {
        lazy_elapsed_ms = timer.elapsed_ms();
        const double root_per_s = root_counter / lazy_elapsed_ms * 1e3;
        const auto stat = fmt::format("[{}][{}][best DL={}] root {}/{}({:.2f}%) {:.2f} root/s, ETA {:.2f} s, visited {}, max depth {}, {:.2f} ms, {:.2f} node/s, cache {:.2f} MB, infi {}/{}({:.2f}%)/{}({:.2f}%)",
          args.problem->problem_id ? *args.problem->problem_id : -1,
          found ? "O" : "X", found ? best_dislikes : -1,
          root_counter, interior_points.size(), 100.0 * root_counter / interior_points.size(), root_per_s,
          double(n_roots) / root_per_s,
          counter, max_depth, lazy_elapsed_ms, counter / lazy_elapsed_ms * 1e3,
          movable_cache_count * sizeof(Point) / 1024.0 / 1024.0,
          infeasible_cache_hit_count, infeasible_cache_query_count, 100.0 * infeasible_cache_hit_count / infeasible_cache_query_count,
          infeasible_cache_set_count, 100.0 * infeasible_cache_hit_count / infeasible_cache_set_count
          );
        LOG(INFO) << stat;
        if (editor) {
          editor->set_oneshot_custom_stat(stat);
          editor->set_marked_indices(create_fixed_indices(s));
          editor->set_pose(args.problem->create_solution(s->vertices));
          editor->show(editor_sleep);
        }
      }

      // first enumerate remaining edge ..
      struct SEdgeItem {
        int num_undetermined_edges = 0;
        int eid = -1;
        // smaller is better.
        bool operator<(const SEdgeItem& rhs) const {
          return num_undetermined_edges != rhs.num_undetermined_edges
            ? num_undetermined_edges > rhs.num_undetermined_edges
            : eid < rhs.eid;
        }
      };
      std::vector<SEdgeItem> remaining_edges;
      for (int eid : edges_from_vertex_cache[s->vid]) {
        auto [u, v] = args.problem->edges[eid];
        auto counter_vid = u == s->vid ? v : u;

        if (s->indices_flag[counter_vid] == REMAINING) {
          // count number of determined and undetermined edges.
          int num_determined_edges = 0;
          int num_undetermined_edges = 0;
          for (auto counter_eid : edges_from_vertex_cache[counter_vid]) {
            auto [j, k] = args.problem->edges[counter_eid];
            auto neighbor_vid = j == counter_vid ? k : j;
            if (s->indices_flag[neighbor_vid] == USED) {
              ++num_determined_edges;
            } else {
              ++num_undetermined_edges;
            }
          }
          remaining_edges.push_back({num_undetermined_edges, eid});
        }
      }
      std::sort(remaining_edges.begin(), remaining_edges.end());

      StatePtr last_pushed;
      for (const SEdgeItem& edge : remaining_edges) {
        const int eid = edge.eid;
        auto [u, v] = args.problem->edges[eid];
        auto counter_vid = u == s->vid ? v : u;

        // push nodes for this unused edge (s->vid, counter_vid)
        auto original_distance2 = distance2(args.problem->vertices[s->vid], args.problem->vertices[counter_vid]);
        auto movable_points = get_movable_points_with_tolerance(s->p, original_distance2);

        for (auto p : movable_points) {
          // all fixed neighbors of counter_vid should agree with this move.
          bool agree = true;
          for (auto counter_eid : edges_from_vertex_cache[counter_vid]) {
            auto [j, k] = args.problem->edges[counter_eid];
            auto neighbor_vid = j == counter_vid ? k : j;
            if (s->indices_flag[neighbor_vid] == USED) {
              const double neighbor_original_distance2 = distance2(args.problem->vertices[neighbor_vid], args.problem->vertices[counter_vid]);
              const double neighbor_distance2 = distance2(s->vertices[neighbor_vid], p);
              if (!tolerate(neighbor_original_distance2, neighbor_distance2, args.problem->epsilon) // check this first!
                || !is_good_edge(s->vertices[neighbor_vid], p)) { // heavy.
                agree = false;
                break;
              }
            }
          }
          if (agree) {
            auto new_remaining_index_flag = s->indices_flag;
            new_remaining_index_flag[counter_vid] = USED;
            auto new_vertices = s->vertices;
            new_vertices[counter_vid] = p;
            auto new_s = std::make_shared<State>(s->depth + 1, counter_vid, p, new_remaining_index_flag, new_vertices);
            new_s->parent = s;
            if (!(use_cache_for_infeasible_placement_set && is_infeasible_placement(new_s))) {
              last_pushed = new_s;
              stack.push(new_s);
            }
          }
        }
      }

      if (last_pushed) last_pushed->is_last = true;

      if (s->parent && s->is_last) {
        // invoke cleanup process of s->parent.
        if (use_cache_for_infeasible_placement_set) {
          //LOG(INFO) << fmt::format("infeasible processing {}(depth={}) ({},{}) hash=0x{:x}", s->parent->vid, s->parent->depth, s->parent->p.first, s->parent->p.second, State_to_zobrist(s->parent));
          set_infeasible_placement(s->parent);
        }
      }
    }
    LOG(INFO) << fmt::format("total nodes = {}, max depth = {}", counter, max_depth);

    return ret;
  }

};
}

REGISTER_SOLVER("NaiveSearchSolver", NNaiveSearchSolver::NaiveSearchSolver);
// vim:ts=2 sw=2 sts=2 et ci


#include "triangle.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <stdexcept>

BOOST_AUTO_TEST_CASE(equilateral_triangles_have_equal_sides)
{
    BOOST_REQUIRE_EQUAL(triangle::equilateral, triangle::kind(2, 2, 2));
}

BOOST_AUTO_TEST_CASE(larger_equilateral_triangles_also_have_equal_sides)
{
    BOOST_REQUIRE_EQUAL(triangle::equilateral, triangle::kind(10, 10, 10));
}

BOOST_AUTO_TEST_CASE(isosceles_triangles_have_last_two_sides_equal)
{
    BOOST_REQUIRE_EQUAL(triangle::isosceles, triangle::kind(3, 4, 4));
}

BOOST_AUTO_TEST_CASE(isosceles_triangles_have_first_and_last_sides_equal)
{
    BOOST_REQUIRE_EQUAL(triangle::isosceles, triangle::kind(4, 3, 4));
}

BOOST_AUTO_TEST_CASE(isosceles_triangles_have_first_two_sides_equal)
{
    BOOST_REQUIRE_EQUAL(triangle::isosceles, triangle::kind(4, 4, 3));
}

BOOST_AUTO_TEST_CASE(isosceles_triangles_have_in_fact_exactly_two_sides_equal)
{
    BOOST_REQUIRE_EQUAL(triangle::isosceles, triangle::kind(10, 10, 2));
}

BOOST_AUTO_TEST_CASE(scalene_triangles_have_no_equal_sides)
{
    BOOST_REQUIRE_EQUAL(triangle::scalene, triangle::kind(3, 4, 5));
}

BOOST_AUTO_TEST_CASE(scalene_triangles_have_no_equal_sides_at_a_larger_scale_too)
{
    BOOST_REQUIRE_EQUAL(triangle::scalene, triangle::kind(10, 11, 12));
}

BOOST_AUTO_TEST_CASE(scalene_triangles_have_no_equal_sides_in_descending_order_either)
{
    BOOST_REQUIRE_EQUAL(triangle::scalene, triangle::kind(5, 4, 2));
}

BOOST_AUTO_TEST_CASE(very_small_triangles_are_legal)
{
    BOOST_REQUIRE_EQUAL(triangle::scalene, triangle::kind(0.4, 0.6, 0.3));
}

BOOST_AUTO_TEST_CASE(triangles_with_no_size_are_illegal)
{
    BOOST_REQUIRE_THROW(triangle::kind(0, 0, 0), std::domain_error);
}

BOOST_AUTO_TEST_CASE(triangles_with_negative_sides_are_illegal)
{
    BOOST_REQUIRE_THROW(triangle::kind(3, 4, -5), std::domain_error);
}

BOOST_AUTO_TEST_CASE(triangles_violating_triangle_inequality_are_illegal)
{
    BOOST_REQUIRE_THROW(triangle::kind(1, 1, 3), std::domain_error);
}

BOOST_AUTO_TEST_CASE(larger_triangles_violating_triangle_inequality_are_illegal)
{
    BOOST_REQUIRE_THROW(triangle::kind(7, 3, 2), std::domain_error);
}
#if defined(EXERCISM_RUN_ALL_TESTS)
#endif


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing drem capabilities

    Computes the drem of division.
    The return value is x-n*y, where n is the value x/y,
    rounded to the nearest integer (using @ref round2even).

    This is a convenient alias of @ref remainder
  **/
  const boost::dispatch::functor<tag::drem_> drem = {};
} }
#endif

#include <boost/simd/function/scalar/remainder.hpp>
#include <boost/simd/function/simd/drem.hpp>

#endif