//
// file DiamondOverlay.cc
// David Cosgrove
// AstraZeneca
// 23rd January 2007
//

#include <iterator>
#include <iostream>

#include "stddefs.H"
#include "DiamondOverlay.H"

#include <boost/lexical_cast.hpp>

namespace DACLIB {
  void eigen_solve_4b4f( float **a , float &value , float *vector );
}

using namespace std;

namespace DACLIB {

  // ****************************************************************************
  DiamondOverlay::DiamondOverlay( const vector<float> &fixed ,
                                  const vector<float> &moving ) :
    rot_matrix_( 0 ) , p_( 0 ) , e_zero_( -1.0 ) , e_( -1.0 ){

    if( fixed.size() != moving.size() ) {
      throw DiamondOverlayError( "fixed and moving vectors must be same size." );
    }

    fixed_ = fixed;
    moving_ = moving;

  }

  // ****************************************************************************
  DiamondOverlay::DiamondOverlay( const vector<float> &fixed ,
                                  const vector<float> &moving ,
                                  const vector<float> &weights ) :
    rot_matrix_( 0 ) , p_( 0 ) , e_zero_( -1.0 ) , e_( -1.0 ) {

    if( fixed.size() != moving.size() ) {
      throw DiamondOverlayError( "fixed and moving vectors must be same size." );
    }

    fixed_ = fixed;
    moving_ = moving;
    weights_ = weights;

  }

  // ****************************************************************************
  DiamondOverlay::DiamondOverlay( const float *fixed , const float *moving ,
                                  int num_points ) :
    rot_matrix_( 0 ) , p_( 0 ) , e_zero_( -1.0 ) , e_( -1.0 ) {

    fixed_ = vector<float>( fixed , fixed + 3 * num_points );
    moving_ = vector<float>( moving , moving + 3 * num_points );

  }

  // ****************************************************************************
  DiamondOverlay::DiamondOverlay( const float *fixed , const float *moving ,
                                  const float *weights , int num_points ) :
    rot_matrix_( 0 ) , p_( 0 ) , e_zero_( -1.0 ) , e_( -1.0 ) {

    fixed_ = vector<float>( fixed , fixed + 3 * num_points );
    moving_ = vector<float>( moving , moving + 3 * num_points );
    weights_ = vector<float>( weights , weights + num_points );

  }

  // ****************************************************************************
  // individual sets of coords.
  DiamondOverlay::DiamondOverlay( const vector<vector<float> > &fixed ,
                                  const vector<vector<float> > &moving ) :
    rot_matrix_( 0 ) , p_( 0 ) , e_zero_( -1.0 ) , e_( -1.0 ) {

    fixed_.reserve( 3 * fixed.size() );
    moving_.reserve( 3 * moving.size() );
    for( int i = 0 , is = fixed.size() ; i < is ; ++i ) {
      if( fixed[i].size() != 3 ) {
        throw DiamondOverlayError( "Bad coords in fixed coords." );
      }
      if( moving[i].size() != 3 ) {
        throw DiamondOverlayError( "Bad coords in moving coords." );
      }

      fixed_.insert( fixed_.end() , fixed[i].begin() , fixed[i].end() );
      moving_.insert( moving_.end() , moving[i].begin() , moving[i].end() );
    }

  }

  // ****************************************************************************
  // individual sets of coords.
  DiamondOverlay::DiamondOverlay( const vector<vector<float> > &fixed ,
                                  const vector<vector<float> > &moving ,
                                  const vector<float> &weights ) :
    rot_matrix_( 0 ) , p_( 0 ) , e_zero_( -1.0 ) , e_( -1.0 ) {

    fixed_.reserve( 3 * fixed.size() );
    moving_.reserve( 3 * moving.size() );
    for( int i = 0 , is = fixed.size() ; i < is ; ++i ) {
      if( fixed[i].size() != 3 ) {
        throw DiamondOverlayError( "Bad coords in fixed coords." );
      }
      if( moving[i].size() != 3 ) {
        throw DiamondOverlayError( "Bad coords in moving coords." );
      }

      fixed_.insert( fixed_.end() , fixed[i].begin() , fixed[i].end() );
      moving_.insert( moving_.end() , moving[i].begin() , moving[i].end() );
    }

    weights_ = weights;

  }

  // ****************************************************************************
  DiamondOverlay::~DiamondOverlay() {

    DACLIB::destroy_square_matrix( rot_matrix_ );
    DACLIB::destroy_square_matrix( p_ );

  }

  // ****************************************************************************
  // calculate the initial residual error
  void DiamondOverlay::calc_e_zero() {

    e_zero_ = 0.0F;
    for( int i = 0 , is = fixed_.size() ; i < is ; i += 3 ) {
      float sq_dist = sq_distance( &fixed_[i] , &moving_[i] );
      if( !weights_.empty() ) {
        sq_dist *= weights_[i/3];
      }
      e_zero_ += sq_dist;
    }

  }

  // ****************************************************************************
  // assemble the M matrix (eq. 16, D88). It is not a symmetric matrix.
  void DiamondOverlay::calc_m( float m[3][3] ) {

    for( int i = 0 ; i < 3 ; ++i ) {
      for( int j = 0 ; j < 3 ; ++j ) {
        m[i][j] = 0.0;
        for( int k = 0 , ks = fixed_.size() ; k < ks ; k += 3 ) {
          if( weights_.empty() ) {
            m[i][j] += moving_[k+i] * fixed_[k+j];
          } else {
            m[i][j] += moving_[k+i] * fixed_[k+j] * weights_[k/3];
	  }
        }
      }
    }

  }

  // ****************************************************************************
  // calculate matrix Q, equation 17, D88.
  void DiamondOverlay::calc_q( const float m[3][3] , float q[3][3] ) {

    float alpha = m[0][0] + m[1][1] + m[2][2];

    for( int i = 0 ; i < 3 ; ++i ) {
      for( int j = 0 ; j < 3 ; ++j ) {
        q[i][j] = m[i][j] + m[j][i];
      }
      q[i][i] -= 2.0 * alpha;
    }

  }

  // ****************************************************************************
  // calculate matrix Q, equation 17, D88.
  void DiamondOverlay::calc_e() {

    if( !rot_matrix_ ) {
      calc_rot_matrix();
    }

    float res[4];
    float e;

    res[0] = p_[0][0] * vector_[0] + p_[0][1] * vector_[1] +
        p_[0][2] * vector_[2] + p_[0][3] * vector_[3];

    res[1] = p_[1][0] * vector_[0] + p_[1][1] * vector_[1] +
        p_[1][2] * vector_[2] + p_[1][3] * vector_[3];

    res[2] = p_[2][0] * vector_[0] + p_[2][1] * vector_[1] +
        p_[2][2] * vector_[2] + p_[2][3] * vector_[3];

    res[3] = p_[3][0] * vector_[0] + p_[3][1] * vector_[1] +
        p_[3][2] * vector_[2] + p_[3][3] * vector_[3];

    e = vector_[0] * res[0] + vector_[1] * res[1] + vector_[2] * res[2] +
        vector_[3] * res[3];

    e_ = e_zero_ - 2.0F * e;

  }

  // ****************************************************************************
  // make sure that there are at least 3 distinct sets of coordinates
  // for at least 1 of the input coord sets.  This includes cases
  // where 2 coords are on top of each other. Throws exception if not
  void DiamondOverlay::check_input_coords() const {

    if( !(at_least_3_coords( fixed_ ) && at_least_3_coords( moving_ )) ) {
      throw DiamondOverlayError( "Not enough distinct coords for overlay" );
    }

  }

  // ****************************************************************************
  // checks given coords for problem above.
  bool DiamondOverlay::at_least_3_coords( const std::vector<float> &cds ) const {

    vector<float> eff_cds = cds;
    bool it_changed( false );

    do {
      it_changed = false;
      for( int i = 0 , is = eff_cds.size() - 3 ; i < is ; i +=3 ) {
        for( int j = i + 3 , js = is + 3 ; j < js ; j += 3 ) {
          if( sq_distance( &cds[i] , &cds[j] ) < 1.0e-3 ) {
            eff_cds.erase( eff_cds.begin() + i , eff_cds.begin() + i + 3 );
            it_changed = true;
            break;
          }
        }
        if( it_changed ) {
          break;
        }
      }

    } while( it_changed && eff_cds.size() > 6 );

    return( eff_cds.size() > 6 );

  }

  // ****************************************************************************
  void DiamondOverlay::calc_p() {

    float m[3][3] , q[3][3];

    calc_e_zero();
    calc_m( m );
    calc_q( m , q );

    // compute v, eq. 18, D88.
    float v[3];
    v[0] = m[1][2] - m[2][1];
    v[1] = m[2][0] - m[0][2];
    v[2] = m[0][1] - m[1][0];

    // now do p
    DACLIB::make_square_matrix( p_ , 4 );

    for( int i = 0 ; i < 3 ; ++i ) {
      for( int j = 0 ; j < 3 ; ++j ) {
        p_[i][j] = q[i][j];
      }
    }

    p_[0][3] = p_[3][0] = v[0];
    p_[1][3] = p_[3][1] = v[1];
    p_[2][3] = p_[3][2] = v[2];
    p_[3][3] = 0.0;

  }

  // ****************************************************************************
  // build the rotation matrix, using eq. 7, D88.
  void DiamondOverlay::calc_rot_matrix() {

    check_input_coords();
    if( !p_ ) {
      calc_p();
    }

    if( !rot_matrix_ ) {
      DACLIB::make_square_matrix( rot_matrix_ , 3 );
    }

    float value;

    // eigen_solve destroys the matrix it works on, so do it on a copy
    static float **p_tmp = 0;
    if( !p_tmp ) {
      DACLIB::make_square_matrix( p_tmp , 4 );
    }

    copy( p_[0] , p_[0] + 16 , p_tmp[0] );
    DACLIB::eigen_solve_4b4f( p_tmp , value , vector_ );

    for( int i = 0 ; i < 4 ; ++i ) {
      try{
        boost::lexical_cast<float>( vector_[i] );
      } catch( boost::bad_lexical_cast &e ) {
        throw DiamondOverlayError( "Failed to find to valid overlay." );
      }
    }

    // use the notation of eq. 1, D88.
    const float lambda = vector_[0];
    const float mu = vector_[1];
    const float nu = vector_[2];
    const float sigma = vector_[3];
    const float mu_sq = mu * mu;
    const float lambda_sq = lambda * lambda;
    const float ups_sq = nu * nu;
    const float sigma_sq = sigma * sigma;

    float *rr = rot_matrix_[0];
    rr[0] = lambda_sq - mu_sq - ups_sq + sigma_sq;
    rr[4] = -lambda_sq + mu_sq - ups_sq + sigma_sq;
    rr[8] = -lambda_sq - mu_sq + ups_sq + sigma_sq;
    rr[3] = 2.0 * ( lambda * mu + nu * sigma );
    rr[6] = 2.0 * ( lambda * nu - mu * sigma );
    rr[1] = 2.0 * ( lambda * mu - nu * sigma );
    rr[7] = 2.0 * ( mu * nu + lambda * sigma );
    rr[2] = 2.0 * ( lambda * nu + mu * sigma );
    rr[5] = 2.0 * ( mu * nu - lambda * sigma );

  }

  // ****************************************************************************
  void DiamondOverlay::get_rot_matrix( float rot[3][3] ) {

    if( !p_ || !rot_matrix_ ) {
      calc_rot_matrix();
    }

    // don't assume that rot is created with DACLIB::make_square_matrix, though
    // we know that rot_matrix__ was.
    float *rr = rot_matrix_[0];
    rot[0][0] = rr[0];
    rot[0][1] = rr[1];
    rot[0][2] = rr[2];
    rot[1][0] = rr[3];
    rot[1][1] = rr[4];
    rot[1][2] = rr[5];
    rot[2][0] = rr[6];
    rot[2][1] = rr[7];
    rot[2][2] = rr[8];

  }

