#ifndef __GLM_INFO_H__
#define __GLM_INFO_H__

#include <armadillo>

/**
 * Information about the estimated parameters.
 */
struct glm_info
{
    /**
    * Standard error of estimated beta.
    */
    arma::vec se_beta;

    /**
    * P-value for each beta, based on Wald test.
    */
    arma::vec p_value;

    /**
     * Estimated mean value for each individual.
     */
    arma::vec mu;

    /**
     * Log likelihood of the model.
     */
    double logl;

    /**
     * Converged and had no inversion problems.
     */
    bool success;

    /**
    * For iterative algorithms, the number of iterations.
    */
    unsigned int num_iters;

    /**
    * For iterative algorithms, True if converged, false otherwise.
    */
    bool converged;
};

#endif /* End of __GLM_INFO_H__ */


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE adiis_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/adiis.h"
#include <votca/tools/eigenio_matrixmarket.h>

using namespace votca::xtp;

BOOST_AUTO_TEST_SUITE(adiis_test)

BOOST_AUTO_TEST_CASE(coeffs_test) {
  std::vector<Eigen::MatrixXd> dmatHist_;
  std::vector<Eigen::MatrixXd> mathist_;

  ADIIS adiis;

  for (votca::Index i = 0; i < 3; i++) {
    dmatHist_.push_back(votca::tools::EigenIO_MatrixMarket::ReadMatrix(
        std::string(XTP_TEST_DATA_FOLDER) + "/adiis/dmatHist_" +
        std::to_string(i) + ".mm"));
    mathist_.push_back(votca::tools::EigenIO_MatrixMarket::ReadMatrix(
        std::string(XTP_TEST_DATA_FOLDER) + "/adiis/mathist_" +
        std::to_string(i) + ".mm"));
  }

  Eigen::VectorXd Coeffs = adiis.CalcCoeff(dmatHist_, mathist_);

  Eigen::VectorXd Ref = Eigen::VectorXd::Zero(3);
  Ref << 4.45639501e-17, 1.76102089e-18, 1;

  bool check_adiis = Coeffs.isApprox(Ref, 0.00001);
  if (!check_adiis) {
    std::cout << "Ref:" << Ref << std::endl;
    std::cout << "Coeffs:" << Coeffs << std::endl;
  }

  BOOST_CHECK_EQUAL(check_adiis, 1);
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_MAXNUM_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_MAXNUM_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/maxnum.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::maxnum_, tag::cpu_,
                         (A0)(X),
                         ((simd_<arithmetic_<A0>,X>))
                         ((simd_<arithmetic_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::max(a0, a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::maxnum_, tag::cpu_,
                         (A0)(X),
                         ((simd_<floating_<A0>,X>))
                         ((simd_<floating_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      const A0 a = select(is_nan(a0),a1,a0);
      const A0 b = select(is_nan(a1),a0,a1);
      return boost::simd::max(a, b);
    }
  };
} } }
#endif


#include "modprop/utils/DerivativeTester.h"
#include "modprop/utils/MatrixUtils.hpp"

#include <boost/foreach.hpp>
#include <iostream>

namespace argus
{
Pipeline::Pipeline() {}

Pipeline::~Pipeline() {}

VectorType Pipeline::GetOutput() const
{
	std::vector<MatrixType> outs;
	BOOST_FOREACH( const SinkModule& out, _outputs )
	{
		outs.push_back( out.GetValue() );
	}
	return flatten_matrices( outs );
}

MatrixType Pipeline::GetDerivative() const
{
	std::vector<MatrixType> accs;
	BOOST_FOREACH( const ConstantModule& mod, _params )
	{
		accs.push_back( mod.GetBackpropValue() );
	}
	return hstack_matrices( accs );
}

void Pipeline::Foreprop()
{
	BOOST_FOREACH( ConstantModule & mod, _params )
	{
		mod.Foreprop();
	}
}

void Pipeline::Backprop()
{
	unsigned int tot = 0;
	BOOST_FOREACH( SinkModule & mod, _outputs )
	{
		tot += mod.GetValue().size();
	}

	unsigned int ind = 0;
	BOOST_FOREACH( SinkModule & mod, _outputs )
	{
		unsigned int n = mod.GetValue().size();
		MatrixType do_dout = MatrixType::Zero( tot, n );
		do_dout.block( ind, 0, n, n ) = MatrixType::Identity( n, n );
		ind += n;
		mod.Backprop( do_dout );
	}
}

void Pipeline::Invalidate()
{
	BOOST_FOREACH( ConstantModule & mod, _params )
	{
		mod.Invalidate();
	}
	BOOST_FOREACH( SinkModule & mod, _outputs )
	{
		mod.Invalidate();
	}
}

VectorType Pipeline::GetParams() const
{
	std::vector<MatrixType> params;
	BOOST_FOREACH( const ConstantModule &m, _params )
	{
		params.push_back( m.GetValue() );
	}
	return flatten_matrices( params );
}

void Pipeline::SetParams( const VectorType& p )
{
	if( p.size() != ParamDim() )
	{
		throw std::invalid_argument( "Incorrect parameter dimension" );
	}

	unsigned int i = 0;
	BOOST_FOREACH( ConstantModule & m, _params )
	{
		Eigen::Map<const MatrixType> param( p.data() + i,
		                                    m.GetValue().rows(),
		                                    m.GetValue().cols() );
		m.SetValue( param );
		i += param.size();
	}
}

unsigned int Pipeline::ParamDim() const
{
	unsigned int n = 0;
	BOOST_FOREACH( const ConstantModule &m, _params )
	{
		n += m.GetValue().size();
	}
	return n;
}

void Pipeline::RegisterInput( InputPort& in, const MatrixType& init )
{
	_params.emplace_back();
	link_ports( _params.back().GetOutput(), in );
	_params.back().SetValue( init );
}

void Pipeline::RegisterOutput( OutputPort& out )
{
	_outputs.emplace_back();
	link_ports( out, _outputs.back().GetInput() );
}

void test_derivatives( Pipeline& pipe, double stepSize, double eps )
{
	VectorType theta0 = pipe.GetParams();

	pipe.Invalidate();
	pipe.Foreprop();
	pipe.Backprop();
	VectorType y0 = pipe.GetOutput();
	MatrixType jacobian = pipe.GetDerivative();

	for( unsigned int i = 0; i < theta0.size(); ++i )
	{
		VectorType delta = VectorType::Zero( theta0.size() );
		delta(i) = stepSize;
		VectorType predDelta = jacobian * delta;

		pipe.SetParams( theta0 + delta );
		pipe.Invalidate();
		pipe.Foreprop();
		VectorType trueDelta = pipe.GetOutput() - y0;

		VectorType err = predDelta - trueDelta;
		if( (err.array().abs() > eps ).any() )
		{
			std::cout << "Derivative " << i << " failed test!" << std::endl;
			std::cout << "Pred delta: " << predDelta.transpose() << std::endl;
			std::cout << "True delta: " << trueDelta.transpose() << std::endl;
		}
		else
		{
			std::cout << "Derivative " << i << " passed!" << std::endl;
		}
	}
}

}


// signals2_collect.cpp
//
#include <iostream>
#include <iterator>
#include <utility>
#include <vector>

#include <boost/phoenix/phoenix.hpp>
#include <boost/signals2.hpp>

namespace test {

template<typename T>
struct min_element
{
    typedef T result_type;

    template<typename InputIt>
    T operator()(InputIt first, InputIt last) const
    {
        std::vector<T> v(first, last);
        return *std::min_element(v.begin(), v.end());
    }
};

template<typename T>
struct collect
{
    typedef std::vector<T> result_type;

    template<typename InputIt>
    std::vector<T> operator()(InputIt first, InputIt last) const
    {
        return std::vector<T>(first, last);
    }
};

}

int main(int argc, char* argv[])
{
    namespace signals2 = boost::signals2;
    namespace phoenix = boost::phoenix;

    signals2::signal<int()> s;
    s.connect(phoenix::val(1));
    s.connect(phoenix::val(3));
    s.connect(phoenix::val(5));
    std::cout << "default(last result): " << *s() << '\n';

    signals2::signal<int(), test::min_element<int> > s1;
    s1.connect(phoenix::val(1));
    s1.connect(phoenix::val(3));
    s1.connect(phoenix::val(5));
    std::cout << "min element: " << s1() << '\n';

    signals2::signal<int(), test::collect<int> > s2;
    s2.connect(phoenix::val(1));
    s2.connect(phoenix::val(3));
    s2.connect(phoenix::val(5));
    std::vector<int> v = s2();
    std::cout << "collect: ";
    std::copy(v.begin(), v.end(), std::ostream_iterator<int>(std::cout, ", "));
    std::cout << '\n';
}


// deal.II includes ----------------------------------------------
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_in.h>
#include <deal.II/lac/vector.h>

#include <deal.II/numerics/data_out.h>

// system includes -----------------------------------------------
#include <hdf5.h>
#include <omp.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <ctime>
#include <fstream>
#include <iostream>
// from eigen unsupported
#include <Eigen/KroneckerProduct>

// own includes --------------------------------------------------
#include "aux/eigen2hdf.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "grid_transfer.hpp"
#include "init/import/load_coefficients.hpp"
#include "post_processing/energy.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"

#include "l2errors.hpp"
#include "spectral/basis/indexer.hpp"
// class SimpleGridHandler, Solution
#include "grid/grid_tools.hpp"
#include "outer_product_helper.hpp"
#include "solution_handler.hpp"
#include "spectral_transfer_matrix.hpp"

#include "export/data_out_hdf5.hpp"

using namespace boltzmann;
using namespace std;

const int dim = 2;

namespace bf = boost::filesystem;
namespace po = boost::program_options;

template <typename SPECTRAL_BASIS>
void make_overlap(std::vector<double>& S, const SPECTRAL_BASIS& spectral_basis)
{
  typedef typename SPECTRAL_BASIS::elem_t elem_t;

  // angular basis
  typedef typename std::tuple_element<0, typename SPECTRAL_BASIS::elem_t::container_t>::type
      angular_elem_t;
  typename elem_t::Acc::template get<angular_elem_t> acc_ang;

  // inverse mass matrix
  S.resize(spectral_basis.n_dofs());
  for (unsigned int j = 0; j < spectral_basis.n_dofs(); ++j) {
    auto& elem = spectral_basis.get_elem(j);
    if (acc_ang(elem).get_id().l == 0)
      S[j] = numbers::PI;
    else
      S[j] = numbers::PI / 2;
  }
}

typedef dealii::DoFHandler<dim> dh_t;
typedef dealii::Vector<double> vector_t;

int main(int argc, char* argv[])
{
  // #pragma omp parallel
  // {
  //   #pragma omp master
  //   {
  //     int num_threads = omp_get_num_threads();
  //     Eigen::setNbThreads(num_threads);
  //     cout << "Eigen is using " << Eigen::nbThreads() << " threads \n";
  //   }

  // }

  boltzmann::Timer<> timer;

  po::options_description options("options");
  options.add_options()
      ("config,c", po::value<string>()->required(), "config file")
      ("help,h", "help");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  po::notify(vm);

  string config_name = vm["config"].as<string>();

  if (!boost::filesystem::is_regular_file(config_name)) {
    cout << "config file not found\n";
    return 1;
  }

  // load yaml config
  YAML::Node config = YAML::LoadFile(config_name);

  string str_input_grid = config["input"]["grid"].as<string>();
  string str_input_path = config["input"]["path"].as<string>();

  string str_ref_grid = config["reference"]["grid"].as<string>();
  string str_ref_path = config["reference"]["path"].as<string>();

  auto cwd = bf::current_path();

  // initialize working directory
  bf::path working_dir = bf::path(str_input_path) / bf::path("convergence_plots");
  bf::create_directory(working_dir);

  // initialize logfile
  std::time_t result = std::time(nullptr);
  cout << argv[0] << "at: " << asctime(localtime(&result)) << ", executed in " << cwd.c_str()
       << endl
       << setw(14) << "Solution: " << (working_dir / bf::path(str_input_path)).c_str() << setw(14)
       << "Reference: " << (working_dir / bf::path(str_ref_path)).c_str() << endl;

  // ------------------------------------------------------------------------------------------
  // transfer matrix
  dealii::FE_Q<dim> fe(1);
  auto ref_grid_ptr = make_shared<SimpleGridHandler>(str_ref_path, str_ref_grid);
  auto grid_ptr = make_shared<SimpleGridHandler>(str_input_path, str_input_grid);

  const auto& ref_dh = ref_grid_ptr->get_dofhandler();
  const auto& input_dh = grid_ptr->get_dofhandler();

  cout << "Reference mesh: " << ref_dh.get_triangulation().n_used_vertices() << " vertices, "
       << ref_dh.get_triangulation().n_active_cells() << " cells." << endl;

  timer.start();
  GridTransfer<dim> grid_transfer;
  grid_transfer.init(ref_dh, input_dh);
  const auto& Tx = grid_transfer.get_transfer_matrix();
  print_timer(timer.stop(), "init GridTransfer");

  // output to hdf5
  hid_t file;
  file = H5Fcreate((working_dir / bf::path("transfer_matrix.h5")).c_str(),
                   H5F_ACC_TRUNC,
                   H5P_DEFAULT,
                   H5P_DEFAULT);
  eigen2hdf::save_sparse(file, "Tx", Tx);

  // spectral transfer matrix
  auto Tv =
      spectral_transfer_matrix(ref_grid_ptr->get_spectral_basis(), grid_ptr->get_spectral_basis());
  eigen2hdf::save_sparse(file, "Tv", Tv);

  // timer.start();
  // Eigen::SparseMatrix<double> T = Eigen::kroneckerProduct(Tx, Tv);
  // print_timer(timer.stop(), "kroneckerProduct");
  // timer.start();
  // eigen2hdf::save_sparse(file, "T", T);
  // print_timer(timer.stop(), "dump transfer matrix");

  // ------------------------------------------------------------------------------------------
  // load solution filenames from config
  // S contains (b_i (v), b_i (v))_R^2
  vector<double> S;
  make_overlap(S, ref_grid_ptr->get_spectral_basis());

  // load permutations from `vertex2dofidx.dat`
  std::vector<unsigned int> ref_perm(ref_grid_ptr->get_dofhandler().n_dofs());
  std::vector<unsigned int> input_perm(grid_ptr->get_dofhandler().n_dofs());
  // load permutation from ``
  load_permutation(ref_perm, str_ref_path);
  load_permutation(input_perm, str_input_path);

  auto ref_perm_tmp = v2d_permutation_vector(ref_dh);
  auto input_perm_tmp = v2d_permutation_vector(input_dh);

  Mass mass(ref_grid_ptr->get_spectral_basis());
  Momentum momentum(ref_grid_ptr->get_spectral_basis());
  Energy energy(ref_grid_ptr->get_spectral_basis());

  dealii::Vector<double> vmass(ref_dh.n_dofs());
  dealii::Vector<double> venergy(ref_dh.n_dofs());
  dealii::Vector<double> vux(ref_dh.n_dofs());  // momentum x
  dealii::Vector<double> vuy(ref_dh.n_dofs());  // momnetum y

  unsigned int nsteps = config["timesteps"].size();
  cout << "__ERRORS__\n";
  cout << setw(15) << "# i" << setw(15) << "t" << setw(15) << "l2_squared" << setw(15)
       << "l2_m_squared" << setw(15) << "l2_u_squared" << setw(15) << "l2_e_squared" << endl;

  for (unsigned int i = 0; i < nsteps; ++i) {
    string str_h5loc_ref = config["timesteps"][i]["reference"]["data"].as<string>();
    string str_h5loc_inp = config["timesteps"][i]["input"]["data"].as<string>();
    double time = config["timesteps"][i]["time"].as<double>();

    Eigen::VectorXd v_inp;  // approximate solution
    Eigen::VectorXd v_ref;  // reference solution

    load_solution_vector(v_inp, str_input_path, str_h5loc_inp);
    load_solution_vector(v_ref, str_ref_path, str_h5loc_ref);

    // transform to vertex ordering
    to_vertex_ordering(v_inp, input_perm);
    to_vertex_ordering(v_ref, ref_perm);

    // transform to active dofhandler ordering
    to_dof_ordering(v_inp, input_perm_tmp);
    to_dof_ordering(v_ref, ref_perm_tmp);

    // Eigen::VectorXd v_sol = T*v_inp;
    Eigen::VectorXd v_sol(v_ref.size());
    sparse_outer_product_multiply(v_sol, Tx, Tv, v_inp);

    Errors errors;
    double l2_error_sq =
        errors.compute(ref_dh, v_sol.data(), v_ref.data(), S, ref_grid_ptr->get_indexer());

    Eigen::VectorXd vdiff = v_sol - v_ref;
    mass.compute(vmass.begin(), vdiff.data(), ref_dh.n_dofs());
    energy.compute(venergy.begin(), vdiff.data(), ref_dh.n_dofs());
    momentum.compute(vux.begin(), vuy.begin(), vdiff.data(), ref_dh.n_dofs());
    std::for_each(vmass.begin(), vmass.end(), [](double v) { return std::abs(v); });
    std::for_each(venergy.begin(), venergy.end(), [](double v) { return std::abs(v); });
    std::transform(vux.begin(), vux.end(), vuy.begin(), vux.begin(), [](double x, double y) {
      return std::sqrt(x * x + y * y);
    });

    double l2diff_m = l2norm(ref_dh, vmass);
    double l2diff_u = l2norm(ref_dh, vux);
    double l2diff_e = l2norm(ref_dh, venergy);

    cout << setw(15) << i << setw(15) << time << setw(15) << scientific << setprecision(5)
         << l2_error_sq << setw(15) << scientific << setprecision(5) << l2diff_m << setw(15)
         << scientific << setprecision(5) << l2diff_u << setw(15) << scientific << setprecision(5)
         << l2diff_e << endl;

    // output
    {
      // dealii::DataOut<dim> data_out;
      dealii::DataOutHDF<dim> data_out;
      // data_out.attach_triangulation(ref_dh.get_triangulation());
      data_out.attach_dof_handler(ref_dh);
      const auto& cell_wise_error = errors.get_cell_wise_error();
      //      cout << "cell_wise_error.size = " << cell_wise_error.size() << endl;
      data_out.add_data_vector(cell_wise_error, "error^2");
      data_out.add_data_vector(vmass, "err_mass");
      data_out.add_data_vector(venergy, "err_energy");
      data_out.add_data_vector(vux, "err_abs(u)");
      //  data_out.add_data_vector(tmp_out, "test_direct");
      data_out.build_patches();

      dealii::DataOutBase::VtkFlags flags;
      data_out.set_flags(flags);
      // auto fname = "errors" + boost::lexical_cast<string>(i) + ".vtk";
      // std::ofstream vtk_output((working_dir / bf::path(fname)).c_str());
      // data_out.write_vtk(vtk_output);
      // vtk_output.close();
      typedef dealii::DataOutBase::DataOutFilterFlags data_out_filter_flags;
      dealii::DataOutBase::DataOutFilter data_out_filter(data_out_filter_flags(true, true));
      data_out.write_filtered_data(data_out_filter);
      string filename =
          (working_dir / bf::path("output" + boost::lexical_cast<string>(i) + ".h5")).c_str();
      string mesh_filename = (working_dir / bf::path("mesh.hdf5")).c_str();
      data_out.write_hdf5(filename, data_out_filter);
      auto xdmf_entry = data_out.create_xdmf_entry(data_out_filter, mesh_filename, filename, time);

      auto xdmf_file = working_dir / bf::path("solution.xdmf");
      std::ofstream fout(xdmf_file.c_str(), std::ios_base::out | std::ios_base::app);
      fout << xdmf_entry.get_xdmf_content(1) << std::endl;
      fout.close();

      if (i == 0) {
        data_out.write_mesh(mesh_filename, data_out_filter);
      }
    }
  }

  H5Fclose(file);

  return 0;
}


#include <boost/test/unit_test.hpp>
#include <array>
#include "utils/random.h"

namespace utf = boost::unit_test;
using namespace utils;

BOOST_AUTO_TEST_SUITE(randoms)

// Check that consecutive random floats are not the same value
BOOST_AUTO_TEST_CASE(random_values, *utf::tolerance(0.00001))
{
    float first;
    float second;
    float third;

    {
        random rand(1);
        first = rand.get_float();

        second = rand.get_float();
        BOOST_TEST(first != second);

        third = rand.get_float();
        BOOST_TEST(first  != third);
        BOOST_TEST(second != third);
    }
}

// Check that random floats with the same seed have the same value at their first call
BOOST_AUTO_TEST_CASE(same_random_float, *utf::tolerance(0.00001))
{
    float first;
    float second;
    float third;

    {
        random rand(1);
        first = rand.get_float();
    }
    {
        random rand(1);
        second = rand.get_float();
    }
    {
        random rand(1);
        third = rand.get_float();
    }
    BOOST_TEST(first == second);
    BOOST_TEST(first == third);
}

// Check that going back to the same seed results in the same random first value
BOOST_AUTO_TEST_CASE(random_float, *utf::tolerance(0.00001))
{
    random rand(1);
    float first = rand.get_float();
    rand.seed(2);
    float second = rand.get_float();
    rand.seed(1);
    float twin = rand.get_float();

    BOOST_TEST(first == twin); // irrelevant difference
}

// Check that random floats with the same seed have the same value at over several calls
BOOST_AUTO_TEST_CASE(random_float_array)
{
    random rand(1);
    const int size = 100;
    std::array<float, size> first_array;
    std::array<float, size> second_array;
    std::array<float, size> twin_array;

    for (int i = 0; i < size; i++)
        first_array[i] = rand.get_float();

    std::array<int, 2> seeds{ 2,1 };
    int repetitions = 4;
    for (int r = 0; r < repetitions; r++)
    {
        rand.seed(2);
        for (int i = 0; i < size; i++)
            second_array[i] = rand.get_float();
        //float second = rand.get_float();
        rand.seed(1);
        for (int i = 0; i < size; i++)
        {
            twin_array[i] = rand.get_float();
            BOOST_TEST(first_array[i] == twin_array[i]);
            BOOST_CHECK(first_array[i] == twin_array[i]);
        }
    }
}

// Check that random integers do not exceed the given range
BOOST_AUTO_TEST_CASE(random_int_range)
{
    random rand(1);
    int tries = 1000;
    int min = 2;
    int max = 10;
    for(int i = 0; i < tries; i++)
    {
        int val = rand.get_int(min,max);
        BOOST_TEST((val >= min && val <= max));
        BOOST_TEST(!(val < min || val > max));
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * 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.
 */
/*!
 * \file
 * \brief Test of create_diff_variable function.
 */
#include "num_collect/auto_diff/backward/create_diff_variable.h"

#include <Eigen/Core>
#include <catch2/catch_template_test_macros.hpp>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE("num_collect::auto_diff::backward::create_diff_variable", "",
    float, double) {
    SECTION("create a variable") {
        constexpr auto value = static_cast<TestType>(1.234);
        const auto var =
            num_collect::auto_diff::backward::create_diff_variable(value);
        REQUIRE_THAT(var.value(), Catch::Matchers::WithinRel(value));
        REQUIRE(var.node());
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::backward::create_diff_variable_vector", "", float,
    double) {
    SECTION("create a vector") {
        using value_vector_type = Eigen::Matrix<TestType, Eigen::Dynamic, 1>;
        using variable_vector_type =
            num_collect::auto_diff::backward::variable_vector_type<
                value_vector_type>;

        const value_vector_type value_vec =
            (value_vector_type(3) << 1.234, 2.345, 3.456).finished();
        const variable_vector_type var =
            num_collect::auto_diff::backward::create_diff_variable_vector(
                value_vec);

        REQUIRE(var.rows() == 3);
        REQUIRE(var.cols() == 1);
        for (num_collect::index_type i = 0; i < 3; ++i) {
            INFO("i = " << i);
            REQUIRE_THAT(
                var(i).value(), Catch::Matchers::WithinRel(value_vec(i)));
            REQUIRE(var(i).node());
        }
    }

    SECTION("create a variable from constant expression") {
        using value_type = TestType;
        using value_vector_type = Eigen::Matrix<TestType, Eigen::Dynamic, 1>;
        using variable_vector_type =
            num_collect::auto_diff::backward::variable_vector_type<
                value_vector_type>;

        const variable_vector_type var =
            num_collect::auto_diff::backward::create_diff_variable_vector(
                value_vector_type::Ones(3));

        REQUIRE(var.rows() == 3);
        REQUIRE(var.cols() == 1);
        for (num_collect::index_type i = 0; i < 3; ++i) {
            INFO("i = " << i);
            REQUIRE_THAT(var(i).value(),
                Catch::Matchers::WithinRel(static_cast<value_type>(1)));
            REQUIRE(var(i).node());
        }
    }
}


#include "HilbertOrder3D.hpp"
#include "HilbertOrder3D_Utils.hpp"
#include <boost/array.hpp>
#include <algorithm>
#include <ctime>
#include <iostream>
#ifdef RICH_MPI
#include <mpi.h>
#endif

#define NUMBER_OF_SHAPES 24
#define MAX_ROTATION_LENGTH 5
#define PI 3.14159

using namespace std;

// The elementary Hilbert Curve shape:
class HilbertCurve3D_shape
{
public:
	// Constructor
	HilbertCurve3D_shape();
	// Comparison:
	bool operator==(HilbertCurve3D_shape & shape);
	// An array of the 7 unit vector steps defining the shape:
	boost::array<Vector3D, 7> m_vShapePoints;

};
// Constructor - uses the reference Hilbert curve shape:
HilbertCurve3D_shape::HilbertCurve3D_shape() : m_vShapePoints(boost::array<Vector3D, 7>())
{
	m_vShapePoints[0] = Vector3D(0, 0, -1);
	m_vShapePoints[1] = Vector3D(0, 1, 0);
	m_vShapePoints[2] = Vector3D(0, 0, 1);
	m_vShapePoints[3] = Vector3D(-1, 0, 0);
	m_vShapePoints[4] = Vector3D(0, 0, -1);
	m_vShapePoints[5] = Vector3D(0, -1, 0);
	m_vShapePoints[6] = Vector3D(0, 0, 1);
}

// Compare to a given Hilbert curve shape by comparing pairs of shape points:
bool HilbertCurve3D_shape::operator==(HilbertCurve3D_shape & shape)
{
	bool b = true;
	for (std::size_t ii = 0; ii < m_vShapePoints.size(); ++ii)
	{
		b = b && (m_vShapePoints[ii] == shape.m_vShapePoints[ii]);
	}

	return b;
}

// The Hilbert Curve class:
class HilbertCurve3D
{
public:
	// Constructor
	HilbertCurve3D(void);
	// Calculate the Hilbert curve distance of a given point, given a required number of iterations:
	unsigned long long int Hilbert3D_xyz2d(Vector3D const & rvPoint, int numOfIterations);

private:
	// Rotate a shape according to a given rotation scheme (in-place):
	void RotateShape(int iShapeIndex, vector<int> vAxes);
	// Rotate a shape according to rotation index, and return the rotated shape:
	void RotateShape(HilbertCurve3D_shape const & roShape, HilbertCurve3D_shape & roShapeOut, int iRotationIndex);
	/*!
	\brief Returns the rotation scheme, according to a rotation index
	\param piRotation - a pointer to the output rotation scheme vector
	\param iRotationIndex - the desired rotation index
	\return The rotation scheme length (the size of the array given by piRotation)
	*/
	int GetRotation(int * piRotation, int iRotationIndex);
	// Find the index of a given shape object:
	int FindShapeIndex(HilbertCurve3D_shape & roShape);
	// Create the recursion rule:
	void BuildRecursionRule();
	// Create the shape order, for all shapes (the order of octants):
	void BuildShapeOrder();

	// Stores all rotated shapes:
	boost::array<HilbertCurve3D_shape, NUMBER_OF_SHAPES> m_vRotatedShapes;
	// Stores all rotation schemes:
	boost::array < vector<int>, NUMBER_OF_SHAPES > m_vRotations;

	// An array of the 8 integers defining the recursion rule of the shape:
	boost::array< boost::array<int, 8> , NUMBER_OF_SHAPES> m_vShapeRecursion;

	// A 2x2x2 matrix indicating the 3 dimensional shape order
	// array< array<int , 8 > , NUMBER_OF_SHAPES > m_mShapeOrder;
	int m_mShapeOrder[NUMBER_OF_SHAPES][2][2][2];
};

// Constructor - performs all required initiallizations and preprocessing:
HilbertCurve3D::HilbertCurve3D() :m_vRotatedShapes(boost::array<HilbertCurve3D_shape, NUMBER_OF_SHAPES> ()), m_vRotations(boost::array < vector<int>, NUMBER_OF_SHAPES >()),
m_vShapeRecursion(boost::array< boost::array<int, 8> , NUMBER_OF_SHAPES>())
{
	int rot[MAX_ROTATION_LENGTH];
	int iRotLength;
	for (int iRotIndex = 1; iRotIndex < NUMBER_OF_SHAPES; ++iRotIndex)
	{
		iRotLength = GetRotation(rot, iRotIndex);
		m_vRotations[iRotIndex].assign(rot, rot + iRotLength);
	}

	for (int ii = 1; ii < NUMBER_OF_SHAPES; ++ii)
	{
		RotateShape(ii, m_vRotations[ii]);
	}

	BuildRecursionRule();
	BuildShapeOrder();
}

// FindShapeIndex - returns the index of a shape:
int HilbertCurve3D::FindShapeIndex(HilbertCurve3D_shape & roShape)
{
	for (int ii = 0; ii < NUMBER_OF_SHAPES; ++ii)
	{
		if (roShape == m_vRotatedShapes[ii])
		{
			return ii;
		}
	}
	// TODO - manage this kind of return value (error)
	return -1;
}

void HilbertCurve3D::BuildRecursionRule()
{
	// Reference recursion rule:
	m_vShapeRecursion[0][0] = 12;
	m_vShapeRecursion[0][1] = 16;
	m_vShapeRecursion[0][2] = 16;
	m_vShapeRecursion[0][3] = 2;
	m_vShapeRecursion[0][4] = 2;
	m_vShapeRecursion[0][5] = 14;
	m_vShapeRecursion[0][6] = 14;
	m_vShapeRecursion[0][7] = 10;

	HilbertCurve3D_shape oTempShape;
	// What about ii=0? not necessary
	for (int ii = 0; ii < NUMBER_OF_SHAPES; ++ii)
	{
		for (int jj = 0; jj < 8; ++jj)
		{
			// Rotate the appropriate block of the reference recursion rule, according to the ii rotation scheme:
			RotateShape(m_vRotatedShapes[m_vShapeRecursion[0][jj]], oTempShape, ii);
			// Find the shape index of the rotated shape:
			m_vShapeRecursion[ii][jj] = FindShapeIndex(oTempShape);
		}
	}

	return;
}

// Return the rotation scheme of the ii rotation (manually precalculated):
int HilbertCurve3D::GetRotation(int * piRotation, int iRotationIndex)
{
	switch (iRotationIndex)
	{
	case 1:
		piRotation[0] = 1;
		return 1;
	case 2:
		piRotation[0] = 1;
		piRotation[1] = 1;
		return 2;
	case 3:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 1;
		return 3;

	case 4:
		piRotation[0] = 2;
		return 1;
	case 5:
		piRotation[0] = 2;
		piRotation[1] = 2;
		return 2;
	case 6:
		piRotation[0] = 2;
		piRotation[1] = 2;
		piRotation[2] = 2;
		return 3;

	case 7:
		piRotation[0] = 3;
		return 1;
	case 8:
		piRotation[0] = 3;
		piRotation[1] = 3;
		return 2;
	case 9:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 3;
		return 3;

	case 10:
		piRotation[0] = 1;
		piRotation[1] = 2;
		return 2;
	case 11:
		piRotation[0] = 2;
		piRotation[1] = 1;
		return 2;
	case 12:
		piRotation[0] = 3;
		piRotation[1] = 1;
		return 2;
	case 13:
		piRotation[0] = 2;
		piRotation[1] = 3;
		return 2;

	case 14:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 1;
		piRotation[3] = 3;
		return 4;
	case 15:
		piRotation[0] = 2;
		piRotation[1] = 2;
		piRotation[2] = 2;
		piRotation[3] = 1;
		return 4;
	case 16:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 3;
		piRotation[3] = 2;
		return 4;
	case 17:
		piRotation[0] = 3;
		piRotation[1] = 2;
		piRotation[2] = 3;
		piRotation[3] = 2;
		return 4;
	case 18:
		piRotation[0] = 3;
		piRotation[1] = 2;
		piRotation[2] = 2;
		return 3;

	case 19:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 1;
		piRotation[3] = 2;
		piRotation[4] = 2;
		return 5;
	case 20:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 2;
		return 3;
	case 21:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 1;
		return 3;
	case 22:
		piRotation[0] = 2;
		piRotation[1] = 2;
		piRotation[2] = 3;
		return 3;
	case 23:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 2;
		return 3;

	default:
		return 0;
		break;
	}
}

// Rotate a shape:
void HilbertCurve3D::RotateShape(int iShapeIndex, vector<int> vAxes)
{
	int iSign = 0;

	for (std::size_t ii = 0; ii < 7; ++ii)
	{
		for (std::size_t iAx = 0; iAx < vAxes.size(); ++iAx)
		{
			// A trick to find the sign of vAxes[iAx]:
			iSign = (vAxes[iAx] > 0) - (vAxes[iAx] < 0);

			switch (abs(vAxes[iAx]))
			{
			case 1:
				m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].RotateX( iSign * PI / 2 );
				break;
			case 2:
				m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].RotateY( iSign * PI / 2 );
				break;
			case 3:
				m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].RotateZ( iSign * PI / 2 );
				break;
			default:
				break;
			}
		}
		// Round off the results:
		m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].Round();
	}
}

