/** em.cc                                                          -*- C++ -*-
    Mathieu Marquis Bolduc, October 28th, 2015
    Copyright (c) 2015 mldb.ai inc.  All rights reserved.

    This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

    Guassian clustering procedure and functions.
*/

#include "em_interface.h"
#include "mldb/plugins/jml/em.h"
#include "mldb/builtin/matrix.h"
#include "mldb/core/mldb_engine.h"
#include "mldb/engine/procedure_collection.h"
#include "mldb/engine/function_collection.h"
#include "mldb/utils/distribution.h"
#include <boost/multi_array.hpp>
#include "base/parallel.h"
#include "mldb/utils/pair_utils.h"
#include "mldb/arch/timers.h"
#include "mldb/types/optional_description.h"
#include "mldb/utils/vector_utils.h"
#include "mldb/types/basic_value_descriptions.h"
#include "mldb/sql/sql_expression.h"
#include "mldb/core/analytics.h"
#include "mldb/types/any_impl.h"
#include "mldb/utils/smart_ptr_utils.h"
#include "mldb/vfs/fs_utils.h"
#include "mldb/builtin/sql_config_validator.h"
#include "mldb/utils/log.h"

using namespace std;

namespace MLDB {

std::vector<double> tovector(boost::multi_array<double, 2>& m)
{
    std::vector<double> embedding;
    for(int i = 0; i < m.shape()[0]; i++)
    {
        for(int j = 0; j < m.shape()[1]; j++) {
             embedding.push_back(m[i][j]); // multiply by elements on diagonal
        }
    }

    return embedding;
}

DEFINE_STRUCTURE_DESCRIPTION(EMConfig);

EMConfigDescription::
EMConfigDescription()
{
    Optional<PolyConfigT<Dataset> > optional;
    optional.emplace(PolyConfigT<Dataset>().
                     withType(EMConfig::defaultOutputDatasetType));

    addField("trainingData", &EMConfig::trainingData,
             "Specification of the data for input to the procedure.  This should be "
             "organized as an embedding, with each selected row containing the same "
             "set of columns with numeric values to be used as coordinates.  The select statement "
             "does not support groupby and having clauses.");
    addField("outputDataset", &EMConfig::output,
             "Dataset for cluster assignment.  This dataset will contain the same "
             "row names as the input dataset, but the coordinates will be replaced "
             "by a single column giving the cluster number that the row was assigned to.",
              optional);
    addField("centroidsDataset", &EMConfig::centroids,
             "Dataset in which the centroids will be recorded.  This dataset will "
             "have the same coordinates (columns) as those selected from the input "
             "dataset, but will have one row per cluster, providing the centroid of "
             "the cluster.",
             PolyConfigT<Dataset>().withType("embedding"));
    addField("modelFileUrl", &EMConfig::modelFileUrl,
             "URL where the model file (with extension '.gs') should be saved. "
             "This file can be loaded by a function of type 'gaussianclustering' to apply "
             "the trained model to new data. "
             "If someone is only interested in how the training input is clustered "
             "then the parameter can be omitted and the outputDataset param can "
             "be provided instead.");
    addField("numInputDimensions", &EMConfig::numInputDimensions,
             "Number of dimensions from the input to use (-1 = all).  This limits "
             "the number of columns used.  Columns will be ordered alphabetically "
             "and the lowest ones kept.",
             -1);
    addField("numClusters", &EMConfig::numClusters,
             "Number of clusters to create.  This will provide the total number of "
             "centroids created.  There must be at least as many rows selected as "
             "clusters.", 10);
    addField("maxIterations", &EMConfig::maxIterations,
             "Maximum number of iterations to perform.  If no convergance is "
             "reached within this number of iterations, the current clustering "
             "will be returned.", 100);
    addField("functionName", &EMConfig::functionName,
             "If specified, a function of this name will be created using "
             "the training result.");
    addParent<ProcedureConfig>();

    onPostValidate = validateFunction<EMConfig>();
}

/*****************************************************************************/
/* EM PROCEDURE                                                           */
/*****************************************************************************/

EMProcedure::
EMProcedure(MldbEngine * owner,
               PolyConfig config,
               const std::function<bool (const Json::Value &)> & onProgress)
    : Procedure(owner)
{
    this->emConfig = config.params.convert<EMConfig>();
}

Any
EMProcedure::
getStatus() const
{
    return Any();

}

RunOutput
EMProcedure::
run(const ProcedureRunConfig & run,
    const std::function<bool (const Json::Value &)> & onProgress) const
{
    auto runProcConf = applyRunConfOverProcConf(emConfig, run);

    auto onProgress2 = [&] (const Json::Value & progress)
        {
            Json::Value value;
            value["dataset"] = progress;
            return onProgress(value);
        };

    if (!runProcConf.modelFileUrl.empty()) {
        checkWritability(runProcConf.modelFileUrl.toDecodedString(),
                         "modelFileUrl");
    }

    SqlExpressionMldbScope context(engine);

    ConvertProgressToJson convertProgressToJson(onProgress);
    auto embeddingOutput = getEmbedding(*runProcConf.trainingData.stm,
                                        context,
                                        runProcConf.numInputDimensions,
                                        convertProgressToJson);

    auto rows = embeddingOutput.first;
    std::vector<KnownColumn> & vars = embeddingOutput.second;

    std::vector<distribution<double> > vecs;

    for (unsigned i = 0;  i < rows.size();  ++i) {
        vecs.emplace_back(distribution<double>(std::get<2>(rows[i]).begin(),
                                                   std::get<2>(rows[i]).end()));
    }

    if (vecs.size() == 0)
        throw AnnotatedException(400, "Gaussian clustering training requires at least 1 datapoint. "
                                  "Make sure your dataset is not empty and that your WHERE expression "
                                  "does not filter all the rows");

    ML::EstimationMaximisation em;
    vector<int> inCluster;

    int numClusters = emConfig.numClusters;
    int numIterations = emConfig.maxIterations;

    DEBUG_MSG(logger) << "EM training start";
    em.train(vecs, inCluster, numClusters, numIterations, 0);
    DEBUG_MSG(logger) << "EM training end";

    // Let the model know about its column names
    std::vector<ColumnPath> columnNames;
    for (auto & v: vars) {
        columnNames.push_back(v.columnName);
        em.columnNames.push_back(v.columnName.toUtf8String());
    }

    // output

    bool saved = false;
    if (!runProcConf.modelFileUrl.empty()) {
        try {
            makeUriDirectory(
                runProcConf.modelFileUrl.toDecodedString());
            em.save(runProcConf.modelFileUrl.toDecodedString());
            saved = true;
        }
        catch (const std::exception & exc) {
            throw AnnotatedException(400, "Error saving gaussian clustering model at location'" +
                                      runProcConf.modelFileUrl.toString() + "': " +
                                      exc.what());
        }
    }

    if (runProcConf.output.get()) {

        PolyConfigT<Dataset> outputDataset = *runProcConf.output;
        if (outputDataset.type.empty())
            outputDataset.type = EMConfig::defaultOutputDatasetType;

        auto output = createDataset(engine, outputDataset, onProgress2, true /*overwrite*/);

        Date applyDate = Date::now();

        for (unsigned i = 0;  i < rows.size();  ++i) {
            std::vector<std::tuple<ColumnPath, CellValue, Date> > cols;
            cols.emplace_back(ColumnPath("cluster"), inCluster[i], applyDate);
            output->recordRow(std::get<1>(rows[i]), cols);
        }

        output->commit();
    }

    if (runProcConf.centroids.type != "" || emConfig.centroids.id != "") {

        auto centroids = createDataset(engine, runProcConf.centroids, onProgress2, true /*overwrite*/);

        Date applyDate = Date::now();

        for (unsigned i = 0;  i < em.clusters.size();  ++i) {
            auto & cluster = em.clusters[i];

            std::vector<std::tuple<ColumnPath, CellValue, Date> > cols;

            for (unsigned j = 0;  j < cluster.centroid.size();  ++j) {
                cols.emplace_back(columnNames[j], cluster.centroid[j], applyDate);
            }

            auto flatmatrix = tovector(cluster.covarianceMatrix);

            for (unsigned j = 0;  j < flatmatrix.size();  ++j) {
                cols.emplace_back(ColumnPath(MLDB::format("c%02d", j)), flatmatrix[j], applyDate);
            }

            centroids->recordRow(RowPath(MLDB::format("%i", i)), cols);
        }

        centroids->commit();
    }

    if (!runProcConf.functionName.empty()) {
        if (saved) {
            EMFunctionConfig funcConf;
            funcConf.modelFileUrl = runProcConf.modelFileUrl;

            PolyConfig emPC;
            emPC.type = "gaussianclustering";
            emPC.id = runProcConf.functionName;
            emPC.params = funcConf;
            obtainFunction(engine, emPC, onProgress);
        } else {
            throw AnnotatedException(400, "Can't create gaussian clustering function '" +
                                      runProcConf.functionName.rawString() +
                                      "'. Have you provided a valid modelFileUrl?",
                                      "modelFileUrl", runProcConf.modelFileUrl.toString());
        }
    }

    return Any();
}

DEFINE_STRUCTURE_DESCRIPTION(EMFunctionConfig);

EMFunctionConfigDescription::
EMFunctionConfigDescription()
{
    addField("modelFileUrl", &EMFunctionConfig::modelFileUrl,
             "URL of the model file (with extension '.gs') to load. "
             "This file is created by a procedure of type 'gaussianclustering.train'.");

    onPostValidate = [] (EMFunctionConfig * cfg,
                         JsonParsingContext & context) {
        // this includes empty url
        if(!cfg->modelFileUrl.valid()) {
            throw MLDB::Exception("modelFileUrl \"" + cfg->modelFileUrl.toString()
                                + "\" is not valid");
        }
    };
}

/*****************************************************************************/
/* EM FUNCTION                                                               */
/*****************************************************************************/

DEFINE_STRUCTURE_DESCRIPTION(EMInput);

EMInputDescription::EMInputDescription()
{
    addField("embedding", &EMInput::embedding,
             "Values to be assigned to a cluster.");
}

DEFINE_STRUCTURE_DESCRIPTION(EMOutput);

EMOutputDescription::EMOutputDescription()
{
    addField("cluster", &EMOutput::cluster,
             "Cluster corresponding to the input values.");
}

struct EMFunction::Impl {
    ML::EstimationMaximisation em;
    std::vector<ColumnPath> columnNames;