  // ****************************************************************************
  void DiamondOverlay::get_p( float **p ) {

    if( !p_ ) {
      calc_p();
    }

    // don't assume that p is created with DACLIB::make_square_matrix, though
    // we know that p_ was.
    float *pp = p_[0];
    p[0][0] = pp[0];
    p[0][1] = pp[1];
    p[0][2] = pp[2];
    p[0][3] = pp[3];

    p[1][0] = pp[4];
    p[1][1] = pp[5];
    p[1][2] = pp[6];
    p[1][3] = pp[7];

    p[2][0] = pp[8];
    p[2][1] = pp[9];
    p[2][2] = pp[10];
    p[2][3] = pp[11];

    p[3][0] = pp[12];
    p[3][1] = pp[13];
    p[3][2] = pp[14];
    p[3][3] = pp[15];

  }

  // ****************************************************************************
  float DiamondOverlay::get_e_zero() {

    if( e_zero_ < 0.0F ) {
      calc_e_zero();
    }

    return e_zero_;

  }

  // ****************************************************************************
  float DiamondOverlay::get_e() {
    if( e_ < 0.0F ) {
      calc_e() ;
    }

    return e_;

  }

} // end of namespace DACLIB


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_DETAIL_COUNT_IF_WITH_BALLOT_HPP
#define BOOST_COMPUTE_ALGORITHM_DETAIL_COUNT_IF_WITH_BALLOT_HPP

#include <boost/compute/context.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/algorithm/reduce.hpp>
#include <boost/compute/functional/detail/nvidia_ballot.hpp>
#include <boost/compute/functional/detail/nvidia_popcount.hpp>
#include <boost/compute/detail/meta_kernel.hpp>

namespace boost {
namespace compute {
namespace detail {

template<class InputIterator, class Predicate>
inline size_t count_if_with_ballot(InputIterator first,
                                   InputIterator last,
                                   Predicate predicate,
                                   command_queue &queue)
{
    size_t count = iterator_range_size(first, last);
    size_t block_size = 32;
    size_t block_count = count / block_size;
    if(block_count * block_size != count){
        block_count++;
    }

    const ::boost::compute::context &context = queue.get_context();

    ::boost::compute::vector<uint_> counts(block_count, context);

    ::boost::compute::detail::nvidia_popcount<uint_> popc;
    ::boost::compute::detail::nvidia_ballot<uint_> ballot;

    meta_kernel k("count_if_with_ballot");
    k <<
        "const uint gid = get_global_id(0);\n" <<

        "bool value = false;\n" <<
        "if(gid < count)\n" <<
        "    value = " << predicate(first[k.var<const uint_>("gid")]) << ";\n" <<

        "uint bits = " << ballot(k.var<const uint_>("value")) << ";\n" <<

        "if(get_local_id(0) == 0)\n" <<
            counts.begin()[k.var<uint_>("get_group_id(0)") ]
                << " = " << popc(k.var<uint_>("bits")) << ";\n";

    k.add_set_arg<const uint_>("count", count);

    k.exec_1d(queue, 0, block_size * block_count, block_size);

    uint_ result;
    ::boost::compute::reduce(
        counts.begin(),
        counts.end(),
        &result,
        queue
    );
    return result;
}

} // end detail namespace
} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_DETAIL_COUNT_IF_WITH_BALLOT_HPP


#define BOOST_TEST_MODULE analytic_grad_shafranov_tests
#define __STDCPP_WANT_MATH_SPEC_FUNCS__ 1

#include <algorithm>
#include <complex>
#include <vector>
#include <utility>

#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>
#include <boost/math/constants/constants.hpp>

BOOST_AUTO_TEST_SUITE( soloviev_test_suite, * boost::unit_test::tolerance( 1e-6 ) )

BOOST_AUTO_TEST_CASE( soloviev_test )
{

}

BOOST_AUTO_TEST_SUITE_END()


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

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

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/meta/hierarchy/logical.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/as_arithmetic.hpp>
// if_else_zero is not stil correct
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/constant/zero.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  // -----------------------------------------------------------------------------------------------
  // genmask from logical
  BOOST_DISPATCH_OVERLOAD ( genmask_
                          , (typename A0, typename X)
                          , bs::simd_
                          , bs::pack_< bs::logical_<A0>, X >
                          )
  {
    using result_t = as_arithmetic_t<A0>;

    BOOST_FORCEINLINE result_t operator()( A0 const& a0 ) const BOOST_NOEXCEPT
    {
      // TODO: Handle non-logical mask values
      return bitwise_cast<result_t>(a0);
    }
  };

   BOOST_DISPATCH_OVERLOAD(genmask_
                          , (typename A0,typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>,X>
                          )

  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
       return if_else(a0,  Allbits<A0>(), Zero<A0>());
    }
  };
} } }

#endif


/*Copyright (c) 2021 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include "TrajectoryAnalysis.h"

#include <cmath>
#include <complex>
#include <Eigen/Dense>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <sstream>
#include <string>

#include "utils.h"

void CAPHamiltonian::set_eta_list(std::map<std::string, std::string> params)
{
	double eta_step;
	size_t nsteps;
	std::stringstream stepss(params["eta_step"]);
	std::stringstream nstepss(params["nsteps"]);
	std::istringstream save_traj(params["save_trajectory"]);
	stepss >> eta_step;
	nstepss >> nsteps;
	save_traj >> std::boolalpha >> do_output;
	eta_step*=1E-5;
	for(size_t i=0;i<=nsteps;i++)
		eta_list.push_back(i*eta_step);
}

CAPHamiltonian::CAPHamiltonian(Eigen::MatrixXd h0,Eigen::MatrixXd cap,std::map<std::string,
		std::string> params,std::string filename)
{
	CAP_MAT = cap;
	ZERO_ORDER_H = h0;
	nstates = h0.cols();
	set_eta_list(params);
	finame = filename;
}

void CAPHamiltonian::track_state(size_t state_idx)
{
	std::cout << "Results for state:" << state_idx << std::endl;
	std::cout << "----------------------" << std::endl;
	EigenvalueTrajectory traj(all_roots[0],state_idx);
	for(size_t i=1;i<all_roots.size();i++)
		traj.add_state(all_roots[i]);
	traj.analyze();
	std::cout << "----------------------" << std::endl;
	trajectories.push_back(traj);
}

void CAPHamiltonian::track_states()
{
	std::cout << "---------------------" << std::endl;
	std::cout << "Trajectory Analysis:" << std::endl;
	std::cout << "--------------------" << std::endl;
	for(size_t i=0;i<nstates;i++)
		track_state(i);
}

void CAPHamiltonian::save_trajectory()
{
	std::ofstream trajFile;
	std::string data_finame = finame + ".data";
	trajFile.open(data_finame);
    trajFile << std::left << std::setw(18) << std::setfill(' ')
    << "State" << std::left << std::setw(18) << std::setfill(' ')
    << "eta" << std::left << std::setw(18) << std::setfill(' ')
    << "Uncorr_Re" << std::left << std::setw(18) << std::setfill(' ')
    << "Uncorr_Im" << std::left << std::setw(18) << std::setfill(' ');
    trajFile << "Corr_Re" << std::left << std::setw(18) << std::setfill(' ') << "Corr_Im" << std::endl;
    trajFile.close();
}

void CAPHamiltonian::run_trajectory()
{
	size_t nstates = CAP_MAT.rows();
	for(size_t i=0;i<eta_list.size();i++)
	{
		std::vector<root> roots;
		Eigen::MatrixXcd CAPH(nstates,nstates);
		CAPH.real() = ZERO_ORDER_H;
		CAPH.imag() = eta_list[i]*CAP_MAT;
		Eigen::ComplexEigenSolver<Eigen::MatrixXcd> ces;
		ces.compute(CAPH);
		for(size_t j=0;j<nstates;j++)
		{
			std::complex<double> energy =  ces.eigenvalues()[j];
			Eigen::VectorXcd vec = ces.eigenvectors().col(j);
			root r1(eta_list[i],energy,vec);
			roots.push_back(r1);
		}
		all_roots.push_back(roots);
	}
	track_states();
	if(do_output)
		save_trajectory();
}

EigenvalueTrajectory::EigenvalueTrajectory(std::vector<root> initial_states, size_t state_idx)
{
	prev = initial_states[state_idx];
	states.push_back(prev);
	uncorrected_energies.push_back(prev.energy);
	corrected_energies.push_back(prev.energy);
}

void EigenvalueTrajectory::add_state(std::vector<root> new_states)
{
	double max_overlap=0.0;
	root best=new_states[0];
	for(auto cur: new_states)
	{
		double overlap = abs(prev.eigv.dot(cur.eigv));
		if(overlap>max_overlap)
		{
			best = cur;
			max_overlap = overlap;
		}
	}
	states.push_back(best);
	prev = best;
	uncorrected_energies.push_back(best.energy);
}

void EigenvalueTrajectory::analyze()
{
	// get corrections, find minimum of logarithmic velocity of uncorrected trajectory

	size_t num_points = uncorrected_energies.size();
	std::vector<double> grad;
	std::vector<double> corr_grad;
	double step = states[1].eta;

	grad.push_back(0.0);
	corrected_energies.push_back(uncorrected_energies[0]);
	for (int i=1;i<uncorrected_energies.size()-1;i++)
	{
		std::complex<double> log_velo = states[i].eta*(uncorrected_energies[i+1]-uncorrected_energies[i-1])/(2.0*step);
	    grad.push_back(std::abs(log_velo));
	    corrected_energies.push_back(uncorrected_energies[i]-log_velo);
	}
	// last point
	std::complex<double> last_deriv = states[num_points-1].eta*(uncorrected_energies[num_points-1]-uncorrected_energies[num_points-2])/step;
	grad.push_back(std::abs(last_deriv));
	corrected_energies.push_back(uncorrected_energies[num_points-1]-last_deriv);

	corr_grad.push_back(0.0);
	for (int i=1;i<corrected_energies.size();i++)
	    corr_grad.push_back(states[i].eta*std::abs((corrected_energies[i+1]-corrected_energies[i-1])/(2.0*step)));

	double min_deriv=1000;
	size_t min_idx = 0;
	double min_deriv_corr=1000;
	size_t min_idx_corr;
	for(size_t i=10;i<grad.size()-1;i++)
	{
		if(grad[i]<min_deriv)
		{
			min_deriv = grad[i];
			min_idx = i;
		}
		if(corr_grad[i]<min_deriv_corr)
		{
			min_deriv_corr=corr_grad[i];
			min_idx_corr = i;
		}
	}

	//uncorrected results
	std::cout << "Results from uncorrected trajectory:" << std::endl;
	std::cout << "Uncorrected energy: " << std::noshowpos << uncorrected_energies[min_idx] << std::endl;
	std::cout << "Eta: " << states[min_idx].eta << " Logarithmic velocity: " << min_deriv << std::endl;
	uc_opt_eta = states[min_idx].eta;
	uc_opt = uncorrected_energies[min_idx];

	// corrected results
	std::cout << "Results from corrected trajectory:" << std::endl;
	std::cout << "Corrected energy: " << std::noshowpos << corrected_energies[min_idx_corr] << std::endl;
	std::cout << "Eta: " << states[min_idx_corr].eta << " Logarithmic velocity: " << min_deriv_corr << std::endl;
	corr_opt_eta = states[min_idx].eta;
	corr_opt = uncorrected_energies[min_idx];
}


/* Include directives */
#include "ffpopsim_highd.h"
#include "multi_population.h"
#include <cstring>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <cmath>
#include <boost/foreach.hpp>
#include <iostream>
#include <fstream>
#include <boost/format.hpp>

//#include <boost/filesystem.hpp>
#define HIGHD_BADARG -1354341
#define NOTHING 1e-10

/* Be verbose? */
#define HIGHD_VERBOSE 0

