#include <Rcpp.h>

//[[Rcpp::depends(RcppEigen)]]
#include <RcppEigen.h>

#include "experimentalSetup.hpp"
#include "individual.hpp"
#include "encodingScheme.hpp"
#include "logLikelihoods.hpp"
#include "estimatePoissonGammaParameters.hpp"

#include "AuxiliaryFunctions.hpp"
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/factorials.hpp>

Individual::Individual()
{
    Fitness = -HUGE_VAL;
}

Individual::Individual(const ExperimentalSetup & ES)
{
    if ((ES.NumberOfContributors - ES.NumberOfKnownContributors) == 0)
    {
        Eigen::MatrixXd decodedProfile = decoding(EncodedProfile, ES.NumberOfAlleles, ES.NumberOfMarkers, ES.NumberOfContributors - ES.NumberOfKnownContributors);

        CreateReducedElements(ES, decodedProfile);
        EstimateParameters(ES);
        CalculateFitness(ES, decodedProfile);
    }
    else
    {
        Rcpp::stop("The only use of this contructor is when the number of contributors equals the number of known contributors under the proposed hypothesis.");
    }
}

Individual::Individual(const Eigen::VectorXd & encodedGenotype, const ExperimentalSetup & ES)
{
    EncodedProfile = encodedGenotype;

    Eigen::MatrixXd decodedProfile = decoding(EncodedProfile, ES.NumberOfAlleles, ES.NumberOfMarkers, ES.NumberOfContributors - ES.NumberOfKnownContributors);
    CreateReducedElements(ES, decodedProfile);

    EstimateParameters(ES);
    CalculateFitness(ES, decodedProfile);
}

Individual::Individual(const Eigen::VectorXd & encodedGenotype, const Eigen::VectorXd & sampleParameters, const Eigen::VectorXd & noiseParameters,
                       const Eigen::VectorXd & mixtureParameters, const Eigen::VectorXd & markerParameters, const ExperimentalSetup & ES)
{
    EncodedProfile = encodedGenotype;

    SampleParameters = sampleParameters;
    MixtureParameters = mixtureParameters;
    MarkerImbalanceParameters = markerParameters;
    NoiseParameters = noiseParameters;

    Eigen::MatrixXd decodedProfile = decoding(EncodedProfile, ES.NumberOfAlleles, ES.NumberOfMarkers, ES.NumberOfContributors - ES.NumberOfKnownContributors);

    CreateReducedElements(ES, decodedProfile);
    CalculateFitness(ES, decodedProfile);
}

Individual::Individual(const Eigen::VectorXd & encodedGenotype, const Eigen::VectorXd & sampleParameters, const Eigen::VectorXd & noiseParameters,
                       const Eigen::VectorXd & mixtureParameters, const Eigen::VectorXd & markerParameters, const double & fitness, const ExperimentalSetup & ES)
{
    EncodedProfile = encodedGenotype;

    SampleParameters = sampleParameters;
    MixtureParameters = mixtureParameters;
    MarkerImbalanceParameters = markerParameters;
    NoiseParameters = noiseParameters;

    Eigen::MatrixXd decodedProfile = decoding(EncodedProfile, ES.NumberOfAlleles, ES.NumberOfMarkers, ES.NumberOfContributors - ES.NumberOfKnownContributors);
    CreateReducedElements(ES, decodedProfile);

    CalculateFitness(ES, decodedProfile);
}

Individual::Individual(const Eigen::VectorXd & encodedGenotype, const std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix,
                       const std::vector<Eigen::VectorXd> reducedAlleleIndex, const std::vector<Eigen::VectorXd> reducedNoiseIndex,
                       const ExperimentalSetup & ES)
{
    EncodedProfile = encodedGenotype;

    ReducedExpectedContributionMatrix = reducedExpectedContributionMatrix;
    ReducedAlleleIndex = reducedAlleleIndex;
    ReducedNoiseIndex = reducedNoiseIndex;

    Eigen::MatrixXd decodedProfile = decoding(EncodedProfile, ES.NumberOfAlleles, ES.NumberOfMarkers, ES.NumberOfContributors - ES.NumberOfKnownContributors);
    EstimateParameters(ES);
    CalculateFitness(ES, decodedProfile);
}

double ParentStutterContribution(const std::size_t & currentAllele, const std::size_t & stutterRecursion, const double & levelOfRecursion,
                                 const Eigen::VectorXd & decodedProfile_mu, const std::vector<Eigen::MatrixXd> & potentialParents_m, const double & numberOfAlleles_m)
{
    if (stutterRecursion == 0)
    {
        return 0.0;
    }

    Eigen::MatrixXd potentialParents_ma = potentialParents_m[currentAllele];
    Eigen::VectorXd potentialParentIndex = potentialParents_ma.col(1);
    Eigen::VectorXd stutterContribution = potentialParents_ma.col(2);

    Eigen::VectorXd potentialParentContribution = Eigen::VectorXd::Zero(potentialParentIndex.size());
    if (potentialParentIndex[0] != -1)
    {
        for (std::size_t i = 0; i < potentialParentIndex.size(); i++)
        {
            double parentContribution = ParentStutterContribution(potentialParentIndex[i] - 1, stutterRecursion - 1, levelOfRecursion + 1, decodedProfile_mu, potentialParents_m, numberOfAlleles_m);
            potentialParentContribution[i] = stutterContribution[i] * (decodedProfile_mu[potentialParentIndex[i] - 1] + parentContribution) / levelOfRecursion; // ;
        }
    }

    double totalParentContribition = potentialParentContribution.sum();
    return totalParentContribition;
}

Eigen::MatrixXd Individual::GenerateExpectedContributionProfile(const ExperimentalSetup & ES, const Eigen::MatrixXd & decodedProfile)
{
    std::size_t numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;
    Eigen::VectorXd partialSumAlleles = partialSumEigen(ES.NumberOfAlleles);

    Eigen::MatrixXd profile(ES.Coverage.size(), ES.NumberOfContributors);
    if (ES.NumberOfKnownContributors > 0)
    {
        profile = bindColumns(ES.KnownProfiles, decodedProfile);
    }
    else
    {
        profile = decodedProfile;
    }

    Eigen::MatrixXd expectedContributionProfile = Eigen::MatrixXd::Zero(decodedProfile.rows(), ES.NumberOfContributors);
    for (std::size_t m = 0; m < ES.NumberOfMarkers; m++)
    {
        std::vector<Eigen::MatrixXd> potentialParents_m = ES.PotentialParents[m];
        Eigen::MatrixXd expectedContributionProfile_m = Eigen::MatrixXd::Zero(ES.NumberOfAlleles[m], ES.NumberOfContributors);
        Eigen::MatrixXd decodedProfile_m = profile.block(partialSumAlleles[m], 0, ES.NumberOfAlleles[m], ES.NumberOfContributors);
        for (std::size_t u = 0; u < ES.NumberOfContributors; u++)
        {
            Eigen::VectorXd decodedProfile_mu = decodedProfile_m.col(u);
            Eigen::VectorXd stutterContribution = Eigen::VectorXd::Zero(ES.NumberOfAlleles[m]);
            for (std::size_t a = 0; a < ES.NumberOfAlleles[m]; a++)
            {
                stutterContribution[a] = ParentStutterContribution(a, ES.LevelsOfStutterRecursion, 1, decodedProfile_mu, potentialParents_m, ES.NumberOfAlleles[m]);
            }

            expectedContributionProfile_m.col(u) = decodedProfile_mu + stutterContribution;
        }

        expectedContributionProfile.block(partialSumAlleles[m], 0, ES.NumberOfAlleles[m], ES.NumberOfContributors) = expectedContributionProfile_m;
    }

    return expectedContributionProfile;
}

Eigen::VectorXd Individual::GenerateNoiseProfile(const ExperimentalSetup & ES, const Eigen::MatrixXd & expectedContributionProfile)
{
    Eigen::VectorXd Ones = Eigen::VectorXd::Ones(expectedContributionProfile.cols());
    Eigen::VectorXd ExpectedContributionProfileRowSum = expectedContributionProfile * Ones;

    std::size_t N = ExpectedContributionProfileRowSum.size();
    Eigen::VectorXd identifiedNoise = Eigen::VectorXd::Zero(N);
    for (std::size_t n = 0; n < N; n++)
    {
        if (!(ExpectedContributionProfileRowSum[n] > 0.0) & (ES.Coverage[n] > 0.0))
        {
            identifiedNoise[n] = 1.0;
        }
    }

    return identifiedNoise;
}

void Individual::CreateReducedElements(const ExperimentalSetup & ES, const Eigen::MatrixXd & decodedProfile)
{
    Eigen::MatrixXd expectedContributionProfile = GenerateExpectedContributionProfile(ES, decodedProfile);
    Eigen::VectorXd noiseProfile = GenerateNoiseProfile(ES, expectedContributionProfile);

    std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix(ES.NumberOfMarkers);
    std::vector<Eigen::VectorXd> reducedAlleleIndex(ES.NumberOfMarkers);
    std::vector<Eigen::VectorXd> reducedNoiseIndex(ES.NumberOfMarkers);

    std::size_t n = 0;
    for (std::size_t m = 0; m < ES.NumberOfMarkers; m++)
    {
        Eigen::VectorXd noiseProfile_m = noiseProfile.segment(ES.PartialSumAlleles[m], ES.NumberOfAlleles[m]);
        std::size_t noiseProfileSize_m = noiseProfile_m.size();
        std::size_t noiseProfileSum_m = noiseProfile_m.sum();

        Eigen::MatrixXd reducedExpectedContributionMatrix_m = Eigen::MatrixXd::Zero(noiseProfileSize_m - noiseProfileSum_m, ES.NumberOfContributors);
        Eigen::VectorXd reducedAlleleIndex_m = Eigen::VectorXd::Zero(noiseProfileSize_m - noiseProfileSum_m);
        Eigen::VectorXd reducedNoiseIndex_m = Eigen::VectorXd::Zero(noiseProfileSum_m);

        std::size_t i = 0, j = 0;
        for (std::size_t a = 0; a < ES.NumberOfAlleles[m]; a++)
        {
            if (noiseProfile[n] == 0)
            {
                reducedExpectedContributionMatrix_m.row(i) = expectedContributionProfile.row(n);
                reducedAlleleIndex_m[i] = a;
                i++;
            }
            else
            {
                reducedNoiseIndex_m[j] = a;
                j++;
            }

            n++;
        }

        reducedExpectedContributionMatrix[m] = reducedExpectedContributionMatrix_m;
        reducedAlleleIndex[m] = reducedAlleleIndex_m;
        reducedNoiseIndex[m] = reducedNoiseIndex_m;
    }

    ReducedExpectedContributionMatrix = reducedExpectedContributionMatrix;
    ReducedAlleleIndex = reducedAlleleIndex;
    ReducedNoiseIndex = reducedNoiseIndex;
}

