// Copyright 2020 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/// \copyright Copyright 2020 Apex.AI, Inc.
/// All rights reserved.

#include <state_estimation_nodes/kalman_filter_wrapper.hpp>

#include <common/types.hpp>
#include <kalman_filter/esrcf.hpp>
#include <motion_model/constant_acceleration.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/time.hpp>
#include <state_estimation_nodes/measurement.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <limits>
#include <cstdint>
#include <algorithm>

namespace
{
constexpr auto kCovarianceMatrixRows = 6U;
constexpr auto kIndexX = 0U;
constexpr auto kIndexY = kCovarianceMatrixRows + 1U;
constexpr auto kCovarianceMatrixRowsSquared = kCovarianceMatrixRows * kCovarianceMatrixRows;
static_assert(
  std::tuple_size<
    geometry_msgs::msg::PoseWithCovariance::_covariance_type>::value ==
  kCovarianceMatrixRowsSquared, "We expect the covariance matrix to have 36 entries.");

/// Convert a chrono timepoint to ros time.
rclcpp::Time to_ros_time(const std::chrono::system_clock::time_point & time_point)
{
  using std::chrono::duration_cast;
  using std::chrono::nanoseconds;
  return rclcpp::Time{duration_cast<nanoseconds>(time_point.time_since_epoch()).count()};
}

}  // namespace

namespace autoware
{
namespace prediction
{

using motion::motion_model::ConstantAcceleration;
using common::types::float64_t;
using common::types::bool8_t;

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
void KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::reset(
  const VectorT<kNumOfStates> & state,
  const SquareMatrixT<kNumOfStates> & initial_covariance_chol,
  const MeasurementBasedTime & event_timestamp,
  const GlobalTime & time_of_event_occurance)
{
  m_ekf->reset(state, initial_covariance_chol);
  m_time_keeper = MeasurementBasedTimeKeeper{time_of_event_occurance, event_timestamp};
  m_ekf_initialized = true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<typename MeasurementT>
void KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::reset(
  const MeasurementT & measurement,
  const GlobalTime & time_of_event_occurance)
{
  reset(
    measurement.get_values_in_full_state(m_motion_model.get_state()),
    m_initial_covariance_factor,
    measurement.get_acquisition_time(),
    time_of_event_occurance);
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<TimeReferenceFrame kTimeReferenceFrame>
common::types::bool8_t  // We cannot use an alias here as Doxygen thinks its a different signature.
KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::temporal_update(
  const Time<kTimeReferenceFrame> & time_of_update)
{
  if (!is_initialized()) {return false;}
  const auto dt = m_time_keeper.time_since_last_temporal_update(time_of_update);
  if (dt <= std::chrono::nanoseconds{0LL}) {return false;}
  m_ekf->temporal_update(dt);
  m_time_keeper.increment_last_temporal_update_time(dt);
  return true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<typename MeasurementT>
common::types::bool8_t  // We cannot use an alias here as Doxygen thinks its a different signature.
KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::observation_update(
  const GlobalTime & global_time_of_message_received,
  const MeasurementT & measurement)
{
  if (!is_initialized()) {
    // TODO(igor): this is not strictly correct, but should be good enough. If we get an observation
    // and the filter is not set to any state, we reset it. In this case we assume that this
    // measurement actually is statefull (not purely differential) and we ignore the variance of
    // this measurement.
    reset(measurement, global_time_of_message_received);
    return true;
  }
  // TODO(igor): I am not sure this should be calling latest_timestamp() as if we had a prediction
  // step with a later timestamp than our measurement here we will discard the measurement. Should
  // be just compare to the latest measurement time stored in the time keeper?
  if (m_time_keeper.latest_timestamp() > measurement.get_acquisition_time()) {return false;}
  if (!temporal_update(measurement.get_acquisition_time())) {return false;}
  // TODO(igor): I see a couple of ways to check mahalanobis distance in case the measurement does
  // not cover the full state. Here I upscale it to the full state, copying the values of the
  // current state for ones missing in the observation. We can alternatively apply the H matrix and
  // only compute the distance in the measurement world. Don't really know which one is best here.
  if (!passes_mahalanobis_gate(
      measurement.get_values_in_full_state(m_motion_model.get_state()),
      m_motion_model.get_state(),
      m_ekf->get_covariance())) {return false;}
  m_ekf->observation_update(
    measurement.get_values(),
    MeasurementT::template get_observation_to_state_mapping<kNumOfStates>(),
    measurement.get_variances());
  m_time_keeper.update_with_measurement(global_time_of_message_received, measurement);
  return true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
bool8_t KalmanFilterWrapper<MotionModelT, kNumOfStates,
  kProcessNoiseDim>::passes_mahalanobis_gate(
  const VectorT<kNumOfStates> & sample,
  const VectorT<kNumOfStates> & mean,
  const SquareMatrixT<kNumOfStates> & covariance_factor) const
{
  // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff
  // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T
  // squared_mahalanobis_distance = diff.T * C.inv() * diff
  // = diff.T * (L * L.T).inv() * diff
  // = diff.T * L.T.inv() * L.inv() * diff
  // = (L.inv() * diff).T * (L.inv() * diff)
  // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff),
  // which can be found as a solution to: L * x = diff.
  const auto diff = sample - mean;
  const auto squared_threshold = m_mahalanobis_threshold * m_mahalanobis_threshold;
  const auto x = covariance_factor.ldlt().solve(diff);
  return x.transpose() * x < squared_threshold;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
nav_msgs::msg::Odometry KalmanFilterWrapper<MotionModelT, kNumOfStates,
  kProcessNoiseDim>::get_state() const
{
  static_assert(
    sizeof(MotionModelT) == -1,
    "You have to have a specialization for get_state() function!");
  // We only throw here because otherwise the linter complaints there is no return value.
  throw std::runtime_error("You have to have a specialization for get_state() function!");
}

template<>
nav_msgs::msg::Odometry ConstantAccelerationFilter::get_state() const
{
  using std::chrono::duration_cast;
  using std::chrono::nanoseconds;
  nav_msgs::msg::Odometry msg{};
  if (!is_initialized()) {
    throw std::runtime_error("Filter not is_initialized, cannot get state.");
  }
  msg.header.stamp = rclcpp::Time{to_ros_time(m_time_keeper.latest_timestamp())};
  msg.header.frame_id = m_frame_id;
  // Fill state.
  msg.pose.pose.position.x =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::POSE_X]);
  msg.pose.pose.position.y =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::POSE_Y]);
  msg.twist.twist.linear.x =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::VELOCITY_X]);
  msg.twist.twist.linear.y =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::VELOCITY_Y]);

  const auto rotation_around_z = std::atan2(msg.twist.twist.linear.y, msg.twist.twist.linear.x);
  tf2::Quaternion rotation;
  rotation.setRPY(0.0, 0.0, rotation_around_z);
  msg.pose.pose.orientation = tf2::toMsg(rotation);

  // Fill covariances.
  const auto & covariance_factor = m_ekf->get_covariance();
  const auto covariance = covariance_factor * covariance_factor.transpose();
  msg.pose.covariance[kIndexX] = static_cast<double>(covariance(
      ConstantAcceleration::States::POSE_X, ConstantAcceleration::States::POSE_X));
  msg.pose.covariance[kIndexY] = static_cast<double>(covariance(
      ConstantAcceleration::States::POSE_Y, ConstantAcceleration::States::POSE_Y));
  msg.twist.covariance[kIndexX] = static_cast<double>(covariance(
      ConstantAcceleration::States::VELOCITY_X, ConstantAcceleration::States::VELOCITY_X));
  msg.twist.covariance[kIndexY] = static_cast<double>(covariance(
      ConstantAcceleration::States::VELOCITY_Y, ConstantAcceleration::States::VELOCITY_Y));
  // TODO(igor): do we need elements off the diagonal?
  return msg;
}

/// Excplicit class instantiation.
template class KalmanFilterWrapper<ConstantAcceleration, 6, 2>;

using MeasurementPose = Measurement<common::types::float32_t,
    ConstantAcceleration::States::POSE_X,
    ConstantAcceleration::States::POSE_Y>;

using MeasurementPoseAndSpeed = Measurement<common::types::float32_t,
    ConstantAcceleration::States::POSE_X,
    ConstantAcceleration::States::POSE_Y,
    ConstantAcceleration::States::VELOCITY_X,
    ConstantAcceleration::States::VELOCITY_Y>;

using MeasurementSpeed = Measurement<common::types::float32_t,
    ConstantAcceleration::States::VELOCITY_X,
    ConstantAcceleration::States::VELOCITY_Y>;

