#include "truncated_svd.hpp"

#include <armadillo>
#include <cmath>
#include <stdexcept>

template <class Real>
void TruncatedSvd(long m, long n, std::vector<Real> &A, Real tolerance,
                  bool is_relative, long &r, std::vector<Real> &U,
                  std::vector<Real> &s, std::vector<Real> &Vt) {
    if (m < 1 || n < 1) {
        throw std::invalid_argument("Matrix dimensions must be positive");
    }

    if (A.size() != m * n) {
        throw std::invalid_argument(
            "Matrix dimensions are incompatible with number of entries");
    }

    const long k = std::min(m, n);

    arma::Mat<Real> A_temp(m, n);
    for (long j = 0; j < n; ++j) {
        for (long i = 0; i < m; ++i) {
            A_temp(i, j) = A[i + j * m];
        }
    }

    arma::Mat<Real> U_thin;
    arma::Col<Real> s_thin;
    arma::Mat<Real> V_thin;

    bool status = arma::svd_econ(U_thin, s_thin, V_thin, A_temp);

    if (!status) {
        throw std::runtime_error("SVD decomposition failed");
    }

    Real frobenius_max_error = static_cast<Real>(0);
    if (is_relative) {
        for (long i = 0; i < k; ++i) {
            frobenius_max_error += std::pow(s_thin(i), 2);
        }

        frobenius_max_error *= std::pow(tolerance, 2);
    } else {
        frobenius_max_error = std::pow(tolerance, 2);
    }

    Real frobenius_error = static_cast<Real>(0);
    r = k;
    while (r > 0) {
        frobenius_error += std::pow(s_thin(r - 1), 2);
        if (frobenius_error > frobenius_max_error) {
            break;
        }
        --r;
    }

    U.resize(m * r);
    for (long j = 0; j < r; ++j) {
        for (long i = 0; i < m; ++i) {
            U[i + j * m] = U_thin(i, j);
        }
    }

    s.resize(r);
    for (long i = 0; i < r; ++i) {
        s[i] = s_thin(i);
    }

    Vt.resize(r * n);
    for (long j = 0; j < n; ++j) {
        for (long i = 0; i < r; ++i) {
            Vt[i + j * r] = V_thin(j, i);
        }
    }
}

template <class Real>
void TruncatedSvd(const arma::Mat<Real> &A, Real tolerance, bool is_relative,
                  arma::Mat<Real> &U, arma::Col<Real> &s, arma::Mat<Real> &V,
                  long &r) {
    arma::Mat<Real> U_thin;
    arma::Col<Real> s_thin;
    arma::Mat<Real> V_thin;
    bool status = arma::svd_econ(U_thin, s_thin, V_thin, A);
    if (!status) {
        throw std::runtime_error("SVD decomposition failed");
    }

    Real frobenius_max_error = static_cast<Real>(0);
    if (is_relative) {
        for (long i = 0; i < s_thin.n_rows; ++i) {
            frobenius_max_error += std::pow(s_thin(i), 2);
        }

        frobenius_max_error *= std::pow(tolerance, 2);
    } else {
        frobenius_max_error = std::pow(tolerance, 2);
    }

    Real frobenius_error = static_cast<Real>(0);
    r = s_thin.n_rows;
    while (r > 0) {
        frobenius_error += std::pow(s_thin(r - 1), 2);
        if (frobenius_error > frobenius_max_error) {
            break;
        }
        --r;
    }

    U.set_size(A.n_rows, r);
    for (long j = 0; j < r; ++j) {
        for (long i = 0; i < A.n_rows; ++i) {
            U(i, j) = U_thin(i, j);
        }
    }

    s.set_size(r);
    for (long i = 0; i < r; ++i) {
        s(i) = s_thin(i);
    }

    V.set_size(A.n_cols, r);
    for (long j = 0; j < r; ++j) {
        for (long i = 0; i < A.n_cols; ++i) {
            V(i, j) = V_thin(i, j);
        }
    }
}

template void TruncatedSvd<float>(long m, long n, std::vector<float> &A,
                                  float tolerance, bool is_relative, long &r,
                                  std::vector<float> &U, std::vector<float> &s,
                                  std::vector<float> &Vt);
template void TruncatedSvd<double>(long m, long n, std::vector<double> &A,
                                   double tolerance, bool is_relative, long &r,
                                   std::vector<double> &U,
                                   std::vector<double> &s,
                                   std::vector<double> &Vt);

template void TruncatedSvd<float>(const arma::Mat<float> &A, float tolerance,
                                  bool is_relative, arma::Mat<float> &U,
                                  arma::Col<float> &s, arma::Mat<float> &V,
                                  long &r);

template void TruncatedSvd<double>(const arma::Mat<double> &A, double tolerance,
                                   bool is_relative, arma::Mat<double> &U,
                                   arma::Col<double> &s, arma::Mat<double> &V,
                                   long &r);


#pragma once

#include <cmath>  // ceil
#include <initializer_list>

#include <Eigen/SparseLU>

#include "InterpolationTemplate.hpp"

namespace intp {

template <typename T, size_t D>
class InterpolationFunction {  // TODO: Add integration
   public:
    using val_type = T;
    using spline_type = BSpline<T, D>;
    using size_type = typename spline_type::size_type;
    using coord_type = typename spline_type::knot_type;

    const size_type order;
    const static size_type dim = D;

   private:
    spline_type __spline;

    template <typename _T>
    using DimArray = std::array<_T, dim>;

    DimArray<coord_type> __dx;
    DimArray<bool> __periodicity;
    DimArray<bool> __uniform;

    friend class InterpolationFunctionTemplate<T, D>;

    // auxiliary methods

    template <size_type... di>
    inline val_type call_op_helper(util::index_sequence<di...>,
                                   DimArray<coord_type> c) const {
        return __spline(std::make_pair(
            c[di], __uniform[di]
                       ? std::min(__spline.knots_num(di) - order - 2,
                                  (size_type)std::ceil(std::max(
                                      0., (c[di] - range(di).first) / __dx[di] -
                                              (__periodicity[di]
                                                   ? 1.
                                                   : .5 * (order + 1)))) +
                                      order)
                       : order)...);
    }

    template <size_type... di>
    inline val_type derivative_helper(util::index_sequence<di...>,
                                      DimArray<coord_type> c,
                                      DimArray<size_type> d) const {
        return __spline.derivative_at(std::make_tuple(
            (coord_type)c[di], (size_type)d[di],
            __uniform[di]
                ? std::min(
                      __spline.knots_num(di) - order - 2,
                      (size_type)std::ceil(std::max(
                          0.,
                          (c[di] - range(di).first) / __dx[di] -
                              (__periodicity[di] ? 1. : .5 * (order + 1)))) +
                          order)
                : order)...);
    }

    // overload for uniform knots
    template <typename _T>
    typename std::enable_if<std::is_arithmetic<_T>::value>::type
    __create_knot_vector(size_type dim_ind,
                         const MeshDimension<dim>& mesh_dimension,
                         DimArray<typename spline_type::KnotContainer>&,
                         std::pair<_T, _T> x_range) {
        __uniform[dim_ind] = true;
        const size_type n = mesh_dimension.dim_size(dim_ind);
        __dx[dim_ind] = (x_range.second - x_range.first) / (n - 1);

        const size_t extra =
            __periodicity[dim_ind] ? 2 * order + (1 - order % 2) : order + 1;

        std::vector<typename spline_type::knot_type> xs(n + extra,
                                                        x_range.first);

        if (__periodicity[dim_ind]) {
            for (size_type i = 0; i < xs.size(); ++i) {
                xs[i] = x_range.first + (i - .5 * extra) * __dx[dim_ind];
            }
        } else {
            for (size_type i = order + 1; i < xs.size() - order - 1; ++i) {
                xs[i] = x_range.first + (i - .5 * extra) * __dx[dim_ind];
            }
            for (size_type i = xs.size() - order - 1; i < xs.size(); ++i) {
                xs[i] = x_range.second;
            }
        }

        __spline.load_knots(dim_ind, std::move(xs), __periodicity[dim_ind]);
    }

    // overload for nonuniform knots, given by iterator pair
    template <typename _T>
    typename std::enable_if<std::is_convertible<
        typename std::iterator_traits<_T>::iterator_category,
        std::input_iterator_tag>::value>::type
    __create_knot_vector(
        size_type dim_ind,
        const MeshDimension<dim>& mesh_dimension,
        DimArray<typename spline_type::KnotContainer>& input_coords,
        std::pair<_T, _T> x_range) {
        __uniform[dim_ind] = false;
        const size_type n = std::distance(x_range.first, x_range.second);
        if (n != mesh_dimension.dim_size(dim_ind)) {
            throw std::range_error(
                std::string("Inconsistency between knot number and "
                            "interpolated value number at dimension ") +
                std::to_string(dim_ind));
        }
        typename spline_type::KnotContainer xs(
            __periodicity[dim_ind] ? n + 2 * order + (1 - order % 2)
                                   : n + order + 1);

        input_coords[dim_ind].reserve(n);
        if (__periodicity[dim_ind]) {
            auto iter = x_range.first;

            input_coords[dim_ind].push_back(*iter);
            for (size_type i = order + 1; i < order + n; ++i) {
                val_type present = *(++iter);
                xs[i] = order % 2 == 0
                            ? .5 * (input_coords[dim_ind].back() + present)
                            : present;
                input_coords[dim_ind].push_back(present);
            }
            val_type period =
                input_coords[dim_ind].back() - input_coords[dim_ind].front();
            for (size_type i = 0; i < order + 1; ++i) {
                xs[i] = xs[n + i - 1] - period;
                xs[xs.size() - i - 1] = xs[xs.size() - i - n] + period;
            }
        } else {
            auto it = x_range.first;
            // Notice that *it++ is not guarantee to work as what you
            // expected for input iterators.
            auto l_knot = *it;
            // fill lestmost *order+1* identical knots
            for (size_type i = 0; i < order + 1; ++i) { xs[i] = l_knot; }
            // first knot is same as first input coordinate
            input_coords[dim_ind].emplace_back(l_knot);
            // Every knot in middle is average of *order* input
            // coordinates. This var is to track the sum of a moving window with
            // width *order*.
            coord_type window_sum{};
            for (size_type i = 1; i < order; ++i) {
                input_coords[dim_ind].emplace_back(*(++it));
                window_sum += input_coords[dim_ind][i];
            }
            for (size_type i = order + 1; i < n; ++i) {
                input_coords[dim_ind].emplace_back(*(++it));
                window_sum += input_coords[dim_ind][i - 1];
                xs[i] = window_sum / order;
                window_sum -= input_coords[dim_ind][i - order];
            }
            auto r_knot = *(++it);
            // fill rightmost *order+1* identical knots
            for (size_type i = n; i < n + order + 1; ++i) { xs[i] = r_knot; }
            // last knot is same as last input coordinate
            input_coords[dim_ind].emplace_back(r_knot);
        }
#ifdef _DEBUG
        std::cout << "[DEBUG] Nonuniform knots along dimension" << dim_ind
                  << ":\n";
        for (auto& c : xs) { std::cout << "[DEBUG] " << c << '\n'; }
        std::cout << std::endl;
#endif

        __spline.load_knots(dim_ind, std::move(xs));
    }

