#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);
}


#ifndef GRAPH_HH
#define GRAPH_HH

#include "NNLayerConfig.hh"
#include "Source.hh"

#include <Eigen/Dense>

#include <vector>
#include <unordered_map>
#include <set>

namespace lwt {

  template<typename T>
  class StackT;

  using Stack = StackT<double>;

  template<typename T>
  class RecurrentStackT;

  using RecurrentStack = RecurrentStackT<double>;

  // node class: will return a VectorXd from ISource
  class INode
  {
  public:
    virtual ~INode() {}
    virtual VectorXd compute(const ISource&) const = 0;
    virtual size_t n_outputs() const = 0;
  };

  class InputNode: public INode
  {
  public:
    InputNode(size_t index, size_t n_outputs);
    virtual VectorXd compute(const ISource&) const override;
    virtual size_t n_outputs() const override;
  private:
    size_t m_index;
    size_t m_n_outputs;
  };

  class FeedForwardNode: public INode
  {
  public:
    FeedForwardNode(const Stack*, const INode* source);
    virtual VectorXd compute(const ISource&) const override;
    virtual size_t n_outputs() const override;
  private:
    const Stack* m_stack;
    const INode* m_source;
  };

  class ConcatenateNode: public INode
  {
  public:
    ConcatenateNode(const std::vector<const INode*>&);
    virtual VectorXd compute(const ISource&) const override;
    virtual size_t n_outputs() const override;
  private:
    std::vector<const INode*> m_sources;
    size_t m_n_outputs;
  };

  // sequence nodes
  class ISequenceNode
  {
  public:
    virtual ~ISequenceNode() {}
    virtual MatrixXd scan(const ISource&) const = 0;
    virtual size_t n_outputs() const = 0;
  };

  class InputSequenceNode: public ISequenceNode
  {
  public:
    InputSequenceNode(size_t index, size_t n_outputs);
    virtual MatrixXd scan(const ISource&) const override;
    virtual size_t n_outputs() const override;
  private:
    size_t m_index;
    size_t m_n_outputs;
  };

  class SequenceNode: public ISequenceNode, public INode
  {
  public:
    SequenceNode(const RecurrentStack*, const ISequenceNode* source);
    virtual MatrixXd scan(const ISource&) const override;
    virtual VectorXd compute(const ISource&) const override;
    virtual size_t n_outputs() const override;
  private:
    const RecurrentStack* m_stack;
    const ISequenceNode* m_source;
  };

  class TimeDistributedNode: public ISequenceNode
  {
  public:
    TimeDistributedNode(const Stack*, const ISequenceNode* source);
    virtual MatrixXd scan(const ISource&) const override;
    virtual size_t n_outputs() const override;
  private:
    const Stack* m_stack;
    const ISequenceNode* m_source;
  };
  class SumNode: public INode
  {
  public:
    SumNode(const ISequenceNode* source);
    virtual VectorXd compute(const ISource&) const override;
    virtual size_t n_outputs() const override;
  private:
    const ISequenceNode* m_source;
  };

  // Graph class, owns the nodes
  class Graph
  {
  public:
    Graph();                    // dummy constructor
    Graph(const std::vector<NodeConfig>& nodes,
          const std::vector<LayerConfig>& layers);
    Graph(Graph&) = delete;
    Graph& operator=(Graph&) = delete;
    ~Graph();
    VectorXd compute(const ISource&, size_t node_number) const;
    VectorXd compute(const ISource&) const;
    MatrixXd scan(const ISource&, size_t node_number) const;
    MatrixXd scan(const ISource&) const;
  private:
    void build_node(const size_t,
                    const std::vector<NodeConfig>& nodes,
                    const std::vector<LayerConfig>& layers,
                    std::set<size_t> cycle_check = {});

    std::unordered_map<size_t, INode*> m_nodes;
    size_t m_last_node; // <-- convenience for graphs with one output
    std::unordered_map<size_t, Stack*> m_stacks;
    std::unordered_map<size_t, ISequenceNode*> m_seq_nodes;
    std::unordered_map<size_t, RecurrentStack*> m_seq_stacks;
    // At some point maybe also convolutional nodes, but we'd have to
    // have a use case for that first.
  };
}

#endif // GRAPH_HH


#include "../src/coinspend.h"
#include "../src/spend_metadata.h"

#include "../bitcoin/uint256.h"

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(sigma_coin_spend_tests)