template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::observation_update<>(
  const GlobalTime &, const MeasurementPose &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::observation_update<>(
  const GlobalTime &, const MeasurementSpeed &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::observation_update<>(
  const GlobalTime &, const MeasurementPoseAndSpeed &);

template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::temporal_update<>(
  const GlobalTime &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::temporal_update<>(
  const MeasurementBasedTime &);

}  // namespace prediction
}  // namespace autoware


// Library includes
#include <string>
#include <ostream>
#include <iostream>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/tokenizer.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/filesystem.hpp>
#include <cmath>
#include <thread>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

// Project includes
#include "parseCommandLineOptions.hpp"
#include "hornRegistration.hpp"
#include "PivotCalibration.hpp"
#include "PA3_4_DataConstants.hpp"
#include "parsePA3_4.hpp"
#include "DistortionCalibration.hpp"
#include "IterativeClosestPoint.hpp"

namespace po = boost::program_options;

/// Produce an output CIS CSV file
/// Note: I tried to make an output function but it probably has bugs
template<typename T>
void output1CISCSV_PA3(std::ostream& ostr, const std::string& outputName = "name-output-3.txt", const T & dk = std::vector<Eigen::Vector3d>(), const T & ck = std::vector<Eigen::Vector3d>(), const std::vector<double> & error = std::vector<double>()){

    ostr << dk.size() << " " << outputName << "\n";

    typename T::const_iterator dIterator = dk.begin();
    typename T::const_iterator cIterator = ck.begin();
    std::vector<double>::const_iterator eIterator = error.begin();
    for(; dIterator != dk.end() && cIterator != ck.end() && eIterator != error.end();
        ++dIterator, ++cIterator, ++eIterator){
            ostr  << (*dIterator)(0) << "   "  << (*dIterator)(1) << "  "  << (*dIterator)(2) << "  " << (*cIterator)(0) << "   "  << (*cIterator)(1) << "  "  << (*cIterator)(2) << "  " << *eIterator << "\n";
    }

    ostr.flush();

}

/// read the command line options from argc,argv and load them into the params object
/// @see boost::program_options for details on how the command line parsing library works.
bool readCommandLine(int argc, char* argv[], ParsedCommandLineCommandsPA3_4 & pclp){

    static const bool optional = false; // false means parameters are not required, and therefore optional
    static const bool required = true;  // true means parameters are required

    po::options_description generalOptions("General Options");
    generalOptions.add_options()
    ("responseFile", po::value<std::string>(), "File containing additional command line parameters")
    CLO_HELP
    CLO_DEBUG
    ("debugParser","display debug information for data file parser")
    ;

    TerminationCriteriaParams tcp; // default termination criteria params

    po::options_description algorithmOptions("Algorithm Options");
    algorithmOptions.add_options()
    ("threads","run each source data file in a separate thread. May speed up execution dramatically.")
    ("icpAlgorithm"                         ,po::value<std::string>()->default_value("spatialIndex"         ),"Options: spatialIndex, simpleSearch. Selects the ICP algorithm version to use. spatialIndex provides a substantial performance boost for large data sets. ")
    ("meanErrorThreshold"                   ,po::value<double>()->default_value(tcp.meanErrorThreshold)       ,"stop ICP when mean error drops below this level")
    ("maxErrorThreshold"                    ,po::value<double>()->default_value(tcp.maxErrorThreshold)       ,"stop ICP when max error drops below this level")
    ("minVarianceInMeanErrorBetweenIterations"                   ,po::value<double>()->default_value(tcp.minVarianceInMeanErrorBetweenIterations)       ,"stop ICP when mean error no longer varies between iterations")
    ("minIterationCount"                   ,po::value<int>()->default_value(tcp.minIterationCount)       ,"Do not stop ICP unless this many iterations have run")
    ("maxIterationCount"                    ,po::value<int>()->default_value(tcp.maxIterationCount)       ,"Stop ICP when the maximum iteration count threshold is reached, superceded by minIterationCount")

    ;

    // create algorithm command line options
    //algorithmOptions.add_options()
	// todo, add options for configuring algorithms

    po::options_description dataOptions("Data Options");

    std::string currentPath(boost::filesystem::path( boost::filesystem::current_path() ).string());

    // create algorithm command line options
    dataOptions.add_options()
            ("pa3", "set automatic programming assignment 3 source data parameters, overrides DataFilenamePrefix, exclusive of pa4")
            ("pa4", "set automatic programming assignment 4 source data parameters, overrides DataFilenamePrefix, exclusive of pa3")

            ("dataFolderPath"                   ,po::value<std::string>()->default_value(currentPath)       ,"folder containing data files, defaults to current working directory"   )
            ("outputDataFolderPath"             ,po::value<std::string>()->default_value(currentPath)       ,"folder for output data files, defaults to current working directory"   )
            ("dataFilenamePrefix"               ,po::value<std::vector<std::string> >()->default_value(PA4DataFilePrefixes(),""),"constant prefix of data filename path. Specify this multiple times to run on many data sources at once"   )
			("dataFilenameProblemPrefix"               ,po::value<std::string >()->default_value(pa4problemPrefix),"constant prefix of data typically starting with \"Problem\" filename path. Specify this multiple times to run on many data sources at once"   )
		  	("suffixAnswer"    ,po::value<std::string>()->default_value(DefaultAnswer         ),"suffix of data filename path"   )
		  	("suffixOutput"    ,po::value<std::string>()->default_value(DefaultOutput         ),"suffix of data filename path"   )
		  	("suffixSample"    ,po::value<std::string>()->default_value(DefaultSampleReadings ),"suffix of data filename path"   )
		  	("suffixMesh"      ,po::value<std::string>()->default_value(DefaultMesh           ),"suffix of data filename path"   )
		  	("suffixBodyA"     ,po::value<std::string>()->default_value(DefaultBodyA          ),"suffix of data filename path"   )
		  	("suffixBodyB"     ,po::value<std::string>()->default_value(DefaultBodyB          ),"suffix of data filename path"   )

		  	("AnswerPath"                    ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("OutputPath"                    ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("SamplePath"                    ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("MeshPath"                      ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("BodyAPath"                     ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("BodyBPath"  			         ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("em_fiducialsPath"  			 ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("em_navPath"        			 ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("output2Path"       			 ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )

			  ;

    po::options_description allOptions;
    allOptions.add(generalOptions).add(algorithmOptions).add(dataOptions);

    po::variables_map vmap;

    try
    {
      po::store(po::command_line_parser(argc, argv).options(allOptions).run(), vmap);
      po::notify(vmap);
    }
    catch (std::exception& e)
    {
      std::cerr << "[Error] " << BOOST_CURRENT_FUNCTION << std::endl
      << "    " << e.what() << std::endl
      << std::endl
      << allOptions << std::endl;
      return false;
    }

    if (vmap.count(CLO_GET_ARG_STR2(CLO_HELP)) || argc < 2)
    {
      std::cout << allOptions << std::endl;
      return false;
    }

    pclp.debugParser = vmap.count("debugParser");

	parseResponseFiles(vmap,allOptions);

	// initalize string params
	std::string  dataFolderPath
				,dataFileNameSuffix_AnswerPath
				,dataFileNameSuffix_OutputPath
				,dataFileNameSuffix_SamplePath
				,dataFileNameSuffix_MeshPath
				,dataFileNameSuffix_BodyAPath
				,dataFileNameSuffix_BodyBPath
				;

    DataSourcePA3_4 datasource;

    std::vector<std::string> dataFilenamePrefixList;
	std::string dataFilenameProblemPrefix, icpAlgorithm;

    // load up parameter values from the variable map
    po::readOption(vmap, "dataFolderPath"                  ,dataFolderPath                     ,optional);
    po::readOption(vmap, "outputDataFolderPath"            ,pclp.outputDataFolderPath          ,optional);
	po::readOption(vmap, "dataFilenamePrefix"              ,dataFilenamePrefixList             ,optional);
	po::readOption(vmap, "dataFilenameProblemPrefix"       ,dataFilenameProblemPrefix          ,optional);
	po::readOption(vmap, "suffixAnswer"                    ,dataFileNameSuffix_AnswerPath      ,optional);
	po::readOption(vmap, "suffixOutput"                    ,dataFileNameSuffix_OutputPath      ,optional);
	po::readOption(vmap, "suffixSample"                    ,dataFileNameSuffix_SamplePath      ,optional);
	po::readOption(vmap, "suffixMesh"                      ,dataFileNameSuffix_MeshPath        ,optional);
	po::readOption(vmap, "suffixBodyA"                     ,dataFileNameSuffix_BodyAPath       ,optional);
	po::readOption(vmap, "suffixBodyB"                     ,dataFileNameSuffix_BodyBPath       ,optional);

    /// @todo consider allowing these values to be specified separately for each data source
	po::readOption(vmap, "meanErrorThreshold"              ,tcp.meanErrorThreshold       ,optional);
	po::readOption(vmap, "maxErrorThreshold"               ,tcp.maxErrorThreshold        ,optional);
	po::readOption(vmap, "minVarianceInMeanErrorBetweenIterations"               ,tcp.minVarianceInMeanErrorBetweenIterations        ,optional);
	po::readOption(vmap, "minIterationCount"               ,tcp.minIterationCount        ,optional);
	po::readOption(vmap, "maxIterationCount"               ,tcp.maxIterationCount        ,optional);

    // min iteration count supercedes max
    tcp.maxIterationCount = std::max(tcp.minIterationCount,tcp.maxIterationCount);

    // enable threads if specified
    pclp.threads = vmap.count("threads");

    // enable spatialIndex if selected
	po::readOption(vmap, "icpAlgorithm"                     ,icpAlgorithm       ,optional);
    pclp.useSpatialIndex = (icpAlgorithm == "spatialIndex");

    if (vmap.count("pa3")) {
        dataFilenamePrefixList = PA3DataFilePrefixes();
		dataFilenameProblemPrefix = pa3problemPrefix;
    } else if (vmap.count("pa4")) {
        dataFilenamePrefixList = PA4DataFilePrefixes();
		dataFilenameProblemPrefix = pa4problemPrefix;
    }

    int prefixCount = dataFilenamePrefixList.size();
    if(!prefixCount){
        pclp.dataSources.push_back(DataSourcePA3_4());
    }

    if(prefixCount<=1){
        po::readOption(vmap,"calbodyPath"                      ,pclp.dataSources[0].BodyA            ,optional);
        po::readOption(vmap,"calreadingsPath"                  ,pclp.dataSources[0].BodyB            ,optional);
        po::readOption(vmap,"empivotPath"                      ,pclp.dataSources[0].SampleReadings   ,optional);
        po::readOption(vmap,"optpivotPath"                     ,pclp.dataSources[0].Answer           ,optional);
        po::readOption(vmap,"output1Path"                      ,pclp.dataSources[0].Output           ,optional);
        po::readOption(vmap,"ct_fiducialsPath"                 ,pclp.dataSources[0].Mesh             ,optional);
    }

	// check if the user supplied a full path, if not assemble a path
	// from the default paths and the defualt prefix/suffix combos
    for(auto&& prefix : dataFilenamePrefixList){
        DataSourcePA3_4 dataSource;
		dataSource.terminationCriteriaParams = tcp;
        dataSource.filenamePrefix = prefix;
        dataSource.filenameProblemPrefix = dataFilenameProblemPrefix;
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenameProblemPrefix       ,dataFileNameSuffix_BodyAPath   ,dataSource.BodyA            ,required);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenameProblemPrefix       ,dataFileNameSuffix_BodyBPath   ,dataSource.BodyB            ,required);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenamePrefix              ,dataFileNameSuffix_SamplePath  ,dataSource.SampleReadings   ,required);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenamePrefix              ,dataFileNameSuffix_AnswerPath  ,dataSource.Answer           ,optional);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenamePrefix              ,dataFileNameSuffix_OutputPath  ,dataSource.Output           ,optional);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenameProblemPrefix       ,dataFileNameSuffix_MeshPath    ,dataSource.Mesh             ,required);

        pclp.dataSources.push_back(dataSource);
    }

    pclp.debug = vmap.count(CLO_GET_ARG_STR2(CLO_DEBUG));

    return false;
}

/// run the PA3 algorithm and create then write the output file
void generateOutputFilePA3_4(AlgorithmDataPA3_4 ad, std::string outputDataFolderPath, std::string dataFilenamePrefix, bool useSpatialIndex = false, TerminationCriteriaParams tcp = TerminationCriteriaParams(), bool debug = false){

    Eigen::MatrixXd skMat;
    Eigen::MatrixXd ckMat;

    std::vector<double> errork;
    Eigen::Affine3d Freg;
    TerminationCriteria tc(tcp);
    tc.description = dataFilenamePrefix;

    Eigen::MatrixXd dkMat = dkKnownMeshPointsBaseFrame(ad.sampleReadings.NA, ad.sampleReadings.NB, ad.bodyA.tip, ad.bodyA.markerLEDs, ad.bodyB.markerLEDs);
    if(!useSpatialIndex){ // original slower way
        ICPwithSimpleSearch(dkMat, ad.mesh.vertices, ad.mesh.vertexTriangleNeighborIndex,tc,Freg,skMat,ckMat,errork,debug);
    } else { // new cool fast big data structure way
        ICPwithSpatialIndex(dkMat, ad.mesh.vertices, ad.mesh.vertexTriangleNeighborIndex,tc,Freg,skMat,ckMat,errork,debug);
    }

    std::vector<Eigen::Vector3d> sk(splitVectors(skMat));
    std::vector<Eigen::Vector3d> ck(splitVectors(ckMat));

    std::string outputFilename =  dataFilenamePrefix + "-Output.txt";
    std::ofstream ofs (outputFilename, std::ofstream::out);
    output1CISCSV_PA3(ofs,outputFilename,sk,ck,errork);

    ofs.close();

}

/**************************************************************************/
/**
 * @brief Main function
 *
 * @param argc  Number of input arguments
 * @param argv  Pointer to input arguments
 *
 * @return int
 */
int main(int argc,char**argv) {

	ParsedCommandLineCommandsPA3_4 pclp;
	readCommandLine(argc,argv,pclp);

    // thread pool to speed up execution
    std::vector<std::thread> th;

    for(auto&& dataSource : pclp.dataSources){
        AlgorithmDataPA3_4 ad;
        ad.bodyA          = parseProblemBody    (loadStringFromFile(dataSource.BodyA)            ,pclp.debugParser       );
        ad.bodyB          = parseProblemBody    (loadStringFromFile(dataSource.BodyB)            ,pclp.debugParser       );
		ad.mesh           = parseMesh           (loadStringFromFile(dataSource.Mesh)             ,pclp.debugParser       );
		ad.sampleReadings = parseSampleReadings (loadStringFromFile(dataSource.SampleReadings)   , ad.bodyA.markerLEDs.rows(), ad.bodyB.markerLEDs.rows() ,pclp.debugParser       );

        if(pclp.threads) {
            // run all data sources in separate threads to speed up execution
            th.push_back(std::thread(generateOutputFilePA3_4,ad, pclp.outputDataFolderPath, dataSource.filenamePrefix,pclp.useSpatialIndex,dataSource.terminationCriteriaParams,pclp.debug));
        } else {
            generateOutputFilePA3_4(ad, pclp.outputDataFolderPath, dataSource.filenamePrefix,pclp.useSpatialIndex,dataSource.terminationCriteriaParams,pclp.debug);
        }
    }

    //Join the threads with the main thread
    for(auto &t : th){
        t.join();
    }

	return 0;
}


/**********************************************************************
*  Copyright (c) 2008-2014, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include "Matrix.hpp"

#include "../core/Optional.hpp"
#include "../math/FloatCompare.hpp"

#include <random>

// this should all be moved to a utilities/core/Random.h
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

namespace openstudio{

  bool operator==(const Matrix& lhs, const Matrix& rhs)
  {
    bool result = false;
    if (lhs.size1() == rhs.size1()){
      if (lhs.size2() == rhs.size2()){
        result = true;
        for (unsigned i = 0; i < lhs.size1(); ++i){
          for (unsigned j = 0; j < lhs.size2(); ++j){
            if (lhs(i,j) != rhs(i,j)){
              return false;
            }
          }
        }
      }
    }
    return result;
  }

  bool operator!=(const Matrix& lhs, const Matrix& rhs)
  {
    return !(lhs==rhs);
  }

  /// linear interpolation of the function v = f(x, y) at point xi, yi
  /// assumes that x and y are strictly increasing
  double interp(const Vector& x, const Vector& y, const Matrix& v, double xi, double yi, InterpMethod interpMethod, ExtrapMethod extrapMethod)
  {
    double result = 0.0;

    unsigned M = x.size();
    unsigned N = y.size();

    if ((M != v.size1()) || (N != v.size2())){
      return result;
    }

    InterpInfo xInfo = interpInfo(x, xi);

    if (xInfo.extrapolated){
      switch(extrapMethod){
        case NoneExtrap:
          // set all weights to zero
          xInfo.wa = 0.0; xInfo.wb = 0.0;
          break;
        case NearestExtrap:
          // pick closest point
          // no-op
          break;
      }
    }else{
      switch(interpMethod){
        case LinearInterp:
          // linear interpolation
          // no-op
          break;
        case NearestInterp:
          // pick closest point
          if(xInfo.wa > xInfo.wb){
            xInfo.wa = 1.0; xInfo.wb = 0.0;
          }else{
            xInfo.wa = 0.0; xInfo.wb = 1.0;
          }
          break;
        case HoldLastInterp:
          // set to previous value
          xInfo.wa = 1.0; xInfo.wb = 0.0;
          break;
        case HoldNextInterp:
          // set to next value
          xInfo.wa = 0.0; xInfo.wb = 1.0;
          break;
      }
    }

    InterpInfo yInfo = interpInfo(y, yi);

    if (yInfo.extrapolated){
      switch(extrapMethod){
        case NoneExtrap:
          // set all weights to zero
          yInfo.wa = 0.0; yInfo.wb = 0.0;
          break;
        case NearestExtrap:
          // pick closest point
          // no-op
          break;
      }
    }else{
      switch(interpMethod){
        case LinearInterp:
          // linear interpolation
          // no-op
          break;
        case NearestInterp:
          // pick closest point
          if(yInfo.wa > yInfo.wb){
            yInfo.wa = 1.0; yInfo.wb = 0.0;
          }else{
            yInfo.wa = 0.0; yInfo.wb = 1.0;
          }
          break;
        case HoldLastInterp:
          // set to previous value
          yInfo.wa = 1.0; yInfo.wb = 0.0;
          break;
        case HoldNextInterp:
          // set to next value
          yInfo.wa = 0.0; yInfo.wb = 1.0;
          break;
      }
    }

    // we have set weights appropriately so that here we can compute in the same way all the time
    result = xInfo.wa*yInfo.wa*v(xInfo.ia, yInfo.ia) +
             xInfo.wa*yInfo.wb*v(xInfo.ia, yInfo.ib) +
             xInfo.wb*yInfo.wa*v(xInfo.ib, yInfo.ia) +
             xInfo.wb*yInfo.wb*v(xInfo.ib, yInfo.ib);

    return result;
  }

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  Vector interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, double yi, InterpMethod interpMethod, ExtrapMethod extrapMethod)
  {
    unsigned M = x.size();

    Vector result(M);

    if (M != v.size1()){
      return result;
    }

    for (unsigned i = 0; i < M; ++i){
      result(i) = interp(x, y, v, xi(i), yi, interpMethod, extrapMethod);
    }

    return result;
  }

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  Vector interp(const Vector& x, const Vector& y, const Matrix& v, double xi, const Vector& yi, InterpMethod interpMethod, ExtrapMethod extrapMethod)
  {
    unsigned N = y.size();

    Vector result(N);

    if (N != v.size2()){
      return result;
    }

    for (unsigned j = 0; j < N; ++j){
      result(j) = interp(x, y, v, xi, yi(j), interpMethod, extrapMethod);
    }

    return result;
  }

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  Matrix interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, const Vector& yi, InterpMethod interpMethod, ExtrapMethod extrapMethod)
  {
    unsigned M = x.size();
    unsigned N = y.size();

    Matrix result(M, N);

    if ((M != v.size1()) || (N != v.size2())){
      return result;
    }

    for (unsigned i = 0; i < M; ++i){
      for (unsigned j = 0; j < N; ++j){
        result(i,j) = interp(x, y, v, xi(i), yi(j), interpMethod, extrapMethod);
      }
    }

    return result;
  }

  /// matrix product
  Matrix prod(const Matrix& lop, const Matrix& rop)
  {
    return boost::numeric::ublas::prod(lop, rop);
  }

  /// vector product
  Vector prod(const Matrix& m, const Vector& v)
  {
    return boost::numeric::ublas::prod(m, v);
  }

  /// outer product
  Matrix outerProd(const Vector& lhs, const Vector& rhs)
  {
    return boost::numeric::ublas::outer_prod(lhs, rhs);
  }

  /// take the natural logarithm of a Matrix
  Matrix log(const Matrix& v)
  {
    unsigned M = v.size1();
    unsigned N = v.size2();
    Matrix result(M, N);
    for (unsigned i = 0; i < M; ++i){
      for (unsigned j = 0; j < N; ++j){
        result(i,j) = std::log(v(i,j));
      }
    }
    return result;
  }

  /// take the logarithm of a Matrix with certain base
  Matrix log(const Matrix& v, double base)
  {
    double logBase = std::log(base);
    unsigned M = v.size1();
    unsigned N = v.size2();
    Matrix result(M, N);
    for (unsigned i = 0; i < M; ++i){
      for (unsigned j = 0; j < N; ++j){
        result(i,j) = std::log(v(i,j)) / logBase;
      }
    }
    return result;
  }

  /// generates a Matrix of M*N points randomly drawn between and including a and b.
  Matrix randMatrix(double a, double b, unsigned M, unsigned N)
  {
    // ETH@20100120. What library does this come from? The user should be able to seed the
    // generator independently of this function.
    // seed random number generator
    static std::minstd_rand generator(42u);

    // define distribution
    boost::uniform_real<> dist(a,b);

    // create a generator
    boost::variate_generator<std::minstd_rand&, boost::uniform_real<> > uniformGenerator(generator, dist);

    // ETH@20120723 Started seeing this as DataFixture.Matrix_RandMatrix hanging on Windows 7,
    // with BoostPro installer.
    // handle degenerate case
    OptionalDouble singlePoint;
    if (equal(a,b)) {
      singlePoint = (a + b) / 2.0;
    }

    Matrix result(M, N);
    for (unsigned i = 0; i < M; ++i){
      for (unsigned j = 0; j < N; ++j){
        if (singlePoint) {
          result(i,j) = *singlePoint;
        }
        else {
          result(i,j) = uniformGenerator();
        }
      }
    }
    return result;
  }

  /// sum
  double sum(const Matrix& matrix)
  {
    double result = 0.0;
    for (unsigned i = 0; i < matrix.size1(); ++i){
      for (unsigned j = 0; j < matrix.size2(); ++j){
        result += matrix(i,j);
      }
    }
    return result;
  }

  /// maximum
  double maximum(const Matrix& matrix)
  {
    double max = 0;
    if ((matrix.size1() > 0) && (matrix.size2() > 0)){
      max = matrix(0,0);
      for (unsigned i = 0; i < matrix.size1(); ++i){
        for (unsigned j = 0; j < matrix.size2(); ++j){
          max = std::max(max, matrix(i,j));
        }
      }
    }
    return max;
  }

  /// minimum
  double minimum(const Matrix& matrix)
  {
    double min = 0;
    if ((matrix.size1() > 0) && (matrix.size2() > 0)){
      min = matrix(0,0);
      for (unsigned i = 0; i < matrix.size1(); ++i){
        for (unsigned j = 0; j < matrix.size2(); ++j){
          min = std::min(min, matrix(i,j));
        }
      }
    }
    return min;
  }

  /// mean
  double mean(const Matrix& matrix)
  {
    double avg = 0;
    unsigned N = matrix.size1()*matrix.size2();
    if (N > 0){
      avg = sum(matrix) / N;
    }
    return avg;
  }

  /// get the connected components from an NxN adjacency matrix (1.0 for i-j connected, 0.0 for i-j not connected)
  std::vector<std::vector<unsigned> > findConnectedComponents(const Matrix& matrix)
  {
    double tol = 0.001;

    std::vector<std::vector<unsigned> > result;

    unsigned N = matrix.size1();
    if (N != matrix.size2()){
      return result;
    }

    Matrix A(N, N, 0.0);
    for (unsigned i = 0; i < N; ++i){

      A(i,i) = 1.0; // must be self connected
      if ( std::abs(matrix(i,i) - 1.0) > tol){
        // warn
      }

      for (unsigned j = i+1; j < N; ++j){

        if (matrix(i,j) < 0){
          // warn
        }else if (matrix(i,j) > tol){
          A(i,j) = 1.0;
        }

        if (matrix(j,i) < 0){
          // warn
        }else if (matrix(j,i) > tol){
          A(j,i) = 1.0;
        }
      }
    }

    // raise A to the Nth power, maximum distance between two nodes
    for (unsigned i = 0; i < N; ++i){
      A = prod(A,A);
    }

    std::set<unsigned> added;
    for (unsigned i = 0; i < N; ++i){
      if (added.find(i) != added.end()){
        continue;
      }

      std::vector<unsigned> group;
      group.push_back(i);
      added.insert(i);

      for (unsigned j = i+1; j < N; ++j){
        if ((A(i,j) > 0) || (A(j,i) > 0)){
          group.push_back(j);
          added.insert(j);
        }
      }

      result.push_back(group);
    }

    return result;
  }

} // openstudio


#pragma once

#include "node.hpp"
#include "random.hpp"
#include "settings.hpp"
#include "data.hpp"
#include "MeanShift.hpp"
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include <vector>
#include <cstdint>
#include <ctime>
#include <queue>

#include <unordered_map>

namespace ISUE {
  namespace RelocForests {
    class Point3D {
    public:
      Point3D(double x, double y, double z) : x(x), y(y), z(z) {};
      double x, y, z;
    };

    struct hashFunc{
      size_t operator()(const Point3D &k) const {
        size_t h1 = std::hash<double>()(k.x);
        size_t h2 = std::hash<double>()(k.y);
        size_t h3 = std::hash<double>()(k.z);
        return (h1 ^ (h2 << 1)) ^ h3;
      }
    };

    struct equalsFunc {
      bool operator()(const Point3D &l, const Point3D &r) const{
        return (l.x == r.x) && (l.y == r.y) && (l.z == r.z);
      }
    };

    typedef std::unordered_map<Point3D, uint32_t, hashFunc, equalsFunc> Point3DMap;

    template <typename D, typename RGB>
    class Tree {
    public:
      Tree()
      {
        root_ = new Node<D, RGB>();
        root_->depth_ = 0;
      };

      ~Tree()
      {
        delete root_;
      };

      void WriteTree(std::ostream& o, Node<D, RGB> *node) const
      {
        if (node == nullptr) {
          o.write("#", sizeof('#'));
          return;
        }
        node->Serialize(o);
        WriteTree(o, node->left_);
        WriteTree(o, node->right_);
      }

      void Serialize(std::ostream& stream) const
      {
        const int majorVersion = 0, minorVersion = 0;

        stream.write(binaryFileHeader_, strlen(binaryFileHeader_));
        stream.write((const char*)(&majorVersion), sizeof(majorVersion));
        stream.write((const char*)(&minorVersion), sizeof(minorVersion));

        //stream.write((const char*)(&settings_->max_tree_depth_), sizeof(settings_->max_tree_depth_));

        WriteTree(stream, root_);
      }

      Node<D, RGB>* ReadTree(std::istream& i)
      {
        int flag = i.peek();
        char val = (char)flag;
        if (val == '#') {
          i.get();
          return nullptr;
        }
        Node<D, RGB> *tmp = new Node<D, RGB>();
        tmp->Deserialize(i);
        tmp->left_ = ReadTree(i);
        tmp->right_ = ReadTree(i);
        return tmp;
      }

      Tree* Deserialize(std::istream& stream)
      {
        settings_ = new Settings();

        std::vector<char> buffer(strlen(binaryFileHeader_) + 1);
        stream.read(&buffer[0], strlen(binaryFileHeader_));
        buffer[buffer.size() - 1] = '\0';
        if (strcmp(&buffer[0], binaryFileHeader_) != 0)
          throw std::runtime_error("Unsupported forest format.");

        const int majorVersion = 0, minorVersion = 0;
        stream.read((char*)(&majorVersion), sizeof(majorVersion));
        stream.read((char*)(&minorVersion), sizeof(minorVersion));

        root_ = ReadTree(stream);

      }

      bool IsValidRecurse(Node<D, RGB> *node, bool prevSplit)
      {

        if (!node && prevSplit)
          return false;

        if (node->is_leaf_)
          return true;

        return IsValidRecurse(node->left_, node->is_split_) && IsValidRecurse(node->right_, node->is_split_);
      }

      bool IsValid()
      {
        return IsValidRecurse(root_, true);
      }

      // learner output
      enum DECISION { LEFT, RIGHT, TRASH };

      //  Evaluates weak learner. Decides whether the point should go left or right.
      //  Returns DECISION enum value.
      DECISION eval_learner(DepthAdaptiveRGB<D, RGB> feature, cv::Mat depth_image, cv::Mat rgb_image, cv::Point2i pos)
      {
        bool valid = true;
        float response = feature.GetResponse(depth_image, rgb_image, pos, *settings_, valid);

        if (!valid) // no depth or out of bounds
          return DECISION::TRASH;

        return (DECISION)(response >= feature.GetThreshold());
      }

      // V(S)
      double variance(std::vector<LabeledPixel> labeled_data)
      {
        if (labeled_data.size() == 0)
          return 0.0;
        double V = (1.0f / (double)labeled_data.size());
        double sum = 0.0f;

        // calculate mean of S
        cv::Point3f tmp;
        for (auto p : labeled_data)
          tmp += p.label_;
        uint32_t size = labeled_data.size();
        cv::Point3f mean(tmp.x / size, tmp.y / size, tmp.z / size);

        for (auto p : labeled_data) {
          cv::Point3f val = (p.label_ - mean);
          sum += val.x * val.x  + val.y * val.y + val.z * val.z;
        }

        return V * sum;
      }

      // Q(S_n, \theta)
      double objective_function(std::vector<LabeledPixel> data, std::vector<LabeledPixel> left, std::vector<LabeledPixel> right)
      {
        double var = variance(data);
        double left_val = ((double)left.size() / (double)data.size()) * variance(left);
        double right_val = ((double)right.size() / (double)data.size()) * variance(right);

        return var - (left_val + right_val);
      }

      Eigen::Vector3d GetLeafMode(std::vector<LabeledPixel> S)
      {
        std::vector<Eigen::Vector3d> data;

        // calc mode for leaf, sub-sample N_SS = 500
        for (uint16_t i = 0; i < (S.size() < 500 ? S.size() : 500); i++) {
          auto p = S.at(i);
          Eigen::Vector3d point{ p.label_.x, p.label_.y, p.label_.z };
          data.push_back(point);
        }

        // cluster
        MeanShift ms = MeanShift(nullptr);
        double kernel_bandwidth = 0.01f; // gaussian
        std::vector<Eigen::Vector3d> cluster = ms.cluster(data, kernel_bandwidth);

        // find mode
        std::vector<Point3D> clustered_points;
        for (auto c : cluster)
          clustered_points.push_back(Point3D(floor(c[0] * 10000) / 10000,
                                             floor(c[1] * 10000) / 10000,
                                             floor(c[2] * 10000) / 10000));

        Point3DMap cluster_map;

        for (auto p : clustered_points)
          cluster_map[p]++;

        std::pair<Point3D, uint32_t> mode(Point3D(0.0, 0.0, 0.0), 0);

        for (auto p : cluster_map)
          if (p.second > mode.second)
            mode = p;

        return Eigen::Vector3d(mode.first.x, mode.first.y, mode.first.z);
      }

      void train_recurse(Node<D, RGB> *node, std::vector<LabeledPixel> S)
      {
        uint16_t height = node->depth_;
        if (S.size() == 1 || ((height == settings_->max_tree_depth_ - 1) && S.size() >= 1)) {

          node->mode_ = GetLeafMode(S);
          node->is_leaf_ = true;
          node->left_ = nullptr;
          node->right_ = nullptr;
          return;
        }

        uint32_t num_candidates = 5,
                feature = 0;
        double minimum_objective = DBL_MAX;

        std::vector<DepthAdaptiveRGB<D, RGB> > candidate_params;
        std::vector<LabeledPixel> left_final, right_final;

        for (uint32_t i = 0; i < num_candidates; ++i) {

          // add candidate
          candidate_params.push_back(DepthAdaptiveRGB<D, RGB>::CreateRandom(random_));

          // partition data with candidate
          std::vector<LabeledPixel> left_data, right_data;

          for (uint32_t j = 0; j < S.size(); ++j) {

            LabeledPixel p = S.at(j);
            DECISION val = eval_learner(candidate_params.at(i), data_->GetDepthImage(p.frame_), data_->GetRGBImage(p.frame_), p.pos_);

            switch (val) {
            case LEFT:
              left_data.push_back(S.at(j));
              break;
            case RIGHT:
              right_data.push_back(S.at(j));
              break;
            case TRASH:
              // do nothing
              break;
            }

          }

          // eval tree training objective function and take best
          // todo: ensure objective function is correct
          double objective = objective_function(S, left_data, right_data);

          if (objective < minimum_objective) {
            feature = i;
            minimum_objective = objective;
            left_final = left_data;
            right_final = right_data;
          }
        }

        // split went only one way
        if (left_final.empty()) {
          node->mode_ = GetLeafMode(right_final);
          node->is_leaf_ = true;
          node->left_ = nullptr;
          node->right_ = nullptr;
          return;
        }
        if (right_final.empty()) {
          node->mode_ = GetLeafMode(left_final);
          node->is_leaf_ = true;
          node->left_ = nullptr;
          node->right_ = nullptr;
          return;
        }

        // set feature
        node->is_split_ = true;
        node->is_leaf_ = false;
        node->feature_ = candidate_params.at(feature);
        node->left_ = new Node<D, RGB>();
        node->right_ = new Node<D, RGB>();
        node->left_->depth_ = node->right_->depth_ = node->depth_ + 1;

        train_recurse(node->left_, left_final);
        train_recurse(node->right_, right_final);
      }

      void Train(Data *data, std::vector<LabeledPixel> labeled_data, Random *random, Settings *settings)
      {
        data_ = data;
        random_ = random;
        settings_ = settings;
        train_recurse(root_, labeled_data);
      }

      Eigen::Vector3d eval_recursive(Node<D, RGB> **node, int row, int col, cv::Mat rgb_image, cv::Mat depth_image, bool &valid)
      {
        if ((*node)->is_leaf_) {
          return (*node)->mode_;
        }

        DECISION val = eval_learner((*node)->feature_, depth_image, rgb_image, cv::Point2i(col, row));

        switch (val) {
        case LEFT:
          return eval_recursive(&(*node)->left_, row, col, rgb_image, depth_image, valid);
          break;
        case RIGHT:
          return eval_recursive(&(*node)->right_, row, col, rgb_image, depth_image, valid);
          break;
        case TRASH:
          valid = false;
          break;
        }
      }

      // Evaluate tree at a pixel
      Eigen::Vector3d Eval(int row, int col, cv::Mat rgb_image, cv::Mat depth_image, bool &valid)
      {
        auto m = eval_recursive(&root_, row, col, rgb_image, depth_image, valid);
        return m;
      }

    private:
      Node<D, RGB> *root_;
      Data *data_;
      Random *random_;
      Settings *settings_;
      const char* binaryFileHeader_ = "ISUE.RelocForests.Tree";
    };
  }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Jose Aparicio
 Copyright (C) 2008 Chris Kenyon
 Copyright (C) 2008 Roland Lichters
 Copyright (C) 2008 StatPro Italia srl
 Copyright (C) 2009 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file hazardratestructure.hpp
    \brief hazard-rate term structure
*/

#ifndef quantlib_hazard_rate_structure_hpp
#define quantlib_hazard_rate_structure_hpp

#include <ql/termstructures/defaulttermstructure.hpp>

namespace QuantLib {

    //! Hazard-rate term structure
    /*! This abstract class acts as an adapter to
        DefaultProbabilityTermStructure allowing the programmer to implement
        only the <tt>hazardRateImpl(Time)</tt> method in derived classes.

        Survival/default probabilities and default densities are calculated
        from hazard rates.

        Hazard rates are defined with annual frequency and continuous
        compounding.

        \ingroup defaultprobabilitytermstructures
    */
    class HazardRateStructure : public DefaultProbabilityTermStructure {
      public:
        /*! \name Constructors
            See the TermStructure documentation for issues regarding
            constructors.
        */
        //@{
        HazardRateStructure(
            const DayCounter& dayCounter = DayCounter(),
            const std::vector<Handle<Quote> >& jumps = std::vector<Handle<Quote> >(),
            const std::vector<Date>& jumpDates = std::vector<Date>());
        HazardRateStructure(
            const Date& referenceDate,
            const Calendar& cal = Calendar(),
            const DayCounter& dayCounter = DayCounter(),
            const std::vector<Handle<Quote> >& jumps = std::vector<Handle<Quote> >(),
            const std::vector<Date>& jumpDates = std::vector<Date>());
        HazardRateStructure(
            Natural settlementDays,
            const Calendar& cal,
            const DayCounter& dayCounter = DayCounter(),
            const std::vector<Handle<Quote> >& jumps = std::vector<Handle<Quote> >(),
            const std::vector<Date>& jumpDates = std::vector<Date>());
        //@}
      protected:
        /*! \name Calculations

            This method must be implemented in derived classes to
            perform the actual calculations. When it is called,
            range check has already been performed; therefore, it
            must assume that extrapolation is required.
        */
        //@{
        //! hazard rate calculation
        virtual Real hazardRateImpl(Time) const = 0;
        //@}

        //! \name DefaultProbabilityTermStructure implementation
        //@{
        /*! survival probability calculation
            implemented in terms of the hazard rate \f$ h(t) \f$ as
            \f[
            S(t) = \exp\left( - \int_0^t h(\tau) d\tau \right).
            \f]

            \warning This default implementation uses numerical integration,
                     which might be inefficient and inaccurate.
                     Derived classes should override it if a more efficient
                     implementation is available.
        */
        Probability survivalProbabilityImpl(Time) const;
        //! default density calculation
        Real defaultDensityImpl(Time) const;
        //@}
    };

    // inline definitions

    inline Real HazardRateStructure::defaultDensityImpl(Time t) const {
        return hazardRateImpl(t)*survivalProbabilityImpl(t);
    }

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Jose Aparicio
 Copyright (C) 2008 Chris Kenyon
 Copyright (C) 2008 Roland Lichters
 Copyright (C) 2008 StatPro Italia srl
 Copyright (C) 2009 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/math/integrals/gaussianquadratures.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

using namespace boost;

namespace QuantLib {

    namespace {

        template <class F>
        struct remapper {
            F f;
            Time T;
            remapper(const F& f, Time T) : f(f), T(T) {}
            // This remaps [-1,1] to [0,T]. No differential included.
            Real operator()(Real x) const {
                const Real arg = (x+1.0)*T/2.0;
                return f(arg);
            }
        };

        template <class F>
        remapper<F> remap(const F& f, Time T) {
            return remapper<F>(f,T);
        }

    }

    inline HazardRateStructure::HazardRateStructure(
                                    const DayCounter& dc,
                                    const std::vector<Handle<Quote> >& jumps,
                                    const std::vector<Date>& jumpDates)
    : DefaultProbabilityTermStructure(dc, jumps, jumpDates) {}

    inline HazardRateStructure::HazardRateStructure(
                                    const Date& refDate,
                                    const Calendar& cal,
                                    const DayCounter& dc,
                                    const std::vector<Handle<Quote> >& jumps,
                                    const std::vector<Date>& jumpDates)
    : DefaultProbabilityTermStructure(refDate, cal, dc, jumps, jumpDates) {}

    inline HazardRateStructure::HazardRateStructure(
                                    Natural settlDays,
                                    const Calendar& cal,
                                    const DayCounter& dc,
                                    const std::vector<Handle<Quote> >& jumps,
                                    const std::vector<Date>& jumpDates)
    : DefaultProbabilityTermStructure(settlDays, cal, dc, jumps, jumpDates) {}

    inline Probability HazardRateStructure::survivalProbabilityImpl(Time t) const {
        static GaussChebyshevIntegration integral(48);
        // this stores the address of the method to integrate (so that
        // we don't have to insert its full expression inside the
        // integral below--it's long enough already)
        Real (HazardRateStructure::*f)(Time) const =
            &HazardRateStructure::hazardRateImpl;
        // the Gauss-Chebyshev quadratures integrate over [-1,1],
        // hence the remapping (and the Jacobian term t/2)
        return std::exp(-integral(remap(bind(f,this,_1), t)) * t/2.0);
    }

}

#endif


/* [auto_generated]
 boost/numeric/odeint/stepper/controlled_runge_kutta.hpp

 [begin_description]
 The default controlled stepper which can be used with all explicit Runge-Kutta error steppers.
 [end_description]

 Copyright 2010-2013 Karsten Ahnert
 Copyright 2010-2015 Mario Mulansky
 Copyright 2012 Christoph Koke

 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 BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED

#include <cmath>

#include <boost/config.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/copy.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>

#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template
<
class Value ,
class Algebra ,
class Operations
>
class default_error_checker
{
public:

    typedef Value value_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;

    default_error_checker(
            value_type eps_abs = static_cast< value_type >( 1.0e-6 ) ,
            value_type eps_rel = static_cast< value_type >( 1.0e-6 ) ,
            value_type a_x = static_cast< value_type >( 1 ) ,
            value_type a_dxdt = static_cast< value_type >( 1 ))
        : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt )
    { }

    template< class State , class Deriv , class Err, class Time >
    value_type error( const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
    {
        return error( algebra_type() , x_old , dxdt_old , x_err , dt );
    }

    template< class State , class Deriv , class Err, class Time >
    value_type error( algebra_type &algebra , const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
    {
        using std::abs;
        // this overwrites x_err !
        algebra.for_each3( x_err , x_old , dxdt_old ,
                typename operations_type::template rel_error< value_type >( m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt * abs(get_unit_value( dt )) ) );

        // value_type res = algebra.reduce( x_err ,
        //        typename operations_type::template maximum< value_type >() , static_cast< value_type >( 0 ) );
        return algebra.norm_inf( x_err );
    }

private:

    value_type m_eps_abs;
    value_type m_eps_rel;
    value_type m_a_x;
    value_type m_a_dxdt;

};

template< typename Value, typename Time >
class default_step_adjuster
{
public:
    typedef Time time_type;
    typedef Value value_type;

    default_step_adjuster(const time_type max_dt=static_cast<time_type>(0))
            : m_max_dt(max_dt)
    {}

    time_type decrease_step(time_type dt, const value_type error, const int error_order) const
    {
        // returns the decreased time step
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        dt *= max
        BOOST_PREVENT_MACRO_SUBSTITUTION(
                static_cast<value_type>( static_cast<value_type>(9) / static_cast<value_type>(10) *
                                         pow(error, static_cast<value_type>(-1) / (error_order - 1))),
                static_cast<value_type>( static_cast<value_type>(1) / static_cast<value_type> (5)));
        if(m_max_dt != static_cast<time_type >(0))
            // limit to maximal stepsize even when decreasing
            dt = detail::min_abs(dt, m_max_dt);
        return dt;
    }

    time_type increase_step(time_type dt, value_type error, const int stepper_order) const
    {
        // returns the increased time step
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        // adjust the size if dt is smaller than max_dt (providede max_dt is not zero)
        if(error < 0.5)
        {
            // error should be > 0
            error = max BOOST_PREVENT_MACRO_SUBSTITUTION (
                    static_cast<value_type>( pow( static_cast<value_type>(5.0) , -static_cast<value_type>(stepper_order) ) ) ,
                    error);
            // time_type dt_old = dt;   unused variable warning
            //error too small - increase dt and keep the evolution and limit scaling factor to 5.0
            dt *= static_cast<value_type>(9)/static_cast<value_type>(10) *
                  pow(error, static_cast<value_type>(-1) / stepper_order);
            if(m_max_dt != static_cast<time_type >(0))
                // limit to maximal stepsize
                dt = detail::min_abs(dt, m_max_dt);
        }
        return dt;
    }

    bool check_step_size_limit(const time_type dt)
    {
        if(m_max_dt != static_cast<time_type >(0))
            return detail::less_eq_with_sign(dt, m_max_dt, dt);
        return true;
    }

    time_type get_max_dt() { return m_max_dt; }

private:
    time_type m_max_dt;
};

/*
 * error stepper category dispatcher
 */
template<
class ErrorStepper ,
class ErrorChecker = default_error_checker< typename ErrorStepper::value_type ,
    typename ErrorStepper::algebra_type ,
    typename ErrorStepper::operations_type > ,
class StepAdjuster = default_step_adjuster< typename ErrorStepper::value_type ,
    typename ErrorStepper::time_type > ,
class Resizer = typename ErrorStepper::resizer_type ,
class ErrorStepperCategory = typename ErrorStepper::stepper_category
>
class controlled_runge_kutta ;

/*
 * explicit stepper version
 *
 * this class introduces the following try_step overloads
    * try_step( sys , x , t , dt )
    * try_step( sys , x , dxdt , t , dt )
    * try_step( sys , in , t , out , dt )
    * try_step( sys , in , dxdt , t , out , dt )
 */
/**
 * \brief Implements step size control for Runge-Kutta steppers with error
 * estimation.
 *
 * This class implements the step size control for standard Runge-Kutta
 * steppers with error estimation.
 *
 * \tparam ErrorStepper The stepper type with error estimation, has to fulfill the ErrorStepper concept.
 * \tparam ErrorChecker The error checker
 * \tparam Resizer The resizer policy type.
 */
template<
class ErrorStepper,
class ErrorChecker,
class StepAdjuster,
class Resizer
>
class controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster, Resizer ,
        explicit_error_stepper_tag >
{

public:

    typedef ErrorStepper stepper_type;
    typedef typename stepper_type::state_type state_type;
    typedef typename stepper_type::value_type value_type;
    typedef typename stepper_type::deriv_type deriv_type;
    typedef typename stepper_type::time_type time_type;
    typedef typename stepper_type::algebra_type algebra_type;
    typedef typename stepper_type::operations_type operations_type;
    typedef Resizer resizer_type;
    typedef ErrorChecker error_checker_type;
    typedef StepAdjuster step_adjuster_type;
    typedef explicit_controlled_stepper_tag stepper_category;

#ifndef DOXYGEN_SKIP
    typedef typename stepper_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;

    typedef controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster ,
            Resizer , explicit_error_stepper_tag > controlled_stepper_type;
#endif //DOXYGEN_SKIP

    /**
     * \brief Constructs the controlled Runge-Kutta stepper.
     * \param error_checker An instance of the error checker.
     * \param stepper An instance of the underlying stepper.
     */
    controlled_runge_kutta(
            const error_checker_type &error_checker = error_checker_type( ) ,
            const step_adjuster_type &step_adjuster = step_adjuster_type() ,
            const stepper_type &stepper = stepper_type( )
    )
        : m_stepper(stepper), m_error_checker(error_checker) , m_step_adjuster(step_adjuster)
    { }

    /*
     * Version 1 : try_step( sys , x , t , dt )
     *
     * The overloads are needed to solve the forwarding problem
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /**
     * \brief Tries to perform one step. Solves the forwarding problem and
     * allows for using boost range as state_type.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful. Can be a boost range.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /*
     * Version 2 : try_step( sys , x , dxdt , t , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut , class DerivIn >
    controlled_step_result try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
    {
        m_xnew_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_xnew_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , dt );
        if( res == success )
        {
            boost::numeric::odeint::copy( m_xnew.m_v , x );
        }
        return res;
    }

    /*
     * Version 3 : try_step( sys , in , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     *
     * the disable is needed to avoid ambiguous overloads if state_type = time_type
     */
    /**
     * \brief Tries to perform one step.
     *
     * \note This method is disabled if state_type=time_type to avoid ambiguity.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class StateOut >
    typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
    try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
        sys( in , m_dxdt.m_v , t );
        return try_step( system , in , m_dxdt.m_v , t , out , dt );
    }

    /*
     * Version 4 : try_step( sys , in , dxdt , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class DerivIn , class StateOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
    {
        if( !m_step_adjuster.check_step_size_limit(dt) )
        {
            // given dt was above step size limit - adjust and return fail;
            dt = m_step_adjuster.get_max_dt();
            return fail;
        }

        m_xerr_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_xerr_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        // do one step with error calculation
        m_stepper.do_step( system , in , dxdt , t , out , dt , m_xerr.m_v );

        value_type max_rel_err = m_error_checker.error( m_stepper.algebra() , in , dxdt , m_xerr.m_v , dt );

        if( max_rel_err > 1.0 )
        {
            // error too big, decrease step size and reject this step
            dt = m_step_adjuster.decrease_step(dt, max_rel_err, m_stepper.error_order());
            return fail;
        } else
        {
            // otherwise, increase step size and accept
            t += dt;
            dt = m_step_adjuster.increase_step(dt, max_rel_err, m_stepper.stepper_order());
            return success;
        }
    }

    /**
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */
    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_m_xerr_impl( x );
        resize_m_dxdt_impl( x );
        resize_m_xnew_impl( x );
        m_stepper.adjust_size( x );
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    stepper_type& stepper( void )
    {
        return m_stepper;
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    const stepper_type& stepper( void ) const
    {
        return m_stepper;
    }

private:

    template< class System , class StateInOut >
    controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        sys( x , m_dxdt.m_v ,t );
        return try_step( system , x , m_dxdt.m_v , t , dt );
    }

    template< class StateIn >
    bool resize_m_xerr_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xerr , x , typename is_resizeable<state_type>::type() );
    }

    template< class StateIn >
    bool resize_m_dxdt_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_xnew_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
    }

    stepper_type m_stepper;
    error_checker_type m_error_checker;
    step_adjuster_type m_step_adjuster;

    resizer_type m_dxdt_resizer;
    resizer_type m_xerr_resizer;
    resizer_type m_xnew_resizer;

    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_xerr;
    wrapped_state_type m_xnew;
};

/*
 * explicit stepper fsal version
 *
 * the class introduces the following try_step overloads
    * try_step( sys , x , t , dt )
    * try_step( sys , in , t , out , dt )
    * try_step( sys , x , dxdt , t , dt )
    * try_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
 */
/**
 * \brief Implements step size control for Runge-Kutta FSAL steppers with
 * error estimation.
 *
 * This class implements the step size control for FSAL Runge-Kutta
 * steppers with error estimation.
 *
 * \tparam ErrorStepper The stepper type with error estimation, has to fulfill the ErrorStepper concept.
 * \tparam ErrorChecker The error checker
 * \tparam Resizer The resizer policy type.
 */
template<
class ErrorStepper ,
class ErrorChecker ,
class StepAdjuster ,
class Resizer
>
class controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster , Resizer , explicit_error_stepper_fsal_tag >
{

public:

    typedef ErrorStepper stepper_type;
    typedef typename stepper_type::state_type state_type;
    typedef typename stepper_type::value_type value_type;
    typedef typename stepper_type::deriv_type deriv_type;
    typedef typename stepper_type::time_type time_type;
    typedef typename stepper_type::algebra_type algebra_type;
    typedef typename stepper_type::operations_type operations_type;
    typedef Resizer resizer_type;
    typedef ErrorChecker error_checker_type;
    typedef StepAdjuster step_adjuster_type;
    typedef explicit_controlled_stepper_fsal_tag stepper_category;

#ifndef DOXYGEN_SKIP
    typedef typename stepper_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;

    typedef controlled_runge_kutta< ErrorStepper , ErrorChecker , StepAdjuster , Resizer , explicit_error_stepper_tag > controlled_stepper_type;
#endif // DOXYGEN_SKIP

    /**
     * \brief Constructs the controlled Runge-Kutta stepper.
     * \param error_checker An instance of the error checker.
     * \param stepper An instance of the underlying stepper.
     */
    controlled_runge_kutta(
            const error_checker_type &error_checker = error_checker_type() ,
            const step_adjuster_type &step_adjuster = step_adjuster_type() ,
            const stepper_type &stepper = stepper_type()
    )
    : m_stepper( stepper ) , m_error_checker( error_checker ) , m_step_adjuster(step_adjuster) ,
      m_first_call( true )
    { }

    /*
     * Version 1 : try_step( sys , x , t , dt )
     *
     * The two overloads are needed in order to solve the forwarding problem
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t , dt );
    }

    /**
     * \brief Tries to perform one step. Solves the forwarding problem and
     * allows for using boost range as state_type.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful. Can be a boost range.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t , dt );
    }

    /*
     * Version 2 : try_step( sys , in , t , out , dt );
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     *
     * The disabler is needed to solve ambiguous overloads
     */
    /**
     * \brief Tries to perform one step.
     *
     * \note This method is disabled if state_type=time_type to avoid ambiguity.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class StateOut >
    typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
    try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
    {
        if( m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
        {
            initialize( system , in , t );
        }
        return try_step( system , in , m_dxdt.m_v , t , out , dt );
    }

    /*
     * Version 3 : try_step( sys , x , dxdt , t , dt )
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateInOut , class DerivInOut >
    controlled_step_result try_step( System system , StateInOut &x , DerivInOut &dxdt , time_type &t , time_type &dt )
    {
        m_xnew_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_xnew_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        m_dxdt_new_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_new_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , m_dxdtnew.m_v , dt );
        if( res == success )
        {
            boost::numeric::odeint::copy( m_xnew.m_v , x );
            boost::numeric::odeint::copy( m_dxdtnew.m_v , dxdt );
        }
        return res;
    }

    /*
     * Version 4 : try_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     */
    /**
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt_in , time_type &t ,
            StateOut &out , DerivOut &dxdt_out , time_type &dt )
    {
        if( !m_step_adjuster.check_step_size_limit(dt) )
        {
            // given dt was above step size limit - adjust and return fail;
            dt = m_step_adjuster.get_max_dt();
            return fail;
        }

        m_xerr_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_xerr_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        //fsal: m_stepper.get_dxdt( dxdt );
        //fsal: m_stepper.do_step( sys , x , dxdt , t , dt , m_x_err );
        m_stepper.do_step( system , in , dxdt_in , t , out , dxdt_out , dt , m_xerr.m_v );

        // this potentially overwrites m_x_err! (standard_error_checker does, at least)
        value_type max_rel_err = m_error_checker.error( m_stepper.algebra() , in , dxdt_in , m_xerr.m_v , dt );

        if( max_rel_err > 1.0 )
        {
            // error too big, decrease step size and reject this step
            dt = m_step_adjuster.decrease_step(dt, max_rel_err, m_stepper.error_order());
            return fail;
        }
        // otherwise, increase step size and accept
        t += dt;
        dt = m_step_adjuster.increase_step(dt, max_rel_err, m_stepper.stepper_order());
        return success;
    }

    /**
     * \brief Resets the internal state of the underlying FSAL stepper.
     */
    void reset( void )
    {
        m_first_call = true;
    }

    /**
     * \brief Initializes the internal state storing an internal copy of the derivative.
     *
     * \param deriv The initial derivative of the ODE.
     */
    template< class DerivIn >
    void initialize( const DerivIn &deriv )
    {
        boost::numeric::odeint::copy( deriv , m_dxdt.m_v );
        m_first_call = false;
    }

    /**
     * \brief Initializes the internal state storing an internal copy of the derivative.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The initial state of the ODE which should be solved.
     * \param t The initial time.
     */
    template< class System , class StateIn >
    void initialize( System system , const StateIn &x , time_type t )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        sys( x , m_dxdt.m_v , t );
        m_first_call = false;
    }

    /**
     * \brief Returns true if the stepper has been initialized, false otherwise.
     *
     * \return true, if the stepper has been initialized, false otherwise.
     */
    bool is_initialized( void ) const
    {
        return ! m_first_call;
    }

    /**
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */
    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_m_xerr_impl( x );
        resize_m_dxdt_impl( x );
        resize_m_dxdt_new_impl( x );
        resize_m_xnew_impl( x );
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    stepper_type& stepper( void )
    {
        return m_stepper;
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    const stepper_type& stepper( void ) const
    {
        return m_stepper;
    }

private:

    template< class StateIn >
    bool resize_m_xerr_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xerr , x , typename is_resizeable<state_type>::type() );
    }

    template< class StateIn >
    bool resize_m_dxdt_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_dxdt_new_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdtnew , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_xnew_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
    }

    template< class System , class StateInOut >
    controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        if( m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
        {
            initialize( system , x , t );
        }
        return try_step( system , x , m_dxdt.m_v , t , dt );
    }

    stepper_type m_stepper;
    error_checker_type m_error_checker;
    step_adjuster_type m_step_adjuster;

    resizer_type m_dxdt_resizer;
    resizer_type m_xerr_resizer;
    resizer_type m_xnew_resizer;
    resizer_type m_dxdt_new_resizer;

    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_xerr;
    wrapped_state_type m_xnew;
    wrapped_deriv_type m_dxdtnew;
    bool m_first_call;
};