    template <typename... Ts, size_type... di>
    void __create_knots(
        util::index_sequence<di...>,
        MeshDimension<dim> mesh_dimension,
        DimArray<typename spline_type::KnotContainer>& input_coords,
        std::pair<Ts, Ts>... x_ranges) {
#if __cplusplus >= 201703L
        (__create_knot_vector(di, f_mesh, input_coords, x_ranges), ...);
#else
        // polyfill of C++17 fold expression over comma
        std::array<std::nullptr_t, sizeof...(Ts)>{
            (__create_knot_vector(di, mesh_dimension, input_coords, x_ranges),
             nullptr)...};
#endif
    }

    inline void __boundary_check(const DimArray<coord_type>& coord) const {
        for (size_type d = 0; d < dim; ++d) {
            if (!__periodicity[d] &&
                (coord[d] < range(d).first || coord[d] > range(d).second)) {
                throw std::domain_error(
                    "Given coordinate out of interpolation function range!");
            }
        }
    }

   public:
    /**
     * @brief Construct a new 1D Interpolation Function object, mimicking
     * Mathematica's `Interpolation` function, with option `Method->"Spline"`.
     *
     * @tparam InputIter
     * @param order order of interpolation, the interpolated function is of
     * $C^{order-1}$
     * @param periodic whether to construct a periodic spline
     * @param f_range a pair of iterators defining to-be-interpolated data
     * @param x_range a pair of x_min and x_max
     */
    template <
        typename InputIter,
        typename C1,
        typename C2,
        typename std::enable_if<
            dim == 1u &&
            std::is_convertible<
                typename std::iterator_traits<InputIter>::iterator_category,
                std::input_iterator_tag>::value>::type* = nullptr>
    InterpolationFunction(size_type order,
                          bool periodic,
                          std::pair<InputIter, InputIter> f_range,
                          std::pair<C1, C2> x_range)
        : InterpolationFunction(
              order,
              {periodic},
              Mesh<val_type, 1u>{std::make_pair(f_range.first, f_range.second)},
              static_cast<std::pair<typename std::common_type<C1, C2>::type,
                                    typename std::common_type<C1, C2>::type>>(
                  x_range)) {}

    template <
        typename InputIter,
        typename C1,
        typename C2,
        typename std::enable_if<
            dim == 1u &&
            std::is_convertible<
                typename std::iterator_traits<InputIter>::iterator_category,
                std::input_iterator_tag>::value>::type* = nullptr>
    InterpolationFunction(size_type order,
                          std::pair<InputIter, InputIter> f_range,
                          std::pair<C1, C2> x_range)
        : InterpolationFunction(order, false, f_range, x_range) {}

    /**
     * @brief Construct a new nD Interpolation Function object, mimicking
     * Mathematica's `Interpolation` function, with option `Method->"Spline"`.
     * Notice: last value of periodic dimension will be discarded since it is
     * considered same of the first value. Thus inconsistency input data for
     * periodic interpolation will be accepted.
     *
     * @param order order of interpolation, the interpolated function is of
     * $C^{order-1}$
     * @param periodicity an array describing periodicity of each dimension
     * @param f_mesh a mesh containing data to be interpolated
     * @param x_ranges pairs of x_min and x_max or begin and end iterator
     */
    template <typename... Ts>
    InterpolationFunction(size_type order,
                          DimArray<bool> periodicity,
                          const Mesh<val_type, dim>& f_mesh,
                          std::pair<Ts, Ts>... x_ranges)
        : InterpolationFunction(InterpolationFunctionTemplate<val_type, dim>{
              order, periodicity, f_mesh.dimension(), x_ranges...}
                                    .interpolate(f_mesh)) {}

    // Non-periodic for all dimension
    template <typename... Ts>
    InterpolationFunction(size_type order,
                          const Mesh<val_type, dim>& f_mesh,
                          std::pair<Ts, Ts>... x_ranges)
        : InterpolationFunction(order, {}, f_mesh, x_ranges...) {}

    // constructor for partial construction, that is, without interpolated
    // values
    template <typename... Ts>
    InterpolationFunction(
        size_type order,
        DimArray<bool> periodicity,
        DimArray<typename spline_type::KnotContainer>& input_coords,
        MeshDimension<dim> mesh_dimension,
        std::pair<Ts, Ts>... x_ranges)
        : order(order),
          __spline(periodicity, order),
          __periodicity(periodicity) {
        // load knots into spline
        __create_knots(util::make_index_sequence_for<Ts...>{},
                       std::move(mesh_dimension), input_coords, x_ranges...);
    }

    /**
     * @brief Get spline value.
     *
     * @param x coordinates
     */
    template <typename... Coords,
              typename Indices = util::make_index_sequence_for<Coords...>,
              typename = typename std::enable_if<std::is_arithmetic<
                  typename std::common_type<Coords...>::type>::value>::type>
    val_type operator()(Coords... x) const {
        return call_op_helper(Indices{}, DimArray<coord_type>{x...});
    }

    /**
     * @brief Get spline value.
     *
     * @param coord coordinate array
     */
    val_type operator()(DimArray<coord_type> coord) const {
        return call_op_helper(util::make_index_sequence<dim>{}, coord);
    }

    /**
     * @brief Get spline value, but with out of boundary check.
     *
     * @param coord coordinate array
     */
    val_type at(DimArray<coord_type> coord) const {
        __boundary_check(coord);
        return call_op_helper(util::make_index_sequence<dim>{}, coord);
    }

    /**
     * @brief Get spline value, but with out of boundary check.
     *
     * @param x coordinates
     */
    template <typename... Coords,
              typename Indices = util::make_index_sequence_for<Coords...>,
              typename = typename std::enable_if<std::is_arithmetic<
                  typename std::common_type<Coords...>::type>::value>::type>
    val_type at(Coords... x) const {
        return at(DimArray<coord_type>{static_cast<coord_type>(x)...});
    }

    /**
     * @brief Get spline derivative value.
     *
     * @param coord coordinate array
     * @param derivatives derivative order array
     */
    val_type derivative(DimArray<coord_type> coord,
                        DimArray<size_type> derivatives) const {
        return derivative_helper(util::make_index_sequence<dim>{}, coord,
                                 derivatives);
    }

    /**
     * @brief Get spline derivative value.
     *
     * @param coord coordinate array
     * @param deriOrder derivative orders
     */
    template <typename... Args>
    val_type derivative(DimArray<coord_type> coord, Args... deriOrder) const {
        return derivative(coord, DimArray<size_type>{(size_type)deriOrder...});
    }

    /**
     * @brief Get spline derivative value.
     *
     * @param coord_deriOrder_pair pairs of coordinate and derivative order
     */
    template <typename... CoordDeriOrderPair>
    val_type derivative(CoordDeriOrderPair... coord_deriOrder_pair) const {
        return derivative(
            DimArray<coord_type>{coord_deriOrder_pair.first...},
            DimArray<size_type>{(size_type)coord_deriOrder_pair.second...});
    }

    /**
     * @brief Get spline derivative value, but with out of boundary check.
     *
     * @param coord coordinate array
     * @param derivatives derivative order array
     */
    val_type derivative_at(DimArray<coord_type> coord,
                           DimArray<size_type> derivatives) const {
        __boundary_check(coord);
        return derivative_helper(util::make_index_sequence<dim>{}, coord,
                                 derivatives);
    }

    /**
     * @brief Get spline derivative value, but with out of boundary check.
     *
     * @param coord coordinate array
     * @param deriOrder derivative orders
     */
    template <typename... Args>
    val_type derivative_at(DimArray<coord_type> coord,
                           Args... deriOrder) const {
        return derivative_at(coord,
                             DimArray<size_type>{(size_type)deriOrder...});
    }

    /**
     * @brief Get spline derivative value, but with out of boundary check.
     *
     * @param coord_deriOrder_pair pairs of coordinate and derivative order
     */
    template <typename... CoordDeriOrderPair>
    val_type derivative_at(CoordDeriOrderPair... coord_deriOrder_pair) const {
        return derivative_at(
            DimArray<coord_type>{coord_deriOrder_pair.first...},
            DimArray<size_type>{(size_type)coord_deriOrder_pair.second...});
    }

    // properties

    bool periodicity(size_type dim_ind) const { return __periodicity[dim_ind]; }

    bool uniform(size_type dim_ind) const { return __uniform[dim_ind]; }

    const std::pair<typename spline_type::knot_type,
                    typename spline_type::knot_type>&
    range(size_type dim_ind) const {
        return __spline.range(dim_ind);
    }

    /**
     * @brief Get a ref of underlying spline object
     *
     * @return spline_type&
     */
    const spline_type& spline() const { return __spline; }
};

template <typename T>
class InterpolationFunction1D : public InterpolationFunction<T, size_t{1}> {
   private:
    using base = InterpolationFunction<T, size_t{1}>;

   public:
    template <typename InputIter>
    InterpolationFunction1D(std::pair<InputIter, InputIter> f_range,
                            typename base::size_type order = 3,
                            bool periodicity = false)
        : InterpolationFunction1D(
              std::make_pair(typename base::coord_type{},
                             static_cast<typename base::coord_type>(
                                 (f_range.second - f_range.first) - 1)),
              f_range,
              order,
              periodicity){};

    template <typename C1, typename C2, typename InputIter>
    InterpolationFunction1D(std::pair<C1, C2> x_range,
                            std::pair<InputIter, InputIter> f_range,
                            typename base::size_type order = 3,
                            bool periodicity = false)
        : base(order, periodicity, f_range, x_range){};
};

}  // namespace intp


#include <string>
#include <math.h>
#include <iostream>
#include <boost/regex.hpp>
#include <boost/format.hpp>
#include <fstream>
#include <unordered_map>

#define SIMULATED_ROUNDS 1000
#define COLLISION_ROUNDS 40

using namespace std;

class Particle {
private:
    long x, y, z;
    long vx, vy, vz;
    long ax, ay, az;
public:
    bool annihilated;

    Particle(string line) {
        boost::regex re("[p,v,a,=,<,>,\\s]+");
        boost::sregex_token_iterator
            p(line.begin(), line.end(), re, -1);
        boost::sregex_token_iterator end;

        vector<int> values;
        while(p != end) {
            if(p->str().length() == 0) {
                p++;
                continue;
            }

            values.push_back(atoi((p++)->str().c_str()));
        }

        x = values[0];
        y = values[1];
        z = values[2];
        vx = values[3];
        vy = values[4];
        vz = values[5];
        ax = values[6];
        ay = values[7];
        az = values[8];
        annihilated = false;
    }