    Impl(const Url & modelFileUrl) {
        em.load(modelFileUrl.toDecodedString());
        for (auto & c: em.columnNames)
            this->columnNames.push_back(PathElement(c));
    }
};

EMFunction::
EMFunction(MldbEngine * owner,
            PolyConfig config,
            const std::function<bool (const Json::Value &)> & onProgress)
    : BaseT(owner, config)
{

    functionConfig = config.params.convert<EMFunctionConfig>();

    impl.reset(new Impl(functionConfig.modelFileUrl));

    dimension = impl->em.clusters[0].centroid.size();

    auto logger = MLDB::getMldbLog<EMFunction>();
    DEBUG_MSG(logger) << "got " << impl->em.clusters.size()
         << " clusters with " << dimension
         << "values";
}

struct EMFunctionApplier: public FunctionApplierT<EMInput, EMOutput> {
    EMFunctionApplier(const EMFunction * owner,
                      const std::shared_ptr<ExpressionValueInfo> & input)
        : FunctionApplierT<EMInput, EMOutput>(owner)
    {
        info = owner->getFunctionInfo();
        extract = input->extractDoubleEmbedding(owner->impl->columnNames);
    }

    ExpressionValueInfo::ExtractDoubleEmbeddingFunction extract;
};

std::unique_ptr<FunctionApplierT<EMInput, EMOutput> >
EMFunction::
bindT(SqlBindingScope & outerContext,
      const std::vector<std::shared_ptr<ExpressionValueInfo> > & input) const
{
    if (input.size() != 1)
        throw AnnotatedException(400, "EM function requires a single input");
    return std::unique_ptr<EMFunctionApplier>
        (new EMFunctionApplier(this, input.at(0)));
}

EMOutput
EMFunction::
applyT(const ApplierT & applier_, EMInput input_) const
{
    // Extract an embedding with the given column names
    ExpressionValue storage;

    const auto * downcast
            = dynamic_cast<const EMFunctionApplier *>(&applier_);

    distribution<double> input = downcast->extract(input_.embedding);
    Date ts = input_.embedding.getEffectiveTimestamp();

    int bestCluster = impl->em.assign(input);

    return {ExpressionValue(bestCluster, ts)};
}

namespace {

RegisterProcedureType<EMProcedure, EMConfig>
regEM(builtinPackage(),
      "Gaussian clustering algorithm using Estimation Maximization on Gaussian Mixture Models",
      "procedures/EMProcedure.md.html",
      nullptr /* static route */,
      { MldbEntity::INTERNAL_ENTITY });

RegisterFunctionType<EMFunction, EMFunctionConfig>
regEMFunction(builtinPackage(), "gaussianclustering",
              "Apply an gaussian clustering to new data",
              "functions/EM.md.html",
              nullptr /* static route */,
              { MldbEntity::INTERNAL_ENTITY });

} // file scope

} // namespace MLDB


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Mar 10, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file stereo_triangulation.hpp
 * @brief File containing the triangulateFast method definition.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_
#define INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_

#include <Eigen/Core>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief
namespace triangulation {

/**
 * @brief Triangulate the intersection of two rays.
 * @warning The rays e1 and e2 need to be normalized!
 * @param[in]  p1 Camera center position of frame A in coordinate frame A
 * @param[in]  e1 Ray through keypoint of frame A in coordinate frame A.
 * @param[in]  p2 Camera center position of frame B in coordinate frame A.
 * @param[in]  e2 Ray through keypoint of frame B in coordinate frame A.
 * @param[in]  sigma Ray uncertainty.
 * @param[out] isValid Is the triangulation valid.
 * @param[out] isParallel Are the rays parallel?
 * @param[out] flipped Does the landmark flip sign to be in front of cameras?
 * @return Homogeneous coordinates of triangulated point.
 */
Eigen::Vector4d triangulateFast(const Eigen::Vector3d & p1,
                                const Eigen::Vector3d & e1,
                                const Eigen::Vector3d & p2,
                                const Eigen::Vector3d & e2, double sigma,
                                bool & isValid, bool & isParallel,
                                bool & flipped);

/**
 * @brief hasLowDisparity
 * @param e1 unit ray in frame A expressed in frame A
 * @param e2 unit ray in frame B expressed in frame B
 * @param e3 unit ray in frame B expressed in frame A, rotation compensated
 * @param sigma raySigma
 * @return
 */
bool hasLowDisparity(const Eigen::Vector3d& e1, const Eigen::Vector3d& e2,
                     const Eigen::Vector3d& e3, double sigma);

}
}

#endif /* INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_ */


/*=============================================================================
Copyright (c) 2016 Paul W. Bible

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

#include <vector>
#include <exception>

#include <ggtk/TermSimilarityInterface.hpp>

#include <boost/tokenizer.hpp>

//! A class to calculate similarity between go terms for 2 sets using a precomuted term similarity matrix.
/*! \class PrecomputedMatrixTermSimilarity
	This class allows the term similarity calculation to be decoupled
	 from term set (gene) similarity measure that use them.
	 Term similarity is loaded from a matrix file.
*/
class PrecomputedMatrixTermSimilarity: public TermSimilarityInterface{

public:

	//! A constructor
	/*!
		Parses a matrix file and creates the PrecomputedMatrixTermSimilarity object
	*/
	inline PrecomputedMatrixTermSimilarity(std::string matrix_file){
		std::ifstream in(matrix_file.c_str());
		//Tokenizer type
		typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
		boost::char_separator<char> tab_sep("\t","",boost::keep_empty_tokens);
		tokenizer::iterator it;
		std::string line;

		std::size_t line_count = 0;

		//first line
		getline(in,line);
		tokenizer tokens(line,tab_sep);
		it = tokens.begin();

		std::string firstTerm = *it;
		_termToIndex[firstTerm] = line_count;
		++line_count;
		++it;

		std::vector<double> firstRow;
		while(it != tokens.end()){
			std::string strval = *it;
			double val = atof(strval.c_str());
			firstRow.push_back(val);
			++it;
		}

		std::size_t nRows = firstRow.size();
		_matrix.reserve(nRows);
		_matrix.push_back(firstRow);

		while(in.good()){
			getline(in,line);
			tokenizer tokens(line,tab_sep);
			it = tokens.begin();
			if(it == tokens.end()){continue;}

			std::string term = *it;
			_termToIndex[term] = line_count;
			++line_count;
			++it;

			std::vector<double> row;
			row.reserve(nRows);
			while(it != tokens.end()){
				std::string strval = *it;
				double val = atof(strval.c_str());
				row.push_back(val);
				++it;
			}
			_matrix.push_back(row);
		}
		//std::cout << "rows " << _matrix.size() << std::endl;
		//std::cout << "cols " << _matrix.at(0).size() << std::endl;
		//std::cout << "terms " << _termToIndex.size() << std::endl;
	}

	//! A method to check if the file exists and fits the format
	/*!
		This method is used to test if a file can be used.
	*/
	bool isFileGood(const std::string &filename){
		std::ifstream in(filename.c_str());
		if (!in.good()){
			return false;
		}

		typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
		boost::char_separator<char> tab_sep("\t", "", boost::keep_empty_tokens);
		tokenizer::iterator it;
		std::string line;

		std::size_t line_count = 0;

		//first line
		getline(in, line);
		tokenizer tokens(line, tab_sep);
		it = tokens.begin();

		std::string firstTerm = *it;
		++line_count;
		++it;

		std::vector<double> firstRow;
		while (it != tokens.end()){
			std::string strval = *it;
			double val = atof(strval.c_str());
			firstRow.push_back(val);
			++it;
		}

		try{
			while (in.good() && line_count < 5){
				getline(in, line);
				tokenizer tokens(line, tab_sep);
				it = tokens.begin();
				if (it == tokens.end()){ continue; }

				std::string term = *it;
				++line_count;
				++it;

				std::vector<double> row;
				row.reserve(firstRow.size());
				while (it != tokens.end()){
					std::string strval = *it;
					double val = atof(strval.c_str());
					row.push_back(val);
					++it;
				}
				if (row.size() != firstRow.size()){
					return false;
				}
			}
		}
		catch (std::exception e){
			return false;
		}

		return true;
	}

	//! A method for calculating term-to-term similarity for GO terms using a precomputed similarity matrix.
	/*!
		This method returns the term similarity as defined by the matrix.
	*/
	inline double calculateTermSimilarity(std::string goTermA, std::string goTermB){
		if(hasTerm(goTermA) && hasTerm(goTermB)){
			std::size_t i = _termToIndex[goTermA];
			std::size_t j = _termToIndex[goTermB];
			return _matrix[i][j];
		}else{
			return 0.0;
		}
	}

	//! A method for calculating term-to-term similarity for GO terms using a precomputed similarity matrix.
	/*!
		This method returns the similarity scaled between 0 and 1 [0,1] inclusive
	*/
	inline double calculateNormalizedTermSimilarity(std::string goTermA, std::string goTermB){
		return calculateTermSimilarity(goTermA,goTermB);
	}

	//! This method projects a set of terms into it the kernel space
	/*!
		This method treats the term similarity matrix as a kernel
		 and projects a set of terms into it.
	*/
	std::vector<double> projectTermSet(const std::vector<std::string> &terms){
		std::vector<double> projectedVec(_matrix.size(),0.0);
		std::vector<std::string>::const_iterator it;

		for(std::size_t col = 0; col < _matrix.size(); ++col){
			double sum = 0.0;
			for(it = terms.begin(); it != terms.end(); ++it){
				std::string prot = *it;
				if(_termToIndex.find(prot) != _termToIndex.end()){
					std::size_t row = _termToIndex[prot];
					sum += _matrix.at(row).at(col);
				}
			}
			projectedVec[col] = sum;
		}
		return projectedVec;
	}

private:
	inline bool hasTerm(const std::string &term){
		return _termToIndex.find(term) != _termToIndex.end();
	}

	boost::unordered_map<std::string, std::size_t> _termToIndex;
	std::vector<std::vector<double> >   _matrix;

};
#endif


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

#include <cassert>

#include "../rocFFT/shared/gpubuf.h"
#include "accuracy_test.h"
#include <boost/scope_exit.hpp>
#include <hipfft/hipfft.h>

void hipfft_transform(const std::vector<size_t>                                  length,
                      const size_t                                               nbatch,
                      const rocfft_precision                                     precision,
                      const rocfft_transform_type                                transformType,
                      const rocfft_result_placement                              placement,
                      const std::vector<size_t>&                                 cpu_istride,
                      const std::vector<size_t>&                                 cpu_ostride,
                      const size_t                                               cpu_idist,
                      const size_t                                               cpu_odist,
                      const rocfft_array_type                                    cpu_itype,
                      const rocfft_array_type                                    cpu_otype,
                      const std::vector<std::vector<char, fftwAllocator<char>>>& cpu_input_copy,
                      const std::vector<std::vector<char, fftwAllocator<char>>>& cpu_output,
                      const VectorNorms&                                         cpu_output_norm,
                      std::thread*                                               cpu_output_thread)

