#ifndef FSTRESULTTRACKERFEATURESTATS_H
#define FSTRESULTTRACKERFEATURESTATS_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    result_tracker_feature_stats.hpp
   \brief   Computes feature occurence statistics in a series of evaluated subsets
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz).
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <list>
#include "error.hpp"
#include "global.hpp"
#include "result_tracker_dupless.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

/*! \brief Collects evaluated subsets to eventually provide Dependency-Aware Feature Ranking coefficients.

	Dependency-Aware Feature ranking (DAF) is a new type of ranking method especially
	suitable for very-high-dimensional feature selection. Unlike standard individual
	feature ranking, the DAF ranking reflects "average feature quality in context"
	and as such is capable of providing significantly better results than BIF
	(provided the data actually do contain mutually dependent features). The method
	has been described in combination with Monte Carlo based feature selection
	permitting Wrapper Criteria even in very-high-dimensional setting.
	For usage see \ref example35 and \ref example36. For more detailed
	information see UTIA Technical Report No. 2295.
*/
template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET>
class Result_Tracker_Feature_Stats : public Result_Tracker_Dupless<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET> {
public:
	typedef Result_Tracker_Dupless<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET> parent;
	typedef typename parent::ResultRec ResultRec;
	typedef ResultRec* PResultRec;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Result_Tracker_Feature_Stats(const IDXTYPE capacity_limit=0):Result_Tracker_Dupless<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET>(capacity_limit), _n(0) {_itersize[0]=0; _itersize[1]=0; _itersize[2]=0; notify("Result_Tracker_Feature_Stats constructor.");}
	Result_Tracker_Feature_Stats(const Result_Tracker_Feature_Stats& rtfs):Result_Tracker_Dupless<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET>(rtfs), _n(0) {_itersize[0]=0; _itersize[1]=0; _itersize[2]=0; notify("Result_Tracker_Feature_Stats copy-constructor.");}
	virtual ~Result_Tracker_Feature_Stats() {notify("Result_Tracker_Feature_Stats destructor.");}

	bool compute_stats(std::ostream& os=std::cout);
	bool print_stats(std::ostream& os=std::cout);

	bool getFirstDAF(RETURNTYPE &value, DIMTYPE &feature, const unsigned int DAFidx=0) const;
	bool getNextDAF(RETURNTYPE &value, DIMTYPE &feature, const unsigned int DAFidx=0) const;

	Result_Tracker_Feature_Stats* clone() const {throw fst_error("Result_Tracker_Feature_Stats::clone() not supported, use Result_Tracker_Feature_Stats::stateless_clone() instead.");}
	Result_Tracker_Feature_Stats* sharing_clone() const {throw fst_error("Result_Tracker_Feature_Stats::sharing_clone() not supported, use Result_Tracker_Feature_Stats::stateless_clone() instead.");}
	Result_Tracker_Feature_Stats* stateless_clone() const;

	virtual std::ostream& print(std::ostream& os) const {os << "Result_Tracker_Feature_Stats(limit=" << parent::_capacity_limit << ") size " << parent::results.size(); return os;}

protected:
	DIMTYPE _n;
	//! Structure to gather feature occurence statistics over probe subset evaluations
	typedef struct {
		RETURNTYPE freq_is, freq_isnot; // no of subsets containing/not-containing this feature
		RETURNTYPE mean_is, mean_isnot; // primary criterion mean value over subsets containing/not-containing this feature
		RETURNTYPE stdev_is, stdev_isnot; // primary criterion stddev over subsets containing/not-containing this feature
		// DAF0 indicator=  mean_is-mean_isnot
		// DAF1 indicator= (mean_is-mean_isnot)/((freq_is/freq)*stdev_is+(freq_isnot/freq)*stdev_isnot)
		// for DAF3 see UTIA Tech Report No. 2295
		RETURNTYPE daf[3];
		bool daf_valid[3];
	} FeatureStat;
	vector<FeatureStat> _stats; // for each feature over all subset sizes + then for each feature and each subset size

	//! Structure to gather probe subset cardinality statistics
	typedef struct {
		RETURNTYPE freq; // no of subsets
		RETURNTYPE mean;
		RETURNTYPE stdev;
		bool valid;
	} SubSizeStat;
	vector<SubSizeStat> _dstat; // for each feature over all subset sizes