    void move() {
        vx += ax;
        vy += ay;
        vz += az;
        x += vx;
        y += vy;
        z += vz;
    }

    float distance() {
        float dist = pow(x, 2);
        dist += pow(y, 2);
        dist += pow(z, 2);
        return dist;
    }

    void simulateMovement(float time) {
        x += int(vx*time) + int(0.5*ax*time*time);
        y += int(vy*time) + int(0.5*ay*time*time);
        z += int(vz*time) + int(0.5*az*time*time);
    }

    string location() {
        return boost::str(boost::format("%d,%d,%d") % x % y % z);
    }

    friend ostream& operator<<(ostream &os, const Particle& p);
};

std::ostream& operator<<(std::ostream &strm, const Particle &p) {
    return strm << boost::format("<%d,%d,%d; %d,%d,%d; %d,%d,%d>") % p.x % p.y % p.z % p.vx % p.vy % p.vz % p.ax % p.ay % p.az;
}

int closestParticle(vector<Particle> particles) {
    vector<float> distances;
    for(int i = 0; i < particles.size(); i++) {
        particles[i].simulateMovement(SIMULATED_ROUNDS);
        distances.push_back(particles[i].distance());
    }

    int minParticle = 0;
    for(int i = 0; i < distances.size(); i++) {
        if(distances[i] < distances[minParticle])
            minParticle = i;
    }
    return minParticle;
}

int numUncollidedParticles(vector<Particle> particles) {
    for(int i = 0; i < COLLISION_ROUNDS; i++) {
        unordered_map<string, int> locations;

        for(int j = 0; j < particles.size(); j++) {
            if(particles[j].annihilated)
                continue;

            particles[j].move();

            string loc = particles[j].location();
            if(locations.find(loc) != locations.end()) {
                particles[j].annihilated = true;
                particles[locations[loc]].annihilated = true;
            } else {
                locations[loc] = j;
            }
        }
    }
    int nonAnnihilated = 0;
    for(int i = 0; i < particles.size(); i++) {
        if(!particles[i].annihilated) nonAnnihilated++;
    }
    return nonAnnihilated;
}

int main() {
    vector<Particle> particles;

    ifstream infile("20.txt");
    string line;
    while (getline(infile, line)) {
        particles.push_back(Particle(line));
    }
    cout << "Part 1: " << closestParticle(particles) << "\n";
    cout << "Part 2: " << numUncollidedParticles(particles) << "\n";
}


#include <boost/numeric/odeint.hpp>
#include <vector>

using namespace boost::numeric::odeint;

/* The type of container used to hold the state vector */
typedef std::vector<double> state_type;

const double gam = 0.15;

/* The rhs of x' = f(x) */
void harmonic_oscillator(const state_type &x, state_type &dxdt, const double /* t */)
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam * x[1];
}

// An example of observer to record steps in the integration
struct push_back_state_and_time
{
    std::vector<state_type> &m_states;
    std::vector<double> &m_times;

    push_back_state_and_time(std::vector<state_type> &states, std::vector<double> &times)
        : m_states(states), m_times(times) {}

    void operator()(const state_type &x, double t)
    {
        m_states.push_back(x);
        m_times.push_back(t);
    }
};

int main()
{

    // Containers to store solution
    std::vector<state_type> x_vec;
    std::vector<double> times;

    // Unknown variables with initial conditions
    state_type x(2);
    x[0] = 1.0; // start at x=1.0, p=0.0
    x[1] = 0.0;

    // Integration process. Returns the number of performed steps. This one determines
    // the stepper automatically.
    //    auto steps = integrate(harmonic_oscillator,
    //                           x, 0.0, 10.0, 0.1,
    //                           push_back_state_and_time(x_vec, times));

    // Or you can choose it explicitly
    runge_kutta4<state_type> stepper;
    auto steps = integrate_const(
        stepper,
        harmonic_oscillator,
        x,
        0.0,
        10.0,
        0.1,
        push_back_state_and_time(x_vec, times));

    /* output */
    for (size_t i = 0; i <= steps; i++)
    {
        std::cout << times[i] << '\t' << x_vec[i][0] << '\t' << x_vec[i][1] << '\n';
    }

    std::cout << "\n*******************\n\n";

    // Or, for a finer control, each step can be performed explicitly as below
    const auto dt = 0.1;
    const auto t0 = 0.0;
    const auto tfinal = 10.0;
    state_type x_explicit(2);
    x_explicit[0] = 1.0; // start at x=1.0, p=0.0
    x_explicit[1] = 0.0;
    std::cout << 0 << '\t' << x_explicit[0] << '\t' << x_explicit[1] << '\n';
    for (double t = t0 + dt; t < tfinal; t += dt)
    {
        stepper.do_step(harmonic_oscillator, x_explicit, t, dt);
        std::cout << t << '\t' << x_explicit[0] << '\t' << x_explicit[1] << '\n';
    }

    return 0;
}


#include "Util.h"

#include "navtypes.h"

#include <iostream>
#include <random>
#include <time.h>

#include <Eigen/LU>
#include <sys/time.h>

using namespace navtypes;

namespace util {
bool almostEqual(double a, double b, double threshold) {
	return std::abs(a - b) < threshold;
}

double quatToHeading(double qw, double qx, double qy, double qz) {
	Eigen::Quaterniond quat(qw, qx, qy, qz);
	return quatToHeading(quat);
}

double quatToHeading(Eigen::Quaterniond quat) {
	quat.normalize();
	Eigen::Matrix3d rotMat = quat.toRotationMatrix();
	Eigen::Vector3d transformedX = rotMat * Eigen::Vector3d::UnitX();
	// flatten to xy-plane
	transformedX(2) = 0;
	// recover heading
	double heading = std::atan2(transformedX(1), transformedX(0));
	return heading;
}

ScopedTimer::ScopedTimer(std::string name)
	: startTime(std::chrono::high_resolution_clock::now()), name(std::move(name)) {}

ScopedTimer::ScopedTimer() : ScopedTimer("") {}

ScopedTimer::~ScopedTimer() {
	if (!name.empty()) {
		auto now = std::chrono::high_resolution_clock::now();
		auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(now - startTime);
		std::cout << "[" << name << "] ElapsedTime: " << elapsed.count() << "us\n";
	}
}

std::chrono::microseconds ScopedTimer::elapsedTime() const {
	auto now = std::chrono::high_resolution_clock::now();
	auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(now - startTime);
	return elapsed;
}

points_t transformReadings(const points_t& ps, const transform_t& tf) {
	transform_t tf_inv = tf.inverse();
	points_t readings({});
	for (point_t p : ps) {
		readings.push_back(tf_inv * p);
	}
	return readings;
}

trajectory_t transformTraj(const trajectory_t& traj, const transform_t& tf) {
	trajectory_t tf_traj({});
	for (const transform_t& tf_i : traj) {
		tf_traj.push_back(tf_i * tf);
	}
	return tf_traj;
}

bool collides(const transform_t& tf, const points_t& ps, double radius) {
	for (const point_t& p : ps) {
		if (p(2) == 0.0) {
			// This point is a "no data" point
			continue;
		}
		point_t tf_p = tf * p;
		tf_p(2) = 0;
		if (tf_p.norm() < radius)
			return true;
	}
	return false;
}

transform_t toTransformRotateFirst(double x, double y, double theta) {
	transform_t m;
	m << cos(theta), sin(theta), -x, -sin(theta), cos(theta), -y, 0, 0, 1;
	return m;
}

double closestHeading(double theta, double prev_theta) {
	while (theta < prev_theta - M_PI)
		theta += 2 * M_PI;
	while (theta > prev_theta + M_PI)
		theta -= 2 * M_PI;
	return theta;
}

pose_t toPose(const transform_t& trf, double prev_theta) {
	pose_t s = trf.inverse() * pose_t(0, 0, 1);
	double cos_theta = trf(0, 0);
	double sin_theta = -trf(1, 0);
	double theta = atan2(sin_theta, cos_theta);
	s(2) = closestHeading(theta, prev_theta);
	return s;
}

transform_t toTransform(const pose_t& pose) {
	return toTransformRotateFirst(0, 0, pose(2)) * toTransformRotateFirst(pose(0), pose(1), 0);
}

/**
 * There still might be some variation due to the dependence of the robot position
 * on when exactly each context switch occurs.
 *
 * For programs with more threads, a more sophisticated solution will be necessary.
 */
static std::normal_distribution<double> stdn_dist(0.0, 1.0);
static long seed = std::chrono::system_clock::now().time_since_epoch().count();
// long seed = 1626474823108702150;
static std::default_random_engine main_generator(seed);
static std::default_random_engine spin_generator(seed);

long getNormalSeed() {
	return seed;
}

double stdn(int thread_id) {
	return stdn_dist(thread_id == 0 ? main_generator : spin_generator);
}

} // namespace util


/*
# This file is part of the Astrometry.net suite.
# Licensed under a 3-clause BSD style license - see LICENSE
 */
//#include <iostream>
#include <Eigen/Dense>
//using namespace std;
using namespace Eigen;

extern "C" {

#include "eigen-math.h"
#include "stdio.h"

#if 0
} // fool emacs indenter
#endif

int eigen_solve_least_squares(ematrix_t* A, evector_t** B,
                               evector_t** X, int NB) {
    int i;
    int r,c;

    Map<Matrix<double, Dynamic, Dynamic, RowMajor> >
        mA(A->data, A->rows, A->cols);

    /*
     printf("mA:\n");
     for (r=0; r<mA.rows(); r++) {
     printf("[");
     for (c=0; c<mA.cols(); c++) {
     printf("%s %8.3g", c ? "," : " ", mA(r,c));
     }
     printf("]\n");
     }
     */

    /*
     for (i=0; i<NB; i++) {
     printf("mB(%i):\n", i);
     Map<VectorXd> mB(B[i]->data, B[i]->N, RowMajor);
     printf("[");
     for (c=0; c<mB.size(); c++) {
     printf("%s %8.3g", c ? "," : " ", mB(c));
     }
     printf("]\n");
     }
     */

    JacobiSVD<MatrixXd> svd(mA, ComputeThinU | ComputeThinV);
    for (i=0; i<NB; i++) {
        Map<VectorXd> b(B[i]->data, B[i]->N, RowMajor);
        VectorXd x = svd.solve(b);
        // copy results back to C space...
        for (r=0; r<x.size(); r++)
            evector_set(X[i], r, x[r]);
    }

    return 0;
}

#if 0
{ // fool emacs indenter
#endif
}


#include <iostream>
#include <stdlib.h>
#include <cmath>
#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

#include "iterative.h"
#include "recursive.h"