{
    const size_t dim = length.size();
    hipfftHandle plan;
    hipfftResult fft_status;
    hipError_t   hip_status;

    rocfft_params gpu_params;
    gpu_params.length         = length;
    gpu_params.precision      = precision;
    gpu_params.placement      = placement;
    gpu_params.transform_type = transformType;
    gpu_params.nbatch         = nbatch;

    gpu_params.istride
        = compute_stride(gpu_params.ilength(),
                         {},
                         gpu_params.placement == rocfft_placement_inplace
                             && gpu_params.transform_type == rocfft_transform_type_real_forward);
    gpu_params.idist = set_idist(
        gpu_params.placement, gpu_params.transform_type, gpu_params.length, gpu_params.istride);
    gpu_params.isize.push_back(gpu_params.idist * gpu_params.nbatch);

    gpu_params.ostride
        = compute_stride(gpu_params.olength(),
                         {},
                         gpu_params.placement == rocfft_placement_inplace
                             && gpu_params.transform_type == rocfft_transform_type_real_inverse);
    gpu_params.odist = set_odist(
        gpu_params.placement, gpu_params.transform_type, gpu_params.length, gpu_params.ostride);
    gpu_params.osize.push_back(gpu_params.odist * gpu_params.nbatch);

    // int array for hipttfPlanMany
    int n[3];
    for(int i = 0; i < dim; ++i)
        n[i] = (int)length[i];

    hipfftType hipfft_transform_type;
    switch(gpu_params.precision)
    {
    case rocfft_precision_single:
        switch(gpu_params.transform_type)
        {
        case rocfft_transform_type_complex_forward:
        case rocfft_transform_type_complex_inverse:
            hipfft_transform_type = HIPFFT_C2C;
            break;
        case rocfft_transform_type_real_forward:
            hipfft_transform_type = HIPFFT_R2C;
            break;
        case rocfft_transform_type_real_inverse:
            hipfft_transform_type = HIPFFT_C2R;
            break;
        default:
            throw std::runtime_error("Invalid transform type");
        }
        break;
    case rocfft_precision_double:
        switch(transformType)
        {
        case rocfft_transform_type_complex_forward:
        case rocfft_transform_type_complex_inverse:
            hipfft_transform_type = HIPFFT_Z2Z;
            break;
        case rocfft_transform_type_real_forward:
            hipfft_transform_type = HIPFFT_D2Z;
            break;
        case rocfft_transform_type_real_inverse:
            hipfft_transform_type = HIPFFT_Z2D;
            break;
        default:
            throw std::runtime_error("Invalid transform type");
        }
        break;
    default:
        throw std::runtime_error("Invalid precision");
    }

    switch(hipfft_transform_type)
    {
    case HIPFFT_C2C:
    case HIPFFT_Z2Z:
        gpu_params.itype = rocfft_array_type_complex_interleaved;
        gpu_params.otype = rocfft_array_type_complex_interleaved;
        break;
    case HIPFFT_R2C:
    case HIPFFT_D2Z:
        gpu_params.itype = rocfft_array_type_real;
        gpu_params.otype = rocfft_array_type_hermitian_interleaved;
        break;
    case HIPFFT_C2R:
    case HIPFFT_Z2D:
        gpu_params.itype = rocfft_array_type_hermitian_interleaved;
        gpu_params.otype = rocfft_array_type_real;
        break;
    default:
        throw std::runtime_error("Invalid transform type");
    }

    if(gpu_params.nbatch > 1)
    {
        fft_status = hipfftPlanMany(&plan,
                                    dim,
                                    n,
                                    nullptr,
                                    1,
                                    gpu_params.idist,
                                    nullptr,
                                    1,
                                    gpu_params.odist,
                                    hipfft_transform_type,
                                    gpu_params.nbatch);
    }
    else
    {
        switch(dim)
        {
        case 1:
            fft_status = hipfftPlan1d(&plan, length[0], hipfft_transform_type, 1);
            break;
        case 2:
            fft_status = hipfftPlan2d(&plan, length[0], length[1], hipfft_transform_type);
            break;
        case 3:
            fft_status
                = hipfftPlan3d(&plan, length[0], length[1], length[2], hipfft_transform_type);
            break;
        default:
            throw std::runtime_error("Invalid dimension");
        }
    }

    EXPECT_TRUE(fft_status == HIPFFT_SUCCESS) << "hipFFT plan creation failure";

    auto gpu_input = allocate_host_buffer<fftwAllocator<char>>(
        gpu_params.precision, gpu_params.itype, {gpu_params.idist * gpu_params.nbatch});

    copy_buffers(cpu_input_copy,
                 gpu_input,
                 gpu_params.ilength(),
                 gpu_params.nbatch,
                 gpu_params.precision,
                 cpu_itype,
                 cpu_istride,
                 cpu_idist,
                 gpu_params.itype,
                 gpu_params.istride,
                 gpu_params.idist,
                 {0},
                 {0});

    gpubuf gpu_output_buffer, gpu_input_buffer;
    hip_status = gpu_input_buffer.alloc(gpu_input[0].size());
    ASSERT_TRUE(hip_status == hipSuccess)
        << "hipMalloc failure for input buffer size " << gpu_input[0].size();
    if(placement == rocfft_placement_notinplace)
    {
        hip_status = gpu_output_buffer.alloc(gpu_params.obuffer_sizes()[0]);
        ASSERT_TRUE(hip_status == hipSuccess)
            << "hipMalloc failure for output buffer size " << gpu_params.obuffer_sizes()[0];
    }

    hip_status = hipMemcpy(
        gpu_input_buffer.data(), gpu_input[0].data(), gpu_input[0].size(), hipMemcpyHostToDevice);
    EXPECT_TRUE(hip_status == hipSuccess) << "hipMemcpy failure";

    void* ibuf = (void*)gpu_input_buffer.data();
    void* obuf;
    if(placement == rocfft_placement_inplace)
        obuf = ibuf;
    else
        obuf = (void*)gpu_output_buffer.data();

    switch(gpu_params.transform_type)
    {
    case(rocfft_transform_type_complex_forward):
    case(rocfft_transform_type_complex_inverse):
        if(gpu_params.precision == rocfft_precision_single)
            hipfftExecC2C(plan,
                          (hipfftComplex*)ibuf,
                          (hipfftComplex*)obuf,
                          gpu_params.transform_type == rocfft_transform_type_complex_forward
                              ? HIPFFT_FORWARD
                              : HIPFFT_BACKWARD);
        else
            hipfftExecZ2Z(plan,
                          (hipfftDoubleComplex*)ibuf,
                          (hipfftDoubleComplex*)obuf,
                          gpu_params.transform_type == rocfft_transform_type_complex_forward
                              ? HIPFFT_FORWARD
                              : HIPFFT_BACKWARD);
        break;
    case(rocfft_transform_type_real_forward):
        if(gpu_params.precision == rocfft_precision_single)
            hipfftExecR2C(plan, (hipfftReal*)ibuf, (hipfftComplex*)obuf);
        else
            hipfftExecD2Z(plan, (hipfftDoubleReal*)ibuf, (hipfftDoubleComplex*)obuf);
        break;
    case(rocfft_transform_type_real_inverse):
        if(gpu_params.precision == rocfft_precision_single)
            hipfftExecC2R(plan, (hipfftComplex*)ibuf, (hipfftReal*)obuf);
        else
            hipfftExecZ2D(plan, (hipfftDoubleComplex*)ibuf, (hipfftDoubleReal*)obuf);
        break;
    }
    EXPECT_TRUE(fft_status == HIPFFT_SUCCESS) << "hipFFT plan execution failure";

    auto gpu_output = allocate_host_buffer<fftwAllocator<char>>(
        gpu_params.precision, gpu_params.otype, {gpu_params.odist * gpu_params.nbatch});
    hip_status = hipMemcpy(gpu_output[0].data(), obuf, gpu_output[0].size(), hipMemcpyDeviceToHost);
    EXPECT_TRUE(hip_status == hipSuccess) << "hipMemcpy failure";

    // std::cout << "gpu_buffer: " << gpu_buffer_size << std::endl;
    // std::cout << "gpu_input:  " << gpu_input[0].size() << std::endl;
    // std::cout << "gpu_output: " << gpu_output[0].size() << std::endl;

    // std::cout << "cpu_input:" << std::endl;
    // printbuffer(precision,
    //             cpu_itype,
    //             cpu_input_copy,
    //             gpu_params.ilength(),
    //             cpu_istride,
    //             nbatch,
    //             cpu_idist,
    //             {0});
    // std::cout << "cpu_output:" << std::endl;
    // printbuffer(gpu_params.precision,
    //             gpu_params.otype,
    //             cpu_output,
    //             gpu_params.olength(),
    //             gpu_params.ostride,
    //             gpu_params.nbatch,
    //             gpu_params.odist,
    //             {0});
    // std::cout << "gpu_input:" << std::endl;
    // printbuffer(gpu_params.precision,
    //             gpu_params.itype,
    //             gpu_input,
    //             gpu_params.ilength(),
    //             gpu_params.istride,
    //             gpu_params.nbatch,
    //             gpu_params.idist,
    //             {0});
    // std::cout << "gpu_output:" << std::endl;
    // printbuffer(gpu_params.precision,
    //             gpu_params.otype,
    //             gpu_output,
    //             gpu_params.olength(),
    //             gpu_params.ostride,
    //             gpu_params.nbatch,
    //             gpu_params.odist,
    //             {0});

    // std::cout << gpu_params.str() << std::endl;

    // Compute the Linf and L2 norm of the GPU output:
    VectorNorms gpu_norm;
    std::thread normthread([&]() {
        gpu_norm = norm(gpu_output,
                        gpu_params.olength(),
                        gpu_params.nbatch,
                        gpu_params.precision,
                        gpu_params.otype,
                        gpu_params.ostride,
                        gpu_params.odist,
                        {0});
    });
    if(cpu_output_thread && cpu_output_thread->joinable())
        cpu_output_thread->join();

    // Compute the Linf and L2 distance between the CPU and GPU output:
    std::vector<std::pair<size_t, size_t>> linf_failures;
    const auto                             total_length
        = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());
    const double linf_cutoff = type_epsilon(precision) * cpu_output_norm.l_inf * log(total_length);
    auto         diff        = distance(cpu_output,
                         gpu_output,
                         gpu_params.olength(),
                         gpu_params.nbatch,
                         gpu_params.precision,
                         cpu_otype,
                         cpu_ostride,
                         cpu_odist,
                         gpu_params.otype,
                         gpu_params.ostride,
                         gpu_params.odist,
                         linf_failures,
                         linf_cutoff,
                         {0},
                         {0});
    normthread.join();

    EXPECT_TRUE(std::isfinite(gpu_norm.l_inf)) << gpu_norm.l_inf << " " << cpu_output_norm.l_inf;
    EXPECT_TRUE(std::isfinite(gpu_norm.l_2)) << gpu_norm.l_2 << " " << cpu_output_norm.l_2;
    EXPECT_TRUE(diff.l_2 / cpu_output_norm.l_2 < sqrt(log(total_length)) * type_epsilon(precision))
        << "L_2 diff failure " << diff.l_2 / cpu_output_norm.l_2 << " " << gpu_params.str();

    fft_status = hipfftDestroy(plan);
    EXPECT_TRUE(fft_status == HIPFFT_SUCCESS) << "hipFFT plan destroy failure";
}