void HilbertCurve3D::RotateShape(HilbertCurve3D_shape const & roShape, HilbertCurve3D_shape & roShapeOut , int iRotationIndex)
{
	int iSign = 0;

	vector<int> vAxes = m_vRotations[iRotationIndex];
	roShapeOut = roShape;

	for (int ii = 0; ii < 7; ++ii)
	{
		for (std::size_t iAx = 0; iAx < vAxes.size(); ++iAx)
		{
			// A trick to find the sign of vAxes[iAx]:
			iSign = (vAxes[iAx] > 0) - (vAxes[iAx] < 0);

			switch (abs(vAxes[iAx]))
			{
			case 1:
				roShapeOut.m_vShapePoints[ii].RotateX(iSign * PI / 2);
				break;
			case 2:
				roShapeOut.m_vShapePoints[ii].RotateY(iSign * PI / 2);
				break;
			case 3:
				roShapeOut.m_vShapePoints[ii].RotateZ(iSign * PI / 2);
				break;
			default:
				break;
			}
		}
		// Round off the results:
		roShapeOut.m_vShapePoints[ii].Round();
	}

	return;
}

void HilbertCurve3D::BuildShapeOrder()
{
	boost::array<int, 8> vShapeVerticesX;
	boost::array<int, 8> vShapeVerticesY;
	boost::array<int, 8> vShapeVerticesZ;

	vShapeVerticesX[0] = 0;
	vShapeVerticesY[0] = 0;
	vShapeVerticesZ[0] = 0;

	for (std::size_t iShapeInd = 0; iShapeInd < NUMBER_OF_SHAPES; ++iShapeInd)
	{
		for (std::size_t ii = 0; ii < m_vRotatedShapes[iShapeInd].m_vShapePoints.size(); ++ii)
		{
			vShapeVerticesX[ii + 1] = (int)(vShapeVerticesX[ii] + m_vRotatedShapes[iShapeInd].m_vShapePoints[ii].x);
			vShapeVerticesY[ii + 1] = (int)(vShapeVerticesY[ii] + m_vRotatedShapes[iShapeInd].m_vShapePoints[ii].y);
			vShapeVerticesZ[ii + 1] = (int)(vShapeVerticesZ[ii] + m_vRotatedShapes[iShapeInd].m_vShapePoints[ii].z);
		}

		int iMinX = *min_element(vShapeVerticesX.begin(), vShapeVerticesX.end());
		int iMinY = *min_element(vShapeVerticesY.begin(), vShapeVerticesY.end());
		int iMinZ = *min_element(vShapeVerticesZ.begin(), vShapeVerticesZ.end());

		for (std::size_t jj = 0; jj < vShapeVerticesX.size(); ++jj)
		{
			vShapeVerticesX[jj] -= iMinX;
			vShapeVerticesY[jj] -= iMinY;
			vShapeVerticesZ[jj] -= iMinZ;
		}

		for (std::size_t kk = 0; kk < vShapeVerticesX.size(); ++kk)
		{
			m_mShapeOrder[iShapeInd][vShapeVerticesX[kk]][vShapeVerticesY[kk]][vShapeVerticesZ[kk]] = static_cast<int>(kk);
		}
	}

	return;
}

