#include <mpi.h>
#include <iostream>
#include <iomanip>
#include <armadillo>
using namespace std;
using namespace arma;

double ** CreateMatrix(int m, int n){
  double ** mat;
  mat = new double*[m];
  for(int i=0;i<m;i++){
    mat[i] = new double[n];
    for(int j=0;j<m;j++)
      mat[i][j] = 0.0;
  }
  return mat;
}

void DestroyMatrix(double ** mat, int m, int n){
  for(int i=0;i<m;i++)
    delete[] mat[i];
  delete[] mat;
}

int Jacobi_P(int, int, int, double **, double *, double *, double);

int main(int argc, char * argv[]){
  int i,j, N = 20;
  double **A,*x,*q;
  int totalnodes,mynode;

  MPI_Init(&argc,&argv);
  MPI_Comm_size(MPI_COMM_WORLD, &totalnodes);
  MPI_Comm_rank(MPI_COMM_WORLD, &mynode);

  if(mynode==0){
    A = CreateMatrix(N,N);
    x = new double[N];
    q = new double[N];

    for(i=0;i<N;i++){
      q[i] = i+1;
      A[i][i] = -2.0;
      if(i<N-1){
        A[i][i+1] = 1.0;
        A[i+1][i] = 1.0;
      }
    }
  }
  Jacobi_P(mynode,totalnodes,N,A,x,q,1.0e-14);
  if(mynode==0){
    for(i=0;i<N;i++)
      cout << x[i] << endl;
    DestroyMatrix(A,N,N);
    delete[] x;
    delete[] q;
  }
  MPI_Finalize();
}

int Jacobi_P(int mynode, int numnodes, int N, double **A, double *x, double *b, double abstol){
  int i,j,k,i_global;
  int maxit = 100000;
  int rows_local,local_offset,last_rows_local,*count,*displacements;
  double sum1,sum2,*xold;
  double error_sum_local, error_sum_global;
  MPI_Status status;

  rows_local = (int) floor((double)N/numnodes);
  local_offset = mynode*rows_local;
  if(mynode == (numnodes-1))
    rows_local = N - rows_local*(numnodes-1);

  /*Distribute the Matrix and R.H.S. among the processors */
  if(mynode == 0){
    for(i=1;i<numnodes-1;i++){
      for(j=0;j<rows_local;j++)
        MPI_Send(A[i*rows_local+j],N,MPI_DOUBLE,i,j,MPI_COMM_WORLD);
      MPI_Send(b+i*rows_local,rows_local,MPI_DOUBLE,i,rows_local,
               MPI_COMM_WORLD);
    }
    last_rows_local = N-rows_local*(numnodes-1);
    for(j=0;j<last_rows_local;j++)
      MPI_Send(A[(numnodes-1)*rows_local+j],N,MPI_DOUBLE,numnodes-1,j,
               MPI_COMM_WORLD);
    MPI_Send(b+(numnodes-1)*rows_local,last_rows_local,MPI_DOUBLE,numnodes-1,
             last_rows_local,MPI_COMM_WORLD);
  }
  else{
    A = CreateMatrix(rows_local,N);
    x = new double[rows_local];
    b = new double[rows_local];
    for(i=0;i<rows_local;i++)
      MPI_Recv(A[i],N,MPI_DOUBLE,0,i,MPI_COMM_WORLD,&status);
    MPI_Recv(b,rows_local,MPI_DOUBLE,0,rows_local,MPI_COMM_WORLD,&status);
  }

  xold = new double[N];
  count = new int[numnodes];
  displacements = new int[numnodes];

  //set initial guess to all 1.0
  for(i=0; i<N; i++){
    xold[i] = 1.0;
  }

  for(i=0;i<numnodes;i++){
    count[i] = (int) floor((double)N/numnodes);
    displacements[i] = i*count[i];
  }
  count[numnodes-1] = N - ((int)floor((double)N/numnodes))*(numnodes-1);

  for(k=0; k<maxit; k++){
    error_sum_local = 0.0;
    for(i = 0; i<rows_local; i++){
      i_global = local_offset+i;
      sum1 = 0.0; sum2 = 0.0;
      for(j=0; j < i_global; j++)
        sum1 = sum1 + A[i][j]*xold[j];
      for(j=i_global+1; j < N; j++)
        sum2 = sum2 + A[i][j]*xold[j];

      x[i] = (-sum1 - sum2 + b[i])/A[i][i_global];
      error_sum_local += (x[i]-xold[i_global])*(x[i]-xold[i_global]);
    }

    MPI_Allreduce(&error_sum_local,&error_sum_global,1,MPI_DOUBLE,
                  MPI_SUM,MPI_COMM_WORLD);
    MPI_Allgatherv(x,rows_local,MPI_DOUBLE,xold,count,displacements,
                   MPI_DOUBLE,MPI_COMM_WORLD);

    if(sqrt(error_sum_global)<abstol){
      if(mynode == 0){
        for(i=0;i<N;i++)
          x[i] = xold[i];
      }
      else{
        DestroyMatrix(A,rows_local,N);
        delete[] x;
        delete[] b;
      }
      delete[] xold;
      delete[] count;
      delete[] displacements;
      return k;
    }
  }

  cerr << "Jacobi: Maximum Number of Interations Reached Without Convergence\n";
  if(mynode == 0){
    for(i=0;i<N;i++)
      x[i] = xold[i];
  }
  else{
    DestroyMatrix(A,rows_local,N);
    delete[] x;
    delete[] b;
  }
  delete[] xold;
  delete[] count;
  delete[] displacements;

  return maxit;
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "weighted_alpha_complex"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <CGAL/Epick_d.h>
#include <CGAL/Epeck_d.h>

#include <cmath>  // float comparison
#include <vector>
#include <random>
#include <array>
#include <cmath> // for std::fabs

#include <gudhi/Alpha_complex.h>
#include <gudhi/Alpha_complex_3d.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Unitary_tests_utils.h>

using list_of_exact_kernel_variants = boost::mpl::list<CGAL::Epeck_d< CGAL::Dynamic_dimension_tag >,
                                                       CGAL::Epeck_d< CGAL::Dimension_tag<4> >
                                                       > ;

BOOST_AUTO_TEST_CASE_TEMPLATE(Zero_weighted_alpha_complex, Kernel, list_of_exact_kernel_variants) {
  // Check that in exact mode for static dimension 4 the code for dD unweighted and for dD weighted with all weights
  // 0 give exactly the same simplex tree (simplices and filtration values).

  // Random points construction
  using Point_d = typename Kernel::Point_d;
  std::vector<Point_d> points;
  std::uniform_real_distribution<double> rd_pts(-10., 10.);
  std::random_device rand_dev;
  std::mt19937 rand_engine(rand_dev());
  for (int idx = 0; idx < 20; idx++) {
    std::vector<double> point {rd_pts(rand_engine), rd_pts(rand_engine), rd_pts(rand_engine), rd_pts(rand_engine)};
    points.emplace_back(point.begin(), point.end());
  }

  // Alpha complex from points
  Gudhi::alpha_complex::Alpha_complex<Kernel, false> alpha_complex_from_points(points);
  Gudhi::Simplex_tree<> simplex;
  Gudhi::Simplex_tree<>::Filtration_value infty = std::numeric_limits<Gudhi::Simplex_tree<>::Filtration_value>::infinity();
  BOOST_CHECK(alpha_complex_from_points.create_complex(simplex, infty, true));
  std::clog << "Iterator on alpha complex simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : simplex.filtration_simplex_range()) {
    std::clog << "   ( ";
    for (auto vertex : simplex.simplex_vertex_range(f_simplex)) {
      std::clog << vertex << " ";
    }
    std::clog << ") -> " << "[" << simplex.filtration(f_simplex) << "] " << std::endl;
  }

  // Alpha complex from zero weighted points
  std::vector<typename Kernel::FT> weights(20, 0.);
  Gudhi::alpha_complex::Alpha_complex<Kernel, true> alpha_complex_from_zero_weighted_points(points, weights);
  Gudhi::Simplex_tree<> zw_simplex;
  BOOST_CHECK(alpha_complex_from_zero_weighted_points.create_complex(zw_simplex, infty, true));

  std::clog << "Iterator on zero weighted alpha complex simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : zw_simplex.filtration_simplex_range()) {
    std::clog << "   ( ";
    for (auto vertex : zw_simplex.simplex_vertex_range(f_simplex)) {
      std::clog << vertex << " ";
    }
    std::clog << ") -> " << "[" << zw_simplex.filtration(f_simplex) << "] " << std::endl;
  }

  BOOST_CHECK(zw_simplex == simplex);
}

template <typename Point_d>
bool cgal_3d_point_sort (Point_d a,Point_d b) {
  if (a[0] != b[0])
    return a[0] < b[0];
  if (a[1] != b[1])
    return a[1] < b[1];
  return a[2] < b[2];
}

BOOST_AUTO_TEST_CASE(Weighted_alpha_complex_3d_comparison) {
  // check that for random weighted 3d points in safe mode the 3D and dD codes give the same result with some tolerance

  // Random points construction
  using Kernel_dD = CGAL::Epeck_d< CGAL::Dimension_tag<3> >;
  using Bare_point_d = typename Kernel_dD::Point_d;
  using Weighted_point_d = typename Kernel_dD::Weighted_point_d;
  std::vector<Weighted_point_d> w_points_d;

  using Exact_weighted_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, true, false>;
  using Bare_point_3 = typename Exact_weighted_alpha_complex_3d::Bare_point_3;
  using Weighted_point_3 = typename Exact_weighted_alpha_complex_3d::Weighted_point_3;
  std::vector<Weighted_point_3> w_points_3;

  std::uniform_real_distribution<double> rd_pts(-10., 10.);
  std::uniform_real_distribution<double> rd_wghts(-0.5, 0.5);
  std::random_device rand_dev;
  std::mt19937 rand_engine(rand_dev());
  for (int idx = 0; idx < 20; idx++) {
    std::vector<double> point {rd_pts(rand_engine), rd_pts(rand_engine), rd_pts(rand_engine)};
    double weight = rd_wghts(rand_engine);
    w_points_d.emplace_back(Bare_point_d(point.begin(), point.end()), weight);
    w_points_3.emplace_back(Bare_point_3(point[0], point[1], point[2]), weight);
  }

  // Structures necessary for comparison
  using Points = std::vector<std::array<double,3>>;
  using Points_and_filtrations = std::map<Points, double>;
  Points_and_filtrations pts_fltr_dD;
  Points_and_filtrations pts_fltr_3d;

  // Weighted alpha complex for dD version
  Gudhi::alpha_complex::Alpha_complex<Kernel_dD, true> alpha_complex_dD_from_weighted_points(w_points_d);
  Gudhi::Simplex_tree<> w_simplex_d;
  BOOST_CHECK(alpha_complex_dD_from_weighted_points.create_complex(w_simplex_d));

  std::clog << "Iterator on weighted alpha complex dD simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : w_simplex_d.filtration_simplex_range()) {
    Points points;
    for (auto vertex : w_simplex_d.simplex_vertex_range(f_simplex)) {
      CGAL::NT_converter<Kernel_dD::RT, double> cgal_converter;
      Bare_point_d pt = alpha_complex_dD_from_weighted_points.get_point(vertex).point();
      points.push_back({cgal_converter(pt[0]), cgal_converter(pt[1]), cgal_converter(pt[2])});
    }
    std::clog << "   ( ";
    std::sort (points.begin(), points.end());
    for (auto point : points) {
      std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
    }
    std::clog << ") -> " << "[" << w_simplex_d.filtration(f_simplex) << "] ";
    std::clog << std::endl;
    pts_fltr_dD[points] = w_simplex_d.filtration(f_simplex);
  }

  // Weighted alpha complex for 3D version
  Exact_weighted_alpha_complex_3d alpha_complex_3D_from_weighted_points(w_points_3);
  Gudhi::Simplex_tree<> w_simplex_3;
  BOOST_CHECK(alpha_complex_3D_from_weighted_points.create_complex(w_simplex_3));

  std::clog << "Iterator on weighted alpha complex 3D simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : w_simplex_3.filtration_simplex_range()) {
    Points points;
    for (auto vertex : w_simplex_3.simplex_vertex_range(f_simplex)) {
      Bare_point_3 pt = alpha_complex_3D_from_weighted_points.get_point(vertex).point();
      CGAL::NT_converter<Exact_weighted_alpha_complex_3d::Kernel::RT, double> cgal_converter;
      points.push_back({cgal_converter(pt[0]), cgal_converter(pt[1]), cgal_converter(pt[2])});
    }
    std::clog << "   ( ";
    std::sort (points.begin(), points.end());
    for (auto point : points) {
      std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
    }
    std::clog << ") -> " << "[" << w_simplex_3.filtration(f_simplex) << "] " << std::endl;
    pts_fltr_3d[points] = w_simplex_d.filtration(f_simplex);
  }

  // Compares structures
  auto d3_itr = pts_fltr_3d.begin();
  auto dD_itr = pts_fltr_dD.begin();
  for (; d3_itr != pts_fltr_3d.end() && dD_itr != pts_fltr_dD.end(); ++d3_itr) {
    if (d3_itr->first != dD_itr->first) {
      for(auto point : d3_itr->first)
        std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
      std::clog << " versus ";
      for(auto point : dD_itr->first)
        std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
      std::clog << std::endl;
      BOOST_CHECK(false);
    }
    // In safe mode, relative error is less than 1e-5 (can be changed with set_relative_precision_of_to_double)
    if (std::fabs(d3_itr->second - dD_itr->second) > 1e-5 * (std::fabs(d3_itr->second) + std::fabs(dD_itr->second))) {
      std::clog << d3_itr->second << " versus " << dD_itr->second << " diff "
                << std::fabs(d3_itr->second - dD_itr->second) << std::endl;
      BOOST_CHECK(false);
    }
    ++dD_itr;
  }
}