TEST_P(hipfft_accuracy_test, vs_fftw)
{
    rocfft_params params;
    params.length         = std::get<0>(GetParam());
    params.nbatch         = std::get<1>(GetParam());
    params.precision      = std::get<2>(GetParam());
    params.transform_type = std::get<3>(GetParam());
    params.placement      = std::get<4>(GetParam());

    // CPU parameters
    params.istride
        = compute_stride(params.ilength(),
                         params.istride,
                         params.placement == rocfft_placement_inplace
                             && params.transform_type == rocfft_transform_type_real_forward);
    params.itype = contiguous_itype(params.transform_type);
    params.idist
        = set_idist(params.placement, params.transform_type, params.length, params.istride);
    params.isize.push_back(params.idist * params.nbatch);

    params.ostride
        = compute_stride(params.olength(),
                         params.ostride,
                         params.placement == rocfft_placement_inplace
                             && params.transform_type == rocfft_transform_type_real_inverse);
    params.odist
        = set_odist(params.placement, params.transform_type, params.length, params.ostride);
    params.otype = contiguous_otype(params.transform_type);
    params.osize.push_back(params.odist * params.nbatch);

    if(!params.valid(verbose))
    {
        GTEST_SKIP();
    }

    // Generate input
    auto cpu_input      = compute_input<fftwAllocator<char>>(params);
    auto cpu_input_copy = cpu_input; // copy of input (might get overwritten by FFTW).

    // FFTW computation
    decltype(cpu_input) cpu_output;
    VectorNorms         cpu_output_norm;
    std::thread         cpu_output_thread([&]() {
        cpu_output = fftw_via_rocfft(params.length,
                                     params.istride,
                                     params.ostride,
                                     params.nbatch,
                                     params.idist,
                                     params.odist,
                                     params.precision,
                                     params.transform_type,
                                     cpu_input);

        cpu_output_norm = norm(cpu_output,
                               params.olength(),
                               params.nbatch,
                               params.precision,
                               params.otype,
                               params.ostride,
                               params.odist,
                               {0});
    });

    // Clean up threads if transform throws...
    BOOST_SCOPE_EXIT_ALL(&cpu_output_thread)
    {
        if(cpu_output_thread.joinable())
            cpu_output_thread.join();
    };

    // GPU computation and comparison
    hipfft_transform(params.length,
                     params.nbatch,
                     params.precision,
                     params.transform_type,
                     params.placement,
                     params.istride,
                     params.ostride,
                     params.idist,
                     params.odist,
                     params.itype,
                     params.otype,
                     cpu_input_copy,
                     cpu_output,
                     cpu_output_norm,
                     &cpu_output_thread);

    if(cpu_output_thread.joinable())
        cpu_output_thread.join();
    ASSERT_TRUE(std::isfinite(cpu_output_norm.l_inf));
    ASSERT_TRUE(std::isfinite(cpu_output_norm.l_2));

    SUCCEED();
}


#ifndef _WAVERAGE_H_
#define _WAVERAGE_H_

#include <fstream>
#include <memory>

#include <cmath>
#include <numeric>

#include <string>
#include <sstream>
#include <iomanip>

#include <algorithm>
#include <vector>
#include <valarray>
#include <tuple>

#include <thread>
#include <future>
#include <chrono>
#include <mutex>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/any.hpp>

#include <CCfits/CCfits>
#include <CCfits/CCfits.h>
#include <CCfits/PHDU.h>

#include <Eigen/Dense>

#if __has_include (<boost/timer/timer.hpp>)
#include <boost/timer/timer.hpp>
#define HAS_BOOST_TIMER /**< boost::timer availability */
#endif

#if __has_include (<sys/syscall.h>)
#include <sys/syscall.h>
#define HAS_SYSCALL /**< linux syscall availability */
#endif

#if __has_include (<filesystem>)
#include <filesystem>
#define FS_STD /**< std::filesystem availability (C++17) */
namespace fs = std::filesystem;
#elif __has_include (<experimental/filesystem>) && !__has_include (<filesystem>)
#include <experimental/filesystem>
#define FS_STDEXP /**< std::experimental::filesystem availability */
namespace fs = std::experimental::filesystem;
#elif __has_include(<boost/filesystem.hpp>) && !__has_include (<filesystem>) && !__has_include (<experimental/filesystem>)
#include <boost/filesystem.hpp>
#define FS_BOOST /**< boost::filesystem availability */
namespace fs = boost::filesystem;
#else
#error "No filesystem header found"
#endif

#if __has_include(<libnotify/notify.h>)
#include <libnotify/notify.h>
#define LNOTIFY  /**< check libnotify library availability. */
#endif

#define PRECISION 10

void hello();

/**
 * \class _io
 * \brief The purpose of this class is only to provide methods to read ascii or FITS spectrum, and to convert vector to valarray.
 */
template<typename _T=double long> // double ?
class _io {
public:

    typedef std::vector<std::valarray<std::valarray<_T> > > Vvv;
    typedef std::valarray<std::valarray<_T> > vv;

    /**
     * \struct vec
     * \brief Define a (x,y) vector (seems better than std::pair)
     */
    typedef struct {
        _T x;
        _T y;
        _T SNR=-1;
    } vec;

    explicit _io();
    explicit _io(Vvv& VvvSpectr);

    virtual ~_io();

    bool read(std::string sFilename); /**<  read one file */
    bool read_dir(std::string sExtension); /**< read the whole directory, only need the extension of files */

    bool read_fits(std::string sFilename); /**< convert fits to vectors */
    bool read_fits_dir(std::string sExtension);
    bool read_fits_dir(std::string sDirectory,std::string sExtension);

    void show_data() const; /**< show a spectrum */
    void show_data(int n) const; /**< show spectrum n */
    void show_data(std::string sName) const; /**< show spectrum sName */

    bool write() const; /**< write the results */
    bool write(std::string sFilename) const; /**< write the results */

    void set_fileIn(std::string sFilename); /**< set the input file name */
    void set_fileOut(std::string sFilename); /**< set the output name */

    void set_data(Vvv& VvvSpectr);

    void set_WaveScale(_T Scale); /**< multiply wavelength by Scale */

    int get_FileIndex(std::string sName) const; /**< return the position of sName in vsFileList */

    const std::vector<vec> get_vector() const; /**< get spectrum  */
    const std::vector<std::vector<vec> > get_vectors() const; /**< get spectra */
    const vv get_valarray(); /**< convert vector to an valarray */
    const Vvv get_valarrays();

    static void notify(std::string sTitle, std::string sMsg, int iTime); /**< send desktop notification using libnotify.*/

private:
    std::string sFnameIn, sFnameOut;
    std::vector<std::string> vsFileList;
    std::string sFilesExt;

    _T Scale;

    std::vector<vec> vvSpec; /**< a spectrum */
    std::vector<std::vector<vec> > VvvSpec; //  /**<vector of spectra */

    bool exists_SNR;

    int Precision;
};

/**
 * \class _op
 * \brief This class is a set of spectrum manipulation methods. It is work with std::valarray.
 * \TODO everything.
 */
template<typename _T=double long>
class _op {
public:
    typedef std::vector<std::valarray<std::valarray<_T> > > Vvv;
    typedef std::valarray<std::valarray<_T> > vv;
    typedef std::vector<std::vector<_T> > VV;

    explicit _op();
    explicit _op(Vvv &VvvSpectr);

    virtual ~_op();

    bool resize_spectr(); /**< resize all spectra to the same size (maximal). */
    bool rebuild_wlStep(); /**< rebuild wavelength axis. */

    void remove_zero(); /**< trim spectra where flux is 0 (assuming zeros are at the beginning or the end). */

    inline bool filter_SG(int n); /**< Savitzky-Golay on spectrum n.*/
    inline vv compute_SG2(vv &vvSpectr) const; /**< Savitzky-Golay 2nd order. */
    bool filter_SG(); /**< Savitzky-Golay on all spectra n.*/

    inline void remove_peaks(int n);

    const vv compute_mean() const; /**< compute arithmetic mean*/
    void compute_wmean(); /**< compute weighted arithmetic mean */

    std::pair<_T, _T> get_wlRange(int n) const; /**< get the minimum and maximum wavelength */
    std::pair<_T, _T> get_wlRangeMin() const; /**< return the smallest support */

    void set_data(Vvv &VvvSpectr); /**< set external spectra. **/

    bool write(_io<_T>& ioInterface); /**< write the data using a _io class. */
    bool write_mean(std::string sFilename) const; /**< write only the w mean. */

private:
    Vvv VvvSpectr;
    vv vvWMean;

    _T Step;

    bool bHaveSNR;

    inline void filter_SG_th(int n); /**< Savitzky-Golay on a given spectrum (for threads). */
    inline std::vector<_T> SG_conv(std::valarray<_T> &X,int i1, int i2, int PolyDeg, int iOrder) const; /**< Savitzky-Golay coefficients. */
    inline _T mean_step(const std::valarray<_T> &vArray) const; /**< mean step for a given support. */
    _T der_snr(const std::valarray<_T> &vFlux) const;
    _T median(const std::valarray<_T> &vFlux) const;

    /**
    * \fn double long CPU_utilization()
    * \brief Get the CPU usage (%)
    */
    double long CPU_utilization();
    std::tuple<double long, double long> get_stat();

    std::mutex mLock;
};