using namespace std;
using namespace NTL;
using namespace chrono;

//#define ITERATIVE

void usage_example_zp(uint npoints, uint flen){
    long degree = npoints-1;

    ZZ prime;
    GenPrime(prime, flen);
    ZZ_p::init(prime);

//  interpolation points:
    ZZ_p* X = new ZZ_p[degree+1];
    ZZ_p* Y = new ZZ_p[degree+1];
    for(unsigned int i=0;i<=degree; i++) {
        random(X[i]);
        random(Y[i]);
    }

    ZZ_pX P;
#ifdef ITERATIVE
    poly_interpolate_zp_iterative(degree, X, Y, P);
#else
    poly_interpolate_zp_recursive(degree, X, Y, P);
#endif

    // EVALUATE
    ZZ_p* X2 = new ZZ_p[degree+1];
    ZZ_p* Y2 = new ZZ_p[degree+1];
    for(unsigned int i=0;i<=degree; i++) {
        random(X[i]);
    }
#ifdef ITERATIVE
    poly_evaluate_zp_iterative(degree, P, X2, Y2);
#else
    poly_evaluate_zp_recursive(degree, P, X2, Y2);
#endif

    delete[] X;
    delete[] Y;
    delete[] X2;
    delete[] Y2;
}

int main(int argc, char *argv[]) {
   uint num_points = pow(2,13)-1;
   uint field_bitlen = 400;
    if(1<argc)
	num_points = atoi(argv[1]);
    if(2<argc)
    	field_bitlen = atoi(argv[2]);
    cout << "num_points: " << num_points << ", field_bitlen: " << field_bitlen << endl;
    usage_example_zp(num_points, field_bitlen);

}


/**
 * @file 	ex6.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jun 20, 2017
 */

#include <iostream>
#include <fstream>
#include <boost/program_options.hpp>
#include <boost/heap/fibonacci_heap.hpp>
#include <boost/timer/timer.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/spirit/include/qi.hpp>

using namespace std;
using namespace boost;
namespace po = boost::program_options;

typedef adjacency_list<vecS, vecS, undirectedS,
		no_property, property<edge_weight_t, double>> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef pair<int,int> Edge;
typedef pair<int, double> Pair;
typename graph_traits<Graph>::out_edge_iterator out_i, out_end;

/**
 * Data that is stored in one node of a heap. Contains an integer and a double.
 * Comparisons are made by the double, smaller has higher priority
 */
struct heap_data
{
    heap::fibonacci_heap<heap_data>::handle_type handle;
    Pair pair;

    heap_data(Pair p):
        pair(p)
    {}

    bool operator<(heap_data const & rhs) const {
        return pair.second > rhs.pair.second;
    }
};

using Heap = heap::fibonacci_heap<heap_data>;

/**
 * My own implementation of Dijkstra using a fibonacci heap from boost
 * @param g the graph which is an adjacency_list from boost
 * @param numVertices the number of vertices in g
 * @param source the source node for Dijkstra
 * @return vector of resulting distances to source
 */
vector<double> myDijkstra(Graph& g, int numVertices, int source) {

	vector<double> distances(numVertices);
	distances[0] = 0;

	Heap heap;

	Heap::handle_type *handles = new Heap::handle_type[numVertices];
	handles[0] = heap.push(make_pair(0,0.));

	//initialization of the heap
	for (int i = 1; i < numVertices; ++i) {
		handles[i] = heap.push(make_pair(i, numeric_limits<double>::infinity()));
		distances[i] = numeric_limits<double>::infinity();
	}

	property_map<Graph, edge_weight_t>::type weights = get(edge_weight, g);
	property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);

	//the actual algorithm
	while (!heap.empty()) {
		Pair min = heap.top().pair;
		heap.pop();
		for (tie(out_i, out_end) = out_edges(*(vertices(g).first+min.first), g);
				out_i != out_end; ++out_i) {
			double tmp = min.second + weights[*out_i];
			int targetIndex = index[target(*out_i, g)];
			if (tmp < distances[targetIndex]) {
				distances[targetIndex] = tmp;
				(*handles[targetIndex]).pair.second = tmp;
				heap.increase(handles[targetIndex]);
			}
		}
	}

	delete[] handles;
	return distances;
}

/**
 * main function which reads a graph from a .gph file, then calculates shortest
 * paths from all vertices to the first vertex with the dijsktra algorithm
 * and prints the furthest vertex together with its distance
 * to the standard output
 * @param numargs number of inputs on command line
 * @param args array of * inputs on command line
 * @return whether the function operated successfully
 */
int main(int numargs, char *args[]) {

	timer::cpu_timer t;
	bool useOwnMethod;

	/*parsing command line options*/
	po::options_description desc("Allowed options");
	desc.add_options()
			("help,h", "produce help message")
			("m1", "use my own dijkstra method")
			("m2", "use dijkstra method from boost")
			("input-file", po::value< string >(), "input file");
	po::positional_options_description p;
	p.add("input-file", -1);
	po::variables_map vm;
	po::store(po::command_line_parser(numargs, args).
			options(desc).positional(p).run(), vm);
	po::notify(vm);

	if (vm.count("help")) {
	    cout << desc << "\n";
	   	exit(EXIT_SUCCESS);
	}

	if (vm.count("m1") && !vm.count("m2")) {
		useOwnMethod = true;
		cout << "using my own method for calculation..." << endl << endl;
	} else if (vm.count("m2") && !vm.count("m1")) {
		useOwnMethod = false;
		cout << "using boost method for calculation..." << endl << endl;
	} else {
		cerr << "please specify the method you want to use (type -h for help)" << endl;
		exit(EXIT_FAILURE);
	}

	string input;
	if (vm.count("input-file")) {
	    input = vm["input-file"].as< string >();
	} else {
		cerr << "please specify an input file in the .gph format" << endl;
		exit(EXIT_FAILURE);
	}
	/*end of parsing command line options*/

	ifstream inputFile;
	inputFile.open(input);							//trying to read file
	if (inputFile.fail()) {
		cerr << "file could not be read" << endl;
		exit(EXIT_FAILURE);
	}

	int numVertices;
	int numEdges;

	string line;
	getline(inputFile, line);					//first line is read
	vector<string> parts;
	split(parts, line, is_any_of(" "));

	if (parts.size() != 2) {
		cerr << "error in file: first line should consist of two integers!" << endl;
		exit(EXIT_FAILURE);
	}

	try {
		numVertices = stoi(parts[0]);		//information from the first line
		numEdges = stoi(parts[1]);			//are stored
	} catch (...) {
		cerr << "error in file: first line should consist of two integers!" << endl;
		exit(EXIT_FAILURE);
	}

	Edge *edges =  new Edge[numEdges];			//in these arrays all information about
	double *weights = new double[numEdges];		//the edges are stored

	int i = 0;

	using namespace boost::spirit;
	using qi::int_;
	using qi::double_;
	using qi::phrase_parse;
	using ascii::space;

	//read line by line using boost to parse each line to int,int,double
	while (getline(inputFile, line)) {
		try {
			auto it = line.begin();
			int start;
			int end;
			double weight;
			bool success = phrase_parse(it, line.end(),
					int_[([&start](int j){ start = j; })]
					>> int_[([&end](int j){ end = j; })]
					>> double_[([&weight](double j){ weight = j; })], space);
			if (success && it == line.end()) {
				edges[i] = Edge(start-1, end-1);
				weights[i] = weight;
			} else {
				cerr << "error in line " << (i+2) << ": line should consists of "
						"two integers and a double!" << endl;
				exit(EXIT_FAILURE);
			}
		} catch(...) {
			cerr << "error in line " << (i+2) << ": line should consists of "
					"two integers and a double!" << endl;
			exit(EXIT_FAILURE);
		}
		++i;
	}

	//undirected graph is constructed with all edges and their weights
	Graph g(edges, edges + numEdges , weights, numVertices);

	//in this vector the resulting distances from dijkstra are stored
	vector<double> distances;

	//call of dijsktra depending on chosen option
	if (!useOwnMethod) {
		distances.resize(numVertices);
		dijkstra_shortest_paths(g, *(vertices(g).first), distance_map(&distances[0]));
	} else {
		distances = myDijkstra(g, numVertices, 0);
	}

	double maxDist = 0;
	int maxIdx = 0;

	//search for furthest vertex
	for (int i = 1; i < numVertices; i++) {
		double tmp = distances[i];
			if (tmp > maxDist) {
				maxDist = tmp;
				maxIdx = i;
			}
	}

	//results are printed to command line
	cout << "RESULT VERTEX " << (maxIdx+1) << endl;
	cout << "RESULT DIST " << maxDist << endl;
	cout << endl << "running time: " << t.format() <<  endl;

	delete[] edges;
	delete[] weights;

	exit(EXIT_SUCCESS);
}


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example man_power.cpp \file man_power.cpp
    \brief Using set style operators to compute with interval sets and maps.

    Interval sets and maps can be filled and manipulated using
    set style operation like union (+=), difference (-=) and intersection
    (&=).

    In this example 'man_power' a number of those operations are
    demonstrated in the process of calculation the available working
    times (man-power) of a company's employees accounting for weekends,
    holidays, sickness times and vacations.

    \include man_power_/man_power.cpp
*/
//[example_man_power
// The next line includes <boost/gregorian/date.hpp>
// and a few lines of adapter code.
#include <boost/icl/gregorian.hpp>
#include <iostream>
#include <boost/icl/discrete_interval.hpp>
#include <boost/icl/interval_map.hpp>

using namespace std;
using namespace boost::gregorian;
using namespace boost::icl;

// Function weekends returns the interval_set of weekends that are contained in
// the date interval 'scope'
interval_set<date> weekends(const discrete_interval<date>& scope)
{
    interval_set<date> weekends;

    date cur_weekend_sat
        = first(scope)
          + days(days_until_weekday(first(scope), greg_weekday(Saturday)))
          - weeks(1);
    week_iterator week_iter(cur_weekend_sat);

    for(; week_iter <= last(scope); ++week_iter)
        weekends += discrete_interval<date>::right_open(*week_iter, *week_iter + days(2));

    weekends &= scope; // cut off the surplus

    return weekends;
}