void Individual::EstimateParameters(const ExperimentalSetup & ES)
{
    //// Estimating parameters
    // Allele parameters
    EstimatePoissonGammaAlleleParameters EPGA(ES.Coverage, ReducedExpectedContributionMatrix, ReducedAlleleIndex,
                                              ES.MarkerImbalances, ES.PartialSumAlleles,
                                              ES.ConvexMarkerImbalanceInterpolation, ES.Tolerance);

    estimateParametersAlleleCoverage(EPGA);
    SampleParameters = EPGA.SampleParameters;
    MixtureParameters = EPGA.MixtureParameters;
    MarkerImbalanceParameters = EPGA.MarkerImbalancesParameters;

    // Noise parameters
    if (ES.DualEstimation) {
        const double varianceUpperLimit = SampleParameters[1];
        EstimatePoissonGammaNoiseParameters EPGN(ES.Coverage, ReducedNoiseIndex, ES.PartialSumAlleles, ES.Tolerance, varianceUpperLimit);
        estimateParametersNoiseCoverage(EPGN);

        NoiseParameters = EPGN.NoiseParameters;
    }
    else {
        NoiseParameters = ES.NoiseParameters;
    }
}

void Individual::CalculateFitness(const ExperimentalSetup & ES, const Eigen::MatrixXd & decodedProfile)
{
    LogLikelihoodAlleleMarker = logLikelihoodAlleleCoverage(ES.Coverage, ReducedExpectedContributionMatrix, ReducedAlleleIndex,
                                                            ES.PartialSumAlleles, SampleParameters, MixtureParameters,
                                                            MarkerImbalanceParameters);
    LogLikelihoodAllele = LogLikelihoodAlleleMarker.sum();

    LogLikelihoodNoiseMarker = logLikelihoodNoiseCoverage(ES.Coverage, ReducedNoiseIndex, ES.PartialSumAlleles, NoiseParameters[0], NoiseParameters[1], NoiseParameters[2]);
    LogLikelihoodNoise = LogLikelihoodNoiseMarker.sum();

    if ((ES.Theta < 0.0) | (ES.AlleleFrequencies.sum() == 0))
    {
        LogPriorGenotypeProbabilityMarker = Eigen::VectorXd::Zero(ES.NumberOfMarkers);
        LogPriorGenotypeProbability = 0.0;
    }
    else
    {
        LogPriorGenotypeProbabilityMarker = logPriorGenotypeProbability(ES.AlleleFrequencies, ES.Theta, decodedProfile, ES.AllKnownProfiles, ES.NumberOfMarkers, ES.NumberOfAlleles);
        LogPriorGenotypeProbability = LogPriorGenotypeProbabilityMarker.sum();
    }

    Fitness = LogLikelihoodAllele + LogLikelihoodNoise + LogPriorGenotypeProbability;
}

Eigen::VectorXd Individual::CalculateResiduals(const ExperimentalSetup & ES, const Eigen::MatrixXd & expectedContributionProfile,
                                               const Eigen::VectorXd & noiseProfile)
{
    const double & referenceMarkerAverage = SampleParameters[0];
    double dispersion;
    const Eigen::VectorXd & Coverage = ES.Coverage;
    const Eigen::VectorXd & MarkerImbalance = ES.MarkerImbalances;
    const Eigen::VectorXd EC = expectedContributionProfile * MixtureParameters;

    std::size_t N = Coverage.size();
    Eigen::VectorXd devianceResiduals = Eigen::VectorXd::Zero(N);
    for (std::size_t n = 0; n < N; n++)
    {
        double mu_ma;
        if (noiseProfile[n] == 0)
        {
            mu_ma = referenceMarkerAverage * MarkerImbalance[n] * EC[n];
            dispersion = SampleParameters[1];
        }
        else
        {
            mu_ma = NoiseParameters[0];
            dispersion = NoiseParameters[1];
        }

        double deviance_ma = (Coverage[n] + dispersion) * (std::log(mu_ma + dispersion) - std::log(Coverage[n] + dispersion));
        if (Coverage[n] > 0)
        {
            deviance_ma += Coverage[n] * (std::log(Coverage[n]) - std::log(mu_ma));
        }

        devianceResiduals[n] = boost::math::sign(Coverage[n] - mu_ma) * std::pow(deviance_ma, 0.5);
    }

    return devianceResiduals;
}

Rcpp::List Individual::ReturnRcppList(const ExperimentalSetup & ES)
{
    Eigen::MatrixXd decodedProfile = decoding(EncodedProfile, ES.NumberOfAlleles, ES.NumberOfMarkers, ES.NumberOfContributors - ES.NumberOfKnownContributors);
    Eigen::MatrixXd expectedContributionProfile = GenerateExpectedContributionProfile(ES, decodedProfile);

    Eigen::VectorXd noiseProfile = GenerateNoiseProfile(ES, expectedContributionProfile);

    CalculateFitness(ES, decodedProfile);
    return Rcpp::List::create(Rcpp::Named("EncodedUnknownProfiles") = EncodedProfile,
                              Rcpp::Named("DecodedUnknownProfiles") = decodedProfile,
                              Rcpp::Named("ExpectedContributionMatrix") = expectedContributionProfile,
                              Rcpp::Named("NoiseVector") = noiseProfile,
                              Rcpp::Named("Parameters") = Rcpp::List::create(
                                  Rcpp::Named("SampleParameters") = SampleParameters,
                                  Rcpp::Named("MixtureParameters") = MixtureParameters,
                                  Rcpp::Named("MarkerImbalanceParameters") = MarkerImbalanceParameters,
                                  Rcpp::Named("NoiseParameters") = NoiseParameters),
                                  Rcpp::Named("LogLikelihoodAlleleCoverage") = LogLikelihoodAlleleMarker,
                                  Rcpp::Named("LogLikelihoods") = Rcpp::NumericVector::create(LogLikelihoodAllele, LogLikelihoodNoise, LogPriorGenotypeProbability),
                                  Rcpp::Named("Fitness") = Fitness);
}

Rcpp::List Individual::ReturnRcppListSimplified()
{
    return Rcpp::List::create(Rcpp::Named("SampleParameters") = SampleParameters,
                              Rcpp::Named("MarkerImbalanceParameters") = MarkerImbalanceParameters,
                              Rcpp::Named("MixtureParameters") = MixtureParameters,
                              Rcpp::Named("NoiseParameters") = NoiseParameters,
                              Rcpp::Named("Fitness") = Fitness);
}