#include <waverage.tpp>

#endif // waverage.h


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositions.h"
#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h"
#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/External/SpiceInterface/spiceEphemeris.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/Interpolators/linearInterpolator.h"
#include "Tudat/SimulationSetup/EstimationSetup/createNumericalSimulator.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h"

namespace tudat
{
namespace unit_tests
{

using namespace simulation_setup;
using namespace basic_astrodynamics;
using namespace spice_interface;
using namespace input_output;
using namespace reference_frames;
using namespace propagators;

BOOST_AUTO_TEST_SUITE( test_environment_updater )

//! Test set up of point mass gravitational accelerations, both direct and third-body.
BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate )
{

    double initialTime = 86400.0;
    double finalTime = 2.0 * 86400.0;

    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Get settings for celestial bodies
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 10.0 * 86400.0 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0,10.0 * 86400.0 );
    bodySettings[ "Moon" ] = getDefaultSingleBodySettings( "Moon", 0.0,10.0 * 86400.0 );
    bodySettings[ "Mars" ] = getDefaultSingleBodySettings( "Mars", 0.0,10.0 * 86400.0 );
    bodySettings[ "Venus" ] = getDefaultSingleBodySettings( "Venus", 0.0,10.0 * 86400.0 );

    // Create settings for sh gravity field.
    double gravitationalParameter = 398600.4418E9;
    Eigen::MatrixXd cosineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0,
              9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7,
              7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7,
              3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0,
              6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7,
              -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7
              ).finished( );
    Eigen::MatrixXd sineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0,
              0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0,
              0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7,
              3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7,
              -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7
              ).finished( );
    bodySettings[ "Earth" ]->gravityFieldSettings = std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                gravitationalParameter, 6378.0E3, cosineCoefficients, sineCoefficients,
                "IAU_Earth" );

    // Create bodies
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Define variables used in tests.
    SelectedAccelerationMap accelerationSettingsMap;
    std::map< std::string, std::string > centralBodies;
    std::vector< std::string > propagatedBodyList;
    std::vector< std::string > centralBodyList;

    std::unordered_map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet;
    double testTime = 0.0;

    {

        // Define (arbitrary) test state.
        Eigen::VectorXd testState = ( Eigen::VectorXd( 6 ) << 1.44E6, 2.234E8, -3343.246E7, 1.2E4, 1.344E3, -22.343E3 ).finished( );
        integratedStateToSet[ translational_state ] = testState;
        testTime = 2.0 * 86400.0;

        // Test if environment is updated for inly central gravity accelerations
        {
            // Define accelerations
            accelerationSettingsMap[ "Moon" ][ "Sun" ].push_back(
                        std::make_shared< AccelerationSettings >( central_gravity ) );
            accelerationSettingsMap[ "Moon" ][ "Earth" ].push_back(
                        std::make_shared< AccelerationSettings >( central_gravity ) );

            // Define origin of integration to be barycenter.
            centralBodies[ "Moon" ] = "SSB";
            propagatedBodyList.push_back( "Moon" );
            centralBodyList.push_back( centralBodies[ "Moon" ] );

            // Create accelerations
            AccelerationMap accelerationsMap = createAccelerationModelsMap(
                        bodyMap, accelerationSettingsMap, centralBodies );

            // Create environment update settings.
            std::shared_ptr< SingleArcPropagatorSettings< double > > propagatorSettings =
                    std::make_shared< TranslationalStatePropagatorSettings< double > >(
                        centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody(
                            "Moon", centralBodies[ "Moon" ], bodyMap, initialTime ), finalTime );
            std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate =
                    createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap );

            // Test update settings
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.size( ), 1 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_translational_state_update ), 1 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_translational_state_update ).size( ), 2 );

            // Create and call updater.
            std::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater =
                    createEnvironmentUpdaterForDynamicalEquations< double, double >(
                        propagatorSettings, bodyMap );
            updater->updateEnvironment( testTime, integratedStateToSet );

            // Test if Earth, Sun and Moon are updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getState( ),
                        bodyMap.at( "Earth" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Sun" )->getState( ),
                        bodyMap.at( "Sun" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Moon" )->getState( ), testState,
                        std::numeric_limits< double >::epsilon( ) );

            // Test if Mars is not updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getState( ), Eigen::Vector6d::Zero( ),
                        std::numeric_limits< double >::epsilon( ) );

            // Update environment to new time, and state from environment.
            updater->updateEnvironment(
                        0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), { translational_state } );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getState( ),
                        bodyMap.at( "Earth" )->getEphemeris( )->getCartesianState( 0.5 * testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Sun" )->getState( ),
                        bodyMap.at( "Sun" )->getEphemeris( )->getCartesianState( 0.5 * testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Moon" )->getState( ),
                        bodyMap.at( "Moon" )->getEphemeris( )->getCartesianState( 0.5 * testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getState( ), Eigen::Vector6d::Zero( ),
                        std::numeric_limits< double >::epsilon( ) );
        }

        // Test third body acceleration updates.
        {
            accelerationSettingsMap.clear( );
            centralBodies.clear( );
            propagatedBodyList.clear( );
            centralBodyList.clear( );

            // Set acceleration models.
            accelerationSettingsMap[ "Moon" ][ "Sun" ].push_back(
                        std::make_shared< AccelerationSettings >( central_gravity ) );
            accelerationSettingsMap[ "Moon" ][ "Mars" ].push_back(
                        std::make_shared< AccelerationSettings >( central_gravity ) );

            // Define origin of integration
            centralBodies[ "Moon" ] = "Earth";
            propagatedBodyList.push_back( "Moon" );
            centralBodyList.push_back( centralBodies[ "Moon" ] );

            // Create accelerations
            AccelerationMap accelerationsMap = createAccelerationModelsMap(
                        bodyMap, accelerationSettingsMap, centralBodies );

            // Create environment update settings.
            std::shared_ptr< SingleArcPropagatorSettings< double > > propagatorSettings =
                    std::make_shared< TranslationalStatePropagatorSettings< double > >(
                        centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody(
                            "Moon", centralBodies[ "Moon" ], bodyMap, initialTime ), finalTime );
            std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate =
                    createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap );

            // Test update settings
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.size( ), 1 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_translational_state_update ), 1 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_translational_state_update ).size( ), 3 );

            // Create and call updater.
            std::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater =
                    createEnvironmentUpdaterForDynamicalEquations< double, double >(
                        propagatorSettings, bodyMap );
            updater->updateEnvironment( testTime, integratedStateToSet );

            // Test if Earth, Sun, Mars and Moon are updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getState( ),
                        bodyMap.at( "Earth" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Sun" )->getState( ),
                        bodyMap.at( "Sun" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Moon" )->getState( ), testState,
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getState( ),
                        bodyMap.at( "Mars" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );

            // Test if Venus is not updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Venus" )->getState( ), Eigen::Vector6d::Zero( ),
                        std::numeric_limits< double >::epsilon( ) );

            // Update environment to new time, and state from environment.
            updater->updateEnvironment(
                        0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), { translational_state } );

        }

        // Test spherical harmonic acceleration update
        {
            accelerationSettingsMap.clear( );
            centralBodies.clear( );
            propagatedBodyList.clear( );
            centralBodyList.clear( );

            // Set acceleration models.
            accelerationSettingsMap[ "Moon" ][ "Sun" ].push_back(
                        std::make_shared< AccelerationSettings >( central_gravity ) );
            accelerationSettingsMap[ "Moon" ][ "Earth" ].push_back(
                        std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ) );
            accelerationSettingsMap[ "Moon" ][ "Mars" ].push_back(
                        std::make_shared< AccelerationSettings >( central_gravity ) );

            // Define origin of integration
            centralBodies[ "Moon" ] = "Earth";
            propagatedBodyList.push_back( "Moon" );
            centralBodyList.push_back( centralBodies[ "Moon" ] );

            // Create accelerations
            AccelerationMap accelerationsMap = createAccelerationModelsMap(
                        bodyMap, accelerationSettingsMap, centralBodies );

            // Create environment update settings.
            std::shared_ptr< SingleArcPropagatorSettings< double > > propagatorSettings =
                    std::make_shared< TranslationalStatePropagatorSettings< double > >(
                        centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody(
                            "Moon", centralBodies[ "Moon" ], bodyMap, initialTime ), finalTime );
            std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate =
                    createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap );

            // Test update settings
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.size( ), 3 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_translational_state_update ), 1 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_translational_state_update ).size( ), 3 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_rotational_state_update ), 1 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_rotational_state_update ).size( ), 1 );
            BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( spherical_harmonic_gravity_field_update ), 1 );

            // Create and call updater.
            std::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater =
                    createEnvironmentUpdaterForDynamicalEquations< double, double >(
                        propagatorSettings, bodyMap );
            updater->updateEnvironment( testTime, integratedStateToSet );

            // Test if Earth, Sun, Mars and Moon are updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getState( ),
                        bodyMap.at( "Earth" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Sun" )->getState( ),
                        bodyMap.at( "Sun" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Moon" )->getState( ), testState,
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getState( ),
                        bodyMap.at( "Mars" )->getEphemeris( )->getCartesianState( testTime ),
                        std::numeric_limits< double >::epsilon( ) );

            // Test if Venus is not updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Venus" )->getState( ), Eigen::Vector6d::Zero( ),
                        std::numeric_limits< double >::epsilon( ) );

            // Test if Earth rotation is updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getCurrentRotationToGlobalFrame( ).toRotationMatrix( ),
                        bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToBaseFrame( testTime ).toRotationMatrix( ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getCurrentRotationToLocalFrame( ).toRotationMatrix( ),
                        bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToTargetFrame( testTime ).toRotationMatrix( ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getCurrentRotationMatrixDerivativeToGlobalFrame( ),
                        bodyMap.at( "Earth" )->getRotationalEphemeris( )->getDerivativeOfRotationToBaseFrame( testTime ),
                        std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Earth" )->getCurrentRotationMatrixDerivativeToLocalFrame( ),
                        bodyMap.at( "Earth" )->getRotationalEphemeris( )->getDerivativeOfRotationToTargetFrame( testTime ),
                        std::numeric_limits< double >::epsilon( ) );

            // Test if Mars rotation is not updated
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getCurrentRotationToLocalFrame( ).toRotationMatrix( ),
                        Eigen::Matrix3d::Identity( ), std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getCurrentRotationToGlobalFrame( ).toRotationMatrix( ),
                        Eigen::Matrix3d::Identity( ), std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getCurrentRotationMatrixDerivativeToGlobalFrame( ),
                        Eigen::Matrix3d::Zero( ), std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        bodyMap.at( "Mars" )->getCurrentRotationMatrixDerivativeToLocalFrame( ),
                        Eigen::Matrix3d::Zero( ), std::numeric_limits< double >::epsilon( ) );

            // Update environment to new time, and state from environment.
            updater->updateEnvironment(
                        0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), { translational_state } );

        }
    }
}

