#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
//include the bie header files
#include "material.hh"
#include "precomputed_kernel.hh"
#include "bimat_interface.hh"
#include "infinite_boundary.hh"
//include the fem header files
#include "mesh_Abaqus_multi_faults.hpp"
#include "bcdof.hpp"
#include "bcdof_ptr.hpp"
#include "cal_ke.hpp"
#include "cal_fe_global_const_ke.hpp"
#include "mapglobal.hpp"
#include "maplocal.hpp"
#include "Slip_Weakening_lumpM.hpp"
#include "cal_slip_sliprate_angle.hpp"
#include "time_advance.hpp"
#include "BIE_correct.hpp"
#include "igl/list_to_matrix.h"
#include <math.h>
using namespace Eigen;
using namespace std;

typedef Eigen::Matrix<int, -1, -1,RowMajor> MatrixXi_rm;

void read_matrix(std::string fileName, Eigen::MatrixXd &outputMat);

double time_fem=0;
double time_bie;
int main() {
    std::string filename = "abaqus_cpp_5degree_100m_10km_4km.inp";
    std::cout<<filename<<std::endl;
    double fault_angle  = 5/180.0*M_PI;
    int dim = 2.0;
    double x_max = 5.0e3;
    double x_min = -5.0e3;
    double dx = 100.0;
    double dy = 100.0;
    std::vector<int> BIE_top, BIE_bot;
    MatrixXd Node;
    MatrixXi_rm Element;
    int num_faults = 1;
    std::vector<std::vector<int>> fault_nodes(2*num_faults);
    mesh_Abaqus_multi_faults(filename, Node, Element, BIE_top, BIE_bot,fault_nodes);
    std::cout<<Node<<std::endl;
    std::cout<<"Node size=" << Node.rows()<<std::endl;
    std::cout<<Element<<std::endl;
    std::cout<<"Element size="<<Element.rows()<<std::endl;
    int nx_BIE = BIE_top.size();
    int n_nodes = Node.rows();
    int n_el = Element.rows();
    int Ndofn = 2;
    int Nnel = Element.cols();
    // Material
    double density = 2670.0;
    double v_s =3.464e3;
    double v_p = 6.0e3;
    double G= pow(v_s,2)*density;
    double Lambda = pow(v_p,2)*density-2.0*G;
    double E  = G*(3.0*Lambda+2.0*G)/(Lambda+G);
    double nu = Lambda/(2.0*(Lambda+G));
    // Time
    double alpha = 0.4;
    double dt = alpha*dx/v_p;
    // Reyleigh Damping
    double beta =0.1;
    double q = beta*dt;
    double time_run = 6.0;
    int numt = time_run/dt;
    //numt = 1;
    VectorXd time = dt*VectorXd::LinSpaced(numt,1,numt);
    // Slip weakening friction parameters
    double Dc = 0.2;
    double mu_d= 0.5;
    double mu_s = 0.6;
    // Intialization
    // disp velocity current and next time step (new)
    VectorXd u_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd u_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd a_n = VectorXd::Zero(n_nodes*Ndofn,1);
    // slip and slip-rate
    std::vector<Eigen::Array<double, -1, 1> > delt_u_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > delt_v_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_c(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_0(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > tau_s(num_faults);

    // Vector containing number of elements on each faults (nx)
    std::vector<int> nx_faults(num_faults);
    for (int i=0;i<num_faults;i++)
    {
        nx_faults[i] =fault_nodes[2*i].size();
    }

    for (int i=0;i<num_faults;i++)
    {
        delt_u_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        delt_v_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_c[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_0[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
       // tau_s[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        tau_s[i] = 0.6*ArrayXd::Ones((nx_faults[i]),1)*50.0e6;
    }
    VectorXd F_ext_global = VectorXd::Zero(n_nodes*Ndofn,1);

    VectorXd x = VectorXd::Zero(nx_faults[0], 1);
    for (int i=0; i<x.size(); i++)
    {
        x(i) = Node(fault_nodes[0][i],0);
    }
    std::ofstream x_output("results/fault_x_coord.txt");
    x_output << x;

    std::cout<<x<<std::endl;
    // Setting intial stress on the fault
    for (int j=0; j<num_faults;j++)
    {
        for (int i=0; i<nx_faults[j]; i++)
        {
            T_0[j](2*i+1) = -50.0e6;
            T_c[j](2*i+1) = T_0[j](2*i+1);
        }
        if (j==0)
        {
            VectorXd x = VectorXd::LinSpaced(nx_faults[j], x_min, x_max);
            for (int i=0 ; i<nx_faults[j]; i++)
            {
                //if ((x(i)<=(fault_pos(j,1)+fault_pos(j,2))/2+0.2e3)&&(x(i)>=(fault_pos(j,1)+fault_pos(j,2))/2-0.2e3))
                if ((x(i)<=(x_min+x_max)/2+0.6e3)&&(x(i)>=(x_min+x_max)/2-0.6e3))
                {
                    T_0[j](2*i) = 31.0e6;
                    T_c[j](2*i) = T_0[j](2*i);
                }
                else
                {
                    T_0[j](2*i) = 27.5e6;
                    T_c[j](2*i) = T_0[j](2*i);
                }
            }
        }
        else
        {
            for (int i=0 ; i<nx_faults[j]; i++)
            {
                T_0[j](2*i) = 27.5e6;
            }
        }
    }
    // Get the index degree of freedome for each element
    VectorXi index_el = VectorXi::Zero(Ndofn*Nnel,1);
    MatrixXi index_store = MatrixXi::Zero(Nnel*Ndofn,n_el);
    for (int i=0;i<n_el;i++)
    {
        bcdof(Element.row(i),dim,index_el);
        index_store.col(i) = index_el;
    }
  //  VectorXi BIE_top_surf_index = VectorXi::Zero(Ndofn*(BIE_top_surf_nodes.size()),1);
   // VectorXi BIE_bot_surf_index = VectorXi::Zero(Ndofn*(BIE_bot_surf_nodes.size()),1);
    VectorXi BIE_top_surf_index = VectorXi::Zero(Ndofn*(BIE_top.size()),1);
    VectorXi BIE_bot_surf_index = VectorXi::Zero(Ndofn*(BIE_bot.size()),1);
    std::vector<Eigen::ArrayXi> Fault_surf_index(num_faults*2);
    for (int i=0; i <num_faults*2; i++)
    {
        Fault_surf_index[i] = ArrayXi::Zero(Ndofn*(fault_nodes[i].size()),1);
    }

    // Getting the faults DOF index
    for (int i=0; i<num_faults*2;i++)
    {
        bcdof_ptr(fault_nodes[i], dim, Fault_surf_index[i].data());
    }

    bcdof_ptr(BIE_top,dim,BIE_top_surf_index.data());
    bcdof_ptr(BIE_bot,dim,BIE_bot_surf_index.data());
    //bcdof(BIE_top_surf_nodes,dim,BIE_top_surf_index);
    //bcdof(BIE_bot_surf_nodes,dim,BIE_bot_surf_index);
    // Calculating the Global Mass Vector (lumped mass)
    // Element mass
    double M=density*dx*dy*1.0;
    VectorXd M_el_vec = M/4*VectorXd::Ones(Nnel*Ndofn,1);
    VectorXd M_global_vec=VectorXd::Zero(n_nodes*Ndofn,1);
    for (int i=0 ; i<n_el;i++)
    {
        index_el = index_store.col(i);
        mapglobal(index_el,M_global_vec,M_el_vec);
    }
    // Element matrix
    MatrixXd ke = MatrixXd::Zero(8,8);
    MatrixXd coord = MatrixXd::Zero(4,2);
    VectorXi Element_0= Element.row(0);
    coord.row(0) = Node.row(Element_0(0));
    coord.row(1) = Node.row(Element_0(1));
    coord.row(2) = Node.row(Element_0(2));
    coord.row(3) = Node.row(Element_0(3));
    cal_ke (coord,E,nu,ke);
    // BIE part initiation
    // Setting up the material property for the BIE code
    Material BIE_top_mat = Material(E,nu,density);
    Material BIE_bot_mat = Material(E,nu,density);
    double length = x_max-x_min;
    // infinte bc BIE call infinite_boundary.cc
    PrecomputedKernel h11("kernels/nu_.25_h11.dat");
    PrecomputedKernel h12("kernels/nu_.25_k12.dat");
    PrecomputedKernel h22("kernels/nu_.25_h22.dat");
    InfiniteBoundary BIE_inf_top(length,nx_BIE,1.0,&BIE_top_mat,&h11,&h12,&h22);
    InfiniteBoundary BIE_inf_bot(length,nx_BIE,-1.0,&BIE_bot_mat,&h11,&h12,&h22);
    // BIE setting time step
    BIE_inf_top.setTimeStep(dt);
    BIE_inf_bot.setTimeStep(dt);
    // BIE initialization
    BIE_inf_top.init();
    BIE_inf_bot.init();
    printf("ready to start\n");
    // Output
    ofstream file;
    file.open("results/num_nodes_fault.bin",ios::binary);
    file.write((char*)(nx_faults.data()),nx_faults.size()*sizeof(int));
    file.close();

    file.open("results/u_n.bin",ios::binary);
   // file.write((char*)(u_n.data()),u_n.size()*sizeof(double));
    file.close();

    for (int i=0;i<num_faults;i++)
    {
        std::string slip = "results/slip_"+std::to_string(i)+".bin";
        file.open(slip);
        file.close();
        std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
        file.open(slip_rate);
        file.close();
        std::string shear = "results/shear_"+std::to_string(i)+".bin";
        file.open(shear);
        file.close();
    }
    std::ofstream Element_output("results/Element.txt");
    std::ofstream Node_output("results/Node.txt");
    Node_output<<Node;
    Element_output<<Element;
    //    std::ofstream shear_x("results/shear.txt");
    //double start = omp_get_wtime();

    // Main time loop
    for (int j=0;j<numt;j++)
    {
        // Compute the global internal force
        VectorXd fe_global= VectorXd::Zero(n_nodes*Ndofn,1);
        cal_fe_global_const_ke(n_nodes, n_el, index_store, q, u_n, v_n, Ndofn, ke, fe_global);
        // Friction subroutine
        VectorXd F_total = F_ext_global-fe_global;
        for (int i=0 ; i<num_faults; i++)
        {
            VectorXd F_fault = VectorXd::Zero(Ndofn*(nx_faults[i]),1);
            VectorXd M_pos = VectorXd::Zero(nx_faults[i], 1);
            VectorXd M_neg = VectorXd::Zero(nx_faults[i],1);
            maplocal(Fault_surf_index[2*i], M_global_vec, M_pos);
            maplocal(Fault_surf_index[2*i+1], M_global_vec, M_neg);
            //std::cout<<M_pos<<std::endl;
            Slip_Weakening_lumpM(M_global_vec, Fault_surf_index[2*i], Fault_surf_index[2*i+1], fe_global, dt, dx, dy, nx_faults[i]-1, delt_v_n[i], delt_u_n[i], T_0[i], tau_s[i], mu_s, mu_d, Dc, Ndofn, M_pos, M_neg , F_fault, T_c[i], fault_angle);
            mapglobal(Fault_surf_index[2*i],F_total,-F_fault);
            mapglobal(Fault_surf_index[2*i+1],F_total,F_fault);
        }

        // Central Difference Time integration
        time_advance(u_n, v_n, F_total, M_global_vec, dt);
        // Get the slip and slip rate

        for (int i=0; i<num_faults; i++)
        {
            cal_slip_slip_rate_angle(u_n, v_n,  Fault_surf_index[2*i], Fault_surf_index[2*i+1] , Ndofn, nx_faults[i]-1, delt_u_n[i], delt_v_n[i],fault_angle);
        }
        // Correct the BIE surf nodes solutions from FEM with the BIE solution
        BIE_correct(BIE_top_surf_index, BIE_bot_surf_index, fe_global, Ndofn, nx_BIE-1, dx, BIE_inf_top, BIE_inf_bot, u_n, v_n);

      //  if (j%4==3)
       // {
        for (int i=0;i<num_faults;i++)
        {
            std::string slip = "results/slip_"+std::to_string(i)+".bin";
            file.open(slip,ios::binary | ios::app);
            file.write((char*)(delt_u_n[i].data()),delt_u_n[i].size()*sizeof(double));
            file.close();

            std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
            file.open(slip_rate,ios::binary | ios::app);
            file.write((char*)(delt_v_n[i].data()),delt_v_n[i].size()*sizeof(double));
            file.close();

            std::string shear = "results/shear_"+std::to_string(i)+".bin";
            file.open(shear,ios::binary | ios::app);
            file.write((char*)(T_c[i].data()),T_c[i].size()*sizeof(double));
            file.close();
        }
        file.open("results/u_n.bin",ios::binary | ios::app);
        file.write((char*)(u_n.data()),u_n.size()*sizeof(double));
        file.close();
       // }
        printf("Simulation time = %f\n",time(j));
      //  double end_t = omp_get_wtime();
       // std::cout<<"time_cpu_t="<<end_t-start<<std::endl;
    //   std::cout<<"time_fem="<< time_fem<<std::endl;
    //   std::cout<<"time_bie="<< time_bie<<std::endl;
    }
    //double end = omp_get_wtime();
    //std::cout<<"time_cpu="<<end-start<<std::endl;
    return 0;
}

void read_matrix(std::string fileName, Eigen::MatrixXd &outputMat) {
    fstream cin;
    cin.open(fileName.c_str());
    if (cin.fail())
    {
        std::cerr << "Failed to open file: " << fileName << std::endl;
        std::cin.get(); }
    string s;
    vector <vector <double> > matrix;
    while (getline(cin, s)) {
        stringstream input(s);
        double temp;
        vector <double> currentLine;
        while (input >> temp)
            currentLine.push_back(temp);
        matrix.push_back(currentLine);
    }
    if (!igl::list_to_matrix(matrix, outputMat))
    { std::cerr << "list tom matrix error" << std::endl; std::cin.get();
        //return false;
    }
}


#include "tbb/parallel_for.h"
#include <iostream>
#include <mex.h>
#include <omp.h>
#include <vector>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
// #include <Eigen/StdVector>

// #include <range/v3/all.hpp>
// #include <range/v3/core.hpp>

// #include "storage_aliases.h"

// #define ngp int(*mxGetPr(prhs[0]))

#define int_space_ptr mxGetPr(prhs[0])
#define delta_ptr mxGetPr(prhs[1])
#define Hdo_raw prhs[2]
#define Hdo_ptr mxGetPr(prhs[2])
#define int_time_ptr mxGetPr(prhs[3])
#define d_alpha_ptr mxGetPr(prhs[4])
#define alpha_ptr mxGetPr(prhs[5])
#define inv_hooke_ptr mxGetPr(prhs[6])
#define gradient_ptr mxGetPr(prhs[7])
#define dof_imposed_raw prhs[8]
#define dof_imposed_ptr mxGetPr(prhs[8])
#define inv_hooke_full_ptr mxGetPr(prhs[9])
#define gradient_width int(*mxGetPr(prhs[10]))

#define output plhs[0]

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  // auto t1 = omp_get_wtime();
  using sparse_matrix = Eigen::SparseMatrix<double, Eigen::RowMajor>;
  using Matrix = Eigen::MatrixXd;
  using Vector = Eigen::VectorXd;
  using matrix6 = Eigen::Matrix<double, 6, 6>;
  using vector6 = Eigen::Matrix<double, 6, 1>;

  auto Hdo_Dimensions = mxGetDimensions(Hdo_raw);
  auto dof_imposed_Dimensions = mxGetDimensions(dof_imposed_raw);
  size_t const ncomp = 6; // TODO 3D only
  size_t const ngp = Hdo_Dimensions[2];
  size_t const ntp = Hdo_Dimensions[3];
  size_t const ndof_imp = dof_imposed_Dimensions[0];
  size_t const tensor_shift = ncomp * ncomp;
  size_t const delta_shift = ncomp;

  Vector alpha = Eigen::Map<Vector>(alpha_ptr, ntp);
  Vector d_alpha = Eigen::Map<Vector>(d_alpha_ptr, ntp);
  Eigen::Map<Matrix> int_time(int_time_ptr, ntp, ntp);
  Eigen::Map<Matrix> gradient(gradient_ptr, ngp * ncomp, gradient_width);
  Eigen::Map<matrix6> inv_hooke(inv_hooke_ptr);
  Eigen::Map<Matrix> inv_hooke_full(inv_hooke_full_ptr, ngp * ncomp,
                                    ngp * ncomp);
  std::vector<std::int32_t> fixed_dofs;
  for (size_t j = 0; j < ndof_imp; j++)
    fixed_dofs.push_back(*(dof_imposed_ptr + j));

  sparse_matrix A(ncomp * ngp, ncomp * ngp);
  sparse_matrix A_noint(ncomp * ngp, ncomp * ngp);
  Vector b(ngp * ncomp);
  Vector Q3_vec(ngp * ncomp);
  Vector space_mode(ngp * ncomp);

  double int_alpha_d_alpha = alpha.transpose() * int_time * d_alpha;
  Vector int_alpha_alpha = int_time * alpha.cwiseProduct(alpha);
  Vector int_alpha = int_time * alpha;
  Vector int_d_alpha = int_time * d_alpha;
  // auto t2 = omp_get_wtime();
  // std::cout << " /* block1 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  // #pragma omp parallel for
  for (size_t i = 0; i < ngp; i++) {
    size_t const position = i * ncomp;
    size_t const int_idx = i * tensor_shift;
    matrix6 B = matrix6::Zeros();
    vector6 Q3 = vector6::Zeros();

    // #pragma omp parallel for
    for (size_t j = 0; j < ntp; j++) {
      int const delta_idx = (j * ngp + i) * delta_shift;
      int const tensor_idx = (j * ngp + i) * tensor_shift;
      matrix6 H = Eigen::Map<matrix6>(Hdo_ptr + tensor_idx);
      vector6 delta = Eigen::Map<vector6>(delta_ptr + delta_idx);
      B.noalias() += H * int_alpha_alpha(j);
      Q3 += delta * int_alpha(j);
    }

    matrix6 Int_space = Eigen::Map<matrix6>(int_space_ptr + int_idx, ncomp,
                                            ncomp); // TODO: alligned
    matrix6 Z = (B + int_alpha_d_alpha * inv_hooke).inverse();
    matrix6 int_Z = Int_space * Z;

    // #pragma omp critical
    for (size_t k = 0; k < ncomp; k++) {
      for (size_t l = 0; l < ncomp; l++) {
        A.insert(position + k, position + l) = int_Z(k, l);
        A_noint.insert(position + k, position + l) = Z(k, l);
      }
    }
    b.segment<6>(position) = -int_Z * Q3;
    Q3_vec.segment<6>(position) = Q3;
  }
  // t2 = omp_get_wtime();
  // std::cout << " /* block2 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  Matrix K = gradient.transpose() * A * gradient;
  Vector F = gradient.transpose() * b;
  for (auto const fixed_dof : fixed_dofs) {
    auto const diagonal_entry = K.coeff(fixed_dof, fixed_dof); // TODO //
    F(fixed_dof) = 0.0;
    for (int i = 0; i < K.rows(); i++) {
      for (int j = 0; j < K.cols(); j++) {
        K.coeffRef(fixed_dof, j) = 0.0;
        K.coeffRef(i, fixed_dof) = 0.0; // TODO K.row(i)=0;
      }
    }
    K.coeffRef(fixed_dof, fixed_dof) = diagonal_entry;
  }
  // if (b.norm() > 0)
  auto U_tilde = K.llt().solve(F); // TODO: is this ok?
  // else {
  //   U_tilde = F;
  // }
  //   t2 = omp_get_wtime();
  //   std::cout << " /* block3 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  auto E_tilde = gradient * U_tilde;
  auto sigma = A_noint * (E_tilde + Q3_vec);
  space_mode = E_tilde / int_alpha_d_alpha - inv_hooke_full * sigma;
  // t2 = omp_get_wtime();
  // std::cout << " /* block4 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  output = mxCreateDoubleMatrix(ngp * ncomp, 1, mxREAL);
  Eigen::Map<Matrix>(mxGetPr(output), ngp * ncomp, 1) = space_mode;
  // t2 = omp_get_wtime();
  // std::cout << " /* block5 */ " << t2 - t1 << '\n';

  return;
}

// mxArray *cpp_to_MexArray(const std::vector<double> &v) {
//   mxArray *mx = mxCreateDoubleMatrix(1, v.size(), mxREAL);
//   std::copy(v.begin(), v.end(), mxGetPr(mx));
//
//   return mx;
// }


#include <iostream>
#include <string>
#include <vector>
#include <valarray>

#include <thread>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>

#include <stdio.h>

#include "refine_variational.h"

using std::cout;
using std::endl;
using std::vector;

namespace OFC
{

  VarRefClass::VarRefClass(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in,
                            const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in,
                           const camparam* cpt_in,const camparam* cpo_in,const optparam* op_in, float *flowout)
  : cpt(cpt_in), cpo(cpo_in), op(op_in)
{

  // initialize parameters
  tvparams.alpha = op->tv_alpha;
  tvparams.beta = 0.0f;  // for matching term, not needed for us
  tvparams.gamma = op->tv_gamma;
  tvparams.delta = op->tv_delta;
  tvparams.n_inner_iteration = op->tv_innerit * (cpt->curr_lv+1);
  tvparams.n_solver_iteration = op->tv_solverit;//5;
  tvparams.sor_omega = op->tv_sor;

  tvparams.tmp_quarter_alpha = 0.25f*tvparams.alpha;
  tvparams.tmp_half_gamma_over3 = tvparams.gamma*0.5f/3.0f;
  tvparams.tmp_half_delta_over3 = tvparams.delta*0.5f/3.0f;
  tvparams.tmp_half_beta = tvparams.beta*0.5f;

  float deriv_filter[3] = {0.0f, -8.0f/12.0f, 1.0f/12.0f};
  deriv = convolution_new(2, deriv_filter, 0);
  float deriv_filter_flow[2] = {0.0f, -0.5f};
  deriv_flow = convolution_new(1, deriv_filter_flow, 0);

  // copy flow initialization into FV structs
  #if (SELECTMODE==1)
  static int noparam = 2; // Optical flow
  #else
  static int noparam = 1; // Only horizontal displacements for stereo depth
  #endif
  std::vector<image_t*> flow_sep(noparam);

  for (int i = 0; i < noparam; ++i )
    flow_sep[i] = image_new(cpt->width,cpt->height);

  for (int iy = 0; iy < cpt->height; ++iy)
    for (int ix = 0; ix < cpt->width; ++ix)
    {
      int i  = iy * cpt->width          + ix;
      int is = iy * flow_sep[0]->stride + ix;
      for (int j = 0; j < noparam; ++j)
        flow_sep[j]->c1[is] = flowout[i*noparam + j];
    }

  // copy image data into FV structs
  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
  image_t * im_ao, *im_bo;
  im_ao = image_new(cpt->width,cpt->height);
  im_bo = image_new(cpt->width,cpt->height);
  #else
  color_image_t * im_ao, *im_bo;
  im_ao = color_image_new(cpt->width,cpt->height);
  im_bo = color_image_new(cpt->width,cpt->height);
  #endif

  copyimage(im_ao_in, im_ao);
  copyimage(im_bo_in, im_bo);

  // Call solver
  #if (SELECTMODE==1)
  RefLevelOF(flow_sep[0], flow_sep[1], im_ao, im_bo);
  #else
  RefLevelDE(flow_sep[0], im_ao, im_bo);
  #endif

  // Copy flow result back
  for (int iy = 0; iy < cpt->height; ++iy)
    for (int ix = 0; ix < cpt->width; ++ix)
    {
      int i  = iy * cpt->width          + ix;
      int is = iy * flow_sep[0]->stride + ix;
      for (int j = 0; j < noparam; ++j)
        flowout[i*noparam + j] = flow_sep[j]->c1[is];
    }

  // free FV structs
  for (int i = 0; i < noparam; ++i )
    image_delete(flow_sep[i]);

  convolution_delete(deriv);
  convolution_delete(deriv_flow);

  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
  image_delete(im_ao);
  image_delete(im_bo);
  #else
  color_image_delete(im_ao);
  color_image_delete(im_bo);
  #endif
}

#if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
void VarRefClass::copyimage(const float* img, image_t * img_t)
#else
void VarRefClass::copyimage(const float* img, color_image_t * img_t)
#endif
{
  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
  const float * img_st = img +     (cpt->tmp_w + 1 ) * (cpt->imgpadding); // remove image padding, start at first valid pixel
  #else
  const float * img_st = img + 3 * (cpt->tmp_w + 1 ) * (cpt->imgpadding);
  #endif

  for (int yi = 0; yi < cpt->height; ++yi)
  {
    for (int xi = 0; xi < cpt->width; ++xi, ++img_st)
    {
      int i    = yi*img_t->stride+ xi;

      img_t->c1[i] =  (*img_st);
      #if (SELECTCHANNEL==3)
      ++img_st; img_t->c2[i] =  (*img_st);
      ++img_st; img_t->c3[i] =  (*img_st);
      #endif
    }
    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
    img_st +=     2 * cpt->imgpadding;
    #else
    img_st += 3 * 2 * cpt->imgpadding;
    #endif
  }
}

#if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
void VarRefClass::RefLevelOF(image_t *wx, image_t *wy, const image_t *im1, const image_t *im2)
#else
void VarRefClass::RefLevelOF(image_t *wx, image_t *wy, const color_image_t *im1, const color_image_t *im2)
#endif
{
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;

    image_t *du = image_new(width,height), *dv = image_new(width,height), // the flow increment
      *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
      *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
      *uu = image_new(width,height), *vv = image_new(width,height), // flow plus flow increment
      *a11 = image_new(width,height), *a12 = image_new(width,height), *a22 = image_new(width,height), // system matrix A of Ax=b for each pixel
      *b1 = image_new(width,height), *b2 = image_new(width,height); // system matrix b of Ax=b for each pixel

    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // use single band image
    image_t *w_im2 = image_new(width,height), // warped second image
        *Ix = image_new(width,height), *Iy = image_new(width,height), *Iz = image_new(width,height), // first order derivatives
        *Ixx = image_new(width,height), *Ixy = image_new(width,height), *Iyy = image_new(width,height), *Ixz = image_new(width,height), *Iyz = image_new(width,height); // second order derivatives
    #else                                     // use RGB image
    color_image_t *w_im2 = color_image_new(width,height), // warped second image
        *Ix = color_image_new(width,height), *Iy = color_image_new(width,height), *Iz = color_image_new(width,height), // first order derivatives
        *Ixx = color_image_new(width,height), *Ixy = color_image_new(width,height), *Iyy = color_image_new(width,height), *Ixz = color_image_new(width,height), *Iyz = color_image_new(width,height); // second order derivatives
    #endif

    // warp second image
    image_warp(w_im2, mask, im2, wx, wy);
    // compute derivatives
    get_derivatives(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
    // erase du and dv
    image_erase(du);
    image_erase(dv);
    // initialize uu and vv
    memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));
    memcpy(vv->c1,wy->c1,wy->stride*wy->height*sizeof(float));
    // inner fixed point iterations
    for(i_inner_iteration = 0 ; i_inner_iteration < tvparams.n_inner_iteration ; i_inner_iteration++)
    {
        //  compute robust function and system
        compute_smoothness(smooth_horiz, smooth_vert, uu, vv, deriv_flow, tvparams.tmp_quarter_alpha );
        //compute_data_and_match(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, desc_weight, desc_flow_x, desc_flow_y, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        compute_data(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        sub_laplacian(b1, wx, smooth_horiz, smooth_vert);
        sub_laplacian(b2, wy, smooth_horiz, smooth_vert);

        // solve system
        #ifdef WITH_OPENMP
        sor_coupled_slow_but_readable(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega); // slower but parallelized
        #else
        sor_coupled(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);
        #endif

        // update flow plus flow increment
        int i;
        v4sf *uup = (v4sf*) uu->c1, *vvp = (v4sf*) vv->c1, *wxp = (v4sf*) wx->c1, *wyp = (v4sf*) wy->c1, *dup = (v4sf*) du->c1, *dvp = (v4sf*) dv->c1;
        for( i=0 ; i<height*stride/4 ; i++)
        {
          (*uup) = (*wxp) + (*dup);
          (*vvp) = (*wyp) + (*dvp);
          uup+=1; vvp+=1; wxp+=1; wyp+=1;dup+=1;dvp+=1;
        }

    }
    // add flow increment to current flow
    memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));
    memcpy(wy->c1,vv->c1,vv->stride*vv->height*sizeof(float));

    // free memory
    image_delete(du); image_delete(dv);
    image_delete(mask);
    image_delete(smooth_horiz); image_delete(smooth_vert);
    image_delete(uu); image_delete(vv);
    image_delete(a11); image_delete(a12); image_delete(a22);
    image_delete(b1); image_delete(b2);

    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // use single band image
    image_delete(w_im2);
    image_delete(Ix); image_delete(Iy); image_delete(Iz);
    image_delete(Ixx); image_delete(Ixy); image_delete(Iyy); image_delete(Ixz); image_delete(Iyz);
    #else
    color_image_delete(w_im2);
    color_image_delete(Ix); color_image_delete(Iy); color_image_delete(Iz);
    color_image_delete(Ixx); color_image_delete(Ixy); color_image_delete(Iyy); color_image_delete(Ixz); color_image_delete(Iyz);
    #endif

}

#if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
void VarRefClass::RefLevelDE(image_t *wx, const image_t *im1, const image_t *im2)
#else
void VarRefClass::RefLevelDE(image_t *wx, const color_image_t *im1, const color_image_t *im2)
#endif
{
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;

      image_t *du = image_new(width,height), *wy_dummy = image_new(width,height), // the flow increment
        *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
        *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
        *uu = image_new(width,height), // flow plus flow increment
        *a11 = image_new(width,height), // system matrix A of Ax=b for each pixel
        *b1 = image_new(width,height); // system matrix b of Ax=b for each pixel

      image_erase(wy_dummy);

      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // use single band image
      image_t *w_im2 = image_new(width,height), // warped second image
          *Ix = image_new(width,height), *Iy = image_new(width,height), *Iz = image_new(width,height), // first order derivatives
          *Ixx = image_new(width,height), *Ixy = image_new(width,height), *Iyy = image_new(width,height), *Ixz = image_new(width,height), *Iyz = image_new(width,height); // second order derivatives
      #else                                     // use RGB image
      color_image_t *w_im2 = color_image_new(width,height), // warped second image
          *Ix = color_image_new(width,height), *Iy = color_image_new(width,height), *Iz = color_image_new(width,height), // first order derivatives
          *Ixx = color_image_new(width,height), *Ixy = color_image_new(width,height), *Iyy = color_image_new(width,height), *Ixz = color_image_new(width,height), *Iyz = color_image_new(width,height); // second order derivatives
      #endif

      // warp second image
      image_warp(w_im2, mask, im2, wx, wy_dummy);
      // compute derivatives
      get_derivatives(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
      // erase du and dv
      image_erase(du);

      // initialize uu and vv
      memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));

      // inner fixed point iterations
      for(i_inner_iteration = 0 ; i_inner_iteration < tvparams.n_inner_iteration ; i_inner_iteration++)
      {
          //  compute robust function and system
          compute_smoothness(smooth_horiz, smooth_vert, uu, wy_dummy, deriv_flow, tvparams.tmp_quarter_alpha );
          compute_data_DE(a11, b1, mask, wx, du, uu, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
          sub_laplacian(b1, wx, smooth_horiz, smooth_vert);

          // solve system
          sor_coupled_slow_but_readable_DE(du, a11, b1, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);

          // update flow plus flow increment
          int i;
          v4sf *uup = (v4sf*) uu->c1, *wxp = (v4sf*) wx->c1, *dup = (v4sf*) du->c1;

          if(cpt->camlr==0)  // check if right or left camera, needed to truncate values above/below zero
          {
            for( i=0 ; i<height*stride/4 ; i++)
            {
                (*uup) = __builtin_ia32_minps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
          }
          else
          {
            for( i=0 ; i<height*stride/4 ; i++)
            {
                (*uup) = __builtin_ia32_maxps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
          }
      }
      // add flow increment to current flow
      memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));

      // free memory
      image_delete(du); image_delete(wy_dummy);
      image_delete(mask);
      image_delete(smooth_horiz); image_delete(smooth_vert);
      image_delete(uu);
      image_delete(a11);
      image_delete(b1);

      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
      image_delete(w_im2);
      image_delete(Ix); image_delete(Iy); image_delete(Iz);
      image_delete(Ixx); image_delete(Ixy); image_delete(Iyy); image_delete(Ixz); image_delete(Iyz);
      #else
      color_image_delete(w_im2);
      color_image_delete(Ix); color_image_delete(Iy); color_image_delete(Iz);
      color_image_delete(Ixx); color_image_delete(Ixy); color_image_delete(Iyy); color_image_delete(Ixz); color_image_delete(Iyz);
      #endif
}

VarRefClass::~VarRefClass()
{

}

}