	typedef vector<DIMTYPE> ORDERTYPE;
	ORDERTYPE _order[3];  // to keep feature ordering according to DAF0, DAF1 and DAF2
	mutable DIMTYPE _itersize[3]; // iterator over feature ordering according to DAF0, DAF1 and DAF2
};

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET>
Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET>* Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET>::stateless_clone() const
{
	Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET> *clone=new Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET>
bool Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET>::compute_stats(std::ostream& os)
{
	if(parent::results.empty()) return false;

	typename parent::CONSTRESULTSITER iter=parent::results.begin();
	// get n from first recorded sub (assumes all others have the same n)
	_n=iter->sub->get_n();
	_stats.resize(_n);
	for(DIMTYPE f=0;f<_n;f++) {
		_stats[f].freq_is=_stats[f].freq_isnot=_stats[f].mean_is=_stats[f].mean_isnot=_stats[f].stdev_is=_stats[f].stdev_isnot=0;
		_stats[f].daf_valid[0]=false;
		_stats[f].daf_valid[1]=false;
		_stats[f].daf_valid[2]=false;
	}
	_dstat.resize(_n+1);
	for(DIMTYPE ss=1;ss<=_n;ss++) {
		_dstat[ss].mean=_dstat[ss].stdev=_dstat[ss].freq=0;
		_dstat[ss].valid=false;
	}

	if(parent::output_normal()) {std::ostringstream sos; sos << std::endl << "compute_stats() 1/4.." << std::endl << std::flush; syncout::print(os,sos);}
	// gather initial stats
	for(iter=parent::results.begin();iter!=parent::results.end();iter++)
	{
		assert(iter->sub->get_n()==_n);
		DIMTYPE d=iter->sub->get_d();
		assert(d>0 && d<=_n);
		for(DIMTYPE f=0;f<_n;f++) {
			if(iter->sub->selected_raw(f)) {
				_stats[f].freq_is++;
				_stats[f].mean_is+=iter->value;
			} else {
				_stats[f].freq_isnot++;
				_stats[f].mean_isnot+=iter->value;
			}
		}
		_dstat[d].mean+=iter->value;
		_dstat[d].freq++;
	}

	// calculate mean values
	for(DIMTYPE f=0;f<_n;f++)
	{
		if(_stats[f].freq_is>0) _stats[f].mean_is/=_stats[f].freq_is;
		if(_stats[f].freq_isnot>0) _stats[f].mean_isnot/=_stats[f].freq_isnot;
	}
	for(DIMTYPE ss=1;ss<=_n;ss++) if(_dstat[ss].freq>0) {
		_dstat[ss].mean/=_dstat[ss].freq;
	}
	if(parent::output_normal()) {std::ostringstream sos; sos << "compute_stats() 2/4.." << std::endl << std::flush; syncout::print(os,sos);}

	// gather more stats
	for(iter=parent::results.begin();iter!=parent::results.end();iter++)
	{
		assert(iter->sub->get_n()==_n);
		DIMTYPE d=iter->sub->get_d();
		assert(d>0 && d<=_n);
		for(DIMTYPE f=0;f<_n;f++) {
			if(iter->sub->selected_raw(f)) {
				_stats[f].stdev_is+=(_stats[f].mean_is-iter->value)*(_stats[f].mean_is-iter->value);
			} else {
				_stats[f].stdev_isnot+=(_stats[f].mean_isnot-iter->value)*(_stats[f].mean_isnot-iter->value);
			}
		}
		_dstat[d].stdev+=(_dstat[d].mean-iter->value)*(_dstat[d].mean-iter->value);
	}

	// calculate stddev values
	for(DIMTYPE f=0;f<_n;f++)
	{
		if(_stats[f].freq_is>0) _stats[f].stdev_is=sqrt(_stats[f].stdev_is/_stats[f].freq_is);
		if(_stats[f].freq_is>0) _stats[f].stdev_isnot=sqrt(_stats[f].stdev_isnot/_stats[f].freq_isnot);
	}
	for(DIMTYPE ss=1;ss<=_n;ss++) if(_dstat[ss].freq>0) {
		_dstat[ss].stdev=sqrt(_dstat[ss].stdev/_dstat[ss].freq);
		_dstat[ss].valid=true;
	}

	if(parent::output_normal()) {std::ostringstream sos; sos << "compute_stats() 3/4.." << std::endl << std::flush; syncout::print(os,sos);}
	// calculate DAF ranks
	for(DIMTYPE f=0;f<_n;f++)
	{
		if(_stats[f].freq_is>0 && _stats[f].freq_isnot>0 && (_stats[f].stdev_is>0 || _stats[f].stdev_isnot>0) )
		{
			RETURNTYPE freq=_stats[f].freq_is+_stats[f].freq_isnot;
			//DAF1
			_stats[f].daf[1]=(_stats[f].mean_is-_stats[f].mean_isnot)/( (_stats[f].freq_is/freq)*_stats[f].stdev_is + (_stats[f].freq_isnot/freq)*_stats[f].stdev_isnot );
			_stats[f].daf_valid[1]=true;
			//DAF0
			_stats[f].daf[0]=_stats[f].mean_is-_stats[f].mean_isnot;
			_stats[f].daf_valid[0]=true;
		}
		else _stats[f].daf_valid[1]=_stats[f].daf_valid[0]=false;
	}
	//DAF2
	vector<RETURNTYPE> mi1,mi2; mi1.resize(_n+1); mi2.resize(_n+1);
	vector<IDXTYPE> cnt1,cnt2; cnt1.resize(_n+1); cnt2.resize(_n+1);
	for(DIMTYPE f=0;f<_n;f++) {mi1[f]=mi2[f]=0; cnt1[f]=cnt2[f]=0;}
	for(iter=parent::results.begin();iter!=parent::results.end();iter++)
	{
		assert(iter->sub->get_n()==_n);
		DIMTYPE d=iter->sub->get_d();
		assert(d>0 && d<=_n);

		if(_dstat[d].stdev>0) {
			for(DIMTYPE f=0;f<_n;f++) {
				if(iter->sub->selected_raw(f)) {mi1[f]+=iter->value/_dstat[d].stdev; cnt1[f]++;}
				else {mi2[f]+=iter->value/_dstat[d].stdev; cnt2[f]++;}
			}
		}
	}
	for(DIMTYPE f=0;f<_n;f++) {
		if(cnt1[f]==0 || cnt2[f]==0) _stats[f].daf[2]=0; else
		{
			mi1[f]/=cnt1[f];
			mi2[f]/=cnt2[f];
			_stats[f].daf[2]=mi1[f]-mi2[f];
		}
		_stats[f].daf_valid[2]=true;
	}

	if(parent::output_normal()) {std::ostringstream sos; sos << "compute_stats() 4/4.." << std::endl << std::endl << std::flush; syncout::print(os,sos);}

	// order features according to DAF0, DAF1 and DAF2
	for(unsigned int DAFidx=0; DAFidx<3; DAFidx++)
	{
		_order[DAFidx].clear();
		for(DIMTYPE f=0;f<_n;f++)
		{
			if(_stats[f].daf_valid[DAFidx]) {
				typename ORDERTYPE::iterator iter=_order[DAFidx].begin();
				while(iter!=_order[DAFidx].end() && _stats[f].daf[DAFidx]<_stats[*iter].daf[DAFidx]) iter++;
				_order[DAFidx].insert(iter,f);
			}
		}
		if(_order[DAFidx].size()<_n && parent::output_normal()) {std::ostringstream sos; sos << "WARNING: DAF"<<DAFidx<<" rank could not be computed for all features due to insufficient number of evaluated probes subsets." << std::endl << std::flush; syncout::print(os,sos);}
	}
	if(parent::output_normal()) {
		RETURNTYPE minfreq=_stats[0].freq_is; if(_stats[0].freq_isnot<_stats[0].freq_is) minfreq=_stats[0].freq_isnot;
		for(DIMTYPE f=1;f<_n;f++) {
			if(_stats[f].freq_isnot<minfreq) minfreq=_stats[f].freq_isnot;
			if(_stats[f].freq_is<minfreq) minfreq=_stats[f].freq_is;
		}
		{
			std::ostringstream sos;
			sos << "Lowest feature evaluation frequency over all probes is "<<minfreq<<"." << std::endl;
			if(minfreq<100) sos << "WARNING: To ensure reliable DAF estimates the minimum feature evaluation frequency should be at least in the order of hundreds. More probes may be needed." << std::endl;
			sos << std::endl << std::flush;
			syncout::print(os,sos);
		}
	}

	return true;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET>
bool Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET>::print_stats(std::ostream& os)
{
	std::ostringstream sos;
	if(_n<1) {sos << "Result_Tracker_Feature_Stats:: Nothing to print out." << std::endl; return false;}
	{
		sos << "Probe subset stats for each (valid) cardinality:"<< std::endl;
		for(DIMTYPE ss=1;ss<=_n;ss++)
			if(_dstat[ss].valid) sos << " d: "<< ss << "  mean=" << _dstat[ss].mean << ", stddev=" << _dstat[ss].stdev << ", freq=" << _dstat[ss].freq << std::endl;
		sos << std::endl << "Feature stats over all probe subsets:"<< std::endl;
		for(DIMTYPE f=0;f<_n;f++) {
			sos << " F:"<<f<< " daf0 = "; if(_stats[f].daf_valid[0]) sos << _stats[f].daf[0]; else sos << "n/a";
			sos << ", daf1 = "; if(_stats[f].daf_valid[1]) sos << _stats[f].daf[1]; else sos << "n/a";
			sos << ", daf2 = "; if(_stats[f].daf_valid[2]) sos << _stats[f].daf[2]; else sos << "n/a";
			sos<<",  freq "<<_stats[f].freq_is<<"|"<<_stats[f].freq_isnot<<",  mean ";
			if(_stats[f].freq_is>0) sos<<_stats[f].mean_is; else sos<< "n/a";
			sos <<"|";
			if(_stats[f].freq_isnot>0) sos<<_stats[f].mean_isnot; else sos<< "n/a";
			sos<<", stdev ";
			if(_stats[f].freq_is>0) sos<<_stats[f].stdev_is; else sos<<"n/a";
			sos<<"|";
			if(_stats[f].freq_isnot>0) sos<<_stats[f].stdev_isnot; else sos << "n/a";
			sos<<std::endl;
		}
		sos<<std::endl;
	}
	syncout::print(os,sos);
	return true;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET>
bool Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET>::getFirstDAF(RETURNTYPE &value, DIMTYPE &feature, const unsigned int DAFidx) const
{
	assert(0<=DAFidx && DAFidx<3);
	_itersize[DAFidx]=0;
	if(!_order[DAFidx].empty())	{
		feature=_order[DAFidx][0];
		assert(feature>=0 && feature<_n);
		value=_stats[feature].daf[DAFidx];
		_itersize[DAFidx]=1;
		return true;
	} else return false;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET>
bool Result_Tracker_Feature_Stats<RETURNTYPE, IDXTYPE, DIMTYPE, SUBSET>::getNextDAF(RETURNTYPE &value, DIMTYPE &feature, const unsigned int DAFidx) const
{
	assert(0<=DAFidx && DAFidx<3);
	if(0<_itersize[DAFidx] && _itersize[DAFidx]<_order[DAFidx].size()) {
		feature=_order[DAFidx][_itersize[DAFidx]];
		assert(feature>=0 && feature<_n);
		value=_stats[feature].daf[DAFidx];
		++_itersize[DAFidx];
		return true;
	}
	return false;
}

} // namespace
#endif // FSTRESULTTRACKERFEATURESTATS_H ///:~


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/main/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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 HOLDER 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.
 */

#ifndef DART_OPTIMIZATION_MULTIOBJECTIVEPROBLEM_HPP_
#define DART_OPTIMIZATION_MULTIOBJECTIVEPROBLEM_HPP_

#include <cstddef>
#include <vector>

#include <Eigen/Dense>

#include "dart/optimization/Function.hpp"

namespace dart {
namespace optimization {

class MultiObjectiveProblem
{
public:
  /// Constructor
  explicit MultiObjectiveProblem(std::size_t dim, std::size_t integerDim = 0u);

  /// Destructor
  virtual ~MultiObjectiveProblem() = default;

  /// \{ \name Solution

  /// Sets the dimension of the solution.
  ///
  /// The element type of the solution can be either \c double or \c int. In the
  /// case that the solution partially contains integers, we assume the integer
  /// part takes place the tail of the solution, and the size of the integer
  /// part is \p integerDim. For example, a solution vector can be
  /// <tt>[floating1, floating2, ..., int1, int2]</tt>.
  ///
  /// Note that the \p dim represents the whole dimension of the solution so
  /// that the dimension of floating-point part is to be (\p dim -
  /// \p integerDim).
  ///
  /// By default, we assume the solution is homogeneously floating-point type (
  /// i.e., \p integerDim is zero).
  ///
  /// \param[in] dim Total dimension of the solution.
  /// \param[in] integerDim The dimension of integer part in the solution.
  virtual void setSolutionDimension(
      std::size_t dim, std::size_t integerDim = 0u);

  /// Returns dimension of the solution
  virtual std::size_t getSolutionDimension() const;

  /// Returns dimension of the floating-point part of the solution
  std::size_t getDoubleDimension() const;

  /// Sets dimension of the integers in the decision vector
  virtual void setIntegerDimension(std::size_t dim);

  /// Returns dimension of the integers in the decision vector
  virtual std::size_t getIntegerDimension() const;

  /// Sets lower bounds for optimization parameters
  void setLowerBounds(const Eigen::VectorXd& lb);

  /// Returns lower bounds for optimization parameters
  const Eigen::VectorXd& getLowerBounds() const;

  /// Sets upper bounds for optimization parameters
  void setUpperBounds(const Eigen::VectorXd& ub);

  /// Returns upper bounds for optimization parameters
  const Eigen::VectorXd& getUpperBounds() const;

  /// \}

  /// \{ \name Objectives

  /// Returns the total dimension of objective functions.
  virtual std::size_t getObjectiveDimension() const = 0;

  /// \}

  /// \{ \name Equality Constraints

  /// Returns the total dimension of equality constraints.
  virtual std::size_t getEqConstraintDimension() const;

  /// \}

  /// \{ \name Inequality Constraints

  /// Returns the total dimension of inequality constraints.
  virtual std::size_t getIneqConstraintDimension() const;

  /// \}

  /// \{ \name Evaluations

  /// Evaluates objectives
  virtual Eigen::VectorXd evaluateObjectives(
      const Eigen::VectorXd& x) const = 0;

  /// Evaluates equality constraints
  virtual Eigen::VectorXd evaluateEqConstraints(const Eigen::VectorXd& x) const;

  /// Evaluates inequality constraints
  virtual Eigen::VectorXd evaluateIneqConstraints(
      const Eigen::VectorXd& x) const;

  /// Return dimension of fitness
  std::size_t getFitnessDimension() const;

  /// Evaluates fitness, which is [objectives, equality constraints, inequality
  /// constraints].
  Eigen::VectorXd evaluateFitness(const Eigen::VectorXd& x) const;

  /// \}

  /// Prints information of this class to a stream.
  virtual std::ostream& print(std::ostream& os) const;

protected:
  /// Dimension of the decision vector (or optimization parameters)
  std::size_t mDimension;

  /// Dimension of integer in the decision vector. The integers are placed in
  /// the tail of the decision vector.
  std::size_t mIntegerDimension;

  /// Lower bounds for optimization parameters
  Eigen::VectorXd mLowerBounds;

  /// Upper bounds for optimization parameters
  Eigen::VectorXd mUpperBounds;
};

} // namespace optimization
} // namespace dart

#endif // DART_OPTIMIZATION_MULTIOBJECTIVEPROBLEM_HPP_


/*
 * Copyright (c) 2013-2016, Graphics Lab, Georgia Tech Research Corporation
 * Copyright (c) 2013-2016, Humanoid Lab, Georgia Tech Research Corporation
 * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
 * All rights reserved.
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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 HOLDER 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.
 */

#ifndef DART_DYNAMICS_POINTMASS_HPP_
#define DART_DYNAMICS_POINTMASS_HPP_

#include <vector>
#include <Eigen/Dense>
#include "dart/math/Helpers.hpp"
#include "dart/dynamics/Entity.hpp"

namespace dart {
namespace dynamics {

class EllipsoidShape;
class SoftBodyNode;

class PointMassNotifier;

///
class PointMass : public common::Subject
{
public:
  friend class SoftBodyNode;

  /// State for each PointMass
  struct State
  {
    /// Position
    Eigen::Vector3d mPositions;

    /// Generalized velocity
    Eigen::Vector3d mVelocities;

    /// Generalized acceleration
    Eigen::Vector3d mAccelerations;

    /// Generalized force
    Eigen::Vector3d mForces;

    /// Default constructor
    State(const Eigen::Vector3d& positions = Eigen::Vector3d::Zero(),
          const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(),
          const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(),
          const Eigen::Vector3d& forces = Eigen::Vector3d::Zero());

    bool operator==(const State& other) const;

    virtual ~State() = default;
  };

  /// Properties for each PointMass
  struct Properties
  {
    /// Resting position viewed in the parent SoftBodyNode frame
    Eigen::Vector3d mX0;

    /// Mass.
    double mMass;

    /// Indices of connected Point Masses
    std::vector<std::size_t> mConnectedPointMassIndices;

    /// Lower limit of position
    Eigen::Vector3d mPositionLowerLimits; // Currently unused

    /// Upper limit of position
    Eigen::Vector3d mPositionUpperLimits; // Currently unused

    /// Min value allowed.
    Eigen::Vector3d mVelocityLowerLimits; // Currently unused

    /// Max value allowed.
    Eigen::Vector3d mVelocityUpperLimits; // Currently unused

    /// Min value allowed.
    Eigen::Vector3d mAccelerationLowerLimits; // Currently unused

    /// upper limit of generalized acceleration
    Eigen::Vector3d mAccelerationUpperLimits; // Currently unused

    /// Min value allowed.
    Eigen::Vector3d mForceLowerLimits; // Currently unused

    /// Max value allowed.
    Eigen::Vector3d mForceUpperLimits; // Currently unused

    Properties(const Eigen::Vector3d& _X0 = Eigen::Vector3d::Zero(),
               double _mass = 0.0005,
               const std::vector<std::size_t>& _connections = std::vector<std::size_t>(),
               const Eigen::Vector3d& _positionLowerLimits =
                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
               const Eigen::Vector3d& _positionUpperLimits =
                                      Eigen::Vector3d::Constant( math::constantsd::inf()),
               const Eigen::Vector3d& _velocityLowerLimits =
                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
               const Eigen::Vector3d& _velocityUpperLimits =
                                      Eigen::Vector3d::Constant( math::constantsd::inf()),
               const Eigen::Vector3d& _accelerationLowerLimits =
                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
               const Eigen::Vector3d& _accelerationUpperLimits =
                                      Eigen::Vector3d::Constant( math::constantsd::inf()),
               const Eigen::Vector3d& _forceLowerLimits =
                                      Eigen::Vector3d::Constant(-math::constantsd::inf()),
               const Eigen::Vector3d& _forceUpperLimits =
                                      Eigen::Vector3d::Constant( math::constantsd::inf()));

    void setRestingPosition(const Eigen::Vector3d& _x);

    void setMass(double _mass);

    bool operator==(const Properties& other) const;

    bool operator!=(const Properties& other) const;

    virtual ~Properties() = default;
  };

  //--------------------------------------------------------------------------
  // Constructor and Desctructor
  //--------------------------------------------------------------------------

  /// Default destructor
  virtual ~PointMass();

  /// State of this PointMass
  State& getState();

  /// State of this PointMass
  const State& getState() const;

  ///
  std::size_t getIndexInSoftBodyNode() const;

  ///
  void setMass(double _mass);

  ///
  double getMass() const;

  ///
  double getPsi() const;

  ///
  double getImplicitPsi() const;

  ///
  double getPi() const;

  ///
  double getImplicitPi() const;

  ///
  void addConnectedPointMass(PointMass* _pointMass);

  ///
  std::size_t getNumConnectedPointMasses() const;

  ///
  PointMass* getConnectedPointMass(std::size_t _idx);

  ///
  const PointMass* getConnectedPointMass(std::size_t _idx) const;

  /// Set whether this point mass is colliding with other objects. Note that
  /// this status is set by the constraint solver during dynamics simulation but
  /// not by collision detector.
  /// \param[in] True if this point mass is colliding.
  void setColliding(bool _isColliding);

  /// Return whether this point mass is set to be colliding with other objects.
  /// \return True if this point mass is colliding.
  bool isColliding();

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

  // Documentation inherited
  std::size_t getNumDofs() const;

//  // Documentation inherited
//  void setIndexInSkeleton(std::size_t _index, std::size_t _indexInSkeleton);

//  // Documentation inherited
//  std::size_t getIndexInSkeleton(std::size_t _index) const;

  //----------------------------------------------------------------------------
  // Position
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setPosition(std::size_t _index, double _position);

  // Documentation inherited
  double getPosition(std::size_t _index) const;

  // Documentation inherited
  void setPositions(const Eigen::Vector3d& _positions);

  // Documentation inherited
  const Eigen::Vector3d& getPositions() const;

  // Documentation inherited
  void resetPositions();

  //----------------------------------------------------------------------------
  // Velocity
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setVelocity(std::size_t _index, double _velocity);

  // Documentation inherited
  double getVelocity(std::size_t _index) const;

  // Documentation inherited
  void setVelocities(const Eigen::Vector3d& _velocities);

  // Documentation inherited
  const Eigen::Vector3d& getVelocities() const;

  // Documentation inherited
  void resetVelocities();

  //----------------------------------------------------------------------------
  // Acceleration
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setAcceleration(std::size_t _index, double _acceleration);

  // Documentation inherited
  double getAcceleration(std::size_t _index) const;

  // Documentation inherited
  void setAccelerations(const Eigen::Vector3d& _accelerations);

  // Documentation inherited
  const Eigen::Vector3d& getAccelerations() const;

  /// Get the Eta term of this PointMass
  const Eigen::Vector3d& getPartialAccelerations() const;

  // Documentation inherited
  void resetAccelerations();

  //----------------------------------------------------------------------------
  // Force
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setForce(std::size_t _index, double _force);

  // Documentation inherited
  double getForce(std::size_t _index);

  // Documentation inherited
  void setForces(const Eigen::Vector3d& _forces);

  // Documentation inherited
  const Eigen::Vector3d& getForces() const;

  // Documentation inherited
  void resetForces();

  //----------------------------------------------------------------------------
  // Velocity change
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setVelocityChange(std::size_t _index, double _velocityChange);

  // Documentation inherited
  double getVelocityChange(std::size_t _index);

  // Documentation inherited
  void resetVelocityChanges();

  //----------------------------------------------------------------------------
  // Constraint impulse
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setConstraintImpulse(std::size_t _index, double _impulse);

  // Documentation inherited
  double getConstraintImpulse(std::size_t _index);

  // Documentation inherited
  void resetConstraintImpulses();

  //----------------------------------------------------------------------------
  // Integration
  //----------------------------------------------------------------------------

  // Documentation inherited
  void integratePositions(double _dt);

  // Documentation inherited
  void integrateVelocities(double _dt);

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

  /// Add linear Cartesian force to this node.
  /// \param[in] _force External force.
  /// \param[in] _isForceLocal True if _force's reference frame is of the parent
  ///                          soft body node. False if _force's reference frame
  ///                          is of the world.
  void addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal = false);

  ///
  void clearExtForce();

  //----------------------------------------------------------------------------
  // Constraints
  //   - Following functions are managed by constraint solver.
  //----------------------------------------------------------------------------
  /// Set constraint impulse
  void setConstraintImpulse(const Eigen::Vector3d& _constImp,
                            bool _isLocal = false);

  /// Add constraint impulse
  void addConstraintImpulse(const Eigen::Vector3d& _constImp,
                            bool _isLocal = false);

  /// Clear constraint impulse
  void clearConstraintImpulse();

  /// Get constraint impulse
  Eigen::Vector3d getConstraintImpulses() const;

  //----------------------------------------------------------------------------
  ///
  void setRestingPosition(const Eigen::Vector3d& _p);

  ///
  const Eigen::Vector3d& getRestingPosition() const;

  ///
  const Eigen::Vector3d& getLocalPosition() const;

  ///
  const Eigen::Vector3d& getWorldPosition() const;

  /// \todo Temporary function.
  Eigen::Matrix<double, 3, Eigen::Dynamic> getBodyJacobian();
  Eigen::Matrix<double, 3, Eigen::Dynamic> getWorldJacobian();

  /// Return velocity change due to impulse
  const Eigen::Vector3d& getBodyVelocityChange() const;

  ///
  SoftBodyNode* getParentSoftBodyNode();

  ///
  const SoftBodyNode* getParentSoftBodyNode() const;

  /// The number of the generalized coordinates by which this node is
  ///        affected.
//  int getNumDependentGenCoords() const;

  /// Return a generalized coordinate index from the array index
  ///        (< getNumDependentDofs).
//  int getDependentGenCoord(int _arrayIndex) const;

  /// Get the generalized velocity at the position of this point mass
  ///        where the velocity is expressed in the parent soft body node frame.
  const Eigen::Vector3d& getBodyVelocity() const;

  /// Get the generalized velocity at the position of this point mass
  ///        where the velocity is expressed in the world frame.
  Eigen::Vector3d getWorldVelocity() const;

  /// Get the generalized acceleration at the position of this point mass
  ///        where the acceleration is expressed in the parent soft body node
  ///        frame.
  const Eigen::Vector3d& getBodyAcceleration() const;

  /// Get the generalized acceleration at the position of this point mass
  ///        where the acceleration is expressed in the world frame.
  Eigen::Vector3d getWorldAcceleration() const;

protected:
  /// Constructor used by SoftBodyNode
  explicit PointMass(SoftBodyNode* _softBodyNode);

  ///
  void init();

  //----------------------------------------------------------------------------
  /// \{ \name Recursive dynamics routines
  //----------------------------------------------------------------------------

  /// \brief Update transformation.
  void updateTransform() const;

  /// \brief Update body velocity.
  void updateVelocity() const;

  /// \brief Update partial body acceleration due to parent joint's velocity.
  void updatePartialAcceleration() const;

  /// \brief Update articulated body inertia. Forward dynamics routine.
  /// \param[in] _timeStep Rquired for implicit joint stiffness and damping.
  void updateArtInertiaFD(double _timeStep) const;

  /// \brief Update bias force associated with the articulated body inertia.
  /// Forward dynamics routine.
  /// \param[in] _gravity Vector of gravitational acceleration
  /// \param[in] _timeStep Rquired for implicit joint stiffness and damping.
  void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity);

  /// \brief Update bias impulse associated with the articulated body inertia.
  /// Impulse-based forward dynamics routine.
  void updateBiasImpulseFD();

  /// \brief Update body acceleration with the partial body acceleration.
  void updateAccelerationID() const;

  /// \brief Update body acceleration. Forward dynamics routine.
  void updateAccelerationFD();

  /// \brief Update body velocity change. Impluse-based forward dynamics
  /// routine.
  void updateVelocityChangeFD();

  /// \brief Update body force. Inverse dynamics routine.
  void updateTransmittedForceID(const Eigen::Vector3d& _gravity,
                                bool _withExternalForces = false);

  /// \brief Update body force. Forward dynamics routine.
  void updateTransmittedForce();

  /// \brief Update body force. Impulse-based forward dynamics routine.
  void updateTransmittedImpulse();

  /// \brief Update the joint force. Inverse dynamics routine.
  void updateJointForceID(double _timeStep,
                          double _withDampingForces,
                          double _withSpringForces);

  /// \brief Update constrained terms due to the constraint impulses. Foward
  /// dynamics routine.
  void updateConstrainedTermsFD(double _timeStep);

  /// \}

  //----------------------------------------------------------------------------
  /// \{ \name Equations of motion related routines
  //----------------------------------------------------------------------------

  ///
  void updateMassMatrix();

  ///
  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, int _col);

  ///
  void aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, int _col,
                              double _timeStep);

  ///
  void updateInvMassMatrix();

  ///
  void updateInvAugMassMatrix();

  ///
  void aggregateInvMassMatrix(Eigen::MatrixXd& _MInvCol, int _col);

  ///
  void aggregateInvAugMassMatrix(Eigen::MatrixXd& _MInvCol, int _col,
                                 double _timeStep);

  ///
  void aggregateGravityForceVector(Eigen::VectorXd& _g,
                                   const Eigen::Vector3d& _gravity);

  ///
  void updateCombinedVector();

  ///
  void aggregateCombinedVector(Eigen::VectorXd& _Cg,
                               const Eigen::Vector3d& _gravity);

  /// Aggregate the external forces mFext in the generalized
  ///        coordinates recursively.
  void aggregateExternalForces(Eigen::VectorXd& _Fext);

  /// \}

  //-------------------- Cache Data for Mass Matrix ----------------------------
  ///
  Eigen::Vector3d mM_dV;

  ///
  Eigen::Vector3d mM_F;

  //----------------- Cache Data for Mass Inverse Matrix -----------------------
  ///
  Eigen::Vector3d mBiasForceForInvMeta;

  //---------------- Cache Data for Gravity Force Vector -----------------------
  ///
  Eigen::Vector3d mG_F;

  //------------------- Cache Data for Combined Vector -------------------------
  ///
  Eigen::Vector3d mCg_dV;

  ///
  Eigen::Vector3d mCg_F;

protected:
  // TODO(JS): Need?
  ///
//  Eigen::Matrix<std::size_t, 3, 1> mIndexInSkeleton;

  /// SoftBodyNode that this PointMass belongs to
  SoftBodyNode* mParentSoftBodyNode;

  /// Index of this PointMass within the SoftBodyNode
  std::size_t mIndex;

  //----------------------------------------------------------------------------
  // Configuration
  //----------------------------------------------------------------------------

  /// Derivatives w.r.t. an arbitrary scalr variable
  Eigen::Vector3d mPositionDeriv;

  //----------------------------------------------------------------------------
  // Velocity
  //----------------------------------------------------------------------------

  /// Derivatives w.r.t. an arbitrary scalr variable
  Eigen::Vector3d mVelocitiesDeriv;

  //----------------------------------------------------------------------------
  // Acceleration
  //----------------------------------------------------------------------------

  /// Derivatives w.r.t. an arbitrary scalr variable
  Eigen::Vector3d mAccelerationsDeriv;

  //----------------------------------------------------------------------------
  // Force
  //----------------------------------------------------------------------------

  /// Derivatives w.r.t. an arbitrary scalr variable
  Eigen::Vector3d mForcesDeriv;

  //----------------------------------------------------------------------------
  // Impulse
  //----------------------------------------------------------------------------

  /// Change of generalized velocity
  Eigen::Vector3d mVelocityChanges;

//  /// Generalized impulse
//  Eigen::Vector3d mImpulse;

  /// Generalized constraint impulse
  Eigen::Vector3d mConstraintImpulses;

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

  /// Current position viewed in world frame.
  mutable Eigen::Vector3d mW;

  /// Current position viewed in parent soft body node frame.
  mutable Eigen::Vector3d mX;

  /// Current velocity viewed in parent soft body node frame.
  mutable Eigen::Vector3d mV;

  /// Partial Acceleration of this PointMass
  mutable Eigen::Vector3d mEta;

  ///
  Eigen::Vector3d mAlpha;

  ///
  Eigen::Vector3d mBeta;

  /// Current acceleration viewed in parent body node frame.
  mutable Eigen::Vector3d mA;

  ///
  Eigen::Vector3d mF;

  ///
  mutable double mPsi;

  ///
  mutable double mImplicitPsi;

  ///
  mutable double mPi;

  ///
  mutable double mImplicitPi;

  /// Bias force
  Eigen::Vector3d mB;

  /// External force.
  Eigen::Vector3d mFext;

  /// A increasingly sorted list of dependent dof indices.
  std::vector<std::size_t> mDependentGenCoordIndices;

  /// Whether the node is currently in collision with another node.
  bool mIsColliding;

  //------------------------- Impulse-based Dyanmics ---------------------------
  /// Velocity change due to constraint impulse
  Eigen::Vector3d mDelV;

  /// Impulsive bias force due to external impulsive force exerted on
  ///        bodies of the parent skeleton.
  Eigen::Vector3d mImpB;

  /// Cache data for mImpB
  Eigen::Vector3d mImpAlpha;

  /// Cache data for mImpB
  Eigen::Vector3d mImpBeta;

  /// Generalized impulsive body force w.r.t. body frame.
  Eigen::Vector3d mImpF;

  PointMassNotifier* mNotifier;

public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//struct PointMassPair
//{
//  PointMass* pm1;
//  PointMass* pm2;
//};

class PointMassNotifier : public Entity
{
public:

  PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name);

  bool needsPartialAccelerationUpdate() const;

  void clearTransformNotice();
  void clearVelocityNotice();
  void clearPartialAccelerationNotice();
  void clearAccelerationNotice();

  void notifyTransformUpdate() override;
  void notifyVelocityUpdate() override;
  void notifyAccelerationUpdate() override;

  // Documentation inherited
  const std::string& setName(const std::string& _name) override;

  // Documentation inherited
  const std::string& getName() const override;

protected:

  std::string mName;

  bool mNeedPartialAccelerationUpdate;

  SoftBodyNode* mParentSoftBodyNode;

};

}  // namespace dynamics
}  // namespace dart