//#define MUTATION_RATE 1e-5
//#define MIGRATION_RATE 0
#define NUMBER_OF_TAITS 2
#define _L 100
#define GENERATIONS_Eq  150000
#define SHIFT_BEGIN 20000
#define SHIFT_END 90000
//#define SELECTIVE 1
//#define SHIFT_RATE 5000
#define GENERATIONS  10000
#define Av_Num 0
//#define SAMPLE_SIZE 1500

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

    //
    // ############################################
    ////
    //// Read the input parameters of the simulation
    ////
    //// ############################################
    //int SELECTIVE = atof(argv[1]); //MIGRATION_RATE = atof(argv[1]);

    int SEQ_LEN = atoi(argv[1]);
    int POPULATION_SIZE = atoi(argv[2]);
    int SAMPLE_NUM = atoi(argv[3]);
    int SAMPLE_FREQ = atoi(argv[4]);
    int SAMPLE_VOL = atoi(argv[5]);
    double MU = atof(argv[6]);
    std::string base_name = argv[7];

    std::cout << "SEQ_LEN = "  <<  SEQ_LEN   << std::endl;
    std::cout << "POPULATION_SIZE = " << POPULATION_SIZE << std::endl;
    std::cout << "SAMPLE_NUM = " << SAMPLE_NUM << std::endl;
    std::cout << "SAMPLE_FREQ = " << SAMPLE_FREQ << std::endl;
    std::cout << "SAMPLE_VOL = " << SAMPLE_VOL << std::endl;
    std::cout << "MU = " << MU << std::endl;

    //const char * fileDir =  argv[3];
    //int locations = atoi(argv[4]);
    //double MUTATION_RATE = atof(argv[5]);
    //double MIGRATION_RATE = atof(argv[6]);
    //// int _L = atoi(argv[5]);

    //int L = SEQ_LEN;
    //int N = POPULATION_SIZE;
    //int Ns = SAMPLE_NUM;
    //int Ts = SAMPLE_FREQ;

    //boost::format fmter("FFpopSim_L%1%_N%2%_Ns%3%_Ts%4%_Nv%5%_Mu%6%");

    haploid_highd pop(SEQ_LEN);
    pop.set_mutation_rate(MU);

    // genealogy
    vector <int> gen_loci;
    gen_loci.push_back(0);
    pop.track_locus_genealogy(gen_loci);

    pop.set_wildtype(POPULATION_SIZE);		// start with a population of the right size

    std::cout << "EQUILIBRATING POPULATION..." << std::endl;
    pop.set_mutation_rate(1e-2);
    pop.evolve(20*POPULATION_SIZE);
    // every site mutates ~5 times -> equilibrium
    pop.set_mutation_rate(MU);
    pop.evolve(30*POPULATION_SIZE);

    std::cout << "GENERATING TIME-RESOLVED SAMPLING..." << std::endl;
    for (int n=0; n<SAMPLE_NUM; n++){
        std::cout << n << " out of " << SAMPLE_NUM << std::endl;
        pop.evolve(SAMPLE_FREQ);
        pop.genealogy.trees[0].check_tree_integrity();
        pop.tree_sample=SAMPLE_VOL;
        pop.evolve(1); // take sample
        pop.genealogy.trees[0].check_tree_integrity();
        pop.tree_sample=0;
    }

    auto ts = pop.genealogy.trees;
    auto tree = ts.at(0);
    rooted_tree new_tree;
    tree.construct_subtree(tree.sampled_leafs, new_tree);

    /// save results
    //fmter % SEQ_LEN % POPULATION_SIZE % SAMPLE_NUM % SAMPLE_FREQ % SAMPLE_VOL % MU;
    //auto base_name = fmter.str();

    std::ofstream ofs;
    ofs.open(base_name + ".nwk");
    ofs << new_tree.print_newick() << std::endl;
    ofs.close();

    ofs.open(base_name + ".bin.fasta");
    ofs << tree.print_sequences() << std::endl;
    ofs.close();

    return 0;

}


/*=========================================================================

Library:   TubeTK

Copyright 2010 Kitware Inc. 28 Corporate Drive,
Clifton Park, NY, 12065, USA.

All rights reserved.

Licensed under the Apache License, Version 2.0 ( the "License" );
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    https://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

=========================================================================*/

#include "tubeShortestPathKernel.h"
#include "tubeWLSubtreeKernel.h"

#include <boost/filesystem.hpp>

#include <itkMatrix.h>

#include "ComputeTubeGraphSimilarityKernelMatrixCLP.h"

enum { GK_SPKernel = 0, GK_WLKernel = 1 };

/** Read-in a list of graphs from a list.
 *  Reads a list of input graphs from JSON file 'fileName' and stores the full
 *  path's the the graph files in 'list', as well as the label ( i.e., class
 *  assignment ) information in 'labels'.
 *
 *  \param fileName Filename of the list file.
 *  \param basePath Absolute path to the directory where graph files reside.
 *  \param list This vector will be filled with filenames of the graph files.
 *  \param labels This vector will be filled with one label for each graph.
 */
void readGraphList( const std::string &fileName,
                   const std::string &basePath,
                   std::vector<std::string> &list,
                   std::vector<int> &labels )
{
  list.clear();
  try
    {
    boost::property_tree::ptree pt;
    read_json( fileName, pt );

    const int nGraphs = boost::lexical_cast<int>(
      pt.get<std::string>( "nGraphs" ) );

    int graphCount = 0;
    BOOST_FOREACH( boost::property_tree::ptree::value_type &v,
                   pt.get_child( "graphList" ) )
      {
      std::string fullGraphFileName;
      if( basePath.empty() )
        {
        // Assume filename is absolute
        fullGraphFileName = boost::lexical_cast<std::string>( v.second.data() );
        }
      else
        {
        // Build absolute filename from basePath and fileName
        boost::filesystem::path base( basePath.c_str() );
        boost::filesystem::path file(
          boost::lexical_cast<std::string>( v.second.data() ) );
        boost::filesystem::path fullPathToGraphFile = base / file;
        fullGraphFileName = fullPathToGraphFile.string();
        }

      // Check existence
      if( !boost::filesystem::exists( fullGraphFileName.c_str() ) )
        {
        tube::FmtErrorMessage( "Graph file %s not found!",
          fullGraphFileName.c_str() );
        throw std::exception();
        }

      list.push_back( fullGraphFileName );
      ++graphCount;
      }

    int labelCount = 0;
    BOOST_FOREACH( boost::property_tree::ptree::value_type &v,
      pt.get_child( "labels" ) )
      {
      int label = boost::lexical_cast<int>( v.second.data() );
      labels.push_back( label );
      ++labelCount;
      }

    if( graphCount != nGraphs || labelCount != nGraphs )
      {
      throw std::exception();
      }
    }
  catch( std::exception &e )
    {
    tube::FmtErrorMessage( "Error reading JSON graph file %s!",
      fileName.c_str() );
    throw std::exception();
    }
}

/** Writes VNL matrix to binary file.
 *
 *  \param baseFileName The base name of the binary file ( .bin is appended ).
 *  \param K The VNL kernel matrix.
 */
void writeKernel( const std::string &baseFileName, const vnl_matrix<double> &K )
{
  std::string outFileName = baseFileName + ".bin";
  std::ofstream ofs;

  ofs.open( outFileName.c_str(), std::ios::binary | std::ios::out );
  if( !ofs )
    {
    tube::FmtErrorMessage( "Could not open kernel matrix %s for writing!",
      outFileName.c_str() );
    throw std::exception();
    }
  ofs.write( ( const char * )K.data_block(), K.size()*sizeof( double ) );
  if( ofs.bad() )
    {
    tube::FmtErrorMessage( "Could not write kernel matrix %s!",
      outFileName.c_str() );
    throw std::exception();
    }
  ofs.close();
  if( ofs.fail() )
    {
    tube::FmtErrorMessage( "Could not close kernel matrix file %s!",
      outFileName.c_str() );
    throw std::exception();
    }
}

/** Writes VNL matrix to plain-text file.
 *  Writes VNL matrix in row-by-row manner to a plain-text file (
 *  One value on each line ).
 *
 *  \param baseFileName name of the plain-text file ( .txt is appended ).
 *  \param K The VNL kernel matrix.
 */
void writeKernelPlainText( const std::string &baseFileName,
                           const vnl_matrix<double> &K )
{
  std::string outFileName = baseFileName + ".txt";
  std::ofstream ofs;

  ofs.open( outFileName.c_str(), std::ios::binary | std::ios::out );
  if( !ofs )
    {
    tube::FmtErrorMessage( "Could not open kernel matrix %s!",
      outFileName.c_str() );
    throw std::exception();
    }
  for( unsigned int r = 0; r < K.rows(); ++r )
    {
    for( unsigned int c = 0; c < K.columns(); ++c )
      {
      ofs << K[r][c] << std::endl;
      }
    }
  ofs.close();
  if( ofs.fail() )
    {
    tube::FmtErrorMessage( "Could not close kernel matrix file %s!",
      outFileName.c_str() );
    throw std::exception();
    }
}

/** Writes VNL matrix to LIBSVM compatible file.
 *
 *  \param baseFileName The basename of LIBSVM file ( .libsvm appended ).
 *  \param K The VNL kernel matrix.
 */
void writeKernelLIBSVM( const std::string &baseFileName,
                       const vnl_matrix<double> &K,
                       const std::vector<int> &labels )
{
  std::string outFileName = baseFileName + ".libsvm";
  std::ofstream ofs;
  ofs.open( outFileName.c_str(), std::ios::binary | std::ios::out );
  if( !ofs )
    {
    tube::FmtErrorMessage( "Could not open kernel matrix %s for writing!",
      outFileName.c_str() );
    throw std::exception();
    }

  assert( K.rows() == labels.size() );

  for( unsigned int r = 0; r < K.rows(); ++r )
    {
    ofs << labels[r] << " " << "0:" << r+1 << " ";
    for( unsigned int c = 0; c < K.cols()-1; ++c )
      {
      ofs << c+1 << ":" << K[r][c] << " ";
      }
    ofs << K.cols() << ":" << K[r][K.cols()-1] << std::endl;
    }
  ofs.close();
  if( ofs.fail() )
    {
    tube::FmtErrorMessage( "Could not close kernel matrix file %s!",
      outFileName.c_str() );
    throw std::exception();
    }
}

/** Build graph from adjacency matrix ( and label information ).
 *
 *  Load a graph file ( as adjacency matrix ) from disk. In case a global label
 *  file is provided, we use that for labeling the nodes. Otherwise, check if
 *  a graph-specific label file ( suffix .vertexLabel ) exists and if so, load
 *  it.
 *
 *  \param graphFile The adjacency matrix file to load.
 *  \param defNodeLabel The type of default node labeling to use.
 *  \param globalLabelFile Filename of a global label file to use.
 *  \return The constructed graph.
 */