unsigned long long int HilbertCurve3D::Hilbert3D_xyz2d(Vector3D const & rvPoint, int numOfIterations)
{
	// Extract the coordinates:
	double x = rvPoint.x;
	double y = rvPoint.y;
	double z = rvPoint.z;

	// The output distance along the 3D-Hilbert Curve:
	unsigned long long int d = 0;

	// The current shape index:
	int iCurrentShape = 0;
	// The octant number:
	int iOctantNum = 0;
	// A temp variable - storing the current (negative) power of 2
	double dbPow2;
	// Variables indicating the current octant:
	bool bX, bY, bZ;
	for (int iN = 1; iN <= numOfIterations; ++iN)
	{
		// Calculate the current power of 0.5:
		dbPow2 = ((double)1) / (1 << iN);
		bX = x > dbPow2;
		bY = y > dbPow2;
		bZ = z > dbPow2;

		x -= dbPow2*bX;
		y -= dbPow2*bY;
		z -= dbPow2*bZ;

		// Multiply the distance by 8 (for every recursion iteration):
		d = d << 3;
		iOctantNum = m_mShapeOrder[iCurrentShape][bX][bY][bZ];
		d = d + iOctantNum;
		iCurrentShape = m_vShapeRecursion[iCurrentShape][iOctantNum];
	}

	return d;
}