#include <graphene/singularity/ncd_aware_rank.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace boost;
using namespace boost::numeric::ublas;
using namespace singularity;

std::shared_ptr<vector_t> ncd_aware_rank::process(
        const matrix_t& outlink_matrix
) {
    sparce_vector_t v = matrix_tools::calculate_correction_vector(outlink_matrix);
    Graph g = create_graph(outlink_matrix);
    scan scan(parameters.clustering_e, parameters.clustering_m);
    scan.process(g);
    std::shared_ptr<matrix_t> ms = create_interlevel_matrix_s(g);
    std::shared_ptr<matrix_t> ml = create_interlevel_matrix_l(g, outlink_matrix);

    return calculate_ncd_aware_rank(outlink_matrix, v, *ms, *ml);
}

std::shared_ptr<vector_t> ncd_aware_rank::iterate(
        const matrix_t& outlink_matrix,
        const sparce_vector_t& outlink_vector,
        const matrix_t& interlevel_matrix_s,
        const matrix_t& interlevel_matrix_l,
        const vector_t& previous,
        const vector_t& teleportation
) {
    unsigned int num_accounts = outlink_matrix.size2();
    vector_t tmp(interlevel_matrix_l.size1(), 0);
    matrix_tools::prod(tmp, interlevel_matrix_l, previous, parameters.num_threads);

    vector_t tmp2(interlevel_matrix_s.size1(), 0);
    std::shared_ptr<vector_t> next(new vector_t(outlink_matrix.size1(), 0));

    matrix_tools::prod(*next, outlink_matrix, previous, parameters.num_threads);
    matrix_tools::prod(tmp2, interlevel_matrix_s, tmp, parameters.num_threads);

    *next += tmp2;

    vector_t correction_vector(num_accounts, inner_prod(outlink_vector, previous));

    *next += correction_vector;
    *next += teleportation;

    return next;
}