// The available working time for the employees of a company is calculated
// for a period of 3 months accounting for weekends and holidays.
//    The available daily working time for the employees is calculated
// using interval_sets and interval_maps demonstrating a number of
// addition, subtraction and intersection operations.
void man_power()
{
    date someday = from_string("2008-08-01");
    date thenday = someday + months(3);

    discrete_interval<date> scope = discrete_interval<date>::right_open(someday, thenday);

    // ------------------------------------------------------------------------
    // (1) In a first step, the regular working times are computed for the
    // company within the given scope. From all available days, the weekends
    // and holidays have to be subtracted:
    interval_set<date> worktime(scope);
    // Subtract the weekends
    worktime -= weekends(scope);
    // Subtract holidays
    worktime -= from_string("2008-10-03"); //German reunification ;)

    // company holidays (fictitious ;)
    worktime -= discrete_interval<date>::closed(from_string("2008-08-18"),
                                                from_string("2008-08-22"));

    //-------------------------------------------------------------------------
    // (2) Now we calculate the individual work times for some employees
    //-------------------------------------------------------------------------
    // In the company works Claudia.
    // This is the map of her regular working times:
    interval_map<date,int> claudias_working_hours;

    // Claudia is working 8 hours a day. So the next statement says
    // that every day in the whole scope is mapped to 8 hours worktime.
    claudias_working_hours += make_pair(scope, 8);

    // But Claudia only works 8 hours on regular working days so we do
    // an intersection of the interval_map with the interval_set worktime:
    claudias_working_hours &= worktime;

    // Yet, in addition Claudia has her own absence times like
    discrete_interval<date> claudias_seminar (from_string("2008-09-16"),
                                              from_string("2008-09-24"),
                                              interval_bounds::closed());
    discrete_interval<date> claudias_vacation(from_string("2008-08-01"),
                                              from_string("2008-08-14"),
                                              interval_bounds::closed());

    interval_set<date> claudias_absence_times(claudias_seminar);
    claudias_absence_times += claudias_vacation;

    // All the absence times have to subtracted from the map of her working times
    claudias_working_hours -= claudias_absence_times;

    //-------------------------------------------------------------------------
    // Claudia's boss is Bodo. He only works part time.
    // This is the map of his regular working times:
    interval_map<date,int> bodos_working_hours;

    // Bodo is working 4 hours a day.
    bodos_working_hours += make_pair(scope, 4);

    // Bodo works only on regular working days
    bodos_working_hours &= worktime;

    // Bodos additional absence times
    discrete_interval<date>      bodos_flu(from_string("2008-09-19"), from_string("2008-09-29"),
                                           interval_bounds::closed());
    discrete_interval<date> bodos_vacation(from_string("2008-08-15"), from_string("2008-09-03"),
                                           interval_bounds::closed());

    interval_set<date> bodos_absence_times(bodos_flu);
    bodos_absence_times += bodos_vacation;

    // All the absence times have to be subtracted from the map of his working times
    bodos_working_hours -= bodos_absence_times;

    //-------------------------------------------------------------------------
    // (3) Finally we want to calculate the available manpower of the company
    // for the selected time scope: This is done by adding up the employees
    // working time maps:
    interval_map<date,int> manpower;
    manpower += claudias_working_hours;
    manpower += bodos_working_hours;

    cout << first(scope) << " - " << last(scope)
         << "    available man-power:" << endl;
    cout << "---------------------------------------------------------------\n";

    for(interval_map<date,int>::iterator it = manpower.begin();
        it != manpower.end(); it++)
    {
        cout << first(it->first) << " - " << last(it->first)
             << " -> " << it->second << endl;
    }
}

int main()
{
    cout << ">>Interval Container Library: Sample man_power.cpp <<\n";
    cout << "---------------------------------------------------------------\n";
    man_power();
    return 0;
}

// Program output:
/*
>>Interval Container Library: Sample man_power.cpp <<
---------------------------------------------------------------
2008-Aug-01 - 2008-Oct-31    available man-power:
---------------------------------------------------------------
2008-Aug-01 - 2008-Aug-01 -> 4
2008-Aug-04 - 2008-Aug-08 -> 4
2008-Aug-11 - 2008-Aug-14 -> 4
2008-Aug-15 - 2008-Aug-15 -> 8
2008-Aug-25 - 2008-Aug-29 -> 8
2008-Sep-01 - 2008-Sep-03 -> 8
2008-Sep-04 - 2008-Sep-05 -> 12
2008-Sep-08 - 2008-Sep-12 -> 12
2008-Sep-15 - 2008-Sep-15 -> 12
2008-Sep-16 - 2008-Sep-18 -> 4
2008-Sep-25 - 2008-Sep-26 -> 8
2008-Sep-29 - 2008-Sep-29 -> 8
2008-Sep-30 - 2008-Oct-02 -> 12
2008-Oct-06 - 2008-Oct-10 -> 12
2008-Oct-13 - 2008-Oct-17 -> 12
2008-Oct-20 - 2008-Oct-24 -> 12
2008-Oct-27 - 2008-Oct-31 -> 12
*/
//]


#include "math/itf/vector_3d.h"

#include <cmath>
#include <boost/assert.hpp>

using namespace lb;

/// Adds the vector \a v to this vector.
Vector3d &Vector3d::operator+=(const Vector3d &v)
{
    x += v.x;
    y += v.y;
    z += v.z;

    return *this;
}

/// Returns whether this vector equals the given vector \a v.
bool Vector3d::operator==(const Vector3d &v) const
{
    return x == v.x && y == v.y && z == v.z;
}

/// Scales this vector by a factor \a f and returns a reference to itself.
Vector3d &Vector3d::operator*=(double f)
{
    x *= f;
    y *= f;
    z *= f;
    return *this;
}

/// Divides this vector by a factor \a f and returns the result vector.
Vector3d Vector3d::operator/(double f) const
{
    BOOST_ASSERT(f != 0);
    double inv = 1.f / f;
    return {x * inv, y * inv, z * inv};
}

/// Divides this vector by a factor \a f and returns a reference to itself.
Vector3d &Vector3d::operator/=(double f)
{
    BOOST_ASSERT(f != 0);
    double inv = 1.f / f;
    x *= inv;
    y *= inv;
    z *= inv;
    return *this;
}

/// Calculates the length of this vector and returns it.
double Vector3d::length() const
{
    return std::sqrt(lengthSquared());
}

/// Calculates the square of the length of this vector and returns it.
double Vector3d::lengthSquared() const
{
    return x * x + y * y + z * z;
}

/// Normalizes this vector to a vector with the same direction and magnitude
/// one.
void Vector3d::normalize()
{
    double oneOverLength = 1.0 / length();
    x *= oneOverLength;
    y *= oneOverLength;
    z *= oneOverLength;
}

/// Calculates a vector that points in the same direction as this vector with
/// length 1 and returns it.
Vector3d Vector3d::normalized() const
{
    return *this / length();
}

/// Streams a textual representation of the vector \a v to \a out.
std::ostream &lb::operator<<(std::ostream &out, const Vector3d &v)
{
    out << "(" << v.x << ", " << v.y << ", " << v.z << ")";
}

/// Returns the absolute value of the dot product of the two vectors \a v and \a
/// w.
double absDot(const Vector3d &v, const Vector3d &w)
{
    return std::fabs(dot(v, w));
}


#ifndef SM_NUMERICAL_DIFF_HPP
#define SM_NUMERICAL_DIFF_HPP

#include <Eigen/Core>
#include <sm/assert_macros.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>

namespace sm { namespace eigen {

    template<typename RESULT_VEC_T, typename INPUT_VEC_T, typename JACOBIAN_T = Eigen::MatrixXd>
    struct NumericalDiffFunctor
    {
      typedef RESULT_VEC_T value_t;
      typedef typename value_t::Scalar scalar_t;
      typedef INPUT_VEC_T input_t;
      typedef JACOBIAN_T jacobian_t;

      NumericalDiffFunctor( boost::function< value_t(input_t) > f) : _f(f){}

      value_t operator()(const input_t & x) { return _f(x); }

      input_t update(const input_t & x, int c, scalar_t delta) { input_t xnew = x; xnew[c] += delta; return xnew; }
      boost::function<value_t(input_t)> _f;
    };

    // A simple implementation of central differences to estimate a Jacobian matrix
    template<typename FUNCTOR_T>
    struct NumericalDiff
    {
      typedef FUNCTOR_T functor_t;
      typedef typename functor_t::input_t input_t;
      typedef typename functor_t::value_t value_t;
      typedef typename functor_t::scalar_t scalar_t;
      typedef typename functor_t::jacobian_t jacobian_t;

      NumericalDiff(functor_t f, scalar_t eps = sqrt(std::numeric_limits<scalar_t>::epsilon())) : functor(f), eps(eps) {}

      jacobian_t estimateJacobian(input_t const & x0)
      {
        // evaluate the function at the operating point:
        value_t fx0 = functor(x0);
        size_t N = x0.size();
        size_t M = fx0.size();

        //std::cout << "Size: " << M << ", " << N << std::endl;
        jacobian_t J;
        J.resize(M, N);

        SM_ASSERT_EQ(std::runtime_error,x0.size(),J.cols(),"Unexpected number of columns for input size");
        SM_ASSERT_EQ(std::runtime_error,fx0.size(),J.rows(),"Unexpected number of columns for output size");

        for(unsigned c = 0; c < N; c++) {
          // Calculate a central difference.
          // This step size was stolen from cminpack: temp = eps * fabs(x[j]);
          scalar_t rcEps = std::max(static_cast<scalar_t>(fabs(x0(c))) * eps,eps);

          value_t fxp = functor(functor.update(x0,c,rcEps));
          value_t fxm = functor(functor.update(x0,c,-rcEps));
          J.block(0, c, M, 1) = (fxp - fxm).template cast<typename jacobian_t::Scalar>()/(typename jacobian_t::Scalar)(rcEps*(scalar_t)2.0);
        }
        return J;
      }

      functor_t functor;
      scalar_t eps;
    };

    template < typename ValueType_, typename InputType_>
    Eigen::MatrixXd numericalDiff(std::function<ValueType_ (const InputType_ &) > function, InputType_ const & input, double eps = sqrt(std::numeric_limits<typename NumericalDiffFunctor<ValueType_, InputType_>::scalar_t>::epsilon())){
      typedef NumericalDiffFunctor<ValueType_, InputType_> Functor;

      NumericalDiff<Functor> numDiff(Functor(function), eps);
      return numDiff.estimateJacobian(input);
    }

  }}

#endif /* SM_NUMERICAL_DIFF_HPP */


//
// Project: Delaunay
// File: main.cpp
//
// Copyright (c) 2021 Miika 'Lehdari' Lehtimäki
// You may use, distribute and modify this code under the terms
// of the licence specified in file LICENSE which is distributed
// with this source code package.
//

#include <random>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <Eigen/Dense>

// 3x3 determinant
template <typename T_Scalar>
T_Scalar inline __attribute__((always_inline)) determinant(
    const T_Scalar& a, const T_Scalar& b, const T_Scalar& c,
    const T_Scalar& d, const T_Scalar& e, const T_Scalar& f,
    const T_Scalar& g, const T_Scalar& h, const T_Scalar& i)
{
    return a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h;
}

#ifndef DELAUNAY_BACKEND
// example implementation of a custom backend

// minimal 2D vector struct
template <typename T_Scalar>
struct Vec2 {
    T_Scalar  data[2];