BOOST_AUTO_TEST_CASE(different_anonymity_set)
{
    auto params = sigma::Params::get_default();

    const sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin;
    pubcoin = privcoin.getPublicCoin();

    std::vector<sigma::PublicCoin> anonymity_set;
    anonymity_set.push_back(pubcoin);

    // Doesn't really matter what metadata we give here, it must pass.
    sigma::SpendMetaData metaData(0, uint256S("120"), uint256S("120"));

    sigma::CoinSpend coin(params,privcoin,anonymity_set, metaData, true);
    sigma::CoinSpend coin2(params,privcoin,anonymity_set, metaData, true);

    BOOST_CHECK(coin.getCoinSerialNumber() == coin2.getCoinSerialNumber());

    // use different anonymity_set
    const sigma::PrivateCoin privcoin3(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin3;
    pubcoin3 = privcoin3.getPublicCoin();

    std::vector<sigma::PublicCoin> anonymity_set3;
    anonymity_set3.push_back(pubcoin);
    anonymity_set3.push_back(pubcoin3);

    sigma::CoinSpend coin3(params, privcoin, anonymity_set3, metaData, true);

    BOOST_CHECK(coin.getCoinSerialNumber() == coin3.getCoinSerialNumber());
}

BOOST_AUTO_TEST_CASE(out_of_anonymity_set)
{
    auto params = sigma::Params::get_default();

    const sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);

    // use different anonymity_set
    const sigma::PrivateCoin privcoin3(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin3;
    pubcoin3 = privcoin3.getPublicCoin();

    // [pubcoin3]
    std::vector<sigma::PublicCoin> anonymity_set3;
    anonymity_set3.push_back(pubcoin3);

    // Doesn't really matter what metadata we give here, it must pass.
    sigma::SpendMetaData metaData(0, uint256S("120"), uint256S("120"));

    // pubcoin of privcoin isn't in [pubcoin3]
    BOOST_CHECK_THROW(sigma::CoinSpend(params, privcoin, anonymity_set3, metaData, true),std::exception);
}

BOOST_AUTO_TEST_CASE(verify_test)
{
    auto params = sigma::Params::get_default();

    const sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin;
    pubcoin = privcoin.getPublicCoin();

    std::vector<sigma::PublicCoin> anonymity_set;
    anonymity_set.push_back(pubcoin);

    // Doesn't really matter what metadata we give here, it must pass.
    sigma::SpendMetaData metaData(0, uint256S("120"), uint256S("120"));

    sigma::CoinSpend spend_coin(params, privcoin, anonymity_set, metaData, true);

    BOOST_CHECK(spend_coin.Verify(anonymity_set, metaData, true));
}

BOOST_AUTO_TEST_CASE(verify_test_valid_set_plus_one)
{
    auto params = sigma::Params::get_default();

    const sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin;
    pubcoin = privcoin.getPublicCoin();

    const sigma::PrivateCoin privcoin2(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin2;
    pubcoin2 = privcoin2.getPublicCoin();

    // [pubcoin]
    std::vector<sigma::PublicCoin> anonymity_set;
    anonymity_set.push_back(pubcoin);

    // Doesn't really matter what metadata we give here, it must pass.
    sigma::SpendMetaData metaData(0, uint256S("120"), uint256S("120"));

    sigma::CoinSpend spend_coin(params,privcoin,anonymity_set, metaData, true);

    // [pubcoin,pubcoin2]
    std::vector<sigma::PublicCoin> anonymity_set2;
    anonymity_set2.push_back(pubcoin);
    anonymity_set2.push_back(pubcoin2);

    BOOST_CHECK(!spend_coin.Verify(anonymity_set2, metaData, true));
}

BOOST_AUTO_TEST_CASE(verify_test_valid_set_subtract_one)
{
    auto params = sigma::Params::get_default();

    const sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin;
    pubcoin = privcoin.getPublicCoin();

    const sigma::PrivateCoin privcoin2(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin2;
    pubcoin2 = privcoin2.getPublicCoin();

    // [pubcoin,pubcoin2]
    std::vector<sigma::PublicCoin> anonymity_set;
    anonymity_set.push_back(pubcoin);
    anonymity_set.push_back(pubcoin2);

    // Doesn't really matter what metadata we give here, it must pass.
    sigma::SpendMetaData metaData(0, uint256S("120"), uint256S("120"));

    sigma::CoinSpend spend_coin(params, privcoin, anonymity_set, metaData, true);

    // [pubcoin]
    std::vector<sigma::PublicCoin> anonymity_set2;
    anonymity_set2.push_back(pubcoin);

    BOOST_CHECK(!spend_coin.Verify(anonymity_set2, metaData, true));
}

BOOST_AUTO_TEST_CASE(verify_test_with_accumulatorBlockHash)
{
    auto params = sigma::Params::get_default();

    const sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PublicCoin pubcoin;
    pubcoin = privcoin.getPublicCoin();

    std::vector<sigma::PublicCoin> anonymity_set;
    anonymity_set.push_back(pubcoin);

    // Doesn't really matter what metadata we give here, it must pass.
    sigma::SpendMetaData metaData(0, uint256S("120"), uint256S("120"));
    sigma::CoinSpend spend_coin(params, privcoin, anonymity_set, metaData, true);

    BOOST_CHECK(spend_coin.Verify(anonymity_set, metaData, true));
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Seed.hpp
 *
 *  Created on: Aug 24, 2012
 *      Author: david
 */

#ifndef DASP_SEED_HPP_
#define DASP_SEED_HPP_

#include <Eigen/Dense>

namespace dasp
{

	/** A cluster seed */
	struct Seed
	{
		int x, y;
		float scala;
		int label;

		// if is_fixed is enabled the cluster 3d position is always set to fixed_world
		bool is_fixed;
		// only used if is_fixed equals true
		Eigen::Vector3f fixed_world;
		Eigen::Vector3f fixed_color;
		Eigen::Vector3f fixed_normal;

		static Seed Dynamic(int x, int y, float scala) {
			return Seed{x, y, scala, -1, false,
				Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
		}
		static Seed Static(int x, int y, float scala, const Eigen::Vector3f& position, const Eigen::Vector3f& color, const Eigen::Vector3f& normal) {
			return Seed{x, y, scala, -1, true,
				position, color, normal};
		}
	};

}

#endif


/*  TODO:
 * Autocreation of grabbing elements according to the given config in tha yaml file
 * Create files directly in the needed json format
 */

#include <ros/ros.h>
#include <string.h>
#include <vector>
#include <map>

#include <custom_parameter/parameterHandler.h>
#include <custom_parameter/parameter.h>

#include "dataelements/Grabcollection.h"
#include "dataelements/Camera.h"
#include "dataelements/FrameRGB.h"
#include "dataelements/FramePC.h"
#include "dataelements/FrameIR.h"
#include "dataelements/FrameDepth.h"

#include <gazebo_msgs/SetModelState.h>
#include <gazebo_msgs/GetModelState.h>
#include <tf/tf.h>
#include <Eigen/Dense>
#include <cmath>
#include <random>
#include <boost/filesystem.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <ropose_datagrabber/SaveCurrentState.h>
#include "../helper/TransformationHandler.h"

#include <vector>
#include <tf/LinearMath/Quaternion.h>

//common stuff
std::string nodeName = "datagrabber";

//ros sutff
ros::NodeHandle* _node;

//parameter stuff
customparameter::ParameterHandler* parameterHandler;
customparameter::Parameter<std::string> paramSavePath;
customparameter::Parameter<float> paramRefreshRate;
customparameter::Parameter<float> paramMinRadius;
customparameter::Parameter<float> paramMaxRadius;
customparameter::Parameter<float> paramMaxPolAngle;
customparameter::Parameter<float> paramMinPolAngle;
customparameter::Parameter<float> paramMaxAziAngle;
customparameter::Parameter<float> paramMinAziAngle;
customparameter::Parameter<float> paramLookAtRange;
customparameter::Parameter<float> paramLookAtYRange;
customparameter::Parameter<bool> paramAutoGrabbing;
customparameter::Parameter<bool> paramSwitchCamPoses;
customparameter::Parameter<bool> paramForceCamPose;
customparameter::Parameter<bool> paramAdjustForcedCamPose;
customparameter::Parameter<std::vector<float>> paramForcedPose;

customparameter::Parameter<XmlRpc::XmlRpcValue> paramCameras;
customparameter::Parameter<std::vector<std::string>> paramTransformNames;

//cam pose Stuff
gazebo_msgs::SetModelState msgCameraModelState;
ros::ServiceClient setModelStateClient;
geometry_msgs::Pose forcedCamPose;

//organize all elements to grab
RoposeGrabber::Grabcollection* grabcollection;

helper::TransformationHandler* transformHandler;

geometry_msgs::Pose ListToPose(std::vector<double> vector)
{
    //set Pose
    geometry_msgs::Pose newPose;

    newPose.position.x = vector[0];
    newPose.position.y = vector[1];
    newPose.position.z = vector[2];

    tf::Quaternion quat;
    quat.setRPY(vector[3], vector[4], vector[5]);

    newPose.orientation.x = quat.x();
    newPose.orientation.y = quat.y();
    newPose.orientation.z = quat.z();
    newPose.orientation.w = quat.w();

    return newPose;
}

double GetRandomValue(double min, double max)
{
    //https://stackoverflow.com/questions/5289613/generate-random-float-between-two-floats/5289624
    double random = ((double) rand()) / (double) RAND_MAX;
    double range = max - min;
    return (random*range) + min;
}

Eigen::Vector3d  GetRandomCamPosition()
{
    double r = GetRandomValue(paramMinRadius.GetValue(), paramMaxRadius.GetValue());
    double polAng = GetRandomValue(paramMinPolAngle.GetValue(), paramMaxPolAngle.GetValue());
    double aziAng = GetRandomValue(paramMinAziAngle.GetValue(), paramMaxAziAngle.GetValue());
    Eigen::Vector3d ret;

    double a = r *cos(aziAng);
    ret[0] = a * cos(polAng);
    ret[1] = r * sin(aziAng);
    ret[2] = a * sin(polAng);

    return ret;
}

Eigen::Vector3d  GetRandomLookAtPosition()
{
    double max = paramLookAtRange.GetValue();
    double max_2 = max/2;
    Eigen::Vector3d ret;
    ret[0] = GetRandomValue(0.0f, max) - max_2;
    ret[1] = GetRandomValue(0.0f, max) - max_2;
    ret[2] = GetRandomValue(0.0f, max) - max_2;

    return ret;
}

Eigen::Vector3d  GetRandomYVector()
{
    float range = paramLookAtYRange.GetValue();

    Eigen::Vector3d ret;
    if (range < 0.0)
    {
        ret[0] = GetRandomValue(0.0f, 1.0f);
        ret[1] = GetRandomValue(0.0f, 1.0f);
        ret[2] = GetRandomValue(0.0f, 1.0f);
    }
    else
    {
        double randRange = GetRandomValue(-range, range);
        if (randRange < 0.0)
        {
            randRange += M_PI;
        }

        double x = 0.0f;
        double y = 0.0f;
        double z = 0.0f;

        if (randRange >= 0.0 && randRange <= M_PI_2)
        {
            y = sin(randRange);
            z = -cos(randRange);
        }
        else if (randRange >= M_PI_2 && randRange <= M_PI)
        {
            //quadrant 2
            y = sin(randRange);
            z = cos(randRange);
        }
        else if (randRange >= M_PI && randRange < 1.5 * M_PI)
        {
            //quadrant 3
            y = -sin(randRange);
            z = cos(randRange);
        }
        else if (randRange >=  1.5 * M_PI && randRange < 2 * M_PI)
        {
            //quadrant 4
            y = -sin(randRange);
            z = -cos(randRange);
        }

        ret[0] = x;
        ret[1] = y;
        ret[2] = z;

    }

    ret.normalize();
    return ret;
}

Eigen::Quaterniond GetLookAtRotation(Eigen::Vector3d  from, Eigen::Vector3d  to)
{
    using namespace Eigen;
    Eigen::Quaterniond quat;
    quat.w() = 1.0;

    if ( from != to )
    {
        //calculate unit vectors of new coordinate system
        Vector3d xVec, yVec, zVec;
        xVec = to - from;
        xVec.normalize();

        yVec = xVec.cross(GetRandomYVector());
        yVec.normalize();

        zVec = xVec.cross(yVec);
        zVec.normalize();

        Eigen::Matrix3d mat;

        mat <<  xVec[0],  yVec[0],  zVec[0],
                xVec[1],  yVec[1],  zVec[1],
                xVec[2],  yVec[2],  zVec[2];

        Eigen::Quaterniond quatSet(mat);

        quatSet.normalize();
        return quatSet;
    }
    return quat;
}

void InitParams() {
    using namespace std;
    std::string subNamespace = "datagrabber";
    //Standard params
    std::string homePath = boost::filesystem::path(getenv("HOME")).string();

    paramSavePath = parameterHandler->AddParameter("SavePath", "", homePath + "/ropose/ropose_data");
    paramRefreshRate = parameterHandler->AddParameter("RefreshRate", "", (float) 3);
    paramMinRadius = parameterHandler->AddParameter("SimCamMinRadius", "", 2.0f);
    paramMaxRadius = parameterHandler->AddParameter("SimCamMaxRadius", "", 10.0f);
    paramMaxPolAngle = parameterHandler->AddParameter("SimCamMaxPolarAng", "", float(M_PI));
    paramMinPolAngle = parameterHandler->AddParameter("SimCamMinPolarAng", "", float(M_PI/10));
    paramMaxAziAngle = parameterHandler->AddParameter("SimCamMaxAzimuthalAng", "", 0.0f);
    paramMinAziAngle = parameterHandler->AddParameter("SimCamMinAzimuthalAng", "", float(2*M_PI));
    paramLookAtRange = parameterHandler->AddParameter("SimCamLookAtRange", "", 2.0f);
    paramLookAtYRange = parameterHandler->AddParameter("SimCamLookAtYRange", "", -1.0f);
    paramAutoGrabbing = parameterHandler->AddParameter("AutoGrabbing", "", true);
    paramSwitchCamPoses = parameterHandler->AddParameter("SwitchCamPoses", "", false);
    paramForceCamPose = parameterHandler->AddParameter("ForceCamPose", "", false);
    paramAdjustForcedCamPose = parameterHandler->AddParameter("AdjustForcedCamPose", "", false);
    paramForcedPose = parameterHandler->AddParameter("ForcedPose", "",
                                                     vector<float>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}));
    //parameters to generate the grabbing units
    paramCameras = parameterHandler->AddParameter("Cameras", "", XmlRpc::XmlRpcValue());
    paramTransformNames = parameterHandler->AddParameter("TransformNames", "", vector<string>());

}

geometry_msgs::Pose GetRandomCamPose()
{
    geometry_msgs::Pose ret;

    Eigen::Vector3d  camPosition = GetRandomCamPosition();

    ret.position.x = camPosition[0];
    ret.position.y = camPosition[1];
    ret.position.z = camPosition[2];

    Eigen::Quaterniond  quat = GetLookAtRotation(camPosition, GetRandomLookAtPosition()).normalized();

    ret.orientation.x = quat.x();
    ret.orientation.y = quat.y();
    ret.orientation.z = quat.z();
    ret.orientation.w = quat.w();

    return ret;
}

void InitServices()
{
    ros::ServiceClient getModelStateClient = _node->serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
    setModelStateClient = _node->serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");

    gazebo_msgs::GetModelState getMsg;
    getMsg.request.model_name ="depthcam1";
    getModelStateClient.call(getMsg);
    msgCameraModelState.request.model_state.pose = getMsg.response.pose;
    msgCameraModelState.request.model_state.model_name = "depthcam1";
}

void ToCvCam(geometry_msgs::Pose* pose)
{
    //Rotate pose to fit with the model used by openCV
    tf2::Quaternion origin(pose->orientation.x, pose->orientation.y, pose->orientation.z, pose->orientation.w);

    tf2::Quaternion rot1;
    rot1.setRPY( 0, 0, -M_PI_2);
    tf2::Quaternion rot2;
    rot2.setRPY( -M_PI_2, 0, 0 );

    origin *= rot1;
    origin *= rot2;

    pose->orientation.x = origin.x();
    pose->orientation.y = origin.y();
    pose->orientation.z = origin.z();
    pose->orientation.w = origin.w();
}

void FromCVCamPose(geometry_msgs::Pose* pose)
{
    //Rotate pose to fit with the model used by openCV
    tf2::Quaternion origin(pose->orientation.x, pose->orientation.y, pose->orientation.z, pose->orientation.w);

    tf2::Quaternion rot1;
    rot1.setRPY( 0, 0, M_PI_2);
    tf2::Quaternion rot2;
    rot2.setRPY( M_PI_2, 0, 0 );

    origin *= rot2;
    origin *= rot1;

    pose->orientation.x = origin.x();
    pose->orientation.y = origin.y();
    pose->orientation.z = origin.z();
    pose->orientation.w = origin.w();
}

void SwitchCamPose()
{
    geometry_msgs::Pose pose = GetRandomCamPose();

    msgCameraModelState.request.model_state.pose = pose;
    setModelStateClient.call(msgCameraModelState);

    ToCvCam(&pose);

    transformHandler->SendTransform(pose, "world", "depthcam1_link");
}

void AdjustForcedPose()
{
    Eigen::Vector3d  camPosition;

    camPosition[0] = forcedCamPose.position.x;
    camPosition[1] = forcedCamPose.position.y;
    camPosition[2] = forcedCamPose.position.z;

    Eigen::Quaterniond  quat = GetLookAtRotation(camPosition, GetRandomLookAtPosition()).normalized();

    forcedCamPose.orientation.x = quat.x();
    forcedCamPose.orientation.y = quat.y();
    forcedCamPose.orientation.z = quat.z();
    forcedCamPose.orientation.w = quat.w();

    msgCameraModelState.request.model_state.pose = forcedCamPose;
    setModelStateClient.call(msgCameraModelState);

    ToCvCam(&forcedCamPose);

    transformHandler->SendTransform(forcedCamPose, "world", "depthcam1_link");
}

void SetForcedCamPose()
{
    std::vector<float> forcedPose = paramForcedPose.GetValue();

    forcedCamPose.position.x = forcedPose[0];
    forcedCamPose.position.y = forcedPose[1];
    forcedCamPose.position.z = forcedPose[2];

    forcedCamPose.orientation.x = forcedPose[3];
    forcedCamPose.orientation.y = forcedPose[4];
    forcedCamPose.orientation.z = forcedPose[5];
    forcedCamPose.orientation.w = forcedPose[6];

    FromCVCamPose(&forcedCamPose);

    msgCameraModelState.request.model_state.pose = forcedCamPose;
    setModelStateClient.call(msgCameraModelState);

    ToCvCam(&forcedCamPose);

    transformHandler->SendTransform(forcedCamPose, "world", "depthcam1_link");
}

void CheckPath(std::string savePath)
{
    namespace fs = boost::filesystem;

    if (fs::exists(savePath))
    {
        //rename dir with timestamp
        boost::posix_time::ptime time = boost::posix_time::second_clock::local_time();
        using namespace std;
        std::string timestamp =  to_string(time.date().day()) + to_string(time.date().month()) +
                to_string(time.date().year()) + "_" + to_string(time.time_of_day().hours()) +
                to_string(time.time_of_day().minutes()) + to_string(time.time_of_day().seconds());

        fs::rename(savePath, savePath + "_" + timestamp);
    }
    else
    {
        fs::create_directories(savePath);
    }
}

sensor_msgs::CameraInfo* tempInfoObject = nullptr;

void CameraInfoCallback(const sensor_msgs::CameraInfo &msg)
{
    tempInfoObject = new sensor_msgs::CameraInfo(msg);
}

void InitGrabbElements()
{
    using namespace RoposeGrabber;
    using namespace std;
    using namespace XmlRpc;

    try
    {
        string savePath = paramSavePath.GetValue();
        CheckPath(savePath);
        grabcollection = new Grabcollection(savePath);

        //add robopose with needed tf-frames
        vector<string> transformNames = paramTransformNames.GetValue();

        //extract all the cameras
        XmlRpcValue cameras = paramCameras.GetValue();

        for (int i = 0; i < cameras.size(); i++) {
            XmlRpcValue camera = cameras[i];

            string name = camera["name"];
            string linkName = camera["linkName"];

            XmlRpcValue frames = camera["frames"];
            XmlRpcValue frameTopics = camera["frameTopics"];
            XmlRpcValue frameLinks = camera["frameLinks"];
            XmlRpcValue isOpticalFrame = camera["isOpticalFrame"];
            XmlRpcValue camInfoTopics = camera["camInfoTopics"];

            //create camera instances
            string camFullName = "/" + name + "/";
            string cameraSavePath = savePath + camFullName;

            Camera* cam = new Camera(camFullName, cameraSavePath, name);

            //counter for proper folder naming
            int rgbCounter = 0;
            int pcCounter = 0;
            int irCounter = 0;
            int depthCounter = 0;

            //generate frames
            for(int frameIndex = 0; frameIndex < frames.size(); frameIndex++)
            {
                string frameType = frames[frameIndex];
                string frameTopic = frameTopics[frameIndex];
                string camInfoTopic = camInfoTopics[frameIndex];
                string baseDirName = name + "/" + frameType + to_string(irCounter);

                auto subscriber = _node->subscribe(camInfoTopic, 1000, CameraInfoCallback);

                //extract camera info to initialize a tf loger to generate project the 2D points
                while(!tempInfoObject)
                {
                    ROS_INFO_STREAM("Waiting for camera info of: " + subscriber.getTopic());
                    sleep(1);
                }

                //array to projection matrix
                Eigen::MatrixXd newProjectionMatrix(3, 3);
                newProjectionMatrix.row(0) << tempInfoObject->K[0],  tempInfoObject->K[1], tempInfoObject->K[2];
                newProjectionMatrix.row(1) << tempInfoObject->K[3],  tempInfoObject->K[4], tempInfoObject->K[5];
                newProjectionMatrix.row(2) << tempInfoObject->K[6],  tempInfoObject->K[7], tempInfoObject->K[8];

                delete tempInfoObject;

                std::string frameLink = frameLinks[frameIndex];
                if (frameLink == "")
                {
                    std::string frameLink = linkName;
                }

                grabcollection->AddPointProjector(baseDirName + "/transforms_2D",
                                                  transformNames, frameLink, newProjectionMatrix,
                                                  isOpticalFrame[frameIndex]);

                if(frameType == "rgb" || frameType == "RGB" || frameType == "COLOR")
                {
                    cam->AddFrame(new FrameRGB(frameTopic, camInfoTopic, cameraSavePath + "rgb" + to_string(rgbCounter)));
                    rgbCounter++;
                }
                else if (frameType == "pointcloud" || frameType == "pc" || frameType == "PC")
                {
                    //cam->AddFrame(new FramePC(frameTopic, cameraSavePath + "pc" + to_string(pcCounter)));
                    pcCounter++;
                }
                else if (frameType == "infrared" || frameType == "ir" || frameType == "IR")
                {
                    cam->AddFrame(new FrameIR(frameTopic, camInfoTopic, cameraSavePath + "ir" + to_string(irCounter)));
                    irCounter++;
                }
                else if (frameType == "depth" || frameType == "DEPTH")
                {
                    cam->AddFrame(new FrameDepth(frameTopic, camInfoTopic,
                                                 cameraSavePath + "depth" + to_string(depthCounter)));
                    depthCounter++;
                }
            }

            grabcollection->AddCamera(cam);

            //also add camera transforms we need to reconstruct the poses
            transformNames.push_back(linkName);
            grabcollection->AddTFLogger(name + "/transforms", transformNames, linkName);
        }

        grabcollection->AddTFLogger("world_transforms", transformNames);

        //init all topics of each element
        grabcollection->InitTopics(_node);
        sleep(2);
    }
    catch(std::exception &e)
    {
        ROS_ERROR_STREAM("Cant parse grabbing elements for " + nodeName + " are you sure all parameters are set correct?");
    }
}

bool SaveCurrentState(ropose_datagrabber::SaveCurrentState::Request &req, ropose_datagrabber::SaveCurrentState::Response &res)
{
    grabcollection->SaveCurrentState();

    if (paramSwitchCamPoses.GetValue())
        SwitchCamPose();

    res.Success = true;
}

void RunAutoNode()
{
    ros::Rate rate(paramRefreshRate.GetValue());

    bool switchCamPose = paramSwitchCamPoses.GetValue();
    bool forcedCamPose = paramForceCamPose.GetValue();
    bool adjustForcedCamPose = paramAdjustForcedCamPose.GetValue();

    if (switchCamPose && !forcedCamPose)
    {
        SwitchCamPose();
        rate.sleep();
    }

    if (forcedCamPose)
    {
        SetForcedCamPose();
        rate.sleep();
    }

    while(_node->ok())
    {
        ros::spinOnce();
        grabcollection->SaveCurrentState();

        if (forcedCamPose)
        {
            if (adjustForcedCamPose)
            {
                AdjustForcedPose();
            }
        }
        else
        {
            if (switchCamPose)
                SwitchCamPose();
        }

        //trigger the ros callbacks and wait the needed time
        rate.sleep();
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, nodeName);
    _node = new ros::NodeHandle(nodeName);

    //init params
    parameterHandler = new customparameter::ParameterHandler(_node);
    transformHandler = new helper::TransformationHandler(_node, 200);
    InitParams();
    InitServices();
    InitGrabbElements();

    //init services
    ros::ServiceServer service = _node->advertiseService("SaveCurrentState", SaveCurrentState);

    if(paramSwitchCamPoses.GetValue() && !paramForceCamPose.GetValue())
    {
        ROS_WARN_STREAM("You have switch campose activated! Be sure you are running a simulation. If not this will"
                                "manipulate the base transformation for your camera and your dataset will be useless!");
    }

    if(paramForceCamPose.GetValue())
    {
        ROS_WARN_STREAM("You forced the simulated cam pose!");
    }

    bool autoGrabing = paramAutoGrabbing.GetValue();
    if(autoGrabing)
    {
        RunAutoNode();
    }
    else
    {
        ros::Rate rate(paramRefreshRate.GetValue());
        while(_node->ok())
        {
            //trigger the ros callbacks and wait the needed time
            ros::spinOnce();
            rate.sleep();
        }

        sleep(5);
    }

    return 0;
}


#ifndef FSTINDEXEDVECTOR_H
#define FSTINDEXEDVECTOR_H

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

   \file    indexed_vector.hpp
   \brief   Vector representation and operations, allows operation in selected subspace
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    October 2010
   \version 3.0.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

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

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <limits>
#include <cmath>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "indexed_matrix.hpp"

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

namespace FST {

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
class Indexed_Matrix;

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
class Indexed_Vector;

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
std::ostream& operator<<(std::ostream& os, const Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>& tdb);

//! Vector representation and operations, allows operation in selected subspace
template<typename DATATYPE, typename DIMTYPE, class SUBSET>
class Indexed_Vector { //!< helper structure for passing matrix op intermediate data between higher-level matrix objects and functions
public:
	typedef boost::shared_ptr<SUBSET> PSubset;
	Indexed_Vector() : _n_max(0), _n(0), _d(0) {notify("Indexed_Vector (empty) constructor");}
	Indexed_Vector(const DIMTYPE n)  : _n_max(0), _n(0), _d(0) {notify("Indexed_Vector constructor"); reset(n);}
	Indexed_Vector(const Indexed_Vector& iv); // copy-constructor
	Indexed_Vector& operator=(Indexed_Vector const &iv);
	~Indexed_Vector() {notify("Indexed_Vector destructor");}
	void reset(const DIMTYPE n_max); // reallocates
	void redim(const DIMTYPE n=0); // keeps main allocations, just resets index arrays and adjusts _n; n==0 here equals _n_max

	DATATYPE& operator[](const DIMTYPE i) {assert(i>=0 && i<_n); assert(_data); return _data[i];} // raw access
	DATATYPE& at_raw(const DIMTYPE i) {assert(i>=0 && i<_n); assert(_data); return _data[i];} // raw access
	DATATYPE& at(const DIMTYPE i) {assert(i>=0 && i<_d); assert(_index); return _data[_index[i]];} // soft (indexed) access
	DATATYPE* get_raw() {return _data.get();} //&(_data[0])

	void narrow_to(const PSubset sub); // fill _index[] -> access to data is then mapped to sub-vector
	void denarrow(); // reset _index[] -> access is reset to the full vector
	DIMTYPE get_n() const {return _n;}
	DIMTYPE get_d() const {return _d;}

	// "raw" operations, ignoring index[]
	void set_all_raw_to(const DATATYPE val);
	void subtract_raw(const Indexed_Vector& vr);
	void copy_raw(const Indexed_Vector& vr);

	friend class Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>;
	friend std::ostream& operator<< <>(std::ostream& os, const Indexed_Vector& tdb);

protected:
	// NOTE: vector size is defined by:
	//       _n_max - hard constant representing maximum allocated space of size _n_max
	//       _n     - "full feature set" dimensionality, _n<=_n_max
	//       _d     - "virtual subset" dimensionality, _d<=_n
	//       (this logic replicates that in Indexed_Matrix, where it is more important)
	DIMTYPE _n_max; // allocated space can hold _n_max*_n_max matrix
	DIMTYPE _n; // full feature set dimensionality
	boost::scoped_array<DATATYPE> _data; // raw vector data
	// sub-vector access tools
	DIMTYPE _d; // virtual sub-vector dimensionality
	boost::scoped_array<DIMTYPE> _index; // maps virtual sub-vector _d dimensions to original _n dimensions
};

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::Indexed_Vector(const Indexed_Vector& iv) : // copy-constructor
	_n_max(iv._n_max),
	_n(iv._n),
	_d(iv._d)
{
	notify("Indexed_Vector copy-constructor.");
	if(_n_max>0)
	{
		_data.reset(new DATATYPE[_n_max]);	memcpy((void *)_data.get(),(void *)(iv._data).get(),sizeof(DATATYPE)*_n_max);
		_index.reset(new DIMTYPE[_n_max]); memcpy((void *)_index.get(),(void *)(iv._index).get(),sizeof(DIMTYPE)*_n_max); // selected features (subspace) - index into _data
	}
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>& Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::operator=(Indexed_Vector const &iv)
{
	notify("Indexed_Vector operator=.");
	if (this != &iv)
	{
		_n=iv._n;
		_d=iv._d;
		if(_n_max!=iv._n_max)
		{
			_n_max=iv._n_max;
			if(_n_max>0)
			{
				_data.reset(new DATATYPE[_n_max]);
				_index.reset(new DIMTYPE[_n_max]);
			} else {
				assert(!iv._data);
				assert(!iv._index);
				_data.reset();
				_index.reset();
			}
		}
		if(_n_max>0)
		{
			memcpy((void *)_data.get(),(void *)(iv._data).get(),sizeof(DATATYPE)*_n_max);
			memcpy((void *)_index.get(),(void *)(iv._index).get(),sizeof(DIMTYPE)*_n_max); // selected features (subspace) - index into _data
		}
	}
	return *this;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::reset(const DIMTYPE n_max)
{
	notify("Indexed_Vector reset.");
	assert(n_max>0);
	if(n_max!=_n_max) {
		_n=_d=_n_max=n_max;
		_data.reset(new DATATYPE[_n_max]);
		_index.reset(new DIMTYPE[n_max]); // selected features (subspace) - index into _data
	}
	for(DIMTYPE i=0;i<_n_max;i++) _index[i]=i;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::redim(const DIMTYPE n)
{
	notify("Indexed_Vector redim.");
	assert(n>=0 && n<=_n_max);
	assert(_data);
	assert(_index);
	if(n==0) _d=_n=_n_max; else _d=_n=n;
	for(DIMTYPE i=0;i<_n;i++) _index[i]=i;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::narrow_to(const PSubset sub) // fill _index[] -> access to data is then mapped to sub-matrix
{
	assert(_n>=sub->get_d_raw()); //enough space to store subset ?
	assert(_index);
	DIMTYPE f;
	_d=0;
	for(bool b=sub->getFirstFeature(f);b!=false;b=sub->getNextFeature(f)) _index[_d++]=f;
	assert(_d>0);
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::denarrow() // reset _index[] -> access is reset to the full matrix
{
	assert(_index);
	for(DIMTYPE i=0;i<_n;i++) _index[i]=i;
	_d=_n;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::set_all_raw_to(const DATATYPE val)
{
	assert(_data);
	for(DIMTYPE i=0;i<_n;i++) _data[i]=val;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::subtract_raw(const Indexed_Vector& mr)
{
	assert(_data);
	assert(mr._data);
	assert(mr.get_n()==_n);
	for(DIMTYPE i=0;i<_n;i++) _data[i]-=mr._data[i];
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>::copy_raw(const Indexed_Vector& mr)
{
	assert(_data);
	assert(mr._data);
	assert(mr.get_n()==_n);
	for(DIMTYPE i=0;i<_n;i++) _data[i]=mr._data[i];
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
std::ostream& operator<<(std::ostream& os, const Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>& tdb)
{
	assert(tdb._n>0);
	assert(tdb._d>0);
	assert(tdb._data);
	assert(tdb._index);
	os << "----------------"<<endl;
	os << "Indexed_Vector::" << endl;
	os << "_index: "; for(DIMTYPE j=0;j<tdb._d;j++) {os << tdb._index[j] << " ";} os<<endl;
	os << "_data:  "; for(DIMTYPE j=0;j<tdb._d;j++) {os << tdb._data[j] << " ";} os << endl;
	os << "----------------"<<endl;
	return os;
}

} // namespace
#endif // FSTINDEXEDVECTOR_H ///:~


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/monad/bind_mcd.hpp>
#include <boost/hana/monad/flatten_mcd.hpp>

#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/monad.hpp>

#include <cassert>
using namespace boost::hana;

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);
struct invalid { };

template <template <typename ...> class mcd>
void test() {
    constexpr auto monad = detail::minimal::monad<mcd>;
    using M = detail::minimal::Monad<mcd>;

    bool executed = false;
    auto m = bind(monad(x<0>), tap<M>([&](auto) { executed = true; }));
    assert(m == monad(x<0>));
    assert(executed);
}

int main() {
    test<Monad::flatten_mcd>();
    test<Monad::bind_mcd>();
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file VanillaGaussNewtonSolver.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_VANILLA_GAUSS_NEWTON_SOLVER_HPP
#define STEAM_VANILLA_GAUSS_NEWTON_SOLVER_HPP

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>

#include <steam/solver/GaussNewtonSolverBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief A vanilla Gauss-Newton solver (no line search or trust region method)
//////////////////////////////////////////////////////////////////////////////////////////////
class VanillaGaussNewtonSolver : public GaussNewtonSolverBase
{
public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Parameter type is the basic
  //////////////////////////////////////////////////////////////////////////////////////////////
  typedef SolverBase::Params Params;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  VanillaGaussNewtonSolver(OptimizationProblem* problem, const Params& params = Params());

private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Build the system, solve for a step size and direction, and update the state
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual bool linearizeSolveAndUpdate(double* newCost, double* gradNorm);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Casts parameters to base type (for SolverBase class)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual const SolverBase::Params& getSolverBaseParams() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Parameters
  //////////////////////////////////////////////////////////////////////////////////////////////
  Params params_;

};

} // steam

#endif // STEAM_VANILLA_GAUSS_NEWTON_SOLVER_HPP


//
// test/codec/rfc4648.cpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#include "benchmark/benchmark.h"

#include <boost/radix/codec/rfc4648/base16.hpp>
#include <boost/radix/codec/rfc4648/base32.hpp>
#include <boost/radix/codec/rfc4648/base32hex.hpp>
#include <boost/radix/codec/rfc4648/base64.hpp>
#include <boost/radix/codec/rfc4648/base64url.hpp>
#include <boost/radix/decode.hpp>
#include <boost/radix/encode.hpp>
#include <boost/radix/encode_iterator.hpp>

#include "../test/common.hpp"

#include <boost/radix/static_ibitstream_lsb.hpp>
#include <boost/radix/static_obitstream_lsb.hpp>

template <typename Iterator>
auto unwrap_iterator(Iterator i) {
#if _HAS_ITERATOR_DEBUGGING
  return i;
#else
  return boost::addressof(*i);
#endif
}

struct base64_lsb : boost::radix::codec::rfc4648::base64 {};

boost::radix::static_obitstream_lsb<
    boost::radix::codec_traits::required_bits<base64_lsb>::value>
get_segment_packer(base64_lsb const&) {
  return boost::radix::static_obitstream_lsb<
      boost::radix::codec_traits::required_bits<base64_lsb>::value>();
}

boost::radix::static_ibitstream_lsb<
    boost::radix::codec_traits::required_bits<base64_lsb>::value>
get_segment_unpacker(base64_lsb const&) {
  return boost::radix::static_ibitstream_lsb<
      boost::radix::codec_traits::required_bits<base64_lsb>::value>();
}

// static void Base64_Encode_BackInserter(benchmark::State& state) {
//  boost::radix::codec::rfc4648::base64 codec;
//
//  std::vector<bits_type> data = generate_random_bytes(state.range(0));
//
//  for(auto _ : state) {
//    std::string result;
//    boost::radix::encode(
//        data.begin(), data.end(), std::back_inserter(result), codec);
//  }
//
//  state.SetBytesProcessed(
//      int64_t(state.iterations()) * int64_t(state.range(0)));
//}
// BENCHMARK(Base64_Encode_BackInserter)
//    ->Arg(128)
//    ->Arg(1024)
//    ->Arg(8 * 1024)
//    ->Arg(64 * 1024)
//    ->Arg(1024 * 1024);
//
// static void Base64_Decode_BackInserter(benchmark::State& state) {
//  boost::radix::codec::rfc4648::base64 codec;
//
//  std::vector<bits_type> data = generate_random_bytes(state.range(0));
//
//  std::string encoded;
//  boost::radix::encode(
//      data.begin(), data.end(), std::back_inserter(encoded), codec);
//
//  for(auto _ : state) {
//    std::vector<bits_type> result;
//    boost::radix::decode(
//        encoded.begin(), encoded.end(), std::back_inserter(result), codec);
//  }
//
//  state.SetBytesProcessed(
//      int64_t(state.iterations()) * int64_t(state.range(0)));
//}
// BENCHMARK(Base64_Decode_BackInserter)
//    ->Arg(128)
//    ->Arg(1024)
//    ->Arg(8 * 1024)
//    ->Arg(64 * 1024)
//    ->Arg(1024 * 1024);

#if !RADIXBENCH_DECODE_NOVALIDATION
static void Base64_Encode_OutputDirect(benchmark::State& state) {
  boost::radix::codec::rfc4648::base64 codec;
  std::vector<bits_type> data = generate_random_bytes(state.range(0));
  std::vector<char_type> result;
  result.resize(encoded_size(data.size(), codec));
  for(auto _ : state) {
    boost::radix::encode(
        data.data(), data.data() + data.size(), result.data(), codec);
    benchmark::DoNotOptimize(result);
  }

  state.SetBytesProcessed(
      int64_t(state.iterations()) * int64_t(state.range(0)));
}
BENCHMARK(Base64_Encode_OutputDirect)
    ->Arg(128)
    ->Arg(1024)
    ->Arg(8 * 1024)
    ->Arg(64 * 1024)
    ->Arg(1024 * 1024);

static void Base64_Lsb_Encode_OutputDirect(benchmark::State& state) {
  base64_lsb codec;
  std::vector<bits_type> data = generate_random_bytes(state.range(0));
  std::string result;
  result.resize(encoded_size(data.size(), codec));
  for(auto _ : state) {
    boost::radix::encode(
        unwrap_iterator(data.begin()), unwrap_iterator(data.end()),
        unwrap_iterator(result.begin()), codec);
    benchmark::DoNotOptimize(result);
  }

  state.SetBytesProcessed(
      int64_t(state.iterations()) * int64_t(state.range(0)));
}
BENCHMARK(Base64_Lsb_Encode_OutputDirect)
    ->Arg(128)
    ->Arg(1024)
    ->Arg(8 * 1024)
    ->Arg(64 * 1024)
    ->Arg(1024 * 1024);

static void Base64_Encoder_OutputDirect(benchmark::State& state) {
  boost::radix::codec::rfc4648::base64 codec;
  std::vector<bits_type> data = generate_random_bytes(state.range(0));
  std::string result;
  result.resize(encoded_size(data.size(), codec));
  for(auto _ : state) {
    auto encoder = boost::radix::make_encoder(codec, result.begin());
    encoder.append(unwrap_iterator(data.begin()), unwrap_iterator(data.end()));
    encoder.resolve();
    benchmark::DoNotOptimize(result);
  }

  state.SetBytesProcessed(
      int64_t(state.iterations()) * int64_t(state.range(0)));
}
BENCHMARK(Base64_Encoder_OutputDirect)
    ->Arg(128)
    ->Arg(1024)
    ->Arg(8 * 1024)
    ->Arg(64 * 1024)
    ->Arg(1024 * 1024);

static void Base64_EncodeIterator_OutputDirect(benchmark::State& state) {
  boost::radix::codec::rfc4648::base64 codec;
  std::vector<bits_type> data = generate_random_bytes(state.range(0));
  std::string result;
  result.resize(encoded_size(data.size(), codec));
  for(auto _ : state) {
    std::copy(
        data.begin(), data.end(),
        boost::radix::make_encode_iterator(
            codec, unwrap_iterator(result.begin())));
    benchmark::DoNotOptimize(result);
  }

  state.SetBytesProcessed(
      int64_t(state.iterations()) * int64_t(state.range(0)));
}
BENCHMARK(Base64_EncodeIterator_OutputDirect)
    ->Arg(128)
    ->Arg(1024)
    ->Arg(8 * 1024)
    ->Arg(64 * 1024)
    ->Arg(1024 * 1024);

#endif // RADIXBENCH_DECODE_NOVALIDATION

#if RADIXBENCH_DECODE_NOVALIDATION
namespace boost { namespace radix { namespace codec { namespace rfc4648 {

template <typename ErrorHandler>
decode_validation::op validate_character(
    base64 const& codec,
    char_type c,
    ErrorHandler&) {
  return decode_validation::op_consume;
}

}}}}

template <typename ErrorHandler>
boost::radix::decode_validation::op validate_character(
    base64_lsb const& codec,
    char_type c,
    ErrorHandler&) {
  return boost::radix::decode_validation::op_consume;
}

#endif

static void Base64_Decode_OutputDirect(benchmark::State& state) {
  boost::radix::codec::rfc4648::base64 codec;
  std::vector<bits_type> data = generate_random_bytes(state.range(0));
  std::string encoded;
  boost::radix::encode(
      data.begin(), data.end(), std::back_inserter(encoded), codec);
  std::vector<bits_type> result(data.size());
  for(auto _ : state) {
    boost::radix::decode(
        unwrap_iterator(encoded.begin()), unwrap_iterator(encoded.end()),
        unwrap_iterator(result.begin()), codec);
    benchmark::DoNotOptimize(result);
  }

  state.SetBytesProcessed(
      int64_t(state.iterations()) * int64_t(state.range(0)));
}
BENCHMARK(Base64_Decode_OutputDirect)
    ->Arg(128)
    ->Arg(1024)
    ->Arg(8 * 1024)
    ->Arg(64 * 1024)
    ->Arg(1024 * 1024);

static void Base64_Lsb_Decode_OutputDirect(benchmark::State& state) {
  base64_lsb codec;
  std::vector<bits_type> data = generate_random_bytes(state.range(0));
  std::string encoded;
  boost::radix::encode(
      data.begin(), data.end(), std::back_inserter(encoded), codec);
  std::vector<bits_type> result(data.size());
  for(auto _ : state) {
    boost::radix::decode(
        unwrap_iterator(encoded.begin()), unwrap_iterator(encoded.end()),
        unwrap_iterator(result.begin()), codec);
    benchmark::DoNotOptimize(result);
  }

  state.SetBytesProcessed(
      int64_t(state.iterations()) * int64_t(state.range(0)));
}
BENCHMARK(Base64_Lsb_Decode_OutputDirect)
    ->Arg(128)
    ->Arg(1024)
    ->Arg(8 * 1024)
    ->Arg(64 * 1024)
    ->Arg(1024 * 1024);

static void Base64_Decoder_OutputDirect(benchmark::State& state) {
  boost::radix::codec::rfc4648::base64 codec;
  std::vector<bits_type> data = generate_random_bytes(state.range(0));
  std::string encoded;
  boost::radix::encode(
      data.begin(), data.end(), std::back_inserter(encoded), codec);
  std::vector<bits_type> result(data.size());
  for(auto _ : state) {
    auto decoder = boost::radix::make_decoder(codec, result.data());
    decoder.append(
        unwrap_iterator(encoded.begin()), unwrap_iterator(encoded.end()));
    benchmark::DoNotOptimize(result);
  }

  state.SetBytesProcessed(
      int64_t(state.iterations()) * int64_t(state.range(0)));
}
BENCHMARK(Base64_Decoder_OutputDirect)
    ->Arg(128)
    ->Arg(1024)
    ->Arg(8 * 1024)
    ->Arg(64 * 1024)
    ->Arg(1024 * 1024);

BENCHMARK_MAIN();


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/type_traits.hpp>

TEST(MetaTraits, containsNonconstantStruct) {
  using stan::contains_nonconstant_struct;
  EXPECT_FALSE(contains_nonconstant_struct<int>::value);
  EXPECT_FALSE(contains_nonconstant_struct<double>::value);
  EXPECT_FALSE(contains_nonconstant_struct<float>::value);
  EXPECT_FALSE(contains_nonconstant_struct<long>::value);
}


// SPDX-License-Identifier: BSD-2-Clause

#include <hdl_graph_slam/graph_slam.hpp>

#include <boost/format.hpp>
#include <g2o/stuff/macros.h>
#include <g2o/core/factory.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/linear_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/slam3d/edge_se3_pointxyz.h>
#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
#include <g2o/edge_se3_plane.hpp>
#include <g2o/edge_se3_priorxy.hpp>
#include <g2o/edge_se3_priorxyz.hpp>
#include <g2o/edge_se3_priorvec.hpp>
#include <g2o/edge_se3_priorquat.hpp>
#include <g2o/edge_plane_prior.hpp>
#include <g2o/edge_plane_identity.hpp>
#include <g2o/edge_plane_parallel.hpp>
#include <g2o/robust_kernel_io.hpp>

#include <g2o/types/slam2d/types_slam2d.h>

G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)  // be aware of that cholmod brings GPL dependency
G2O_USE_OPTIMIZATION_LIBRARY(csparse)  // be aware of that csparse brings LGPL unless it is dynamically linked

namespace g2o {
G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane)
G2O_REGISTER_TYPE(EDGE_SE3_PRIORXY, EdgeSE3PriorXY)
G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ)
G2O_REGISTER_TYPE(EDGE_SE3_PRIORVEC, EdgeSE3PriorVec)
G2O_REGISTER_TYPE(EDGE_SE3_PRIORQUAT, EdgeSE3PriorQuat)
G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_NORMAL, EdgePlanePriorNormal)
G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_DISTANCE, EdgePlanePriorDistance)
G2O_REGISTER_TYPE(EDGE_PLANE_IDENTITY, EdgePlaneIdentity)
G2O_REGISTER_TYPE(EDGE_PLANE_PARALLEL, EdgePlaneParallel)
G2O_REGISTER_TYPE(EDGE_PLANE_PAERPENDICULAR, EdgePlanePerpendicular)
}  // namespace g2o

namespace hdl_graph_slam {

/**
 * @brief constructor
 */
GraphSLAM::GraphSLAM(const std::string& solver_type) {
  graph.reset(new g2o::SparseOptimizer());
  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());

  g2o::ParameterSE3Offset* offset = new g2o::ParameterSE3Offset;
  offset->setId(0);
  graph->addParameter(offset);

  std::cout << "construct solver: " << solver_type << std::endl;
  g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
  g2o::OptimizationAlgorithmProperty solver_property;
  g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property);
  graph->setAlgorithm(solver);

  if(!graph->solver()) {
    std::cerr << std::endl;
    std::cerr << "error : failed to allocate solver!!" << std::endl;
    solver_factory->listSolvers(std::cerr);
    std::cerr << "-------------" << std::endl;
    std::cin.ignore(1);
    return;
  }
  std::cout << "done" << std::endl;

  robust_kernel_factory = g2o::RobustKernelFactory::instance();
}

/**
 * @brief destructor
 */
GraphSLAM::~GraphSLAM() {
  graph.reset();
}

void GraphSLAM::set_solver(const std::string& solver_type) {
  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());

  g2o::ParameterSE3Offset* offset = new g2o::ParameterSE3Offset;
  offset->setId(0);
  graph->addParameter(offset);

  std::cout << "construct solver: " << solver_type << std::endl;
  g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
  g2o::OptimizationAlgorithmProperty solver_property;
  g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property);
  graph->setAlgorithm(solver);

  if(!graph->solver()) {
    std::cerr << std::endl;
    std::cerr << "error : failed to allocate solver!!" << std::endl;
    solver_factory->listSolvers(std::cerr);
    std::cerr << "-------------" << std::endl;
    std::cin.ignore(1);
    return;
  }
  std::cout << "done" << std::endl;
}

int GraphSLAM::num_vertices() const {
  return graph->vertices().size();
}
int GraphSLAM::num_edges() const {
  return graph->edges().size();
}

g2o::VertexSE3* GraphSLAM::add_se3_node(const Eigen::Isometry3d& pose) {
  g2o::VertexSE3* vertex(new g2o::VertexSE3());
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(pose);
  graph->addVertex(vertex);

  return vertex;
}

g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& plane_coeffs) {
  g2o::VertexPlane* vertex(new g2o::VertexPlane());
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(plane_coeffs);
  graph->addVertex(vertex);

  return vertex;
}

g2o::VertexPointXYZ* GraphSLAM::add_point_xyz_node(const Eigen::Vector3d& xyz) {
  g2o::VertexPointXYZ* vertex(new g2o::VertexPointXYZ());
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(xyz);
  graph->addVertex(vertex);

  return vertex;
}

g2o::EdgeSE3* GraphSLAM::add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3* edge(new g2o::EdgeSE3());
  edge->setMeasurement(relative_pose);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v1;
  edge->vertices()[1] = v2;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane());
  edge->setMeasurement(plane_coeffs);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  edge->vertices()[1] = v_plane;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PointXYZ* GraphSLAM::add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PointXYZ* edge(new g2o::EdgeSE3PointXYZ());
  edge->setMeasurement(xyz);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  edge->vertices()[1] = v_xyz;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgePlanePriorNormal* GraphSLAM::add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgePlanePriorNormal* edge(new g2o::EdgePlanePriorNormal());
  edge->setMeasurement(normal);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgePlanePriorDistance* GraphSLAM::add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgePlanePriorDistance* edge(new g2o::EdgePlanePriorDistance());
  edge->setMeasurement(distance);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY());
  edge->setMeasurement(xy);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PriorXYZ* GraphSLAM::add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorXYZ* edge(new g2o::EdgeSE3PriorXYZ());
  edge->setMeasurement(xyz);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PriorVec* GraphSLAM::add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix) {
  Eigen::Matrix<double, 6, 1> m;
  m.head<3>() = direction;
  m.tail<3>() = measurement;

  g2o::EdgeSE3PriorVec* edge(new g2o::EdgeSE3PriorVec());
  edge->setMeasurement(m);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3PriorQuat* GraphSLAM::add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3PriorQuat* edge(new g2o::EdgeSE3PriorQuat());
  edge->setMeasurement(quat);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v_se3;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgePlane* GraphSLAM::add_plane_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) {
  g2o::EdgePlane* edge(new g2o::EdgePlane());
  edge->setMeasurement(measurement);
  edge->setInformation(information);
  edge->vertices()[0] = v_plane1;
  edge->vertices()[1] = v_plane2;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgePlaneIdentity* GraphSLAM::add_plane_identity_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) {
  g2o::EdgePlaneIdentity* edge(new g2o::EdgePlaneIdentity());
  edge->setMeasurement(measurement);
  edge->setInformation(information);
  edge->vertices()[0] = v_plane1;
  edge->vertices()[1] = v_plane2;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgePlaneParallel* GraphSLAM::add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information) {
  g2o::EdgePlaneParallel* edge(new g2o::EdgePlaneParallel());
  edge->setMeasurement(measurement);
  edge->setInformation(information);
  edge->vertices()[0] = v_plane1;
  edge->vertices()[1] = v_plane2;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgePlanePerpendicular* GraphSLAM::add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information) {
  g2o::EdgePlanePerpendicular* edge(new g2o::EdgePlanePerpendicular());
  edge->setMeasurement(measurement);
  edge->setInformation(information);
  edge->vertices()[0] = v_plane1;
  edge->vertices()[1] = v_plane2;
  graph->addEdge(edge);

  return edge;
}