#endif  // DART_DYNAMICS_POINTMASS_HPP_


/*
 * Mercury7.hpp
 *
 *  Copyright (C) 2012 Marc Kirchner
 *
 * This file is part of the Mass Spectrometry Toolkit (MSTK).
 *
 * 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.
 */
#ifndef __MSTK_INCLUDE_MSTK_IPACA_MERCURY7_HPP__
#define __MSTK_INCLUDE_MSTK_IPACA_MERCURY7_HPP__
#include <MSTK/config.hpp>
#include <MSTK/ipaca/Mercury7Impl.hpp>
#include <MSTK/ipaca/Spectrum.hpp>
#include <MSTK/ipaca/Stoichiometry.hpp>
#include <MSTK/common/Types.hpp>
#include <MSTK/ipaca/Traits.hpp>
#include <boost/shared_ptr.hpp>

namespace mstk {

namespace ipaca {

/** Calculates a theoretical isotope distribution from an
 *  elemental composition (stoichiometry).
 *
 *  The client is required to provide type information for the
 *  stoichiometry and the resulting spectra as well as two conversion
 *  function in \c ipaca::Traits<U,V> that specifiy how the user-
 *  defined stoichiometry and spectrum types are converted into
 *  the internale representation used by ipaca (i.e. \c ipaca::Spectrum
 *  and \c ipaca::Stoichiometry).
 *  See \c test/Mercury7-test.cpp for a simple example.
 */
template<typename StoichiometryType, typename SpectrumType>
class Mercury7
{
public:
    /** The type of particle that carries the charge.
     */
    enum Particle
    {
        ELECTRON, PROTON
    };

    /** Constructor.
     */
    Mercury7();

    /** Functor method to calculate the theoretical isotope
     *         distribution of a compound.
     * @param stoichiometry The stoichiometry for which the isotope
     *                      distribution should be calculated.
     * @param limit The abundance limit below which peaks are pruned
     *              during the processing
     *
     * The procedure is based on Perttu Haimi's and Alan Rockwood's
     * sparse/binary convolution algorithm.
     */
    SpectrumType
    operator()(const StoichiometryType& stoichiometry, const int charge,
        const Particle particle, const Double limit = 1e-26) const;

    /** calculate the monoisotopic mass of a given stoichiometry
     *  @param stoichiometry The stoichiometry to calculate the mass for.
     *  @param charge The charge at which the monoisotopic mass is desired
     *                (zero for theoretical but unobservable mass).
     */
    Double
    getMonoisotopicMass(const StoichiometryType& stoichiometry) const;

    /** calculate the average mass of a given stoichiometry
     *  @param stoichiometry The stoichiometry to calculate the mass for.
     *  @param charge The charge at which the average mass is desired
     *                (zero for theoretical but unobservable mass).
     */
    Double getAverageMass(const StoichiometryType& stoichiometry) const;
private:
    boost::shared_ptr<detail::Mercury7Impl> pImpl_;
};

//
// template implementation
//

template<typename StoichiometryType, typename SpectrumType>
Mercury7<StoichiometryType, SpectrumType>::Mercury7() :
    pImpl_(new detail::Mercury7Impl)
{
}

template<typename StoichiometryType, typename SpectrumType>
SpectrumType Mercury7<StoichiometryType, SpectrumType>::operator()(
    const StoichiometryType& stoichiometry, const int charge,
    const Particle particle, const Double limit) const
{
    // convert the user type to our internal type
    detail::Stoichiometry s;
    typename Traits<StoichiometryType, SpectrumType>::stoichiometry_converter
            stoi_conv;
    stoi_conv(stoichiometry, s);
    // adjust the stoichiometry for charge and particle type

    // Adjust the number of hydrogens.
    if (charge != 0 && particle == PROTON) {
        detail::adjustStoichiometryForProtonation<StoichiometryType, SpectrumType>(s, charge);
    }
    detail::Spectrum result = pImpl_->operator()(s, limit);
    // Do the charge adjustment. This is the same for all types of charges
    // because we adjusted the number of hydrogens earlier.
    if (charge != 0) {
        Int absCharge = (abs)(charge);
        Double e = Traits<StoichiometryType, SpectrumType>::getElectronMass();
        typedef detail::Spectrum::iterator IT;
        for (IT i = result.begin(); i != result.end(); ++i) {
            i->mz = (i->mz - (charge * e)) / absCharge;
        }
    }
    SpectrumType spectrum;
    typename Traits<StoichiometryType, SpectrumType>::spectrum_converter
            spec_conv;
    spec_conv(result, spectrum);
    return spectrum;
}

template<typename StoichiometryType, typename SpectrumType>
Double Mercury7<StoichiometryType, SpectrumType>::getMonoisotopicMass(
    const StoichiometryType& stoichiometry) const
{
    detail::Stoichiometry s;
    typename Traits<StoichiometryType, SpectrumType>::stoichiometry_converter
            stoi_conv;
    stoi_conv(stoichiometry, s);
    return pImpl_->getMonoisotopicMass(s);
}

template<typename StoichiometryType, typename SpectrumType>
Double Mercury7<StoichiometryType, SpectrumType>::getAverageMass(
    const StoichiometryType& stoichiometry) const
{
    detail::Stoichiometry s;
    typename Traits<StoichiometryType, SpectrumType>::stoichiometry_converter
            stoi_conv;
    stoi_conv(stoichiometry, s);
    return pImpl_->getAverageMass(s);
}

} // namespace ipaca

} // namespace mstk

#endif