vector<std::size_t> GetGlobalHibertIndeces(vector<Vector3D> const& cor,Vector3D const& ll,Vector3D const& ur,size_t &Hmax)
{
	vector<std::size_t> res;
#ifdef RICH_MPI
	int Nlocal = static_cast<int>(cor.size());
	int Ntotal = Nlocal;
	MPI_Allreduce(&Nlocal, &Ntotal, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);
#endif
	std::size_t Niter = 20;
	Hmax = static_cast<size_t>(std::pow(static_cast<size_t>(2), static_cast<size_t>(3*Niter)));
	HilbertCurve3D oHilbert;
	Vector3D dx = ur - ll,vtemp;
	std::size_t Ncor = cor.size();
	res.resize(Ncor);
	for (size_t i = 0; i < Ncor; ++i)
	{
		vtemp.x = (cor[i].x - ll.x) / dx.x;
		vtemp.y = (cor[i].y - ll.y) / dx.y;
		vtemp.z = (cor[i].z - ll.z) / dx.z;
		res[i]=static_cast<size_t>(oHilbert.Hilbert3D_xyz2d(vtemp,static_cast<int>(Niter)));
	}
	return res;
}

vector<std::size_t> HilbertOrder3D(vector<Vector3D> const& cor)
{
	// If only 1 or 2 points are provided - do not reorder them
	if ( 2 >= cor.size() )
	{
		vector<std::size_t> vIndSort( cor.size() );
		for (std::size_t ii = 0; ii < cor.size(); ++ii)
		{
			vIndSort[ii] = ii;
		}
		return vIndSort;
	}
	// Create a 3D-Hilbert Curve Object:
	HilbertCurve3D oHilbert;

	// Allocate an output vector:
	size_t N = cor.size();
	vector<unsigned long long int> vOut;
	vOut.resize(N);

	// Estimate the number of required iterations:
	int numOfIterations = EstimateHilbertIterationNum(cor);

	vector<Vector3D> vAdjustedPoints;

	// Adjust the points coordinates to the unit cube:
	AdjustPoints(cor, vAdjustedPoints);

	// Run throught the points, and calculate the Hilbert distance of each:
	for (size_t ii = 0; ii < N; ++ii)
	{
		vOut[ii]=oHilbert.Hilbert3D_xyz2d(vAdjustedPoints[ii], numOfIterations+8);
		//vOut.push_back(oHilbert.Hilbert3D_xyz2d(vAdjustedPoints[ii], 2));
	}
	// Get the sorting indices:
	vector<std::size_t> vIndSort;
	ordered(vOut, vIndSort);
	// Reorder the Hilbert distances vector (according to the sorting indices):
	reorder( vOut, vIndSort );

	// Find indices with repeated Hilbert distance:
	vector<vector<std::size_t> > vEqualIndices;
	FindEqualIndices(vOut, vEqualIndices);

	// If all points have different Hilbert distances, return the sorting indices:
	if (vEqualIndices.empty())
	{
		return vIndSort;
	}
	else
	{
		for (std::size_t ii = 0; ii < vEqualIndices.size(); ++ii)
		{
			vector<Vector3D> vPointsInner(vEqualIndices[ii].size() );
			vector<std::size_t> vIndInner(vEqualIndices[ii].size());
			vector<std::size_t> vIndSortInner(vEqualIndices[ii].size());
			vector<std::size_t> vIndSortInner_cpy(vEqualIndices[ii].size());

			// Store the points with the equal indices
			for (std::size_t jj = 0; jj < vEqualIndices[ii].size() ; ++jj)
			{
				vIndInner[jj] = vIndSort[vEqualIndices[ii][jj]];
				vPointsInner[jj] = cor[vIndInner[jj]];
				vIndSortInner_cpy[jj] = vIndSort[vIndInner[jj]];
			}

			// Sort the repeated points:
			vIndSortInner = HilbertOrder3D(vPointsInner);
	//		vector<std::size_t> vIndSortTemp = vIndSort;
			for (std::size_t kk = 0; kk < vIndSortInner.size(); ++kk)
			{
				vIndSort[vIndInner[kk]] = vIndSortInner_cpy[vIndSortInner[kk]];
			}
		}

		// Return the sorting indices:
		return vIndSort;
	}
}


#include "CalibratorReset.h"
#include "SIMDMacros.h"

#include <boost/bind.hpp>
#include <iostream>

// it's probably a bad idea to put this here!
static const int max_threads = 16;
static tbb::task_scheduler_init init(max_threads);

CalibratorReset::CalibratorReset(int rows, int cols)
{
    m_rows = rows;
    m_cols = cols;
    allocGainMem();
}

CalibratorReset::~CalibratorReset()
{

}

void CalibratorReset::processFrame(MemBlockI16& input, MemBlockF& output)
{
    for(int i=0;i<m_rows;++i)
    {
        processFrameRow(input, output, i);
    }
}

void CalibratorReset::processFrameP(MemBlockI16& input, MemBlockF& output)
{
    auto fn = boost::bind(&CalibratorReset::processFrameRowsTBB, this, &input, &output, _1);
    tbb::parallel_for( tbb::blocked_range<int>(0,m_rows,50), fn, tbb::simple_partitioner());
}

void CalibratorReset::processFrameRowsTBB(MemBlockI16* pInput, MemBlockF* pOutput, tbb::blocked_range<int> rows)
{
    for(int r = rows.begin(); r<rows.end(); ++r)
    {
        processFrameRowSIMD(*pInput, *pOutput, r);
    }
}

inline void CalibratorReset::processFrameRow(MemBlockI16& input, MemBlockF& output, int row)
{
    // seems that we have 7 rows in the calib arrays
    float idealOf = 128.0f * 32.0f;
    int row_start_idx = row * m_cols;
    for(int col=0;col<m_cols;++col)
    {
          // Decode
          size_t pixel_index = row_start_idx + col;
          uint16_t pixel = input.at(pixel_index);
          // ADC decoding
          // Note for reset frames, the gain bits are invalid; should be 0 always.
          register uint16_t fine = (pixel & 0x1fe0) >> 5;
          register uint16_t coarse = (pixel & 0x001f);
          // ADC Correction
          register float decoded_input = idealOf + ( m_Gc.at(pixel_index) * (coarse - m_Oc.at(pixel_index)) )
                                      + ( m_Gf.at(pixel_index) * (fine - m_Of.at(pixel_index)) );

          output.at(pixel_index) = decoded_input;
    }
}

inline void CalibratorReset::processFrameRowSIMD(MemBlockI16& input, MemBlockF& output, int row)
{
    float idealOf = 128.0f * 32.0f;
    int row_start_idx = row * m_cols;
    for(int col=0;col<m_cols;col+=8)
    {
        int curElt = row_start_idx + col;
        uint16_t* pIn = input.data() + curElt;
        SIMD8i16 in = Load4i(pIn);

        // could move these outside the loop?
        SIMD8i16 maskCoarse = SetAll8i16(0x1f);
        SIMD8i16 maskFine = SetAll8i16(0xff);
        SIMD8f idealOffset8f = SetAll8f(128.0f * 32.0f);
        SIMD8f zero8f = SetAll8f(0.0f);

        SIMD8i16 coarseStep1 = And4i(ShiftRight8i16(in, 0), maskCoarse);
        SIMD8i16 fineStep1 = And4i(ShiftRight8i16(in, 5), maskFine);

        SIMD4i loCoarseStep2 = ExtendLoto4i(coarseStep1);
        SIMD4i hiCoarseStep2 = ExtendHito4i(coarseStep1);
        SIMD4i loFineStep2 = ExtendLoto4i(fineStep1);
        SIMD4i hiFineStep2 = ExtendHito4i(fineStep1);
        SIMD8i coarseStep3 = Set8i(loCoarseStep2,hiCoarseStep2);
        SIMD8i fineStep3 = Set8i(loFineStep2,hiFineStep2);

        SIMD8f coarsef = Convert8ito8f(coarseStep3);
        SIMD8f finef = Convert8ito8f(fineStep3);

        SIMD8f Gc8f = Load8f(m_Gc.data()+curElt);
        SIMD8f Oc8f = Load8f(m_Oc.data()+curElt);
        SIMD8f Gf8f = Load8f(m_Gf.data()+curElt);
        SIMD8f Of8f = Load8f(m_Of.data()+curElt);

        SIMD8f coarseStep4 = Sub8f(coarsef, Oc8f);
        SIMD8f fineStep4 = Sub8f(finef, Of8f);

        SIMD8f coarseStep5 = Multiply8f(coarseStep4, Gc8f);
        SIMD8f fineStep5 = Multiply8f(fineStep4, Gf8f);

        SIMD8f result8f = Add8f(coarseStep5, idealOffset8f);
        result8f = Add8f(result8f, fineStep5);

        Store8f(&output.at(curElt), result8f);
    }
}