tube::GraphKernel::GraphType loadGraph( std::string graphFile,
  tube::GraphKernel::DefaultNodeLabelingType defNodeLabel =
    tube::GraphKernel::LABEL_BY_NUM,
  const std::string & globalLabelFile = std::string() )
{
  // Will stay 0 as long as there is NO per-graph label file
  const char * labelFile = 0;

  // Used to build the graph-specific label file name
  std::string labelFileStr;

  // Global label file given
  if( !globalLabelFile.empty() )
    {
    labelFile = globalLabelFile.c_str();
    tube::FmtInfoMessage( "Trying to use global label file %s",
      labelFile );
    }
  // Build graph-specific label file name
  else
    {
    labelFileStr = graphFile + ".vertexLabel";
    labelFile = labelFileStr.c_str();
    tube::FmtInfoMessage( "Trying to use graph-specific label file %s",
      labelFile );
    }
  // In case it does not exist, reset to 0
  if( !boost::filesystem::exists( labelFile ) )
    {
    tube::FmtInfoMessage( "Label file %s not existent - fallback to default!",
      labelFile );
    labelFile = 0;
    }
  // Load graph and return
  return tube::GraphKernel::GraphFromAdjFile(
    graphFile.c_str(),
    labelFile,
    defNodeLabel );
}

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

  try
    {
    switch( argGraphKernelType )
      {
      case GK_WLKernel:
      case GK_SPKernel:
        break;
      default:
        tube::ErrorMessage( "Unsupported kernel!" );
        return EXIT_FAILURE;
      }

    /*
     * Read graph lists 'listA' and 'listB' and fill class labels;
     * NOTE: The labels are currently unused, since classification
     * is done in the driver script.
     */

    std::vector<std::string> listA, listB;
    std::vector<int> labelsB, labelsA;
    readGraphList( argGraphListA, argBasePath, listA, labelsA );
    readGraphList( argGraphListB, argBasePath, listB, labelsB );

    int N = listA.size();
    int M = listB.size();

    assert( N > 0 && M > 0 );

    tube::FmtDebugMessage( "Read N=%d entries from %s.", N,
      argGraphListA.c_str() );
    tube::FmtDebugMessage( "Read M=%d entries from %s.", M,
      argGraphListB.c_str() );

    vnl_matrix<double> K( N, M );
    K.fill( 0.0 );

    /*
     * In case we use the Weisfeiler-Lehman kernel, we need to build
     * the label compression mapping beforehand. This means, we need
     * to load the graphs, compress labels and store the mappings.
     *
     * NOTE: We only need to do this for the first set of graphs, which
     * in case of training data, will determine the compression mapping.
     * In case of testing, the first list is still the list of training
     * graphs and the second list will use that mapping. TODO: Make the
     * list loadable from file/
     */

    if( !tube::GraphKernel::IsValidDefaultNodeLabeling(
      argDefaultLabelType ) )
      {
      tube::ErrorMessage( "Labeling strategy not supported!" );
      return EXIT_FAILURE;
      }
    tube::GraphKernel::DefaultNodeLabelingType defLabelType =
      static_cast<tube::GraphKernel::DefaultNodeLabelingType>(
        argDefaultLabelType );

    tube::WLSubtreeKernel::LabelMapVectorType labelMap( argSubtreeHeight );
    int labelCount = 0;

    if( argGraphKernelType == GK_WLKernel )
      {
      for( int i = 0; i < N; ++i )
        {
        tube::FmtInfoMessage( "Adding data from graph %s",
          listA[i].c_str() );

        tube::GraphKernel::GraphType f = loadGraph( listA[i],
                                                    defLabelType,
                                                    argGlobalLabelFileName );
        tube::WLSubtreeKernel::UpdateLabelCompression( f,
                                                       labelMap,
                                                       labelCount,
                                                       argSubtreeHeight );
        }
      }

    /*
     * Next, we build the kernel matrix K, where the K_ij-th entry
     * is the WL kernel value between the i-th graph of the first
     * ( i.e., 'listA' ) list and the j-th graph of the second list
     * ( i.e., 'listB' ).
     */

    for( int i = 0; i < N; ++i )
      {
      tube::GraphKernel::GraphType f = loadGraph( listA[i],
        defLabelType, argGlobalLabelFileName );
      for( int j = 0; j < M; ++j )
        {
        tube::GraphKernel::GraphType g = loadGraph( listB[j],
          defLabelType, argGlobalLabelFileName );

        tube::FmtInfoMessage( "Running kernel on graphs ( %d,%d )",
          i, j );

        tube::GraphKernel *gk = 0;
        switch( argGraphKernelType )
          {
          case GK_SPKernel:
            {
            gk = new tube::ShortestPathKernel( f, g );
            K[i][j] = gk->Compute();
            delete gk;
            break;
            }
          case GK_WLKernel:
            {
            gk = new tube::WLSubtreeKernel( f,
                                            g,
                                            labelMap,
                                            labelCount,
                                            argSubtreeHeight );

            K[i][j] = gk->Compute();
            delete gk;
            break;
            }
          }
        }
      }

    /*
     * Eventually, dump the kernel to disk - 1 ) in LIBSVM comp.
     * format ( directly usable by svm-train ), 2 ) as a binary
     * kernel matrix that can be loaded in Python for instance
     * ( e.g., for scikits-learn SVM ) and 3 ) as plain text.
     */

    writeKernel( argOutputKernel, K );
    writeKernelLIBSVM( argOutputKernel, K, labelsA );
    writeKernelPlainText( argOutputKernel, K );
    }
  catch( std::exception &e )
    {
    tube::ErrorMessage( "Exiting ..." );
    return EXIT_FAILURE;
    }
  return EXIT_SUCCESS;
}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_cmaes

#include <boost/test/unit_test.hpp>

#include <limbo/limbo.hpp>
#include <limbo/opt/cmaes.hpp>

using namespace limbo;

struct Params {
    struct opt_cmaes : public limbo::defaults::opt_cmaes {
    };
};

opt::eval_t fsphere(const Eigen::VectorXd& params, bool g)
{
    return opt::no_grad(-params(0) * params(0) - params(1) * params(1));
}

BOOST_AUTO_TEST_CASE(test_cmaes_unbounded)
{
    size_t N = 100;
    size_t errors = 0;
    for (size_t i = 0; i < N; i++) {
        Eigen::VectorXd g = limbo::opt::Cmaes<Params>()(fsphere, Eigen::VectorXd::Zero(2), false);

        if (std::abs(g(0)) > 0.00000001 || std::abs(g(1)) > 0.00000001)
            errors++;
    }

    BOOST_CHECK((double(errors) / double(N)) <= 0.3);
}

BOOST_AUTO_TEST_CASE(test_cmaes_bounded)
{
    size_t N = 100;
    size_t errors = 0;
    for (size_t i = 0; i < N; i++) {
        Eigen::VectorXd g = limbo::opt::Cmaes<Params>()(fsphere, Eigen::VectorXd::Zero(2), true);

        if (std::abs(g(0)) > 0.00000001 || std::abs(g(1)) > 0.00000001)
            errors++;
    }

    BOOST_CHECK((double(errors) / double(N)) <= 0.3);
}


#include "nodehyperplane.h"
#include "nodegini.h"
#include "nodeinfogain.h"
#include "utilities.h"
#include <boost/foreach.hpp>
#include <algorithm>
#include <boost/lexical_cast.hpp>

#ifdef WIN32
inline double round(double x)
{
	return (x-floor(x))>0.5 ? ceil(x) : floor(x);
}
#endif
using namespace std;

NodeHyperPlane::NodeHyperPlane(const HyperParameters &hp, int depth) : Node(hp, depth), m_bestThreshold( 0.0 )
{
}

NodeHyperPlane::NodeHyperPlane(const HyperParameters &hp, int depth, int reset) : Node(hp, depth, reset), m_bestThreshold( 0.0 )
{
}

NodeHyperPlane::NodeHyperPlane(const HyperParameters &hp, int reset, const xmlNodePtr nodeNode) : Node(hp,0,reset)
{
	m_isLeaf = (readStringProp(nodeNode,"isLeaf") == "true") ? true : false;

	if (m_isLeaf)
	{
		m_nodeLabel = readIntProp( nodeNode, "label", 0 );
		xmlNodePtr cur = nodeNode->xmlChildrenNode;
		while ( cur != 0 )
		{
			if ( xmlStrcmp( cur->name, reinterpret_cast<const xmlChar*>( "confidence" ) ) == 0 )
			{
				m_nodeConf.push_back( static_cast<float>(readDoubleProp( cur, "conf", 0 )) );
			}
			cur = cur->next;
		}
	}
	else
	{
		xmlNodePtr cur = nodeNode->xmlChildrenNode;
		while ( cur != 0 )
		{
			if ( xmlStrcmp( cur->name, reinterpret_cast<const xmlChar*>( "feature" ) ) == 0 )
			{
                for (int i=0;i<m_hp.numRandomFeatures;++i){
                    string s = "feat"+boost::lexical_cast<string>( i );
                    int tmp = readIntProp( cur, s.c_str(), 0 );
                    m_bestFeatures[i]=tmp;
                    string s2 = "weight"+boost::lexical_cast<string>( i );
                    double tmpW=(float)readDoubleProp(cur,s2.c_str(), 0);
                    m_bestWeights[i]=tmpW;
                }

				m_bestThreshold = (float)readDoubleProp( cur, "threshold", 0 );
			}
			else if ( xmlStrcmp( cur->name, reinterpret_cast<const xmlChar*>( "node" ) ) == 0 )
			{
				const std::string childNode = readStringProp(cur,"child");
				if ( childNode == "left" )
				{
					const std::string type = readStringProp(cur,"type");
					if (type == NODE_GINI)
					{
						m_leftChildNode = NodeGini::Ptr(new NodeGini(m_hp,-1,cur));
					}
					else if (type == NODE_INFO_GAIN)
					{
						m_leftChildNode = NodeInfoGain::Ptr(new NodeInfoGain(m_hp,-1, cur));
					}
				}
				else
				{
					const std::string type = readStringProp(nodeNode,"type");
					if (type == NODE_GINI)
					{
						m_rightChildNode = NodeGini::Ptr(new NodeGini(m_hp,-1,cur));
					}
					else if (type == NODE_INFO_GAIN)
					{
						m_rightChildNode = NodeInfoGain::Ptr(new NodeInfoGain(m_hp,-1,cur));
					}
				}
			}
			cur = cur->next;
		}
	}
}

xmlNodePtr NodeHyperPlane::saveFeature() const
{
	xmlNodePtr node = xmlNewNode( NULL, reinterpret_cast<const xmlChar*>( "feature" ) );
    //std::cout<<m_hp.numRandomFeatures<<" "<< m_bestFeatures.size()<<std::endl;
    for (int i=0;i<m_hp. numRandomFeatures;++i){
        string s = "feat"+boost::lexical_cast<string>( i );
        addIntProp(node, s.c_str(), m_bestFeatures[i]);
        string s2 = "weight"+boost::lexical_cast<string>( i );
        addDoubleProp(node, s2.c_str(), m_bestWeights[i]);
    }
	addDoubleProp(node, "threshold", m_bestThreshold);

	return node;
}

xmlNodePtr NodeHyperPlane::save() const
{
	xmlNodePtr node = xmlNewNode( NULL, reinterpret_cast<const xmlChar*>( "node" ) );
	xmlNewProp( node, reinterpret_cast<const xmlChar*>( "type" ),
			reinterpret_cast<const xmlChar*>( NODE_GINI ) );
	const char* isLeaf = (m_isLeaf) ? "true" : "false";
	xmlNewProp( node, reinterpret_cast<const xmlChar*>( "isLeaf" ),
			reinterpret_cast<const xmlChar*>( isLeaf ) );
	if (!m_isLeaf)
	{
		xmlAddChild(node, saveFeature());
		xmlNodePtr leftChildNode = m_leftChildNode->save();
		xmlNewProp( leftChildNode, reinterpret_cast<const xmlChar*>( "child" ),
				reinterpret_cast<const xmlChar*>( LEFT_CHILD_NODE ) );
		xmlAddChild( node, leftChildNode );

		xmlNodePtr rightChildNode = m_rightChildNode->save();
		xmlNewProp( rightChildNode, reinterpret_cast<const xmlChar*>( "child" ),
				reinterpret_cast<const xmlChar*>( RIGHT_CHILD_NODE ) );
		xmlAddChild( node, rightChildNode );
	}
	else
	{
		addIntProp( node, "label", m_nodeLabel);
		std::vector<float>::const_iterator it(m_nodeConf.begin()),end(m_nodeConf.end());
		int idx = 0;
		for (;it != end;it++,idx++)
		{
			xmlAddChild(node,saveConfidence(idx,*it));
		}
	}

	return node;
}