//! Dummy function to calculate mass as a function of time.
double getBodyMass( const double time )
{
    return 1000.0 - 500.0 * ( time / ( 10.0 * 86400.0 ) );
}

//! Test radiation pressure acceleration
BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate )
{
    double initialTime = 86400.0;
    double finalTime = 2.0 * 86400.0;

    using namespace tudat::simulation_setup;
    using namespace tudat;

    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Get settings for celestial bodies
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 10.0 * 86400.0 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0,10.0 * 86400.0 );

    // Get settings for vehicle
    double area = 2.34;
    double coefficient = 1.2;
    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ]->radiationPressureSettings[ "Sun" ] =
            std::make_shared< CannonBallRadiationPressureInterfaceSettings >( "Sun", area, coefficient );
    bodySettings[ "Vehicle" ]->ephemerisSettings =
            std::make_shared< KeplerEphemerisSettings >(
                ( Eigen::Vector6d( ) << 7000.0E3, 0.05, 0.3, 0.0, 0.0, 0.0 ).finished( ),
                0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" );

    // Create bodies
    NamedBodyMap bodyMap = createBodies( bodySettings );
    bodyMap[ "Vehicle" ]->setAerodynamicCoefficientInterface(
                getApolloCoefficientInterface( ) );
    bodyMap[ "Vehicle" ]->setBodyMassFunction( &getBodyMass );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Define test time and state.
    double testTime = 2.0 * 86400.0;
    std::unordered_map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet;
    Eigen::VectorXd testState = 1.1 * bodyMap[ "Vehicle" ]->getEphemeris( )->getCartesianState( testTime ) +
            bodyMap.at( "Earth" )->getEphemeris( )->getCartesianState( testTime );
    integratedStateToSet[ translational_state ] = testState;

    {
        // Define settings for accelerations
        SelectedAccelerationMap accelerationSettingsMap;
        accelerationSettingsMap[ "Vehicle" ][ "Sun" ].push_back(
                    std::make_shared< AccelerationSettings >( cannon_ball_radiation_pressure ) );

        // Define origin of integration
        std::map< std::string, std::string > centralBodies;
        centralBodies[ "Vehicle" ] = "Earth";
        std::vector< std::string > propagatedBodyList;
        propagatedBodyList.push_back( "Vehicle" );
        std::vector< std::string > centralBodyList;
        centralBodyList.push_back( centralBodies[ "Vehicle" ] );

        // Create accelerations
        AccelerationMap accelerationsMap = createAccelerationModelsMap(
                    bodyMap, accelerationSettingsMap, centralBodies );

        // Create environment update settings.
        std::shared_ptr< SingleArcPropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >(
                    centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody(
                        "Vehicle", centralBodies[ "Vehicle" ], bodyMap, initialTime ), finalTime );
        std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate =
                createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap );

        BOOST_CHECK_EQUAL( environmentModelsToUpdate.size( ), 3 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_translational_state_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_translational_state_update ).size( ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( radiation_pressure_interface_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( radiation_pressure_interface_update ).size( ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_mass_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_mass_update ).size( ), 1 );

        std::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater =
                createEnvironmentUpdaterForDynamicalEquations< double, double >(
                    propagatorSettings, bodyMap );

        updater->updateEnvironment( testTime, integratedStateToSet );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    bodyMap.at( "Sun" )->getState( ),
                    bodyMap.at( "Sun" )->getEphemeris( )->getCartesianState( testTime ),
                    std::numeric_limits< double >::epsilon( ) );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    bodyMap.at( "Vehicle" )->getState( ), testState,
                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION(
                    bodyMap.at( "Vehicle" )->getBodyMass( ), getBodyMass( testTime ),
                    std::numeric_limits< double >::epsilon( ) );

        updater->updateEnvironment(
                    0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), { translational_state } );
    }

    {
        // Define settings for accelerations
        SelectedAccelerationMap accelerationSettingsMap;
        accelerationSettingsMap[ "Vehicle" ][ "Sun" ].push_back(
                    std::make_shared< AccelerationSettings >( cannon_ball_radiation_pressure ) );
        accelerationSettingsMap[ "Vehicle" ][ "Earth" ].push_back(
                    std::make_shared< AccelerationSettings >( aerodynamic ) );

        // Define origin of integration
        std::map< std::string, std::string > centralBodies;
        centralBodies[ "Vehicle" ] = "Earth";
        std::vector< std::string > propagatedBodyList;
        propagatedBodyList.push_back( "Vehicle" );
        std::vector< std::string > centralBodyList;
        centralBodyList.push_back( centralBodies[ "Vehicle" ] );

        // Create accelerations
        AccelerationMap accelerationsMap = createAccelerationModelsMap(
                    bodyMap, accelerationSettingsMap, centralBodies );

        // Define orientation angles.
        std::shared_ptr< aerodynamics::AtmosphericFlightConditions > vehicleFlightConditions =
                std::dynamic_pointer_cast< aerodynamics::AtmosphericFlightConditions >(
                    bodyMap[ "Vehicle" ]->getFlightConditions( ) );
        double angleOfAttack = 35.0 * mathematical_constants::PI / 180.0;
        double angleOfSideslip = -0.00322;
        double bankAngle = 2.323432;
        vehicleFlightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions(
                    [ & ]( ){ return angleOfAttack; },
                    [ & ]( ){ return angleOfSideslip; },
                    [ & ]( ){ return bankAngle; } );

        std::shared_ptr< SingleArcPropagatorSettings< double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >(
                    centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody(
                        "Vehicle", centralBodies[ "Vehicle" ], bodyMap, initialTime ), finalTime );
        std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate =
                createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap );

        // Test update settings
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.size( ), 5 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_translational_state_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_translational_state_update ).size( ), 2 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( radiation_pressure_interface_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( radiation_pressure_interface_update ).size( ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_mass_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_mass_update ).size( ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_rotational_state_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_rotational_state_update ).size( ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( vehicle_flight_conditions_update ), 1 );
        BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( vehicle_flight_conditions_update ).size( ), 1 );

        // Create and call updater.
        std::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater =
                createEnvironmentUpdaterForDynamicalEquations< double, double >(
                    propagatorSettings, bodyMap );
        updater->updateEnvironment( testTime, integratedStateToSet );

        // Test if Earth, Sun and Vehicle states are updated.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    bodyMap.at( "Earth" )->getState( ),
                    bodyMap.at( "Earth" )->getEphemeris( )->getCartesianState( testTime ),
                    std::numeric_limits< double >::epsilon( ) );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    bodyMap.at( "Sun" )->getState( ),
                    bodyMap.at( "Sun" )->getEphemeris( )->getCartesianState( testTime ),
                    std::numeric_limits< double >::epsilon( ) );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    bodyMap.at( "Vehicle" )->getState( ), testState,
                    std::numeric_limits< double >::epsilon( ) );

        // Test if Earth rotation is updated.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    bodyMap.at( "Earth" )->getCurrentRotationToGlobalFrame( ).toRotationMatrix( ),
                    bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToBaseFrame( testTime ).toRotationMatrix( ),
                    std::numeric_limits< double >::epsilon( ) );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    bodyMap.at( "Earth" )->getCurrentRotationToLocalFrame( ).toRotationMatrix( ),
                    bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToTargetFrame( testTime ).toRotationMatrix( ),
                    std::numeric_limits< double >::epsilon( ) );

        // Test if body mass is updated
        BOOST_CHECK_CLOSE_FRACTION(
                    bodyMap.at( "Vehicle" )->getBodyMass( ), getBodyMass( testTime ),
                    std::numeric_limits< double >::epsilon( ) );

        // Check if flight conditions update has been called
        BOOST_CHECK_EQUAL(
                    ( vehicleFlightConditions->getCurrentAirspeed( ) == vehicleFlightConditions->getCurrentAirspeed( ) ), 1 );
        BOOST_CHECK_EQUAL(
                    ( vehicleFlightConditions->getCurrentAltitude( ) == vehicleFlightConditions->getCurrentAltitude( ) ), 1 );
        BOOST_CHECK_EQUAL(
                    ( vehicleFlightConditions->getCurrentDensity( ) == vehicleFlightConditions->getCurrentDensity( ) ), 1 );
        BOOST_CHECK_EQUAL( vehicleFlightConditions->getCurrentTime( ), testTime );

        // Check if radiation pressure update is updated.
        std::shared_ptr< electro_magnetism::RadiationPressureInterface > radiationPressureInterface =
                bodyMap.at( "Vehicle" )->getRadiationPressureInterfaces( ).at( "Sun" );
        BOOST_CHECK_EQUAL(
                    ( radiationPressureInterface->getCurrentTime( ) == radiationPressureInterface->getCurrentTime( ) ), 1 );
        BOOST_CHECK_EQUAL(
                    ( radiationPressureInterface->getCurrentRadiationPressure( ) ==
                radiationPressureInterface->getCurrentRadiationPressure( ) ), 1 );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    radiationPressureInterface->getCurrentSolarVector( ),
                    ( bodyMap.at( "Sun" )->getPosition( ) - bodyMap.at( "Vehicle" )->getPosition( ) ),
                    std::numeric_limits< double >::epsilon( ) );

        updater->updateEnvironment(
                    0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), { translational_state } );

    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/**
* This file is part of DSO.
*
* Copyright 2016 Technical University of Munich and Intel.
* Developed by Jakob Engel <engelj at in dot tum dot de>,
* for more information see <http://vision.in.tum.de/dso>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* DSO is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSO 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with DSO. If not, see <http://www.gnu.org/licenses/>.
*/

#include "FullSystem/FullSystem.h"

#include "stdio.h"
#include "util/globalFuncs.h"
#include <Eigen/LU>
#include <algorithm>
#include "IOWrapper/ImageDisplay.h"
#include "util/globalCalib.h"
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include "FullSystem/ResidualProjections.h"

#include "OptimizationBackend/EnergyFunctional.h"
#include "OptimizationBackend/EnergyFunctionalStructs.h"

#include <cmath>

#include <algorithm>