std::shared_ptr<vector_t> ncd_aware_rank::calculate_ncd_aware_rank(
        const matrix_t& outlink_matrix,
        const sparce_vector_t& outlink_vector,
        const matrix_t& interlevel_matrix_s,
        const matrix_t& interlevel_matrix_l
) {
    unsigned int num_accounts = outlink_matrix.size2();
    double initialValue = 1.0/num_accounts;
    std::shared_ptr<vector_t> next;
    std::shared_ptr<vector_t> previous(new vector_t(num_accounts, initialValue));
    vector_t teleportation = (*previous) * (1.0 - parameters.outlink_weight - parameters.interlevel_weight) ;

    matrix_t outlink_matrix_weighted = outlink_matrix * parameters.outlink_weight;
    matrix_t interlevel_matrix_s_weighted = interlevel_matrix_s * parameters.interlevel_weight;
    sparce_vector_t outlink_vector_weighted = outlink_vector * parameters.outlink_weight;

    for (uint i = 0; i < MAX_ITERATIONS; i++) {
        next  = iterate(outlink_matrix_weighted, outlink_vector_weighted, interlevel_matrix_s_weighted, interlevel_matrix_l, *previous, teleportation);
        double norm = norm_1(*next - *previous);
        if (norm <= precision) {
            return next;
        } else {
            previous = next;
        }
    }

    return next;
}