//[[Rcpp::export(.setupIndividual)]]
Rcpp::List setupIndividual(const std::size_t & numberOfMarkers,
                           const Eigen::VectorXd & numberOfAlleles,
                           const std::size_t & numberOfContributors,
                           const std::size_t & numberOfKnownContributors,
                           const Eigen::MatrixXd & knownProfiles,
                           const Eigen::VectorXd & coverage,
                           const std::vector< std::vector < Eigen::MatrixXd > > & potentialParents,
                           const Eigen::VectorXd & markerImbalances,
                           const double & convexMarkerImbalanceInterpolation,
                           const Eigen::VectorXd & noiseParameters,
                           const Eigen::VectorXd & tolerance,
                           const double & theta,
                           const Eigen::VectorXd & alleleFrequencies,
                           const std::size_t & levelsOfStutterRecursion,
                           const bool & dualEstimation)
{
    const Eigen::MatrixXd allProfilesEmpty;
    const ExperimentalSetup ES(numberOfMarkers, numberOfAlleles, numberOfContributors, numberOfKnownContributors,
                               knownProfiles, allProfilesEmpty, coverage, potentialParents, markerImbalances,
                               convexMarkerImbalanceInterpolation, noiseParameters, tolerance, theta, alleleFrequencies,
                               levelsOfStutterRecursion, dualEstimation);
    Individual I(ES);

    Eigen::MatrixXd decodedProfile = decoding(I.EncodedProfile, ES.NumberOfAlleles, ES.NumberOfMarkers, ES.NumberOfContributors - ES.NumberOfKnownContributors);
    Eigen::MatrixXd expectedContributionProfile = I.GenerateExpectedContributionProfile(ES, decodedProfile);
    Eigen::VectorXd noiseProfile = I.GenerateNoiseProfile(ES, expectedContributionProfile);

    return Rcpp::List::create(Rcpp::Named("EncodedUnknownProfiles") = I.EncodedProfile,
                              Rcpp::Named("DecodedUnknownProfiles") = decodedProfile,
                              Rcpp::Named("ExpectedContributionMatrix") = expectedContributionProfile,
                              Rcpp::Named("NoiseVector") = noiseProfile,
                              Rcpp::Named("Parameters") = Rcpp::List::create(
                                  Rcpp::Named("SampleParameters") = I.SampleParameters,
                                  Rcpp::Named("MixtureParameters") = I.MixtureParameters,
                                  Rcpp::Named("MarkerImbalanceParameters") = I.MarkerImbalanceParameters,
                                  Rcpp::Named("NoiseParameters") = I.NoiseParameters),
                              Rcpp::Named("LogLikelihoodAlleleCoverage") = I.LogLikelihoodAlleleMarker,
                              Rcpp::Named("LogLikelihoods") = Rcpp::NumericVector::create(I.LogLikelihoodAllele, I.LogLikelihoodNoise, I.LogPriorGenotypeProbability),
                              Rcpp::Named("Fitness") = I.Fitness);
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#ifndef KINDR_ROTATIONS_EIGEN_ROTATIONMATRIX_HPP_
#define KINDR_ROTATIONS_EIGEN_ROTATIONMATRIX_HPP_

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationBase.hpp"
#include "kindr/rotations/eigen/RotationEigenFunctions.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*! \class RotationMatrix
 *  \brief Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
 *
 *  The following four typedefs are provided for convenience:
 *   - \ref eigen_impl::RotationMatrixAD "RotationMatrixAD" for active rotation and double primitive type
 *   - \ref eigen_impl::RotationMatrixAF "RotationMatrixAF" for active rotation and float primitive type
 *   - \ref eigen_impl::RotationMatrixPD "RotationMatrixPD" for passive rotation and double primitive type
 *   - \ref eigen_impl::RotationMatrixPF "RotationMatrixPF" for passive rotation and float primitive type
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *  \tparam Usage_ the rotation usage which is either active or passive
 *
 *  \ingroup rotations
 */
template<typename PrimType_, enum RotationUsage Usage_>
class RotationMatrix : public RotationMatrixBase<RotationMatrix<PrimType_, Usage_>, Usage_>, private Eigen::Matrix<PrimType_, 3, 3> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::Matrix<PrimType_, 3, 3> Base;
 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  /*! \brief Default constructor using identity rotation.
   */
  RotationMatrix()
    : Base(Base::Identity()) {
  }

  /*! \brief Constructor using nine scalars.
   *  In debug mode, an assertion is thrown if the matrix is not a rotation matrix.
   *  \param r11     entry in row 1, col 1
   *  \param r12     entry in row 1, col 2
   *  \param r13     entry in row 1, col 3
   *  \param r21     entry in row 2, col 1
   *  \param r22     entry in row 2, col 2
   *  \param r23     entry in row 2, col 3
   *  \param r31     entry in row 3, col 1
   *  \param r32     entry in row 3, col 2
   *  \param r33     entry in row 3, col 3
   */
  RotationMatrix(Scalar r11, Scalar r12, Scalar r13,
                 Scalar r21, Scalar r22, Scalar r23,
                 Scalar r31, Scalar r32, Scalar r33) {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
    } else {
      *this << r11,r21,r31,r12,r22,r32,r13,r23,r33;
    }
    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, this->toImplementation() * this->toImplementation().transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param other   Eigen::Matrix<PrimType_,3,3>
   */
  explicit RotationMatrix(const Base& other)
  // : Base(other)
  {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      this->toImplementation() = other;
    } else {
      this->toImplementation() = other.transpose();
    }
    KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, other * other.transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, other.determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit RotationMatrix(const RotationBase<OtherDerived_, Usage_>& other)
  // : Base(internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation())
  {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns referece
   */
  template<typename OtherDerived_>
  RotationMatrix& operator =(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  RotationMatrix& operator ()(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  RotationMatrix inverted() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  RotationMatrix& invert() {
    *this = this->inverted();
    return *this;
  }

  /*! \brief Returns the transpose of the rotation matrix.
   *  \returns the inverse of the rotation
   */
  RotationMatrix transposed() const {
    RotationMatrix matrix;
    matrix.toImplementation() = this->toImplementation().transpose();
    return matrix;
  }

  /*! \brief Transposes the rotation matrix.
   *  \returns reference
   */
  RotationMatrix& transpose() {
    *this = this->transposed();
    return *this;
  }

  /*! \brief Returns the determinant of the rotation matrix.
   *  \returns determinant of the rotation matrix
   */
  Scalar determinant() const {
  return toImplementation().determinant();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(*this);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(*this);
  }

  /*! \brief Reading access to the rotation matrix.
   *  \returns rotation matrix (matrix) with reading access
   */
  inline Implementation matrix() const {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      return this->toImplementation();
    } else {
      return this->toImplementation().transpose();
    }
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(const Implementation & input) {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      this->toImplementation() = input;
    } else {
      this->toImplementation() = input.transpose();
    }
  }

  /*! \brief  Writing access to the rotation matrix.
   */
  inline void setMatrix(Scalar r11, Scalar r12, Scalar r13,
                        Scalar r21, Scalar r22, Scalar r23,
                        Scalar r31, Scalar r32, Scalar r33) {
    if(Usage_ == RotationUsage::ACTIVE)
    {
      *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
    } else {
      *this << r11,r21,r31,r12,r22,r32,r13,r23,r33;
    }
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  RotationMatrix& setIdentity() {
    this->Implementation::setIdentity();
    return *this;
  }

  /*! \brief Returns a unique matrix rotation.
   *  A rotation matrix is always unique.
   *  This function is used to compare different rotations.
   *  \returns copy of the matrix rotation which is unique
   */
  RotationMatrix getUnique() const {
    return *this;
  }

  /*! \brief Modifies the matrix rotation such that it becomes unique.
   *  A rotation matrix is always unique.
   *  \returns reference
   */
  RotationMatrix& setUnique() {
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using RotationMatrixBase<RotationMatrix<PrimType_, Usage_>, Usage_>::operator*; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator==.
   *  \returns true if two rotations are similar.
   */
  using RotationMatrixBase<RotationMatrix<PrimType_, Usage_>, Usage_>::operator==; // otherwise ambiguous RotationBase and Eigen

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationMatrix& rotationMatrix) {
    if(Usage_ == RotationUsage::ACTIVE) {
      out << rotationMatrix.toImplementation();
    } else {
      out << rotationMatrix.inverted().toImplementation();
    }
    return out;
  }
};

//! \brief Active matrix rotation with double primitive type
typedef RotationMatrix<double, RotationUsage::ACTIVE>  RotationMatrixAD;
//! \brief Active matrix rotation with float primitive type
typedef RotationMatrix<float,  RotationUsage::ACTIVE>  RotationMatrixAF;
//! \brief Passive matrix rotation with double primitive type
typedef RotationMatrix<double, RotationUsage::PASSIVE> RotationMatrixPD;
//! \brief Passive matrix rotation with float primitive type
typedef RotationMatrix<float,  RotationUsage::PASSIVE> RotationMatrixPF;

} // namespace eigen_impl

namespace internal {

template<typename PrimType_, enum RotationUsage Usage_>
class get_scalar<eigen_impl::RotationMatrix<PrimType_, Usage_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_, enum RotationUsage Usage_>
class get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>{
 public:
  typedef int  IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>> {
 public:
  typedef eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE> OtherUsage;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE>> {
 public:
  typedef eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE> OtherUsage;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationMatrix<DestPrimType_, Usage_>, eigen_impl::AngleAxis<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<DestPrimType_, Usage_> convert(const eigen_impl::AngleAxis<SourcePrimType_, Usage_>& aa) {
    eigen_impl::RotationMatrix<DestPrimType_, Usage_> matrix;
    matrix.toImplementation() = eigen_impl::eigen_internal::getRotationMatrixFromAngleAxis<SourcePrimType_, DestPrimType_>(aa.toImplementation());
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationMatrix<DestPrimType_, Usage_>, eigen_impl::RotationVector<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<DestPrimType_, Usage_> convert(const eigen_impl::RotationVector<SourcePrimType_, Usage_>& rotationVector) {
    typename eigen_impl::RotationMatrix<DestPrimType_, Usage_>::Implementation matrixdata;
    typedef typename eigen_impl::RotationVector<SourcePrimType_, Usage_>::Scalar Scalar;
    const  typename eigen_impl::RotationVector<DestPrimType_, Usage_>::Implementation rv = rotationVector.toImplementation().template cast<DestPrimType_>();
    const SourcePrimType_ v1 = rv.x();
    const SourcePrimType_ v2 = rv.y();
    const SourcePrimType_ v3 = rv.z();
    const SourcePrimType_ v = rv.norm();

    if (v < common::internal::NumTraits<Scalar>::dummy_precision())  {

      // active to passive
//      matrixdata << 1.0,  v3, -v2,
//                        -v3, 1.0,  v1,
//                          v2, -v1, 1.0;
      // active to active
      matrixdata << 1.0,  -v3, v2,
                    v3, 1.0,  -v1,
                   -v2, v1, 1.0;
    } else {
      // active rotation vector to active matrix // not matlab code (is transposed)
      const DestPrimType_ t3 = v*(1.0/2.0);
      const DestPrimType_ t2 = sin(t3);
      const DestPrimType_ t4 = cos(t3);
      const DestPrimType_ t5 = 1.0/(v*v);
      const DestPrimType_ t6 = t4*v*v3;
      const DestPrimType_ t7 = t2*v1*v2;
      const DestPrimType_ t8 = t2*t2;
      const DestPrimType_ t9 = v1*v1;
      const DestPrimType_ t10 = v2*v2;
      const DestPrimType_ t11 = v3*v3;
      const DestPrimType_ t12 = v*v;
      const DestPrimType_ t13 = t4*t4;
      const DestPrimType_ t14 = t12*t13;
      const DestPrimType_ t15 = t2*v1*v3;
      const DestPrimType_ t16 = t4*v*v1;
      const DestPrimType_ t17 = t2*v2*v3;
      matrixdata(0,0) = t5*(t14-t8*(-t9+t10+t11));
      matrixdata(1,0) = t2*t5*(t6+t7)*2.0;
      matrixdata(2,0) = t2*t5*(t15-t4*v*v2)*2.0;
      matrixdata(0,1) = t2*t5*(t6-t7)*-2.0;
      matrixdata(1,1) = t5*(t14-t8*(t9-t10+t11));
      matrixdata(2,1) = t2*t5*(t16+t17)*2.0;
      matrixdata(0,2) = t2*t5*(t15+t4*v*v2)*2.0;
      matrixdata(1,2) = t2*t5*(t16-t17)*-2.0;
      matrixdata(2,2) = t5*(t14-t8*(t9+t10-t11));

    }

    eigen_impl::RotationMatrix<DestPrimType_, Usage_> matrix;
    matrix.toImplementation() = matrixdata;
    return matrix;

  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationMatrix<DestPrimType_, Usage_>, eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<DestPrimType_, Usage_> convert(const eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>& q) {
    eigen_impl::RotationMatrix<DestPrimType_, Usage_> matrix;
    matrix.toImplementation() = eigen_impl::eigen_internal::getRotationMatrixFromQuaternion<SourcePrimType_, DestPrimType_>(q.toImplementation());
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationMatrix<DestPrimType_, Usage_>, eigen_impl::RotationMatrix<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<DestPrimType_, Usage_> convert(const eigen_impl::RotationMatrix<SourcePrimType_, Usage_>& R) {
    eigen_impl::RotationMatrix<DestPrimType_, Usage_> matrix;
    matrix.toImplementation() = R.toImplementation().template cast<DestPrimType_>();
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationMatrix<DestPrimType_, Usage_>, eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>& xyz) {
    eigen_impl::RotationMatrix<DestPrimType_, Usage_> matrix;
    matrix.toImplementation() = eigen_impl::eigen_internal::getRotationMatrixFromRpy<SourcePrimType_, DestPrimType_>(xyz.toImplementation());
    return matrix;
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::RotationMatrix<DestPrimType_, Usage_>, eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>& zyx) {
    eigen_impl::RotationMatrix<DestPrimType_, Usage_> matrix;
    matrix.toImplementation() = eigen_impl::eigen_internal::getRotationMatrixFromYpr<SourcePrimType_, DestPrimType_>(zyx.toImplementation());
    return matrix;
  }
};

/*! \brief Multiplication of two rotation matrices
 */
template<typename PrimType_, enum RotationUsage Usage_>
class MultiplicationTraits<RotationBase<eigen_impl::RotationMatrix<PrimType_, Usage_>, Usage_>, RotationBase<eigen_impl::RotationMatrix<PrimType_, Usage_>, Usage_>> {
 public:
  inline static eigen_impl::RotationMatrix<PrimType_, Usage_> mult(const eigen_impl::RotationMatrix<PrimType_, Usage_>& lhs, const eigen_impl::RotationMatrix<PrimType_, Usage_>& rhs) {
    if(Usage_ == RotationUsage::ACTIVE) {
      eigen_impl::RotationMatrix<PrimType_, Usage_> result;
      result.toImplementation() = lhs.toImplementation() * rhs.toImplementation();
      return result;
    } else {
      eigen_impl::RotationMatrix<PrimType_, Usage_> result;
      result.toImplementation() = rhs.toImplementation() * lhs.toImplementation();
      return result;
    }
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
//template<typename PrimType_, enum RotationUsage Usage_>
//class RotationTraits<eigen_impl::RotationMatrix<PrimType_, Usage_>> {
// public:
//  template<typename get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::IndexType Cols>
//  inline static typename get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<Cols> rotate(const eigen_impl::RotationMatrix<PrimType_, Usage_>& R, const typename get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<Cols>& m){
//    return R.toImplementation() * m;
//  }
//};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Usage Conversion Traits - required?
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

template<typename PrimType_>
class UsageConversionTraits<eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE>,RotationUsage::PASSIVE> {
 public:
  inline static typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE>>::OtherUsage getActive(const eigen_impl::RotationMatrix<PrimType_,RotationUsage::PASSIVE>& in) {
    return typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::PASSIVE>>::OtherUsage(in.inverted().toImplementation());
  }

  // getPassive() does not exist (on purpose)
};

template<typename PrimType_>
class UsageConversionTraits<eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>,RotationUsage::ACTIVE> {
 public:
  inline static typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>>::OtherUsage getPassive(const eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>& in) {
    return typename get_other_usage<eigen_impl::RotationMatrix<PrimType_, RotationUsage::ACTIVE>>::OtherUsage(in.toImplementation());
  }

  // getActive() does not exist (on purpose)
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Box Operations - required?
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

//template<typename LeftPrimType_, typename RightPrimType_, enum RotationUsage Usage_>
//class BoxOperationTraits<RotationBase<eigen_impl::RotationMatrix<LeftPrimType_, Usage_>, Usage_>, RotationBase<eigen_impl::RotationMatrix<RightPrimType_, Usage_>, Usage_>> {
// public:
//  inline static typename internal::get_matrix3X<eigen_impl::RotationMatrix<LeftPrimType_, Usage_>>::template Matrix3X<1> box_minus(const eigen_impl::RotationMatrix<LeftPrimType_, Usage_>& lhs, const eigen_impl::RotationMatrix<RightPrimType_, Usage_>& rhs) {
//    return (lhs*rhs.inverted()).getLogarithmicMap();
//  }
//
//  inline static  eigen_impl::RotationMatrix<LeftPrimType_, Usage_> box_plus(const eigen_impl::RotationMatrix<RightPrimType_, Usage_>& rotation, const typename internal::get_matrix3X<eigen_impl::RotationMatrix<RightPrimType_, Usage_>>::template Matrix3X<1>& vector) {
//    return eigen_impl::RotationMatrix<LeftPrimType_, Usage_>((MapTraits<RotationBase<eigen_impl::RotationMatrix<RightPrimType_,Usage_>, Usage_>>::set_exponential_map(vector)).toImplementation()*rotation.toImplementation());
//  }
//};

//template<typename PrimType_, enum RotationUsage Usage_>
//class MapTraits<eigen_impl::RotationMatrix<PrimType_, Usage_>> {
// public:
//
//  inline static eigen_impl::RotationMatrix<PrimType_, Usage_> set_exponential_map(const typename internal::get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<1>& vector) {
//    typedef typename get_scalar<eigen_impl::RotationMatrix<PrimType_, Usage_>>::Scalar Scalar;
//    if (Usage_ == RotationUsage::ACTIVE) {
//      return eigen_impl::RotationMatrix<PrimType_, Usage_>(eigen_impl::RotationVector<Scalar, Usage_>(vector));
//    }
//    if (Usage_ == RotationUsage::PASSIVE) {
//      return eigen_impl::RotationMatrix<PrimType_, Usage_>(eigen_impl::RotationVector<Scalar, Usage_>(vector));
//    }
//  }
//
////  inline static typename internal::get_matrix3X<Rotation_>::template Matrix3X<1> get_logarithmic_map(const Rotation_& rotation) {
////    typedef typename get_scalar<Rotation_>::Scalar Scalar;
////    eigen_impl::RotationVector<Scalar, Rotation_::Usage> rotationVector(rotation);
////    return rotationVector.getUnique().toImplementation();
////  }
//
//  inline static typename internal::get_matrix3X<eigen_impl::RotationMatrix<PrimType_, Usage_>>::template Matrix3X<1> get_logarithmic_map(const eigen_impl::RotationMatrix<PrimType_, Usage_>& rotation) {
//    typedef typename get_scalar<eigen_impl::RotationMatrix<PrimType_, Usage_>>::Scalar Scalar;
//
//
//
//    if (Usage_ == RotationUsage::ACTIVE) {
//      return eigen_impl::RotationVector<Scalar, Usage_>(rotation).toImplementation();
//    }
//    if (Usage_ == RotationUsage::PASSIVE) {
//      return eigen_impl::RotationVector<Scalar, Usage_>(rotation).toImplementation();
//    }
//  }
//
//};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class FixingTraits<eigen_impl::RotationMatrix<PrimType_, Usage_>> {
 public:
  inline static void fix(eigen_impl::RotationMatrix<PrimType_, Usage_>& R) {
    const PrimType_ factor = 1/pow(R.determinant(), 1.0/3.0);
    R.setMatrix(factor*R.matrix()(0,0),
                factor*R.matrix()(0,1),
                factor*R.matrix()(0,2),
                factor*R.matrix()(1,0),
                factor*R.matrix()(1,1),
                factor*R.matrix()(1,2),
                factor*R.matrix()(2,0),
                factor*R.matrix()(2,1),
                factor*R.matrix()(2,2));
  }
};

} // namespace internal
} // namespace rotations
} // namespace kindr

#endif /* KINDR_ROTATIONS_EIGEN_ROTATIONMATRIX_HPP_ */


/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/**
 *
 * \brief This application performs MAP inference on Markov Nets
 * provided in standard UAI file format via Dual-Decomposition.
 *
 *
 *  \authors Dhruv Batra, André Martins, Aroma Mahendru
 */

#ifndef _AD3_QP_HPP_
#define _AD3_QP_HPP_

#include <Eigen/Eigenvalues>
#include <math.h>
#include <limits>
#include "dd_grlab.hpp"

#define NEARLY_ZERO_TOL(a,tol) (((a)<=(tol)) && ((a)>=(-(tol))))
#define NEARLY_EQ_TOL(a,b,tol) (((a)-(b))*((a)-(b))<=(tol))
#define num_max_iterations_QP_ 10

/**
 * \brief ADMM vertex program  general implements Alternating direction dual
 * decomposition for general dense factors.
 */

struct admm_vertex_program_general:public admm_vertex_program {

/**
 * \brief Maximize returns the maximum value and configuration with reference to
 * input additional and variable log potentials. addtional log potential corresponds
 * factor potentials and variable potential corresponds to sum of lagrange
 * multipliers and unary potentials divided by degree of the unary vertex.
 */

void Maximize(vertex_type& vertex, vec additional_log_potentials, vec variable_log_potentials,
                Configuration &configuration,
                double *value) {

    vector <Configuration> states(vertex.data().nvars,-1);
    int best = -1;
    *value = -1e12;
    for (int index = 0;
         index < additional_log_potentials.size();
         ++index) {
      double score = additional_log_potentials[index];
      get_configuration_states(vertex,index, &states);
      int offset = 0;
      for (int i = 0; i < vertex.data().nvars; ++i) {
        score += variable_log_potentials[offset+states[i]];
        offset = vertex.data().cards[i];

      }

      if (configuration < 0 || score > *value) {
        configuration = index;
        *value = score;
      }
    }
    assert(configuration >= 0);

  }

 void DeleteConfiguration(Configuration &configuration) {
    configuration = -1;
  }

/**
 * \brief InvertAfterInsertion function is used to invert the matrix. Used in solveQP
 */

 bool InvertAfterInsertion(vertex_type& vertex, vector <double> & inverse_A_,
        const vector<Configuration> &active_set, const Configuration &inserted_element) {

  vector<double> inverse_A = inverse_A_;
  int size_A = active_set.size() + 1;
  vector<double> r(size_A);

  r[0] = 1.0;
  for (int i = 0; i < active_set.size(); ++i) {
    // Count how many variable values the new assignment
    // have in common with the i-th assignment.
    int num_common_values = CountCommonValues(vertex, active_set[i], inserted_element);
    r[i+1] = static_cast<double>(num_common_values);
  }

  double r0 = static_cast<double>(CountCommonValues(vertex,
      inserted_element, inserted_element));
  double s = r0;
  for (int i = 0; i < size_A; ++i) {
    if (r[i] == 0.0) continue;
    s -= r[i] * r[i] * inverse_A[i * size_A + i];
    for (int j = i+1; j < size_A; ++j) {
      if (r[j] == 0.0) continue;
      s -= 2 * r[i] * r[j] * inverse_A[i * size_A + j];
    }
  }

    if (NEARLY_ZERO_TOL(s, 1e-9)) {
         if (opts.verbose> 2) {
      cout << "Warning: updated matrix will become singular after insertion."
           << endl;
    }
    return false;
  }

  double invs = 1.0 / s;
  vector<double> d(size_A, 0.0);
  for (int i = 0; i < size_A; ++i) {
    if (r[i] == 0.0) continue;
    for (int j = 0; j < size_A; ++j) {
      d[j] += inverse_A[i * size_A + j] * r[i];
    }
  }

  int size_A_after = size_A + 1;
  inverse_A_.resize(size_A_after * size_A_after);
  for (int i = 0; i < size_A; ++i) {
    for (int j = 0; j < size_A; ++j) {
      inverse_A_[i * size_A_after + j] = inverse_A[i * size_A + j] +
          invs * d[i] * d[j];
    }
    inverse_A_[i * size_A_after + size_A] = -invs * d[i];
    inverse_A_[size_A * size_A_after + i] = -invs * d[i];
  }
  inverse_A_[size_A * size_A_after + size_A] = invs;

  return true;
}

/**
 * \brief InvertAfterRemoval function is used to invert the matrix. Used in solveQP
 */
void InvertAfterRemoval(vector <double> &inverse_A_,const vector<Configuration> &active_set,
                                       int removed_index) {
  vector<double> inverse_A = inverse_A_;
  int size_A = active_set.size() + 1;
  vector<double> r(size_A);

  ++removed_index; // Index in A has an offset of 1.
  double invs = inverse_A[removed_index * size_A + removed_index];
  assert(!NEARLY_ZERO_TOL(invs, 1e-12));
  double s = 1.0 / invs;
  vector<double> d(size_A - 1, 0.0);
  int k = 0;
  for (int i = 0; i < size_A; ++i) {
    if (i == removed_index) continue;
    d[k] = -s * inverse_A[removed_index * size_A + i];
    ++k;
  }

  int size_A_after = size_A - 1;
  inverse_A_.resize(size_A_after * size_A_after);
  k = 0;
  for (int i = 0; i < size_A; ++i) {
    if (i == removed_index) continue;
    int l = 0;
    for (int j = 0; j < size_A; ++j) {
      if (j == removed_index) continue;
      inverse_A_[k * size_A_after + l] = inverse_A[i * size_A + j] -
          invs * d[k] * d[l];
      ++l;
    }
    ++k;
  }
}

/**
 * \brief ComputeActiveSetSimilarities computes Mnz'*Mnz. Used in solveQP
 */
void ComputeActiveSetSimilarities(vertex_type& vertex,
    const vector<Configuration> &active_set,
    vector<double> *similarities) {
  int size = active_set.size();

  // Compute similarity matrix.
  similarities->resize(size * size);
  (*similarities)[0] = 0.0;
  for (int i = 0; i < active_set.size(); ++i) {
    (*similarities)[i*size + i] = static_cast<double>(
        CountCommonValues(vertex,active_set[i], active_set[i]) );
    for (int j = i+1; j < active_set.size(); ++j) {
      // Count how many variable values the i-th and j-th
      // assignments have in common.
      int num_common_values = CountCommonValues(vertex,active_set[i], active_set[j]);
      (*similarities)[i*size + j] = num_common_values;
      (*similarities)[j*size + i] = num_common_values;
    }
  }
}

/**
 * \brief  ComputeMarginalsFromSparseDistribution computes marginalvalues for unary
 * factor from given factor distribution.
 */

void ComputeMarginalsFromSparseDistribution( vertex_type& vertex,
    const vector<Configuration> &active_set,
    const vector<double> &distribution,
    vec  &variable_posteriors,
    vec &additional_posteriors) {
    variable_posteriors.setZero();
    additional_posteriors.setZero();
    for (int i = 0; i < active_set.size(); ++i) {
    UpdateMarginalsFromConfiguration(vertex,active_set[i],
                                       distribution[i],
                                       variable_posteriors,
                                       additional_posteriors);
    }
  }

   // Given a configuration with a probability (weight),
  // increment the vectors of variable and additional posteriors.
  void UpdateMarginalsFromConfiguration(vertex_type& vertex,
    const Configuration &configuration,
    double weight,
    vec &variable_posteriors,
    vec &additional_posteriors) {

     vector <Configuration> states(vertex.data().nvars, -1);
     get_configuration_states(vertex, configuration, &states);

            int offset = 0;

            for (int k = 0; k < vertex.data().nvars; ++k)
            {   variable_posteriors[offset + states[k]] += weight;
                offset += vertex.data().cards[k];
            }
    additional_posteriors[configuration] += weight;

  }
  // Count how many common values two configurations have.
  int CountCommonValues(vertex_type& vertex,Configuration configuration1,
                        Configuration configuration2) {

    //assert(states1->size() == states2->size());
    int count = 0;
    vector <Configuration> states1(vertex.data().nvars, -1);
    vector <Configuration> states2(vertex.data().nvars, -1);
    get_configuration_states(vertex, configuration1, &states1);
    get_configuration_states(vertex, configuration2, &states2);
    for(int i = 0; i< vertex.data().nvars; i++)
    {  if (states1[i] == states2[i])
      { count++;} }
    return count;
  }

/**
 * \brief Evaluate returns the maximum value  with reference to
 * input additional and variable log potentials and configuration. addtional
 * log potential corresponds factor potentials and variable potential corresponds
 * to sum of lagrange  * multipliers and unary potentials divided by degree of
 * the unary vertex.
 */

void Evaluate(vertex_type& vertex, vec additional_log_potentials, vec variable_log_potentials,
                const Configuration configuration,
                double *value) {

    vector<Configuration> states(vertex.data().nvars, -1);
    get_configuration_states(vertex, configuration, &states);
    *value = 0.0;
    int offset = 0;
    for (int i = 0;i<vertex.data().nvars; ++i) {
      *value += variable_log_potentials[offset + states[i]];
      offset = vertex.data().cards[i];
    }
    *value += additional_log_potentials[configuration];
  }

  void EigenDecompose(vector<double> *similarities,
                            vector<double> *eigenvalues) {

  int size = sqrt(similarities->size());

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
  Eigen::MatrixXd sim(size, size);
  int t = 0;
  for (int i = 0; i < size; ++i) {
    for (int j = 0; j < size; ++j) {
      sim(i, j) = (*similarities)[t];
      ++t;
    }
  }
  es.compute(sim);
  const Eigen::VectorXd &eigvals = es.eigenvalues();
  eigenvalues->resize(size);
  for (int i = 0; i < size; ++i) {
    (*eigenvalues)[i] = eigvals[i];
  }
  const Eigen::MatrixXd &eigvectors = es.eigenvectors().transpose();
  t = 0;
  for (int i = 0; i < size; ++i) {
    for (int j = 0; j < size; ++j) {
      (*similarities)[t] = eigvectors(i, j);
      ++t;
    }
  }

}

// Function to solve each quadratic sub problem for dense factors.
// It uses active set method. Caching is deactivated
// TODO: Activate caching feature

void SolveQP_dense(vertex_type& vertex,const gather_type& total,
            vec& variable_posteriors, vec& additional_posteriors) {
   vertex_data& vdata = vertex.data();
   //cout<<"loop 1 in solveQP .."<<endl;
   vec additional_log_potentials = vdata.potentials;
   vec variable_log_potentials = total.neighbor_distribution + total.messages;
   vector <Configuration> active_set_;
   vector<double> distribution_;
   vector<double> inverse_A_;
  // Initialize the active set.

   if (active_set_.size() == 0) {
    variable_posteriors.resize(variable_log_potentials.size());
    additional_posteriors.resize(additional_log_potentials.size());
    distribution_.clear();
    // Initialize by solving the LP, discarding the quadratic
    // term.
    Configuration configuration = -1;
    double value;
    Maximize(vertex, additional_log_potentials, variable_log_potentials,
             configuration,
             &value);
    active_set_.push_back(configuration);
    distribution_.push_back(1.0);

    // Initialize inv(A) as [-M,1;1,0].
    inverse_A_.resize(4);
    inverse_A_[0] = static_cast<double>(
        -CountCommonValues(vertex,configuration, configuration));
    inverse_A_[1] = 1;
    inverse_A_[2] = 1;
    inverse_A_[3] = 0;
  }

  bool changed_active_set = true;
  vector<double> z;
  int num_max_iterations = num_max_iterations_QP_;
  double tau = 0;
  for (int iter = 0; iter < num_max_iterations; ++iter) {
    bool same_as_before = true;
    bool unbounded = false;
    if (changed_active_set) {
      // Recompute vector b.
      vector<double> b(active_set_.size() + 1, 0.0);
      b[0] = 1.0;
      for (int i = 0; i < active_set_.size(); ++i) {
        const Configuration &configuration = active_set_[i];
        double score;
        Evaluate(vertex, additional_log_potentials, variable_log_potentials,
                 configuration,
                 &score);
        b[i+1] = score;
      }
      // Solve the system Az = b.
      z.resize(active_set_.size());
      int size_A = active_set_.size() + 1;
      for (int i = 0; i < active_set_.size(); ++i) {
        z[i] = 0.0;
        for (int j = 0; j < size_A; ++j) {
          z[i] += inverse_A_[(i+1) * size_A + j] * b[j];
        }
      }
      tau = 0.0;
      for (int j = 0; j < size_A; ++j) {
        tau += inverse_A_[j] * b[j];
      }

      same_as_before = false;
    }

    if (same_as_before) {
      // Compute the variable marginals from the full distribution
      // stored in z.
      ComputeMarginalsFromSparseDistribution(vertex, active_set_,
                                             z,
                                             variable_posteriors,
                                             additional_posteriors);
      // Get the most violated constraint
      // (by calling the black box that computes the MAP).
      vec scores = variable_log_potentials;
      for (int i = 0; i < scores.size(); ++i) {
        scores[i] -= variable_posteriors[i];
      }
      Configuration configuration = -1;
      double value = 0.0;

      Maximize(vertex,
                additional_log_potentials, scores,
               configuration,
               &value);
      double very_small_threshold = 1e-9;
      if (value <= tau + very_small_threshold) { // value <= tau.
        // We have found the solution;
        // the distribution, active set, and inv(A) are cached for the next round.
        DeleteConfiguration(configuration);
        return;
      } else {
        for (int k = 0; k < active_set_.size(); ++k) {
          // This is expensive and should just be a sanity check.
          // However, in practice, numerical issues force an already existing
          // configuration to try to be added. Therefore, we always check
          // if a configuration already exists before inserting it.
          // If it does, that means the active set method converged to a
          // solution (but numerical issues had prevented us to see it.)
          if (active_set_[k] == configuration) {
            if (opts.verbose > 2) {
              cout << "Warning: value - tau = "
                   << value - tau << " " << value << " " << tau
                   << endl;
            }
            // We have found the solution;
            // the distribution, active set, and inv(A)
            // are cached for the next round.
            DeleteConfiguration(configuration);

            // Just in case, clean the cache.
            // This may prevent eventual numerical problems in the future.
            for (int j = 0; j < active_set_.size(); ++j) {
              if (j == k) continue; // This configuration was deleted already.
              DeleteConfiguration(active_set_[j]);
            }
            active_set_.clear();
            inverse_A_.clear();
            distribution_.clear();

            // Return.
            return;
          }
        }
        z.push_back(0.0);
        distribution_ = z;

        // Update inv(A).
        bool singular = !InvertAfterInsertion(vertex, inverse_A_, active_set_, configuration);
        if (singular) {
          // If adding a new configuration causes the matrix to be singular,
          // don't just add it. Instead, look for a configuration in the null
          // space and remove it before inserting the new one.
          // Right now, if more than one such configuration exists, we just
          // remove the first one we find. There's a chance this could cause
          // some cyclic behaviour. If that is the case, we should randomize
          // this choice.
          // Note: This step is expensive and requires an eigendecomposition.
          // TODO: I think there is a graph interpretation for this problem.
          // Maybe some specialized graph algorithm is cheaper than doing
          // the eigendecomposition.
          vector<double> similarities(active_set_.size() * active_set_.size());
          ComputeActiveSetSimilarities(vertex, active_set_, &similarities);

          //cout<<"compute active similarities in solveQP .."<<endl;
          vector<double> padded_similarities((active_set_.size()+2) *
                                             (active_set_.size()+2), 1.0);
          for (int i = 0; i < active_set_.size(); ++i) {
            for (int j = 0; j < active_set_.size(); ++j) {
              padded_similarities[(i+1)*(active_set_.size()+2) + (j+1)] =
                  similarities[i*active_set_.size() + j];
            }
          }
          padded_similarities[0] = 0.0;
          for (int i = 0; i < active_set_.size(); ++i) {
            double value = static_cast<double>(
                CountCommonValues(vertex, configuration, active_set_[i]));
            padded_similarities[(i+1)*(active_set_.size()+2) +
                                (active_set_.size()+1)] = value;
            padded_similarities[(active_set_.size()+1)*(active_set_.size()+2) +
                                (i+1)] = value;
          }
          double value = static_cast<double>(
              CountCommonValues(vertex, configuration, configuration));
          padded_similarities[(active_set_.size()+1)*(active_set_.size()+2) +
                              (active_set_.size()+1)] = value;

          vector<double> eigenvalues(active_set_.size()+2);
          EigenDecompose(&padded_similarities, &eigenvalues);
          int zero_eigenvalue = -1;
          for (int i = 0; i < active_set_.size()+2; ++i) {
            if (NEARLY_EQ_TOL(eigenvalues[i], 0.0, 1e-9)) {
              if (zero_eigenvalue >= 0) {
                // If this happens, something failed. Maybe a numerical problem
                // may cause this. In that case, just give up, clean the cache
                // and return. Hopefully the next iteration will fix it.
                cout << "Multiple zero eigenvalues: "
                     << eigenvalues[zero_eigenvalue] << " and "
                     << eigenvalues[i] << endl;
                cout << "Warning: Giving up." << endl;
                // Clean the cache.
                for (int j = 0; j < active_set_.size(); ++j) {
                  DeleteConfiguration(active_set_[j]);
                }
                active_set_.clear();
                inverse_A_.clear();
                distribution_.clear();
                return;
              }
              zero_eigenvalue = i;
            }
          }
          assert(zero_eigenvalue >= 0);
          vector<int> configurations_to_remove;
          for (int j = 1; j < active_set_.size()+1; ++j) {
            double value = padded_similarities[zero_eigenvalue*(active_set_.size()+2) + j];
            if (!NEARLY_EQ_TOL(value, 0.0, 1e-9)) {
              configurations_to_remove.push_back(j-1);
            }
          }
          if (opts.verbose > 2) {
            cout << "Pick a configuration to remove (" << configurations_to_remove.size()
                 << " out of " << active_set_.size() << ")." << endl;
          }

          assert(configurations_to_remove.size() >= 1);
          int j = configurations_to_remove[0];

          // Update inv(A).
          InvertAfterRemoval(inverse_A_, active_set_, j);

          // Remove blocking constraint from the active set.
          DeleteConfiguration(active_set_[j]); // Delete configutation.
          active_set_.erase(active_set_.begin() + j);

          singular = !InvertAfterInsertion(vertex, inverse_A_, active_set_, configuration);
          assert(!singular);
        }

        // Insert configuration to active set.
        if (opts.verbose > 2) {
          cout << "Inserted one element to the active set (iteration "
               << iter << ")." << endl;
        }
        active_set_.push_back(configuration);
        changed_active_set = true;
      }
    } else {
      // Solution has changed from the previous iteration.
      // Look for blocking constraints.
      int blocking = -1;
      bool exist_blocking = false;
      double alpha = 1.0;
      for (int i = 0; i < active_set_.size(); ++i) {
        assert(distribution_[i] >= -1e-12);
        if (z[i] >= distribution_[i]) continue;
        if (z[i] < 0) exist_blocking = true;
        double tmp = distribution_[i] / (distribution_[i] - z[i]);
        if (blocking < 0 || tmp < alpha) {
          alpha = tmp;
          blocking = i;
        }
      }

      if (!exist_blocking) {
        // No blocking constraints.
        assert(!unbounded);
        distribution_ = z;
        alpha = 1.0;
        changed_active_set = false;
      } else {
        if (alpha > 1.0 && !unbounded) alpha = 1.0;
        // Interpolate between factor_posteriors_[i] and z.
        if (alpha == 1.0) {
          distribution_ = z;
        } else {
          for (int i = 0; i < active_set_.size(); ++i) {
            z[i] = (1 - alpha) * distribution_[i] + alpha * z[i];
            distribution_[i] = z[i];
          }
        }

        // Update inv(A).
        InvertAfterRemoval(inverse_A_, active_set_, blocking);

        // Remove blocking constraint from the active set.
        if (opts.verbose > 2) {
          cout << "Removed one element to the active set (iteration "
               << iter << ")." << endl;
        }

        DeleteConfiguration(active_set_[blocking]); // Delete configutation.
        active_set_.erase(active_set_.begin() + blocking);

        z.erase(z.begin() + blocking);
        distribution_.erase(distribution_.begin() + blocking);
        changed_active_set = true;
        for (int i = 0; i < distribution_.size(); ++i) {
          assert(distribution_[i] > -1e-16);
        }
      }
    }
  }

  // Maximum number of iterations reached.
  // Return the best existing solution by computing the variable marginals
  // from the full distribution stored in z.
  //assert(false);
  ComputeMarginalsFromSparseDistribution(vertex, active_set_,
                                         z,
                                         variable_posteriors,
                                         additional_posteriors);
  }

  void InsertionSort(pair<double, int> arr[], int length) {
  int i, j;
  pair<double, int> tmp;

  for (i = 1; i < length; i++) {
    j = i;
    while (j > 0 && arr[j - 1].first > arr[j].first) {
      tmp = arr[j];
      arr[j] = arr[j - 1];
      arr[j - 1] = tmp;
      j--;
    }
  }
}

  int project_onto_budget_constraint_cached(vec& x,
                                          int d,
                                          double budget,
                                          vector<pair<double,int> >& y) {
  int j, k, l, level;
  double s = 0.0;
  double tau = 0.0, tightsum;
  double left, right = -std::numeric_limits<double>::infinity();

  // Load x into a reordered y (the reordering is cached).
  if (y.size() != d) {
    y.resize(d);
    for (j = 0; j < d; j++) {
      s -= x[j];
      y[j].first = -x[j];
      y[j].second = j;
    }
    sort(y.begin(), y.end());
  } else {
    for (j = 0; j < d; j++) {
      s -= x[j];
      y[j].first = -x[y[j].second];
    }
    // If reordering is cached, use a sorting algorithm
    // which is fast when the vector is almost sorted.
    InsertionSort(&y[0], d);
  }

  tightsum = s;
  s += budget;

  k = l = level = 0;
  bool found = false;
  double val_a, val_b;
  while (k < d && l < d) {
    if (level != 0) {
      tau = (s - tightsum) / static_cast<double>(level);
    }
    if (k < d) val_a = y[k].first;
    val_b = 1.0 + y[l].first;
    left = right;
    if (k == d || val_b <= val_a) {
      right = val_b;
    } else {
      right = val_a;
    }
    if ((level == 0 && s == tightsum) || (level != 0 && tau <= right)) {
      // Found the right split-point!
      found = true;
      break;
    }
    if (k == d || val_b <= val_a) {
      tightsum += val_b;
      --level;
      ++l;
    } else {
      tightsum -= val_a;
      ++level;
      ++k;
    }
  }

  if (!found) {
    left = right;
    right = std::numeric_limits<double>::infinity();
  }

  for (j = 0; j < d; j++) {
    if (-x[j] >= right) {
      x[j] = 0.0;
    } else if (1.0 - x[j] <= left) {
      x[j] = 1.0;
    } else {
      x[j] += tau;
    }
  }

  return 0;
}

  // Solve the QP subproblem for budget factor.
  // TODO Enable caching
void SolveQP_budget(vertex_type& vertex,const gather_type& total,
            vec& variable_posteriors, vec& additional_posteriors){

  vertex_data& vdata =  vertex.data();
  vec variable_log_potentials = total.neighbor_distribution + total.messages;
  vector<pair<double,int> > last_sort_;
  for (int f = 0; f < variable_log_potentials.size(); ++f) {
    variable_posteriors[f] = variable_log_potentials[f];
    if (variable_posteriors[f] < 0.0) {
      variable_posteriors[f] = 0.0;
    } else if (variable_posteriors[f] > 1.0) {
      variable_posteriors[f] = 1.0;
    }
  }

  double s = 0.0;
  for (int f = 0; f < vdata.nvars; ++f) {
    s += variable_posteriors[f];
  }

  if (s > static_cast<double>(vdata.budget)) {
    for (int f = 0; f < variable_log_potentials.size(); ++f) {
      variable_posteriors[f] = variable_log_potentials[f];
    }
    project_onto_budget_constraint_cached(variable_posteriors,
                                          variable_log_potentials.size(),
                                          static_cast<double>(vdata.budget),
                                          last_sort_);
  }

}

// Finds best configuration ofr budget factors
void SolveMAP_budget(vertex_type& vertex,const gather_type& total,
            vec& variable_posteriors, vec& additional_posteriors, double& value) {

   vertex_data& vdata = vertex.data();
  // Create a local copy of the log potentials.
   vec log_potentials(total.messages);
  double valaux;

  value = 0.0;

  int num_active = 0;
  double sum = 0.0;

  for (int f = 0; f < vdata.nvars; ++f) {
    valaux = log_potentials[f];
    if (valaux < 0.0) {
      variable_posteriors[f] =  0.0;
    } else {
      sum += valaux;
      variable_posteriors[f] = 1.0;
    }
    ++num_active;
  }
  if (num_active > vdata.budget) {
    vector<pair<double,int> > scores(vdata.nvars);
    for (int f = 0; f < vdata.nvars; ++f) {
      scores[f].first = -log_potentials[f];
      scores[f].second = f;
    }

    sort(scores.begin(), scores.end());
    num_active = 0;
    sum = 0.0;
    for (int k = 0; k < vdata.budget; ++k) {
      valaux = -scores[k].first;
      if (valaux < 0.0) break;
      int f = scores[k].second;
      variable_posteriors[f] = 1.0;
      sum += valaux;
      ++num_active;
    }

    for (int k = num_active; k < vdata.nvars; ++k) {
      int f = scores[k].second;
      variable_posteriors[f] = 0.0;
    }
   // cout<<"third inner loop ..."<<endl;
  }

  value += sum;

}

// Finds best configuration for dense factors
void SolveMAP_dense(vertex_type& vertex,const gather_type& total,
            vec& variable_posteriors, vec& additional_posteriors, double& value){
     vertex_data& vdata = vertex.data();
     vec beliefs = vdata.potentials;
     int num_configurations = vdata.potentials.size();
     for (int index_configuration = 0;
           index_configuration < num_configurations;
            ++index_configuration) {
        vector<int> states(vdata.nvars, -1);
        get_configuration_states(vertex, index_configuration, &states);
        int offset = 0;
        for (int k = 0; k < vdata.nvars; ++k) {
             beliefs[index_configuration] += total.messages[offset + states[k]];
             offset += vdata.cards[k];}
    }

        value = beliefs.maxCoeff();

 }

// Finds beliefs using dense and budget factors
void compute_beliefs(vertex_type& vertex,const gather_type& total,
            vec& variable_posteriors, vec& additional_posteriors){
switch(vertex.data().factor_type){

case 0: SolveQP_dense(vertex,total, variable_posteriors, additional_posteriors);
        break;
case 1: SolveQP_budget(vertex,total, variable_posteriors, additional_posteriors);

}
};

// General solvMAP function
void SolveMAP(vertex_type& vertex,const gather_type& total,
            vec& variable_posteriors, vec& additional_posteriors, double& value){
switch(vertex.data().factor_type){

case 0: SolveMAP_dense(vertex,total, variable_posteriors, additional_posteriors, value);
        break;
case 1: SolveMAP_budget(vertex,total, variable_posteriors, additional_posteriors, value);
  }
 };

};

#endif


//
// Copyright (c) 2009, Markus Rickert
// 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.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream>
#include <memory>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <rl/math/Unit.h>
#include <rl/mdl/Body.h>
#include <rl/mdl/Dynamic.h>
#include <rl/mdl/Fixed.h>
#include <rl/mdl/Frame.h>
#include <rl/mdl/Revolute.h>
#include <rl/mdl/RungeKuttaNystromIntegrator.h>
#include <rl/mdl/World.h>

std::shared_ptr<rl::mdl::Dynamic>
createPlanar2()
{
	std::shared_ptr<rl::mdl::Dynamic> dynamic = std::make_shared<rl::mdl::Dynamic>();
	dynamic->setName("planar2");

	// frames

	std::shared_ptr<rl::mdl::World> world = std::make_shared<rl::mdl::World>();
	world->setName("world");
	world->setGravity(rl::math::Vector3(0, 0, rl::math::constants::gravity));
	dynamic->add(world);

	std::shared_ptr<rl::mdl::Body> link0 = std::make_shared<rl::mdl::Body>();
	link0->setName("link0");
	dynamic->add(link0);

	std::shared_ptr<rl::mdl::Body> link1 = std::make_shared<rl::mdl::Body>();
	link1->setCenterOfMass(static_cast<rl::math::Real>(0.5), 0, 0);
	link1->setInertia(static_cast<rl::math::Real>(0.01), static_cast<rl::math::Real>(0.083333333), static_cast<rl::math::Real>(0.083333333), 0, 0, 0);
	link1->setMass(1);
	link1->setName("link1");
	dynamic->add(link1);

	std::shared_ptr<rl::mdl::Frame> frame0 = std::make_shared<rl::mdl::Frame>();
	frame0->setName("frame0");
	dynamic->add(frame0);

	std::shared_ptr<rl::mdl::Body> link2 = std::make_shared<rl::mdl::Body>();
	link2->setCenterOfMass(static_cast<rl::math::Real>(0.5), 0, 0);
	link2->setInertia(static_cast<rl::math::Real>(0.01), static_cast<rl::math::Real>(0.083333333), static_cast<rl::math::Real>(0.083333333), 0, 0, 0);
	link2->setMass(1);
	link2->setName("link2");
	dynamic->add(link2);

	std::shared_ptr<rl::mdl::Frame> frame1 = std::make_shared<rl::mdl::Frame>();
	frame1->setName("frame1");
	dynamic->add(frame1);

	// selfcollision

	link0->setCollision(false);
	link1->setCollision(link0.get(), false);
	link1->setCollision(link2.get(), false);
	link2->setCollision(link1.get(), false);

	// transforms

	std::shared_ptr<rl::mdl::Fixed> fixed0 = std::make_shared<rl::mdl::Fixed>();
	fixed0->setName("fixed0");
	dynamic->add(fixed0, world.get(), link0.get());

	std::shared_ptr<rl::mdl::Revolute> joint0 = std::make_shared<rl::mdl::Revolute>();
	joint0->setMaximum(rl::math::Vector::Constant(1, 360 * rl::math::constants::deg2rad));
	joint0->setMinimum(rl::math::Vector::Constant(1, -360 * rl::math::constants::deg2rad));
	joint0->setName("joint0");
	dynamic->add(joint0, link0.get(), link1.get());

	std::shared_ptr<rl::mdl::Fixed> fixed1 = std::make_shared<rl::mdl::Fixed>();
	fixed1->setName("fixed1");
	fixed1->setTransform(rl::math::Transform(rl::math::Translation(rl::math::Vector3(1, 0, 0))));
	dynamic->add(fixed1, link1.get(), frame0.get());

	std::shared_ptr<rl::mdl::Revolute> joint1 = std::make_shared<rl::mdl::Revolute>();
	joint1->setMaximum(rl::math::Vector::Constant(1, 360 * rl::math::constants::deg2rad));
	joint1->setMinimum(rl::math::Vector::Constant(1, -360 * rl::math::constants::deg2rad));
	joint1->setName("joint1");
	dynamic->add(joint1, frame0.get(), link2.get());

	std::shared_ptr<rl::mdl::Fixed> fixed2 = std::make_shared<rl::mdl::Fixed>();
	fixed2->setName("fixed2");
	fixed2->setTransform(rl::math::Transform(rl::math::Translation(rl::math::Vector3(1, 0, 0))));
	dynamic->add(fixed2, link2.get(), frame1.get());

	// initialize

	dynamic->update();

	return dynamic;
}

int
main(int argc, char** argv)
{
	if (argc < 7)
	{
		std::cout << "Usage: rlDynamics1Planar2Demo Q1 Q2 QD1 QD2 QDD1 QDD2" << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		std::shared_ptr<rl::mdl::Dynamic> dynamic = createPlanar2();

		rl::math::Vector q(dynamic->getDofPosition());
		rl::math::Vector qd(dynamic->getDof());
		rl::math::Vector qdd(dynamic->getDof());

		for (std::size_t i = 0; i < dynamic->getDofPosition(); ++i)
		{
			q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 1]);
		}

		for (std::size_t i = 0; i < dynamic->getDof(); ++i)
		{
			qd(i) = boost::lexical_cast<rl::math::Real>(argv[i + 1 + dynamic->getDofPosition()]);
			qdd(i) = boost::lexical_cast<rl::math::Real>(argv[i + 1 + dynamic->getDofPosition() + dynamic->getDof()]);
		}

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);

		dynamic->inverseDynamics();
		std::cout << "tau = " << dynamic->getTorque().transpose() << std::endl;

		dynamic->forwardDynamics();
		std::cout << "qdd = " << dynamic->getAcceleration().transpose() << std::endl;

		rl::mdl::RungeKuttaNystromIntegrator integrator(dynamic.get());
		integrator.integrate(1);
		std::cout << "q = " << dynamic->getPosition().transpose() << std::endl;
		std::cout << "qd = " << dynamic->getVelocity().transpose() << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the tsppd program and library for solving           */
/*  Traveling Salesman Problems with Pickup and Delivery. tsppd requires     */
/*  other commercial and open source software to build. tsppd is decribed    */
/*  in the paper "Exact Methods for Solving Traveling Salesman Problems      */
/*  with Pickup and Delivery in Real Time".                                  */
/*                                                                           */
/*  Copyright (C) 2017 Ryan J. O'Neil <roneil1@gmu.edu>                      */
/*                                                                           */
/*  tsppd is distributed under the terms of the ZIB Academic License.        */
/*  You should have received a copy of the ZIB Academic License along with   */
/*  tsppd. See the file LICENSE. If not, email roneil1@gmu.edu.              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <algorithm>
#include <cmath>
#include <iterator>
#include <limits>
#include <utility>

#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <tsppd/solver/focacci/filter/one_tree/focacci_tsp_one_tree.h>

using namespace Gecode;
using namespace TSPPD::Data;
using namespace TSPPD::Solver;
using namespace std;

OneTree::OneTree(
    ViewArray<Int::IntView>& next,
    const TSPPDProblem& problem,
    const unsigned int max_iterations) :
    OneTree(next, problem, max_iterations, nullptr) { }

OneTree::OneTree(
    ViewArray<Int::IntView>& next,
    const TSPPDProblem& problem,
    const unsigned int max_iterations,
    TSPPD::AP::PrimalDualAPSolver* ap) :
    next(next),
    problem(problem),
    max_iterations(max_iterations),
    ap(ap),
    graph(next.size() - 2),
    potentials(next.size(), 0),
    edges(next.size(), set<int>()),
    start_index(problem.index("+0")),
    end_index(problem.index("-0")),
    iteration(1),
    w(0) {

    weights = boost::get(boost::edge_weight, graph);
    initialize_one_tree();
}

double OneTree::bound() {
    double best_w = 0;
    vector<set<int>> best_edges;

    while (!done) {
        improve();

        if (iteration == 1 || w > best_w) {
            best_w = w;
            best_edges = edges;
        }

        if (done)
            break;
    }

    w = best_w;
    edges = best_edges;
    return w;
}

void OneTree::improve() {
    // Find the max min 1-tree by updating node potentials based on violation of degree constraints.
    bool is_tour = true;

    // Compute optimal 1-tree.
    edges = vector<set<int>>(next.size(), set<int>());
    auto new_w = minimize_one_tree();

    // Update step size
    auto M = max_iterations;
    auto m = iteration;
    if (m == 1) {
        t1 = new_w / (2.0 * next.size());
        ti = t1;
    } else {
        ti = t1*(m - 1)*(2*M - 5)/(2*(M-1)) - t1*(m-2) + t1*(m-1)*(m-2)/(2*(M-1)*(M-2));
    }

    // Remove node potentials from tour.
    for (auto pi : potentials)
        new_w -= 2 * pi;

    // Update node potentials.
    for (int node = 0; node < next.size(); ++node) {
        if (node != start_index && node != end_index && edges[node].size() != 2)
            is_tour = false;
        potentials[node] += (((int) edges[node].size()) - 2) * ti;
    }

    if (is_tour || iteration++ > max_iterations)
        done = true;
    else
        update_one_tree();

    w = new_w;
}

bool OneTree::has_edge(int from, int to) {
    return edges[from].find(to) != edges[from].end();
}

int OneTree::marginal_cost(int from, int to) {
    vector<bool> seen(edges.size(), false);
    seen[to] = true;
    return marginal_cost(from, to, seen, to, 0);
}

void OneTree::initialize_one_tree() {
    for (int i = 0; i < next.size(); ++i) {
        // Arcs are undirected for MST.
        if (i == start_index || i == end_index)
            continue;

        for (int j = i + 1; j < next.size(); ++j) {
            if (j == start_index || j == end_index)
                continue;

            if (!(next[i].in(j) || next[j].in(i)))
                continue;

            OneTreeEdge e;
            bool inserted;
            boost::tie(e, inserted) = boost::add_edge(i, j, graph);
            weights[e] = undirected_cost(i, j);
        }
    }
}

void OneTree::update_one_tree() {
    auto es = boost::edges(graph);
    for (auto eit = es.first; eit != es.second; ++eit) {
        auto e = *eit;
        weights[e] = transformed_cost(e.m_source, e.m_target);
    }
}

double OneTree::minimize_one_tree() {
    vector<OneTreeEdge> mst;
    boost::kruskal_minimum_spanning_tree(graph, back_inserter(mst));

    // Calculate cost of the MST and pull out edges.
    double z = 0;
    for (auto e : mst) {
        edges[e.m_source].insert(e.m_target);
        edges[e.m_target].insert(e.m_source);
        z += weights[e];
    }

    // Add cheapest arc connecting +0.
    int min_p0_idx = -1;
    double min_p0 = numeric_limits<double>::max();
    for (auto to = next[start_index].min(); to <= next[start_index].max(); ++to) {
        if (!next[start_index].in(to))
            continue;

        auto c = transformed_cost(start_index, to);
        if (c < min_p0) {
            min_p0_idx = to;
            min_p0 = c;
        }
    }
    edges[start_index].insert(min_p0_idx);
    edges[min_p0_idx].insert(start_index);

    // Add cheapest arc connecting -0.
    int min_d0_idx = -1;
    double min_d0 = numeric_limits<double>::max();
    for (int from = 0; from < next.size(); ++from) {
        if (!next[from].in(end_index))
            continue;

        auto c = transformed_cost(from, end_index);
        if (c < min_d0) {
            min_d0_idx = from;
            min_d0 = c;
        }
    }
    edges[end_index].insert(min_d0_idx);
    edges[min_d0_idx].insert(end_index);

    edges[start_index].insert(end_index);
    edges[end_index].insert(start_index);

    return z + min_p0 + min_d0;
}

int OneTree::undirected_cost(int i, int j) {
    auto c_ij = next[i].in(j) ? cost(i, j) : numeric_limits<int>::max();
    auto c_ji = next[j].in(i) ? cost(j, i) : numeric_limits<int>::max();
    return min(c_ij, c_ji);
}

double OneTree::transformed_cost(int i, int j) {
    return undirected_cost(i, j) + potentials[i] + potentials[j];
}

int OneTree::marginal_cost(
    int from,
    int to,
    vector<bool> seen,
    int node,
    int max_edge_cost) {

    // Introducing a nonbasic arc into the basis would create a cycle.
    // The marginal cost of this operation is the cost of the new arc
    // minus the max cost in the cycle. This can be found using DFS.
    for (auto next : edges[node]) {
        if ((node == start_index && next == end_index) || (node == end_index && next == start_index))
            continue;

        if (seen[next])
            continue;

        // If we loop back to the from node, then compute marginal cost.
        if (next == from && node != to)
            return cost(from, to) - max_edge_cost;

        vector<bool> new_seen(seen);
        new_seen[next] = true;

        int new_max = max(max_edge_cost, undirected_cost(node, next));
        auto cost = marginal_cost(from, to, new_seen, next, new_max);
        if (cost > -1)
            return cost;
    }

    return -1;
}

int OneTree::cost(int i, int j) {
    if (ap == nullptr)
        return problem.cost(i, j);
    return ap->get_rc({i, j});
}


/*Copyright (c) 2021 James Gayvert

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

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

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

#include "TrajectoryAnalysis.h"

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

#include "utils.h"

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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