    // access with functor operator required only for demo dataset construction
    // so it is identical with eigen
    T_Scalar& operator()(int d) { return data[d]; }
    const T_Scalar& operator()(int d) const { return data[d]; }

    Vec2(T_Scalar x, T_Scalar y) :
        data    {x,y}
    {}
};

// macros required for backend specification
#define DELAUNAY_VEC Vec2<T_Scalar>
#define DELAUNAY_VEC_ACCESS(V,D) V.data[D]
#define DELAUNAY_DETERMINANT determinant

#endif // ifndef DELAUNAY_BACKEND

#include "Delaunay.hpp"

#define RND ((rnd()%10000001)*0.0000001)

#if DELAUNAY_BACKEND == DELAUNAY_BACKEND_EIGEN

using Vec2d = Eigen::Matrix<double, 2, 1>;
using Vec2f = Eigen::Matrix<float, 2, 1>;
template <typename T>
using Vector = std::vector<T, Eigen::aligned_allocator<T>>;

#else // custom backend

using Vec2f = Vec2<float>;
using Vec2d = Vec2<double>;
template <typename T>
using Vector = std::vector<T>;

#endif

// generate some non-uniform data (not very fancy but does the job)
void createPoints(Vector<Vec2d>& v, const Vec2d& min, const Vec2d& max, int depth = 0)
{
    static std::default_random_engine rnd(7155178);

    double terminationProbability = 0.02*depth*depth;
    if (RND < terminationProbability) {
        v.emplace_back(min(0) + RND*(max(0)-min(0)), min(1) + RND*(max(1)-min(1)));
        v.emplace_back(min(0) + RND*(max(0)-min(0)), min(1) + RND*(max(1)-min(1)));
    }
    else {
        Vec2d half(min(0)+(0.3+0.4*RND)*(max(0)-min(0)), min(1)+(0.3+0.4*RND)*(max(1)-min(1)));
        createPoints(v, min, half, depth+1);
        createPoints(v, Vec2d(half(0), min(1)), Vec2d(max(0), half(1)), depth+1);
        createPoints(v, Vec2d(min(0), half(1)), Vec2d(half(0), max(1)), depth+1);
        createPoints(v, half, max, depth+1);
    }
}

template <typename T>
void createPointsUniform(Vector<T>& v, const T& min, const T& max, int n)
{
    std::default_random_engine rnd(1507715517);
    v.reserve(n);

    for (int i=0; i<n; ++i) {
        v.emplace_back(min(0) + RND*(max(0)-min(0)), min(1) + RND*(max(1)-min(1)));
    }
}

template <typename T>
void visualize(const Vector<T>& points, std::vector<int32_t>& triangulation,
    const std::string& windowName, const cv::Scalar& lineColor, int wait=0)
{
    cv::Mat img(1024, 1024, CV_8UC3, cv::Scalar(0, 0, 0));

    for (int i=0; i<triangulation.size(); i+=3) {
        cv::line(img,
            cv::Point(points[triangulation[i]](0), 1024-(int)points[triangulation[i]](1)),
            cv::Point(points[triangulation[i+1]](0), 1024-(int)points[triangulation[i+1]](1)),
            lineColor);
        if (triangulation[i+2] != -1) {
            cv::line(img,
                cv::Point(points[triangulation[i+1]](0), 1024-(int)points[triangulation[i+1]](1)),
                cv::Point(points[triangulation[i+2]](0), 1024-(int)points[triangulation[i+2]](1)),
                lineColor);
            cv::line(img,
                cv::Point(points[triangulation[i+2]](0), 1024-(int)points[triangulation[i+2]](1)),
                cv::Point(points[triangulation[i]](0), 1024-(int)points[triangulation[i]](1)),
                lineColor);
        }
    }

    for (auto& p : points) {
        img.at<cv::Vec3b>(1023-(int)p(1), (int)p(0)) = cv::Vec3b(255, 255, 255);
    }

    cv::imshow(windowName, img);
    cv::waitKey(wait);
}

template <typename T>
inline __attribute__((always_inline)) bool inCircle2(
    const T& a, const T& b, const T& c, const T& d)
{
    T dd(d(0)*d(0), d(1)*d(1));
    return determinant<double>(
        a(0)-d(0), a(1)-d(1), (a(0)*a(0)-dd(0))+(a(1)*a(1)-dd(1)),
        b(0)-d(0), b(1)-d(1), (b(0)*b(0)-dd(0))+(b(1)*b(1)-dd(1)),
        c(0)-d(0), c(1)-d(1), (c(0)*c(0)-dd(0))+(c(1)*c(1)-dd(1))) > 0.0;
}

template <typename T>
bool checkTriangulation(const Vector<T>& points, std::vector<int32_t>& triangulation)
{
    for (size_t i=0; i<triangulation.size(); i+=3) {
        int p1 = triangulation[i];
        int p2 = triangulation[i+1];
        int p3 = triangulation[i+2];
        if (p3 == -1)
            continue;

        for (size_t j=0; j<points.size(); ++j) {
            if (j == p1 || j == p2 || j == p3)
                continue;
            if (inCircle2(points[p1], points[p2], points[p3], points[j]))
                return false;
        }
    }

    return true;
}

void benchmark(void) {
    int nPoints = 10;
    bool breakLoop = false;
    while (true) {
        Vector<Vec2f> pointsFloat;
        Vector<Vec2d> pointsDouble;
        pointsFloat.clear();
        pointsDouble.clear();
        createPointsUniform(pointsFloat, Vec2f(0.0, 0.0), Vec2f(1024.0, 1024.0), nPoints);
        createPointsUniform(pointsDouble, Vec2d(0.0, 0.0), Vec2d(1024.0, 1024.0), nPoints);

        auto t1 = std::chrono::high_resolution_clock::now();
        auto triangulationFloat = delaunay::triangulate(pointsFloat);
        auto t2 = std::chrono::high_resolution_clock::now();
        auto triangulationDouble = delaunay::triangulate(pointsDouble);
        auto t3 = std::chrono::high_resolution_clock::now();

        printf("nPoints: %d, tFloat: %0.5f, tDouble: %0.5f\n", nPoints,
            std::chrono::duration<double, std::milli>(t2-t1).count(),
            std::chrono::duration<double, std::milli>(t3-t2).count());

        visualize(pointsDouble, triangulationDouble, "triangulationDouble", cv::Scalar(120, 120, 0), 20);
        visualize(pointsFloat, triangulationFloat, "triangulationFloat", cv::Scalar(0, 80, 160), 20);

        if (!checkTriangulation(pointsFloat, triangulationFloat)) {
            printf("Triangulation is non-delaunay!\n");
            breakLoop = true;
        }

        if (breakLoop) {
            cv::waitKey(0);
            break;
        }

        nPoints *= 1.1;
    }
}

int main()
{
#if 0
    Vector<Vec2d> points;
    createPoints(points, Vec2d(0.0f, 0.0f), Vec2d(1024.0f, 1024.0f));

    auto triangulation = delaunay::triangulate(points);

    visualize(points, triangulation, "delaunay demo", cv::Scalar(120, 120, 0));
#else
    benchmark();
#endif

    return 0;
}


///3
#include <iostream>
#include <algorithm>
#include <vector>
#include <queue>
#include <stack>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
// BGL graph definitions
// =====================
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
  boost::no_property, boost::property<boost::edge_weight_t, long> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;
typedef  boost::graph_traits<weighted_graph>::out_edge_iterator      out_edge_it;

// Main
void testcase() {
  // build graph
  long n, m, x, k;
  std::cin >> n >> m >> x >> k;
  weighted_graph G(n);
  weight_map weights = boost::get(boost::edge_weight, G);

  edge_desc e;

  std::vector<int> num_outgoing(n, 0);

  for(int i = 0; i < m; i++) {
    long u, v, p;
    std::cin >> u >> v>> p;
    e = boost::add_edge(u, v, G).first; weights[e]=p;
    num_outgoing[u] += 1;
  }

  std::vector<std::vector<long>> maxpoints_after(k + 1);
  for(int i = 0; i <= k; i++) {
    maxpoints_after[i] = std::vector<long>(n, 0);
  }

  out_edge_it ebeg, eend;
  for(int j = 0; j < k; j++) {
    for(int i = 0; i < n; i++) {
      long maxpoints = 0;
      for (boost::tie(ebeg, eend) = boost::out_edges(i, G); ebeg != eend; ++ebeg) {
        const int v = boost::target(*ebeg, G);
        const long pi = weights[*ebeg];
        maxpoints = std::max(maxpoints, maxpoints_after[j][v] + pi);
        if(i == 0 && maxpoints >= x) {
          std::cout << j + 1 << std::endl;
          return;
        }
      }
      if(num_outgoing[i] == 0) {
        maxpoints_after[j + 1][i] = maxpoints_after[j + 1][0];
      } else {
        maxpoints_after[j + 1][i] = maxpoints;
      }
    }
  }
  std::cout << "Impossible" << std::endl;
}

int main() {
  std::ios_base::sync_with_stdio(false);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) testcase();
  return 0;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_eg_10

/*`

To understand how the rounding policies for
the discrete distributions can be used, we'll
use the 50-sample binomial distribution with a
success fraction of 0.5 once again, and calculate
all the possible quantiles at 0.05 and 0.95.

Begin by including the needed headers (and some using statements for conciseness):

*/
#include <iostream>
using std::cout; using std::endl;
using std::left; using std::fixed; using std::right; using std::scientific;
#include <iomanip>
using std::setw;
using std::setprecision;

#include <boost/math/distributions/binomial.hpp>
/*`

Next we'll bring the needed declarations into scope, and
define distribution types for all the available rounding policies:

*/
// Avoid
// using namespace std; // and
// using namespace boost::math;
// to avoid potential ambiguity of names, like binomial.
// using namespace boost::math::policies; is small risk, but
// the necessary items are brought into scope thus:

using boost::math::binomial_distribution;
using boost::math::policies::policy;
using boost::math::policies::discrete_quantile;

using boost::math::policies::integer_round_outwards;
using boost::math::policies::integer_round_down;
using boost::math::policies::integer_round_up;
using boost::math::policies::integer_round_nearest;
using boost::math::policies::integer_round_inwards;
using boost::math::policies::real;

using boost::math::binomial_distribution; // Not std::binomial_distribution.

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_outwards> > >
        binom_round_outwards;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_inwards> > >
        binom_round_inwards;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_down> > >
        binom_round_down;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_up> > >
        binom_round_up;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_nearest> > >
        binom_round_nearest;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<real> > >
        binom_real_quantile;

/*`
Now let's set to work calling those quantiles:
*/