g2o::EdgeSE3Prior* GraphSLAM::add_se3_edge_prior(g2o::VertexSE3* v1, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE3Prior* edge(new g2o::EdgeSE3Prior());
  edge->setMeasurement(relative_pose);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v1;
  edge->setParameterId(0, 0);
  graph->addEdge(edge);

  return edge;
}

g2o::VertexSE2* GraphSLAM::add_se2_node(const Eigen::Isometry2d& pose) {
  g2o::VertexSE2* vertex(new g2o::VertexSE2());
  vertex->setId(static_cast<int>(graph->vertices().size()));
  vertex->setEstimate(pose);
  graph->addVertex(vertex);

  return vertex;
}

g2o::EdgeSE2* GraphSLAM::add_se2_edge(g2o::VertexSE2* v1, g2o::VertexSE2* v2, const Eigen::Isometry2d& relative_pose, const Eigen::MatrixXd& information_matrix) {
  g2o::EdgeSE2* edge(new g2o::EdgeSE2());
  edge->setMeasurement(relative_pose);
  edge->setInformation(information_matrix);
  edge->vertices()[0] = v1;
  edge->vertices()[1] = v2;
  graph->addEdge(edge);

  return edge;
}

void GraphSLAM::add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size) {
  if(kernel_type == "NONE") {
    return;
  }

  g2o::RobustKernel* kernel = robust_kernel_factory->construct(kernel_type);
  if(kernel == nullptr) {
    std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl;
    return;
  }

  kernel->setDelta(kernel_size);

  g2o::OptimizableGraph::Edge* edge_ = dynamic_cast<g2o::OptimizableGraph::Edge*>(edge);
  edge_->setRobustKernel(kernel);
}