using list_of_1d_kernel_variants = boost::mpl::list<CGAL::Epeck_d< CGAL::Dynamic_dimension_tag >,
                                                    CGAL::Epeck_d< CGAL::Dimension_tag<1>>,
                                                    CGAL::Epick_d< CGAL::Dynamic_dimension_tag >,
                                                    CGAL::Epick_d< CGAL::Dimension_tag<1>>
                                                    >;

BOOST_AUTO_TEST_CASE_TEMPLATE(Weighted_alpha_complex_non_visible_points, Kernel, list_of_1d_kernel_variants) {
  // check that for 2 closed weighted 1-d points, one with a high weight to hide the second one with a small weight,
  // that the point with a small weight has the same high filtration value than the edge formed by the 2 points
  using Point_d = typename Kernel::Point_d;
  std::vector<Point_d> points;
  std::vector<double> p1 {0.};
  points.emplace_back(p1.begin(), p1.end());
  // closed enough points
  std::vector<double> p2 {0.1};
  points.emplace_back(p2.begin(), p2.end());
  std::vector<typename Kernel::FT> weights {100., 0.01};

  Gudhi::alpha_complex::Alpha_complex<Kernel, true> alpha_complex(points, weights);
  Gudhi::Simplex_tree<> stree;
  BOOST_CHECK(alpha_complex.create_complex(stree));

  std::clog << "Iterator on weighted alpha complex simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : stree.filtration_simplex_range()) {
    std::clog << "   ( ";
    for (auto vertex : stree.simplex_vertex_range(f_simplex)) {
      std::clog << vertex << " ";
    }
    std::clog << ") -> " << "[" << stree.filtration(f_simplex) << "] " << std::endl;
  }

  BOOST_CHECK(stree.filtration(stree.find({0})) == -100.);
  BOOST_CHECK(stree.filtration(stree.find({1})) == stree.filtration(stree.find({0, 1})));
  BOOST_CHECK(stree.filtration(stree.find({1})) > 100000);
}


#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <set>
#include <boost/numeric/ublas/matrix.hpp>

#include "../Point.h"
#include "../ANN.h"
#include "../RMSUtils.h"
#include "../IOUtil.h"

using namespace std;

vector<Point> validDirs;
vector<double> validResults;

bool validate_coreset(const vector<Point> &fatP, vector<size_t> &idxs, double epsilon) {
    assert(idxs.size() <= fatP.size());

    if (fatP.empty())
        return true;

    if (idxs.empty())
        return false;

    assert(idxs[0] >= 0 && idxs[idxs.size() - 1] < fatP.size());

    for (size_t i = 0; i < validDirs.size(); i++) {
        double cor_max = fatP[idxs[0]].dotP(validDirs[i]);
        for (size_t j = 1; j < idxs.size(); j++) {
            double corval = fatP[idxs[j]].dotP(validDirs[i]);
            if (corval > cor_max)
                cor_max = corval;
        }

        double ptval = validResults[i];
        if (cor_max < (1 - epsilon) * ptval)
            return false;
    }
    return true;
}

bool validate_eps_kernel(const vector<Point> &fatP, vector<size_t> &idxs, double epsilon) {
    assert(idxs.size() <= fatP.size());

    if (fatP.empty())
        return true;

    if (idxs.empty())
        return false;

    assert(idxs[0] >= 0 && idxs[idxs.size() - 1] < fatP.size());

    size_t m = validDirs.size() / 2;
    for (size_t i = 0; i < m; i++) {
        double cor_max = fatP[idxs[0]].dotP(validDirs[2 * i]);
        double cor_min = fatP[idxs[0]].dotP(validDirs[2 * i + 1]);
        for (size_t j = 1; j < idxs.size(); j++) {
            double cor_val1 = fatP[idxs[j]].dotP(validDirs[2 * i]);
            double cor_val2 = fatP[idxs[j]].dotP(validDirs[2 * i + 1]);
            if (cor_val1 > cor_max)
                cor_max = cor_val1;
            if (cor_val2 > cor_min)
                cor_min = cor_val2;
        }

        if ((cor_max + cor_min) < (1 - epsilon) * (validResults[2 * i] + validResults[2 * i + 1]))
            return false;
    }
    return true;
}

void coreset_by_sample(ANN *ann_ds, size_t dim, double outer_rad, double delta, size_t deltam, vector<size_t> &idxs) {
    vector<Point> randomP;
    RMSUtils::get_random_sphere_points(outer_rad, dim, deltam, randomP, false);

    vector<size_t> new_idxs;
    ann_ds->getANNs(randomP, delta, new_idxs);

    //figure out all the distinct indices
    set<size_t> unique_idxs;
    for (size_t i = 0; i < idxs.size(); i++)
        unique_idxs.insert(idxs[i]);
    for (size_t i = 0; i < new_idxs.size(); i++)
        unique_idxs.insert(new_idxs[i]);

    idxs.clear();

    set<size_t>::iterator it;
    for (it = unique_idxs.begin(); it != unique_idxs.end(); ++it)
        idxs.push_back((*it));
}

vector<size_t> get_coreset(const vector<Point> &fatP, const double epsilon, double &time) {
    clock_t clockS = clock();

    size_t dim = fatP[0].get_dimension();

    double outer_rad;
    outer_rad = 1 + sqrt(dim);
    double delta = epsilon / (2 * outer_rad);

    vector<size_t> idxs;
    ANN *ann_ds = new ANN();
    ann_ds->insertPts(fatP);

    clock_t clockE = clock();

    time += (double) (clockE - clockS);

    size_t m = 10;
    bool b = false;
    while (!b) {
        clockS = clock();
        coreset_by_sample(ann_ds, dim, outer_rad, delta, m, idxs);
        clockE = clock();
        time += (double) (clockE - clockS);

        b = validate_coreset(fatP, idxs, epsilon);
        m *= 2;
    }

    delete ann_ds;

    return idxs;
}

int main(int argc, char **argv) {
    if (argc < 7) {
        cerr << "coreset: Usage " << argv[0] << "<eps> <dim> <data_path> <query_path> <valid_path> <output_path>\n";
        exit(1);
    }

    double eps = atof(argv[1]);
    size_t dim = atoi(argv[2]);

    vector<Point> fatP;

    IOUtil::read_input_points(argv[3], dim, fatP);
    IOUtil::read_validate_dirs(argv[4], dim, validDirs);
    IOUtil::read_validate_results(argv[5], validResults);

    ofstream result_file;
    result_file.open(argv[6], ofstream::out | ofstream::app);

    cout << "ann " << argv[3] << " " << fatP.size() << " " << dim << endl;

    result_file << "dataset=" << argv[3] << " eps=" << eps << "\n" << flush;

    for (int r = 0; r < 10; ++r) {
        vector<size_t> idxs;
        double time = 0;
        idxs = get_coreset(fatP, eps, time);

        int size = idxs.size();
        time = (double) time / CLOCKS_PER_SEC;

        result_file << "time=" << (time * 1000.0) << " size=" << size << "\n" << flush;
    }
    result_file << "\n" << flush;
    result_file.close();
}


// Copyright (C) 2013 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:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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 of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include "gtest/gtest.h"

#include "theia/math/util.h"
#include "theia/util/random.h"
#include "theia/sfm/pose/four_point_homography.h"
#include "theia/sfm/pose/test_util.h"