namespace dso
{

void FullSystem::linearizeAll_Reductor(bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid)
{
	for(int k=min;k<max;k++)
	{
		PointFrameResidual* r = activeResiduals[k];
		(*stats)[0] += r->linearize(&Hcalib);

		if(fixLinearization)
		{
			r->applyRes(true);

			if(r->efResidual->isActive())
			{
				if(r->isNew)
				{
					PointHessian* p = r->point;
					Vec3f ptp_inf = r->host->targetPrecalc[r->target->idx].PRE_KRKiTll * Vec3f(p->u,p->v, 1);	// projected point assuming infinite depth.
					Vec3f ptp = ptp_inf + r->host->targetPrecalc[r->target->idx].PRE_KtTll*p->idepth_scaled;	// projected point with real depth.
					float relBS = 0.01*((ptp_inf.head<2>() / ptp_inf[2])-(ptp.head<2>() / ptp[2])).norm();	// 0.01 = one pixel.

					if(relBS > p->maxRelBaseline)
						p->maxRelBaseline = relBS;

					p->numGoodResiduals++;
				}
			}
			else
			{
				toRemove[tid].push_back(activeResiduals[k]);
			}
		}
	}
}

void FullSystem::applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid)
{
	for(int k=min;k<max;k++)
		activeResiduals[k]->applyRes(true);
}
void FullSystem::setNewFrameEnergyTH()
{

	// collect all residuals and make decision on TH.
	allResVec.clear();
	allResVec.reserve(activeResiduals.size()*2);
	FrameHessian* newFrame = frameHessians.back();

	for(PointFrameResidual* r : activeResiduals)
		if(r->state_NewEnergyWithOutlier >= 0 && r->target == newFrame)
		{
			allResVec.push_back(r->state_NewEnergyWithOutlier);

		}

	if(allResVec.size()==0)
	{
		newFrame->frameEnergyTH = 12*12*patternNum;
		return;		// should never happen, but lets make sure.
	}

	int nthIdx = setting_frameEnergyTHN*allResVec.size();

	assert(nthIdx < (int)allResVec.size());
	assert(setting_frameEnergyTHN < 1);

	std::nth_element(allResVec.begin(), allResVec.begin()+nthIdx, allResVec.end());
	float nthElement = sqrtf(allResVec[nthIdx]);

    newFrame->frameEnergyTH = nthElement*setting_frameEnergyTHFacMedian;
	newFrame->frameEnergyTH = 26.0f*setting_frameEnergyTHConstWeight + newFrame->frameEnergyTH*(1-setting_frameEnergyTHConstWeight);
	newFrame->frameEnergyTH = newFrame->frameEnergyTH*newFrame->frameEnergyTH;
	newFrame->frameEnergyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight;

//
//	int good=0,bad=0;
//	for(float f : allResVec) if(f<newFrame->frameEnergyTH) good++; else bad++;
//	printf("EnergyTH: mean %f, median %f, result %f (in %d, out %d)! \n",
//			meanElement, nthElement, sqrtf(newFrame->frameEnergyTH),
//			good, bad);
}
Vec3 FullSystem::linearizeAll(bool fixLinearization)
{
	double lastEnergyP = 0;
	double lastEnergyR = 0;
	double num = 0;

	std::vector<PointFrameResidual*> toRemove[NUM_THREADS];
	for(int i=0;i<NUM_THREADS;i++) toRemove[i].clear();

	if(multiThreading)
	{
		treadReduce.reduce(boost::bind(&FullSystem::linearizeAll_Reductor, this, fixLinearization, toRemove, _1, _2, _3, _4), 0, activeResiduals.size(), 0);
		lastEnergyP = treadReduce.stats[0];
	}
	else
	{
		Vec10 stats;
		linearizeAll_Reductor(fixLinearization, toRemove, 0,activeResiduals.size(),&stats,0);
		lastEnergyP = stats[0];
	}

	setNewFrameEnergyTH();

	if(fixLinearization)
	{

		for(PointFrameResidual* r : activeResiduals)
		{
			PointHessian* ph = r->point;
			if(ph->lastResiduals[0].first == r)
				ph->lastResiduals[0].second = r->state_state;
			else if(ph->lastResiduals[1].first == r)
				ph->lastResiduals[1].second = r->state_state;

		}

		int nResRemoved=0;
		for(int i=0;i<NUM_THREADS;i++)
		{
			for(PointFrameResidual* r : toRemove[i])
			{
				PointHessian* ph = r->point;

				if(ph->lastResiduals[0].first == r)
					ph->lastResiduals[0].first=0;
				else if(ph->lastResiduals[1].first == r)
					ph->lastResiduals[1].first=0;

				for(unsigned int k=0; k<ph->residuals.size();k++)
					if(ph->residuals[k] == r)
					{
						ef->dropResidual(r->efResidual);
						deleteOut<PointFrameResidual>(ph->residuals,k);
						nResRemoved++;
						break;
					}
			}
		}
		//printf("FINAL LINEARIZATION: removed %d / %d residuals!\n", nResRemoved, (int)activeResiduals.size());

	}

	return Vec3(lastEnergyP, lastEnergyR, num);
}

// applies step to linearization point.
bool FullSystem::doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD)
{
//	float meanStepC=0,meanStepP=0,meanStepD=0;
//	meanStepC += Hcalib.step.norm();

	Vec10 pstepfac;
	pstepfac.segment<3>(0).setConstant(stepfacT);
	pstepfac.segment<3>(3).setConstant(stepfacR);
	pstepfac.segment<4>(6).setConstant(stepfacA);

	float sumA=0, sumB=0, sumT=0, sumR=0, sumID=0, numID=0;

	float sumNID=0;

	if(setting_solverMode & SOLVER_MOMENTUM)
	{
		Hcalib.setValue(Hcalib.value_backup + Hcalib.step);
		for(FrameHessian* fh : frameHessians)
		{
			Vec10 step = fh->step;
			step.head<6>() += 0.5f*(fh->step_backup.head<6>());

			fh->setState(fh->state_backup + step);
			sumA += step[6]*step[6];
			sumB += step[7]*step[7];
			sumT += step.segment<3>(0).squaredNorm();
			sumR += step.segment<3>(3).squaredNorm();

			for(PointHessian* ph : fh->pointHessians)
			{
				float step = ph->step+0.5f*(ph->step_backup);
				ph->setIdepth(ph->idepth_backup + step);
				sumID += step*step;
				sumNID += fabsf(ph->idepth_backup);
				numID++;

                ph->setIdepthZero(ph->idepth_backup + step);
			}
		}
	}
	else
	{
		Hcalib.setValue(Hcalib.value_backup + stepfacC*Hcalib.step);
		for(FrameHessian* fh : frameHessians)
		{
			fh->setState(fh->state_backup + pstepfac.cwiseProduct(fh->step));
			sumA += fh->step[6]*fh->step[6];
			sumB += fh->step[7]*fh->step[7];
			sumT += fh->step.segment<3>(0).squaredNorm();
			sumR += fh->step.segment<3>(3).squaredNorm();

			for(PointHessian* ph : fh->pointHessians)
			{
				ph->setIdepth(ph->idepth_backup + stepfacD*ph->step);
				sumID += ph->step*ph->step;
				sumNID += fabsf(ph->idepth_backup);
				numID++;

                ph->setIdepthZero(ph->idepth_backup + stepfacD*ph->step);
			}
		}
	}

	sumA /= frameHessians.size();
	sumB /= frameHessians.size();
	sumR /= frameHessians.size();
	sumT /= frameHessians.size();
	sumID /= numID;
	sumNID /= numID;

    if(!setting_debugout_runquiet)
        printf("STEPS: A %.1f; B %.1f; R %.1f; T %.1f. \t",
                sqrtf(sumA) / (0.0005*setting_thOptIterations),
                sqrtf(sumB) / (0.00005*setting_thOptIterations),
                sqrtf(sumR) / (0.00005*setting_thOptIterations),
                sqrtf(sumT)*sumNID / (0.00005*setting_thOptIterations));

	EFDeltaValid=false;
	setPrecalcValues();

	return sqrtf(sumA) < 0.0005*setting_thOptIterations &&
			sqrtf(sumB) < 0.00005*setting_thOptIterations &&
			sqrtf(sumR) < 0.00005*setting_thOptIterations &&
			sqrtf(sumT)*sumNID < 0.00005*setting_thOptIterations;
//
//	printf("mean steps: %f %f %f!\n",
//			meanStepC, meanStepP, meanStepD);
}

// sets linearization point.
void FullSystem::backupState(bool backupLastStep)
{
	if(setting_solverMode & SOLVER_MOMENTUM)
	{
		if(backupLastStep)
		{
			Hcalib.step_backup = Hcalib.step;
			Hcalib.value_backup = Hcalib.value;
			for(FrameHessian* fh : frameHessians)
			{
				fh->step_backup = fh->step;
				fh->state_backup = fh->get_state();
				for(PointHessian* ph : fh->pointHessians)
				{
					ph->idepth_backup = ph->idepth;
					ph->step_backup = ph->step;
				}
			}
		}
		else
		{
			Hcalib.step_backup.setZero();
			Hcalib.value_backup = Hcalib.value;
			for(FrameHessian* fh : frameHessians)
			{
				fh->step_backup.setZero();
				fh->state_backup = fh->get_state();
				for(PointHessian* ph : fh->pointHessians)
				{
					ph->idepth_backup = ph->idepth;
					ph->step_backup=0;
				}
			}
		}
	}
	else
	{
		Hcalib.value_backup = Hcalib.value;
		for(FrameHessian* fh : frameHessians)
		{
			fh->state_backup = fh->get_state();
			for(PointHessian* ph : fh->pointHessians)
				ph->idepth_backup = ph->idepth;
		}
	}
}

// sets linearization point.
void FullSystem::loadSateBackup()
{
	Hcalib.setValue(Hcalib.value_backup);
	for(FrameHessian* fh : frameHessians)
	{
		fh->setState(fh->state_backup);
		for(PointHessian* ph : fh->pointHessians)
		{
			ph->setIdepth(ph->idepth_backup);

            ph->setIdepthZero(ph->idepth_backup);
		}

	}

	EFDeltaValid=false;
	setPrecalcValues();
}

double FullSystem::calcMEnergy()
{
	if(setting_forceAceptStep) return 0;
	// calculate (x-x0)^T * [2b + H * (x-x0)] for everything saved in L.
	//ef->makeIDX();
	//ef->setDeltaF(&Hcalib);
	return ef->calcMEnergyF();

}