std::shared_ptr<matrix_t> ncd_aware_rank::create_interlevel_matrix_s(const Graph& g)
{
    Graph::vertex_iterator current, end;

    unsigned int num_clasters = get_property(g, graph_num_clusters);

    std::shared_ptr<matrix_t> S(new matrix_t(num_vertices(g), num_clasters));

    tie(current, end) = vertices(g);

    for ( ; current != end; current++) {
        unsigned int index = get(vertex_index, g, *current);
        unsigned int cluster_id = get(vertex_cluster_id, g, *current);
        (*S)(index, cluster_id) = 1;
    }

    matrix_tools::normalize_columns(*S);

    return S;
}

std::shared_ptr<matrix_t> ncd_aware_rank::create_interlevel_matrix_l(
        const Graph& g,
        const matrix_t& outlink_matrix
)
{
    unsigned int num_clusters = get_property(g, graph_num_clusters);

    Graph::vertex_iterator start, end;

    tie(start, end) = vertices(g);

    std::shared_ptr<matrix_t> L(new matrix_t(num_clusters, num_vertices(g)));

    for (matrix_t::const_iterator1 i = outlink_matrix.begin1(); i != outlink_matrix.end1(); i++)
    {
        Graph::vertex_descriptor vertex = start[i.index1()];
        unsigned int clusterId = get(vertex_cluster_id, g, vertex);
        (*L)(clusterId, i.index1()) = 1;
        for (matrix_t::const_iterator2 j = i.begin(); j != i.end(); j++)
        {
            if (*j > 0) {
                (*L)(clusterId, j.index2()) = 1;
            }
        }
    }

    matrix_tools::normalize_columns(*L);

    return L;
}

Graph ncd_aware_rank::create_graph(const matrix_t& m)
{
    Graph g(m.size2());

    Graph::vertex_iterator v,ve;

    tie(v, ve) = vertices(g);

    unsigned int id = 0;

    for (matrix_t::const_iterator1 i = m.begin1(); i != m.end1(); i++)
    {
        for (matrix_t::const_iterator2 j = i.begin(); j != i.end(); j++)
        {
            Graph::edge_descriptor edge;
            bool added = false;
            if (*j > 0) {
                tie(edge, added) = add_edge(v[j.index1()], v[j.index2()], g);
                if (added) {
                    put(edge_index, g, edge, id++);
                }
            }
        }
    }

    return g;
}


#pragma once
#include "bank/interfaces/DataSource/IDataObjectMetaInfo.h"
#include "bank/DataSources//VirtualObjectMetaInfo.h"
#include "constants.h"
#include <boost/date_time/gregorian/gregorian.hpp>
#include <string>
#include <vector>
#include <ctime>

#define AllowCreating virtual void makeVirtual() override {}

namespace DatabaseObjects {
    using namespace boost::gregorian;
    using namespace constants;

    using id = std::size_t;

    struct DatabaseObject {
        id object_id;
        interfaces::DataSource::IDataObjectMetaInfo* meta_info;

        virtual void makeVirtual() = 0;
    };

    struct Client: DatabaseObject {
        std::string name;
        std::string surname;
        std::string address;
        std::string passport;

        AllowCreating;
    };

    struct BankAccount: DatabaseObject {
        id client_id;
        date opened_date;
        double money;

        virtual BankAccount* lookFuture(date futureMoment, date_duration periodInterval) = 0;

    protected:
        struct PaymentsResult {
            date closest_payment;
            date latest_payment;
            size_t num_payments;
        };

        PaymentsResult getPaymentsBound(date today, date future_moment, date_duration period_interval) {
            size_t num_payments = 0;

            auto it = day_iterator(opened_date, period_interval.days());
            while (today > *it) {
                ++it;
            }
            date closest_payment = *it;
            while (future_moment >= *it) {
                ++it;
                num_payments += 1;
            }
            date last_payment = *(--it);
            return {closest_payment, last_payment, num_payments};
        }
    };

    struct DebitAccount: BankAccount {
        double interest_rate;
        double accumulated;

        virtual BankAccount* lookFuture(date future_moment, date_duration period_interval) override {
            DebitAccount* object = new DebitAccount();
            object->meta_info = new VirtualDataObjectMetaInfo();
            object->opened_date = opened_date;
            object->client_id = client_id;
            object->interest_rate = interest_rate;

            date today = day_clock::local_day();
            auto res = getPaymentsBound(today, future_moment, period_interval);

            double future_money = accumulated + money * (1 + interest_rate / daysPerYear * (res.closest_payment - today).days());
            future_money *= pow(1 + interest_rate / daysPerYear * period_interval.days(), res.num_payments - 1);
            object->money = future_money;

            return object;
        }

        AllowCreating;
    };

    struct DepositAccount: BankAccount {
        double interest_rate;
        double accumulated;
        date end_date;

        virtual BankAccount* lookFuture(date future_moment, date_duration period_interval) override {
            DepositAccount* object = new DepositAccount();
            object->meta_info = new VirtualDataObjectMetaInfo();
            object->opened_date = opened_date;
            object->client_id = client_id;
            object->interest_rate = interest_rate;

            date today = day_clock::local_day();
            future_moment = std::min(future_moment, end_date);
            auto res = getPaymentsBound(today, future_moment, period_interval);

            double future_money = accumulated + money * (1 + interest_rate / daysPerYear * (res.closest_payment - today).days());
            future_money *= pow(1 + interest_rate / daysPerYear * period_interval.days(), res.num_payments - 1);
            object->money = future_money;

            return object;
        }

        AllowCreating;
    };

    struct CreditAccount: BankAccount {
        double credit_limit;
        double commission;

        virtual BankAccount* lookFuture(date future_moment, date_duration period_interval) override {
            CreditAccount* object = new CreditAccount();
            object->meta_info = new VirtualDataObjectMetaInfo();
            object->opened_date = opened_date;
            object->client_id = client_id;
            object->commission = commission;
            object->credit_limit = credit_limit;
            object->money = money;
            return object;
        }

        AllowCreating;
    };

    struct Transaction: DatabaseObject {
        time_t time;
        double money;

        virtual std::vector<id> getAssociatedAccounts() = 0;
    };

    struct PutTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct SendTransaction: Transaction {
        id from_bank_account_id;
        id to_bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {from_bank_account_id, to_bank_account_id};
        }

        AllowCreating;
    };

    struct GetTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct DebitPercentsTransaction: Transaction {
        date begin_period;
        date end_period;
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct DepositEndTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct CreditPercentsTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct CancelTransaction: Transaction {
        id transaction_id;
        id bank_account_id;
        std::string reason;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };
}


//#include "pipeline.hpp"
#include <boost/filesystem.hpp>
#include <cstdlib>
#include <iostream>
#include <string>
#include <iterator>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/corefinement.h>
#include <fstream>

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

typedef CGAL::Exact_predicates_inexact_constructions_kernel K; /*inexact*/
typedef CGAL::Exact_predicates_exact_constructions_kernel EK;
typedef CGAL::Surface_mesh<K::Point_3> Mesh;
typedef CGAL::Surface_mesh<EK::Point_3> Mesh_exact;

using namespace std;

//#include <CGAL/MP_Float.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>

typedef K::Compare_dihedral_angle_3                    Compare_dihedral_angle_3;

typedef boost::graph_traits<Mesh>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor     edge_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor     face_descriptor;

//typedef CGAL::Quotient<CGAL::MP_Float> mp_number;

template <typename G>
struct Constraint : public boost::put_get_helper<bool,Constraint<G> >
{
  typedef typename boost::graph_traits<G>::edge_descriptor edge_descriptor;
  typedef boost::readable_property_map_tag      category;
  typedef bool                                  value_type;
  typedef bool                                  reference;
  typedef edge_descriptor                       key_type;

  Constraint()
    :g_(NULL)
  {}

  Constraint(G& g, double bound)
    : g_(&g), bound_(bound)
  {}

  bool operator[](edge_descriptor e) const
  {
    const G& g = *g_;
    return compare_(g.point(source(e, g)),
                    g.point(target(e, g)),
                    g.point(target(next(halfedge(e, g), g), g)),
                    g.point(target(next(opposite(halfedge(e, g), g), g), g)),
                   bound_) == CGAL::SMALLER;
  }

  const G* g_;
  Compare_dihedral_angle_3 compare_;
  double bound_;
};

struct halfedge2edge
{
  halfedge2edge(const Mesh& m, vector<edge_descriptor>& edges)
    : m_mesh(m), m_edges(edges)
  {}
  void operator()(const halfedge_descriptor& h) const
  {
    m_edges.push_back(edge(h, m_mesh));
  }
  const Mesh& m_mesh;
  vector<edge_descriptor>& m_edges;
};

using namespace CGAL ;

using namespace Polygon_mesh_processing;

template <typename PolygonMesh
        , typename NamedParameters>
size_t make_vector_of_connected_components(
    PolygonMesh& pmesh,
    pair<int, int> pair_,
    vector<PolygonMesh> &mesh_vec,
    vector<pair<int, int> >  &pair_vec,
    const NamedParameters& np
)
{
  typedef PolygonMesh PM;
  typedef typename boost::graph_traits<PM>::face_descriptor face_descriptor;
  using boost::choose_param;
  using boost::get_param;
                                                                            //FaceIndexMap
  typedef typename GetFaceIndexMap<PM,  NamedParameters>::type FaceIndexMap;
  FaceIndexMap fimap = choose_param(get_param(np, internal_np::face_index),
                                    get_property_map(boost::face_index, pmesh));
                                                                            //vector_property_map
  boost::vector_property_map<size_t, FaceIndexMap> face_cc(fimap);
  size_t num = connected_components(pmesh, face_cc, np);
  vector< pair<size_t, size_t> > component_size(num);

  for(size_t i=0; i < num; i++)
    component_size[i] = make_pair(i,0);

  BOOST_FOREACH(face_descriptor f, faces(pmesh))
    ++component_size[face_cc[f]].second;
                                                // we sort the range [0, num) by component size
  sort(component_size.begin(), component_size.end(), PMP::internal::MoreSecond());
  vector<size_t> cc_to_keep;

  for(size_t i=0; i<num; ++i) {
      PM tmp_mesh = pmesh;
      cc_to_keep.clear();
      cc_to_keep.push_back( component_size[i].first );
      keep_connected_components(tmp_mesh, cc_to_keep, face_cc, np);
      mesh_vec.push_back(tmp_mesh);
      pair_vec.push_back(pair_);
  }
  int indx=0;                                   // dbg verify written to mesh_vec
  BOOST_FOREACH(PM cc_mesh , mesh_vec){
      string filename =  "data/blobby_vec";
      filename +=  to_string(indx) + ".off";
      ofstream outfile(filename);
      outfile << cc_mesh;
      outfile.close();
      indx++;
  }
  return num;
}