std::pair<float, float> NodeHyperPlane::calcGiniAndThreshold(const std::vector<int>& labels,
		const std::vector<std::pair<float, int> >& responses)
{
	// Initialize the counters: left takes all at the begining
	double DGini, LGini, RGini, LTotal, RTotal, bestW0 = 0, bestDGini = 1e10;
	std::vector<double> LCount(m_hp.numClasses, 0.0), RCount(m_hp.numClasses, 0.0);

	if (m_hp.isExtreme)
	{
		std::vector<std::pair<float, int> >::const_iterator resIt(responses.begin()), resEnd(responses.end());
		bestW0 = 2.0 * randomDouble( 1.0 ) - 1.0;
		RTotal = 0;
		LTotal = 0;
		for (; resIt != resEnd; resIt++)
		{
			if (resIt->first > bestW0)
			{
				RTotal++;
				RCount[labels[resIt->second]]++;
			}
			else
			{
				LTotal++;
				LCount[labels[resIt->second]]++;
			}
		}

		LGini = 0;
		RGini = 0;
		std::vector<double>::iterator LIt = LCount.begin(), RIt = RCount.begin(), end = LCount.end(), REnd = RCount.end();
		for (; LIt != end; LIt++, RIt++)      // Calculate Gini index
				{
			if (LTotal)
			{
				LGini += (*LIt/LTotal)*(1 - *LIt/LTotal);
			}
			if (RTotal)
			{
				RGini += (*RIt/RTotal)*(1 - *RIt/RTotal);
			}
				}

		bestDGini = (LTotal*LGini + RTotal*RGini)/responses.size();
	}
	else
	{
		RTotal = responses.size();
		LTotal = 0;

		// Count the number of samples in each class
		std::vector<std::pair<float, int> >::const_iterator resIt(responses.begin()), resEnd(responses.end()), tmpResIt;
		for (; resIt != resEnd; resIt++)
		{
			RCount[labels[resIt->second]]++;
		}

		// Loop over the sorted values and find the min DGini
		std::vector<double>::iterator LIt = LCount.begin(), RIt = RCount.begin(), end = LCount.end(), REnd = RCount.end();
		resIt = responses.begin();
		++resIt;
		for (; resIt != resEnd; resIt++)
		{
			tmpResIt = resIt;
			--tmpResIt;

			RTotal--;
			LTotal++;
			RCount[labels[tmpResIt->second]]--;
			LCount[labels[tmpResIt->second]]++;

			if (resIt->first != tmpResIt->first)
			{
				LGini = 0;
				RGini = 0;
				LIt = LCount.begin();
				RIt = RCount.begin();
				for (; LIt != end; LIt++, RIt++)      // Calculate Gini index
						{
					LGini += (*LIt/LTotal)*(1 - *LIt/LTotal);
					RGini += (*RIt/RTotal)*(1 - *RIt/RTotal);
						}

				DGini = (LTotal*LGini + RTotal*RGini)/responses.size();
				if (DGini < bestDGini)
				{
					bestDGini = DGini;
					bestW0 = (resIt->first + tmpResIt->first)*0.5;
				}
			}
		}
	}

	return std::pair<float,float>((float)bestDGini,(float)bestW0);
}

std::pair<float, float> NodeHyperPlane::calcInfoGainAndThreshold(const std::vector<int>& labels,
		const std::vector<std::pair<float, int> >& responses)
{
	// Initialize the counters: left takes all at the begining
	double DInfo, LInfo, RInfo, LTotal, RTotal, bestW0 = 0.0, bestDInfo = 1e10;
	std::vector<double> LCount(m_hp.numClasses, 0.0), RCount(m_hp.numClasses, 0.0);

	RTotal = responses.size();
	LTotal = 0.0;
	// Count the number of samples in each class
	std::vector<std::pair<float, int> >::const_iterator resIt(responses.begin()), resEnd(responses.end()), tmpResIt;
	for (; resIt != resEnd; resIt++)
	{
		RCount[labels[resIt->second]]++;
	}

	// Loop over the sorted values and find the max DInfo
	std::vector<double>::iterator LIt = LCount.begin(), RIt = RCount.begin(), end = LCount.end(), REnd = RCount.end();
	resIt = responses.begin();
	++resIt;
	for (; resIt != resEnd; resIt++)
	{
		tmpResIt = resIt;
		--tmpResIt;

		RTotal--;
		LTotal++;
		RCount[labels[tmpResIt->second]]--;
		LCount[labels[tmpResIt->second]]++;

		if (resIt->first != tmpResIt->first)
		{
			LInfo = 0.0;
			RInfo = 0.0;
			LIt = LCount.begin();
			RIt = RCount.begin();
			for (; LIt != end; LIt++, RIt++)      // Calculate Info index
					{
				if (*LIt)
				{
					LInfo -= (*LIt/LTotal)*log(*LIt/LTotal);
				}
				if (*RIt)
				{
					RInfo -= (*RIt/RTotal)*log(*RIt/RTotal);
				}
					}

			DInfo = (LTotal*LInfo + RTotal*RInfo)/responses.size();
			if (DInfo < bestDInfo)
			{
				bestDInfo = DInfo;
				bestW0 = (resIt->first + tmpResIt->first)*0.5;
			}
		}
	}

	return std::pair<float,float>((float)bestDInfo,(float)bestW0);
}

std::pair<float, float> NodeHyperPlane::calcInfoGainAndThreshold(const std::vector<int>& labels, const std::vector<double>& weights,
		const std::vector<std::pair<float, int> >& responses)
{
	// Initialize the counters: left takes all at the begining
	double DInfo, LInfo, RInfo, LTotal, RTotal, bestW0 = 0.0, bestDInfo = 1e10;
	std::vector<double> LCount(m_hp.numClasses, 0.0), RCount(m_hp.numClasses, 0.0);

	RTotal = 0.0;
	LTotal = 0.0;
	// Count the number of samples in each class
	std::vector<std::pair<float, int> >::const_iterator resIt(responses.begin()), resEnd(responses.end()), tmpResIt;
	for (; resIt != resEnd; resIt++)
	{
		RCount[labels[resIt->second]] += weights[resIt->second];
		RTotal += weights[resIt->second];
	}

	// Loop over the sorted values and find the max DInfo
	std::vector<double>::iterator LIt = LCount.begin(), RIt = RCount.begin(), end = LCount.end(), REnd = RCount.end();
	resIt = responses.begin();
	++resIt;
	for (; resIt != resEnd; resIt++)
	{
		tmpResIt = resIt;
		--tmpResIt;

		RTotal -= weights[tmpResIt->second];
		LTotal += weights[tmpResIt->second];
		RCount[labels[tmpResIt->second]] -= weights[tmpResIt->second];
		LCount[labels[tmpResIt->second]] += weights[tmpResIt->second];

		if (resIt->first != tmpResIt->first)
		{
			LInfo = 0.0;
			RInfo = 0.0;
			LIt = LCount.begin();
			RIt = RCount.begin();
			for (; LIt != end; LIt++, RIt++)      // Calculate Info index
					{
				if (*LIt)
				{
					LInfo -= (*LIt/LTotal)*log(*LIt/LTotal);
				}
				if (*RIt)
				{
					RInfo -= (*RIt/RTotal)*log(*RIt/RTotal);
				}
					}

			DInfo = (LTotal*LInfo + RTotal*RInfo)/(LTotal + RTotal);
			if (DInfo < bestDInfo)
			{
				bestDInfo = DInfo;
				bestW0 = (resIt->first + tmpResIt->first)*0.5;
			}
		}
	}

	return std::pair<float,float>((float)bestDInfo,(float)bestW0);
}

std::pair<float, float> NodeHyperPlane::calcGiniAndThreshold(const std::vector<int>& labels, const std::vector<double>& weights,
		const std::vector<std::pair<float, int> >& responses, const bool useUnlabeledData)
{
	// Initialize the counters: left takes all at the begining
	double DGini, LGini, RGini, LTotal, RTotal, bestW0 = 0, bestDGini = 1e10;
	std::vector<double> LCount(m_hp.numClasses, 0.0), RCount(m_hp.numClasses, 0.0);

	RTotal = 0;
	LTotal = 0;

	// Count the number of samples in each class
	std::vector<std::pair<float, int> >::const_iterator resIt(responses.begin()), resEnd(responses.end()), tmpResIt;
	for (; resIt != resEnd; resIt++)
	{
		if (useUnlabeledData || resIt->second < m_hp.numLabeled)
		{
			RCount[labels[resIt->second]] += weights[resIt->second];
			RTotal += weights[resIt->second];
		}
	}

	// Loop over the sorted values and find the min DGini
	std::vector<double>::iterator LIt = LCount.begin(), RIt = RCount.begin(), end = LCount.end(), REnd = RCount.end();
	resIt = responses.begin();
	++resIt;
	for (; resIt != resEnd; resIt++)
	{
		if (useUnlabeledData || resIt->second < m_hp.numLabeled)
		{
			tmpResIt = resIt;
			--tmpResIt;

			RTotal -= weights[tmpResIt->second];
			LTotal += weights[tmpResIt->second];
			RCount[labels[tmpResIt->second]] -= weights[tmpResIt->second];
			LCount[labels[tmpResIt->second]] += weights[tmpResIt->second];

			if (resIt->first != tmpResIt->first)
			{
				LGini = 0;
				RGini = 0;
				LIt = LCount.begin();
				RIt = RCount.begin();
				for (; LIt != end; LIt++, RIt++)      // Calculate Gini index
						{
					LGini += (*LIt/LTotal)*(1 - *LIt/LTotal);
					RGini += (*RIt/RTotal)*(1 - *RIt/RTotal);
						}

				DGini = (LTotal*LGini + RTotal*RGini)/(LTotal + RTotal);
				if (DGini < bestDGini)
				{
					bestDGini = DGini;
					bestW0 = (resIt->first + tmpResIt->first)*0.5;
				}
			}
		}
	}

	return std::pair<float,float>((float)bestDGini,(float)bestW0);
}

void NodeHyperPlane::findHypotheses(const matrix<float>& data, const std::vector<int>& labels,
		const std::vector<int>& inBagSamples, const std::vector<int>& randFeatures, int numTries)
{
	std::vector<double> gini(m_hp.numRandomFeatures), thresholds(m_hp.numRandomFeatures);
	std::vector<int>::const_iterator it(randFeatures.begin());
	std::vector<int>::const_iterator end(randFeatures.end());

	std::vector<int>::const_iterator bagIt;
	std::vector<int>::const_iterator bagEnd(inBagSamples.end());

	double bestDGini = 1e10, bestThreshold = 0;
	std::pair<float,float> curGiniThresh;
	std::vector<std::pair<float, int> > responses;
	std::vector<float> bestWeights(randFeatures.size(),0.0);
	std::vector<float> tmpWeights(randFeatures.size(),0.0);
	float tmp = 0.0;
	for ( int i = 0; i < numTries; i++)
	{
		fillWithRandomNumbers(tmpWeights);
		responses.clear();
		responses.reserve(inBagSamples.size());
		bagIt = inBagSamples.begin();
		while ( bagIt != bagEnd )
		{
			tmp = 0.0;
			int counter = 0;
			BOOST_FOREACH(int feat, randFeatures)
			{
				tmp += data(*bagIt,feat)*tmpWeights[counter];
				counter++;
			}

			responses.push_back(std::pair<float, int>(tmp,*bagIt));
			++bagIt;
		}

		sort(responses.begin(), responses.end());

		if (m_hp.useInfoGain)
		{
			curGiniThresh = calcInfoGainAndThreshold(labels, responses);
		}
		else
		{
			curGiniThresh = calcGiniAndThreshold(labels, responses);
		}

		if (curGiniThresh.first < bestDGini)
		{
			bestDGini = curGiniThresh.first;
			bestThreshold = curGiniThresh.second;
			bestWeights = tmpWeights;
		}
	}

	m_bestWeights = bestWeights;
	m_bestFeatures = randFeatures;
	m_bestThreshold = (float) bestThreshold;
}

void NodeHyperPlane::findHypothesesLU(const matrix<float>& data, const std::vector<int>& labels,
		const std::vector<int>& inBagSamples, const std::vector<int>& randFeatures, int numTries)
{
	std::vector<double> gini(m_hp.numRandomFeatures), thresholds(m_hp.numRandomFeatures);
	std::vector<int>::const_iterator it(randFeatures.begin());
	std::vector<int>::const_iterator end(randFeatures.end());

	std::vector<int>::const_iterator bagIt;
	std::vector<int>::const_iterator bagEnd(inBagSamples.end());

	double bestDGini = 1e10, bestThreshold = 0;
	std::pair<float,float> curGiniThresh;
	std::vector<std::pair<float, int> > responses;
	std::vector<float> bestWeights(randFeatures.size(),0.0);
	std::vector<float> tmpWeights(randFeatures.size(),0.0);
	float tmp = 0.0;
	for ( int i = 0; i < numTries; i++)
	{
		fillWithRandomNumbers(tmpWeights);
		responses.clear();
		responses.reserve(inBagSamples.size());
		bagIt = inBagSamples.begin();
		while ( bagIt != bagEnd )
		{
			tmp = 0.0;
			int counter = 0;
			BOOST_FOREACH(int feat, randFeatures)
			{
				tmp += data(*bagIt,feat)*tmpWeights[counter];
				counter++;
			}

			responses.push_back(std::pair<float, int>(tmp,*bagIt));
			++bagIt;
		}

		sort(responses.begin(), responses.end());

		if (inBagSamples[0] < m_hp.numLabeled)
		{
			curGiniThresh = calcGiniAndThreshold(labels, responses);
		}
		else
		{
			curGiniThresh = calcClusterScoreAndThreshold(data, inBagSamples, responses);
		}
		if (curGiniThresh.first < bestDGini)
		{
			bestDGini = curGiniThresh.first;
			bestThreshold = curGiniThresh.second;
			bestWeights = tmpWeights;
		}
	}

	m_bestWeights = bestWeights;
	m_bestFeatures = randFeatures;
	m_bestThreshold = (float) bestThreshold;
}