int main()
{
   cout <<
      "Testing rounding policies for a 50 sample binomial distribution,\n"
      "with a success fraction of 0.5.\n\n"
      "Lower quantiles are calculated at p = 0.05\n\n"
      "Upper quantiles at p = 0.95.\n\n";

   cout << setw(25) << right
      << "Policy"<< setw(18) << right
      << "Lower Quantile" << setw(18) << right
      << "Upper Quantile" << endl;

   // Test integer_round_outwards:
   cout << setw(25) << right
      << "integer_round_outwards"
      << setw(18) << right
      << quantile(binom_round_outwards(50, 0.5), 0.05)
      << setw(18) << right
      << quantile(binom_round_outwards(50, 0.5), 0.95)
      << endl;

   // Test integer_round_inwards:
   cout << setw(25) << right
      << "integer_round_inwards"
      << setw(18) << right
      << quantile(binom_round_inwards(50, 0.5), 0.05)
      << setw(18) << right
      << quantile(binom_round_inwards(50, 0.5), 0.95)
      << endl;

   // Test integer_round_down:
   cout << setw(25) << right
      << "integer_round_down"
      << setw(18) << right
      << quantile(binom_round_down(50, 0.5), 0.05)
      << setw(18) << right
      << quantile(binom_round_down(50, 0.5), 0.95)
      << endl;

   // Test integer_round_up:
   cout << setw(25) << right
      << "integer_round_up"
      << setw(18) << right
      << quantile(binom_round_up(50, 0.5), 0.05)
      << setw(18) << right
      << quantile(binom_round_up(50, 0.5), 0.95)
      << endl;

   // Test integer_round_nearest:
   cout << setw(25) << right
      << "integer_round_nearest"
      << setw(18) << right
      << quantile(binom_round_nearest(50, 0.5), 0.05)
      << setw(18) << right
      << quantile(binom_round_nearest(50, 0.5), 0.95)
      << endl;

   // Test real:
   cout << setw(25) << right
      << "real"
      << setw(18) << right
      << quantile(binom_real_quantile(50, 0.5), 0.05)
      << setw(18) << right
      << quantile(binom_real_quantile(50, 0.5), 0.95)
      << endl;
} // int main()

/*`

Which produces the program output:

[pre
  policy_eg_10.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\policy_eg_10.exe
  Testing rounding policies for a 50 sample binomial distribution,
  with a success fraction of 0.5.

  Lower quantiles are calculated at p = 0.05

  Upper quantiles at p = 0.95.

                     Policy    Lower Quantile    Upper Quantile
     integer_round_outwards                18                31
      integer_round_inwards                19                30
         integer_round_down                18                30
           integer_round_up                19                31
      integer_round_nearest                19                30
                       real            18.701            30.299
]

*/

//] //[policy_eg_10] ends quickbook import.


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>

#include <CGAL/Labeled_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>

#include <boost/version.hpp>

// Domain
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::FT FT;
typedef K::Point_3 Point;
typedef FT (Function)(const Point&);
typedef CGAL::Labeled_mesh_domain_3<K> Mesh_domain;

typedef CGAL::Parallel_if_available_tag Concurrency_tag;

// Triangulation
typedef CGAL::Mesh_triangulation_3<Mesh_domain,K,Concurrency_tag>::type Tr;

typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr> C3t3;

// Criteria
typedef CGAL::Mesh_criteria_3<Tr> Mesh_criteria;

// To avoid verbose function and named parameters call
using namespace CGAL::parameters;

// Function, a capsule:
//   a cylinder centered at x=y=0, with z in [-5, -5]
//   with radius 1 and "round ends".
FT capsule_function(const Point& p)
{
  const FT base = CGAL::square(p.x())+CGAL::square(p.y()) - 1;
  const FT z = p.z();
  if(z > FT(5)) return base+CGAL::square(z-5);
  else if(z < FT(-5)) return base+CGAL::square(z+5);
  else return base;
}
#if BOOST_VERSION >= 106600
auto field = [](const Point& p, const int, const Mesh_domain::Index)
             {
               if(p.z() > 2) return 0.025;
               if(p.z() < -3) return 0.01;
               else return 1.;
             };
#else
struct Field {
  typedef ::FT FT;

  FT operator()(const Point& p, const int, const Mesh_domain::Index) const {
    if(p.z() > 2) return 0.025;
    if(p.z() < -3) return 0.01;
    else return 1;
  }
} field;
#endif

int main()
{
  Mesh_domain domain =
    Mesh_domain::create_implicit_mesh_domain(capsule_function,
                                             K::Sphere_3(CGAL::ORIGIN, 49.));

  // Mesh criteria
  Mesh_criteria criteria(facet_angle=30, facet_size=0.5,
                         facet_distance=field);

  // Mesh generation
  C3t3 c3t3 = CGAL::make_mesh_3<C3t3>(domain, criteria);

  // Output
  std::ofstream medit_file("out.mesh");
  c3t3.output_to_medit(medit_file);

  return 0;
}


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
 *          David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

#include "triangulation.h"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <glog/logging.h>

#include "../util/types.h"