int mkvec_cc(
        vector<Mesh_exact> &patch_vec,
        vector<pair<int, int> >  &pair_vec,
        std::string out_dir
            )
{
    boost::filesystem::path p (out_dir.c_str());
    vector<Mesh_exact> new_patch_vec;
    vector<Mesh_exact> new_patch_vec2;
    vector<pair<int, int> >  new_pair_vec;
    const double bound = cos(0.75 * CGAL_PI);

    int idx =0;
    BOOST_FOREACH(Mesh_exact pmesh, patch_vec){
        make_vector_of_connected_components(pmesh, pair_vec.at(idx), new_patch_vec, new_pair_vec,
                PMP::parameters::edge_is_constrained_map(Constraint<Mesh_exact>(pmesh, bound)) );
        idx++;    // no match for epick  epeck
    }

    for(int i=0; i< patch_vec.size() ;i++){
        std::string filename = p.c_str();
        filename += "/patch_num_" + to_string(i) + ".off";
        std::ofstream output(filename);
        output << patch_vec.at(i);
        output.close();
    }

    patch_vec.swap(new_patch_vec); /*new_patch_vec2*/
    pair_vec.swap(new_pair_vec);
    return patch_vec.size();
}

// int iso_remesh(Mesh &nmesh, unsigned int nb_iter, double target_edge_length){
//     vector<edge_descriptor> border;
//     PMP::border_halfedges(
//                 faces(nmesh),
//                 nmesh,
//                 boost::make_function_output_iterator(halfedge2edge(nmesh, border))
//                 );
//     PMP::split_long_edges(border, target_edge_length, nmesh);
//
//     PMP::isotropic_remeshing(
//                 faces(nmesh),
//                 target_edge_length,
//                 nmesh,                                    //protect border, here
//                 PMP::parameters::number_of_iterations(nb_iter).protect_constraints(true)
//                 );
// }


#include <boost/config.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <iostream>

int main() {
    static const std::string provider =
#ifdef BOOST_WINDOWS
        "Microsoft Strong Cryptographic Provider"
#else
        "/dev/urandom"
#endif
    ;

    boost::random_device device(provider);
    boost::random::uniform_int_distribution<unsigned short> random(1000);

    for (unsigned int i = 0; i < 100; ++i) {
        std::cerr << random(device) << '\t';
    }
}


/******************************************************************************
 * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * 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 "feature_utils.h"
#include <Eigen/Dense>

namespace XP {
namespace warp {

using Eigen::Vector2f;
using Eigen::Vector3f;
using Eigen::Matrix2f;
using Eigen::Matrix3f;

// Compute affine warp matrix A_ref_cur
// The warping matrix is warping the ref patch (at level_ref) to the current frame (at pyr 0)
//输入左右相机类,左相机像素坐标,归一化坐标,估计的深度,特征点所在的金字塔层,外参
bool getWarpMatrixAffine(const vio::cameras::CameraBase& cam_ref,
                         const vio::cameras::CameraBase& cam_cur,
                         const Vector2f& px_ref,  // distorted pixel at pyr0
                         const Vector3f& f_ref,   // undist ray in unit plane
                         const float depth_ref,
                         const Matrix3f& R_cur_ref,
                         const Vector3f& t_cur_ref,
                         const int level_ref,
                         Eigen::Matrix2f* A_cur_ref) {
  CHECK_NOTNULL(A_cur_ref);
  // TODO(mingyu): tune the *d_unit* size in pixel for different 1st order approximation
  const int halfpatch_size = 5;
  const Vector3f xyz_ref(f_ref * depth_ref);// 特征点在左相机坐标中的位置
  //这个是一半块的大小,在不同的金子塔层patch的大小也是需要缩放的
  float d_unit = halfpatch_size * (1 << level_ref);
  //这里在算以px_ref为原点,uv的方向
  Vector2f du_ref(px_ref + Vector2f(d_unit, 0));
  Vector2f dv_ref(px_ref + Vector2f(0, d_unit));
  Vector3f xyz_du_ref, xyz_dv_ref;
  //反投影
  if (cam_ref.backProject(du_ref, &xyz_du_ref) && cam_ref.backProject(dv_ref, &xyz_dv_ref))
  {
    // Make sure the back project succeed for both du_ref & dv_ref
    //初始深度
    xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2];
    xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2];
    Vector2f px_cur, du_cur, dv_cur;
      // 利用外参把这三点变换到右相机坐标系下
    if (vio::cameras::CameraBase::ProjectionStatus::Successful ==
        cam_cur.project(R_cur_ref * xyz_ref + t_cur_ref, &px_cur) &&
        vio::cameras::CameraBase::ProjectionStatus::Successful ==
            cam_cur.project(R_cur_ref * xyz_du_ref + t_cur_ref, &du_cur) &&
        vio::cameras::CameraBase::ProjectionStatus::Successful ==
            cam_cur.project(R_cur_ref * xyz_dv_ref + t_cur_ref, &dv_cur)) {
        //如果都投影成功的话,计算仿射变换(每列就是某轴变换以后的方向)
      A_cur_ref->col(0) = (du_cur - px_cur) / halfpatch_size;
      A_cur_ref->col(1) = (dv_cur - px_cur) / halfpatch_size;
      return true;
    }
  }
  A_cur_ref->setIdentity();  // No warping
  return false;
}
// 找到合适金字塔层
// Compute patch level in other image (based on pyramid level 0)
int getBestSearchLevel(const Eigen::Matrix2f& A_cur_ref,
                       const int max_level) {
  int search_level = 0;
  float D = A_cur_ref.determinant();
  //行列式小于3为止
  while (D > 3.f && search_level < max_level) {
    ++search_level;
    D *= 0.25;
  }
  return search_level;
}

namespace {
// Return value between 0 and 255
// [NOTE] Does not check whether the x/y is within the border
inline float interpolateMat_8u(const cv::Mat& mat, float u, float v) {
  CHECK_EQ(mat.type(), CV_8U);
  int x = floor(u);
  int y = floor(v);
  float subpix_x = u - x;
  float subpix_y = v - y;

  float w00 = (1.0f - subpix_x) * (1.0f - subpix_y);
  float w01 = (1.0f - subpix_x) * subpix_y;
  float w10 = subpix_x * (1.0f - subpix_y);
  float w11 = 1.0f - w00 - w01 - w10;

  const int stride = mat.step.p[0];
  uint8_t* ptr = mat.data + y * stride + x;
  return w00 * ptr[0] + w01 * ptr[stride] + w10 * ptr[1] + w11 * ptr[stride+1];
}
}  // namespace

//将左相机图像特征点中心的图像块warp到右相机图像坐标系中
// Compute acc squared patch that is *warperd* from img_ref with A_cur_ref.
//输入之前得到的粗略的仿射矩阵,左相机特征点所在所在金字塔图像,左特征点像素坐标,左特征的金字塔层，右相机需要搜索的金字塔层,
bool warpAffine(const Eigen::Matrix2f& A_cur_ref,
                const cv::Mat& img_ref,         // at pyramid level_ref
                const Eigen::Vector2f& px_ref,  // at pyramid 0
                const int level_ref,
                const int level_cur,
                const int halfpatch_size,
                uint8_t* patch) {
  const int patch_size = halfpatch_size * 2;
  const Matrix2f A_ref_cur = A_cur_ref.inverse();
  if (std::isnan(A_ref_cur(0, 0))) {
    // TODO(mingyu): Use looser criteria for invalid affine warp?
    //               I suspect A_ref_cur can barely hit NaN.
    LOG(ERROR) << "Invalid affine warp matrix (NaN)";
    return false;
  }

  // px_ref is at pyr0, img_ref is at level_ref pyr already
  CHECK_NOTNULL(patch);
  uint8_t* patch_ptr = patch;
  const Vector2f px_ref_pyr = px_ref / (1<< level_ref);  // pixel at pyramid level_ref//变换到对应的金字塔层坐标上
  for (int y = 0; y < patch_size; ++y)
  {
    for (int x = 0; x < patch_size; ++x, ++patch_ptr)// // 以建立patch坐标系
    {
      Vector2f px_patch(x - halfpatch_size, y - halfpatch_size);
      px_patch *= (1 << level_cur);//缩放
      const Vector2f px(A_ref_cur * px_patch + px_ref_pyr);  // pixel at pyramid level_ref
      if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || px[1] >= img_ref.rows - 1) {
        *patch_ptr = 0;
      } else {
        *patch_ptr = interpolateMat_8u(img_ref, px[0], px[1]);//将左相机图像warp到右相机图像坐标系中
      }
    }
  }
  return true;
}

}  // namespace warp
}  // namespace XP


/*
 * Copyright 2018, LAAS-CNRS
 * Author: Steve Tonneau
 */

#ifndef BEZIER_COM_TRAJ_LIB_UTILS_H
#define BEZIER_COM_TRAJ_LIB_UTILS_H

#include <hpp/bezier-com-traj/local_config.hh>
#include <hpp/bezier-com-traj/definitions.hh>
#include <hpp/bezier-com-traj/flags.hh>

#include <Eigen/Dense>

#include <vector>

namespace bezier_com_traj
{

template<typename T> T initwp();
waypoint_t initwp(const size_t rows, const size_t cols);
waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2);
waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2);
waypoint_t operator*(const double k, const waypoint_t& w);
waypoint_t operator*(const waypoint_t& w,const double k);

struct waypoint_t{
    MatrixXX first;
    VectorX second;

    waypoint_t():first(MatrixXX()),second(VectorX())
    {}

    waypoint_t(MatrixXX A,VectorX b):first(A),second(b)
    {}

    static waypoint_t Zero(size_t dim){
        return initwp(dim,dim);
    }

};

/**
 * @brief Compute the Bernstein polynoms for a given degree
 * @param degree required degree
 * @return the bernstein polynoms
 */
BEZIER_COM_TRAJ_DLLAPI std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);

/**
 * @brief given the constraints of the problem, and a set of waypoints, return
 * the bezier curve corresponding
 * @param pData problem data
 * @param T total trajectory time
 * @param pis list of waypoints
 * @return the bezier curve
 */
template<typename Bezier, typename Point>
BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag, const double T,
                                                   const std::vector<Point>& pi, const Point& x);

/**
 * @brief computeDiscretizedTime build an array of discretized points in time,
 * such that there is the same number of point in each phase. Doesn't contain t=0,
 * is of size pointsPerPhase*phaseTimings.size()
 * @param phaseTimings
 * @param pointsPerPhase
 * @return
 */
T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase );

/**
 * @brief computeDiscretizedTime build an array of discretized points in time,
 * given the timestep. Doesn't contain t=0,
 * is of size pointsPerPhase*phaseTimings.size()
 * @param phaseTimings
 * @param timeStep
 * @return */
T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep);

/**
 * @brief write a polytope describe by A x <= b linear constraints in
 * a given filename
 * @return the bernstein polynoms
 */
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,
                    const std::string& fileName,bool clipZ = false);