std::pair<float, float> NodeHyperPlane::calcClusterScoreAndThreshold(const matrix<float>& data, const std::vector<int>& inBagSamples,
		const std::vector<double>& weights,
		const std::vector<std::pair<float, int> >& responses)
{
	// Find the mid-point using responsess
			int numResponse = responses.size();
	int midPointIndex = (int) round(numResponse/2 - 1);
	float threshold = (responses[midPointIndex].first + responses[midPointIndex + 1].first)/2;

	// Calculate the weighted cluster center for left and right splits
	double LWeight = 0, RWeight = 0;
	std::vector<float> LCenter(data.size2(), 0.0), RCenter(data.size2(), 0.0);
	for (int m = 0; m < (int) data.size2(); m++)
	{
		for (int n = 0; n < midPointIndex; n++)
		{
			LCenter[m] += (float)weights[responses[n].second]*data(responses[n].second, m);
			if (m == 0)
			{
				LWeight += weights[responses[n].second];
			}
		}
		LCenter[m] /= (float)LWeight;

		for (int n = midPointIndex; n < (int) responses.size(); n++)
		{
			RCenter[m] += (float)weights[responses[n].second]*data(responses[n].second, m);
			if (m == 0)
			{
				RWeight += weights[responses[n].second];
			}
		}
		RCenter[m] /= (float)RWeight;
	}

	// Calculate the weighted distance from each point to the centers
	float LScore = 0, RScore = 0;
	for (int m = 0; m < (int) data.size2(); m++)
	{
		for (int n = 0; n < midPointIndex; n++)
		{
			LScore += weights[responses[n].second]*pow((double) (data(responses[n].second, m) - LCenter[m]), 2.0);
		}
		LScore /= (float)LWeight;
		for (int n = midPointIndex; n < (int) responses.size(); n++)
		{
			RScore += (float)weights[responses[n].second]*pow((double) (data(responses[n].second, m) - RCenter[m]), 2.0);
		}
		RScore /= (float)RWeight;
	}

	return std::pair<float,float>(0.5f*(LScore + RScore), threshold);
}

std::pair<float, float> NodeHyperPlane::calcClusterScoreAndThreshold(const matrix<float>& data, const std::vector<int>& inBagSamples,
		const std::vector<std::pair<float, int> >& responses)
{
	// Find the mid-point using responsess
	int numResponse = responses.size();
	int midPointIndex = (int) round(numResponse/2 - 1);
	float threshold = (responses[midPointIndex].first + responses[midPointIndex + 1].first)/2;

	// Calculate the weighted cluster center for left and right splits
	double LWeight = 0, RWeight = 0;
	std::vector<float> LCenter(data.size2(), 0.0), RCenter(data.size2(), 0.0);
	for (int m = 0; m < (int) data.size2(); m++)
	{
		for (int n = 0; n < midPointIndex; n++)
		{
			LCenter[m] += data(responses[n].second, m);
			if (m == 0)
			{
				LWeight++;
			}
		}
		LCenter[m] /= LWeight;

		for (int n = midPointIndex; n < (int) responses.size(); n++)
		{
			RCenter[m] += data(responses[n].second, m);
			if (m == 0)
			{
				RWeight++;
			}
		}
		RCenter[m] /= RWeight;
	}

	// Calculate the weighted distance from each point to the centers
	float LScore = 0, RScore = 0;
	for (int m = 0; m < (int) data.size2(); m++)
	{
		for (int n = 0; n < midPointIndex; n++)
		{
			LScore += pow((double) (data(responses[n].second, m) - LCenter[m]), 2.0);
		}
		LScore /= LWeight;
		for (int n = midPointIndex; n < (int) responses.size(); n++)
		{
			RScore += pow((double) (data(responses[n].second, m) - RCenter[m]), 2.0);
		}
		RScore /= RWeight;
	}

	return std::pair<float,float>(0.5*(LScore + RScore), threshold);
}

void NodeHyperPlane::findHypotheses(const matrix<float>& data, const std::vector<int>& labels,
		const std::vector<double>& weights,
		const std::vector<int>& inBagSamples, const std::vector<int>& randFeatures, int numTries)
{
	std::vector<double> gini(m_hp.numRandomFeatures), thresholds(m_hp.numRandomFeatures);
	std::vector<int>::const_iterator it(randFeatures.begin());
	std::vector<int>::const_iterator end(randFeatures.end());
	std::vector<int>::const_iterator bagIt;
	std::vector<int>::const_iterator bagEnd(inBagSamples.end());

	double bestDGini = 1e10, bestThreshold = 0.0;
	std::pair<float,float> curGiniThresh;
	std::vector<std::pair<float, int> > responses;

	std::vector<float> bestWeights(randFeatures.size(),0.0);
	std::vector<float> tmpWeights(randFeatures.size(),0.0);
	float tmp = 0.0;
	bool doClustering = clusterOrGini(), useUnlabeledData = true;
	for ( int i = 0; i < numTries; i++)
	{
		fillWithRandomNumbers(tmpWeights);
		responses.clear();
		responses.reserve(inBagSamples.size());
		bagIt = inBagSamples.begin();
		while ( bagIt != bagEnd )
		{
			tmp = 0.0;
			int counter = 0;
			BOOST_FOREACH(int feat, randFeatures)
			{
				tmp += data(*bagIt,feat)*tmpWeights[counter];
				counter++;
			}

			responses.push_back(std::pair<float, int>(tmp,*bagIt));
			++bagIt;
		}
		sort(responses.begin(), responses.end());

		if (!doClustering)
		{
			if (m_hp.useInfoGain)
			{
				curGiniThresh = calcInfoGainAndThreshold(labels, weights, responses);
			}
			else
			{
				curGiniThresh = calcGiniAndThreshold(labels, weights, responses, useUnlabeledData);
			}
		}
		else
		{
			curGiniThresh = calcClusterScoreAndThreshold(data, inBagSamples, weights, responses);
		}

		if (curGiniThresh.first < bestDGini)
		{
			bestDGini = curGiniThresh.first;
			bestThreshold = curGiniThresh.second;
			bestWeights = tmpWeights;
		}
	}

	m_bestFeatures = randFeatures;
	m_bestWeights = bestWeights;
	m_bestThreshold = (float) bestThreshold;
}

NODE_TRAIN_STATUS NodeHyperPlane::trainLU(const matrix<float>& data, const std::vector<int>& labels,
		std::vector<int>& inBagSamples, matrix<float>& confidences, std::vector<int>& predictions)
{
	bool doSplit = shouldISplitLU(labels,inBagSamples);
	NODE_TRAIN_STATUS myTrainingStatus = IS_NOT_LEAF;

	if ( doSplit )
	{
		m_isLeaf = false;

		//train here the node: Select random features and evaluate them
		std::vector<int> randFeatures = randPerm(data.size2(), m_hp.numProjFeatures );
		int numTries = m_hp.numRandomFeatures;// * (m_depth+1);
		findHypothesesLU(data, labels, inBagSamples, randFeatures, numTries);
		if (m_hp.verbose)
		{
			cout << "Node #: " << m_nodeIndex;
			cout << " and the threshold is: " << m_bestThreshold << " at depth " << m_depth << endl;
		}

		// split the data
		std::vector<int> leftNodeSamples, rightNodeSamples;
		evalNode(data,inBagSamples,leftNodeSamples,rightNodeSamples);

		// pass them to the left and right child, respectively
		m_leftChildNode = Ptr(new NodeHyperPlane(m_hp,m_depth + 1));
		m_rightChildNode = Ptr(new NodeHyperPlane(m_hp,m_depth + 1));

		m_leftChildNode->train(data,labels,leftNodeSamples,confidences,predictions);
		m_rightChildNode->train(data,labels,rightNodeSamples,confidences,predictions);
	}
	else
	{
		if (m_hp.verbose)
		{
			cout << "Node #: " << m_nodeIndex << " is terminal, at depth " << m_depth << endl;
		}

		// calc confidence, labels, etc
		m_isLeaf = true;
		myTrainingStatus = IS_LEAF;
		m_nodeConf.resize(m_hp.numClasses, 0.0);
		int numNodeLabeled = 0;
		BOOST_FOREACH(int n, inBagSamples)
		{
			if (n < m_hp.numLabeled)
			{
				m_nodeConf[labels[n]]++;
				numNodeLabeled++;
			}
		}

		int bestClass = 0, tmpN = 0;
		float bestConf = 0;
		std::vector<float>::iterator confItr = m_nodeConf.begin(), confEnd = m_nodeConf.end();
		for (; confItr != confEnd; confItr++)
		{
			if (numNodeLabeled)
			{
				*confItr /= numNodeLabeled;
				if (*confItr > bestConf)
				{
					bestConf = *confItr;
					bestClass = tmpN;
				}
				tmpN++;
			}
			else
			{
				*confItr = 1.0/m_hp.numClasses;
			}
		}
		m_nodeLabel = bestClass;

		BOOST_FOREACH(int n, inBagSamples)
		{
			predictions[n] = m_nodeLabel;
			tmpN = 0;
			BOOST_FOREACH(float conf, m_nodeConf)
			{
				confidences(n, tmpN) = conf;
				tmpN++;
			}
		}
	}

	return myTrainingStatus;
}

bool NodeHyperPlane::clusterOrGini()
{
	return false;
}

void NodeHyperPlane::evalNode(const matrix<float>& data, const std::vector<int>& inBagSamples,
		std::vector<int>& leftNodeSamples, std::vector<int>& rightNodeSamples)
{
	float tmp;
	BOOST_FOREACH(int n, inBagSamples)
	{
		tmp = 0.0;
		int counter = 0;
		BOOST_FOREACH(int feat, m_bestFeatures)
		{
			tmp += data(n,feat)*m_bestWeights[counter];
			counter++;
		}

		if (tmp > m_bestThreshold)
		{
			rightNodeSamples.push_back(n);
		}
		else
		{
			leftNodeSamples.push_back(n);
		}
	}
}

NODE_TRAIN_STATUS NodeHyperPlane::train(const matrix<float>& data, const std::vector<int>& labels,
		std::vector<int>& inBagSamples, matrix<float>& confidences, std::vector<int>& predictions)
{
	bool doSplit = shouldISplit(labels,inBagSamples);
	NODE_TRAIN_STATUS myTrainingStatus = IS_NOT_LEAF;

	if ( doSplit )
	{
		m_isLeaf = false;

		//train here the node: Select random features and evaluate them
		std::vector<int> randFeatures = randPerm(data.size2(), m_hp.numProjFeatures );
		int numTries = m_hp.numRandomFeatures;// * (m_depth+1);
		findHypotheses(data, labels, inBagSamples, randFeatures, numTries);
		if (m_hp.verbose)
		{
			cout << "Node #: " << m_nodeIndex;
			cout << " and the threshold is: " << m_bestThreshold << " at depth " << m_depth << endl;
		}

		// split the data
		std::vector<int> leftNodeSamples, rightNodeSamples;
		evalNode(data,inBagSamples,leftNodeSamples,rightNodeSamples);

		// pass them to the left and right child, respectively
		m_leftChildNode = Ptr(new NodeHyperPlane(m_hp,m_depth + 1));
		m_rightChildNode = Ptr(new NodeHyperPlane(m_hp,m_depth + 1));

		NODE_TRAIN_STATUS leftChildStatus = m_leftChildNode->train(data,labels,leftNodeSamples,confidences,predictions);
		NODE_TRAIN_STATUS rightChildStatus= m_rightChildNode->train(data,labels,rightNodeSamples,confidences,predictions);

	}
	else
	{
		if (m_hp.verbose)
		{
			cout << "Node #: " << m_nodeIndex << " is terminal, at depth " << m_depth << endl;
		}

		// calc confidence, labels, etc
		m_isLeaf = true;
		myTrainingStatus = IS_LEAF;
		m_nodeConf.resize(m_hp.numClasses, 0.0);

		BOOST_FOREACH(int n, inBagSamples)
		{
			m_nodeConf[labels[n]]++;
		}

		int bestClass = 0, tmpN = 0;
		float bestConf = 0;
		std::vector<float>::iterator confItr = m_nodeConf.begin(), confEnd = m_nodeConf.end();
		for (; confItr != confEnd; confItr++)
		{
			*confItr /= inBagSamples.size();
			if (*confItr > bestConf)
			{
				bestConf = *confItr;
				bestClass = tmpN;
			}
			tmpN++;
		}
		m_nodeLabel = bestClass;

		BOOST_FOREACH(int n, inBagSamples)
		{
			predictions[n] = m_nodeLabel;
			tmpN = 0;
			BOOST_FOREACH(float conf, m_nodeConf)
			{
				confidences(n, tmpN) = conf;
				tmpN++;
			}
		}
	}

	return myTrainingStatus;
}