void CalibratorReset::allocGainMem()
{
    m_Gc.init(m_rows, m_cols);
    m_Oc.init(m_rows, m_cols);
    m_Gf.init(m_rows, m_cols);
    m_Of.init(m_rows, m_cols);
}

int64_t CalibratorReset::loadADCGain(std::string filename)
{
    int64_t rc = 0;

    std::string group = "/reset";
    // group should look like "/sample" or "/reset"

    m_Gc.loadFromH5(filename, group + "/coarse/slope", 0);
    m_Oc.loadFromH5(filename, group + "/coarse/offset", 0);
    m_Gf.loadFromH5(filename, group + "/fine/slope", 0);
    m_Of.loadFromH5(filename, group + "/fine/offset", 0);

   // listen up folks! The equation that they want is this:
   // idOf = 128.0*32;
   // idSl = idOf / 1.5;
   // result = idOf + (idSl / Sc[r,c]) * (coarse_adc[r,c] + 1.0 - Oc[r,c]) - (idSl / Sf[r,c]) * (fine_adc[r,c] - Of[r,c]);
   //
   // so we are going to alter the values in the Gc, Gf to make this easier
   // the equation we apply is:
   // res = idOf + Gc * (coarse - Oc) + Gf * (fine - Of)

   float k = 128.0f * 32.0f / 1.5f;
   for(int r=0;r<m_rows;++r)
   {
       for(int c=0;c<m_cols;++c)
       {
           m_Gc.at(r,c) = k / m_Gc.at(r,c);
           m_Gf.at(r,c) = -1.0f * k / m_Gf.at(r,c);
           m_Oc.at(r,c) -= 1.0f;
       }
   }

   return rc;
}


// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "opencv2/slideio/imagetools.hpp"
#include "opencv2/slideio.hpp"
#include <boost/format.hpp>
#include <boost/filesystem.hpp>

using namespace cv;

int slideio::ImageTools::dataTypeSize(slideio::DataType dt)
{
    switch(dt)
    {
    case DataType::DT_Byte:
    case DataType::DT_Int8:
        return 1;
    case DataType::DT_UInt16:
    case DataType::DT_Int16:
    case DataType::DT_Float16:
        return 2;
    case DataType::DT_Int32:
    case DataType::DT_Float32:
        return 4;
    case DataType::DT_Float64:
        return 8;
    }
    throw std::runtime_error(
        (boost::format("Unknown data type: %1%") % (int)dt).str());
}

void slideio::ImageTools::scaleRect(const cv::Rect& srcRect, const cv::Size& newSize, cv::Rect& trgRect)
{
    double scaleX = static_cast<double>(newSize.width) / static_cast<double>(srcRect.width);
    double scaleY = static_cast<double>(newSize.height) / static_cast<double>(srcRect.height);
    trgRect.x = static_cast<int>(std::floor(static_cast<double>(srcRect.x)*scaleX));
    trgRect.y = static_cast<int>(std::floor(static_cast<double>(srcRect.y)*scaleY));
    trgRect.width = newSize.width;
    trgRect.height = newSize.height;
}