/**
 * @brief skew symmetric matrix
 */
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);

/**
 * @brief normalize inequality constraints
 */
int Normalize(Ref_matrixXX A, Ref_vectorX b);

} // end namespace bezier_com_traj

template<typename Bezier, typename Point>
Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const double T,
                                           const std::vector<Point>& pi, const Point& x)
{
    std::vector<Point> wps;
    size_t i = 0;
    if(flag & INIT_POS ){
        wps.push_back(pi[i]);
        i++;
        if(flag & INIT_VEL){
            wps.push_back(pi[i]);
            i++;
            if(flag & INIT_ACC){
                wps.push_back(pi[i]);
                i++;
            }
        }
    }
    wps.push_back(x);
    i++;
    if(flag & (END_VEL) && !(flag & (END_POS) ))
    {
        wps.push_back(x);
        i++;
    }
    else
    {
        if(flag & END_ACC){
            assert(flag & END_VEL && "You cannot constrain final acceleration if final velocity is not constrained.");
            wps.push_back(pi[i]);
            i++;
        }
        if(flag & END_VEL){
            assert(flag & END_POS && "You cannot constrain final velocity if final position is not constrained.");
            wps.push_back(pi[i]);
            i++;
        }
        if(flag & END_POS){
            wps.push_back(pi[i]);
            i++;
        }
    }
    return Bezier (wps.begin(), wps.end(),T);
}

#endif


/* boost random/uniform_on_sphere.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_on_sphere.hpp,v 1.9 2002/12/22 22:03:11 jmaurer Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP
#define BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP

#include <vector>
#include <algorithm>     // std::transform
#include <functional>    // std::bind2nd, std::divides
#include <boost/random/normal_distribution.hpp>

namespace boost {

template<class UniformRandomNumberGenerator, class RealType = double,
         class Cont = std::vector<RealType>,
         class Adaptor = uniform_01<UniformRandomNumberGenerator, RealType> >
class uniform_on_sphere
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef Cont result_type;

  explicit uniform_on_sphere(base_type & rng, int dim = 2)
    : _rng(rng), _container(dim), _dim(dim) { }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng.adaptor(); }
  base_type& base() const { return _rng.base(); }
  void reset() { _rng.reset(); }

  const result_type & operator()()
  {
    RealType sqsum = 0;
    for(typename Cont::iterator it = _container.begin();
        it != _container.end();
        ++it) {
      RealType val = _rng();
      *it = val;
      sqsum += val * val;
    }
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    // for all i: result[i] /= sqrt(sqsum)
    std::transform(_container.begin(), _container.end(), _container.begin(),
                   std::bind2nd(std::divides<RealType>(), sqrt(sqsum)));
    return _container;
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const uniform_on_sphere& x,
                         const uniform_on_sphere& y)
  { return x._dim == y._dim && x._rng == y._rng; }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_on_sphere& sd)
  {
    os << sd._dim;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_on_sphere& sd)
  {
    is >> std::ws >> sd._dim;
    sd._container.resize(sd._dim);
    return is;
  }
#endif

#else
  // Use a member function
  bool operator==(const uniform_on_sphere& rhs) const
  { return _dim == rhs._dim && _rng == rhs._rng; }
#endif
private:
  normal_distribution<base_type, RealType, Adaptor> _rng;
  result_type _container;
  int _dim;
};

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_ON_SPHERE_HPP


// Copyright (c) 2021 Marcus Valtonen Örnhag
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <Eigen/Dense>
#include <vector>
#include <algorithm>
#include "get_valtonenornhag_arxiv_2021.hpp"
#include "normalize2dpts.hpp"
#include "radial.hpp"
#include "relpose.hpp"
#include "solver_frEfr.hpp"

namespace DronePoseLib {
namespace ValtonenOrnhagArxiv2021 {
    inline Eigen::Vector3d extract_translation(
        const double f,
        const double r,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2,
        const Eigen::Matrix2d &x1,
        const Eigen::Matrix2d &x2);

    std::vector<RelPose> get_frEfr(
        const Eigen::MatrixXd &p1,
        const Eigen::MatrixXd &p2,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2,
        const bool use_fast_solver
    ) {
        // This is a 4-point method
        const int nbr_pts = 4;

        // We expect inhomogenous input data, i.e. p1 and p2 are 2x3 matrices
        assert(p1.rows() == 2);
        assert(p2.rows() == 2);
        assert(p1.cols() == nbr_pts);
        assert(p2.cols() == nbr_pts);

        // Compute normalization matrix
        double scale1 = normalize2dpts(p1);
        double scale2 = normalize2dpts(p2);
        double scale = std::max(scale1, scale2);
        Eigen::Vector3d s;
        s << scale, scale, 1.0;
        Eigen::DiagonalMatrix<double, 3> S = s.asDiagonal();

        // Normalize data
        Eigen::Matrix<double, 3, nbr_pts> x1;
        Eigen::Matrix<double, 3, nbr_pts> x2;
        x1 = p1.colwise().homogeneous();
        x2 = p2.colwise().homogeneous();
        x1 = S * x1;
        x2 = S * x2;

        Eigen::Matrix<double, 2, nbr_pts> x1t;
        Eigen::Matrix<double, 2, nbr_pts> x2t;
        x1t << x1.colwise().hnormalized();
        x2t << x2.colwise().hnormalized();

        // Compute relative rotation
        Eigen::Matrix3d R = R2 * R1.transpose();

        // Wrap input data to expected format
        Eigen::VectorXd input(25);
        input << Eigen::Map<Eigen::VectorXd>(x1t.data(), 8),
                 Eigen::Map<Eigen::VectorXd>(x2t.data(), 8),
                 Eigen::Map<Eigen::VectorXd>(R.data(), 9);

        // Extract solution
        Eigen::MatrixXcd sols = DronePoseLib::ValtonenOrnhagArxiv2021::solver_frEfr(input, use_fast_solver);

        // Pre-processing: Remove complex-valued solutions
        double thresh = 1e-12;
        Eigen::ArrayXd real_sols(11);
        real_sols = sols.imag().cwiseAbs().colwise().sum();
        int nbr_real_sols = (real_sols <= thresh).count();

        // Construct putative output
        // Eigen::Vector3d t;
        std::vector<RelPose> output;
        RelPose relpose;
        double f, r;
        Eigen::Vector3d kinv;
        Eigen::DiagonalMatrix<double, 3> Kinv;
        Eigen::Matrix3d skew_t;

        // Loop over real solutions
        for (int i=0; i < real_sols.size(); i++) {
            if (real_sols(i) < thresh) {
                f = sols(0, i).real();
                r = sols(1, i).real();

                // Extract translation
                relpose.t = extract_translation(f, r, R1, R2, x1t.leftCols<2>(), x2t.leftCols<2>());
                relpose.f = f / scale;
                relpose.r = r * std::pow(scale, 2);

                // Compute fundamental matrix
                kinv << 1.0 / relpose.f, 1.0 / relpose.f, 1.0;
                Kinv = kinv.asDiagonal();
                skew_t << 0, -relpose.t(2), relpose.t(1),
                          relpose.t(2), 0, -relpose.t(0),
                         -relpose.t(1), relpose.t(0), 0;
                relpose.F = Kinv * skew_t * R * Kinv;

                // Add
                output.push_back(relpose);
            }
        }

        return output;
    }

    inline Eigen::Vector3d extract_translation(
        const double f,
        const double r,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2,
        const Eigen::Matrix2d &x1,
        const Eigen::Matrix2d &x2
    ) {
        Eigen::Vector3d fmat;
        fmat << 1.0 / f, 1.0 / f, 1.0;
        Eigen::DiagonalMatrix<double, 3> Kinv = fmat.asDiagonal();

        // Transform points
        Eigen::Matrix<double, 3, 2> y1, y2;
        y1 = R1.transpose() * Kinv * DronePoseLib::radialundistort(x1, r).colwise().homogeneous();
        y2 = R2.transpose() * Kinv * DronePoseLib::radialundistort(x2, r).colwise().homogeneous();

        // Extract translation
        Eigen::Vector3d t;
        t = R2 * y1.col(0).cross(y2.col(0)).cross(y1.col(1).cross(y2.col(1)));
        return t;
    }
}  // namespace ValtonenOrnhagArxiv2021
}  // namespace DronePoseLib


#include "Surfelizer.hpp"

#include "Utils.hpp"

#include <boost/circular_buffer.hpp>

using namespace maps;

struct Surfelizer::Helper {
  struct ScanData {
    std::vector<Eigen::Vector3f> mPoints;
    std::vector<float> mRanges;
    std::vector<bool> mValid;
    Eigen::Isometry3f mPose;
    float mIntraScanAngle;
  };

  int mScanRadius;
  int mPointRadius;
  SizeMethod mSizeMethod;
  float mNominalSize;
  int mDecimation;
  boost::circular_buffer<ScanData> mDataBuffer;

  std::vector<Surfel> addScan(const maps::PointSet& iScan) {
    // cache some basic information
    ScanData data;
    data.mPose = Utils::getPose(*iScan.mCloud);
    data.mRanges.resize(iScan.mCloud->size());
    data.mValid.resize(iScan.mCloud->size());
    data.mPoints.resize(iScan.mCloud->size());
    data.mIntraScanAngle = 0;
    int counter = 0;
    for (int i = 0; i < iScan.mCloud->size(); ++i) {
      data.mPoints[i] = data.mPose*(*(iScan.mCloud))[i].getVector3fMap();
      Eigen::Vector3f ray = data.mPoints[i] - data.mPose.translation();
      data.mRanges[i] = ray.norm();
      data.mValid[i] = (data.mRanges[i] >= iScan.mMinRange) &&
        (data.mRanges[i] <= iScan.mMaxRange);
      if ((i>0) && data.mValid[i] && data.mValid[i-1]) {
        Eigen::Vector3f ray2 = data.mPoints[i-1] - data.mPose.translation();
        data.mIntraScanAngle += acos(ray.normalized().dot(ray2.normalized()));
        ++counter;
      }
    }
    data.mIntraScanAngle /= counter;
    mDataBuffer.push_back(data);

    // check to see whether we have enough scans in the buffer to proceed
    std::vector<Surfel> surfels;
    int r1 = mScanRadius;
    int r2 = mPointRadius;
    int w1 = 2*r1+1;
    int w2 = 2*r2+1;
    if (mDataBuffer.size() < w1) return surfels;

    const ScanData& curScan = mDataBuffer[r1];
    surfels.reserve(curScan.mPoints.size());
    std::vector<Eigen::Vector3f> points;
    points.reserve(w1*w2);

    float interScanAngle =
      acos(mDataBuffer[r1-1].mPose.linear().col(2).dot
           (mDataBuffer[r1+1].mPose.linear().col(2))) / 2;

    // loop over scan points
    int numPoints = curScan.mPoints.size();
    for (int i = r2; i < numPoints - r2; i += mDecimation) {
      if (!curScan.mValid[i]) continue;

      // initiate surfel
      Surfel surfel;
      surfel.mCenter = curScan.mPoints[i];

      // approximate surfel orientation
      points.clear();
      for (int j = 0; j < w1; ++j) {
        int kMax = std::min(i+r2, (int)mDataBuffer[j].mValid.size()-1);
        for (int k = i-r2; k <= kMax; ++k) {
          if (mDataBuffer[j].mValid[k]) {
            points.push_back(mDataBuffer[j].mPoints[k]);
          }
        }
      }
      if (points.size() < 3) continue;
      surfel.mOrientation = estimateOrientation(points,curScan.mPose.linear());

      // approximate surfel size
      surfel.mSize = Eigen::Vector2f(mNominalSize, mNominalSize);
      if (mSizeMethod == SizeMethodRange) {
        surfel.mSize *= curScan.mRanges[i];
      }
      else if (mSizeMethod == SizeMethodAngles) {
        float range = curScan.mRanges[i];
        Eigen::Vector3f xPt = curScan.mPose.translation() +
          curScan.mPose.linear().col(0)*range;
        float dist = (xPt-surfel.mCenter).norm();
        surfel.mSize = Eigen::Vector2f(interScanAngle*dist,
                                       curScan.mIntraScanAngle*range);
      }
      else if (mSizeMethod == SizeMethodNeighbors) {
        if (!mDataBuffer[r1+1].mValid[i] || !mDataBuffer[r1-1].mValid[i] ||
            !mDataBuffer[r1].mValid[i-1] || !mDataBuffer[r1].mValid[i+1]) {
          continue;
        }
        float d1 = (mDataBuffer[r1+1].mPoints[i] -
                    mDataBuffer[r1-1].mPoints[i]).norm();
        float d2 = (mDataBuffer[r1].mPoints[i-1] -
                    mDataBuffer[r1].mPoints[i+1]).norm();
        surfel.mSize = Eigen::Vector2f(d1/2,d2/2);
      }

      surfels.push_back(surfel);
    }
    return surfels;
  }

  Eigen::Matrix3f
  estimateOrientation(const std::vector<Eigen::Vector3f>& iPoints,
                      const Eigen::Matrix3f& iScanOrientation) {
    Eigen::Vector3f mean = Eigen::Vector3f::Zero();
    Eigen::Matrix3f meanSq = Eigen::Matrix3f::Zero();
    int n = iPoints.size();
    for (int i = 0; i < n; ++i) {
      mean += iPoints[i];
      meanSq += iPoints[i]*iPoints[i].transpose();
    }
    mean /= n;
    meanSq /= n;
    Eigen::Matrix3f cov = meanSq - mean*mean.transpose();
    Eigen::JacobiSVD<Eigen::Matrix3f> svd;
    svd.compute(cov, Eigen::ComputeFullV);
    Eigen::Matrix3f v = svd.matrixV();

    Eigen::Matrix3f orientation;
    orientation.col(2) = v.col(2);
    orientation.col(1) = v.col(2).cross(iScanOrientation.col(2));
    orientation.col(0) = orientation.col(1).cross(v.col(2));
    for (int k = 0; k < 3; ++k) orientation.col(k).normalize();
    return orientation;
  }

};

Surfelizer::
Surfelizer() {
  mHelper.reset(new Helper());
  setScanRadius(1);
  setPointRadius(1);
  setSizeMethod(SizeMethodAngles);
  setNominalSize(1);
  setDecimation(1);
}

Surfelizer::
~Surfelizer() {
}

void Surfelizer::
setScanRadius(const int iRadius) {
  mHelper->mScanRadius = iRadius;
  mHelper->mDataBuffer.set_capacity(2*iRadius+1);
}

void Surfelizer::
setPointRadius(const int iRadius) {
  mHelper->mPointRadius = iRadius;
}

void Surfelizer::
setSizeMethod(const SizeMethod iMethod) {
  mHelper->mSizeMethod = iMethod;
}

void Surfelizer::
setNominalSize(const float iSize) {
  mHelper->mNominalSize = iSize;
}

void Surfelizer::
setDecimation(const int iDecimation) {
  mHelper->mDecimation = iDecimation;
}

std::vector<Surfelizer::Surfel> Surfelizer::
addScan(const maps::PointSet& iScan) {
  return mHelper->addScan(iScan);
}


// Boost.Range 2.0 Extension library
// via PStade Oven Library
//
// Copyright Akira Takahashi 2011.
// Copyright Shunsuke Sogame 2005-2007.
// 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 <iostream>
#include <boost/detail/lightweight_test.hpp>
#include <boost/range/algorithm/equal.hpp>

#include <vector>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/taken.hpp>

#include <boost/range/adaptor/regular_extension/filtered.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/dropped.hpp>
#include <boost/range/access/front.hpp>
#include <boost/range/iteration.hpp>
#include <boost/range/any_range.hpp>
#include <boost/lambda/lambda.hpp>

typedef
    boost::any_range<int, boost::single_pass_traversal_tag, int, std::ptrdiff_t>
range;

using boost::lambda::_1;
using namespace boost::adaptors;
using boost::range::access::value_front;

range sieve(range r)
{
    return r | dropped(1) |+ filtered(_1 % value_front(r) != 0);
}

int main()
{
    range primes = boost::iteration(range(boost::iteration(2, boost::regular(_1 + 1))), sieve)
                     | transformed(value_front);

    BOOST_TEST(boost::equal(
        primes | taken(5),
        boost::assign::list_of(2)(3)(5)(7)(11)
    ));

    return boost::report_errors();
}


#include <sparse.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(element_prod_test)
{
  typedef sparse::Block<2,1,float>   block_type;
  typedef sparse::Vector<block_type> vector_type;

  vector_type a;
  vector_type b;
  vector_type c;

  a.resize(2);
  b.resize(2);
  c.resize(2);

  a(0)(0) = 1.0f;
  a(0)(1) = 2.0f;
  a(1)(0) = 3.0f;
  a(1)(1) = 4.0f;

  b(0)(0) = 4.0f;
  b(0)(1) = 3.0f;
  b(1)(0) = 2.0f;
  b(1)(1) = 1.0f;

  c(0)(0) = 1.0f;
  c(0)(1) = 1.0f;
  c(1)(0) = 1.0f;
  c(1)(1) = 1.0f;

  sparse::element_prod( a, b, c);

  BOOST_CHECK( c(0)(0) == 4.0f );
  BOOST_CHECK( c(0)(1) == 6.0f );
  BOOST_CHECK( c(1)(0) == 6.0f );
  BOOST_CHECK( c(1)(1) == 4.0f );

}

BOOST_AUTO_TEST_SUITE_END();


/*
 *
 * matrix.hpp
 * functions in matrix_ops.cpp
 *
 */