namespace theia {
namespace {
using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;

RandomNumberGenerator rng(53);

// Creates a test scenario from ground truth 3D points and ground truth rotation
// and translation. Projection (i.e., image) noise is optional (set to 0 for no
// noise). The fundamental matrix is computed to ensure that the reprojection
// errors are sufficiently small.
void GenerateImagePoints(const std::vector<Vector3d>& points_3d,
                         const double projection_noise_std_dev,
                         const Quaterniond& expected_rotation,
                         const Vector3d& expected_translation,
                         std::vector<Vector2d>* image_1_points,
                         std::vector<Vector2d>* image_2_points) {
  image_1_points->reserve(points_3d.size());
  image_2_points->reserve(points_3d.size());
  for (int i = 0; i < points_3d.size(); i++) {
    image_1_points->push_back(points_3d[i].hnormalized());
    image_2_points->push_back((expected_rotation * points_3d[i] +
                               expected_translation).hnormalized());
  }

  if (projection_noise_std_dev) {
    for (int i = 0; i < points_3d.size(); i++) {
      AddNoiseToProjection(projection_noise_std_dev, &rng,
                           &((*image_1_points)[i]));
      AddNoiseToProjection(projection_noise_std_dev, &rng,
                           &((*image_2_points)[i]));
    }
  }
}

// Check that the symmetric error is small. NOTE: this is a different error than
// the reprojection error.
void CheckSymmetricError(const std::vector<Vector2d>& image_1_points,
                         const std::vector<Vector2d>& image_2_points,
                         const Matrix3d& homography_matrix,
                         const double max_symmetric_error) {
  const Matrix3d inv_homography = homography_matrix.inverse();
  for (int i = 0; i < image_1_points.size(); i++) {
    const Vector3d image_1_hat =
        inv_homography * image_2_points[i].homogeneous();
    const Vector3d image_2_hat =
        homography_matrix * image_1_points[i].homogeneous();
    // Compute reprojection error.
    const double img_1_error =
        (image_1_points[i] - image_1_hat.hnormalized()).squaredNorm();
    const double img_2_error =
        (image_2_points[i] - image_2_hat.hnormalized()).squaredNorm();

    EXPECT_LT(img_1_error, max_symmetric_error);
    EXPECT_LT(img_2_error, max_symmetric_error);
  }
}

// Run a test for the homography with at least 4 points.
void FourPointHomographyWithNoiseTest(const std::vector<Vector3d>& points_3d,
                                      const double projection_noise_std_dev,
                                      const Quaterniond& expected_rotation,
                                      const Vector3d& expected_translation,
                                      const double kMaxSymmetricError) {
  std::vector<Vector2d> image_1_points;
  std::vector<Vector2d> image_2_points;
  GenerateImagePoints(points_3d, projection_noise_std_dev, expected_rotation,
                      expected_translation, &image_1_points, &image_2_points);
  // Compute homography matrix.
  Matrix3d homography_matrix;
  EXPECT_TRUE(FourPointHomography(image_1_points,
                                  image_2_points,
                                  &homography_matrix));

  CheckSymmetricError(image_1_points, image_2_points, homography_matrix,
                         kMaxSymmetricError);
}

void BasicTest() {
  const std::vector<Vector3d> points_3d = {
    Vector3d(-1.0, 3.0, 3.0),
    Vector3d(1.0, -1.0, 2.0),
    Vector3d(-1.0, 1.0, 2.0),
    Vector3d(2.0, 1.0, 3.0),
  };

  const Quaterniond soln_rotation(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  const double kNoise = 0.0 / 512.0;
  const double kMaxSymmetricError = 1e-12;

  FourPointHomographyWithNoiseTest(
      points_3d, kNoise, soln_rotation, soln_translation,
      kMaxSymmetricError);
}

TEST(FourPointHomography, BasicTest) {
  BasicTest();
}

TEST(FourPointHomography, NoiseTest) {
  const std::vector<Vector3d> points_3d = {
    Vector3d(-1.0, 3.0, 3.0),
    Vector3d(1.0, -1.0, 2.0),
    Vector3d(-1.0, 1.0, 2.0),
    Vector3d(2.0, 1.0, 3.0),
  };

  const Quaterniond soln_rotation(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxSymmetricError = 1e-4;

  FourPointHomographyWithNoiseTest(points_3d, kNoise, soln_rotation,
                                    soln_translation, kMaxSymmetricError);
}

TEST(FourPointHomography, PlanarPoints) {
  const std::vector<Vector3d> points_3d = {
    Vector3d(-1.0, 3.0, 5.0),
    Vector3d(1.0, -1.0, 5.0),
    Vector3d(-1.0, 1.0, 5.0),
    Vector3d(2.0, 1.0, 5.0),
  };

  const Quaterniond soln_rotation(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, -0.5, -1.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxSymmetricError = 1e-4;

  FourPointHomographyWithNoiseTest(points_3d, kNoise, soln_rotation,
                                    soln_translation, kMaxSymmetricError);
}

void ManyPointsTest() {
  const Quaterniond soln_rotation(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxSymmetricError = 1e-4;
  const int num_points = 100;

  std::vector<Vector3d> points_3d(num_points);
  for (int j = 0; j < num_points; j++) {
    points_3d[j] = Vector3d(rng.RandDouble(-2.0, 2.0),
                            rng.RandDouble(-2.0, 2.0),
                            rng.RandDouble(1.0, 5.0));
  }

  FourPointHomographyWithNoiseTest(points_3d, kNoise, soln_rotation,
                                   soln_translation, kMaxSymmetricError);
}

TEST(FourPointHomography, ManyPoints) {
  ManyPointsTest();
}

}  // namespace
}  // namespace theia


#include <blitz/timer.h>

BZ_USING_NAMESPACE(blitz)

void initialize(double& c, double& d, double* a, double* b, int& N);

template<class T>
void sink(T&)
{ }

void benchmarkLoops(int, long);

int main()
{
    cout << "This program measures the performance of DAXPY operations"
         << endl << "using various C loop structures." << endl << endl;

    cout << endl << "In-cache:" << endl;

    benchmarkLoops(400,50000);

    cout << endl << "Out of cache:" << endl;

    benchmarkLoops(1000000,50);

    return 0;
}

void benchmarkLoops(int N, long iterations)
{
    double* _bz_restrict a = new double[N];
    double* _bz_restrict b = new double[N];
    double c, d;
    double t1, t2;

    initialize(c, d, a, b, N);

    double mflops = iterations * 4.0 * N / (1024.0 * 1024.0);

    Timer timer;

    cout << "Mflops/s Description" << endl;

    long iter;
    int i;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        for (i=0; i < N; ++i)
            a[i] += c * b[i];

        for (i=0; i < N; ++i)
            a[i] += d * b[i];
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "   for, indirection, unit stride" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        for (i=0; i < N; ++i)
            a[i] = a[i] + c * b[i];

        for (i=0; i < N; ++i)
            a[i] = a[i] + d * b[i];
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "   for, indirection, unit stride, no +=" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        for (i=N-1; i >= 0; --i)
            a[i] += c * b[i];

        for (i=N-1; i >= 0; --i)
            a[i] += d * b[i];
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "   for, indirection, unit stride, backwards loops" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 3;
        for (i=0; i < n1; ++i)
            a[i] += c2 * b[i];

        for (; i < N; i += 4)
        {
            a[i] += c2 * b[i];
            a[i+1] += c2 * b[i+1];
            a[i+2] += c2 * b[i+2];
            a[i+3] += c2 * b[i+3];
        }

        double d2 = d;
        int n2 = N & 3;
        for (i=0; i < n2; ++i)
            a[i] += d2 * b[i];

        for (; i < N; i += 4)
        {
            a[i] += d2 * b[i];
            a[i+1] += d2 * b[i+1];
            a[i+2] += d2 * b[i+2];
            a[i+3] += d2 * b[i+3];
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=4, unit stride, constants loaded into temps"
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 3;
        for (i=0; i < n1; ++i)
            a[i] += c2 * b[i];

        for (; i < N; i += 4)
        {
            double t1 = c2 * b[i];
            double t2 = c2 * b[i+1];
            double t3 = c2 * b[i+2];
            double t4 = c2 * b[i+3];

            a[i] += t1;
            a[i+1] += t2;
            a[i+2] += t3;
            a[i+3] += t4;
        }

        double d2 = d;
        int n2 = N & 3;
        for (i=0; i < n2; ++i)
            a[i] += d2 * b[i];

        for (; i < N; i += 4)
        {
            double t1 = d2 * b[i];
            double t2 = d2 * b[i+1];
            double t3 = d2 * b[i+2];
            double t4 = d2 * b[i+3];

            a[i] += t1;
            a[i+1] += t2;
            a[i+2] += t3;
            a[i+3] += t4;
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=4, unit stride, constants loaded into temps,"
         << endl << "\t\t4 read then 4 write"
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 3;
        for (i=0; i < n1; ++i)
            a[i] += c2 * b[i];

        for (; i < N; i += 4)
        {
            a[i] = a[i] + c2 * b[i];
            a[i+1] = a[i+1] + c2 * b[i+1];
            a[i+2] = a[i+2] + c2 * b[i+2];
            a[i+3] = a[i+3] + c2 * b[i+3];
        }

        double d2 = d;
        int n2 = N & 3;
        for (i=0; i < n2; ++i)
            a[i] += d2 * b[i];

        for (; i < N; i += 4)
        {
            a[i] = a[i] + d2 * b[i];
            a[i+1] = a[i+1] + d2 * b[i+1];
            a[i+2] = a[i+2] + d2 * b[i+2];
            a[i+3] = a[i+3] + d2 * b[i+3];
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=4, unit stride, constants loaded into temps,"
         << endl << "            no += "
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 3;
        for (i=0; i < n1; ++i)
            a[i] += c2 * b[i];

        for (; i < N; i += 4)
        {
            int i1 = i + 1;
            a[i] += c2 * b[i];
            int i2 = i + 2;
            a[i1] += c2 * b[i1];
            int i3 = i + 3;
            a[i2] += c2 * b[i2];
            a[i3] += c2 * b[i3];
        }

        double d2 = d;
        int n2 = N & 3;
        for (i=0; i < n2; ++i)
            a[i] += d2 * b[i];

        for (; i < N; i += 4)
        {
            int i1 = i + 1;
            a[i] += d2 * b[i];
            int i2 = i + 2;
            a[i1] += d2 * b[i1];
            int i3 = i + 3;
            a[i2] += d2 * b[i2];
            a[i3] += d2 * b[i3];
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=4, unit stride, constants loaded into temps,"
         << endl << "        CSE for index offsets"
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 3;
        for (i=0; i < n1; ++i)
            a[i] += c2 * b[i];

        double* pa = a+n1;
        double* pb = b+n1;

        int top = N - n1 - 4;

        for (i=top; i >= 0; i -= 4)
        {
            pa[i] += c2 * pb[i];
            pa[i+1] += c2 * pb[i+1];
            pa[i+2] += c2 * pb[i+2];
            pa[i+3] += c2 * pb[i+3];
        }

        double d2 = d;
        int n2 = N & 3;
        for (i=0; i < n2; ++i)
            a[i] += d2 * b[i];

        pa = a+n2;
        pb = b+n2;

        top = N - n2 - 4;
        for (i=top; i >= 0; i -= 4)
        {
            pa[i] += d2 * pb[i];
            pa[i+1] += d2 * pb[i+1];
            pa[i+2] += d2 * pb[i+2];
            pa[i+3] += d2 * pb[i+3];
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=4, unit stride, constants loaded into temps,"
         << "            backwards"
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 7;
        for (i=0; i < n1; ++i)
            a[i] += c2 * b[i];

        for (; i < N; i += 8)
        {
            a[i] += c2 * b[i];
            a[i+1] += c2 * b[i+1];
            a[i+2] += c2 * b[i+2];
            a[i+3] += c2 * b[i+3];
            a[i+4] += c2 * b[i+4];
            a[i+5] += c2 * b[i+5];
            a[i+6] += c2 * b[i+6];
            a[i+7] += c2 * b[i+7];
        }

        double d2 = d;
        int n2 = N & 7;
        for (i=0; i < n2; ++i)
            a[i] += d2 * b[i];

        for (; i < N; i += 8)
        {
            a[i] += d2 * b[i];
            a[i+1] += d2 * b[i+1];
            a[i+2] += d2 * b[i+2];
            a[i+3] += d2 * b[i+3];
            a[i+4] += d2 * b[i+4];
            a[i+5] += d2 * b[i+5];
            a[i+6] += d2 * b[i+6];
            a[i+7] += d2 * b[i+7];
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=8, unit stride, constants loaded into temps"
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;
        for (i=0; i < N; ++i)
            a[i] += c2 * b[i];

        double d2 = d;
        for (i=0; i < N; ++i)
            a[i] += d2 * b[i];
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "   for, indirection, unit stride, constants into temps"
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        int stride = 1;
        sink(stride);    // Prevent copy propagation

        for (i=0; i < N; i += stride)
            a[i] += c * b[i];

        for (i=0; i < N; i += stride)
            a[i] += d * b[i];
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, indirection, non-unit stride" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        int stride = 1;
        sink(stride);    // Prevent copy propagation

        double c2 = c;
        for (i=0; i < N; i += stride)
            a[i] += c2 * b[i];

        double d2 = d;
        for (i=0; i < N; i += stride)
            a[i] += d2 * b[i];
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, indirection, non-unit stride, constants "
            "loaded into temps" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double * _bz_restrict pa1 = a,
               * _bz_restrict pb1 = b;
        double * _bz_restrict paend1 = a + N;
        while (pa1 != paend1)
        {
            *pa1 += c * (*pb1);
            ++pa1;
            ++pb1;
        }

        double * _bz_restrict pa2 = a,
               * _bz_restrict pb2 = b;
        double * _bz_restrict paend2 = a + N;
        while (pa2 != paend2)
        {
            *pa2 += d * (*pb2);
            ++pa2;
            ++pb2;
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    while, pointer increment, unit stride" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double * _bz_restrict pa1 = a,
               * _bz_restrict pb1 = b;
        double * _bz_restrict paend1 = a + N;
        double c2 = c;
        while (pa1 != paend1)
        {
            *pa1 += c2 * (*pb1);
            ++pa1;
            ++pb1;
        }

        double * _bz_restrict pa2 = a,
               * _bz_restrict pb2 = b;
        double * _bz_restrict paend2 = a + N;
        double d2 = d;
        while (pa2 != paend2)
        {
            *pa2 += d2 * (*pb2);
            ++pa2;
            ++pb2;
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    while, pointer increment, unit stride, " << endl
         << "    constants loaded into temps"
         << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        int stride = 1;
        sink(stride);

        double * _bz_restrict pa1 = a,
               * _bz_restrict pb1 = b;
        double * _bz_restrict paend1 = a + N * stride;
        while (pa1 != paend1)
        {
            *pa1 += c * (*pb1);
            pa1 += stride;
            pb1 += stride;
        }

        double * _bz_restrict pa2 = a,
               * _bz_restrict pb2 = b;
        double * _bz_restrict paend2 = a + N * stride;
        while (pa2 != paend2)
        {
            *pa2 += d * (*pb2);
            pa2 += stride;
            pb2 += stride;
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    while, pointer increment, non-unit stride" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        int stride = 1;
        sink(stride);

        double * _bz_restrict pa1 = a,
               * _bz_restrict pb1 = b;
        double * _bz_restrict paend1 = a + N * stride;
        double c2 = c;
        int n1 = N & 3;

        for (i=0; i < n1; ++i)
        {
            *pa1 += c2 * (*pb1);
            pa1 += stride;
            pb1 += stride;
        }

        while (pa1 != paend1)
        {
            pa1[0] += c2 * pb1[0];
            pa1[1] += c2 * pb1[1];
            pa1[2] += c2 * pb1[2];
            pa1[3] += c2 * pb1[3];
            pa1 += 4 * stride;
            pb1 += 4 * stride;
        }

        double * _bz_restrict pa2 = a,
               * _bz_restrict pb2 = b;
        double * _bz_restrict paend2 = a + N * stride;
        double d2 = d;
        int n2 = N & 3;

        for (i=0; i < n2; ++i)
        {
            *pa2 += d2 * (*pb2);
            pa2 += stride;
            pb2 += stride;
        }

        while (pa2 != paend2)
        {
            pa2[0] += d2 * pb2[0];
            pa2[1] += d2 * pb2[1];
            pa2[2] += d2 * pb2[2];
            pa2[3] += d2 * pb2[3];
            pa2 += 4 * stride;
            pb2 += 4 * stride;
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    while, pointer increment, unroll=4, non-unit stride," << endl
         << "     constants loaded into temps" << endl;

    /*********************************************************************/

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 3;
        for (i=0; i < n1; ++i)
            a[i] += c2 * b[i];

        for (; i < N; i += 4)
        {
            t1 = a[i+4];
            a[i] += c2 * b[i];
            a[i+1] += c2 * b[i+1];
            t2 = b[i+4];
            a[i+2] += c2 * b[i+2];
            a[i+3] += c2 * b[i+3];
        }

        double d2 = d;
        int n2 = N & 3;
        for (i=0; i < n2; ++i)
            a[i] += d2 * b[i];

        for (; i < N; i += 4)
        {
            t1 = a[i+4];
            a[i] += d2 * b[i];
            a[i+1] += d2 * b[i+1];
            t2 = b[i+4];
            a[i+2] += d2 * b[i+2];
            a[i+3] += d2 * b[i+3];
        }
    }
    timer.stop();

    sink(t1);
    sink(t2);

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=4, unit stride, constants loaded into temps,"
         << "            prefetching"
         << endl;

    /********************************************************************/

    struct vectorPair {
        double a;
        double b;
    };
    vectorPair* v = new vectorPair[N];
    int N2 = 2*N;
    initialize(c, d, (double*)v, (double*)v, N2);

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        for (i=0; i < N; ++i)
            v[i].a += c * v[i].b;

        for (i=0; i < N; ++i)
            v[i].a += d * v[i].b;
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "   interlaced, for, indirection, unit stride" << endl;

    /*********************************************************************/

    initialize(c, d, (double*)v, (double*)v, N2);

    timer.start();
    for (iter=0; iter < iterations; ++iter)
    {
        double c2 = c;

        int n1 = N & 3;
        for (i=0; i < n1; ++i)
            v[i].a += c2 * v[i].b;

        for (; i < N; i += 4)
        {
            v[i].a += c2 * v[i].b;
            v[i+1].a += c2 * v[i+1].b;
            v[i+2].a += c2 * v[i+2].b;
            v[i+3].a += c2 * v[i+3].b;
        }

        double d2 = d;
        int n2 = N & 3;
        for (i=0; i < n2; ++i)
            v[i].a += d2 * v[i].b;

        for (; i < N; i += 4)
        {
            v[i].a += d2 * v[i].b;
            v[i+1].a += d2 * v[i+1].b;
            v[i+2].a += d2 * v[i+2].b;
            v[i+3].a += d2 * v[i+3].b;
        }
    }
    timer.stop();

    cout << setw(7) << setprecision(5) << (mflops/timer.elapsedSeconds())
         << "    for, unroll=4, unit stride, interlaced, " << endl
         << "\t\tconstants loaded into temps"
         << endl;

    delete [] v;

    /********************************************************************/

    delete [] a;
    delete [] b;
}

void initialize(double& c, double& d, double* a, double* b, int& N)
{
    for (int i=0; i < N; ++i)
    {
        a[i] = 1/7.;
        b[i] = 1/3.;
    }
    c = 0.398192839842;
    d = - c;
}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iomanip>
#include <iostream>
#include <functional>
#include <boost/math/tools/roots.hpp>

template <class T>
struct fn
{
    T operator()(const T& x )
    {
        return (x-2.)*(x-5.) ;
    }
};

template <class T>
struct tolerance
{
    bool operator()(const T& min, const T& max )
    {
        return (max - min) < 0.001 ;
    }
};

int main()
{

   fn<float> f ;
   tolerance<float> tol ;

   float min = 1 ;
   float max = 3 ;

   std::pair<float, float> r = boost::math::tools::bisect(f, min, max, tol );

   std::cout
      << " r " << std::setw(15) << std::fixed << std::setprecision(4) << r.first
      << " " << r.second
      << std::endl
      ;

    return 0 ;
}


#pragma once

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>

#include <boost/functional/hash.hpp>

#include <common/traits.hpp>
#include <common/types.hpp>

namespace vitamine
{
	template <typename T, typename Tag = void>
	struct Vector2XZ
	{
		using ComponentType = T;

		T x, z;

		Vector2XZ() noexcept = default;
		constexpr Vector2XZ(T x, T z) noexcept : x(x), z(z) {}

		[[nodiscard]]
		bool withinOrdered(Vector2XZ from, Vector2XZ to) const
		{
			assert(from.x <= to.x);
			assert(from.z <= to.z);
			return x >= from.x && x <= to.x
			    && z >= from.z && z <= to.z;
		}

		template <typename U = T>
		[[nodiscard]]
		constexpr T lengthSquared() const
		{
			auto x = static_cast<U>(this->x);
			auto z = static_cast<U>(this->z);
			return x * x + z * z;
		}

		template <typename F = Float64>
		[[nodiscard]]
		F length() const
		{
			using std::sqrt;
			return sqrt(static_cast<F>(lengthSquared()));
		}
	};

	template <typename T, typename Tag = void>
	struct Vector3XYZ
	{
		using ComponentType = T;

		T x, y, z;

		Vector3XYZ() noexcept = default;
		constexpr Vector3XYZ(T x, T y, T z) noexcept : x(x), y(y), z(z) {}

		[[nodiscard]]
		bool withinOrdered(Vector3XYZ from, Vector3XYZ to)
		{
			assert(from.x <= to.x);
			assert(from.y <= to.y);
			assert(from.z <= to.z);
			return x >= from.x && x <= to.x
			    && y >= from.y && y <= to.y
			    && z >= from.z && z <= to.z;
		}

		template <typename U = T>
		[[nodiscard]]
		constexpr T lengthSquared() const
		{
			auto x = static_cast<U>(this->x);
			auto y = static_cast<U>(this->y);
			auto z = static_cast<U>(this->z);
			return x * x + y * y + z * z;
		}

		template <typename F = Float64>
		[[nodiscard]]
		F length() const
		{
			using std::sqrt;
			return sqrt(static_cast<F>(lengthSquared()));
		}
	};

	static_assert(std::is_trivial_v<Vector2XZ <Int>>);
	static_assert(std::is_trivial_v<Vector3XYZ<Int>>);

	static_assert(sizeof(Vector2XZ <Int>) == 2 * sizeof(Int));
	static_assert(sizeof(Vector3XYZ<Int>) == 3 * sizeof(Int));

	template <typename T, typename Tag>
	constexpr bool operator==(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return a.x == b.x && a.z == b.z;
	}

	template <typename T, typename Tag>
	constexpr bool operator==(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return a.x == b.x && a.y == b.y && a.z == b.z;
	}

	template <typename T, typename Tag>
	constexpr bool operator!=(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return !(a == b);
	}

	template <typename T, typename Tag>
	constexpr bool operator!=(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return !(a == b);
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag>& operator+=(Vector2XZ<T, Tag>& a, Vector2XZ<T, Tag> b)
	{
		a.x += b.x;
		a.z += b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag>& operator+=(Vector3XYZ<T, Tag>& a, Vector3XYZ<T, Tag> b)
	{
		a.x += b.x;
		a.y += b.y;
		a.z += b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag>& operator-=(Vector2XZ<T, Tag>& a, Vector2XZ<T, Tag> b)
	{
		a.x -= b.x;
		a.z -= b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag>& operator-=(Vector3XYZ<T, Tag>& a, Vector3XYZ<T, Tag> b)
	{
		a.x -= b.x;
		a.y -= b.y;
		a.z -= b.z;
		return a;
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator+(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return {a.x + b.x, a.z + b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator+(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return {a.x + b.x, a.y + b.y, a.z + b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator-(Vector2XZ<T, Tag> a, Vector2XZ<T, Tag> b)
	{
		return {a.x - b.x, a.z - b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator-(Vector3XYZ<T, Tag> a, Vector3XYZ<T, Tag> b)
	{
		return {a.x - b.x, a.y - b.y, a.z - b.z};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator*(Vector2XZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x * n, v.z * n};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator*(Vector3XYZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x * n, v.y * n, v.z * n};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator*(typename Identity<T>::Type n, Vector2XZ<T, Tag> v)
	{
		return {n * v.x, n * v.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator*(typename Identity<T>::Type n, Vector3XYZ<T, Tag> v)
	{
		return {n * v.x, n * v.y, n * v.z};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator/(Vector2XZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x / n, v.z / n};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator/(Vector3XYZ<T, Tag> v, typename Identity<T>::Type n)
	{
		return {v.x / n, v.y / n, v.z / n};
	}

	template <typename T, typename Tag>
	constexpr Vector2XZ<T, Tag> operator/(typename Identity<T>::Type n, Vector2XZ<T, Tag> v)
	{
		return {n / v.x, n / v.z};
	}

	template <typename T, typename Tag>
	constexpr Vector3XYZ<T, Tag> operator/(typename Identity<T>::Type n, Vector3XYZ<T, Tag> v)
	{
		return {n / v.x, n / v.y, n / v.z};
	}

	template <typename T, typename Tag>
	std::string toString(Vector2XZ<T, Tag> v)
	{
		return '(' + std::to_string(v.x) + ',' + ' ' + std::to_string(v.z) + ')';
	}

	template <typename T, typename Tag>
	std::string toString(Vector3XYZ<T, Tag> v)
	{
		return '(' + std::to_string(v.x) + ',' + ' ' + std::to_string(v.y) + ',' + ' ' + std::to_string(v.z) + ')';
	}
}

namespace std
{
	template <typename T, typename Tag>
	struct hash<vitamine::Vector2XZ<T, Tag>>
	{
		std::size_t operator()(vitamine::Vector2XZ<T, Tag> v) const noexcept
		{
			return boost::hash_value(std::tie(v.x, v.z));
		}
	};

	template <typename T, typename Tag>
	struct hash<vitamine::Vector3XYZ<T, Tag>>
	{
		std::size_t operator()(vitamine::Vector3XYZ<T, Tag> v) const noexcept
		{
			return boost::hash_value(std::tie(v.x, v.y, v.z));
		}
	};

	template <typename T, typename Tag>
	struct less<vitamine::Vector2XZ<T, Tag>>
	{
		constexpr bool operator()(vitamine::Vector2XZ<T, Tag> a, vitamine::Vector2XZ<T, Tag> b) const noexcept
		{
			if(a.x < b.x)
				return true;

			if(a.x > b.x)
				return false;

			return a.z < b.z;
		}
	};

	template <typename T, typename Tag>
	struct less<vitamine::Vector3XYZ<T, Tag>>
	{
		constexpr bool operator()(vitamine::Vector3XYZ<T, Tag> a, vitamine::Vector3XYZ<T, Tag> b) const noexcept
		{
			if(a.x < b.x)
				return true;

			if(a.x > b.x)
				return false;

			if(a.y < b.y)
				return true;

			if(a.y > b.y)
				return false;

			return a.z < b.z;
		}
	};
}


// Author(s)     : Michael Hemmer <mhemmer@uni-mainz.de>

/*! \file CGAL/Residue.C
  test for number type modul
*/

#include <CGAL/basic.h>
#include <cassert>
#include <CGAL/Residue.h>
#include <CGAL/Modular_traits.h>
#include <CGAL/Sqrt_extension.h>
#include <CGAL/Polynomial.h>
#include <CGAL/Lazy_exact_nt.h>
//#include <CGAL/MP_Float.h>

#ifdef CGAL_USE_LEDA
#include <CGAL/leda_integer.h>
#include <CGAL/leda_rational.h>
#endif // CGAL_USE_LEDA

#ifdef CGAL_USE_CORE
#include <CGAL/CORE_BigInt.h>
#endif // CGAL_USE_CORE

#ifdef CGAL_USE_GMP
#include <CGAL/Gmpz.h>
#endif

#ifdef CGAL_USE_GMPXX
#include <CGAL/mpz_class.h>
#endif // CGAL_USE_GMP

#include <cstdlib>

#include <boost/type_traits.hpp>

template <class TESTT>
void test_modular_traits(){

        typedef CGAL::Residue Residue;
        typedef CGAL::Modular_traits<TESTT> MT;
        typedef typename MT::Residue_type Residue_type;
        typedef typename MT::Modular_image Modular_image;
        typedef typename MT::Modular_image_representative Modular_image_representative;
        typedef typename MT::Is_modularizable Is_modularizable;
        typedef typename MT::NT NT;

        assert(
            !(::boost::is_same<CGAL::Null_functor,Modular_image>::value));
        assert(
            !(::boost::is_same<CGAL::Null_functor,Modular_image_representative>::value));
        assert(
            (::boost::is_same<CGAL::Tag_true,Is_modularizable>::value));
        assert(
            (::boost::is_same<TESTT,NT>::value));

        Residue::set_current_prime(7);
        Modular_image modular_image;
        assert(modular_image(TESTT(10)+TESTT(10)) == Residue_type(-1));
        assert(modular_image(TESTT(2) *TESTT(10)) == Residue_type(-1));
        assert(modular_image(TESTT(20)) == Residue_type(-1));
        assert(modular_image(TESTT(20)) == Residue_type(6));
        assert(modular_image(TESTT(21)) == Residue_type(0));
        assert(modular_image(TESTT(22)) == Residue_type(1));
        assert(modular_image(TESTT(777777722)) == Residue_type(1));

        Modular_image_representative modular_image_representative;
        assert(modular_image_representative(modular_image(TESTT(20)))
            == TESTT(-1));
}

int main()
{
  // Enforce IEEE double precision and rounding mode to nearest
  CGAL::Protect_FPU_rounding<true> pfr(CGAL_FE_TONEAREST);

    test_modular_traits<int>();

#ifdef CGAL_USE_LEDA
    test_modular_traits<leda::integer>();
    test_modular_traits<CGAL::Polynomial< leda::integer > >();
    test_modular_traits<CGAL::Lazy_exact_nt< leda::integer > >();
    test_modular_traits<CGAL::Sqrt_extension< leda::integer , leda::integer > >();
#endif
#ifdef CGAL_USE_CORE
    test_modular_traits<CORE::BigInt>();
    test_modular_traits<CGAL::Polynomial< CORE::BigInt > >();
    test_modular_traits<CGAL::Lazy_exact_nt< CORE::BigInt > >();
    test_modular_traits<CGAL::Sqrt_extension< CORE::BigInt , CORE::BigInt > >();
#endif

#ifdef CGAL_USE_GMP
    test_modular_traits<CGAL::Gmpz>();
#endif

#ifdef CGAL_USE_GMPXX
    test_modular_traits< mpz_class >();
#endif

    // test Sqrt_extension
    test_modular_traits<CGAL::Sqrt_extension< int , int > >();
    assert(
        (!CGAL::Modular_traits<CGAL::Sqrt_extension<double,double> >
            ::Is_modularizable::value));

    // test Polynomial
    test_modular_traits<CGAL::Polynomial< int > >();
    assert(
        !CGAL::Modular_traits<CGAL::Polynomial<double> >
        ::Is_modularizable::value);

    // test_modular_traits<CGAL::MP_Float >();

    test_modular_traits< CGAL::Lazy_exact_nt<int> >();
    assert(
        !CGAL::Modular_traits<CGAL::Lazy_exact_nt< double > >
        ::Is_modularizable::value);

}


#include <iostream>
#include <vector>
#include <queue>
#include <limits>
#include <cmath>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;

// Custom edge adder class, highly recommended
class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

struct edge {
  int u;
  int v;
  int length;
  int width;
};

// Strategy:
// - Find all edges that are part of some shortest path
// - Run a max-flow algorithm on those edges
void solve() {
  int n, m, s, f;
  cin >> n >> m >> s >> f;

  // Dijkstra Graph
  weighted_graph G(n);

  // Read neighbors
  vector<edge> edges;
  edges.reserve(m);
  int u, v, length, width;
  for (int i = 0; i < m; ++i) {
    cin >> u >> v >> width >> length;
    boost::add_edge(u, v, length, G);
    edges.push_back({u, v, length, width});
  }

  // Run dijkstra
  std::vector<int> d(n);
  boost::dijkstra_shortest_paths(G, s, boost::distance_map(boost::make_iterator_property_map( d.begin(), boost::get(boost::vertex_index, G))));

  // Flow graph
  graph gFlow(n);
  edge_adder adder(gFlow);

  // Find edges on a shortest path
  vector<edge> shortestPathEdges;
  for (auto e : edges) {
    int diff = d[e.v] - d[e.u];
    if (e.length == diff) {
      adder.add_edge(e.u, e.v, e.width);
    }
    else if (e.length == -diff) {
      adder.add_edge(e.v, e.u, e.width);
    }
  }

  // Compute flow
  long flow = boost::push_relabel_max_flow(gFlow, s, f);
  cout << flow << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  while (t--)
  {
    solve();
  }
  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/io/wkt/read.hpp>

template <typename Geometry>
void test_circle(std::string const& wkt_geometry, bool expected)
{
    typedef bg::model::nsphere<bg::model::d2::point_xy<double>, double> circle_type;
    circle_type circle;
    bg::assign(circle, 1.0, 1.0, 3.0);

    Geometry geometry;
    bg::read_wkt(wkt_geometry, geometry);

    /* todo: fix
    bool detected = bg::within(geometry, circle);

    BOOST_CHECK_MESSAGE(detected == expected,
        "within: " << wkt_geometry
        << " in circle (1,1) with radius 3"
        << " -> Expected: " << expected
        << " detected: " << detected);
    */
}

template <typename P>
void test_circles()
{
    test_circle<P>("POINT(2 1)", true);
    test_circle<P>("POINT(12 1)", false);

    test_circle<bg::model::linestring<P> >("LINESTRING(1 1,2 1,2 2)", true);
    test_circle<bg::model::linestring<P> >("LINESTRING(1 1,2 1,2 2,10 10)", false);
}

int test_main( int , char* [] )
{
    test_circles<bg::model::d2::point_xy<double> >();

    return 0;
}


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <range/v3/all.hpp>
#include "combinations_algo.h"
#include "matrix.h"
struct Combinations
{
  FixedMatrix<int, 8, 8> networks_;
  std::array<int, 8> a = { 0, 1, 2, 3, 4, 5, 6, 7 };
  int min = std::numeric_limits<int>::max();
  int min_distance() noexcept
  {
    auto f = [this](auto b, auto e) {
      int s = 0;
      for (; b != e - 1; ++b) {
        s += networks_(*b, *(b + 1));
      }
      min = std::min(s, min);
      return false;
    };
    for_each_reversible_permutation(a.begin(), a.end(), a.end(), f);
    return min;
  }
};
int main(int argc, char **argv)
{
  if (argc > 1) {
    Combinations com;
    std::ifstream ifs(argv[1]);
    std::string s;
    for (int i = 0; i < 7; ++i) {
      int row = i;

      int col = i + 1;
      for (; col < 8; ++col) {
        std::getline(ifs, s);
        std::istringstream iss(s);
        std::string ignore;
        int value;
        iss >> ignore >> ignore >> ignore >> ignore >> value;
        com.networks_(row, col) = value;
        com.networks_(col, row) = value;
      }
    }
    std::cout << com.networks_ << std::endl;
    std::cout << com.min_distance() << std::endl;
  }
}


#include <igl/eigs.h>
#include <igl/cotmatrix.h>
#include <igl/massmatrix.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/parula.h>
#include <igl/read_triangle_mesh.h>
#include <Eigen/Sparse>
#include <iostream>
#include <queue>
#include "tutorial_shared_path.h"

Eigen::MatrixXd V,U;
Eigen::MatrixXi F;
int c=0;
double bbd = 1;
bool twod = 0;
int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  VectorXd D;
  if(!read_triangle_mesh(
     argc>1?argv[1]: TUTORIAL_SHARED_PATH "/beetle.off",V,F))
  {
    cout<<"failed to load mesh"<<endl;
  }
  twod = V.col(2).minCoeff()==V.col(2).maxCoeff();
  bbd = (V.colwise().maxCoeff()-V.colwise().minCoeff()).norm();
  SparseMatrix<double> L,M;
  cotmatrix(V,F,L);
  L = (-L).eval();
  massmatrix(V,F,MASSMATRIX_TYPE_DEFAULT,M);
  const size_t k = 5;
  if(!eigs(L,M,k+1,EIGS_TYPE_SM,U,D))
  {
    cout<<"failed."<<endl;
  }
  // Normalize
  U = ((U.array()-U.minCoeff())/(U.maxCoeff()-U.minCoeff())).eval();

  igl::opengl::glfw::Viewer viewer;
  viewer.callback_key_down = [&](igl::opengl::glfw::Viewer & viewer,unsigned char key,int)->bool
  {
    switch(key)
    {
      default:
        return false;
      case ' ':
      {
        U = U.rightCols(k).eval();
        // Rescale eigen vectors for visualization
        VectorXd Z =
          bbd*0.5*U.col(c);
        Eigen::MatrixXd C;
        igl::parula(U.col(c).eval(),false,C);
        c = (c+1)%U.cols();
        if(twod)
        {
          V.col(2) = Z;
        }
        viewer.data().set_mesh(V,F);
        viewer.data().compute_normals();
        viewer.data().set_colors(C);
        return true;
      }
    }
  };
  viewer.callback_key_down(viewer,' ',0);
  viewer.data().show_lines = false;
  std::cout<<
R"(
  [space] Cycle through eigen modes
)";
  viewer.launch();
}


/*
 * 这是 TJU Robomasters 上位机源码，未经管理层允许严禁传播给其他人（包括队内以及队外）
 *
 * 该文件包含各种预测模型，并封装到了以抽象类Predictor类为基类的类中
 */

#pragma once

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "util.hpp"
#include <atomic>
#include "KalmanFilter.hpp"
using namespace cv;

typedef struct _prd_path_pt{

    Point3f worldPosition;
    Point2f targetPTZAngle;
    Point2f selfWorldPosition;
    double duration;

    _prd_path_pt(Point3d _worldp,Point2f _tgtAngle,Point2f _sfworldp,float _dur)
    {
        worldPosition = _worldp;
        targetPTZAngle = _tgtAngle;
        selfWorldPosition = _sfworldp;
        duration = _dur;
    }

    _prd_path_pt()
    { }

}PredictionPathPoint;

class Predictor
{
public:
    // 添加一个轨迹点，该函数应该能够自主甄别轨迹点是否与之前追踪的目标一样，如果不是
    // 预测器自动清空历史数据重新开始预测
    virtual void AddPredictPoint(PredictionPathPoint ppp) = 0;

    // 预测一段时间之后目标的位置
    virtual void Predict(double prdTime) = 0;

    // 主动清除历史记录
    virtual void ClearHistory() = 0;
};
// 线性预测器，由历史数据给出线性的预测（假设目标匀速直线运动）
class KFCPredictor : public Predictor
{
public:
    static const int HistorySize = 6;
    Mat P, Q, H, R, F, K, x_;
    int xstate = 0;
    PredictionPathPoint present;
    PredictionPathPoint lastpoint;

    KFCPredictor()
    {
        Mat P_in = Mat::eye(6,6,CV_32FC1);
        Mat Q_in = Mat::eye(6,6,CV_32FC1);
        Mat H_in = Mat::eye(6,6,CV_32FC1);
        Mat R_in = Mat::eye(6,6,CV_32FC1);
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    KFCPredictor(Mat P_in, Mat Q_in, Mat H_in, Mat R_in)
    {
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    void AddPredictPoint(PredictionPathPoint ppp)
    {
        if (Length(ppp.targetPTZAngle - lastpoint.targetPTZAngle) > 10)
            ClearHistory();
        present = ppp;
    }

    void ClearHistory()
    {
        xstate = 0;
        P = Mat::eye(6,6,CV_32FC1);
    };

    void FirstFind()
    {
        p_tx_old = present.worldPosition.x;
        p_ty_old = present.worldPosition.y;
        p_tz_old = present.worldPosition.z;
        lastpoint = present;
    }

    void FirstSetFilter()
    {
        //将视觉根据图像计算出的装甲板在相机坐标系下的位置传给预测类中的worldposition
        //以当前的计算作为根据上一次计算进行预测的观测值
        double t = present.duration;
        float v_tx_now = (present.worldPosition.x - p_tx_old)/t;
        float v_ty_now =(present.worldPosition.y - p_ty_old)/t;
        float v_tz_now = (present.worldPosition.z - p_tz_old)/t;
        x_ = (Mat_<float>(6,1) <<
                   present.worldPosition.x, present.worldPosition.y, present.worldPosition.z,
                   v_tx_now, v_ty_now, v_tz_now
                   );
        xstate = 1;
        p_tx_old = present.worldPosition.x;
        p_ty_old = present.worldPosition.y;
        p_tz_old = present.worldPosition.z;
        lastpoint = present;
    }
    void ContinueSetFilter()
    {
        double t = present.duration;
        float v_tx_now = (present.worldPosition.x - p_tx_old)/t;
        float v_ty_now = (present.worldPosition.y - p_ty_old)/t;
        float v_tz_now = (present.worldPosition.z - p_tz_old)/t;

        Mat z = (Mat_<float>(6,1) <<
                   present.worldPosition.x, present.worldPosition.y, present.worldPosition.z,
                   v_tx_now, v_ty_now, v_tz_now
                   );

        Predict(t);
        update(z);

        p_tx_old = x_.at<float>(0, 0);
        p_ty_old = x_.at<float>(1, 0);
        p_tz_old = x_.at<float>(2, 0);
        lastpoint = present;
    }

    void Predict(double prdTime)
    {
        double t = prdTime;
        F = (Mat_<float>(6,6) <<
                   1.0, 0.0, 0.0, t, 0.0, 0.0,
                   0.0, 1.0, 0.0, 0.0, t, 0.0,
                   0.0, 0.0, 1.0, 0.0, 0.0, t,
                   0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
                   0.0, 0.0, 0.0, 0.0, 1.0,0.0,
                   0.0, 0.0, 0.0, 0.0, 0.0, 1.0
                   );
        //起
        x_ = F * x_;
        // return Point3f(x_.at<float>(0,0),x_.at<float>(0,1),x_.at<float>(0,2));
    }

    void update(Mat z)
    {
        //按
        P = F*P*F.t() + Q;
        //顿
        Mat S = H*P*H.t() + R;
        K = P*H.t()*S.inv();
        //挫
        Mat y = z - H*x_;
        x_ = x_ + (K*y);
        //下笔风雷
        Mat I = Mat::eye(6,6,CV_32FC1);
        P = (I - K*H)*P;
    }
protected:
    float p_tx_old;                  //位置保留量
    float p_ty_old;
    float p_tz_old;
};

class AnglePredictor : public Predictor
{
public:

    Mat P, Q, H, R, F, K, x_;
    int xstate = 0;
    PredictionPathPoint present;
    PredictionPathPoint lastpoint;

    AnglePredictor()
    {
        Mat P_in = Mat::eye(4,4,CV_32FC1);
        Mat Q_in = (Mat_<float>(4,4) <<
                   5,0,0,0,
                   0,1,0,0,
                   0,0,5,0,
                   0,0,0,1
                   );
        Mat H_in = (Mat_<float>(2,4) <<
                   1,0,0,0,
                   0,1,0,0
                   );
        Mat R_in = (Mat_<float>(2,2) <<
                   200,0,
                   0,200
                   );
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    AnglePredictor(Mat P_in, Mat Q_in, Mat H_in, Mat R_in)
    {
        P = P_in;
        Q = Q_in;
        H = H_in;
        R = R_in;
        lastpoint.targetPTZAngle=Point2f(0,0);
    }

    void AddPredictPoint(PredictionPathPoint ppp)
    {
        if (Length(ppp.targetPTZAngle - lastpoint.targetPTZAngle) > 10)
            ClearHistory();
        present = ppp;
    }

    void ClearHistory()
    {
        xstate = 0;
        P = Mat::eye(4,4,CV_32FC1);
    };

    void FirstFind()
    {

        lastpoint = present;
    }

    void FirstSetFilter()
    {
        double t = present.duration;
        float vx = (present.targetPTZAngle.x - lastpoint.targetPTZAngle.x)/t;
        float vy =(present.targetPTZAngle.y - lastpoint.targetPTZAngle.y)/t;

        x_ = (Mat_<float>(4,1) <<
                   present.targetPTZAngle.x, present.targetPTZAngle.y,
                   vx, vy
                   );
        xstate = 1;

        lastpoint = present;
    }
    void ContinueSetFilter()
    {
        double t = present.duration;
        float vx = (present.targetPTZAngle.x - lastpoint.targetPTZAngle.x)/t;
        float vy =(present.targetPTZAngle.y - lastpoint.targetPTZAngle.y)/t;
        Mat z = (Mat_<float>(2,1) <<
                   present.targetPTZAngle.x, present.targetPTZAngle.y
                   );

        Predict(t);
        update(z);
        // cout << "presemtangle: " << present.targetPTZAngle << " lastangle: " << lastpoint.targetPTZAngle << endl;
        // cout << "speedx: " << (present.targetPTZAngle.x-lastpoint.targetPTZAngle.x)/present.duration << endl;
        // cout << "speedy: " << (present.targetPTZAngle.y-lastpoint.targetPTZAngle.y)/present.duration << endl;
        lastpoint = present;
    }

    void Predict(double prdTime)
    {
        double t = prdTime;
        F = (Mat_<float>(4,4) <<
                   1.0, 0.0, t, 0.0,
                   0.0, 1.0, 0.0, t,
                   0.0, 0.0, 1.0, 0.0,
                   0.0, 0.0, 0.0, 1.0
                   );
        //起
        x_ = F * x_;

        //return Point2f(myx_.at<float>(0,0),myx_.at<float>(0,1));
    }
    Point2f PredictReal(double prdTime)
    {
        double t = prdTime;
        F = (Mat_<float>(4,4) <<
                   1.0, 0.0, t, 0.0,
                   0.0, 1.0, 0.0, t,
                   0.0, 0.0, 1.0, 0.0,
                   0.0, 0.0, 0.0, 1.0
                   );
        //起

        Mat myx_ = F * x_;
        return Point2f(myx_.at<float>(0,0),myx_.at<float>(0,1));
    }

    void update(Mat z)
    {
        //按
        P = F*P*F.t() + Q;
        //顿
        Mat S = H*P*H.t() + R;
        K = P*H.t()*S.inv();
        //挫
        Mat y = z - H*x_;
        x_ = x_ + (K*y);
        //下笔风雷
        Mat I = Mat::eye(4,4,CV_32FC1);
        P = (I - K*H)*P;
    }
};

class KalmanPredictor{
public:
KalmanPredictor(){

	Eigen::MatrixXd A(stateSize, stateSize);
	A << 1, 0, 1, 0,
		0, 1, 0, 1,
		0, 0, 1, 0,
		0, 0, 0, 1 ;

	Eigen::MatrixXd H(measureSize, stateSize);
	H << 2, 0, 0, 0,
		0, 2, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1;

	Eigen::MatrixXd P(stateSize, stateSize);
	P << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1;

	Eigen::MatrixXd Q(stateSize, stateSize);
	Q << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1;

	Eigen::MatrixXd R(measureSize, measureSize);
	R << 900, 0, 0, 0,
		0, 900, 0, 0,
		0, 0, 700, 0,
		0, 0, 0, 700;

	KF.init(stateSize, measureSize, A, P, R, Q, H);

	x.resize(stateSize);
	x << 0, 0, 0, 0;
}
public:
	Eigen::MatrixXd F;
	Eigen::VectorXd x;
    cv::Point2f predict(float yaw, float pitch,float distance, float deltatime,int bulletSpeed) {
	    return kalmanPredict(yaw, pitch, (double)deltatime,distance/bulletSpeed * 0.001);
	}

	cv::Point2f kalmanPredict(float yaw, float pitch, double deltatime,float time){
	if (abs(x(0) - yaw) > 200) {
		x(0) = (double)yaw;
		yawSpeed = 0;
		x(2) = yawSpeed;
	}
	if (abs(x(2) - pitch) > 200) {
		x(1) = (double)pitch;
		pitchSpeed = 0;
		x(3) = pitchSpeed;
	}

	yawSpeed = (yaw - lastYaw) / deltatime;
	pitchSpeed = (pitch - lastPitch) / deltatime;
	if(abs(yawSpeed) > 50){
		yawSpeed = 0;
	}
	if(abs(pitchSpeed > 20)){
		pitchSpeed = 0;
	}
	Eigen::VectorXd z(measureSize);
	Eigen::VectorXd output;
	F = Eigen::MatrixXd(4, 4);
	cout << "hello3"<<endl;
    F << 1.0,0.0,(double)deltatime,0.0,
        0.0,1.0,0.0,(double)deltatime,
        0.0,0.0,1.0,0.0,
        0.0,0.0,0.0,1.0;
	z << (double)yaw, (double)pitch, (double)yawSpeed, (double)pitchSpeed;
	KF.predict(x,F);
	KF.update(x, z);
	lastYaw = yaw;
	lastPitch = pitch;
	//cout << " X:" << x(0) << " v:" << x(2) << endl;
	double predictYaw = x(0) + (time + 0.35) * x(2);
	double predictPitch = x(1) + (time + 0.35) * x(3);

	return cv::Point2f((float)predictYaw, (float)predictPitch);
}
private:
	EigenKalman::KalmanFilter KF;
	double lastYaw;
	double lastPitch;
	double yawSpeed = 0;
	double pitchSpeed = 0;
	int measureSize = 4;
	int stateSize = 4;
public:
	atomic<int> shootSpeed;//发射速度 单位;米/秒
};


#include <iostream>
#include <boost/concept_check.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <random>
#include <chrono>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>

using namespace std;
using namespace g2o;

class hello_vertex: public BaseVertex<1, Eigen::Matrix<double,1,1>> {//数据类型要以Eigen::Matrix给出,因为需要转置操作
          /**
         * update the position of the node from the parameters in v.
         * Implement in your class!
         */
	//optimizable_graph.h
        virtual void oplusImpl(const double* v) {
		cout << "v: " << *v << endl;
		_estimate(0,0) += *v;				//_estimate的类型即为继承模板时传入的类型
		cout << "_estimate: " << _estimate(0,0) << endl;
	}

        //! sets the node to the origin (used in the multilevel stuff)
        //optimizable_graph.h
        virtual void setToOriginImpl() {
		cout << "setToOriginImpl " << _estimate(0,0)  << endl;
		_estimate(0,0) = 0;
	}

	//! read the vertex from a stream, i.e., the internal state of the vertex
        virtual bool read(std::istream& is) {}
        //! write the vertex to a stream
        virtual bool write(std::ostream& os) const {}
};

class hello_edge: public BaseUnaryEdge<1, double, hello_vertex> {
	// computes the error of the edge and stores it in an internal structure
	//optimizable_graph.h
        virtual void computeError() {
		const hello_vertex* v = static_cast<const hello_vertex*> (_vertices[0]);
		const Eigen::Matrix<double,1,1> est = v->estimate();
		//typedef Eigen::Matrix<double, D, 1, Eigen::ColMajor> ErrorVector;
		//_error = est - (Eigen::Matrix<double, 1, 1> )(10);
		cout << "computeError: " ;
		//_error(0,0) = _measurement - est(0,0);
		//此处误差的定义影响后续解析求导时Jacob矩阵前边是否添加负号
		_error(0,0) = -_measurement + (est(0,0) + 1)*(est(0,0) + 1);
		cout << _error(0,0) << endl;
	}

	virtual void linearizeOplus() {
		const hello_vertex* v = static_cast<const hello_vertex*> (_vertices[0]);
		const Eigen::Matrix<double,1,1> est = v->estimate();
		//需要提供负梯度,与Ceres相同
		//之所以是负梯度,因为err = _measurement - f(_estimate)
		//因此为负的
		_jacobianOplusXi(0,0) = 2 * (est(0,0) + 1);
		cout << "_jacobianOplusXi: " << _jacobianOplusXi << endl;
	}

	//! read the vertex from a stream, i.e., the internal state of the vertex
        virtual bool read(std::istream& is) {}
        //! write the vertex to a stream
        virtual bool write(std::ostream& os) const {}
};
/*
 *  default_random_engine generator;
 *  normal_distribution<double> distribution(0.0,0.5);
 *  distribution(generator);
 */

int main(int argc, char** argv)
{

	// 构建图优化，先设定g2o
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<1,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
	Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
	// 梯度下降方法，从GN, LM, DogLeg 中选
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
	// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
	// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm( solver );   // 设置求解器
	optimizer.setVerbose( true );       // 打开调试输出

	// 往图中增加顶点
	hello_vertex* v = new hello_vertex();
	v->setEstimate((Eigen::Matrix<double,1,1>) 100);
	v->setId(0);
	optimizer.addVertex( v );

	//加边
	hello_edge* edge = new hello_edge;
	edge->setId(0);
	edge->setVertex( 0, v );                // 设置连接的顶点
	edge->setMeasurement(66.6);      // 观测数值
	edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*0.25); // 信息矩阵：协方差矩阵之逆
	optimizer.addEdge( edge );

	// 执行优化
	cout<<"start optimization"<<endl;
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	optimizer.initializeOptimization();
	optimizer.optimize(100);
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
	cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

	// 输出优化值
	Eigen::Matrix<double,1,1> abc_estimate = v->estimate();
	cout<<"estimated value: "<<abc_estimate.transpose()<<endl;

	return 0;
}


#pragma once

#include <random>

#include <boost/random.hpp>
#include <boost/random/random_device.hpp>

#include "ContainerByBitness.hh"
#include "RandomGenerator.hh"
#include "Typedefs.hh"

namespace cml {

template <Uint32 numberBitness, typename ResultType = typename ContainerByBitness<numberBitness>::Type>
class Mt19937RandomGenerator : public RandomGenerator<ResultType> {
public:
    static constexpr Uint32 bitness = numberBitness;

    using Base = RandomGenerator<ResultType>;
    using typename Base::Result;
    using Engine = boost::random::independent_bits_engine<std::mt19937, bitness, Result>;

    Mt19937RandomGenerator() = default;
    explicit Mt19937RandomGenerator(const typename Engine::result_type& seed) : m_randomEngine(seed) {}

    Result random() override
    {
        return m_randomEngine();
    }

    Result random(Result min, Result max) override
    {
        boost::random::uniform_int_distribution<Result> uid{ min, max };
        return uid(m_randomEngine);
    }

private:
    Engine m_randomEngine{ boost::random::random_device{}() };
};

} // namespace cml


/*
 * MedianFillFilter.hpp
 *
 *  Created on: September 7, 2020
 *      Author: Magnus Gärtner
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <Eigen/Core>
#include <string>

#include <filters/filter_base.hpp>
#include <grid_map_core/GridMap.hpp>
#include <grid_map_core/TypeDefs.hpp>
#include <opencv2/core.hpp>

namespace grid_map {

/*!
 * Uses std::nth_element to fill holes in the input layer by the median of the surrounding values. The result is put into the output_layer.
 * Note: Only values for which the fill_layer is true will be filled. The fill_layer is auto computed if not present in the input.
 */
class MedianFillFilter : public filters::FilterBase<GridMap> {
 public:
  /*!
   * Constructor
   */
  MedianFillFilter();

  /*!
   * Destructor.
   */
  ~MedianFillFilter() override;

  /*!
   * Configures the filter from parameters on the Parameter Server
   */
  bool configure() override;

  /*!
   * Adds a new output layer to the map.
   * Uses the Boost accumulator median in the input layer.
   * Saves the filter output in mapOut[output_layer].
   * @param mapIn grid map containing input layer
   * @param mapOut grid map containing mapIn and median filtered input layer.
   */
  bool update(const GridMap& mapIn, GridMap& mapOut) override;

 protected:
  /*!
   * Returns the median of the values in inputData in the neighbourhood around the centerIndex. The size of the quadratic neighbourhood is
   * specified by radiusInPixels. If the number of values is even the "lower center" value is taken, eg with four values the second lowest
   * is taken as median.
   * @param inputMap The data layer to compute a local median.
   * @param centerIndex The center cell of the neighbourhood.
   * @param radiusInPixels The maximum L_inf distance from index.
   * @param bufferSize The buffer size of the input
   * @return The median of finites in the specified neighbourhood.
   */
  static float getMedian(Eigen::Ref<const Matrix> inputMap, const Index& centerIndex, size_t radiusInPixels, Size bufferSize);

  /**
   * Computes a mask of which cells to fill-in based on the validity of input cells. I.e small holes between (and including) valid cells are
   * marked to be filled.
   *
   * @remark The returned fill_mask is also added as layer to the output to be reused in following iterations of this filter.
   *         If debug is enabled, an intermediate mask is added as layer to the output grid map.
   * @param inputMap The input layer, used to check which cells contain valid values.
   * @param mapOut The output GridMap will contain the additional fill_mask layer afterwards.
   * @return An eigen mask indicating which cells should be filled by the median filter.
   */
  Eigen::MatrixXf computeAndAddFillMask(const Eigen::MatrixXf& inputMap, GridMap& mapOut);

  /**
   * Remove sparse valid regions by morphological opening.
   * @remark Check https://docs.opencv.org/master/d9/d61/tutorial_py_morphological_ops.html
   * for more information about the opening operation.
   * @param inputMask Initial mask possibly containing also sparse valid regions that will be removed.
   * @return An opencv mask of the same size as input mask with small sparse valid regions removed.
   */
  static cv::Mat_<bool> cleanedMask(const cv::Mat_<bool>& inputMask);

  /**
   * Performs morphological closing on a boolean cv matrix mask.
   * @param [in] isValidMask A 2d mask where holes up to a certain size will be filled.
   * @param [in] numDilationClosingIterations Algorithm specific parameter. Higher means that bigger holes will still be filled.
   * @return A mask of the same size as isValidMask but with small holes filled.
   */
  static cv::Mat_<bool> fillHoles(const cv::Mat_<bool>& isValidMask, size_t numDilationClosingIterations);

  /**
   * Adds a float cv matrix as layer to a given map.
   * @param [in, out] gridMap The map to add the layer.
   * @param [in] cvLayer The cv matrix to add.
   * @param [in] layerName The layer name
   */
  static void addCvMatAsLayer(GridMap& gridMap, const cv::Mat& cvLayer, const std::string& layerName);

 private:
  //! Median filtering radius of NaN values in the input.
  double fillHoleRadius_;

  //! Median filtering radius for existing values in the input.
  double existingValueRadius_;

  //! Flag indicating whether to also filter finite values.
  bool filterExistingValues_;

  //! Number of erode-dilate iterations to calculate mask. Higher means that bigger holes will still be filled.
  int numErodeDilationIterations_;

  //! Input layer name.
  std::string inputLayer_;

  //! Output layer name.
  std::string outputLayer_;

  //! Layer containing indicating which areas to fill, will be computed if not present.
  std::string fillMaskLayer_ = "should_fill";

  //! Layer used to visualize the intermediate, sparse outlier removed fill mask.
  std::string debugInfillMaskLayer_ = "debug_infill_mask";

  //! If set, the filtered grid_map is augmented with additional debug layers.
  bool debug_;
};

}  // namespace grid_map


// g++ -O3 -std=c++17 -fopenmp stifness_assembly.cpp -I"/usr/include/tbb"
// -I"/usr/include/eigen3" && ./a.out

// // #include <tbb/task_scheduler_init.h>
// #include <tbb/tbb.h>

// clang-format off
// Eigen::initParallel(); //TODO try this

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <omp.h>
#include <tbb/tbb.h>

int main() {
  double t1 = omp_get_wtime();

  auto const n = 10000;   // number of nodes
  auto const n_el = 20; // number of elements

  Eigen::MatrixXd Kdense = Eigen::MatrixXd::Zero(n, n); // global stiffness matrix
  Eigen::MatrixXd Ke = Eigen::MatrixXd::Ones(n_el, n_el); // elemental stiffness
#pragma omp declare reduction(+ : Eigen::MatrixXd : omp_out =  omp_out + omp_in) \
                initializer(omp_priv=Eigen::MatrixXd::Zero(n,n))

#pragma omp parallel for reduction(+ : Kdense)
  for (int i = 0; i < n - n_el + 1; i++) {
    Kdense.block(i, i, n_el, n_el) += Ke;
  }

  double t2 = omp_get_wtime();
  std::cout << Kdense.block(n - n_el, n - n_el, n_el, n_el) << std::endl;
  std::cout << "/* message */" << t2 - t1 << '\n';

  using namespace tbb;
    std::vector<double> a(20);
    tbb::parallel_for( size_t(0), 10,1,  [&]( size_t i ) {
        a.at(i)=i; return;
    } );

}

// Eigen::SparseMatrix<double, Eigen::RowMajor> K(n, n);
// K.coeffs() = 0.0;
// auto Ke = Eigen::MatrixXd::Ones(n_el, n_el);

// #pragma omp critical
//     {
//       for (auto b = 0; b < dofs.size(); b++) {
//         for (auto a = 0; a < dofs.size(); a++) {
// #pragma omp critical
//           K.coeffRef(int(dofs[a]), int(dofs[b])) += Ke(a, b);
//         }
// }
// }


/**
 * \file rsa_key.cpp
 * \author Julien Kauffmann <julien.kauffmann@freelan.org>
 * \brief A RSA sample file.
 */

#include <cryptoplus/cryptoplus.hpp>
#include <cryptoplus/buffer.hpp>
#include <cryptoplus/bio/bio_chain.hpp>
#include <cryptoplus/pkey/rsa_key.hpp>
#include <cryptoplus/hash/message_digest_context.hpp>
#include <cryptoplus/error/error_strings.hpp>

#include <boost/shared_ptr.hpp>

#include <iostream>
#include <string>
#include <cstdio>

#ifndef STDOUT_FILENO
#define STDOUT_FILENO 1
#endif

#ifdef MSV
#include <openssl/applink.c>
#endif

using cryptoplus::buffer;

namespace
{
	int pem_passphrase_callback(char* buf, int buf_len, int rwflag, void*)
	{
		std::cout << "Passphrase (max: " << buf_len << " characters): " << std::flush;
		std::string passphrase;
		std::getline(std::cin, passphrase);

		if (passphrase.empty())
		{
			std::cerr << "Passphrase cannot be empty." << std::endl;
			return 0;
		}

		if (passphrase.size() > static_cast<size_t>(buf_len))
		{
			std::cerr << "Passphrase cannot exceed " << buf_len << " characters." << std::endl;
			return 0;
		}

		if (rwflag != 0)
		{
			std::cout << "Confirm: " << std::flush;
			std::string passphrase_confirmation;
			std::getline(std::cin, passphrase_confirmation);

			if (passphrase_confirmation != passphrase)
			{
				std::cerr << "The two passphrases do not match !" << std::endl;
				return 0;
			}
		}

		std::copy(passphrase.begin(), passphrase.end(), buf);
		return passphrase.size();
	}
}

int main()
{
	cryptoplus::crypto_initializer crypto_initializer;
	cryptoplus::algorithms_initializer algorithms_initializer;
	cryptoplus::error::error_strings_initializer error_strings_initializer;

	std::cout << "RSA sample" << std::endl;
	std::cout << "==========" << std::endl;
	std::cout << std::endl;

	const std::string private_key_filename = "private_key.pem";
	const std::string public_key_filename = "public_key.pem";
	const std::string certificate_public_key_filename = "certificate_public_key.pem";

	boost::shared_ptr<FILE> private_key_file(fopen(private_key_filename.c_str(), "w"), fclose);
	boost::shared_ptr<FILE> public_key_file(fopen(public_key_filename.c_str(), "w"), fclose);
	boost::shared_ptr<FILE> certificate_public_key_file(fopen(certificate_public_key_filename.c_str(), "w"), fclose);

	if (!private_key_file)
	{
		std::cerr << "Unable to open \"" << private_key_filename << "\" for writing." << std::endl;

		return EXIT_FAILURE;
	}

	if (!public_key_file)
	{
		std::cerr << "Unable to open \"" << public_key_filename << "\" for writing." << std::endl;

		return EXIT_FAILURE;
	}

	if (!certificate_public_key_file)
	{
		std::cerr << "Unable to open \"" << certificate_public_key_filename << "\" for writing." << std::endl;

		return EXIT_FAILURE;
	}

	try
	{
		std::cout << "Generating RSA key. This can take some time..." << std::endl;

		cryptoplus::pkey::rsa_key rsa_key = cryptoplus::pkey::rsa_key::generate_private_key(1024, 17);

		std::cout << "Done." << std::endl;

		rsa_key.write_private_key(private_key_file.get(), cryptoplus::cipher::cipher_algorithm("AES256"), pem_passphrase_callback);

		std::cout << "Private RSA key written succesfully to \"" << private_key_filename << "\"." << std::endl;

		rsa_key.write_public_key(public_key_file.get());

		std::cout << "Public RSA key written succesfully to \"" << public_key_filename << "\"." << std::endl;

		rsa_key.write_certificate_public_key(certificate_public_key_file.get());

		std::cout << "Certificate public RSA key written succesfully to \"" << certificate_public_key_filename << "\"." << std::endl;
	}
	catch (std::exception& ex)
	{
		std::cerr << "Exception: " << ex.what() << std::endl;

		return EXIT_FAILURE;
	}

	certificate_public_key_file.reset();
	public_key_file.reset();
	private_key_file.reset(fopen(private_key_filename.c_str(), "r"), fclose);

	if (!private_key_file)
	{
		std::cerr << "Unable to open \"" << private_key_filename << "\" for reading." << std::endl;

		return EXIT_FAILURE;
	}

	try
	{
		std::cout << "Trying to read back the private RSA key from \"" << private_key_filename << "\"..." << std::endl;

		cryptoplus::pkey::rsa_key rsa_key = cryptoplus::pkey::rsa_key::from_private_key(private_key_file.get(), pem_passphrase_callback);

		std::cout << "Done." << std::endl;

		cryptoplus::bio::bio_chain bio_chain(BIO_new_fd(STDOUT_FILENO, BIO_NOCLOSE));
		rsa_key.print(bio_chain.first());

		const std::string str = "Hello World !";
		const std::string hash = "SHA256";

		std::cout << "Generating " << hash << " message digest for \"" << str << "\"..." << std::endl;

		cryptoplus::hash::message_digest_algorithm algorithm(hash);
		cryptoplus::hash::message_digest_context context;
		context.initialize(algorithm);
		context.update(str.c_str(), str.size());
    const buffer str_hash = context.finalize();

		std::cout << "Done." << std::endl;

		std::cout << "Generating RSA signature..." << std::endl;

    buffer str_sign = rsa_key.sign(str_hash, algorithm.type());

		std::cout << "Done." << std::endl;

		std::cout << "Verifying RSA signature..." << std::endl;

		rsa_key.verify(str_sign, str_hash, algorithm.type());

		std::cout << "Done." << std::endl;
	}
	catch (std::exception& ex)
	{
		std::cerr << "Exception: " << ex.what() << std::endl;

		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


#include "rotate.h"

#include "math_lib/degrees.h"
#include "math_lib/radians.h"

#include "dynamic_value/boolean_value.h"
#include "dynamic_value/float_value.h"
#include "dynamic_value/get_value_as.h"
#include "geometry_component.h"
#include "geometry_system.h"
#include "hierarchical_component.h"
#include "hierarchical_system.h"
#include "math_lib/matrix_base.h"
#include "procedural_object_system.h"

#include "geometry_operations/matrix_transform.h"

#include <boost/qvm/map_vec_mat.hpp>
#include <boost/qvm/mat_operations.hpp>

namespace pagoda
{
const std::string Rotate::s_inputGeometry("in");
const std::string Rotate::s_outputGeometry("out");

Rotate::Rotate(ProceduralObjectSystemPtr objectSystem) : ProceduralOperation(objectSystem)
{
	CreateInputInterface(s_inputGeometry);
	CreateOutputInterface(s_outputGeometry);

	RegisterValues({{"x", std::make_shared<FloatValue>(0.0f)},
	                {"y", std::make_shared<FloatValue>(0.0f)},
	                {"z", std::make_shared<FloatValue>(0.0f)},
	                {"rotation_order", std::make_shared<String>("xyz")},
	                {"world", std::make_shared<Boolean>(false)}});
}

Rotate::~Rotate() {}

void Rotate::DoWork()
{
	START_PROFILE;

	auto geometrySystem = m_proceduralObjectSystem->GetComponentSystem<GeometrySystem>();
	auto hierarchicalSystem = m_proceduralObjectSystem->GetComponentSystem<HierarchicalSystem>();

	while (HasInput(s_inputGeometry))
	{
		ProceduralObjectPtr inObject = GetInputProceduralObject(s_inputGeometry);
		ProceduralObjectPtr outObject = CreateOutputProceduralObject(s_outputGeometry);

		auto inGeometryComponent = geometrySystem->GetComponentAs<GeometryComponent>(inObject);
		GeometryPtr inGeometry = inGeometryComponent->GetGeometry();
		auto outGeometryComponent = geometrySystem->CreateComponentAs<GeometryComponent>(outObject);
		auto outGeometry = std::make_shared<Geometry>();
		outGeometryComponent->SetGeometry(outGeometry);

		auto inScope = inGeometryComponent->GetScope();
		UpdateValue("x");
		UpdateValue("y");
		UpdateValue("z");
		UpdateValue("rotation_order");
		UpdateValue("world");

		auto x = Degrees<float>(get_value_as<float>(*GetValue("x")));
		auto y = Degrees<float>(get_value_as<float>(*GetValue("y")));
		auto z = Degrees<float>(get_value_as<float>(*GetValue("z")));
		auto rotationOrder = get_value_as<std::string>(*GetValue("rotation_order"));
		auto world = get_value_as<std::string>(*GetValue("world")) == "true";

		Mat4x4F matrix(boost::qvm::diag_mat(Vec4F{1.0f, 1.0f, 1.0f, 1.0f}));
		if (world)
		{
			auto rot = inScope.GetRotation();
			boost::qvm::col<0>(matrix) = XYZ0(boost::qvm::col<0>(rot));
			boost::qvm::col<1>(matrix) = XYZ0(boost::qvm::col<1>(rot));
			boost::qvm::col<2>(matrix) = XYZ0(boost::qvm::col<2>(rot));
			boost::qvm::col<3>(matrix) = Vec4F{0, 0, 0, 1};
		}

		for (std::size_t i = rotationOrder.size(); i > 0; --i)
		{
			char order = rotationOrder[i - 1];
			switch (order)
			{
				case 'x':
					matrix = matrix * boost::qvm::rotx_mat<4>(static_cast<float>(Radians(x)));
					break;
				case 'y':
					matrix = matrix * boost::qvm::roty_mat<4>(static_cast<float>(Radians(y)));
					break;
				case 'z':
					matrix = matrix * boost::qvm::rotz_mat<4>(static_cast<float>(Radians(z)));
					break;
				default:
					throw Exception("Invalid rotation order " + std::string(1, order));
			}
		}

		if (world)
		{
			auto rot = inScope.GetInverseRotation();
			Mat4x4F invRot;
			boost::qvm::col<0>(invRot) = XYZ0(boost::qvm::col<0>(rot));
			boost::qvm::col<1>(invRot) = XYZ0(boost::qvm::col<1>(rot));
			boost::qvm::col<2>(invRot) = XYZ0(boost::qvm::col<2>(rot));
			boost::qvm::col<3>(invRot) = Vec4F{0, 0, 0, 1};
			matrix = matrix * invRot;
		}

		MatrixTransform<Geometry> transform(matrix);
		transform.Execute(inGeometry, outGeometry);
		Mat3x3F rot;
		boost::qvm::col<0>(rot) = XYZ(boost::qvm::col<0>(matrix));
		boost::qvm::col<1>(rot) = XYZ(boost::qvm::col<1>(matrix));
		boost::qvm::col<2>(rot) = XYZ(boost::qvm::col<2>(matrix));
		outGeometryComponent->SetScope(
		    Scope::FromGeometryAndConstrainedRotation(outGeometry, rot * inScope.GetRotation()));

		auto inHierarchicalComponent = hierarchicalSystem->GetComponentAs<HierarchicalComponent>(inObject);
		auto outHierarchicalComponent = hierarchicalSystem->CreateComponentAs<HierarchicalComponent>(outObject);
		hierarchicalSystem->SetParent(outHierarchicalComponent, inHierarchicalComponent);
	}
}  // namespace pagoda
}  // namespace pagoda


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;

class Radix {
private:
    const char* s;
    int a[128];
public:
    Radix(const char* s = "0123456789ABCDEF") : s(s) {
        int i;
        for(i = 0; s[i]; ++i)
            a[(int)s[i]] = i;
    }
    std::string to(long long p, int q) {
        int i;
        if(!p)
            return "0";
        char t[64] = { };
        for(i = 62; p; --i) {
            t[i] = s[p % q];
            p /= q;
        }
        return std::string(t + i + 1);
    }
    std::string to(const std::string& t, int p, int q) {
        return to(to(t, p), q);
    }
    long long to(const std::string& t, int p) {
        int i;
        long long sm = a[(int)t[0]];
        for(i = 1; i < (int)t.length(); ++i)
            sm = sm * p + a[(int)t[i]];
        return sm;
    }
};
int main() {
    int k; cin >> k;
    string a, b; cin >> a >> b;
    Radix r;
    boost::multiprecision::cpp_int x = boost::lexical_cast<boost::multiprecision::cpp_int>(r.to(a, k));
    boost::multiprecision::cpp_int y = boost::lexical_cast<boost::multiprecision::cpp_int>(r.to(b, k));
    cout << x * y << endl;
}