NODE_TRAIN_STATUS NodeHyperPlane::train(const matrix<float>& data, const std::vector<int>& labels, const std::vector<double>& weights,
		std::vector<int>& inBagSamples, matrix<float>& confidences, std::vector<int>& predictions)
{
	bool doSplit = shouldISplit(labels,inBagSamples);
	NODE_TRAIN_STATUS myTrainingStatus = IS_NOT_LEAF;

	if ( doSplit )
	{
		m_isLeaf = false;

		//train here the node: Select random features and evaluate them
		std::vector<int> randFeatures = randPerm(data.size2(),m_hp.numProjFeatures );
		int numTries = m_hp.numRandomFeatures * (m_depth+1);
		findHypotheses(data, labels, weights, inBagSamples, randFeatures,numTries);
		if (m_hp.verbose)
		{
			cout << "Node #: " << m_nodeIndex;
			cout << " and the threshold is: " << m_bestThreshold << " at depth " << m_depth << endl;
		}

		// split the data
		std::vector<int> leftNodeSamples, rightNodeSamples;
		evalNode(data,inBagSamples,leftNodeSamples,rightNodeSamples);

		// pass them to the left and right child, respectively
		m_leftChildNode = Ptr(new NodeHyperPlane(m_hp,m_depth + 1));
		m_rightChildNode = Ptr(new NodeHyperPlane(m_hp,m_depth + 1));

		NODE_TRAIN_STATUS leftChildStatus = m_leftChildNode->train(data,labels,weights,leftNodeSamples,confidences,predictions);
		NODE_TRAIN_STATUS rightChildStatus= m_rightChildNode->train(data,labels,weights,rightNodeSamples,confidences,predictions);

	}
	else
	{
		if (m_hp.verbose)
		{
			cout << "Node #: " << m_nodeIndex << " is terminal, at depth " << m_depth << endl;
		}

		// calc confidence, labels, etc
		m_isLeaf = true;
		myTrainingStatus = IS_LEAF;
		m_nodeConf.resize(m_hp.numClasses, 0.0);

		double totalW = 0;
		BOOST_FOREACH(int n, inBagSamples)
		{
			m_nodeConf[labels[n]] += weights[n];
			totalW += weights[n];
		}

		int bestClass = 0, tmpN = 0;
		float bestConf = 0;
		std::vector<float>::iterator confItr = m_nodeConf.begin(), confEnd = m_nodeConf.end();
		for (; confItr != confEnd; confItr++)
		{
			*confItr /= (totalW + 1e-10);
			if (*confItr > bestConf)
			{
				bestConf = *confItr;
				bestClass = tmpN;
			}
			tmpN++;
		}
		m_nodeLabel = bestClass;

		BOOST_FOREACH(int n, inBagSamples)
		{
			predictions[n] = m_nodeLabel;
			tmpN = 0;
			BOOST_FOREACH(float conf, m_nodeConf)
			{
				confidences(n, tmpN) = conf;
				tmpN++;
			}
		}
	}

	return myTrainingStatus;
}

void NodeHyperPlane::eval(const matrix<float>& data, const std::vector<int>& sampleIndeces,
		matrix<float>& confidences, std::vector<int>& predictions)
{
	if (m_isLeaf)
	{
		// Make predictions and confidences
		int tmpN;
		BOOST_FOREACH( int n, sampleIndeces)
		{
			predictions[n] = m_nodeLabel;
			tmpN = 0;
			BOOST_FOREACH(float conf, m_nodeConf)
			{
				confidences(n, tmpN) = conf;
				tmpN++;
			}
		}
	}
	else
	{
		// split the data
		std::vector<int> leftNodeSamples, rightNodeSamples;
		evalNode(data,sampleIndeces,leftNodeSamples,rightNodeSamples);

		m_leftChildNode->eval(data,leftNodeSamples,confidences,predictions);
		m_rightChildNode->eval(data,rightNodeSamples,confidences,predictions);
	}
}

void NodeHyperPlane::getPath(const matrix<float>& data, const std::vector<int>& sampleIndeces, std::vector<std::vector<int> >& path)
{
	BOOST_FOREACH(int n, sampleIndeces)
    		{
		path[n].push_back(m_nodeIndex);
    		}

	if (!m_isLeaf)
	{
		// split the data
		std::vector<int> leftNodeSamples, rightNodeSamples;
		evalNode(data,sampleIndeces,leftNodeSamples,rightNodeSamples);

		m_leftChildNode->getPath(data,leftNodeSamples,path);
		m_rightChildNode->getPath(data,rightNodeSamples,path);
	}
}

void NodeHyperPlane::refine(const matrix<float>& data, const std::vector<int>& labels,
		std::vector<int>& samples, matrix<float>& confidences, std::vector<int>& predictions)
{
	if ( !m_isLeaf )
	{
		// split the data
		std::vector<int> leftNodeSamples, rightNodeSamples;
		evalNode(data,samples,leftNodeSamples,rightNodeSamples);

		m_leftChildNode->refine(data,labels,leftNodeSamples,confidences,predictions);
		m_rightChildNode->refine(data,labels,rightNodeSamples,confidences,predictions);
	}
	else
	{
		// calc confidence, labels, etc
		m_nodeConf.resize(m_hp.numClasses,0.0);
		BOOST_FOREACH(int n, samples)
		{
			m_nodeConf[labels[n]]++;
		}

		int bestClass = 0, tmpN = 0;
		float bestConf = 0;
		std::vector<float>::iterator confItr = m_nodeConf.begin(), confEnd = m_nodeConf.end();
		for (; confItr != confEnd; confItr++)
		{
			*confItr /= samples.size();
			if (*confItr > bestConf)
			{
				bestConf = *confItr;
				bestClass = tmpN;
			}
			tmpN++;
		}
		m_nodeLabel = bestClass;

		BOOST_FOREACH(int n, samples)
		{
			predictions[n] = m_nodeLabel;
			tmpN = 0;
			BOOST_FOREACH(float conf, m_nodeConf)
			{
				confidences(n, tmpN) = conf;
				tmpN++;
			}
		}
	}
}

void NodeHyperPlane::refine(const matrix<float>& data, const std::vector<int>& labels, const std::vector<double>& weights,
		std::vector<int>& samples, matrix<float>& confidences, std::vector<int>& predictions)
{
	// calc confidence, labels, etc
	if (!m_isLeaf)
	{
		// split the data
		std::vector<int> leftNodeSamples, rightNodeSamples;
		evalNode(data,samples,leftNodeSamples,rightNodeSamples);

		m_leftChildNode->refine(data,labels,weights,leftNodeSamples,confidences,predictions);
		m_rightChildNode->refine(data,labels,weights,rightNodeSamples,confidences,predictions);
	}
	else
	{
		m_nodeConf.resize(m_hp.numClasses,0.0);
		std::vector<float>::iterator confItr2 = m_nodeConf.begin(), confEnd2 = m_nodeConf.end();
		for (; confItr2 != confEnd2; confItr2++)
		{
			*confItr2=0;
//			std::cout<<*confItr2<<std::endl;
		}
		double totalW = 0;
		BOOST_FOREACH(int n, samples)
		{
			m_nodeConf[labels[n]] += weights[n];
			totalW += weights[n];
//			std::cout<<n<<" "<<labels[n]<<" "<<weights[n]<<" "<<m_nodeConf[labels[n]]<<" "<<(totalW + 1e-10)<<" "<<m_nodeConf[labels[n]]/(totalW + 1e-10)<<std::endl;

		}

		int bestClass = 0, tmpN = 0;
		float bestConf = 0;
		std::vector<float>::iterator confItr = m_nodeConf.begin(), confEnd = m_nodeConf.end();
		for (; confItr != confEnd; confItr++)
		{
			*confItr /= (totalW + 1e-10);
			if (*confItr > bestConf)
			{
				bestConf = *confItr;
				bestClass = tmpN;
			}
			tmpN++;
		}
		m_nodeLabel = bestClass;

		BOOST_FOREACH(int n, samples)
		{
			predictions[n] = m_nodeLabel;
			tmpN = 0;
			BOOST_FOREACH(float conf, m_nodeConf)
			{
				confidences(n, tmpN) = conf;
				tmpN++;
			}
		}
	}
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_HASH_SHA3_HPP
#define CRYPTO3_HASH_SHA3_HPP

#include <boost/crypto3/hash/detail/sponge_construction.hpp>
#include <boost/crypto3/hash/detail/block_stream_processor.hpp>
#include <boost/crypto3/hash/detail/sha3/sha3_functions.hpp>
#include <boost/crypto3/hash/detail/sha3/sha3_policy.hpp>
#include <boost/crypto3/hash/detail/sha3/sha3_finalizer.hpp>
#include <boost/crypto3/hash/detail/sha3/sha3_padding.hpp>

#include <boost/endian/conversion.hpp>

namespace boost {
    namespace crypto3 {
        namespace hashes {
            template<std::size_t DigestBits = 512>
            class sha3_compressor {
            protected:
                typedef detail::sha3_functions<DigestBits> policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t state_bits = policy_type::state_bits;
                constexpr static const std::size_t state_words = policy_type::state_words;
                typedef typename policy_type::state_type state_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                static void process_block(state_type &state, const block_type &block) {
                    for (std::size_t i = 0; i != block_words; ++i)
                        state[i] ^= block[i];

                    for (std::size_t i = 0; i != state_words; ++i)
                        boost::endian::endian_reverse_inplace(state[i]);

                    policy_type::permute(state);

                    for (std::size_t i = 0; i != state_words; ++i)
                        boost::endian::endian_reverse_inplace(state[i]);
                }
            };

            /*!
             * @brief
             * @tparam DigestBits
             * @ingroup hashes
             */
            template<std::size_t DigestBits = 512>
            class sha3 {
                typedef detail::sha3_policy<DigestBits> policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                typedef typename policy_type::digest_type digest_type;

                struct construction {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t length_bits = policy_type::length_bits;
                        constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                    };

                    typedef sponge_construction<params_type, typename policy_type::iv_generator,
                                                sha3_compressor<DigestBits>, detail::sha3_padding<policy_type>,
                                                detail::sha3_finalizer<policy_type>>
                        type;
                };

                template<typename StateAccumulator, std::size_t ValueBits>
                struct stream_processor {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t value_bits = ValueBits;
                    };
                    typedef block_stream_processor<construction, StateAccumulator, params_type> type;
                };
            };
        }    // namespace hashes
    }        // namespace crypto3
}    // namespace boost

#endif


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

/// \file StereoSessionPinhole.cc
///

// Ames Stereo Pipeline
#include <asp/Core/StereoSettings.h>
#include <asp/Sessions/Pinhole/StereoSessionPinhole.h>

// Vision Workbench
#include <vw/FileIO/DiskImageView.h>
#include <vw/Camera/PinholeModel.h>
#include <vw/Camera/CAHVModel.h>
#include <vw/Camera/CAHVORModel.h>
#include <vw/Camera/CAHVOREModel.h>
#include <vw/Camera/CameraTransform.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/Stereo/DisparityMap.h>
#include <vw/Math.h>