void FullSystem::printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b)
{
	printf("A(%f)=(AV %.3f). Num: A(%'d) + M(%'d); ab %f %f!\n",
			res[0],
			sqrtf((float)(res[0] / (patternNum*ef->resInA))),
			ef->resInA,
			ef->resInM,
			a,
			b
	);

}

float FullSystem::optimize(int mnumOptIts)
{

	if(frameHessians.size() < 2) return 0;
	if(frameHessians.size() < 3) mnumOptIts = 20;
	if(frameHessians.size() < 4) mnumOptIts = 15;

	// get statistics and active residuals.

	activeResiduals.clear();
	int numPoints = 0;
	int numLRes = 0;
	for(FrameHessian* fh : frameHessians)
		for(PointHessian* ph : fh->pointHessians)
		{
			for(PointFrameResidual* r : ph->residuals)
			{
				if(!r->efResidual->isLinearized)
				{
					activeResiduals.push_back(r);
					r->resetOOB();
				}
				else
					numLRes++;
			}
			numPoints++;
		}

    if(!setting_debugout_runquiet)
        printf("OPTIMIZE %d pts, %d active res, %d lin res!\n",ef->nPoints,(int)activeResiduals.size(), numLRes);

	Vec3 lastEnergy = linearizeAll(false);
	double lastEnergyL = calcLEnergy();
	double lastEnergyM = calcMEnergy();

	if(multiThreading)
		treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);
	else
		applyRes_Reductor(true,0,activeResiduals.size(),0,0);

    if(!setting_debugout_runquiet)
    {
        printf("Initial Error       \t");
        printOptRes(lastEnergy, lastEnergyL, lastEnergyM, 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);
    }

	debugPlotTracking();

	double lambda = 1e-1;
	float stepsize=1;
	VecX previousX = VecX::Constant(CPARS+ 8*frameHessians.size(), NAN);
	for(int iteration=0;iteration<mnumOptIts;iteration++)
	{
		// solve!
		backupState(iteration!=0);
		//solveSystemNew(0);
		solveSystem(iteration, lambda);
		double incDirChange = (1e-20 + previousX.dot(ef->lastX)) / (1e-20 + previousX.norm() * ef->lastX.norm());
		previousX = ef->lastX;

		if(std::isfinite(incDirChange) && (setting_solverMode & SOLVER_STEPMOMENTUM))
		{
			float newStepsize = exp(incDirChange*1.4);
			if(incDirChange<0 && stepsize>1) stepsize=1;

			stepsize = sqrtf(sqrtf(newStepsize*stepsize*stepsize*stepsize));
			if(stepsize > 2) stepsize=2;
			if(stepsize <0.25) stepsize=0.25;
		}

		bool canbreak = doStepFromBackup(stepsize,stepsize,stepsize,stepsize,stepsize);

		// eval new energy!
		Vec3 newEnergy = linearizeAll(false);
		double newEnergyL = calcLEnergy();
		double newEnergyM = calcMEnergy();

        if(!setting_debugout_runquiet)
        {
            printf("%s %d (L %.2f, dir %.2f, ss %.1f): \t",
				(newEnergy[0] +  newEnergy[1] +  newEnergyL + newEnergyM <
						lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM) ? "ACCEPT" : "REJECT",
				iteration,
				log10(lambda),
				incDirChange,
				stepsize);
            printOptRes(newEnergy, newEnergyL, newEnergyM , 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);
        }

		if(setting_forceAceptStep || (newEnergy[0] +  newEnergy[1] +  newEnergyL + newEnergyM <
				lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM))
		{

			if(multiThreading)
				treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);
			else
				applyRes_Reductor(true,0,activeResiduals.size(),0,0);

			lastEnergy = newEnergy;
			lastEnergyL = newEnergyL;
			lastEnergyM = newEnergyM;

			lambda *= 0.25;
		}
		else
		{
			loadSateBackup();
			lastEnergy = linearizeAll(false);
			lastEnergyL = calcLEnergy();
			lastEnergyM = calcMEnergy();
			lambda *= 1e2;
		}

		if(canbreak && iteration >= setting_minOptIterations) break;
	}

	Vec10 newStateZero = Vec10::Zero();
	newStateZero.segment<2>(6) = frameHessians.back()->get_state().segment<2>(6);

	frameHessians.back()->setEvalPT(frameHessians.back()->PRE_worldToCam,
			newStateZero);
	EFDeltaValid=false;
	EFAdjointsValid=false;
	ef->setAdjointsF(&Hcalib);
	setPrecalcValues();

	lastEnergy = linearizeAll(true);

	if(!std::isfinite((double)lastEnergy[0]) || !std::isfinite((double)lastEnergy[1]) || !std::isfinite((double)lastEnergy[2]))
    {
        printf("KF Tracking failed: LOST!\n");
		isLost=true;
    }

	statistics_lastFineTrackRMSE = sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA)));

	if(calibLog != 0)
	{
		(*calibLog) << Hcalib.value_scaled.transpose() <<
				" " << frameHessians.back()->get_state_scaled().transpose() <<
				" " << sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))) <<
				" " << ef->resInM << "\n";
		calibLog->flush();
	}

	{
		boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
		for(FrameHessian* fh : frameHessians)
		{
			fh->shell->camToWorld = fh->PRE_camToWorld;
			fh->shell->aff_g2l = fh->aff_g2l();
		}
	}

	debugPlotTracking();

	return sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA)));

}

void FullSystem::solveSystem(int iteration, double lambda)
{
	ef->lastNullspaces_forLogging = getNullspaces(
			ef->lastNullspaces_pose,
			ef->lastNullspaces_scale,
			ef->lastNullspaces_affA,
			ef->lastNullspaces_affB);

	ef->solveSystemF(iteration, lambda,&Hcalib);
}

double FullSystem::calcLEnergy()
{
	if(setting_forceAceptStep) return 0;

	double Ef = ef->calcLEnergyF_MT();
	return Ef;

}

void FullSystem::removeOutliers()
{
	int numPointsDropped=0;
	for(FrameHessian* fh : frameHessians)
	{
		for(unsigned int i=0;i<fh->pointHessians.size();i++)
		{
			PointHessian* ph = fh->pointHessians[i];
			if(ph==0) continue;

			if(ph->residuals.size() == 0)
			{
				fh->pointHessiansOut.push_back(ph);
				ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
				fh->pointHessians[i] = fh->pointHessians.back();
				fh->pointHessians.pop_back();
				i--;
				numPointsDropped++;
			}
		}
	}
	ef->dropPointsF();
}

std::vector<VecX> FullSystem::getNullspaces(
		std::vector<VecX> &nullspaces_pose,
		std::vector<VecX> &nullspaces_scale,
		std::vector<VecX> &nullspaces_affA,
		std::vector<VecX> &nullspaces_affB)
{
	nullspaces_pose.clear();
	nullspaces_scale.clear();
	nullspaces_affA.clear();
	nullspaces_affB.clear();

	int n=CPARS+frameHessians.size()*8;
	std::vector<VecX> nullspaces_x0_pre;
	for(int i=0;i<6;i++)
	{
		VecX nullspace_x0(n);
		nullspace_x0.setZero();
		for(FrameHessian* fh : frameHessians)
		{
			nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_pose.col(i);
			nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE;
			nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE;
		}
		nullspaces_x0_pre.push_back(nullspace_x0);
		nullspaces_pose.push_back(nullspace_x0);
	}
	for(int i=0;i<2;i++)
	{
		VecX nullspace_x0(n);
		nullspace_x0.setZero();
		for(FrameHessian* fh : frameHessians)
		{
			nullspace_x0.segment<2>(CPARS+fh->idx*8+6) = fh->nullspaces_affine.col(i).head<2>();
			nullspace_x0[CPARS+fh->idx*8+6] *= SCALE_A_INVERSE;
			nullspace_x0[CPARS+fh->idx*8+7] *= SCALE_B_INVERSE;
		}
		nullspaces_x0_pre.push_back(nullspace_x0);
		if(i==0) nullspaces_affA.push_back(nullspace_x0);
		if(i==1) nullspaces_affB.push_back(nullspace_x0);
	}

	VecX nullspace_x0(n);
	nullspace_x0.setZero();
	for(FrameHessian* fh : frameHessians)
	{
		nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_scale;
		nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE;
		nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE;
	}
	nullspaces_x0_pre.push_back(nullspace_x0);
	nullspaces_scale.push_back(nullspace_x0);

	return nullspaces_x0_pre;
}

}


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

/// \file StereoSessionRMAX.cc
///

#include <boost/shared_ptr.hpp>

#include <asp/Sessions/RMAX/StereoSessionRmax.h>
#include <asp/Sessions/RMAX/RMAX.h>

using namespace vw;
using namespace vw::camera;

boost::shared_ptr<vw::camera::CameraModel>
asp::StereoSessionRmax::camera_model(std::string const& image_file,
                                     std::string const& /*camera_file*/) {
  ImageInfo info;
  read_image_info( image_file, info );
  CAHVORModel* cahvor = new CAHVORModel;
  *cahvor = rmax_image_camera_model(info);
  return boost::shared_ptr<camera::CameraModel>(cahvor);
}


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


// Configurable command generator
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <robotcontrol/hw/dynamiccommandgenerator.h>
#include <boost/bind.hpp>

namespace robotcontrol
{

DynamicCommandGenerator::DynamicCommandGenerator(const std::string& conf_prefix)
 : ServoCommandGenerator()
 , m_param_stribeckOne(conf_prefix + "stribeckOne", -0.005, 0.001, 1.0, 0)
 , m_param_stribeckTwo(conf_prefix + "stribeckTwo", 0.0, 0.001, 1.0, 0)
 , m_param_viscuous(conf_prefix + "viscuous", 0.0, 0.01, 1.0, 0.5)
 , m_param_km(conf_prefix + "km", 0.0, 0.01, 1.0, 0.2)
{
	m_param_km.setCallback(boost::bind(&DynamicCommandGenerator::updateParameters, this));
	m_param_viscuous.setCallback(boost::bind(&DynamicCommandGenerator::updateParameters, this));
	m_param_stribeckOne.setCallback(boost::bind(&DynamicCommandGenerator::updateParameters, this));
	m_param_stribeckTwo.setCallback(boost::bind(&DynamicCommandGenerator::updateParameters, this));
	updateParameters();
}

DynamicCommandGenerator::~DynamicCommandGenerator()
{
}

void DynamicCommandGenerator::updateParameters()
{
	setKM(m_param_km());
	setViscousFriction(m_param_viscuous());
	setStribeckOne(m_param_stribeckOne());
	setStribeckTwo(m_param_stribeckTwo());
}

}