namespace bsfm {

using Eigen::MatrixXd;

// Triangulates a single 3D point from > 2 views using the inhomogeneous DLT
// method from H&Z: Multi-View Geometry, Ch 2.2.
bool Triangulate(const FeatureList& features,
                 const std::vector<Camera>& cameras,
                 Point3D& point,
                 double& uncertainty) {
  if (features.size() != cameras.size()) {
    LOG(WARNING)
        << "Number of features does not match number of cameras.";
    return false;
  }

  if (features.size() < 2) {
    LOG(WARNING) << "Need at least two features and cameras to triangulate.";
    return false;
  }

  // Construct the A matrix on page 312.
  MatrixXd A;
  A.resize(features.size() * 2, 4);
  for (size_t ii = 0; ii < features.size(); ++ii) {
    double u = features[ii].u_;
    double v = features[ii].v_;

    const Matrix34d P = cameras[ii].P();
    A.row(2*ii+0) = u * P.row(2) - P.row(0);
    A.row(2*ii+1) = v * P.row(2) - P.row(1);
  }

  // Get svd(A). Save some time and compute a thin U. We still need a full V.
  Eigen::JacobiSVD<MatrixXd> svd;
  svd.compute(A, Eigen::ComputeThinU | Eigen::ComputeFullV);
  if (!svd.computeV()) {
    VLOG(1) << "Failed to compute a singular value decomposition of A matrix.";
    return false;
  }

  // The 3D point is the eigenvector corresponding to the minimum eigenvalue.
  point = Point3D(svd.matrixV().block(0, 3, 3, 1) / svd.matrixV()(3,3));

  // Return false if the point is not visible from all cameras.
  for (size_t ii = 0; ii < cameras.size(); ++ii) {
    double u = 0.0, v = 0.0;
    if (!cameras[ii].WorldToImage(point.X(), point.Y(), point.Z(), &u, &v)) {
      return false;
    }
  }

  // Store the uncertainty as the inverse of the triangulation angle.
  const double angle = MaximumAngle(cameras, point);
  if (angle == 0.0) {  // we actually do want floating point comparison.
    uncertainty = std::numeric_limits<double>::max();
  } else {
    uncertainty = 1.0 / angle;
  }

  return true;
}

// Triangulate the 3D position of a point from a 2D correspondence and two sets
// of camera extrinsics and intrinsics.
bool Triangulate(const FeatureMatch& feature_match, const Camera& camera1,
                 const Camera& camera2, Point3D& point, double& uncertainty) {
  FeatureList features;
  features.push_back(feature_match.feature1_);
  features.push_back(feature_match.feature2_);

  std::vector<Camera> cameras;
  cameras.push_back(camera1);
  cameras.push_back(camera2);

  return Triangulate(features, cameras, point, uncertainty);
}

// Repeats the above function on a list of feature matches, returning a list of
// triangulated 3D points, where each point is computed from a single 2D <--> 2D
// correspondence.
bool Triangulate(const FeatureMatchList& feature_matches, const Camera& camera1,
                 const Camera& camera2, Point3DList& points, double& uncertainty) {
  // Clear output.
  points.clear();

  bool triangulated_all_points = true;
  for (size_t ii = 0; ii < feature_matches.size(); ++ii) {
    Point3D point;

    // Continue on failure, but store (0, 0, 0).
    double point_uncertainty = 0.0;
    if (!Triangulate(feature_matches[ii],
                     camera1,
                     camera2,
                     point,
                     point_uncertainty)) {
      uncertainty += point_uncertainty;
      triangulated_all_points = false;
      point = Point3D();
    }
    uncertainty += point_uncertainty;
    points.push_back(point);
  }

  return triangulated_all_points;
}

// Compute the maximum angle between each pair of observation angles.
double MaximumAngle(const std::vector<Camera>& cameras, const Point3D& point) {
  std::vector<Vector3d> vecs;
  for (const auto& camera : cameras)
    vecs.push_back((point.Get() - camera.Translation()).normalized());

  double largest_angle = 0.0;
  for (size_t ii = 0; ii < cameras.size() - 1; ++ii) {
    for (size_t jj = ii + 1; jj < cameras.size(); ++jj) {
      double angle = std::acos(vecs[ii].dot(vecs[jj]) - 1e-8);
      if (std::isnan(angle) || std::isinf(angle)) {
        LOG(WARNING) << "Observation angle is NaN.";
        continue;
      }

      // We want two 90 degree observations to have the maximum possible angle.
      // Two observations with a 180 degree angle should map back to 0, as the
      // triangulated point will be colinear with the translation between the
      // two cameras, which makes it unobservable.
      if (angle > M_PI_2) {
        angle = M_PI - angle;
      }

      if (angle > largest_angle) {
        largest_angle = angle;
      }
    }
  }
  return largest_angle;
}

}  //\namespace bsfm


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Pio_6 Pio_6 (function template)

  Generates the constant \f$\frac\pi{6}\f$.

  @headerref{<boost/simd/constant/pio_6.hpp>}

  @par Description

  1.  @code
      template<typename T> T Pio_6();
      @endcode

  2.  @code
      template<typename T> T Pio_6( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to \f$\frac\pi{6}\f$.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(0.52359877559829887307710723054658)`

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/pio_6.hpp>
#include <boost/simd/constant/simd/pio_6.hpp>

#endif


#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>

using namespace Eigen;

/*
 * Function shape
 *
 * @desript Display shape information of given matrix.
 */
void shape(const char *msg, Matrix<float, Dynamic, Dynamic> in)
{
  std::cout << msg << "("  << in.rows() << "," << in.cols() << ")" << std::endl;
}

/*
 * Function loge()
 *
 * @desript Calculate logf() for each items in given matrix.
 */
Matrix<float, Dynamic, Dynamic> loge(Matrix<float, Dynamic, Dynamic> in)
{
  Matrix<float, Dynamic, Dynamic> out = in;

  out = out.array() + 1e-7f;

  for(int i=0; i<in.cols(); i++)
    {
      out(0, i) = logf(out(0,i));
    }

  return out;
}

/*
 * Function cross_entropy()
 *
 * @desript Calculate Cross Entropy Loss value from 2 input matrix.
 */
float cross_entropy(Matrix<float, Dynamic, Dynamic> output, Matrix<float, Dynamic, Dynamic> expect)
{
  Matrix<float, Dynamic, Dynamic> tmp;
  float out = 0.f;
  tmp = expect.array() * loge(output).array();
  out = tmp.sum();
  return -out;
};

/*
 * Class NNLayer
 *
 * @desript Base class of Neural Network Layer.
 */
class NNLayer {
  protected:
    NNLayer *next;
    NNLayer *back;

  public:
    NNLayer() : next(NULL), back(NULL)
      {
      };

    virtual ~NNLayer() {};

    virtual Matrix<float, Dynamic, Dynamic> forward(Matrix<float, Dynamic, Dynamic> m) = 0;
    virtual Matrix<float, Dynamic, Dynamic> backward(float train_ratio, Matrix<float, Dynamic, Dynamic> m) = 0;
    virtual void resize(int rows, int cols) = 0;

    void setNext(NNLayer *n)
      {
        next = n;
        n->back = this;
      };

    NNLayer * getNext()
      {
        return next;
      };

};

/*
 * Class LastActivation
 *
 * @desript Base class of Final Activation Layer. This has loss() method function.
 */
class LastActivation : public NNLayer {
  public:
    LastActivation() : NNLayer()
      {
      };
    virtual ~LastActivation(){};

    virtual float loss(Matrix<float, Dynamic, Dynamic> expect) = 0;
};

/*
 * Class SoftMaxLayer
 *
 * @desript A kind of LastActivation Layer.
 */
class SoftMaxLayer : public LastActivation {
  Matrix<float, 1, Dynamic> output;
  public:
    SoftMaxLayer() : LastActivation()
      {
      };

    ~SoftMaxLayer() {};

    Matrix<float, Dynamic, Dynamic> forward(Matrix<float, Dynamic, Dynamic> m)
      {
        float sum;
        float max = m.maxCoeff(); // Maxvalue in the matrix.

        output = m.array() - max; // Measures of overflow.

        output = output.array().exp();
        sum = output.sum();
        output = output.array() / sum;

        if (next != NULL)
          return next->forward(output);
        else
          return output;
      };

    Matrix<float, Dynamic, Dynamic> backward(float train_ratio, Matrix<float, Dynamic, Dynamic> m)
      {
        Matrix<float, Dynamic, Dynamic> ret = output - m;
        if (back != NULL)
          return back->backward(train_ratio, ret);
        else
          return ret;
      };

    float loss(Matrix<float, Dynamic, Dynamic> expect)
      {
        return cross_entropy(output, expect);
      };

    void resize(int rows, int cols)
      {
      };
};

/*
 * Class ReLULayer
 *
 * @desript A kind of Activation Layer.
 */
class ReLULayer : public NNLayer {
  Matrix<float, 1, Dynamic> output;
  public:
    ReLULayer() : NNLayer()
      {
      };

    ~ReLULayer() {}

    Matrix<float, Dynamic, Dynamic> forward(Matrix<float, Dynamic, Dynamic> m)
      {
        output = m;
        for(int i=0; i<output.cols(); i++)
          {
            output(0, i) = output(0, i) <= 0 ? 0 : output(0,i);
          }

        if (next != NULL)
          return next->forward(output);
        else
          return output;
      };

    Matrix<float, Dynamic, Dynamic> backward(float train_ratio, Matrix<float, Dynamic, Dynamic> m)
      {
        Matrix<float, Dynamic, Dynamic> ret = m;
        for(int i=0; i<ret.cols(); i++)
          {
            ret(0, i) = output(0, i) <= 0 ? 0 : ret(0,i);
          }

        if (back != NULL)
          return back->backward(train_ratio, ret);
        else
          return ret;
      };

    void resize(int rows, int cols)
      {
        output = Matrix<float, 1, Dynamic>::Random(1, cols);
      };
};

/*
 * Class AffineLayer
 *
 * @desript A kind of Neural Network Layer.
 */
class AffineLayer : public NNLayer {
  Matrix<float, Dynamic, Dynamic> w;
  Matrix<float, 1, Dynamic> bias;
  Matrix<float, 1, Dynamic> output;
  Matrix<float, 1, Dynamic> inputx;

  public:
    AffineLayer() : NNLayer()
      {
      }

    AffineLayer(int rows, int cols) : NNLayer()
      {
        resize(rows, cols);
      }

    ~AffineLayer() {};

    void resize(int rows, int cols)
      {
        w.resize(rows, cols);
        bias.resize(1, cols);
        output.resize(1, cols);

        w      = Matrix<float, Dynamic, Dynamic>::Random(rows, cols);
        bias   = Matrix<float, 1, Dynamic>::Random(1, cols);
        output = Matrix<float, 1, Dynamic>::Random(1, cols);
      }

    Matrix<float, Dynamic, Dynamic> forward(Matrix<float, Dynamic, Dynamic> m)
      {
        inputx = m;
        output = (m * w) + bias;

        if (next != NULL)
          {
            return next->forward(output);
          }
        else
          {
            return output;
          }
      }

    Matrix<float, Dynamic, Dynamic> backward(float train_ratio, Matrix<float, Dynamic, Dynamic> m)
      {
        Matrix<float, Dynamic, Dynamic> ret;
        Matrix<float, Dynamic, Dynamic> dw;
        float db;

        ret = m * w.transpose();
        dw = inputx.transpose() * m;
        db = m.sum();

        w = w - (dw * train_ratio);
        bias = bias.array() - (db * train_ratio);

        if (back != NULL)
          return back->backward(train_ratio, ret);
        else
          return ret;
      };
};

/*
 * Class AffineLayer
 *
 * @desript A kind of Neural Network Layer.
 */
class NeuralNetwork {
  NNLayer *top_layer;
  NNLayer *last_layer;
  Matrix<float, Dynamic, Dynamic> output;

  public:
    NeuralNetwork() : top_layer(NULL), last_layer(NULL)
      {
      };
    ~NeuralNetwork()
      {
        NNLayer *tmp;
        for(NNLayer *layer = top_layer; layer != NULL; )
          {
            tmp = layer->getNext();
            delete layer;
            layer = tmp;
          }
      };

    void createNewLayer(int innum, int outnum, bool is_last=false)
      {
        NNLayer *new_affine;
        NNLayer *activation;

        new_affine = new AffineLayer(innum, outnum);

        if (is_last)
          {
            activation = new SoftMaxLayer();
          }
        else
          {
            activation = new ReLULayer();
          }

        new_affine->setNext(activation);

        if( last_layer == NULL )
          {
            top_layer = new_affine;
          }
        else
          {
            last_layer->setNext(new_affine);
          }

        last_layer = activation;
      };

    void print_layers()
      {
        for(NNLayer *layer = top_layer; layer != NULL; layer = layer->getNext())
          {
            std::cout << layer << " --> ";
          };
        std::cout << "NULL" << std::endl;
      };

    Matrix<float, Dynamic, Dynamic> forward(Matrix<float, Dynamic, Dynamic> input)
      {
        output = top_layer->forward(input);
        return output;
      };

    float backward(float train_ratio, Matrix<float, Dynamic, Dynamic> m)
      {
        float ret = ((LastActivation *)last_layer)->loss(m);
        last_layer->backward(train_ratio, m);
        return ret;
      };

};

int main(void)
{
  NeuralNetwork nn;

  float loss;
  Matrix<float, Dynamic, Dynamic> out;

#if 1
  Matrix<float, 1, 2> input[4];
  Matrix<float, 1, 1> expect[4];

  input[0] << 0, 0; expect[0] << 0;
  input[1] << 0, 1; expect[1] << 1;
  input[2] << 1, 0; expect[2] << 1;
  input[3] << 1, 1; expect[3] << 0;

  nn.createNewLayer(2,2);
  nn.createNewLayer(2,1, true);

  loss = 100.f;
  while (loss > 0.001f)
    {
      loss = 0.f;
      for(int i=0; i<4; i++)
        {
          // std::cout << "==== " << i+1 << " ===="  << std::endl;
          out = nn.forward( input[i] );
          loss += nn.backward(0.1f, expect[i]);
          // std::cout << "   out = " << out << std::endl;
        }
      loss /= 4;
      std::cout << "loss = " << loss << std::endl;
    }

  for(int i=0; i<4; i++)
    {
      std::cout << "Expect = " << expect[i] << " Input  = " << input[i] << std::endl;
      out = nn.forward( input[i] );
      std::cout << "Output = " << out << std::endl;
    }

#else
  Matrix<float, 1, 2> input;
  Matrix<float, 1, 2> expect;

  input << 3, 3;
  expect << 0, 1;

  nn.createNewLayer(2, 5);
  nn.createNewLayer(5, 3);
  nn.createNewLayer(3, 2, true);

  std::cout << "tmp input = " << input << std::endl;
  std::cout << "expect = " << expect << std::endl;
  std::cout << "input * expect = " << input.array() * expect.array() << std::endl;

  std::cout << "input = " << input << std::endl;
  input = input.array() + 1;
  std::cout << "input = " << input << std::endl;

  out = nn.forward( input );
  std::cout << "out.rows = " << out.rows() << std::endl;
  std::cout << "out.cols = " << out.cols() << std::endl;
  std::cout << "out = " << out << std::endl;

  loss = 100.f;
  while (loss > 0.001f)
    {
      out = nn.forward( input );
      loss = nn.backward(0.1f, expect);
      std::cout << "out = " << out << std::endl;
      std::cout << "loss = " << loss << std::endl;
    }
#endif

  return 0;
}


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <vector>

using namespace std;
using boost::multiprecision::cpp_int;

/*
Using the partition generating function..
p(n) = p(n – 1)  + p(k – 2) – p(k – 5) – p(k – 7) + p(k – 12) + p(k – 15) – p(k – 22)…
Where p(0) = 1 and p(n) = 0 for n < 0..
and k = m / 2 + 1 (k % 2 == 0) || -m / 2 + 1.
*/

int main(int argc, char *argv[]) {
	// We will need the generalized pentagonal numbers first.
	vector<cpp_int> k;
	k.push_back(1);
	int n = 1;
	// Loop through everything until we get something that fits our requirement.
	while(true) {
		int i = 0;
		int penta = 1;
		k.push_back(0);
		while(penta <= n) {
			int sign = (i % 4 > 1) ? -1 : 1;
			k[n] += sign * k[n - penta];
			k[n] %= 1'000'000;
			i++;
			// Generate the next pentagonal (generalized number).
			int j = (i % 2 == 0) ? i / 2 + 1 : -(i / 2 + 1);
			penta = j * (3 * j - 1) / 2;
		}
		if(!k[n]) {
			break;
		}
		n++;
	}
	cout << n << endl;
	return 0;
}