#pragma once

#include <vector>
#include <cstdint>
#include <cstddef>
#include <string>

#include <Eigen/Dense>

#include "matrix_idx.hpp"

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> NumpyMatrix;
typedef std::tuple<std::vector<long>, std::vector<long>, std::vector<float>> sparse_coo;
typedef std::tuple<std::vector<long>, std::vector<long>, std::vector<long>> network_coo;

NumpyMatrix long_to_square(const Eigen::VectorXf &rrDists,
                           const Eigen::VectorXf &qrDists,
                           const Eigen::VectorXf &qqDists,
                           unsigned int num_threads = 1);

Eigen::VectorXf square_to_long(const NumpyMatrix &squareDists,
                               const unsigned int num_threads);

sparse_coo sparsify_dists(const NumpyMatrix &denseDists,
                          const float distCutoff,
                          const unsigned long int kNN,
                          bool reciprocal_only,
                          bool all_neighbours);


/**
 * @date Sat Mar 19 22:14:10 2011 +0100
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <boost/shared_array.hpp>

#include <bob.math/svd.h>

#include <bob.core/assert.h>
#include <bob.core/check.h>
#include <bob.core/array_copy.h>

// Declaration of the external LAPACK function (Divide and conquer SVD)
extern "C" void dgesdd_( const char *jobz, const int *M, const int *N,
  double *A, const int *lda, double *S, double *U, const int* ldu, double *VT,
  const int *ldvt, double *work, const int *lwork, int *iwork, int *info);

// Declaration of the external LAPACK function ('Slow' but 'safe' SVD)
extern "C" void dgesvd_( const char *jobu, const char *jobvt, const int *M,
  const int *N, double *A, const int *lda, double *S, double *U,
  const int* ldu, double *VT, const int *ldvt, double *work, const int *lwork,
  int *info);

static void svd_lapack( const char jobz, const int M, const int N,
  double *A, const int lda, double *S, double *U, const int ldu, double *VT,
  const int ldvt, const bool safe)
{
  // Calls the LAPACK function:
  // We use dgesdd by default which is faster than its predecessor dgesvd,
  // when computing the singular vectors.
  //   (cf. http://www.netlib.org/lapack/lug/node71.html)
  // However, dgesdd is failing on some matrices:
  //   see #171: http://github.com/idiap/bob/issues/171
  // Please note that matlab is relying on dgesvd.
  int info = 0;
  if (safe) {
    // A/ Queries the optimal size of the working array
    const int lwork_query = -1;
    double work_query;
    dgesvd_( &jobz, &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, &work_query, &lwork_query, &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesvd function returned a non-zero value.");

    // B/ Computes
    const int lwork = static_cast<int>(work_query);
    boost::shared_array<double> work(new double[lwork]);
    dgesvd_( &jobz, &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, work.get(), &lwork, &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesvd function returned a non-zero value.");
  }
  else {
    // Integer (workspace) array, dimension (8*min(M,N))
    const int l_iwork = 8*std::min(M,N);
    boost::shared_array<int> iwork(new int[l_iwork]);

    // A/ Queries the optimal size of the working array
    const int lwork_query = -1;
    double work_query;
    dgesdd_( &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, &work_query, &lwork_query, iwork.get(), &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesdd function returned a non-zero value. You may consider using LAPACK dgsevd instead (see #171) by enabling the 'safe' option.");

    // B/ Computes
    const int lwork = static_cast<int>(work_query);
    boost::shared_array<double> work(new double[lwork]);
    dgesdd_( &jobz, &M, &N, A, &lda, S, U, &ldu,
      VT, &ldvt, work.get(), &lwork, iwork.get(), &info );
    // Check info variable
    if (info != 0)
      throw std::runtime_error("The LAPACK dgesdd function returned a non-zero value. You may consider using LAPACK dgsevd instead (see #171) by enabling the 'safe' option.");
  }

  // Defining the sign of the eigenvectors
  // Approch extracted from page 8 - http://prod.sandia.gov/techlib/access-control.cgi/2007/076422.pdf
  if(U[0] < 0){
    int ucol=0; ucol= (jobz=='A')? M : std::min(M,N);
    for (int i=0; i<ldu*ucol; i++){
      U[i] = -1*U[i];
    }

    for (int i=0; i<ldvt*N; i++){
      VT[i] = -1*VT[i];
    }
  }
}

void bob::math::svd(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, blitz::Array<double,2>& Vt, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Checks zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(U);
  bob::core::array::assertZeroBase(sigma);
  bob::core::array::assertZeroBase(Vt);
  // Checks and resizes if required
  bob::core::array::assertSameDimensionLength(U.extent(0), M);
  bob::core::array::assertSameDimensionLength(U.extent(1), M);
  bob::core::array::assertSameDimensionLength(sigma.extent(0), nb_singular);
  bob::core::array::assertSameDimensionLength(Vt.extent(0), N);
  bob::core::array::assertSameDimensionLength(Vt.extent(1), N);

  bob::math::svd_(A, U, sigma, Vt, safe);
}

void bob::math::svd_(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, blitz::Array<double,2>& Vt, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Prepares to call LAPACK function:
  // We will decompose A^T rather than A to reduce the required number of copy
  // We recall that FORTRAN/LAPACK is column-major order whereas blitz arrays
  // are row-major order by default.
  // If A = U.S.V^T, then A^T = V.S.U^T

  // Initialises LAPACK variables
  const char jobz = 'A'; // Get All left singular vectors
  const int lda = N;
  const int ldu = N;
  const int ldvt = M;
  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack(bob::core::array::ccopy(A));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use U, Vt and S directly to limit the number of copy()
  // S_lapack = S
  blitz::Array<double,1> S_blitz_lapack;
  const bool sigma_direct_use = bob::core::array::isCZeroBaseContiguous(sigma);
  if (!sigma_direct_use) S_blitz_lapack.resize(nb_singular);
  else                   S_blitz_lapack.reference(sigma);
  double *S_lapack = S_blitz_lapack.data();
  // U_lapack = V^T
  blitz::Array<double,2> U_blitz_lapack;
  const bool U_direct_use = bob::core::array::isCZeroBaseContiguous(Vt);
  if (!U_direct_use) U_blitz_lapack.resize(N,N);
  else               U_blitz_lapack.reference(Vt);
  double *U_lapack = U_blitz_lapack.data();
  // V^T_lapack = U
  blitz::Array<double,2> VT_blitz_lapack;
  const bool VT_direct_use = bob::core::array::isCZeroBaseContiguous(U);
  if (!VT_direct_use) VT_blitz_lapack.resize(M,M);
  else                VT_blitz_lapack.reference(U);
  double *VT_lapack = VT_blitz_lapack.data();

  // Call the LAPACK function
  svd_lapack(jobz, N, M, A_lapack, lda, S_lapack, U_lapack, ldu,
    VT_lapack, ldvt, safe);

  // Copy singular vectors back to U, V and sigma if required
  if (!U_direct_use)  Vt = U_blitz_lapack;
  if (!VT_direct_use) U = VT_blitz_lapack;
  if (!sigma_direct_use) sigma = S_blitz_lapack;
}

void bob::math::svd(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Checks zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(U);
  bob::core::array::assertZeroBase(sigma);
  // Checks and resizes if required
  bob::core::array::assertSameDimensionLength(U.extent(0), M);
  bob::core::array::assertSameDimensionLength(U.extent(1), nb_singular);
  bob::core::array::assertSameDimensionLength(sigma.extent(0), nb_singular);

  bob::math::svd_(A, U, sigma, safe);
}

void bob::math::svd_(const blitz::Array<double,2>& A, blitz::Array<double,2>& U,
  blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Prepares to call LAPACK function

  // Initialises LAPACK variables
  const char jobz = 'S'; // Get first min(M,N) columns of U
  const int lda = M;
  const int ldu = M;
  const int ldvt = std::min(M,N);

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack(bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use U and S directly to limit the number of copy()
  // S_lapack = S
  blitz::Array<double,1> S_blitz_lapack;
  const bool sigma_direct_use = bob::core::array::isCZeroBaseContiguous(sigma);
  if (!sigma_direct_use) S_blitz_lapack.resize(nb_singular);
  else                   S_blitz_lapack.reference(sigma);
  double *S_lapack = S_blitz_lapack.data();
  // U_lapack = U^T
  blitz::Array<double,2> U_blitz_lapack;
  blitz::Array<double,2> Ut = U.transpose(1,0);
  const bool U_direct_use = bob::core::array::isCZeroBaseContiguous(Ut);
  if (!U_direct_use) U_blitz_lapack.resize(nb_singular,M);
  else               U_blitz_lapack.reference(Ut);
  double *U_lapack = U_blitz_lapack.data();
  boost::shared_array<double> VT_lapack(new double[nb_singular*N]);

  // Call the LAPACK function
  svd_lapack(jobz, M, N, A_lapack, lda, S_lapack, U_lapack, ldu,
    VT_lapack.get(), ldvt, safe);

  // Copy singular vectors back to U, V and sigma if required
  if (!U_direct_use) Ut = U_blitz_lapack;
  if (!sigma_direct_use) sigma = S_blitz_lapack;
}

void bob::math::svd(const blitz::Array<double,2>& A, blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Checks zero base
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(sigma);
  // Checks and resizes if required
  bob::core::array::assertSameDimensionLength(sigma.extent(0), nb_singular);

  bob::math::svd_(A, sigma, safe);
}

void bob::math::svd_(const blitz::Array<double,2>& A, blitz::Array<double,1>& sigma, bool safe)
{
  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int nb_singular = std::min(M,N);

  // Prepares to call LAPACK function

  // Initialises LAPACK variables
  const char jobz = 'N'; // Get first min(M,N) columns of U
  const int lda = M;
  const int ldu = M;
  const int ldvt = std::min(M,N);

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();
  // Tries to use S directly to limit the number of copy()
  // S_lapack = S
  blitz::Array<double,1> S_blitz_lapack;
  const bool sigma_direct_use = bob::core::array::isCZeroBaseContiguous(sigma);
  if (!sigma_direct_use) S_blitz_lapack.resize(nb_singular);
  else                   S_blitz_lapack.reference(sigma);
  double *S_lapack = S_blitz_lapack.data();
  double *U_lapack = 0;
  double *VT_lapack = 0;

  // Call the LAPACK function
  svd_lapack(jobz, M, N, A_lapack, lda, S_lapack, U_lapack, ldu,
    VT_lapack, ldvt, safe);

  // Copy singular vectors back to U, V and sigma if required
  if (!sigma_direct_use) sigma = S_blitz_lapack;
}


//----------------------------------------------------------------------------
/** @file SgMathTest.cpp
    Unit tests for SgMath.
*/
//----------------------------------------------------------------------------