void slideio::ImageTools::scaleRect(const cv::Rect& srcRect, double scaleX, double scaleY, cv::Rect& trgRect)
{
    trgRect.x = static_cast<int>(std::floor(static_cast<double>(srcRect.x)*scaleX));
    trgRect.y = static_cast<int>(std::floor(static_cast<double>(srcRect.y)*scaleY));
    int xn = srcRect.x + srcRect.width;
    int yn = srcRect.y + srcRect.height;
    int dxn = static_cast<int>(std::ceil(static_cast<double>(xn)* scaleX));
    int dyn = static_cast<int>(std::ceil(static_cast<double>(yn)* scaleY));
    trgRect.width = dxn - trgRect.x;
    trgRect.height = dyn - trgRect.y;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SCHURBLOCKLU_SOLVER_HH
#define SCHURBLOCKLU_SOLVER_HH

/**
 * @file
 * @brief  Routines for the solution of (sparse) linear systems
 * @author Anton Schiela
 */

#include <iostream>
#include <memory> // std::unique_ptr

#include "dune/common/fmatrix.hh"
#include "dune/istl/matrix.hh"

#include "linalg/umfpack_solve.hh"

#include "linalg/linearsystem.hh"
#include "linalg/simpleLAPmatrix.hh"

#include <boost/timer/timer.hpp>

namespace Kaskade
{

/** \ingroup linalg
 *\ brief Adapter class for DUNE::IterativeSolver
 */

void printVec(std::vector<double> const&v, int vend=1000000);

template<class Factorization=UMFFactorization<double> >
class BlockLUFactorization
{
public:
/// needs a matrix
  static const bool needMatrix = true;
template<class Sys>
BlockLUFactorization(Sys const& lin, int start2, int end2, int start3, int end3)
  {
    MatrixAsTriplet<double> matL;
    lin.getMatrixBlocks(matL,start3,end3,start2,end2);
    matA.resize(0);
    lin.getMatrixBlocks(matA,start2,end2,start2,end2);
    std::cout << "Inner, " << std::flush;
    boost::timer::cpu_timer timer;
    factoredL.reset(new Factorization(matL.nrows(),
                                      2,
                                      matL.ridx,
                                      matL.cidx,
                                      matL.data,
                                      MatrixProperties::GENERAL));
    std::cout << "Finished: " << (double)(timer.elapsed().user)/1e9 << " sec." << std::endl;
  }

  void resetBlock22(MatrixAsTriplet<double>const & matA_) { matA = matA_; };

  void solve(std::vector<double>const& rhs, std::vector<double>& sol, int nr=1);
  MatrixAsTriplet<double> matA;
  std::unique_ptr<Factorization> factoredL;
};

struct BlockSchurParameters
{
  BlockSchurParameters(bool reg_)
    : refactorizeInner(true), refactorizeOuter(true)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  BlockSchurParameters(bool reg_, bool innerf_, bool outerf_)
    : refactorizeInner(innerf_), refactorizeOuter(outerf_)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  bool refactorizeInner, refactorizeOuter;
  typedef enum { None=0, AddId=1, CG = 2} RegularizationMethod;

  RegularizationMethod regularizationMethod;
};

/// Solver, which is especially designed for the hyperthermia planning problem.
template<class Factorization>
class DirectBlockSchurSolver
{
public:
  int start1, end1, start2, end2, start3, end3;

/// needs a matrix
  static const bool needMatrix = true;

  DirectBlockSchurSolver(bool doregularize = false) :
    start1(0), end1(1),
    start2(1), end2(2),
    start3(2), end3(3),
    report(false), paras(doregularize)
  {}

  void ax(std::vector<double>& sol, std::vector<double>const &r) const;

  void resolve(std::vector<double>& sol, SparseLinearSystem const& lin) const;

  void resolveAdjAndNormal(std::vector<double>& sol, SparseLinearSystem const& lin) const;

  void resolveNormal(std::vector<double>& sol, SparseLinearSystem const& lin,std::vector<double>const *addrhs=0);

  void solve(std::vector<double>& sol,
             SparseLinearSystem const& lin);

  void solveAdjAndNormal(std::vector<double>& sol,
             SparseLinearSystem const& lin);

  void solveTCG(std::vector<double>& sol1, std::vector<double>& sol2,
                SparseLinearSystem const& linT, SparseLinearSystem const& linN, std::vector<double>const & normalStep, double nu0);

  /// Solves always exactly
  void setRelativeAccuracy(double) {}

  /// Always exact solution
  double getRelativeAccuracy() {return 0.0;}

  void onChangedLinearization() {flushFactorization(); }

  void flushFactorization()
  {
    if(factorization.get() && paras.refactorizeInner) factorization.reset();
    mC.setSize(0,0);
    B.resize(0);
    AinvB.resize(0);
    matANormal.flush();
  }

  bool report;

  /// Always exact solution
  double getAbsoluteAccuracy() {return 0.0;}

  bool improvementPossible() { return false; }

  void resetParameters(BlockSchurParameters const& p_) { paras=p_; }

private:
  void resolveN(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;

  void tsolve(std::vector<double>& sol1,std::vector<double>& sol2,
              std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;

  void fwd(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;
  void bwd(std::vector<double>& sol, std::vector<double>const &x2,std::vector<double>const &s,std::vector<double>const &t) const;

  void buildNewSchurComplement(SparseLinearSystem const& lin,int task);

  std::unique_ptr<BlockLUFactorization<Factorization> > factorization;
  MatrixAsTriplet<double> matANormal;
  Dune::Matrix<Dune::FieldMatrix<double,1,1> > mC,mCNormal;
  std::vector<double> B,AinvB;

  int rowsB, colsBC, rowsC;
  int rows1, rows2, rows3;

  BlockSchurParameters paras;

};

class ModifiedSparseSystem : public SparseLinearSystem
{
public:
  ModifiedSparseSystem(SparseLinearSystem const &lin_, MatrixAsTriplet<double> const& mat_, MatrixAsTriplet<double> const& mat2_
                       , std::vector<double> const& scaling_) :
    scaling(scaling_), lin(&lin_), mat(mat_), mat2(mat2_)
  {
  }

  virtual int rows(int rbegin, int rend) const { return lin->rows(rbegin,rend);}
  virtual int cols(int colbegin, int colend) const { return lin->cols(colbegin,colend);}

  /// Return matrix blocks of the linear system in triplet format
  virtual void getMatrixBlocks(MatrixAsTriplet<double>& m, int rbegin, int rend, int colbegin, int colend) const
  {
    lin->getMatrixBlocks(m,rbegin,rend,colbegin,colend);
    MatrixAsTriplet<double> mm(mat);
    mm *= -1.0;
    if(rbegin==0 && colbegin == 0) m+=mm;
  }

  void resetLin(SparseLinearSystem const& lin_)
  {
    lin=&lin_;
  }

  /// value of function
  virtual double getValue() const { return lin->getValue();}

  /// Return components of the right hand side of the linear system
  virtual void getRHSBlocks(std::vector<double>& rhs, int rbegin, int rend) const
  {
    rhs.resize(0);
    lin->getRHSBlocks(rhs,rbegin,rend);
    if(rbegin==0)
    {
      std::vector<double> t(lin->rows(3,4),0.0);
      std::vector<double> Mt(lin->rows(0,1),0.0);
      lin->getRHSBlocks(t,3,4);
      for(int i=0; i<t.size();++i)
      {
        t[i] *=scaling[i];
      }
      mat2.ax(Mt,t);
      for(int i=0; i<Mt.size();++i)
      {
//        std::cout << Mt[i] << std::endl;
      }
      for(int i=0; i<lin->rows(0,1); ++i)
        rhs[i] -= Mt[i];
    }
  }

  /// number of column blocks
  virtual int nColBlocks() const { return lin->nColBlocks();};

  /// number of row blocks
  virtual int nRowBlocks() const { return lin->nRowBlocks();};
private:
  std::vector<double> scaling;
  SparseLinearSystem const* lin;
  MatrixAsTriplet<double> mat;
  MatrixAsTriplet<double> mat2;
};

/// Solver, which is especially designed for the hyperthermia planning problem with amplitude ratio
template<class Factorization>
class ARDirectBlockSchurSolver
{
public:
/// needs a matrix
  static const bool needMatrix = true;

  ARDirectBlockSchurSolver(bool doregularize = false) : report(false), DBSSolver(doregularize), justsolved(false)
{
}

  void resolve(std::vector<double>& sol, SparseLinearSystem const& lin) const;

  void solve(std::vector<double>& sol,
             SparseLinearSystem const& lin);

  /// Solves always exactly
  void setRelativeAccuracy(double) {}

  /// Always exact solution
  double getRelativeAccuracy() {return 0.0;}

  void onChangedLinearization() {flushFactorization(); }

  void flushFactorization()
  {
    DBSSolver.flushFactorization();
    F.setSize(0,0);
    FT.setSize(0,0);
    FTVinv.setSize(0,0);
    FTVinvF.setSize(0,0);
    Vinv.setSize(0,0);
  }

  bool report;

  /// Always exact solution
  double getAbsoluteAccuracy() {return 0.0;}

  bool improvementPossible() { return false; }

private:
  std::vector<double> scaling;
  DirectBlockSchurSolver<Factorization> DBSSolver;
  Dune::Matrix<Dune::FieldMatrix<double,1,1> > Vinv,F,FT, FTVinv, FTVinvF;
  std::unique_ptr<MatrixAsTriplet<double> > L,FTVi;
  std::unique_ptr<ModifiedSparseSystem> linMod;
  bool justsolved;
};

}  // namespace Kaskade
#endif


/*
 * Copyright META1 (c) 2020
 */

#include <random>

#include <boost/test/unit_test.hpp>

#include <graphene/chain/hardfork.hpp>

#include "meta1_fixture.hpp"

using namespace graphene::chain;
using namespace graphene::chain::test;

namespace graphene {
   namespace chain {

      bool meta1_fixture::is_property_exists(uint32_t property_id) const {
         const auto &idx = db.get_index_type<property_index>().indices().get<by_property_id>();
         auto itr = idx.find(property_id);
         if (itr != idx.end())
            return true;
         else
            return false;
      }

      property_create_operation meta1_fixture::create_property_operation(const string issuer,
                                                                         const uint64_t appraised_property_value,
                                                                         const uint32_t allocation_duration_minutes,
                                                                         const string backed_by_asset_symbol,
                                                                         const property_options common) const {
         try {
            // Invoke get_asset to check the existence of the backed_by_asset_symbol
            get_asset(backed_by_asset_symbol);
            // FC_ASSERT(*backing_asset == nullptr, "Asset with that symbol not exists!");
            std::random_device rd;
            std::mt19937 mersenne(rd());
            uint32_t rand_id;
            const account_object &issuer_account = get_account(issuer);
            property_create_operation create_op;

            //generating new property id and regenerate if such id is exists
            while (true) {
               rand_id = mersenne();
               bool flag = is_property_exists(rand_id);
               if (!flag) { break; }
            }

            create_op.property_id = rand_id;
            create_op.issuer = issuer_account.id;
            create_op.appraised_property_value = appraised_property_value;
            create_op.allocation_duration_minutes = allocation_duration_minutes;
            create_op.backed_by_asset_symbol = backed_by_asset_symbol;
            create_op.common_options = common;

            return create_op;
         }
         FC_CAPTURE_AND_RETHROW()
      }

      const uint64_t
      meta1_fixture::calculate_meta1_valuation(uint64_t appraised_value,
                                               const uint64_t progress_numerator,
                                               const uint64_t progress_denominator) {
         BOOST_CHECK_LE(progress_numerator, progress_denominator);
         // The arithmetic will be sequenced to reduce loss of precision errors
         // Note: Precision is lost during division
         // Note: Multiplication can result in an overflow of the numerical type
         // Therefore the following arithmetic rules will be followed:
         // - variables that will be multiplied will expressed within a higher precision numerical type before multiplication
         // - after multiplication, check for overflow
         // - division is to be minimized, to the extent possible
         // - division is to be delayed, to the extent possible, until after all multiplications are completed

         // The total appraisal sums the appraisal of every property
         // This test contains only one property

         // Overflow checks during arithmetic is performed with the fc::safe struct
         const safe<fc::uint128_t> appraised_value_128(appraised_value);
         const safe<fc::uint128_t> safe_meta1_valuation_128 = (appraised_value_128 * 10 * progress_numerator);
         // "Un-safe" before division to avoid compiler warning
         fc::uint128_t meta1_valuation_128 = safe_meta1_valuation_128.value;
         meta1_valuation_128 /= progress_denominator;

         uint64_t meta1_valuation = static_cast<uint64_t>(meta1_valuation_128);

         return meta1_valuation;
      }

      const uint64_t meta1_fixture::calculate_meta1_valuation(uint64_t appraised_value, const ratio_type progress) {
         return calculate_meta1_valuation(appraised_value, progress.numerator(), progress.denominator());
      }

      void meta1_fixture::publish_asset_price(const string asset_symbol, const price_ratio price,
                                              const account_id_type authorizing_id,
                                              const private_key_type authorizing_private_key) {
         asset_price_publish_operation publish_op;
         publish_op.symbol = asset_symbol;
         publish_op.usd_price = price;
         publish_op.fee_paying_account = authorizing_id;

         trx.clear();
         trx.operations.push_back(publish_op);
         sign(trx, authorizing_private_key);

         PUSH_TX(db, trx);

         trx.clear();
      }

      void meta1_fixture::initialize_allocation_object(const string asset_to_back, const account_id_type authorizing_id,
                                                       const private_key_type authorizing_private_key) {
         const auto &asset_limitation_idx = db.get_index_type<asset_limitation_index>().indices().get<by_limit_symbol>();
         auto itr = asset_limitation_idx.find(asset_to_back);
         if (itr != asset_limitation_idx.end()) {
            // The object already exists
            return;
         }

         asset_limitation_object_create_operation creator;

         creator.issuer = authorizing_id;
         creator.fee = asset();
         creator.limit_symbol = asset_to_back;

         trx.clear();
         trx.operations.push_back(std::move(creator));
         sign(trx, authorizing_private_key);

         PUSH_TX(db, trx, ~0);

         trx.clear();
      }

      const property_id_type meta1_fixture::allocate_property(const uint64_t appraised_property_value,
                                                              const uint32_t allocation_duration_minutes,
                                                              const property_options options,
                                                              const account_id_type authorizing_id,
                                                              const private_key_type authorizing_private_key) {

         const string asset_to_back = GRAPHENE_SYMBOL;
         initialize_allocation_object(asset_to_back, authorizing_id, authorizing_private_key);

         const string issuer = "meta1";
         property_create_operation prop_op = create_property_operation(issuer, appraised_property_value,
                                                                       allocation_duration_minutes, asset_to_back,
                                                                       options);
         trx.clear();
         trx.operations.push_back(prop_op);
         set_expiration(db, trx);
         sign(trx, authorizing_private_key);

         PUSH_TX(db, trx);

         /**
          * Approve the asset by using the update operation
          */
         graphene::chain::property_object property = db.get_property(prop_op.property_id);
         const property_id_type prop_id = property.id;
         property_approve_operation aop;
         aop.issuer = authorizing_id;
         aop.property_to_approve = property.id;

         trx.clear();
         trx.operations.push_back(aop);
         sign(trx, authorizing_private_key);

         PUSH_TX(db, trx);

         /**
          * Advance to the 100% moment
          * The appreciation should be at 100%
          */
         property = db.get_property(prop_op.property_id);
         const uint32_t allocation_duration_seconds = (property.approval_end_date.sec_since_epoch() -
                                                       property.creation_date.sec_since_epoch());
         const time_point_sec time_to_100_percent = property.creation_date + (allocation_duration_seconds) * 4 / 4;
         generate_blocks(time_to_100_percent, false);
         set_expiration(db, trx);
         trx.clear();

         return prop_id;
      }

      const property_id_type meta1_fixture::allocate_property(const uint64_t appraised_property_value,
                                                              const uint32_t allocation_duration_minutes,
                                                              const account_id_type authorizing_id,
                                                              const private_key_type authorizing_private_key) {
         // Create the property
         const property_options property_options = {
                 "some description",
                 "some title",
                 "my@email.com",
                 "you",
                 "https://fsf.com",
                 "https://purepng.com/metal-1701528976849tkdsl.png",
                 "222",
                 1,
                 33104,
         };

         return allocate_property(appraised_property_value, allocation_duration_minutes, property_options,
                                  authorizing_id, authorizing_private_key);
      }

   }
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/lo.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( hi_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0,unsigned>;

    BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return lo( shr(a0, sizeof(result_t)*(CHAR_BIT/2)));
    }
  };

} } }

#endif


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

/// \file ipalign.cc
///
/// Example program demonstrating how to align two images using the
/// Interest Point module.

#include <vw/Core/Log.h>
#include <vw/Core/FundamentalTypes.h>
#include <vw/Math/Geometry.h>
#include <vw/Math/RANSAC.h>
#include <vw/Math/Vector.h>
#include <vw/Math/Matrix.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/ImageMath.h>
#include <vw/Image/Transform.h>
#include <vw/Image/Manipulation.h>
#include <vw/Image/PixelTypes.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/InterestPoint/Descriptor.h>
#include <vw/InterestPoint/Detector.h>
#include <vw/InterestPoint/Matcher.h>
#include <vw/InterestPoint/IntegralDetector.h>
#include <vw/InterestPoint/IntegralInterestOperator.h>
#include <vw/Camera/CameraUtilities.h>

#include <vw/Camera/LensDistortion.h>

using namespace vw;
using namespace vw::ip;
using namespace vw::camera;

#include <boost/program_options.hpp>
namespace po = boost::program_options;
#include <boost/filesystem/path.hpp>
namespace fs = boost::filesystem;
#include <boost/foreach.hpp>

#define MAX_POINTS_TO_DRAW 1000

  // Image Alignment
  //
  // Images are aligned by computing interest points, matching
  // them using a standard 2-Norm nearest-neighor metric, and then
  // rejecting outliers by fitting a similarity between the
  // putative matches using RANSAC.

/// Program parameters
struct Options {
  // Input
  std::string left_image_file,  right_image_file;
  std::string left_camera_file, right_camera_file;

  // Settings
  std::string output_prefix, interest_operator, descriptor_generator;
  std::string align_method;
  float matcher_threshold, detect_gain, tile_size;
  float inlier_threshold;
  int   ransac_iterations;
  bool  single_scale, debug_images;
  bool  save_intermediate;
};

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

// TODO: DELETE TEST CODE!

/*
// Tool to solve for inverse transform parameters

struct LensOptimizeFunctor :  public math::LeastSquaresModelBase<LensOptimizeFunctor> {
  typedef Vector<double> result_type;
  typedef Vector4 domain_type;
  typedef Matrix<double> jacobian_type;

  const camera::PinholeModel& m_cam;
  const std::vector<Vector2>& m_raw_coords;

  LensOptimizeFunctor(const camera::PinholeModel& cam, const std::vector<Vector2>& raw_coords)
    : m_cam(cam), m_raw_coords(raw_coords) {}

  inline result_type operator()( domain_type const& x ) const {

    TsaiLensDistortion lens(x); // Construct lens distortion model with given parameters

    result_type out_vec;
    out_vec.set_size(m_raw_coords.size()*2);
    for (size_t i=0; i<m_raw_coords.size(); ++i) {
      Vector2 loc = lens.distorted_coordinates(m_cam, m_raw_coords[i]);
      out_vec[2*i  ] = loc[0];
      out_vec[2*i+1] = loc[1];
    }
    return out_vec;
  }
}; // End class LensOptimizeFunctor

boost::shared_ptr<LensDistortion>
 solve_inverse_lens_distortion(PinholeModel const& pinhole_in, Vector2 image_size) {

  // For now, just solve for undistorted --> distorted with the Icebridge example

  const int sample_spacing = 50;

  // Get input camera information
  const double pixel_pitch = pinhole_in.pixel_pitch();
  const vw::camera::LensDistortion* distortion = pinhole_in.lens_distortion();

  // Generate a set of point pairs
  std::vector<Vector2> undistorted_coords;
  Vector<double> distorted_coords;
  int num_cols   = image_size[0]/sample_spacing;
  int num_rows   = image_size[1]/sample_spacing;
  int num_coords = num_rows*num_cols;
  std::cout << "Generating " << num_coords << " points.\n";
  undistorted_coords.resize(num_coords);
  distorted_coords.set_size(num_coords*2);

  int index = 0;
  for (int r=0; r<num_rows; ++r) {
    int row = r*sample_spacing;
    for (int c=0; c<num_cols; ++c) {

      int col = c*sample_spacing;

      // Note that the pixel pairs are corrected for pitch
      Vector2 raw_pixel       = Vector2(col, row) * pixel_pitch;
      Vector2 distorted_pixel = distortion->distorted_coordinates(pinhole_in, raw_pixel);

      //std::cout << "Raw: " << raw_pixel << " ---- Distorted: " << distorted_pixel << std::endl;

      undistorted_coords[index    ] = raw_pixel;
      distorted_coords  [2*index  ] = distorted_pixel[0];
      distorted_coords  [2*index+1] = distorted_pixel[1];
      ++index;
    }
  }

  // Check the error
  TsaiLensDistortion dummy_tsai(Vector4(0,0,0,0));
  double diff = 0;
  for (size_t i=0; i<undistorted_coords.size(); ++i) {
    Vector2 distorted        = dummy_tsai.distorted_coordinates(pinhole_in, undistorted_coords[i]);
    Vector2 actual_distorted = Vector2(distorted_coords[2*i], distorted_coords[2*i+1]);
    //std::cout << "New: " << distorted << " ---- Actual: " << actual_distorted << std::endl;
    diff += norm_2(distorted - actual_distorted);
  }
  diff /= static_cast<double>(undistorted_coords.size());
  std::cout << "Mean coord diff prior to solving = " << diff << std::endl;

  // Now solve for a complementary lens distortion scheme

  // Init solver object with the undistorted coordinates
  LensOptimizeFunctor solver_model(pinhole_in, undistorted_coords);
  int status;
  Vector4 seed; // Start with all zeros (no distortion)
  // Solve for the best tsai params that give us the distorted coordinates from the undistorted coordinates.
  std::cout << "Solving...\n";
  Vector4 tsai_params = math::levenberg_marquardt( solver_model, seed, distorted_coords, status);

  std::cout << "Solved for tsai params: " << tsai_params << std::endl;

  // Check the error
  TsaiLensDistortion new_tsai(tsai_params);
  diff = 0;
  for (size_t i=0; i<undistorted_coords.size(); ++i) {
    Vector2 distorted        = new_tsai.distorted_coordinates(pinhole_in, undistorted_coords[i]);
    Vector2 actual_distorted = Vector2(distorted_coords[2*i], distorted_coords[2*i+1]);
    //std::cout << "New: " << distorted << " ---- Actual: " << actual_distorted << std::endl;
    diff += norm_2(distorted - actual_distorted);
  }
  diff /= static_cast<double>(undistorted_coords.size());
  std::cout << "Mean coord diff = " << diff << std::endl;

 return boost::shared_ptr<LensDistortion>(new TsaiLensDistortion(tsai_params));
}

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

// Copied from LensDistortion.cc
struct DistortOptimizeFunctor :  public math::LeastSquaresModelBase<DistortOptimizeFunctor> {
  typedef Vector2 result_type;
  typedef Vector2 domain_type;
  typedef Matrix<double> jacobian_type;

  const camera::PinholeModel  & m_cam;
  const camera::LensDistortion& m_distort;

  DistortOptimizeFunctor(const camera::PinholeModel& cam, const camera::LensDistortion& d) : m_cam(cam), m_distort(d) {}

  inline result_type operator()( domain_type const& x ) const {
    return m_distort.undistorted_coordinates(m_cam, x);
  }
};

// TODO: This needs to become a view!

template <class ImageInT, class ImageOutT>
void fast_epipolar_transform_sub(ImageInT     const& image_in,
                                 PinholeModel const& camera_model_in,
                                 CAHVModel    const& epipolar_cahv,
                                 ImageOutT      & image_out) {

  std::cout << "Testing TSAI solver code..." << std::endl;
  boost::shared_ptr<LensDistortion> inverse_lens =
      solve_inverse_lens_distortion(camera_model_in, Vector2(image_in.cols(), image_in.rows()));
  std::cout << "Finished testing!" << std::endl;
  //vw_throw(ArgumentErr() << "DEBUG!\n");

  // TODO: The output size is larger than by our standard method!
  // Figure out the output size
  CameraTransform<PinholeModel, CAHVModel> cam_trans( camera_model_in, epipolar_cahv );

  BBox2f output_roi = compute_transformed_bbox_fast(image_in, cam_trans);

  image_out.set_size(output_roi.width(), output_roi.height());

  // Set up input image for interpolation
  InterpolationView<EdgeExtensionView<ImageInT, ZeroEdgeExtension>, BilinearInterpolation> input_wrapper(image_in);

  // Compute all of the pixel values

  // Since these are pinhole cameras we can treat their center as constant for all pixels
  Vector3 cahv_center = epipolar_cahv.camera_center(Vector2(0,0));

  // Extract some pinhole information
  const double pixel_pitch = camera_model_in.pixel_pitch();
  //const vw::camera::LensDistortion* distortion = camera_model_in.lens_distortion();
  const vw::camera::LensDistortion* distortion = inverse_lens.get();

  // TODO: Convert inverse distortion model here!

  DistortOptimizeFunctor model(camera_model_in, *distortion);
  int status;

  double abs_tolerance  = 0.10;
  double rel_tolerance  = 0.10;
  double max_iterations = 6;

  Vector2 loc_d;

  // TODO: Speed this up!!
  for (int r=0; r<image_out.rows(); ++r) {
    for (int c=0; c<image_out.cols(); ++c) {

  // DEBUG
  //for (int r=500; r<1000; ++r) {
  //  for (int c=500; c<1000; ++c) {

      // (1) Call src PixelToVector to find the vector emanating from the camera center.
      Vector3 vec = epipolar_cahv.pixel_to_vector(Vector2(c,r)); // This step is fast

      // (2) take resulting vector and call dest camera's VectorToPixel on it
      Vector2 loc_nd = camera_model_in.point_to_pixel_no_distortion(vec+cahv_center);

      Vector2 loc_raw = loc_nd*pixel_pitch;

      Vector2 loc_d = distortion->distorted_coordinates(camera_model_in, loc_raw);
      //Vector2 seed = loc_raw;
      ////if (c != 500)
      ////  seed = loc_d + Vector2(1.0, 0);
      //loc_d = math::levenberg_marquardt( model, seed, loc_raw, status, abs_tolerance, rel_tolerance, max_iterations );

      //std::cout << "Delta = " << loc_d - loc_raw<< std::endl;

      Vector2 loc = loc_d/pixel_pitch;

      image_out(c,r) = input_wrapper(loc[0], loc[1]);

      // (1) Call src PixelToVector to find the vector emanating from the camera center.
      Vector3 vec = epipolar_cahv.pixel_to_vector(Vector2(c,r)); // This step is fast

      // (2) take resulting vector and call dest camera's VectorToPixel on it
      Vector2f loc = camera_model_in.point_to_pixel(vec+cahv_center);

      // This method mimics the regular method and gets the same result.
      Vector2f loc = cam_trans.reverse(Vector2(c,r));
      image_out(c,r) = input_wrapper(loc[0], loc[1]);

    }
  }

}

*/

// Given two epipolar rectified CAHV camera models and input images,
// get the aligned version of the images suitable for writing to disk and processing.
template <class ImageInT, class ImageOutT>
void fast_epipolar_transform(std::string const& left_camera_file,
                                     std::string const& right_camera_file,
                                     boost::shared_ptr<camera::CameraModel> const left_cahv_camera,
                                     boost::shared_ptr<camera::CameraModel> const right_cahv_camera,
                                     ImageInT  const& left_image_in,
                                     ImageInT  const& right_image_in,
                                     ImageOutT      & left_image_out,
                                     ImageOutT      & right_image_out) {

    // In the epipolar alignment case, the "camera_models" function returns the CAHVModel type!
    CAHVModel* left_epipolar_cahv  = dynamic_cast<CAHVModel*>(vw::camera::unadjusted_model(&(*left_cahv_camera )));
    CAHVModel* right_epipolar_cahv = dynamic_cast<CAHVModel*>(vw::camera::unadjusted_model(&(*right_cahv_camera)));
    if (!left_epipolar_cahv || !right_epipolar_cahv) {
      vw_throw(ArgumentErr() << "load_cahv_pinhole_camera_model: CAHVModel cast failed!\n");
    }

    std::string lcase_file = boost::to_lower_copy(left_camera_file);

    PinholeModel left_pin (left_camera_file );
    PinholeModel right_pin(right_camera_file);

    update_pinhole_for_fast_point2pixel(left_pin,  Vector2i(left_image_in.cols(),  left_image_in.rows()));
    update_pinhole_for_fast_point2pixel(right_pin, Vector2i(right_image_in.cols(), right_image_in.rows()));

                       //Dest image =  Source image, source model, dest model
    left_image_out  = camera_transform(left_image_in,  left_pin,  *left_epipolar_cahv );
    right_image_out = camera_transform(right_image_in, right_pin, *right_epipolar_cahv);

    //fast_epipolar_transform_sub(left_image_in,  left_pin,  *left_epipolar_cahv,  left_image_out );
    //fast_epipolar_transform_sub(right_image_in, right_pin, *right_epipolar_cahv, right_image_out);

}

/// Draw the interest points and write as an image.
template <class ViewT>
void write_point_image(std::string out_file_name,
                       ImageViewBase<ViewT> const& src,
                       InterestPointList const& points) {

  ImageView<PixelRGB<uint8> > viz = pixel_cast<PixelRGB<uint8> >(channel_cast_rescale<uint8>(src));

  // Draw points into color planes
  int n = 0;
  for (InterestPointList::const_iterator pt = points.begin();
       pt != points.end() && n < MAX_POINTS_TO_DRAW; ++pt, ++n) {
    // Draw a red line from the point outward along the orientation
    for (int r=0; r<(int)(8*(*pt).scale); ++r){
      int i = (int)(0.5 + (*pt).x + r*cos((*pt).orientation));
      int j = (int)(0.5 + (*pt).y + r*sin((*pt).orientation));
      // red, green, blue
      viz(i,j) = PixelRGB<uint8>(255, 0, 0);
    }
    // Draw a green 3x3 filled square at the point to indicate center
    int i0 = (int)(0.5 + (*pt).x);
    int j0 = (int)(0.5 + (*pt).y);
    for (int j=j0-1; j<=j0+1; ++j){
      for (int i=i0-1; i<=i0+1; ++i){
        // red, green, blue
        viz(i,j) = PixelRGB<uint8>(0, 255, 0);
      }
    }
  }

  write_image(out_file_name, viz);
}

/// Produces Interest Points from files
void find_interest_points( std::string const& image_name, InterestPointList& ip, Options const& opt ) {
  const float IDEAL_LOG_THRESHOLD    = .03;
  const float IDEAL_OBALOG_THRESHOLD = .07;
  const float IDEAL_HARRIS_THRESHOLD = 1.2e-5;

  DiskImageView<PixelRGB<uint8> > input_image( image_name );
  ip.clear();

  // Run an interest point detector using the selected parameters.
  // - Templated types require a lot of if statements to process
  vw_out(InfoMessage) << "\nInterest Point Detection: "  << image_name << "\n";
  if (opt.interest_operator == "harris") {
    HarrisInterestOperator
      interest_operator(IDEAL_HARRIS_THRESHOLD/opt.detect_gain);
    if (!opt.single_scale) {
      ScaledInterestPointDetector<HarrisInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    } else {
      InterestPointDetector<HarrisInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    }
  } else if (opt.interest_operator == "log" ) {
    // Use a scale-space Laplacian of Gaussian feature detector. The
    // associated threshold is abs(interest) > interest_threshold.
    LogInterestOperator interest_operator(IDEAL_LOG_THRESHOLD/opt.detect_gain);
    if (!opt.single_scale) {
      ScaledInterestPointDetector<LogInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    } else {
      InterestPointDetector<LogInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    }
  } else if (opt.interest_operator == "obalog" ) {
    OBALoGInterestOperator
      interest_operator(IDEAL_OBALOG_THRESHOLD/opt.detect_gain);
    IntegralInterestPointDetector<OBALoGInterestOperator>
      detector( interest_operator );
    ip = detect_interest_points(input_image, detector);
  } else {
    std::cout << "Unknown interest operator: " << opt.interest_operator << ".  Options are : [ Harris, LoG, OBALoG ]\n";
    exit(0);
  }
  vw_out() << "\t Found " << ip.size() << " interest points.\n";

  // Write out images with interest points marked
  if (opt.debug_images)
    write_point_image(opt.output_prefix + "-debug.tif", input_image, ip);

  // Generate descriptors for interest points using selected method.
  vw_out(InfoMessage) << "Generating descriptors using " << opt.descriptor_generator << " generator... " << std::flush;
  if (opt.descriptor_generator == "patch") {
    PatchDescriptorGenerator descriptor;
    describe_interest_points( input_image, descriptor, ip );
  } else if ( opt.descriptor_generator == "pca" ) {
    PCASIFTDescriptorGenerator descriptor("pca_basis.exr", "pca_avg.exr");
    describe_interest_points( input_image, descriptor, ip );
  } else if ( opt.descriptor_generator == "sgrad" ) {
    SGradDescriptorGenerator descriptor;
    describe_interest_points( input_image, descriptor, ip );
  } else {
    std::cout << "Unknown interest descriptor: " << opt.descriptor_generator << ".  Options are : [ patch, pca, sgrad ]\n";
    exit(0);
  }
  vw_out(InfoMessage) << "done." << std::endl;
  if ( opt.save_intermediate ) // Optionally write out a binary interest point file
    write_binary_ip_file(fs::path(image_name).replace_extension("vwip").string(), ip);
}

/// Find the transform between image pairs and write transformed images to disk
void align_images( Options & opt ) {

  // Determine the size of the reference image
  Vector2i ref_size;
  DiskImageView<PixelRGB<uint8> > ref_image  ( opt.left_image_file  );
  DiskImageView<PixelRGB<uint8> > input_image( opt.right_image_file );
  ref_size[0] = ref_image.cols();
  ref_size[1] = ref_image.rows();

  // TODO: Refactor some code into functions!

  Matrix<double> align_matrix;
  if ( opt.align_method != "epipolar" ) {
    // Find IPs
    InterestPointList ref_ip, input_ip;
    find_interest_points( opt.left_image_file, ref_ip, opt );
    find_interest_points( opt.right_image_file, input_ip, opt );

    // Convert from InterestPointList to a std::vector of InterestPoints
    std::vector<InterestPoint> ref_ip_v( ref_ip.size() ),
                               input_ip_v( input_ip.size() );
    std::copy( ref_ip.begin(),   ref_ip.end(),   ref_ip_v.begin() );
    std::copy( input_ip.begin(), input_ip.end(), input_ip_v.begin() );

    // Use an interest point matcher to find matched pairs of interest points
    DefaultMatcher matcher(opt.matcher_threshold);
    std::vector<InterestPoint> matched_ip1, matched_ip2;
    matcher(ref_ip_v, input_ip_v, matched_ip1, matched_ip2,
            TerminalProgressCallback( "tools.ipalign", "Matching:"));
    vw_out(InfoMessage) << "\tFound " << matched_ip1.size() << " putative matches.\n";

    // RANSAC is used to fit a similarity transform between the matched sets of points
    // - We generate the transformation matrix and the lists of inlier indices
    std::vector<Vector3> ransac_ip1 = iplist_to_vectorlist(matched_ip1),
                         ransac_ip2 = iplist_to_vectorlist(matched_ip2);
    std::vector<size_t>  indices;

    if ( opt.align_method == "homography" ) { // Full projective transform
      math::RandomSampleConsensus<math::HomographyFittingFunctor, math::InterestPointErrorMetric>
                  ransac(math::HomographyFittingFunctor(), math::InterestPointErrorMetric(), opt.ransac_iterations,
                         opt.inlier_threshold, ransac_ip1.size()/2, true);
      align_matrix = ransac(ransac_ip2, ransac_ip1);
      indices      = ransac.inlier_indices(align_matrix, ransac_ip2, ransac_ip1);
    }
    if ( opt.align_method == "similarity" ) { // Similarity transform
      math::RandomSampleConsensus<math::AffineFittingFunctor, math::InterestPointErrorMetric>
                  ransac(math::AffineFittingFunctor(), math::InterestPointErrorMetric(), opt.ransac_iterations,
                         opt.inlier_threshold, ransac_ip1.size()/2, true);
      align_matrix = ransac(ransac_ip2, ransac_ip1);
      indices      = ransac.inlier_indices(align_matrix, ransac_ip2, ransac_ip1);
    }

    if ( opt.save_intermediate ) { // Save the list of matched pixels to a binary file
      std::string output_ip_filename = opt.output_prefix + "-left_right_ip.match";
      std::vector<InterestPoint> final_ip1, final_ip2;
      BOOST_FOREACH( size_t& index, indices ) {
        final_ip1.push_back(matched_ip1[index]);
        final_ip2.push_back(matched_ip2[index]);
      }
      write_binary_match_file(output_ip_filename, final_ip1, final_ip2);
    }
  } // End finding align matrix case

  // Write the transformed input image to a file
  ImageViewRef< PixelRGB<uint8> > left_aligned, right_aligned;
  if ( opt.align_method == "epipolar" ) {

    // Load both input images as CAHV models.
    boost::shared_ptr<CAHVModel> left_cahv, right_cahv;
    left_cahv  = load_cahv_pinhole_camera_model(opt.left_image_file,  opt.left_camera_file );
    right_cahv = load_cahv_pinhole_camera_model(opt.right_image_file, opt.right_camera_file);

    // Generate epipolar-aligned camera models
    boost::shared_ptr<CAHVModel> epipolar_left_cahv (new CAHVModel);
    boost::shared_ptr<CAHVModel> epipolar_right_cahv(new CAHVModel);
    epipolar(*(left_cahv.get()),  *(right_cahv.get()),
             *(epipolar_left_cahv.get()), *(epipolar_right_cahv.get()));

/*
    // Now that we have our epipolar aligned models, transform the input images to match.
    get_epipolar_transformed_images(opt.left_camera_file,   opt.right_camera_file,
                        epipolar_left_cahv, epipolar_right_cahv,
                            ref_image, input_image,
                            left_aligned, right_aligned);
*/

    ImageView< PixelRGB<uint8> > left_aligned2, right_aligned2;
/*    fast_epipolar_transform(opt.left_camera_file,   opt.right_camera_file,
                            epipolar_left_cahv, epipolar_right_cahv,
                            ref_image, input_image,
                            left_aligned2, right_aligned2);
*/
    // TODO: Why is writing to an ImageView first SO MUCH faster???
    get_epipolar_transformed_images(opt.left_camera_file,   opt.right_camera_file,
                        epipolar_left_cahv, epipolar_right_cahv,
                            ref_image, input_image,
                            left_aligned2, right_aligned2);

    std::string output_path = opt.output_prefix + "_right_image.tif";
    write_image(output_path, crop(edge_extend(right_aligned2,ZeroEdgeExtension()),bounding_box(ref_image)),
                TerminalProgressCallback( "tools.ipalign", "Writing right:") );
    output_path = opt.output_prefix + "_left_image.tif";
    write_image(output_path, left_aligned2,  TerminalProgressCallback( "tools.ipalign", "Writing left :") );

  }
  else { // Non-epipolar case
    std::string output_path = opt.output_prefix + "_transformed_right_image.tif";
    right_aligned = transform(input_image, HomographyTransform(align_matrix),
                              ref_size[0], ref_size[1]);
    write_image(output_path, right_aligned, TerminalProgressCallback( "tools.ipalign", "Writing:") );
  }

}

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

void handle_arguments( int argc, char* argv[], Options& opt ) {
  po::options_description general_options("Options");
  general_options.add_options()
    ("help,h", "Display this help message")
    ("debug-images,d", po::bool_switch(&opt.debug_images),
                       "Produce additional debugging images as well as the aligned image.")
    ("tile-size", po::value(&opt.tile_size)->default_value(1024),
                  "Specify the tile size for detecting interest points.")
    ("output-prefix,o", po::value(&opt.output_prefix)->default_value("aligned"),
                        "Specify the output prefix")
    ("save-intermediate,s", po::bool_switch(&opt.save_intermediate),
                            "Save working VWIP and Match files")

    // Interest point detector options
    ("detector-gain,g", po::value(&opt.detect_gain)->default_value(1.0),
                        "Increasing this number will increase the  gain at which interest points are detected.")
    ("interest-operator", po::value(&opt.interest_operator)->default_value("OBALoG"),
                          "Choose an interest metric from [LoG, Harris, OBALoG]")
    ("single-scale", po::bool_switch(&opt.single_scale),
                     "Do not use the scale-space interest point detector.")

    // Descriptor generator options
    ("descriptor-generator", po::value(&opt.descriptor_generator)->default_value("sgrad"),
                             "Choose a descriptor generator from [patch, pca, sgrad]")

    // Alignment options
    ("matcher-threshold,t", po::value(&opt.matcher_threshold)->default_value(0.5),
                            "Rejects points during matching if best > matcher_threshold * second_best")
    ("inlier-threshold,i", po::value(&opt.inlier_threshold)->default_value(10),
                           "RANSAC inlier threshold.")
    ("ransac-iterations", po::value(&opt.ransac_iterations)->default_value(100),
                          "Number of RANSAC iterations.")
    ("align-method", po::value(&opt.align_method)->default_value("similarity"),
                   "Choose similarity, homography, or epipolar image alignment.");

  po::options_description positional("");
  positional.add_options()
    ("left-image",   po::value(&opt.left_image_file),  "Left image"  )
    ("right-image",  po::value(&opt.right_image_file), "Right image" )
    ("left-camera",  po::value(&opt.left_camera_file), "Left camera" )
    ("right-camera", po::value(&opt.right_camera_file),"Right camera");

  po::positional_options_description positional_desc;
  positional_desc.add("left-image",   1);
  positional_desc.add("right-image",  1);
  positional_desc.add("left-camera",  1);
  positional_desc.add("right-camera", 1);

  po::options_description all_options;
  all_options.add(general_options).add(positional);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(all_options).positional(positional_desc).run(), vm );
    po::notify( vm );
  } catch (const po::error& e) {
    vw_throw( ArgumentErr() << "Error parsing input:\n\t" << e.what() << general_options );
  }

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " <left image> <right image> <left camera model> <right camera model>\n";

  if ( vm.count("help") )
    vw_throw( ArgumentErr() << usage.str() << general_options );

  boost::to_lower(opt.interest_operator);
  boost::to_lower(opt.descriptor_generator);
  boost::to_lower(opt.align_method);

  if (opt.left_image_file.empty() || opt.right_image_file.empty())
      vw_throw( ArgumentErr() << "Missing required image files.\n\n" << usage );

  if ((opt.align_method == "epipolar") && (opt.left_camera_file.empty() || opt.right_camera_file.empty()))
      vw_throw( ArgumentErr() << "Missing required camera model files.\n\n" << usage );
}

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

  Options opt;
  //try {
    handle_arguments( argc, argv, opt ); // Load user arguments
    align_images( opt );                 // Do all of the work!
    /*
  } catch ( const ArgumentErr& e ) {     // Everything else is error catching...
    vw_out() << e.what() << "\n";
    return 1;
  } catch ( const Exception& e ) {
    std::cerr << "\n\nVW Error: " << e.what() << "\n";
    return 1;
  } catch ( const std::bad_alloc& e ) {
    std::cerr << "\n\nError: Ran out of Memory!\n";
    return 1;
  } catch ( const std::exception& e ) {
    std::cerr << "\n\nError: " << e.what() << "\n";
    return 1;
  }
*/
  return 0;
}