// Boost
#include <boost/shared_ptr.hpp>
#include <boost/filesystem/operations.hpp>
namespace fs = boost::filesystem;

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

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
}

boost::shared_ptr<vw::camera::CameraModel>
asp::StereoSessionPinhole::camera_model(std::string const& /*image_file*/,
                                        std::string const& camera_file) {
  // Epipolar Alignment
  if ( stereo_settings().epipolar_alignment ) {
    // Load the image
    DiskImageView<PixelGray<float> > left_image(m_left_image_file);
    DiskImageView<PixelGray<float> > right_image(m_right_image_file);

    Vector2i left_image_size( left_image.cols(), left_image.rows() ),
      right_image_size( right_image.cols(), right_image.rows() );

    bool is_left_camera = true;
    if (camera_file == m_left_camera_file)
      is_left_camera = true;
    else if (camera_file == m_right_camera_file)
      is_left_camera = false;
    else
      (ArgumentErr() << "StereoSessionPinhole: supplied camera model filename does not match the name supplied in the constructor.");

    // Return the appropriate camera model object
    std::string lcase_file = boost::to_lower_copy(m_left_camera_file);
    CAHVModel left_cahv, right_cahv;
    if (boost::ends_with(lcase_file, ".cahvore") ) {
      CAHVOREModel left_cahvore(m_left_camera_file);
      CAHVOREModel right_cahvore(m_right_camera_file);
      left_cahv =
        linearize_camera(left_cahvore, left_image_size, left_image_size);
      right_cahv =
        linearize_camera(right_cahvore, right_image_size, right_image_size);
    } else if (boost::ends_with(lcase_file, ".cahvor")  ||
               boost::ends_with(lcase_file, ".cmod") ) {
      CAHVORModel left_cahvor(m_left_camera_file);
      CAHVORModel right_cahvor(m_right_camera_file);
      left_cahv =
        linearize_camera(left_cahvor, left_image_size, left_image_size);
      right_cahv =
        linearize_camera(right_cahvor, right_image_size, right_image_size);
    } else if ( boost::ends_with(lcase_file, ".cahv") ||
                boost::ends_with(lcase_file, ".pin" )) {
      left_cahv = CAHVModel(m_left_camera_file);
      right_cahv = CAHVModel(m_right_camera_file);

    } else if ( boost::ends_with(lcase_file, ".pinhole") ||
                boost::ends_with(lcase_file, ".tsai") ) {
      PinholeModel left_pin(m_left_camera_file);
      PinholeModel right_pin(m_right_camera_file);
      left_cahv = linearize_camera(left_pin);
      right_cahv = linearize_camera(right_pin);
    } else {
      vw_throw(ArgumentErr() << "PinholeStereoSession: unsupported camera file type.\n");
    }

    // Create epipolar recitified camera views
    boost::shared_ptr<CAHVModel> epipolar_left_cahv(new CAHVModel);
    boost::shared_ptr<CAHVModel> epipolar_right_cahv(new CAHVModel);
    epipolar(left_cahv, right_cahv, *epipolar_left_cahv, *epipolar_right_cahv);

    if (is_left_camera)
      return epipolar_left_cahv;
    else
      return epipolar_right_cahv;
  } else {
    // Keypoint alignment and everything else just gets camera models
    std::string lcase_file = boost::to_lower_copy(camera_file);
    if (boost::ends_with(lcase_file,".cahvore") ) {
      return boost::shared_ptr<vw::camera::CameraModel>( new CAHVOREModel(camera_file) );
    } else if (boost::ends_with(lcase_file,".cahvor") ||
               boost::ends_with(lcase_file,".cmod") ) {
      return boost::shared_ptr<vw::camera::CameraModel>( new CAHVORModel(camera_file) );
    } else if ( boost::ends_with(lcase_file,".cahv") ||
                boost::ends_with(lcase_file,".pin") ) {
      return boost::shared_ptr<vw::camera::CameraModel>( new CAHVModel(camera_file) );
    } else if ( boost::ends_with(lcase_file,".pinhole") ||
                boost::ends_with(lcase_file,".tsai") ) {
      return boost::shared_ptr<vw::camera::CameraModel> ( new PinholeModel(camera_file) );
    } else {
      vw_throw(ArgumentErr() << "PinholeStereoSession: unsupported camera file type.\n");
    }

  }
  return boost::shared_ptr<vw::camera::CameraModel>(); // Never reached
}

void asp::StereoSessionPinhole::pre_preprocessing_hook(std::string const& input_file1,
                                                       std::string const& input_file2,
                                                       std::string &output_file1,
                                                       std::string &output_file2) {

  // Load the images
  DiskImageView<PixelGray<float> > left_disk_image(m_left_image_file);
  DiskImageView<PixelGray<float> > right_disk_image(m_right_image_file);

  Vector4f left_stats = gather_stats( left_disk_image, "left" ),
    right_stats = gather_stats( right_disk_image, "right" );

  ImageViewRef<PixelGray<float> > Limg, Rimg;
  std::string lcase_file = boost::to_lower_copy(m_left_camera_file);

  if ( stereo_settings().epipolar_alignment ) {

    vw_out() << "\t--> Performing epipolar alignment\n";

    // Load the two images and fetch the two camera models
    boost::shared_ptr<camera::CameraModel> left_camera =
      this->camera_model(input_file1, m_left_camera_file);
    boost::shared_ptr<camera::CameraModel> right_camera =
      this->camera_model(input_file2, m_right_camera_file);
    CAHVModel* left_epipolar_cahv = dynamic_cast<CAHVModel*>(&(*left_camera));
    CAHVModel* right_epipolar_cahv = dynamic_cast<CAHVModel*>(&(*right_camera));

    // Remove lens distortion and create epipolar rectified images.
    if (boost::ends_with(lcase_file, ".cahvore")) {
      CAHVOREModel left_cahvore(m_left_camera_file);
      CAHVOREModel right_cahvore(m_right_camera_file);
      Limg = transform(left_disk_image, CameraTransform<CAHVOREModel, CAHVModel>(left_cahvore, *left_epipolar_cahv));

      Rimg = transform(right_disk_image, CameraTransform<CAHVOREModel, CAHVModel>(right_cahvore, *right_epipolar_cahv));
    } else if (boost::ends_with(lcase_file, ".cahvor") ||
               boost::ends_with(lcase_file, ".cmod") ) {
      CAHVORModel left_cahvor(m_left_camera_file);
      CAHVORModel right_cahvor(m_right_camera_file);
      Limg = transform(left_disk_image, CameraTransform<CAHVORModel, CAHVModel>(left_cahvor, *left_epipolar_cahv));
      Rimg = transform(right_disk_image, CameraTransform<CAHVORModel, CAHVModel>(right_cahvor, *right_epipolar_cahv));

    } else if ( boost::ends_with(lcase_file, ".cahv") ||
                boost::ends_with(lcase_file, ".pin" )) {
      CAHVModel left_cahv(m_left_camera_file);
      CAHVModel right_cahv(m_right_camera_file);
      Limg = transform(left_disk_image, CameraTransform<CAHVModel, CAHVModel>(left_cahv, *left_epipolar_cahv));
      Rimg = transform(right_disk_image, CameraTransform<CAHVModel, CAHVModel>(right_cahv, *right_epipolar_cahv));

    } else if ( boost::ends_with(lcase_file, ".pinhole") ||
                boost::ends_with(lcase_file, ".tsai") ) {
      PinholeModel left_pin(m_left_camera_file);
      PinholeModel right_pin(m_right_camera_file);
      Limg = transform(left_disk_image, CameraTransform<PinholeModel, CAHVModel>(left_pin, *left_epipolar_cahv));
      Rimg = transform(right_disk_image, CameraTransform<PinholeModel, CAHVModel>(right_pin, *right_epipolar_cahv));

    } else {
      vw_throw(ArgumentErr() << "PinholeStereoSession: unsupported camera file type.\n");
    }

  } else if ( stereo_settings().keypoint_alignment ) {

    Matrix<double> align_matrix;
    align_matrix = determine_image_align( input_file1, input_file2,
                                          left_disk_image, right_disk_image);
    write_matrix( m_out_prefix + "-align.exr", align_matrix );

    // Applying alignment transform
    Limg = left_disk_image;
    Rimg = transform(right_disk_image,
                     HomographyTransform(align_matrix),
                     left_disk_image.cols(), left_disk_image.rows());

  } else {
    // Do nothing just provide the original files.
    Limg = left_disk_image;
    Rimg = right_disk_image;
  }

  // Apply our normalization options
  if ( stereo_settings().force_max_min > 0 ) {
    if ( stereo_settings().individually_normalize > 0 ) {
      vw_out() << "\t--> Individually normalize images to their respective Min Max\n";
      Limg = normalize( Limg, left_stats[0], left_stats[1], 0, 1.0 );
      Rimg = normalize( Rimg, right_stats[0], right_stats[1], 0, 1.0 );
    } else {
      float low = std::min(left_stats[0], right_stats[0]);
      float hi  = std::max(left_stats[1], right_stats[1]);
      vw_out() << "\t--> Normalizing globally to: [" << low << " " << hi << "]\n";
      Limg = normalize( Limg, low, hi, 0, 1.0 );
      Rimg = normalize( Rimg, low, hi, 0, 1.0 );
    }
  } else {
    if ( stereo_settings().individually_normalize > 0 ) {
      vw_out() << "\t--> Individually normalize images to their respective 4 std dev window\n";
      Limg = normalize( Limg, left_stats[2] - 2*left_stats[3],
                        left_stats[2] + 2*left_stats[3], 0, 1.0 );
      Rimg = normalize( Rimg, right_stats[2] - 2*right_stats[3],
                        right_stats[2] + 2*right_stats[3], 0, 1.0 );
    } else {
      float low = std::min(left_stats[2] - 2*left_stats[3],
                           right_stats[2] - 2*right_stats[3]);
      float hi  = std::max(left_stats[2] + 2*left_stats[3],
                           right_stats[2] + 2*right_stats[3]);
      vw_out() << "\t--> Normalizing globally to: [" << low << " " << hi << "]\n";
      Limg = normalize( Limg, low, hi, 0, 1.0 );
      Rimg = normalize( Rimg, low, hi, 0, 1.0 );
    }
  }

  output_file1 = m_out_prefix + "-L.tif";
  output_file2 = m_out_prefix + "-R.tif";
  vw_out() << "\t--> Writing pre-aligned images.\n";
  block_write_gdal_image( output_file1, Limg, m_options,
                          TerminalProgressCallback("asp","\t  L:  ") );
  block_write_gdal_image( output_file2, crop(edge_extend(Rimg,ConstantEdgeExtension()),bounding_box(Limg)), m_options,
                          TerminalProgressCallback("asp","\t  R:  ") );
}

// Reverse any pre-alignment that might have been done to the disparity map
void asp::StereoSessionPinhole::pre_pointcloud_hook(std::string const& input_file,
                                                    std::string & output_file ) {

  if ( stereo_settings().keypoint_alignment ) {

    DiskImageView<PixelMask<Vector2f> > disparity_map( input_file );
    output_file = m_out_prefix + "-F-corrected.tif";

    vw::Matrix<double> align_matrix;
    try {
      read_matrix(align_matrix, m_out_prefix + "-align.exr");
      vw_out(DebugMessage) << "Alignment Matrix: " << align_matrix << "\n";
    } catch ( vw::IOErr const& e ) {
      vw_out() << "\nCould not read in alignment matrix: " << m_out_prefix
               << "-align.exr. Exiting. \n\n";
      exit(1);
    }

    // Remove pixels that are outside the bounds of the second image
    DiskImageView<PixelGray<float> > right_disk_image( m_right_image_file );
    ImageViewRef<PixelMask<Vector2f> > result =
      stereo::disparity_range_mask( stereo::transform_disparities( disparity_map,
                                             HomographyTransform(align_matrix)),
                                    Vector2f(0,0),
                                    Vector2f( right_disk_image.cols(),
                                              right_disk_image.rows()) );

    block_write_gdal_image( output_file, result, m_options,
                            TerminalProgressCallback("asp", "\t    Saving: ") );
  } else {
    output_file = input_file;
  }
}