#include "SgSystem.h"

#include <boost/test/auto_unit_test.hpp>
#include "SgMath.h"

using namespace std;

//----------------------------------------------------------------------------

namespace {

BOOST_AUTO_TEST_CASE(SgMathTest_RoundToInt)
{
    BOOST_CHECK_EQUAL(SgMath::RoundToInt(-0.8), -1);
    BOOST_CHECK_EQUAL(SgMath::RoundToInt(-0.3), 0);
    BOOST_CHECK_EQUAL(SgMath::RoundToInt(0.3), 0);
    BOOST_CHECK_EQUAL(SgMath::RoundToInt(0.8), 1);
}

} // namespace

//----------------------------------------------------------------------------


#include <ros/ros.h>
#include "AdaptiveController.hpp"
#include <Eigen/Dense>

#define RAD_TO_DEG 		(double)(180.0/M_PI)
#define DEG_TO_RAD 		(double)(M_PI/180.0)

#define MAX_ROLL_DEG	20.0
#define MAX_PITCH_DEG	20.0
#define MAX_YAW_DEG		160.0
#define MAX_THROTTLE	65000
#define TRIM_THROTTLE	40000

using namespace Eigen;

AdaptiveController::AdaptiveController(void (*_phi_functions) (VectorXd&, const VectorXd))
{
	(*this).phi_functions = _phi_functions;
}
AdaptiveController::~AdaptiveController() {}

void AdaptiveController::reset()
{
	this->model.reset();
	this->law.reset();
}
void AdaptiveController::initialize(const PID_Config_t _pid_x_config,
                					const PID_Config_t _pid_y_config,
                					const PID_Config_t _pid_z_config,
                					const ReferenceModel_Config_t _model_config,
                                    const AdaptiveLaw_Config_t _law_config,
                                    const double _dt)
{
    Controller::initialize(_pid_x_config, _pid_y_config, _pid_z_config);
    this->model.initialize(_model_config);
    this->law.initialize(_law_config);

    this->last_zpos = 0.0;
    this->dt = _dt;

    switch_to_standby();
}

void AdaptiveController::compute_outputs(geometry_msgs::Twist* _cmd)
{
    // position error in world frame
    double p_err_world[3], p_err_body[3];
    for(int i=0;i<3;i++) {
    	p_err_world[i] = target_position[i] - current_position[i];
    }
	rotate(current_orientation[2], p_err_world, p_err_body);

	// PID controllers
	double roll_cmd  =  pid_x.get_output(-p_err_body[0]) * RAD_TO_DEG;
	double pitch_cmd = -pid_y.get_output(-p_err_body[1]) * RAD_TO_DEG;
	double yaw_cmd	 =  target_yaw* RAD_TO_DEG;
	double pid_throttle  =  pid_z.get_output( p_err_body[2]);

	// Z Adaptive controller
	VectorXd zRefCommands, zStates;		// Input to func, need to initialize
	VectorXd zRefOutputs, zPhi;	// Output from func
	MatrixXd zGains;

	// Note: model is z down (-z is above ground)
	zRefCommands = VectorXd(1);
	zStates = VectorXd(2);
	zRefCommands(0) = -target_position[2];
	zStates(0) = -current_position[2];				// pose z
	zStates(1) = (zStates(0)-last_zpos)/this->dt;	// vel z
	last_zpos = zStates(0);

    this->model.update(zRefCommands);
    this->model.get_outputs(zRefOutputs);

	(*(*this).phi_functions) (zPhi, zStates);

    this->law.update(zStates - zRefOutputs, zPhi);
    this->law.get_gains(zGains);

    VectorXd adp_throttle = -zGains.transpose()*zPhi;
    if (adp_throttle.size() != 1)
        throw std::range_error("Adaptive gain is not a scaler!");
    adp_throttle *= -4.403669725e5; // convert to PWM from force (model)
    adp_throttle(0) = limit(adp_throttle(0), -3e4, 3e4);

    double total_throttle = pid_throttle + adp_throttle(0);
    //ROS_INFO("%.4f, %.4f, %.1lf, %.6lf, %.6lf", zStates(0), zRefOutputs(0),
    //											adp_throttle(0),
    //											zGains(0), zGains(1));

	// Send Commands
	_cmd->linear.y  = limit(roll_cmd, -MAX_ROLL_DEG, MAX_ROLL_DEG);
	_cmd->linear.x  = limit(pitch_cmd, -MAX_PITCH_DEG, MAX_PITCH_DEG);
	_cmd->angular.z = yaw_cmd;
	switch(flight_state) {
		case STANDBY:
			_cmd->linear.z  = limit(0, 0, MAX_THROTTLE);
			break;
		case EMERGENCY:
			_cmd->linear.z  = limit(0, 0, MAX_THROTTLE);
			break;
		default:
            _cmd->linear.z  = limit(total_throttle + TRIM_THROTTLE, 0.0, MAX_THROTTLE);
	}
}

void AdaptiveController::get_reference_states(VectorXd &_states) {
	this->model.get_states(_states);
}

void AdaptiveController::get_adaptive_gains(MatrixXd &gains)
{
	this->law.get_gains(gains);
}


#ifndef RADIUMENGINE_POINT_CLOUD_HPP_
#define RADIUMENGINE_POINT_CLOUD_HPP_

#include <Core/RaCore.hpp>

#include <Eigen/Eigenvalues>

#include <Core/Math/Math.hpp>
#include <Core/Math/LinearAlgebra.hpp>
#include <Core/Math/Obb.hpp>
#include <Core/Containers/VectorArray.hpp>

namespace Ra
{
    namespace Core
    {
        /// This file contains functions operating on any unstructured set of points.
        /// If not stated otherwise the functions behaviour is undefined if the set
        /// of points is empty.
        namespace PointCloud
        {
            /// Compute the mean point of a set of points, i.e. the barycenter.
            RA_CORE_API inline Vector3 meanPoint(const Vector3Array& pts);

            /// Returns a transform computed by PCA of the given set of points.
            /// The rotation gives you the principal directions in increasing
            /// order of importance (Z = principal direction)
            /// The translation is the barycenter of the point set.
            RA_CORE_API inline Transform principalAxis(const Vector3Array& pts);

            /// Returns the axis-aligned bounding box of a set of points.
            /// This function returns an empty AABB if the set of points is
            /// empty.
            RA_CORE_API inline Aabb aabb(const Vector3Array& pts);

            /// Computes an oriented bounding box based on PCA of the points coordinates.
            RA_CORE_API inline Obb pcaObb(const Vector3Array& pts);

        }
    }
}

#include <Core/Geometry/PointCloud/PointCloud.inl>

#endif // RADIUMENGINE_POINT_CLOUD_HPP_