int GraphSLAM::optimize(int num_iterations) {
  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
  if(graph->edges().size() < 10) {
    return -1;
  }

  std::cout << std::endl;
  std::cout << "--- pose graph optimization ---" << std::endl;
  std::cout << "nodes: " << graph->vertices().size() << "   edges: " << graph->edges().size() << std::endl;
  std::cout << "optimizing... " << std::flush;

  std::cout << "init" << std::endl;
  graph->initializeOptimization();
  graph->setVerbose(true);

  std::cout << "chi2" << std::endl;
  double chi2 = graph->chi2();

  std::cout << "optimize!!" << std::endl;
  auto t1 = ros::WallTime::now();
  int iterations = graph->optimize(num_iterations);

  auto t2 = ros::WallTime::now();
  std::cout << "done" << std::endl;
  std::cout << "iterations: " << iterations << " / " << num_iterations << std::endl;
  std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl;
  std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;

  return iterations;
}

void GraphSLAM::save(const std::string& filename) {
  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());

  std::ofstream ofs(filename);
  graph->save(ofs);

  g2o::save_robust_kernels(filename + ".kernels", graph);
}

bool GraphSLAM::load(const std::string& filename) {
  std::cout << "loading pose graph..." << std::endl;
  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());

  std::ifstream ifs(filename);
  if(!graph->load(ifs, true)) {
    return false;
  }

  std::cout << "nodes  : " << graph->vertices().size() << std::endl;
  std::cout << "edges  : " << graph->edges().size() << std::endl;

  if(!g2o::load_robust_kernels(filename + ".kernels", graph)) {
    return false;
  }

  return true;
}

}  // namespace hdl_graph_slam