/********** DOXYGEN **********/

/**** DEFAULT ERROR CHECKER ****/

/**
 * \class default_error_checker
 * \brief The default error checker to be used with Runge-Kutta error steppers
 *
 * This class provides the default mechanism to compare the error estimates
 * reported by Runge-Kutta error steppers with user defined error bounds.
 * It is used by the controlled_runge_kutta steppers.
 *
 * \tparam Value The value type.
 * \tparam Time The time type.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 */

    /**
     * \fn default_error_checker( value_type eps_abs , value_type eps_rel , value_type a_x , value_type a_dxdt ,
     * time_type max_dt)
     * \brief Constructs the error checker.
     *
     * The error is calculated as follows: ????
     *
     * \param eps_abs Absolute tolerance level.
     * \param eps_rel Relative tolerance level.
     * \param a_x Factor for the weight of the state.
     * \param a_dxdt Factor for the weight of the derivative.
     * \param max_dt Maximum allowed step size.
     */

    /**
     * \fn error( const State &x_old , const Deriv &dxdt_old , Err &x_err , time_type dt ) const
     * \brief Calculates the error level.
     *
     * If the returned error level is greater than 1, the estimated error was
     * larger than the permitted error bounds and the step should be repeated
     * with a smaller step size.
     *
     * \param x_old State at the beginning of the step.
     * \param dxdt_old Derivative at the beginning of the step.
     * \param x_err Error estimate.
     * \param dt Time step.
     * \return error
     */

    /**
     * \fn error( algebra_type &algebra , const State &x_old , const Deriv &dxdt_old , Err &x_err , time_type dt ) const
     * \brief Calculates the error level using a given algebra.
     *
     * If the returned error level is greater than 1, the estimated error was
     * larger than the permitted error bounds and the step should be repeated
     * with a smaller step size.
     *
     * \param algebra The algebra used for calculation of the error.
     * \param x_old State at the beginning of the step.
     * \param dxdt_old Derivative at the beginning of the step.
     * \param x_err Error estimate.
     * \param dt Time step.
     * \return error
     */

    /**
     * \fn time_type decrease_step(const time_type dt, const value_type error, const int error_order)
     * \brief Returns a decreased step size based on the given error and order
     *
     * Calculates a new smaller step size based on the given error and its order.
     *
     * \param dt The old step size.
     * \param error The computed error estimate.
     * \param error_order The error order of the stepper.
     * \return dt_new The new, reduced step size.
     */

    /**
     * \fn time_type increase_step(const time_type dt, const value_type error, const int error_order)
     * \brief Returns an increased step size based on the given error and order.
     *
     * Calculates a new bigger step size based on the given error and its order. If max_dt != 0, the
     * new step size is limited to max_dt.
     *
     * \param dt The old step size.
     * \param error The computed error estimate.
     * \param error_order The order of the stepper.
     * \return dt_new The new, increased step size.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED