///////////////////////////////////////////////////////////////////////////////
// density.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_DENSITY_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_DENSITY_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <functional>
#include <boost/range.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/accumulators_fwd.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>

namespace boost { namespace accumulators
{

///////////////////////////////////////////////////////////////////////////////
// cache_size and num_bins named parameters
//
BOOST_PARAMETER_NESTED_KEYWORD(tag, density_cache_size, cache_size)
BOOST_PARAMETER_NESTED_KEYWORD(tag, density_num_bins, num_bins)

BOOST_ACCUMULATORS_IGNORE_GLOBAL(density_cache_size)
BOOST_ACCUMULATORS_IGNORE_GLOBAL(density_num_bins)

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // density_impl
    //  density histogram
    /**
        @brief Histogram density estimator

        The histogram density estimator returns a histogram of the sample distribution. The positions and sizes of the bins
        are determined using a specifiable number of cached samples (cache_size). The range between the minimum and the
        maximum of the cached samples is subdivided into a specifiable number of bins (num_bins) of same size. Additionally,
        an under- and an overflow bin is added to capture future under- and overflow samples. Once the bins are determined,
        the cached samples and all subsequent samples are added to the correct bins. At the end, a range of std::pair is
        return, where each pair contains the position of the bin (lower bound) and the samples count (normalized with the
        total number of samples).

        @param  density_cache_size Number of first samples used to determine min and max.
        @param  density_num_bins Number of bins (two additional bins collect under- and overflow samples).
    */
    template<typename Sample>
    struct density_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef std::vector<float_type> array_type;
        // for boost::result_of
        typedef iterator_range<typename histogram_type::iterator> result_type;

        template<typename Args>
        density_impl(Args const &args)
            : cache_size(args[density_cache_size])
            , cache(cache_size)
            , num_bins(args[density_num_bins])
            , samples_in_bin(num_bins + 2, 0.)
            , bin_positions(num_bins + 2)
            , histogram(
                num_bins + 2
              , std::make_pair(
                    numeric::average(args[sample | Sample()],(std::size_t)1)
                  , numeric::average(args[sample | Sample()],(std::size_t)1)
                )
              )
            , is_dirty(true)
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            this->is_dirty = true;

            std::size_t cnt = count(args);

            // Fill up cache with cache_size first samples
            if (cnt <= this->cache_size)
            {
                this->cache[cnt - 1] = args[sample];
            }

            // Once cache_size samples have been accumulated, create num_bins bins of same size between
            // the minimum and maximum of the cached samples as well as under and overflow bins.
            // Store their lower bounds (bin_positions) and fill the bins with the cached samples (samples_in_bin).
            if (cnt == this->cache_size)
            {
                float_type minimum = numeric::average((min)(args), (std::size_t)1);
                float_type maximum = numeric::average((max)(args), (std::size_t)1);
                float_type bin_size = numeric::average(maximum - minimum, this->num_bins );

                // determine bin positions (their lower bounds)
                for (std::size_t i = 0; i < this->num_bins + 2; ++i)
                {
                    this->bin_positions[i] = minimum + (i - 1.) * bin_size;
                }

                for (typename array_type::const_iterator iter = this->cache.begin(); iter != this->cache.end(); ++iter)
                {
                    if (*iter < this->bin_positions[1])
                    {
                        ++(this->samples_in_bin[0]);
                    }
                    else if (*iter >= this->bin_positions[this->num_bins + 1])
                    {
                        ++(this->samples_in_bin[this->num_bins + 1]);
                    }
                    else
                    {
                        typename array_type::iterator it = std::upper_bound(
                            this->bin_positions.begin()
                          , this->bin_positions.end()
                          , *iter
                        );

                        std::size_t d = std::distance(this->bin_positions.begin(), it);
                        ++(this->samples_in_bin[d - 1]);
                    }
                }
            }
            // Add each subsequent sample to the correct bin
            else if (cnt > this->cache_size)
            {
                if (args[sample] < this->bin_positions[1])
                {
                    ++(this->samples_in_bin[0]);
                }
                else if (args[sample] >= this->bin_positions[this->num_bins + 1])
                {
                    ++(this->samples_in_bin[this->num_bins + 1]);
                }
                else
                {
                    typename array_type::iterator it = std::upper_bound(
                        this->bin_positions.begin()
                      , this->bin_positions.end()
                      , args[sample]
                    );

                    std::size_t d = std::distance(this->bin_positions.begin(), it);
                    ++(this->samples_in_bin[d - 1]);
                }
            }
        }

        /**
            @pre The number of samples must meet or exceed the cache size
        */
        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                // creates a vector of std::pair where each pair i holds
                // the values bin_positions[i] (x-axis of histogram) and
                // samples_in_bin[i] / cnt (y-axis of histogram).

                for (std::size_t i = 0; i < this->num_bins + 2; ++i)
                {
                    this->histogram[i] = std::make_pair(this->bin_positions[i], numeric::average(this->samples_in_bin[i], count(args)));
                }
            }
            // returns a range of pairs
            return make_iterator_range(this->histogram);
        }

    private:
        std::size_t            cache_size;      // number of cached samples
        array_type             cache;           // cache to store the first cache_size samples
        std::size_t            num_bins;        // number of bins
        array_type             samples_in_bin;  // number of samples in each bin
        array_type             bin_positions;   // lower bounds of bins
        mutable histogram_type histogram;       // histogram
        mutable bool is_dirty;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::density
//
namespace tag
{
    struct density
      : depends_on<count, min, max>
      , density_cache_size
      , density_num_bins
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::density_impl<mpl::_1> impl;

        #ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
        /// tag::density::cache_size named parameter
        /// tag::density::num_bins named parameter
        static boost::parameter::keyword<density_cache_size> const cache_size;
        static boost::parameter::keyword<density_num_bins> const num_bins;
        #endif
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::density
//
namespace extract
{
    extractor<tag::density> const density = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(density)
}

using extract::density;

// So that density can be automatically substituted
// with weighted_density when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::density>
{
    typedef tag::weighted_density type;
};

template<>
struct feature_of<tag::weighted_density>
  : feature_of<tag::density>
{
};

}} // namespace boost::accumulators

#endif


//
//  Copyright (c) 2013, Novartis Institutes for BioMedical Research Inc.
//  All rights reserved.
//
// 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 Novartis Institutes for BioMedical Research Inc.
//       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.
//

#include <Geometry/point.h>
#include <Numerics/Vector.h>
#include <GraphMol/RDKitBase.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/Substruct/SubstructMatch.h>
#include "USRDescriptor.h"

#include <boost/flyweight.hpp>
#include <boost/flyweight/key_value.hpp>
#include <boost/flyweight/no_tracking.hpp>

namespace RDKit {

namespace {
void calcDistances(const RDGeom::Point3DConstPtrVect &coords,
                   const RDGeom::Point3D &point,
                   std::vector<double> &distances) {
  distances.resize(coords.size());
  unsigned int i = 0;
  // loop over coordinates
  for (const auto *tpp : coords) {
    distances[i++] = (*tpp - point).length();
  }
}

void calcCentroid(const RDGeom::Point3DConstPtrVect &coords,
                  RDGeom::Point3D &pt) {
  PRECONDITION(!coords.empty(), "no coordinates");
  // set pt to zero
  pt *= 0.0;
  // loop over coordinates
  for (const auto *opt : coords) {
    pt += *opt;
  }
  pt /= coords.size();
}

unsigned int largestValId(const std::vector<double> &v) {
  PRECONDITION(!v.empty(), "no values");
  double res = v[0];
  unsigned int id = 0;
  for (unsigned int i = 1; i < v.size(); ++i) {
    if (v[i] > res) {
      res = v[i];
      id = i;
    }
  }
  return id;
}

unsigned int smallestValId(const std::vector<double> &v) {
  PRECONDITION(!v.empty(), "no values");
  double res = v[0];
  unsigned int id = 0;
  for (unsigned int i = 1; i < v.size(); ++i) {
    if (v[i] < res) {
      res = v[i];
      id = i;
    }
  }
  return id;
}

void calcMoments(const std::vector<double> &dist,
                 std::vector<double> &descriptor, int idx) {
  std::vector<double> moments(3, 0.0);
  unsigned int numPts = dist.size();
  if (numPts > 0) {
    // 1. moment: mean
    for (unsigned int i = 0; i < numPts; ++i) {
      moments[0] += dist[i];
    }
    moments[0] /= numPts;
    // 2. moment: standard deviation
    // 3. moment: cubic root of skewness
    for (unsigned int i = 0; i < numPts; ++i) {
      double diff = dist[i] - moments[0];
      moments[1] += diff * diff;
      moments[2] += diff * diff * diff;
    }
    moments[1] = sqrt(moments[1] / numPts);
    moments[2] /= numPts;
    if (moments[1] == 0) {
      moments[2] = 0.0;
    } else {
#ifdef WIN32
      moments[2] = moments[2] / (moments[1] * moments[1] * moments[1]);
      if (moments[2] >= 0)
        moments[2] = pow(moments[2], 1. / 3.);
      else
        moments[2] = -1. * pow(-1. * moments[2], 1. / 3.);
#else
      moments[2] = cbrt(moments[2] / (moments[1] * moments[1] * moments[1]));
#endif
    }
  }
  // add moments to descriptor
  std::copy(moments.begin(), moments.end(), descriptor.begin() + idx);
}

class ss_matcher {
 public:
  ss_matcher(){};
  ss_matcher(const std::string &pattern) {
    RDKit::RWMol *p = RDKit::SmartsToMol(pattern);
    TEST_ASSERT(p);
    m_matcher.reset(p);
  };
  const RDKit::ROMol *getMatcher() const { return m_matcher.get(); };

 private:
  RDKit::ROMOL_SPTR m_matcher;
};

// Definitions for feature points from
// https://bitbucket.org/aschreyer/usrcat/src/2aa77f970c2c/usrcat/__init__.py
const char *smartsPatterns[4] = {
    "[#6+0!$(*~[#7,#8,F]),SH0+0v2,s+0,S^3,Cl+0,Br+0,I+0]",  // hydrophobic
    "[a]",                                                  // aromatic
    "[$([O,S;H1;v2]-[!$(*=[O,N,P,S])]),$([O,S;H0;v2]),$([O,S;-]),\
$([N&v3;H1,H2]-[!$(*=[O,N,P,S])]),$([N;v3;H0]),$([n,o,s;+0]),F]",  // acceptor
    "[N!H0v3,N!H0+v4,OH+0,SH+0,nH+0]"                              // donor
};
std::vector<std::string> featureSmarts(smartsPatterns, smartsPatterns + 4);
typedef boost::flyweight<boost::flyweights::key_value<std::string, ss_matcher>,
                         boost::flyweights::no_tracking>
    pattern_flyweight;

void getAtomIdsForFeatures(const ROMol &mol,
                           std::vector<std::vector<unsigned int>> &atomIds) {
  unsigned int numFeatures = featureSmarts.size();
  PRECONDITION(atomIds.size() == numFeatures,
               "atomIds must have be the same size as featureSmarts");
  std::vector<const ROMol *> featureMatchers;
  featureMatchers.reserve(numFeatures);
  for (const auto &feature : featureSmarts) {
    const ROMol *matcher = pattern_flyweight(feature).get().getMatcher();
    featureMatchers.push_back(matcher);
  }
  for (unsigned int i = 0; i < numFeatures; ++i) {
    std::vector<MatchVectType> matchVect;
    // to maintain thread safety, we have to copy the pattern molecules:
    SubstructMatch(mol, ROMol(*featureMatchers[i], true), matchVect);
    for (const auto &mv : matchVect) {
      for (auto mi : mv) {
        atomIds[i].push_back(mi.second);
      }
    }
  }  // end loop over features
}

}  // end namespace

namespace Descriptors {

void USR(const ROMol &mol, std::vector<double> &descriptor, int confId) {
  PRECONDITION(descriptor.size() == 12, "descriptor must have 12 elements");
  unsigned int na = mol.getNumAtoms();
  // check that number of atoms > 3
  if (na < 3) {
    throw ValueErrorException("Number of atoms must be greater than 3");
  }
  // check that minimum a conformer exists
  if (mol.getNumConformers() == 0) {
    throw ConformerException("No conformations available on this molecule");
  }

  const Conformer &conf = mol.getConformer(confId);
  RDGeom::Point3DConstPtrVect coords(na);
  // loop over atoms
  for (unsigned int ai = 0; ai < na; ++ai) {
    coords[ai] = &conf.getAtomPos(ai);
  }
  // the four distances
  std::vector<std::vector<double>> dist(4);
  std::vector<RDGeom::Point3D> points(4);
  calcUSRDistributions(coords, dist, points);

  calcUSRFromDistributions(dist, descriptor);
}

void USRCAT(const ROMol &mol, std::vector<double> &descriptor,
            std::vector<std::vector<unsigned int>> &atomIds, int confId) {
  unsigned int na = mol.getNumAtoms();
  // check that number of atoms > 3
  if (na < 3) {
    throw ValueErrorException("Number of atoms must be greater than 3");
  }
  // check that minimum a conformer exists
  if (mol.getNumConformers() == 0) {
    throw ConformerException("No conformations available on this molecule");
  }

  // get atom selections
  unsigned int numClasses = atomIds.size();
  if (numClasses == 0) {  // no user input, use default values
    numClasses = featureSmarts.size();
    atomIds.resize(numClasses);
    getAtomIdsForFeatures(mol, atomIds);
  }
  PRECONDITION(descriptor.size() == 12 * (numClasses + 1),
               "descriptor wrong size");

  const Conformer &conf = mol.getConformer(confId);
  RDGeom::Point3DConstPtrVect coords(na);
  // loop over atoms
  for (unsigned int ai = 0; ai < na; ++ai) {
    coords[ai] = &conf.getAtomPos(ai);
  }
  // the original USR
  std::vector<std::vector<double>> distribs(4);
  std::vector<RDGeom::Point3D> points(4);
  calcUSRDistributions(coords, distribs, points);
  std::vector<double> tmpDescriptor(12);
  calcUSRFromDistributions(distribs, tmpDescriptor);
  std::copy(tmpDescriptor.begin(), tmpDescriptor.end(), descriptor.begin());

  // loop over the atom selections
  unsigned int featIdx = 12;
  for (const auto &atomsInClass : atomIds) {
    // reduce the coordinates to the atoms of interest
    RDGeom::Point3DConstPtrVect reducedCoords;
    reducedCoords.reserve(atomsInClass.size());
    for (const auto idx : atomsInClass) {
      reducedCoords.push_back(coords[idx]);
    }
    calcUSRDistributionsFromPoints(reducedCoords, points, distribs);
    calcUSRFromDistributions(distribs, tmpDescriptor);
    std::copy(tmpDescriptor.begin(), tmpDescriptor.end(),
              descriptor.begin() + featIdx);
    featIdx += 12;
  }
}

void calcUSRDistributions(const RDGeom::Point3DConstPtrVect &coords,
                          std::vector<std::vector<double>> &dist,
                          std::vector<RDGeom::Point3D> &points) {
  PRECONDITION(dist.size() == 4, "dist must have 4 elements");
  PRECONDITION(points.size() == 4, "points must have 4 elements");
  // ctd = centroid
  calcCentroid(coords, points[0]);
  calcDistances(coords, points[0], dist[0]);
  // catc = closest atom to centroid
  points[1] = (*coords[smallestValId(dist[0])]);
  calcDistances(coords, points[1], dist[1]);
  // fatc = farthest atom to centroid
  points[2] = (*coords[largestValId(dist[0])]);
  calcDistances(coords, points[2], dist[2]);
  // fatf = farthest atom to fatc
  points[3] = (*coords[largestValId(dist[2])]);
  calcDistances(coords, points[3], dist[3]);
}

void calcUSRDistributionsFromPoints(const RDGeom::Point3DConstPtrVect &coords,
                                    const std::vector<RDGeom::Point3D> &points,
                                    std::vector<std::vector<double>> &dist) {
  PRECONDITION(points.size() == dist.size(),
               "points and dist must have the same size");
  for (unsigned int i = 0; i < points.size(); ++i) {
    calcDistances(coords, points[i], dist[i]);
  }
}

void calcUSRFromDistributions(const std::vector<std::vector<double>> &dist,
                              std::vector<double> &descriptor) {
  PRECONDITION(descriptor.size() == 3 * dist.size(),
               "descriptor must have 3 times more elements than dist");
  for (unsigned int i = 0; i < dist.size(); ++i) {
    calcMoments(dist[i], descriptor, 3 * i);
  }
}

double calcUSRScore(const std::vector<double> &d1,
                    const std::vector<double> &d2,
                    const std::vector<double> &weights) {
  unsigned int num = 12;  // length of each subset
  PRECONDITION(d1.size() == d2.size(), "descriptors must have the same size");
  PRECONDITION(weights.size() == (d1.size() / num),
               "size of weights not correct");
  double score = 1.0;
  for (unsigned int w = 0; w < (d1.size() / num); ++w) {
    double tmpScore = 0.0;
    unsigned int offset = num * w;
    for (unsigned int i = 0; i < num; ++i) {
      tmpScore += fabs(d1[i + offset] - d2[i + offset]);
    }
    tmpScore /= num;
    score += weights[w] * tmpScore;
  }
  return 1.0 / score;
}

}  // end of namespace Descriptors
}  // end of namespace RDKit


/* ------------------------------------------------------------------------- */
/* UMFPACK Version 4.1 (Apr. 30, 2003), Copyright (c) 2003 by Timothy A.     */
/* Davis.  All Rights Reserved.  See ../README for License.                  */
/* email: davis@cise.ufl.edu    CISE Department, Univ. of Florida.           */
/* web: http://www.cise.ufl.edu/research/sparse/umfpack                      */
/* ------------------------------------------------------------------------- */

/***********************************************************************/
/*         UMFPACK Copyright, License and Availability                 */
/***********************************************************************/
/*
 *
 * UMFPACK Version 4.1 (Apr. 30, 2003),  Copyright (c) 2003 by Timothy A.
 * Davis.  All Rights Reserved.
 *
 * UMFPACK License:
 *
 *   Your use or distribution of UMFPACK or any modified version of
 *   UMFPACK implies that you agree to this License.
 *
 *   THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
 *   EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
 *
 *   Permission is hereby granted to use or copy this program, provided
 *   that the Copyright, this License, and the Availability of the original
 *   version is retained on all copies.  User documentation of any code that
 *   uses UMFPACK or any modified version of UMFPACK code must cite the
 *   Copyright, this License, the Availability note, and "Used by permission."
 *   Permission to modify the code and to distribute modified code is granted,
 *   provided the Copyright, this License, and the Availability note are
 *   retained, and a notice that the code was modified is included.  This
 *   software was developed with support from the National Science Foundation,
 *   and is provided to you free of charge.
 *
 * Availability:
 *
 *   http://www.cise.ufl.edu/research/sparse/umfpack
 *
 */

/* Used by permission. */

/* Simple demo program for UMFPACK               */
/* from UMFPACK Version 4.1 Quick Start Guide    */

/* modified by Kresimir Fresl, 2003              */
/* UMFPACK bindings & ublas::compressed_matrix<> */

#if !defined(TEST_MATLIB_UBLAS) && !defined(TEST_MATLIB_GLAS) && !defined(TEST_MATLIB_MTL) && !defined(TEST_MATLIB_EIGEN)
#define TEST_MATLIB_UBLAS
#endif

#include <iostream>
#include <boost/numeric/bindings/umfpack/umfpack.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#if defined(TEST_MATLIB_UBLAS)

#include <boost/numeric/bindings/ublas/matrix_sparse.hpp>
namespace ublas = boost::numeric::ublas;
typedef ublas::compressed_matrix<double, ublas::column_major, 0,
    ublas::unbounded_array<int>, ublas::unbounded_array<double> > m_t;
typedef m_t& mi_t;
#define AA(i,j, val) a(i,j) = val

#elif defined(TEST_MATLIB_GLAS)

#include <boost/numeric/bindings/glas/compressed.hpp>
typedef glas::sparse_matrix< double, glas::compressed_sparse_structure<
    glas::column_orientation, std::ptrdiff_t, std::ptrdiff_t, 0> > m_t;
typedef m_t& mi_t;
#define AA(i,j, val) insert(a,i,j, val)

#elif defined(TEST_MATLIB_MTL)

#include <boost/numeric/bindings/mtl/compressed2D.hpp>
typedef mtl::compressed2D<double, mtl::matrix::parameters<mtl::tag::col_major> > m_t;
typedef mtl::matrix::inserter<m_t> mi_t;
#define AA(i,j, val) a(i,j) = val

#elif defined(TEST_MATLIB_EIGEN)

#include <boost/numeric/bindings/eigen/sparsematrix.hpp>
typedef Eigen::SparseMatrix<double> m_t;
typedef Eigen::RandomSetter<m_t> mi_t;
#define AA(i,j, val) a(i,j) = val

#endif

typedef boost::numeric::ublas::vector<double> v_t;
namespace umf = boost::numeric::bindings::umfpack;

int main() {

  m_t A (5,5
#if !defined(TEST_MATLIB_EIGEN)
        ,12
#endif
        );
  v_t B (5), X (5);

  {
    mi_t a(A);
    AA(0,0, 2.); AA(0,1, 3.);
    AA(1,0, 3.); AA(1,2, 4.); AA(1,4, 6.);
    AA(2,1,-1.); AA(2,2,-3.); AA(2,3, 2.);
    AA(3,2, 1.);
    AA(4,1, 4.); AA(4,2, 2.); AA(4,4, 1.);
  }
  B(0) = 8.; B(1) = 45.; B(2) = -3.; B(3) = 3.; B(4) = 19.;

  umf::symbolic_type<double> Symbolic;
  umf::numeric_type<double> Numeric;

  umf::symbolic (A, Symbolic);
  umf::numeric (A, Symbolic, Numeric);
  umf::solve (A, X, B, Numeric);

  std::cout << X << std::endl;  // output: [5](1,2,3,4,5)
}


/*=========================================================================
 Program:   ORFEO Toolbox
 Language:  C++
 Date:      $Date$
 Version:   $Revision$
 Created By:Bouceffa Walid
 Email:	    bouceffa.walid@gmail.com

 Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
 See OTBCopyright.txt for details.

 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.

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

#include "otbFeatureSelection.h"

#include <boost/smart_ptr.hpp>
#include <exception>
#include <iostream>
#include <cstdlib>
#include <string>
#include <vector>
#include "error.hpp"
#include "global.hpp"
#include "subset.hpp"

#include "data_intervaller.hpp"
#include "data_splitter.hpp"
#include "data_splitter_5050.hpp"
#include "data_splitter_cv.hpp"
//#include "data_splitter_holdout.hpp"
//#include "data_splitter_leave1out.hpp"
//#include "data_splitter_resub.hpp"
#include "data_splitter_randrand.hpp"
//#include "data_splitter_randfix.hpp"
#include "data_scaler.hpp"
#include "data_scaler_void.hpp"
//#include "data_scaler_to01.hpp"
//#include "data_scaler_white.hpp"

#include "data_accessor_splitting_memOTB.hpp"

#include "criterion_normal_bhattacharyya.hpp"
//#include "criterion_normal_gmahalanobis.hpp"
//#include "criterion_normal_divergence.hpp"
//#include "criterion_multinom_bhattacharyya.hpp"
#include "criterion_wrapper.hpp"
//#include "criterion_wrapper_bias_estimate.hpp"
//#include "criterion_subsetsize.hpp"
//#include "criterion_sumofweights.hpp"
//#include "criterion_negative.hpp"

#include "distance_euclid.hpp"
//#include "distance_L1.hpp"
#include "distance_Lp.hpp"
#include "classifier_knn.hpp"
//#include "classifier_normal_bayes.hpp"
//#include "classifier_multinom_naivebayes.hpp"
#include "classifier_svm.hpp"

//#include "search_bif.hpp"
//#include "search_bif_threaded.hpp"

//#include "search_exhaustive.hpp"
//#include "search_exhaustive_threaded.hpp"
#include "branch_and_bound_predictor_averaging.hpp"
//#include "search_branch_and_bound_basic.hpp"
#include "search_branch_and_bound_improved.hpp"
#include "search_branch_and_bound_partial_prediction.hpp"
#include "search_branch_and_bound_fast.hpp"
#include "seq_step_straight.hpp"
//#include "seq_step_straight_threaded.hpp"
//#include "seq_step_hybrid.hpp"
//#include "seq_step_ensemble.hpp"
//#include "search_seq_sfs.hpp"
#include "search_seq_sffs.hpp"
//#include "search_seq_sfrs.hpp"
//#include "search_seq_os.hpp"
#include "search_seq_dos.hpp"
#include "result_tracker_dupless.hpp"
//#include "result_tracker_regularizer.hpp"
#include "result_tracker_feature_stats.hpp"
//#include "result_tracker_stabileval.hpp"

namespace otb
{
namespace Wrapper
{

typedef double RETURNTYPE;
typedef float DATATYPE;
typedef double REALTYPE;
typedef int IDXTYPE;
typedef unsigned int DIMTYPE;
typedef short BINTYPE;
typedef FST::Subset<BINTYPE, DIMTYPE> SUBSET;

typedef FST::Data_Intervaller<std::vector<FST::Data_Interval<IDXTYPE> >,IDXTYPE> INTERVALLER;
typedef boost::shared_ptr<FST::Data_Splitter<INTERVALLER,IDXTYPE> > PSPLITTER;
typedef FST::Data_Splitter_CV<INTERVALLER,IDXTYPE> SPLITTERCV;
typedef FST::Data_Splitter_5050<INTERVALLER,IDXTYPE> SPLITTER5050;
typedef FST::Data_Splitter_RandomRandom<INTERVALLER,IDXTYPE,BINTYPE> SPLITTERRR;
typedef FST::Data_Accessor_Splitting_MemOTB<DATATYPE,IDXTYPE,INTERVALLER> DATAACCESSOR;
typedef FST::Distance_Euclid<DATATYPE,DIMTYPE,SUBSET> DISTANCE;

typedef FST::Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE> CLASSIFIERKNN;
typedef FST::Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> CLASSIFIERSVM;

typedef FST::Criterion_Wrapper<RETURNTYPE,SUBSET,CLASSIFIERKNN,DATAACCESSOR> WRAPPERKNN;
typedef FST::Criterion_Wrapper<RETURNTYPE,SUBSET,CLASSIFIERSVM,DATAACCESSOR> WRAPPERSVM;

typedef FST::Sequential_Step_Straight<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN> EVALUATOR;
typedef FST::Sequential_Step_Straight<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERSVM> EVALUATOR_SVM;

typedef FST::Result_Tracker_Feature_Stats<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET> TRACKERSTATS;

void FeatureSelection::InitFSTParams()
{

   AddChoice ( "method.sffs", "sffs Wrapper-based feature selection with Floating Search" );
   AddParameter ( ParameterType_Choice, "method.sffs.searchdirection", "Sequential Forward Floating Selection search procedure" );
   AddChoice("method.sffs.searchdirection.forward", "forward search ");
   AddChoice("method.sffs.searchdirection.backward", "backward search ");
   SetParameterString("method.sffs.searchdirection", "forward");

   AddChoice ( "method.dos", "dos Combined feature subset contents, size and SVM parameters optimization.." );
   AddParameter ( ParameterType_Choice, "method.dos.svmkernel", "svm kernel to use " );
   AddChoice("method.dos.svmkernel.linear", "linear kernel");
   AddChoice("method.dos.svmkernel.rbf", "rbf kernel  ");
   AddChoice("method.dos.svmkernel.poly", "poly kernel ");
   SetParameterString("method.dos.svmkernel", "rbf");
}

FeatureSelection::ImportanceVector FeatureSelection::getFeatureImportanceDOS ( ListSampleType::Pointer trainingListSample,
        LabelListSampleType::Pointer trainingLabeledListSample )
{

          std::cout << "Starting (Missing data substitution) Combined feature subset contents, size and SVM parameters optimization..." << std::endl;
        // randomly sample 50% of data for training and randomly sample (disjunct) 40% for independent testing of final classification performance
        PSPLITTER dsp_outer ( new SPLITTERRR ( 1, 50, 40 ) ); // (there will be one outer randomized split only)
        // in the course of search use the first half of data by 3-fold cross-validation in wrapper FS criterion evaluation
        PSPLITTER dsp_inner ( new SPLITTERCV ( 3 ) );
        // do not scale data
        const DATATYPE missing_value_code=5;
        boost::shared_ptr<FST::Data_Scaler<DATATYPE> > dsc ( new FST::Data_Scaler_void<DATATYPE> ( missing_value_code,1/*to choose correct constructor*/ ) );
        // set-up data access
        boost::shared_ptr<std::vector<PSPLITTER> > splitters ( new std::vector<PSPLITTER> );
        splitters->push_back ( dsp_outer );
        splitters->push_back ( dsp_inner );
        boost::shared_ptr<DATAACCESSOR> da ( new DATAACCESSOR ( trainingListSample,trainingLabeledListSample,splitters,dsc ) );
        da->initialize();
        // initiate access to split data parts
        da->setSplittingDepth ( 0 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "50/40 random data split failed." );
        da->setSplittingDepth ( 1 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "3-fold cross-validation failure." );
        // initiate the storage for subset to-be-selected + another one as temporary storage
        boost::shared_ptr<SUBSET> sub ( new SUBSET ( da->getNoOfFeatures() ) );
        boost::shared_ptr<SUBSET> sub_temp ( new SUBSET ( da->getNoOfFeatures() ) );
        // set-up SVM (interface to external library LibSVM)
        boost::shared_ptr<CLASSIFIERSVM> csvm ( new CLASSIFIERSVM );

        //csvm->set_kernel_type ( RBF ); // (option: LINEAR, RBF, POLY)

        std::string svm_kernel  = GetParameterAsString("method.dos.svmkernel");

	if( svm_kernel == "linear")
	    csvm->set_kernel_type ( LINEAR );

	else if( svm_kernel == "poly")
	   csvm->set_kernel_type ( POLY );

	else
	     csvm->set_kernel_type ( RBF );

        csvm->initialize ( da );
        // wrap the SVM classifier to enable its usage as FS criterion (criterion value will be estimated by 3-fold cross-val.)
        boost::shared_ptr<WRAPPERSVM> wsvm ( new WRAPPERSVM );
        wsvm->initialize ( csvm,da );
        // set-up the standard sequential search step object (option: hybrid, ensemble)
        boost::shared_ptr<EVALUATOR_SVM> eval ( new EVALUATOR_SVM );
        // set-up Dynamic Oscillating Search procedure
        FST::Search_DOS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERSVM,EVALUATOR_SVM> srch ( eval );
        srch.set_delta ( 3 );
        // run the search
        std::cout << "Feature selection setup:" << std::endl << *da << std::endl << srch << std::endl << *wsvm << std::endl << std::endl;
        RETURNTYPE bestcritval_train, critval_train, critval_test;
        sub->select_all();
        csvm->optimize_parameters ( da,sub );
        double best_svm_param_C=csvm->get_parameter_C();
        double best_svm_param_gamma=csvm->get_parameter_gamma();
        double best_svm_param_coef0=csvm->get_parameter_coef0();
        bool stop=false;
        sub->deselect_all();
        if ( !srch.search ( 0,bestcritval_train,sub,wsvm, std::cout ) ) throw FST::fst_error ( "Search not finished." );

        /**
        		sub_temp->stateless_copy(*sub);
        		while(!stop)
        		{
        			csvm->optimize_parameters(da,sub);
        			if(!srch.search(0,critval_train,sub_temp,wsvm,std::cout)) throw FST::fst_error("Search not finished.");
        			if(critval_train>bestcritval_train)
        			{
        				bestcritval_train=critval_train;
        				sub->stateless_copy(*sub_temp);
        				best_svm_param_C=csvm->get_parameter_C();
        				best_svm_param_gamma=csvm->get_parameter_gamma();
        				best_svm_param_coef0=csvm->get_parameter_coef0();
        			} else stop=true;
        		}
        		std::cout << std::endl << "Search result: " << std::endl << *sub << std::endl << "Criterion value=" << bestcritval_train << std::endl << std::endl;
        **/
        // 	// (optionally) validate result by estimating SVM accuracy on selected feature sub-space on independent test data
// 		da->setSplittingDepth(0);
// 		csvm->set_parameter_C(best_svm_param_C);
// 		csvm->set_parameter_gamma(best_svm_param_gamma);
// 		csvm->set_parameter_coef0(best_svm_param_coef0);
// 		csvm->train(da,sub);
// 		csvm->test(critval_test,da);
// 		std::cout << "Validated SVM accuracy=" << critval_test << std::endl << std::endl;
//

ImportanceVector vImp;
        vImp.reserve ( sub->get_n() );
        for ( DIMTYPE d=0; d<sub->get_n(); d++ )
        {
            if ( sub->selected_raw ( d ) )
                vImp.push_back ( std::make_pair ( d+1, 1 ) );
            else
                vImp.push_back ( std::make_pair ( d+1, 0 ) );
        }

  return vImp;
}

FeatureSelection::ImportanceVector FeatureSelection::getFeatureImportanceSFFS ( ListSampleType::Pointer trainingListSample,
        LabelListSampleType::Pointer trainingLabeledListSample )
{

  // otbAppLogINFO ( "SFFS GetFeatureImporance " );

    std::cout << "Generalized sequential feature subset search..." << std::endl;
        // keep second half of data for independent testing of final classification performance
        PSPLITTER dsp_outer ( new SPLITTER5050() );
        // in the course of search use the first half of data by 3-fold cross-validation in wrapper FS criterion evaluation
        PSPLITTER dsp_inner ( new SPLITTERCV ( 3 ) );
        // do not scale data
        boost::shared_ptr<FST::Data_Scaler<DATATYPE> > dsc ( new FST::Data_Scaler_void<DATATYPE>() );
        // set-up data access
        boost::shared_ptr<std::vector<PSPLITTER> > splitters ( new std::vector<PSPLITTER> );
        splitters->push_back ( dsp_outer );
        splitters->push_back ( dsp_inner );

        //boost::shared_ptr<DATAACCESSOR> da(new DATAACCESSOR("data/speech_15.trn",splitters,dsc));
        boost::shared_ptr<DATAACCESSOR> da ( new DATAACCESSOR ( trainingListSample,trainingLabeledListSample,splitters,dsc ) );

        da->initialize();
        // initiate access to split data parts
        da->setSplittingDepth ( 0 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "50/50 data split failed." );
        da->setSplittingDepth ( 1 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "3-fold cross-validation failure." );
        // initiate the storage for subset to-be-selected
        boost::shared_ptr<SUBSET> sub ( new SUBSET ( da->getNoOfFeatures() ) );
        sub->deselect_all();
        // set-up 3-Nearest Neighbor classifier based on Euclidean distances
        boost::shared_ptr<CLASSIFIERKNN> cknn ( new CLASSIFIERKNN );
        cknn->set_k ( 5 );
        // wrap the 3-NN classifier to enable its usage as FS criterion (criterion value will be estimated by 3-fold cross-val.)
        boost::shared_ptr<WRAPPERKNN> wknn ( new WRAPPERKNN );
        wknn->initialize ( cknn,da );
        // set-up the standard sequential search step object (option: hybrid, ensemble, etc.)
        boost::shared_ptr<EVALUATOR> eval ( new EVALUATOR );
        // set-up Sequential Forward Floating Selection search procedure
        FST::Search_SFFS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,EVALUATOR> srch ( eval );

	//srch.set_search_direction ( FST::FORWARD ); // try FST::BACKWARD
      std::string search_method = GetParameterAsString("method.sffs.searchdirection") ;

      if(search_method == "backward")
	srch.set_search_direction ( FST::BACKWARD );
      else
	srch.set_search_direction ( FST::FORWARD );

      // set the size of feature groups to be evaluated for inclusion/removal in each sequential step (can be applied to SFS, SFFS, OS, DOS, SFRS)
        srch.set_generalization_level ( 2 );
        // run the search
        std::cout << "Feature selection setup:" << std::endl << *da << std::endl << srch << std::endl << *wknn << std::endl << std::endl;
        RETURNTYPE critval_train, critval_test;
        srch.set_output_detail ( FST::NORMAL ); // set FST::SILENT to disable all text output in the course of search (FST::NORMAL is default)
        if ( !srch.search ( 0,critval_train,sub,wknn,std::cout ) ) throw FST::fst_error ( "Search not finished." );
        // (optionally) validate result by estimating kNN accuracy on selected feature sub-space on independent test data
        //da->setSplittingDepth(0);
        //cknn->train(da,sub);
        //cknn->test(critval_test,da);
        //std::cout << "Validated "<<cknn->get_k()<<"-NN accuracy=" << critval_test << std::endl << std::endl;
        // (optionally) list the best known solutions for each cardinality as recorded throughout the course of search
        //std::cout << "Best recorded solution for subset size:" << std::endl;
        //for(DIMTYPE d=1;d<=sub->get_n();d++)
        //if(srch.get_result(d,critval_train,sub)) std::cout << d << ": val="<< critval_train << ", "<<*sub << std::endl;

	ImportanceVector vImp;

        vImp.reserve ( sub->get_n() );
        for ( DIMTYPE d=0; d<sub->get_n(); d++ )
        {
            if ( sub->selected_raw ( d ) )
                vImp.push_back ( std::make_pair ( d+1, 1 ) );
            else
                vImp.push_back ( std::make_pair ( d+1, 0 ) );
        }

    return vImp;
}

} //end namespace wrapper
} //end namespace otb


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// 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 The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <glog/logging.h>
#include <gflags/gflags.h>
#include <theia/theia.h>

#include <memory>
#include <string>

DEFINE_string(1dsfm_dataset_directory, "",
              "Dataset where the 1dSFM dataset is located. Do not include a "
              "trailing slash.");

using theia::Reconstruction;
using theia::TrackId;
using theia::ViewId;

// Computes the error in the relative rotation based on the ground truth
// rotations rotation1 and rotation2 (which specify world-to-camera
// transformations).
double ComputeRelativeRotationError(const Eigen::Matrix3d& rotation1,
                                    const Eigen::Matrix3d& rotation2,
                                    const Eigen::Vector3d& relative_rotation) {
  Eigen::Matrix3d relative_rotation_matrix;
  ceres::AngleAxisToRotationMatrix(
      relative_rotation.data(),
      ceres::ColumnMajorAdapter3x3(relative_rotation_matrix.data()));
  const Eigen::Matrix3d loop_rotation = relative_rotation_matrix.transpose() *
                                        (rotation2 * rotation1.transpose());
  Eigen::Vector3d loop_rotation_aa;
  ceres::RotationMatrixToAngleAxis(
      ceres::ColumnMajorAdapter3x3(loop_rotation.data()),
      loop_rotation_aa.data());
  return theia::RadToDeg(loop_rotation_aa.norm());
}

double ComputeRelativeTranslationError(
    const Eigen::Vector3d& position1,
    const Eigen::Vector3d& position2,
    const Eigen::Matrix3d& rotation1,
    const Eigen::Vector3d& relative_translation) {
  const Eigen::Vector3d world_translation =
      rotation1 * (position2 - position1).normalized();
  return theia::RadToDeg(acos(
      theia::Clamp(relative_translation.dot(world_translation), -1.0, 1.0)));
}

void EvaluateRelativeError(
    const theia::ViewGraph& view_graph,
    const Reconstruction& reconstruction_1dsfm,
    const Reconstruction& gt_reconstruction) {
  // For each edge, get the rotate translation and check the error.
  std::vector<double> histogram_bins = {2,  5,   10,  15,  25,  50,
                                        90, 135, 180, 225, 270, 315};
  theia::PoseError pose_error(histogram_bins, histogram_bins);

  const auto& edges = view_graph.GetAllEdges();
  for (const auto& edge : edges) {
    // The reconstruction/view graph from 1dSFM may have a different mapping of
    // names to ViewIds than the ground truth reconstruction, so we have to do a
    // name lookup in the ground truth reconstruction to ensure we have the same
    // view.
    const theia::View* view1_1dsfm =
        reconstruction_1dsfm.View(edge.first.first);
    const theia::View* view2_1dsfm =
        reconstruction_1dsfm.View(edge.first.second);
    if (view1_1dsfm == nullptr || view2_1dsfm == nullptr) {
      continue;
    }

    const ViewId view_id1 =
        gt_reconstruction.ViewIdFromName(view1_1dsfm->Name());
    const ViewId view_id2 =
        gt_reconstruction.ViewIdFromName(view2_1dsfm->Name());
    const theia::View* view1 = gt_reconstruction.View(view_id1);
    const theia::View* view2 = gt_reconstruction.View(view_id2);
    if (view1 == nullptr || view2 == nullptr) {
      continue;
    }
    const theia::Camera& camera1 = view1->Camera();
    const theia::Camera& camera2 = view2->Camera();

    const double rotation_angular_error =
        ComputeRelativeRotationError(camera1.GetOrientationAsRotationMatrix(),
                                     camera2.GetOrientationAsRotationMatrix(),
                                     edge.second.rotation_2);

    const double translation_angular_error = ComputeRelativeTranslationError(
        camera1.GetPosition(),
        camera2.GetPosition(),
        camera1.GetOrientationAsRotationMatrix(),
        edge.second.position_2);
    pose_error.AddError(rotation_angular_error, translation_angular_error);
  }

  LOG(INFO) << "Relative pose errors for 1dsfm = \n"
            << pose_error.PrintMeanMedianHistogram();
}

int main(int argc, char* argv[]) {
  google::InitGoogleLogging(argv[0]);
  THEIA_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);

  LOG(INFO) << "Reading the relative poses from the 1dsfm dataset.";
  std::unique_ptr<Reconstruction> reconstruction_1dsfm(new Reconstruction);
  std::unique_ptr<theia::ViewGraph> view_graph_1dsfm(new theia::ViewGraph);
  CHECK(Read1DSFM(FLAGS_1dsfm_dataset_directory,
                  reconstruction_1dsfm.get(),
                  view_graph_1dsfm.get()))
      << "Could not read 1dsfm dataset from " << FLAGS_1dsfm_dataset_directory;

  const std::string lists_file = FLAGS_1dsfm_dataset_directory + "/list.txt";
  const std::string bundle_file =
      FLAGS_1dsfm_dataset_directory + "/gt_bundle.out";
  std::unique_ptr<theia::Reconstruction> gt_reconstruction(
      new theia::Reconstruction());
  LOG(INFO) << "Converting ground truth bundler file to Theia reconstruction.";
  CHECK(
      theia::ReadBundlerFiles(lists_file, bundle_file, gt_reconstruction.get()))
      << "Could not the ground truth Bundler file at " << bundle_file;

  EvaluateRelativeError(*view_graph_1dsfm,
                        *reconstruction_1dsfm,
                        *gt_reconstruction);

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_DETAILS_RREF_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_DETAILS_RREF_HPP_INCLUDED

#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/expand.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/functions/ismatrix.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/mnorminf.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <boost/dispatch/meta/mpl.hpp>
#include <nt2/core/container/table/table.hpp>
//  rref   reduced row echelon form.
//     r = rref(a) produces the reduced row echelon form of a.

//     [r,jb] = rref(a) also returns a vector, jb, so that:
//         r = length(jb) is this algorithm's idea of the rank of a,
//         x(jb) are the bound variables in a linear system, ax = b,
//         a(:,jb) is a basis for the range of a,
//         r(1:r,jb) is the r-by-r identity matrix.

//     [r,jb] = rref(a,tol) uses the given tolerance in the rank tests.

//     roundoff errors may cause this algorithm to compute a different
//     value for the rank than rank, orth and null.

namespace nt2 { namespace details
{
  template<class T> struct rref_result
  {
    typedef typename meta::strip<T>::type                   source_t;
    typedef typename source_t::value_type                     type_t;
    typedef typename source_t::index_type                    index_t;
    typedef typename meta::as_real<type_t>::type              base_t;
    typedef T                                                 data_t;
    typedef typename meta::as_integer<base_t, signed>::type  itype_t;
    typedef nt2::container::table<type_t,nt2::matlab_index_>              tab_t;
    typedef nt2::container::table<base_t,nt2::matlab_index_>             btab_t;
    typedef nt2::container::table<itype_t,nt2::matlab_index_>            itab_t;
    typedef nt2::container::table<nt2_la_int,nt2::matlab_index_>         ibuf_t;
    typedef nt2::container::table<type_t,index_t>                   result_type;
    typedef nt2::container::table<base_t,index_t>                  bresult_type;
    typedef nt2::container::table<itype_t,index_t>                 iresult_type;
    //must be dry I think

    template<class Input>
    rref_result ( Input& xpr, base_t tol)
      : tol_(tol)
      , a_(xpr)
      , n_( nt2::height(a_)  )
      , m_( nt2::width(a_)  )
      , jb_(of_size(1, n_))
    {
      BOOST_ASSERT_MSG(ismatrix(a_), "input to rref must be matrix");
      if (tol < Zero<base_t>()) tol = nt2::max(m_,n_)*nt2::Eps<base_t>()*nt2::mnorminf(a_);
      itype_t i = 1, j = 1;
      itype_t k = 0;
      base_t p;
      itype_t cnt = 1;
      while(i <= m_ && j <= n_)
      {
        //          tie(p, k) =  nt2::max(nt2::abs(a_(_(i, m_),j))); //TODO
        p = nt2::globalmax(nt2::abs(a_(_(i, m_),j)));
        for(int l = i; l <= m_; ++l) if (nt2::abs(a_(l, j)) == p) { k = l; break; }
        //k = k+i-1;
        if (p <= tol)
        {
          // the column is negligible, zero it out.
          a_(_(i, m_),j) = nt2::zeros(m_-i+1, 1, meta::as_<type_t>());
          ++j;
        }
        else
        {
          // remember column index
          //jb_ = cath(jb_, j); //TODO
          jb_(cnt) = j; ++cnt;
          // swap i-th and k-th rows.
          tab_t tmp = a_(i, _(j, n_));
          a_(i, _(j, n_)) = a_(k, _(j, n_));
          a_(k, _(j, n_)) = tmp;
          //              a_(cath(i, k),_(j, n)) = a_(cath(k, i),_(j, n));
          // divide the pivot row by the pivot element.
          type_t tmp1 =  a_(i, j);
          a_(i,_(j, n_)) = a_(i,_(j, n_))/tmp1;
          // subtract multiples of the pivot row from all the other rows.
          for (itype_t kk = 1; kk <= m_; ++kk)//[1:i-1 i+1:m]
          {
            if (kk!=i)
            {
              type_t tmp2 = a_(kk,j);
              a_(kk,_(j, n_)) = a_(kk,_(j, n_))- tmp2*a_(i,_(j, n_));
            }
          }
          ++i; ++j;
        }
      }
      jb_ =  nt2::expand(jb_, 1, --cnt);
    }

    rref_result& operator=(rref_result const& src)
    {
      tol_    = src.tol_;
      a_      = src.a_;
      n_      = src.n_;
      m_      = src.m_;
      jb_     = src.jb_;
      return *this;
    }

    rref_result(rref_result const& src)
      : tol_(src.tol_),
        a_(src.a_),
        n_(src.n_),
        m_(src.m_),
        jb_(src.jb_)
    {}

    //==========================================================================
    // Return raw values
    //==========================================================================
    data_t values() const { return a_; }
    data_t rref() const { return a_; }

    //==========================================================================
    // Return permutation
    //==========================================================================
    const itab_t& jb() const
    {
      //typedef typename boost::mpl::at_c<typename index_t::type,0>::type base;
      return jb_; //+ base::value + Mone<itype_t>();
    }

  private:
    btab_t                         tol_;
    data_t                           a_;
    nt2_la_int                       n_;
    nt2_la_int                       m_;
    itab_t                          jb_;
  };
} }

#endif

// /////////////////////////////////////////////////////////////////////////////
// End of rref.hpp
// /////////////////////////////////////////////////////////////////////////////


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_ITL_INCLUDE
#define ITL_ITL_INCLUDE

#include <boost/numeric/itl/iteration/basic_iteration.hpp>
#include <boost/numeric/itl/iteration/cyclic_iteration.hpp>
#include <boost/numeric/itl/iteration/noisy_iteration.hpp>

#include <boost/numeric/itl/krylov/cg.hpp>
#include <boost/numeric/itl/krylov/cgs.hpp>
#include <boost/numeric/itl/krylov/bicg.hpp>
#include <boost/numeric/itl/krylov/bicgstab.hpp>
#include <boost/numeric/itl/krylov/bicgstab_2.hpp>
#include <boost/numeric/itl/krylov/bicgstab_ell.hpp>
#include <boost/numeric/itl/krylov/fsm.hpp>
#include <boost/numeric/itl/krylov/idr_s.hpp>
#include <boost/numeric/itl/krylov/gmres.hpp>
#include <boost/numeric/itl/krylov/tfqmr.hpp>
#include <boost/numeric/itl/krylov/qmr.hpp>
#include <boost/numeric/itl/krylov/pc_solver.hpp>

#include <boost/numeric/itl/krylov/repeating_solver.hpp>

#include <boost/numeric/itl/minimization/quasi_newton.hpp>

#include <boost/numeric/itl/pc/identity.hpp>
#include <boost/numeric/itl/pc/is_identity.hpp>
#include <boost/numeric/itl/pc/diagonal.hpp>
#include <boost/numeric/itl/pc/ilu.hpp>
#include <boost/numeric/itl/pc/ilu_0.hpp>
#include <boost/numeric/itl/pc/ilut.hpp>
#include <boost/numeric/itl/pc/ic_0.hpp>

#include <boost/numeric/itl/pc/imf_preconditioner.hpp>
#include <boost/numeric/itl/pc/imf_algorithms.hpp>

#include <boost/numeric/itl/pc/sub_matrix_pc.hpp>
#include <boost/numeric/itl/pc/concat.hpp>

#include <boost/numeric/itl/smoother/gauss_seidel.hpp>

#include <boost/numeric/itl/stepper/armijo.hpp>
#include <boost/numeric/itl/stepper/wolf.hpp>

#include <boost/numeric/itl/updater/bfgs.hpp>
#include <boost/numeric/itl/updater/broyden.hpp>
#include <boost/numeric/itl/updater/dfp.hpp>
#include <boost/numeric/itl/updater/psb.hpp>
#include <boost/numeric/itl/updater/sr1.hpp>

#endif // ITL_ITL_INCLUDE


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::product_pdf::product_pdf.hpp              //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_PRODUCT_PDF_PRODUCT_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_PRODUCT_PDF_PRODUCT_PDF_HPP_ER_2009
#include <boost/statistics/detail/distribution_common/meta/inherit/policy.hpp>
#include <boost/statistics/detail/distribution_common/meta/inherit/value.hpp>
#include <boost/statistics/detail/distribution_toolkit/meta/is_pseudo_scalar_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    // A pseudo distribution resulting from product of the pdf of two
    // distributions
    template<typename A,typename B>
    class product_pdf : public distribution::meta::inherit_policy<
        A,
        distribution::meta::inherit_value<A>
    >{

        public:

        product_pdf(){}
        explicit product_pdf(
            const A& a,
            const B& b
        ):a_(a),b_(b){}

        product_pdf(const product_pdf& that):a_(that.a_),b_(that.b_){}

        product_pdf&
        operator=(const product_pdf& that){
            if(&that!=this){
                a_ = that.a_;
                b_ = that.b_;
            }
            return *this;
        }

        const A& first()const{ return a_; }
        const B& second()const{ return b_; }

        protected:
        A a_;
        B b_;
    };

    template<typename A,typename B>
    product_pdf<A,B>
    make_product_pdf(const A& a,const B& b){
        return product_pdf<A,B>(a,b);
    }

}// distribution
}// toolkit

}// detail
}// statistics
}// boost

#endif


#include <include/common.hpp>

#include <include/options.hpp>

#include <include/prob.hpp>
#include <include/util.hpp>

#include <vector>
#include <iostream>
#include <random>

#include <boost/math/special_functions/binomial.hpp>

// random number generator object for std random library
std::mt19937 gen;

// probability of equality of two calls of the SAME reference position
double p_equal_calls = 0.0;
double q_equal_calls = 0.0;

// probability of 'sequenced position' which represents the probability of a read
// starting at a given position of the reference
double p_read_start = 0.0;

// geometric distribution for inter-read distance
std::geometric_distribution<> geom;

// lookup tables for probability of the form
// p^x * (1-p)^{m-s}
double *power_peq_lookup = NULL; // p^i
double *power_qeq_lookup = NULL; // q^i = (1-p)^i

/* lookup tables for the approximated expected score defined as:
                         4^s * tildeI(s, peq)
     tildeE(A,B,s) =  ----------------------------
                      4^s * tildeI(s, peq) + N - 1
 */
double *approxExpScoreNum = NULL;
double *approxExpScoreDen = NULL;

void initProbabilities() {

    std::random_device rd;
    gen = std::mt19937(rd());

    // convenience variables
    double p_err = Options::opts.pe;
    double q_err = 1.0 - p_err;

    // init p_equal_calls and q_equal_call
    p_equal_calls = q_err * q_err + (1.0 / 3.0) * (p_err * p_err);
    q_equal_calls = (1.0 - p_equal_calls) / 3.0;

    // init p_read_start
    p_read_start = (double) Options::opts.M / (double) Options::opts.N;

    // init online inter-read distance distribution (i.e., geomtric)
    geom = std::geometric_distribution<>(p_read_start);

    // lookup tables
    int m = Options::opts.m;
    int N = Options::opts.N;
    power_peq_lookup = new double[m + 1];
    power_qeq_lookup = new double[m + 1];
    approxExpScoreNum = new double[m + 1];
    approxExpScoreDen = new double[m + 1];
    for (int s = 0; s <= m; ++s) {
        power_peq_lookup[s] = pow(p_equal_calls, s);
        power_qeq_lookup[s] = pow(q_equal_calls, s);
        double tmp = p_equal_calls * p_equal_calls + q_equal_calls * (1 - p_equal_calls);
        double tildeI = pow(tmp, s);
        approxExpScoreNum[s] = pow(4, s) * tildeI;
        approxExpScoreDen[s] = (pow(4, s) * tildeI) + N - 1;

    }
}

void clearProbabilities() {
    delete[] approxExpScoreNum;
    delete[] approxExpScoreDen;
    delete[] power_qeq_lookup;
    delete[] power_peq_lookup;

}

double indicatorErr(const std::string &r1, const std::string &r2, size_t s) {
    if (s > 0) {
        size_t hamm_d = prefixSuffixHammingDistance(r1, r2, s);
        return (power_peq_lookup[s - hamm_d] * power_qeq_lookup[hamm_d]);
    }
    return 1.0;

}

double indicatorNoErr(const std::string &r1, const std::string &r2, size_t s) {
    return (prefixSuffixHammingDistance(r1, r2, s) == 0);
}

double probabilityReads(const std::string &s1, const std::string &s2, size_t d) {

    // probability of D = d distance between reads
    double p_s = p_read_start * pow(1.0 - p_read_start, d);
    // no overlap
    if (d >= Options::opts.m) {
        return pow(4, 2 * Options::opts.m) * p_s;
    }

    size_t dh = prefixSuffixHammingDistance(s1, s2, Options::opts.m - d);
    // probability of dh unequal bases on the common part
    double p_err = pow(p_equal_calls, (Options::opts.m - d - dh)) * pow(1.0 - p_equal_calls, dh);
    // probability of indipendent parts
    double p_ind = pow(4, 2 * d);

    return p_s * p_err * p_ind;
}

/**
 * Compute
 *  \sum_{s}{I(A,B,s)4^s}
 */
double overlappingStringsSum(const std::string &s1, const std::string &s2) {
    size_t m = Options::opts.m;
    double sum = 0.0;
    for (size_t s = 1; s <= m - 1; ++s) {
        size_t indicator_ab = (prefixSuffixHammingDistance(s2, s1, s) == 0) ? 1 : 0;
        size_t indicator_ba = (prefixSuffixHammingDistance(s1, s2, s) == 0) ? 1 : 0;
        sum += power4_lookup[s] * (indicator_ab + indicator_ba);
    }
    size_t indic_m = (prefixSuffixHammingDistance(s2, s1, m) == 0) ? 1 : 0;
    sum += indic_m * power4_lookup[m];
    return sum;
}

double overlappingStringsSumWithErr(const std::string &s1, const std::string &s2) {
    double sum = 0.0;
    size_t m = Options::opts.m;
    for (size_t s = 1; s <= m - 1; ++s) {
        // WARNING: If needed here we can use lookup tables to make things faster
        double tab = indicatorErr(s1, s2, s);
        double tba = indicatorErr(s2, s1, s);
        sum += power4_lookup[s] * (tab + tba);
    }
    sum += power4_lookup[m] * indicatorErr(s1, s2, m);
    return sum;
}

/**
 * Computes probability of a random overlap of 's' bases with an hamming distance dh
 */
double randomReadsOverlapProbNoErr(const std::string &s1, const std::string &s2, size_t s) {
    size_t dh_for_s = prefixSuffixHammingDistance(s1, s2, s);
    if (dh_for_s != 0) {
        return 0;
    }
    double olap_p = overlappingStringsSum(s1, s2) + (double) Options::opts.N - 2 * (double) Options::opts.m + 1.0;
    return (pow(4, s) / (olap_p));
}

/**
 * Gets the distance until the next read
 */
size_t generateInterReadDistance() {
    return geom(gen);
}

EmpiricalDistribution::EmpiricalDistribution(double a, double b, size_t N)
        : f(N + 1, 0.0) {
    this->xa = a;
    this->xb = b;
    this->n = N + 1;
    this->step = (b - a) / (double) N;
    this->total = 0;
}

size_t EmpiricalDistribution::indexForSample(double x) const {
    return floor((x - xa) / step);
}

double EmpiricalDistribution::valueAtIndex(size_t i) const {
    return (f[i] / (double) total);
}

void EmpiricalDistribution::addSample(double x) {
    total++;
    size_t i = indexForSample(x);
    f[i] += 1;
}

void EmpiricalDistribution::getCDF(std::vector<double> &cdf) const {
    if (this->n == 0) {
        return;
    }
    cdf.reserve(this->n);
    cdf[0] = this->f[0];
    for (size_t i = 1; i < this->n; ++i) {
        cdf[i] = cdf[i - 1] + f[i];
    }
    // normalize between 0 and 1
    for (size_t i = 0; i < this->n; ++i) {
        cdf[i] /= cdf[this->n - 1];
    }
}

size_t percentileIndex(const std::vector<double> &cdf, double perc) {
    size_t idx = 0;
    size_t n = cdf.size();
    for (size_t i = 0; i < n; ++i) {
        if (cdf[i] >= perc) {
            break;
        }
        ++idx;
    }
    return idx;
}

double approximatedScore(size_t s, double *num_den) {
    num_den[0] = approxExpScoreNum[s];
    num_den[1] = approxExpScoreDen[s];
    return approxExpScoreNum[s] / approxExpScoreDen[s];
}

double approximatedScore(size_t s) {
    return approxExpScoreNum[s] / approxExpScoreDen[s];
}

double score(const std::string &r1, const std::string &r2, size_t s) {

    static double iidTerm = power4_lookup[0] *
                            ((double) Options::opts.N - 2.0 * (double) Options::opts.m + 1.0);

    double den = iidTerm + overlappingStringsSumWithErr(r1, r2);
    double num = indicatorErr(r1, r2, s) * power4_lookup[s];

    return num / den;
}

double scoreExt(const std::string &r1, const std::string &r2, size_t s, double *num_den) {

    static double iidTerm = power4_lookup[0] *
                            ((double) Options::opts.N - 2.0 * (double) Options::opts.m + 1.0);

    num_den[0] = iidTerm + overlappingStringsSumWithErr(r1, r2);
    num_den[1] = indicatorErr(r1, r2, s) * power4_lookup[s];

    return num_den[1] / num_den[0];
}

namespace lbio {
namespace prob {

//////////////////////////////////////////////////////////////////////
//               SamplingEstimationProcess CLASS
//////////////////////////////////////////////////////////////////////

SamplingEstimationProcess::SamplingEstimationProcess(size_t n_)
        : n{n_}, cumulativeSum{0}, cumulativeSumSquare{0}, k{0} {
    this->frequency = new size_t[n + 1];
    std::fill_n(this->frequency, n + 1, 0);
}

SamplingEstimationProcess::~SamplingEstimationProcess() {
    delete[] frequency;
}

void
SamplingEstimationProcess::newSample(size_t sample) {
    this->frequency[sample]++;
    this->cumulativeSum += sample;
    this->cumulativeSumSquare += (sample * sample);
    this->k++;
}

double
SamplingEstimationProcess::sampleMean() const {
    return ((double) cumulativeSum) / ((double) k);
}

double
SamplingEstimationProcess::sampleVariance() const {
    double sMean = sampleMean();
    double meanTerm = ((double) k) * sMean * sMean;
    return ((this->cumulativeSumSquare - meanTerm) / ((double) k - 1));
}

double
SamplingEstimationProcess::standardError() const {
    return std::sqrt(sampleMean() / sampleVariance());
}

size_t
SamplingEstimationProcess::medianForSampleDistribution() const {
    return medianFromFrequency<size_t>(frequency, n + 1);
}

size_t
SamplingEstimationProcess::sampleSize() const {
    return k;
}

void
SamplingEstimationProcess::writeFrequencyOnFile(const std::string &path) {
    std::ofstream os(path, std::ofstream::out);
    writeVectorOnStream<size_t>(frequency, n + 1, os);
    os.close();
}

SampleEstimates
SamplingEstimationProcess::toSampleEstimates() const {
    SampleEstimates est;
    est.sampleSize = k;
    est.sampleMean = sampleMean();
    est.sampleVariance = sampleVariance();
    return est;
}

}
} // namespaces


/********************************************************************************
 *
 * Data structures for the symbolic manipulation of extended linear constraints.
 *
 * Author: Maxime Arthaud (maxime@arthaud.me)
 *
 * Copyright (c) 2014 Carnegie Mellon University
 *
 * 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 IKOS_EXTENDED_CONSTRAINTS_HPP
#define IKOS_EXTENDED_CONSTRAINTS_HPP

#include <boost/optional.hpp>
#include <ikos/common.hpp>
#include <ikos/patricia_trees.hpp>
#include <ikos/collections.hpp>
#include <ikos/linear_constraints.hpp>

namespace ikos {

  template< typename Number, typename VariableName >
  class extended_constraint: public writeable {

  public:
    typedef extended_constraint< Number, VariableName > extended_constraint_t;
    typedef linear_constraint< Number, VariableName > linear_constraint_t;
    typedef variable< Number, VariableName > variable_t;
    typedef linear_expression< Number, VariableName > linear_expression_t;
    typedef patricia_tree_set< variable_t > variable_set_t;
    typedef enum {
      INF,
      INF_EQ,
      SUP,
      SUP_EQ,
      EQ,
      NOT_EQ,
      MOD
    } kind_t;
    typedef typename linear_expression_t::iterator iterator;

  private:
    kind_t _kind;
    linear_expression_t _expr;
    boost::optional<Number> _modulus;

  public:
    extended_constraint(): _kind(EQ) { }

    extended_constraint(linear_expression_t expr, kind_t kind, Number modulus): _kind(kind), _expr(expr), _modulus(modulus) { }

    extended_constraint(linear_expression_t expr, kind_t kind): _kind(kind), _expr(expr) { }

    extended_constraint(linear_constraint_t constraint): _kind(EQ), _expr(constraint.expression()) {
      if(constraint.is_inequality()) {
        _kind = INF_EQ;
      } else if(constraint.is_equality()) {
        _kind = EQ;
      } else {
        _kind = NOT_EQ;
      }
    }

    bool is_tautology() {
      switch (this->_kind) {
        case INF: {
          return (this->_expr.is_constant() && this->_expr.constant() < 0);
        }
        case INF_EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() <= 0);
        }
        case SUP: {
          return (this->_expr.is_constant() && this->_expr.constant() > 0);
        }
        case SUP_EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() >= 0);
        }
        case EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() == 0);
        }
        case NOT_EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() != 0);
        }
        case MOD: {
          return false; // TODO: type dependent
        }
        default: {
          throw error("Unreachable");
        }
      }
    }

    bool is_contradiction() {
      switch (this->_kind) {
        case INF: {
          return (this->_expr.is_constant() && this->_expr.constant() >= 0);
        }
        case INF_EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() > 0);
        }
        case SUP: {
          return (this->_expr.is_constant() && this->_expr.constant() <= 0);
        }
        case SUP_EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() < 0);
        }
        case EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() != 0);
        }
        case NOT_EQ: {
          return (this->_expr.is_constant() && this->_expr.constant() == 0);
        }
        case MOD: {
          return false; // TODO: type dependent
        }
        default: {
          throw error("Unreachable");
        }
      }
    }

    linear_expression_t expression() {
      return this->_expr;
    }

    kind_t kind() {
      return this->_kind;
    }

    boost::optional<Number> modulus() {
      return this->_modulus;
    }

    iterator begin() {
      return this->_expr.begin();
    }

    iterator end() {
      return this->_expr.end();
    }

    Number constant() {
      return -this->_expr.constant();
    }

    std::size_t size() {
      return this->_expr.size();
    }

    Number operator[](variable_t x) {
      return this->_expr.operator[](x);
    }

    variable_set_t variables() {
      return this->_expr.variables();
    }

    std::ostream& write(std::ostream& o) {
      if (this->is_contradiction()) {
        o << "false";
      } else if (this->is_tautology()) {
        o << "true";
      } else {
        linear_expression_t e = this->_expr - this->_expr.constant();
        o << e;
        switch (this->_kind) {
          case INF: {
            o << " < ";
            break;
          }
          case INF_EQ: {
            o << " <= ";
            break;
          }
          case SUP: {
            o << " > ";
            break;
          }
          case SUP_EQ: {
            o << " >= ";
            break;
          }
          case EQ: {
            o << " = ";
            break;
          }
          case NOT_EQ: {
            o << " != ";
            break;
          }
          case MOD: {
            o << " = ";
            break;
          }
          default: {
            throw error("Unreachable");
          }
        }
        Number c = -this->_expr.constant();
        o << c;

        if (this->_kind == MOD) {
          o << " [";
          o << *_modulus;
          o << "]";
        }
      }
      return o;
    }

  }; // class extended_constraint

  template< typename Number, typename VariableName >
  class extended_constraint_system: public writeable {

  public:
    typedef extended_constraint< Number, VariableName > extended_constraint_t;
    typedef extended_constraint_system< Number, VariableName > extended_constraint_system_t;
    typedef linear_constraint< Number, VariableName > linear_constraint_t;
    typedef linear_constraint_system< Number, VariableName > linear_constraint_system_t;
    typedef variable< Number, VariableName > variable_t;
    typedef patricia_tree_set< variable_t > variable_set_t;

  private:
    typedef collection< extended_constraint_t > cst_collection_t;

  public:
    typedef typename cst_collection_t::iterator iterator;

  private:
    cst_collection_t _csts;

  public:
    extended_constraint_system() { }

    extended_constraint_system(extended_constraint_t cst) {
      this->_csts += cst;
    }

    extended_constraint_system_t& operator+=(extended_constraint_t cst) {
      this->_csts += cst;
      return *this;
    }

    extended_constraint_system_t& operator+=(extended_constraint_system_t s) {
      this->_csts += s._csts;
      return *this;
    }

    extended_constraint_system_t& operator+=(linear_constraint_t cst) {
      this->_csts += extended_constraint_t(cst);
      return *this;
    }

    extended_constraint_system_t& operator+=(linear_constraint_system_t csts) {
      for(typename linear_constraint_system_t::iterator it = csts.begin(); it != csts.end(); ++it) {
        this->_csts += extended_constraint_t(*it);
      }

      return *this;
    }

    extended_constraint_system_t operator+(extended_constraint_system_t s) {
      extended_constraint_system_t r;
      r.operator+=(s);
      r.operator+=(*this);
      return r;
    }

    iterator begin() {
      return this->_csts.begin();
    }

    iterator end() {
      return this->_csts.end();
    }

    variable_set_t variables() {
      variable_set_t variables;
      for (iterator it = this->begin(); it != this->end(); ++it) {
        variables |= it->variables();
      }
      return variables;
    }

    std::size_t size() {
      return this->_csts.size();
    }

    std::ostream& write(std::ostream& o) {
      return this->_csts.write(o);
    }

  }; // class extended_constraint_system
}

#endif // IKOS_EXTENDED_CONSTRAINTS_HPP


#include <Data/DATA.h>
#include <Driver/SIMULATION.h>
#include <Equation/EQUATION.h>
#include <Equation/NONLINEAR_EQUATION.h>
#include <Equation/TRUST_REGION.h>
#include <Force/FORCE.h>
#include <Parsing/PARSER_REGISTRY.h>
#include <Utilities/EIGEN_HELPERS.h>
#include <Utilities/LOG.h>
#include <Utilities/MATH.h>
#include <Utilities/RANDOM.h>
#include <Eigen/Eigenvalues>
#include <iomanip>
using namespace Mechanics;
///////////////////////////////////////////////////////////////////////
template<class TV> TRUST_REGION<TV>::
TRUST_REGION()
{
    precision=1e-6;
    contract_factor=.25;
    expand_factor=2.5;
    contract_threshold=.4;
    expand_threshold=.8;
    expand_threshold_rad=.8;
    trust_iterations=20;
    tol=1e-8;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Linearize(SIMULATION<TV>& simulation,const T dt,const T time)
{
    DATA<TV>& data=simulation.data;
    FORCE<TV>& force=simulation.force;

    // zero velocities
    equation->Initialize(data,force);
    current_velocities.resize(equation->Velocity_DOF(),1);current_velocities.setZero();
    equation->Unpack_Velocities(data,current_velocities);

    // zero forces
    force.Pack_Forces(solve_forces);
    solve_forces.setZero();
    force.Unpack_Forces(solve_forces);

    // store positions
    data.Pack_Positions(positions);

    equation->Linearize(data,force,dt,time,true);
    equation->RHS(rhs);
    force.Pack_Forces(solve_forces);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Linearize_Around(SIMULATION<TV>& simulation,const T dt,const T time,const Vector& solve_vector)
{
    DATA<TV>& data=simulation.data;
    FORCE<TV>& force=simulation.force;
    // sk is solve_vector
    int velocity_dof=equation->Velocity_DOF();
    Vector solve_velocities=solve_vector.block(0,0,velocity_dof,1);
    solve_forces.Set(solve_vector.block(velocity_dof,0,solve_vector.rows()-velocity_dof,1));
    data.Unpack_Positions(positions);
    force.Increment_Forces(solve_forces,1);

    candidate_velocities=current_velocities+solve_velocities;
    equation->Unpack_Velocities(data,candidate_velocities);
    data.Step();
    equation->Linearize(data,force,dt,time,false);
    force.Increment_Forces(solve_forces,-1);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Increment_X(SIMULATION<TV>& simulation)
{
    // sk is solve_vector
    int velocity_dof=equation->Velocity_DOF();
    Vector solve_velocities=sk.block(0,0,velocity_dof,1);
    solve_forces.Set(sk.block(velocity_dof,0,sk.rows()-velocity_dof,1));
    simulation.force.Increment_Forces(solve_forces,1);
    simulation.force.Pack_Forces(solve_forces);
    current_velocities+=solve_velocities;
    rhs=try_rhs;

    // store errors
    T one_over_maxabs=1;
    if(rhs.rows()>0){
        T maxabs=rhs.array().abs().maxCoeff();
        if(!maxabs){maxabs=1;}
        one_over_maxabs=1/maxabs;}
    equation->Store_Errors(simulation.data,rhs.block(0,0,velocity_dof,1)*one_over_maxabs);
    solve_forces.Set(rhs.block(velocity_dof,0,rhs.rows()-velocity_dof,1)*one_over_maxabs);
    simulation.force.Store_Errors(solve_forces);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Step(SIMULATION<TV>& simulation,const T dt,const T time)
{
    static int call_count=0;
    static int total_steps=0;
    // loosely based on trustOptim implementation
    status=CONTINUE;

    iteration=0;
    max_iterations=100;
    radius=1;
    min_radius=1e-9;
    preconditioner_refresh_frequency=1;

    Linearize(simulation,dt,time);
    f=equation->Evaluate();
    equation->Gradient(gk);
    norm_gk=gk.norm();
    Update_Hessian(false);
    Update_Preconditioner(false);

    static int failed_radius=0;
    do{
        iteration++;
        LOG::cout<<std::endl<<"BEGINNING STEP "<<iteration<<std::endl;
        status=Update_One_Step(simulation,dt,time);
        LOG::cout<<"norm_gk: "<<norm_gk<<" norm_gk/sqrt(nvars): "<<norm_gk/sqrt(T(nvars))<<std::endl;
        //if(norm_gk/sqrt(T(nvars))<=precision && f<=precision){status=SUCCESS;}
        if(norm_gk/sqrt(T(nvars))<=precision){status=SUCCESS;}
        if(iteration>=max_iterations){status=EMAXITER;}
        if(radius<=min_radius){ // trust region collapse
            status=ETOLG;
            failed_radius++;
        }

        // update Hessian
        if(status==MOVED || status==EXPAND){
            //Update_Hessian(status!=EXPAND);
            Update_Hessian(false);
            //Check_Derivative(simulation,dt,time);
            if(simulation.force.Equations_Changed() || iteration%preconditioner_refresh_frequency==0){Update_Preconditioner(false);}
            status=CONTINUE;}
        if(simulation.substeps){
            std::string frame_name="Frame "+std::to_string(simulation.current_frame)+" substep "+std::to_string(iteration)+" real "+std::to_string(int(status==CONTINUE))+ " f "+std::to_string(f);
            simulation.Write(frame_name);}
        if(status==CONTRACT){status=CONTINUE;}
    }while(status==CONTINUE);
    std::string frame_name="End frame "+std::to_string(simulation.current_frame)+" substep "+std::to_string(iteration)+" real "+std::to_string(int(status==CONTINUE))+ " f "+std::to_string(f);
    simulation.Write(frame_name);
    //Check_Derivative(simulation,dt,time);
    LOG::cout<<"SOLVE STEPS: "<<iteration<<" Failed due to radius: "<<failed_radius<<std::endl;
    call_count++;
    total_steps+=iteration;
    LOG::cout<<"Current average: "<<total_steps/(T)call_count<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Update_Preconditioner(bool identity)
{
    /*Matrix<T,Dynamic,Dynamic> dense(hessian);
    EigenSolver<Matrix<T,Dynamic,Dynamic>> es(dense,false);
    LOG::cout<<es.eigenvalues()<<std::endl;*/
    if(!identity){
        preconditioner.compute(hessian);}
    if(identity || preconditioner.info()!=ComputationInfo::Success){
        LOG::cout<<"Preconditioner computation failed; using identity"<<std::endl;
        SparseMatrix<T> BB(nvars,nvars);
        BB.setIdentity();
        preconditioner.compute(BB);}
    /*inverse_scale.resize(nvars);
    for(int i=0;i<nvars;i++){
    inverse_scale(i)=1/preconditioner.scalingS()(i);}*/
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Update_Hessian(bool use_accurate_hessian)
{
    if(use_accurate_hessian){
        equation->Accurate_Hessian(hessian);}
    else{
        equation->Hessian(hessian);}
    equation->Jacobian(jacobian);
    nvars=hessian.rows();
}
///////////////////////////////////////////////////////////////////////
template<class M>
void Print_No_Angular(const M& m)
{
    for(int i=0;i<m.rows();i++){
        if(i%6>2){continue;}
        for(int j=0;j<m.cols();j++){
            if(j%6>2){continue;}
            LOG::cout<<std::setw(13)<<m(i,j)<<" ";
        }
        LOG::cout<<std::endl;
    }
}
template<class TV> typename TRUST_REGION<TV>::STATUS TRUST_REGION<TV>::
Update_One_Step(SIMULATION<TV>& simulation,const T dt,const T time)
{
    auto step_status=UNKNOWN;
    T try_f,step_quality,predicted_reduction;
    /*LOG::cout<<"Jacobian adjoint: "<<std::endl;
    Print_No_Angular(Matrix<T,Dynamic,Dynamic>(jacobian.adjoint()));
    LOG::cout<<"Error: "<<std::endl;
    LOG::cout<<"Un-adjusted hessian sk:"<<std::endl;*/
    //hessian=jacobian.adjoint()*jacobian;
    //Solve_Trust_Conjugate_Gradient(sk);
    //equation->Hessian(hessian);
    Solve_Trust_Conjugate_Gradient(sk);
    //Solve_Trust_MINRES(sk);
    T norm_sk_scaled=Norm(preconditioner,sk,wd);
    if(!std::isfinite(norm_sk_scaled)){step_status=FAILEDCG;}
    else{
        Linearize_Around(simulation,dt,time,sk);
        try_f=equation->Evaluate();
        if(std::isfinite(try_f)){
            T actual_reduction=f-try_f;
            T gs=gk.dot(sk);
            T sBs=sk.dot(hessian.template selfadjointView<Lower>()*sk);
            predicted_reduction=-(gs+sBs/2);
            //Jsk=jacobian*sk;
            //componentwise_prediction=gk.cwiseProduct(Jsk)+Jsk.cwiseProduct(Jsk)/2;
            equation->RHS(try_rhs);
            //LOG::cout<<"Try RHS: "<<std::endl<<try_rhs<<std::endl;
            //LOG::cout<<"Componentwise quality: "<<componentwise_prediction.sum()<<std::endl<<rhs.Diff(try_rhs).cwiseQuotient(componentwise_prediction)<<std::endl;
            //int index;
            //LOG::cout<<"Min value at index "<<index<<" is "<<componentwise_prediction.minCoeff(&index)<<std::endl;
            //LOG::cout<<"Max value at index "<<index<<" is "<<componentwise_prediction.maxCoeff(&index)<<std::endl;
            if(predicted_reduction<0){step_status=ENEGMOVE;}
            step_quality=actual_reduction/predicted_reduction;
            LOG::cout<<"Candidate error with value "<<try_f<<":"<<std::endl;
            /*Matrix<T,Dynamic,1> error;
            equation->RHS(error);
            LOG::cout<<"Current error: "<<std::endl;
            Print_No_Angular(error.transpose());*/
            LOG::cout<<"AP: "<<step_quality<<" old f: "<<f<<" try f: "<<try_f<<" ared: "<<actual_reduction<<" pred: "<<predicted_reduction<<" radius: "<<radius<<" gs: "<<gs<<" sBs: "<<sBs<<" norm_sk_scaled: "<<norm_sk_scaled<<std::endl;}
        else{step_status=FAILEDCG;}}
    if(step_status!=FAILEDCG && step_status!=ENEGMOVE){
        if(step_quality>contract_threshold){
            equation->Gradient(try_g);
            if(std::isfinite(try_g.norm())){
                f=try_f;
                Increment_X(simulation);
                gk=try_g;
                norm_gk=gk.norm();
                /*LOG::cout<<"Resolved error:"<<std::endl;
                Matrix<T,Dynamic,1> error;
                equation->RHS(error);
                Print_No_Angular(error.transpose());*/
                if(step_quality>expand_threshold){// && norm_sk_scaled>=expand_threshold_rad*radius){
                    step_status=EXPAND;}
                else{step_status=MOVED;}}
            else{step_status=FAILEDCG;}}
        else if(step_quality<0){step_status=NEGRATIO;}
        else{step_status=CONTRACT;}}

    LOG::cout<<"Step status: "<<step_status<<std::endl;
    switch(step_status){
        case NEGRATIO:{
            T gksk=gk.dot(sk);
            T gamma_bad=(1-contract_threshold)*gksk/((1-contract_threshold)*(f+gksk+contract_threshold*(f-predicted_reduction)-try_f));
            radius=std::min(contract_factor*norm_sk_scaled,std::max((T)0.0625,gamma_bad)*radius);
            step_status=CONTRACT;
            break;}
        case CONTRACT:
        case FAILEDCG:
        case ENEGMOVE:
            step_status=CONTRACT;
            //radius=norm_sk_scaled*contract_factor;
            radius*=contract_factor;
            break;
        case EXPAND:
            //radius=std::max(expand_factor*norm_sk_scaled,radius);
            radius*=expand_factor;
            break;
        default:
            break;
    };
    return step_status;
}
///////////////////////////////////////////////////////////////////////
template<class TV> typename TV::Scalar TRUST_REGION<TV>::
Norm(const Preconditioner& preconditioner,const Vector& v,Vector& scratch)
{
    if(preconditioner.permutationP().rows() == v.rows()){
        scratch=preconditioner.permutationP()*v;}
    else{scratch=v;}
    scratch=preconditioner.matrixL().adjoint().template triangularView<Upper>()*scratch;
    return scratch.norm();
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Solve_Trust_MINRES(Vector& sol)
{
    std::array<T,2> alpha,beta,res;
    std::array<Vector,2> v,d;
    int n=hessian.rows();
    for(int i=0;i<2;i++){
        v[i].resize(n);
        v[i].setZero();
        d[i].resize(n);
        d[i].setZero();
    }
    T norm_A=0;
    T cond_A=1;
    T c=-1,s=0; // givens

    T gamma_min=1e99;
    std::array<T,2> delta1,delta2,ep,gamma1,gamma2;
    delta1[1]=0;
    Vector pk,tk,old_sol;
    sol.resize(n);sol.setZero();
    T tau=0;
    T epsilon=1e-8;
    beta[0]=0;
    T norm_rhs=gk.norm();
    beta[1]=norm_rhs;
    v[1]=-gk/norm_rhs;
    res[0]=norm_rhs;
    tau=norm_rhs;

    auto sign=[&](T x) {return (std::fabs(x)<epsilon?0:x/std::fabs(x));};

    int i;
    std::stringstream reason;
    for(i=0;i<trust_iterations;i++){
        int cur=(i+1)%2,next=i%2;

        pk=hessian*v[cur];
        alpha[cur]=v[cur].dot(pk);
        pk-=alpha[cur]*v[cur];
        v[next]=pk-beta[cur]*v[next];
        beta[next]=v[next].norm();
        if(fabs(beta[next])>epsilon){
            v[next]/=beta[next];}

        delta2[cur]=c*delta1[cur]+s*alpha[cur];
        gamma1[cur]=s*delta1[cur]-c*alpha[cur];

        ep[next]=s*beta[next];
        delta1[next]=-c*beta[next];

        T a=gamma1[cur],b=beta[next];
        if(fabs(b)<epsilon){
            s=0;
            gamma2[cur]=fabs(a);
            if(fabs(a)<epsilon){
                c=1;}
            else{
                c=sign(a);}
        }
        else if(fabs(a)<epsilon){
            c=0;
            s=sign(b);
            gamma2[cur]=fabs(b);
        }
        else if(fabs(b)>fabs(a)){
            T t=a/b;
            s=sign(b)/sqrt(1+sqr(t));
            c=s*t;
            gamma2[cur]=b/s;}
        else{
            T t=b/a;
            c=sign(a)/sqrt(1+sqr(t));
            s=c*t;
            gamma2[cur]=a/c;}

        tau=c*res[next];
        res[cur]=s*res[next];

        if(i==0){norm_A=sqrt(sqr(alpha[cur])+sqr(beta[next]));}
        else{
            T tnorm=sqrt(sqr(alpha[cur])+sqr(beta[next])+sqr(beta[cur]));
            norm_A=std::max(norm_A,tnorm);}

        if(fabs(gamma2[cur])>epsilon){
            d[cur]=(v[cur]-delta2[cur]*d[next]-ep[cur]*d[cur])/gamma2[cur];

            old_sol=sol;
            sol+=tau*d[cur];
            if(sol.norm()>=radius){
                LOG::cout<<"tau was originally "<<tau<<std::endl;
                LOG::cout<<"Direction "<<d[cur].transpose()<<std::endl;
                d[cur]*=tau;
                tau=Find_Tau(old_sol,d[cur]);
                LOG::cout<<"Chosen tau is "<<tau<<std::endl;
                sol=old_sol+tau*d[cur];
                reason<<"Intersect TR bound";
                break;
            }
            gamma_min=std::min(gamma_min,gamma2[cur]);
            cond_A=norm_A/gamma_min;
        }

        LOG::cout<<"residual: "<<res[i%2]<<std::endl;
        if(res[i%2]/norm_rhs<tol){
            reason<<"Reached tolerance";
            break;
        }
    }
    CG_stop_reason=reason.str();
    LOG::cout<<"MINRES reason: "<<CG_stop_reason<<" iterations: "<<i<<std::endl;
    LOG::cout<<"solutn: "<<sol.transpose()<<std::endl;
    LOG::cout<<"result: "<<(hessian*sol).transpose()<<std::endl;
    LOG::cout<<"actual: "<<-gk.transpose()<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Solve_Trust_Conjugate_Gradient(Vector& pk)
{
    T dot_ry,dot_ry_old,aj,tau,dBd,p_norm_gk;
    int j;
    zj.resize(hessian.rows());
    zj.setZero();
    rj=-gk;
    p_norm_gk=Norm(preconditioner,gk,wd);
    T local_tol=std::min((T).5,(T)sqrt(p_norm_gk))*p_norm_gk;
    LOG::cout<<"Local tol: "<<local_tol<<" tol: "<<tol<<std::endl;

    // Solve LL'y=r
    yj=preconditioner.solve(rj);
    dj=yj;

    std::stringstream reason;
    for(j=0;j<trust_iterations;j++){
        dBd=dj.dot(hessian.template selfadjointView<Lower>()*dj);
        if(dBd<=0){
            tau=Find_Tau(zj,dj);
            pk.noalias()=zj+tau*dj;
            num_CG_iterations=j+1;
            reason<<"Negative curvature: "<<dBd;
            break;}

        aj=rj.dot(yj)/dBd;
        zj_old=zj;
        zj.noalias()+=aj*dj;

        if(Norm(preconditioner,zj,wd)>=radius){
            // find tau>=0 s.t. p intersects trust region
            tau=Find_Tau(zj_old,dj);
            pk.noalias()=zj_old+tau*dj;
            num_CG_iterations=j+1;
            reason<<"Intersect TR bound";
            break;}

        dot_ry=rj.dot(yj);
        rj.noalias()-=aj*(hessian.template selfadjointView<Lower>()*dj).eval();

        if(Norm(preconditioner,rj,wd)/p_norm_gk<local_tol){
            pk=zj;
            num_CG_iterations=j+1;
            reason<<"Reached tolerance";
            break;}

        dot_ry_old=dot_ry;

        //updating yj
        yj=preconditioner.solve(rj);
        dot_ry=rj.dot(yj);
        dj*=dot_ry/dot_ry_old;
        dj.noalias()+=yj;}

    if(j>=trust_iterations){
        pk=zj;
        num_CG_iterations=j;
        reason<<"Exceeded max CG iterations";}

    CG_stop_reason=reason.str();
    LOG::cout<<"CG reason: "<<CG_stop_reason<<" iterations: "<<num_CG_iterations<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Multiply(const Preconditioner& X,const Vector& v,Vector& out)
{
    if(X.permutationP().rows() == v.rows()){
        out=X.permutationP()*v;}
    else{out=v;}
    out=X.matrixL().adjoint().template triangularView<Upper>()*out;
    out=X.matrixL().template triangularView<Lower>()*out;
    if(X.permutationP().rows() == v.rows()){
        out=X.permutationP().inverse()*out;}
}
///////////////////////////////////////////////////////////////////////
template<class TV> typename TV::Scalar TRUST_REGION<TV>::
Find_Tau(const Vector& z,const Vector& d)
{
    Multiply(preconditioner,d,wd);
    Multiply(preconditioner,z,wz);

    T pCd=z.dot(wd);
    T dCd=d.dot(wd);
    T pCp=z.dot(wz);
    return (-2*pCd+sqrt(4*pCd*pCd-4*dCd*(pCp-radius*radius)))/(2*dCd);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Check_Derivative(SIMULATION<TV>& simulation,const T dt,const T time)
{
    DATA<TV>& data=simulation.data;
    FORCE<TV>& force=simulation.force;

    T epsilon=1e-2;
    Matrix<T,Dynamic,1> variables=equation->Get_Unknowns(data,force);
    variables.setZero();
    if(!sk.rows()){sk.resize(variables.rows());sk.setZero();}
    Linearize_Around(simulation,dt,time,variables);

    data.random.Direction(variables);
    T f0=equation->Evaluate();
    Matrix<T,Dynamic,1> gradient;equation->Gradient(gradient);
    SparseMatrix<T> h;equation->Hessian(h);
    auto Evaluate_Step_Error = [&](T eps){
        Linearize_Around(simulation,dt,time,eps*variables);

        T f1=equation->Evaluate();
        T predicted_delta_f=gradient.dot(eps*variables)+(T).5*eps*eps*variables.transpose()*h*variables;
        T error=f1-f0-predicted_delta_f;
        LOG::cout<<"Error for "<<eps<<": "<<error<<std::endl;
        return error;
    };
    T last_error=Evaluate_Step_Error(epsilon);
    int divisors=8;
    for(int i=0;i<divisors;i++){
        epsilon/=2;
        T new_error=Evaluate_Step_Error(epsilon);
        LOG::cout<<"Ratio: "<<last_error/new_error<<std::endl;
        last_error=new_error;
    }
    //LOG::cout<<"Ratio: "<<Evaluate_Step_Error(epsilon)/Evaluate_Step_Error(epsilon/2)<<std::endl;
    Linearize_Around(simulation,dt,time,sk);
}
///////////////////////////////////////////////////////////////////////
GENERIC_TYPE_DEFINITION(TRUST_REGION)
DEFINE_AND_REGISTER_PARSER(TRUST_REGION,void)
{
    auto step=std::make_shared<TRUST_REGION<TV>>();
    step->equation=new NONLINEAR_EQUATION<TV>();
    Parse_Scalar(node["precision"],step->precision,step->precision);
    Parse_Scalar(node["contract_threshold"],step->contract_threshold,step->contract_threshold);
    Parse_Scalar(node["expand_threshold"],step->expand_threshold,step->expand_threshold);
    Parse_Scalar(node["contract_factor"],step->contract_factor,step->contract_factor);
    Parse_Scalar(node["expand_factor"],step->expand_factor,step->expand_factor);
    Parse_Scalar(node["trust_iterations"],step->trust_iterations,step->trust_iterations);
    Parse_Scalar(node["tol"],step->tol,step->tol);
    simulation.evolution.push_back(step);
    return 0;
}