#include <Rcpp.h>
#include <RcppEigen.h>
#include <vector>

#include "experimentalSetup.hpp"
#include "encodingScheme.hpp"
#include "individual.hpp"
#include "population.hpp"
#include "evolutionaryAlgorithm.hpp"
#include "AuxiliaryFunctions.hpp"
#include "logLikelihoods.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/normal.hpp>

EvolutionaryAlgorithm::EvolutionaryAlgorithm() { }

EvolutionaryAlgorithm::EvolutionaryAlgorithm(ExperimentalSetup & ES, const std::size_t & populationSize, InitialPopulationRandomVariates & RV)
{
    PopulationSize = populationSize;
    CurrentPopulation = InitialisePopulation(ES, RV);
}

EvolutionaryAlgorithm::EvolutionaryAlgorithm(ExperimentalSetup & ES, const std::size_t & populationSize,
                                             const std::size_t & numberOfIterations, const std::size_t & numberOfIterationsEqualMinMax,
                                             const std::size_t & numberOfFittestIndividuals,
                                             const int & parentSelectionWindowSize, const bool & allowParentSurvival,
                                             const double & crossoverProbability, const double & mutationProbabilityLowerLimit,
                                             const std::size_t & mutationIterations, const double & mutationDegreesOfFreedom,
                                             const Eigen::VectorXd & mutationDecay, const double & fractionFittestIndividuals,
                                             const std::size_t & hillClimbingIterations, InitialPopulationRandomVariates & RV)
{
    PopulationSize = populationSize;
    NumberOfFittestIndividuals = numberOfFittestIndividuals;
    NumberOfIterations = numberOfIterations;
    NumberOfIterationsEqualMinMax = numberOfIterationsEqualMinMax;

    ParentSelectionWindowSize = parentSelectionWindowSize;
    AllowParentSurvival = allowParentSurvival;

    CrossoverProbability = crossoverProbability;

    MutationProbabilityLowerLimit = mutationProbabilityLowerLimit;
    MutationIterations = mutationIterations;

    MutationDegreesOfFreedom = mutationDegreesOfFreedom;
    MutationDecay = mutationDecay;
    MutationDecay_t = MutationDecay[0];

    FittestEnsuredSurvivalFraction = fractionFittestIndividuals;
    FitnessEnsuredSurvivalFraction = -HUGE_VAL;

    HillClimbingIterations = hillClimbingIterations;

    CurrentPopulation = InitialisePopulation(ES, RV);
    Iteration = 0;

    // Finds maximum fitness index
    Eigen::VectorXd::Index fittestIndividualIndex;
    CurrentPopulation.Fitness.maxCoeff(&fittestIndividualIndex);
    FittestIndividualIndex = fittestIndividualIndex;
}

EvolutionaryAlgorithm::EvolutionaryAlgorithm(ExperimentalSetup & ES, Population & P,
                                             const std::size_t & numberOfIterations, const std::size_t & numberOfIterationsEqualMinMax,
                                             const std::size_t & numberOfFittestIndividuals,
                                             const int & parentSelectionWindowSize, const bool & allowParentSurvival,
                                             const double & crossoverProbability, const double & mutationProbabilityLowerLimit,
                                             const std::size_t & mutationIterations, const double & mutationDegreesOfFreedom,
                                             const Eigen::VectorXd & mutationDecay, const double & fractionFittestIndividuals,
                                             const std::size_t & hillClimbingIterations)
{
    CurrentPopulation = P;

    PopulationSize = P.Individuals.size();
    NumberOfFittestIndividuals = numberOfFittestIndividuals;
    NumberOfIterations = numberOfIterations;
    NumberOfIterationsEqualMinMax = numberOfIterationsEqualMinMax;

    ParentSelectionWindowSize = parentSelectionWindowSize;
    AllowParentSurvival = allowParentSurvival;

    CrossoverProbability = crossoverProbability;

    MutationProbabilityLowerLimit = mutationProbabilityLowerLimit;
    MutationIterations = mutationIterations;
    MutationDegreesOfFreedom = mutationDegreesOfFreedom;
    MutationDecay = mutationDecay;
    MutationDecay_t = MutationDecay[0];

    FittestEnsuredSurvivalFraction = fractionFittestIndividuals;
    FitnessEnsuredSurvivalFraction = -HUGE_VAL;

    HillClimbingIterations = hillClimbingIterations;
    Iteration = 0;

    // Finds maximum fitness index
    Eigen::VectorXd::Index fittestIndividualIndex;
    CurrentPopulation.Fitness.maxCoeff(&fittestIndividualIndex);
    FittestIndividualIndex = fittestIndividualIndex;
}

void EvolutionaryAlgorithm::RestructingIndividual(Individual & I, const ExperimentalSetup & ES)
{
    const std::size_t & NumberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;
    // Sorting unknown contributor mixtures
    const Eigen::VectorXd & Mixtures = I.MixtureParameters.segment(ES.NumberOfKnownContributors, NumberOfUnknownContributors);

    const std::vector<int> & sortedMixtures = sortedIndex(Mixtures);
    for (std::size_t n = 0; n < NumberOfUnknownContributors; n++)
    {
        I.MixtureParameters[ES.NumberOfKnownContributors + n] = Mixtures[sortedMixtures[n]];
    }

    // Sorting unknown genotypes
    const Eigen::VectorXd unsortedEncodedProfile = I.EncodedProfile;
    const std::vector<Eigen::MatrixXd> unsortedReducedContributionMatrix = I.ReducedExpectedContributionMatrix;
    for (std::size_t m = 0; m < ES.NumberOfMarkers; m++)
    {
        const Eigen::MatrixXd & unsortedReducedContributionMatrix_m = unsortedReducedContributionMatrix[m];
        for (std::size_t c = 0; c < NumberOfUnknownContributors; c++)
        {
            I.ReducedExpectedContributionMatrix[m].col(c) = unsortedReducedContributionMatrix_m.col(sortedMixtures[c]);

            std::size_t k = 2 * NumberOfUnknownContributors * m + 2 * c;
            std::size_t contributorElement = 2 * NumberOfUnknownContributors * m + 2 * sortedMixtures[c];

            if (unsortedEncodedProfile[contributorElement] <= unsortedEncodedProfile[contributorElement + 1]) {
                I.EncodedProfile[k] = unsortedEncodedProfile[contributorElement];
                I.EncodedProfile[k + 1] = unsortedEncodedProfile[contributorElement + 1];
            }
            else {
                I.EncodedProfile[k] = unsortedEncodedProfile[contributorElement + 1];
                I.EncodedProfile[k + 1] = unsortedEncodedProfile[contributorElement];
            }
        }
    }
}

Population EvolutionaryAlgorithm::InitialisePopulation(ExperimentalSetup & ES, InitialPopulationRandomVariates & RV)
{
    std::vector<Individual> I(PopulationSize);
    for (std::size_t n = 0; n < PopulationSize; n++)
    {
        Eigen::VectorXd U = ES.GenerateUnknownGenotype(RV);

        Individual I_n = Individual(U, ES);

        RestructingIndividual(I_n, ES);
        I[n] = I_n;
    }

    Population P(I);
    return P;
}

std::size_t EvolutionaryAlgorithm::ChoosePartner(const Population & P, int currentIndividual, RandomVariates & RV)
{
    int lowerWindow = std::round(std::max(0.0, static_cast<double>(currentIndividual - ParentSelectionWindowSize)));
    int upperWindow = std::round(std::min(currentIndividual + ParentSelectionWindowSize, static_cast<int>(PopulationSize - 1)));

    int leftBound = (currentIndividual - ParentSelectionWindowSize + PopulationSize) % PopulationSize;
    int rightBound = (currentIndividual + ParentSelectionWindowSize) % PopulationSize;

    Eigen::VectorXd neighbourhood = Eigen::VectorXd::Zero(2 * ParentSelectionWindowSize);
    Eigen::VectorXd logNeighbourhoodFitness = Eigen::VectorXd::Zero(2 * ParentSelectionWindowSize);
    for (std::size_t i = 0; i < 2 * ParentSelectionWindowSize; i++)
    {
        bool j = (i >= ParentSelectionWindowSize);
        int k = (leftBound + i + j) % PopulationSize;
        neighbourhood[i] = k;
        logNeighbourhoodFitness[i] = P.Fitness[k];
    }

    double logNeighbourhoodFitnessCenter = logNeighbourhoodFitness.maxCoeff();
    Eigen::VectorXd neighbourhoodFitness = Eigen::VectorXd::Zero(2 * ParentSelectionWindowSize);
    for (std::size_t i = 0; i < 2 * ParentSelectionWindowSize; i++)
    {
        neighbourhoodFitness[i] = std::exp(logNeighbourhoodFitness[i] - logNeighbourhoodFitnessCenter);
    }

    Eigen::VectorXd windowProbabilities = partialSumEigen(neighbourhoodFitness / neighbourhoodFitness.sum());

    double u = RV.generate_uniform_real();

    int h = 0;
    while ((u > windowProbabilities[h + 1])) {
        h++;
    }

    std::size_t partnerIndex = neighbourhood[h];
    return partnerIndex;
}

Individual EvolutionaryAlgorithm::Crossover(const Individual & I, const Individual & J, const ExperimentalSetup & ES, RandomVariates & RV)
{
    Eigen::MatrixXd E_IJ = bindColumns(I.EncodedProfile, J.EncodedProfile);

    int columnIndex = RV.generate_uniform_binary();

    std::size_t N = E_IJ.rows();
    Eigen::VectorXd E = Eigen::VectorXd::Zero(N);
    for (std::size_t n = 0; n < N; n++)
    {
        double p = RV.generate_uniform_real();
        if (p < CrossoverProbability)
        {
            columnIndex = (columnIndex + 1) % 2;
        }

        E[n] = E_IJ.row(n)[columnIndex];
    }

    Individual K(E, ES);
    RestructingIndividual(K, ES);

    return K;
}

Eigen::VectorXd EvolutionaryAlgorithm::CreateMutationProbability(Individual & I, const ExperimentalSetup & ES)
{
    const Eigen::VectorXd & E = I.EncodedProfile;
    const double & referenceMarkerAverage = I.SampleParameters[0];
    const double & dispersion = I.SampleParameters[1];

    const Eigen::VectorXd & Coverage = ES.Coverage;
    const Eigen::VectorXd & MarkerImbalance = I.MarkerImbalanceParameters;
    const Eigen::VectorXd & NumberOfAlleles = ES.NumberOfAlleles;

    std::size_t N = ES.Coverage.size();
    boost::math::students_t devianceDistribution(MutationDegreesOfFreedom);

    Eigen::VectorXd mutation = MutationProbabilityLowerLimit * Eigen::VectorXd::Ones(E.size());
    if (std::abs(MutationDecay_t - MutationProbabilityLowerLimit) > DBL_EPSILON)
    {
        for (std::size_t m = 0; m < MarkerImbalance.size(); m++)
        {
            const Eigen::VectorXd & alleleIndex_m = I.ReducedAlleleIndex[m];
            const std::size_t A = alleleIndex_m.size();

            const Eigen::MatrixXd & expectedContributionProfile_m = I.ReducedExpectedContributionMatrix[m];
            const Eigen::VectorXd & EC = expectedContributionProfile_m * I.MixtureParameters;

            const Eigen::VectorXd & E_m = E.segment(2 * (ES.NumberOfContributors - ES.NumberOfKnownContributors) * m, 2 * (ES.NumberOfContributors - ES.NumberOfKnownContributors));
            for (std::size_t i = 0; i < E_m.size(); i++)
            {
                const std::size_t & n = ES.PartialSumAlleles[m] + E_m[i];
                std::size_t a = 0;
                while ((alleleIndex_m[a] != E_m[i]) & (a < A - 1))
                {
                    a++;
                }

                double mu_ma = referenceMarkerAverage * MarkerImbalance[m] * EC[a];
                if ((mu_ma == 0) & (Coverage[n] != 0))
                {
                    mu_ma += 2e-8;
                }

                double deviance_n = devianceResidualPoissonGammaDistribution(Coverage[n], mu_ma, mu_ma / dispersion);
                if (std::isnan(deviance_n))
                {
                    deviance_n = 0.0;
                    Rcpp::warning("Deviance returned 'nan'.");
                }
                else if (std::isinf(deviance_n))
                {
                    deviance_n = HUGE_VAL;
                    Rcpp::warning("Deviance returned 'inf'.");
                }

                mutation[2 * (ES.NumberOfContributors - ES.NumberOfKnownContributors) * m + i] =
                    MutationDecay_t - (MutationDecay_t - MutationProbabilityLowerLimit) * boost::math::pdf(devianceDistribution, deviance_n) /
                        boost::math::pdf(devianceDistribution, 0.0);
            }
        }
    }

    return mutation;
}

void updateMarkerIndividual(const Eigen::VectorXd & E, std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix, std::vector<Eigen::VectorXd> reducedAlleleIndex,
                            std::vector<Eigen::VectorXd> reducedNoiseIndex, const std::size_t & m, const ExperimentalSetup & ES)
{
    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    // Creating genotype matrix of marker m
    Eigen::MatrixXd decodedProfile_m = Eigen::MatrixXd::Zero(ES.NumberOfAlleles[m], ES.NumberOfContributors);
    for (std::size_t i = 0; i < ES.NumberOfAlleles[m]; i++)
    {
        for (std::size_t j = 0; j < ES.NumberOfKnownContributors; j++)
        {
            decodedProfile_m(i, j) = ES.KnownProfiles(ES.PartialSumAlleles[m] + i, j);
        }
    }

    for (std::size_t i = 0; i < numberOfUnknownContributors; i++)
    {
        for (std::size_t j = 0; j <= 1; j++)
        {
            std::size_t l = 2 * m * numberOfUnknownContributors + 2 * i + j;
            decodedProfile_m(E[l], i + ES.NumberOfKnownContributors) += 1;
        }
    }

    // Creating ECM of row n
    std::vector<Eigen::MatrixXd> potentialParents_m = ES.PotentialParents[m];
    Eigen::MatrixXd expectedContributionProfile_m = Eigen::MatrixXd::Zero(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;
    }

    double noiseProfileSum_m = 0.0;
    double noiseProfileSize_m = expectedContributionProfile_m.rows();
    for (std::size_t i = 0; i < noiseProfileSize_m; i++)
    {
        const double & ecps = expectedContributionProfile_m.row(i).sum();
        if (!(ecps > 2e-16))
        {
            noiseProfileSum_m++;
        }
    }

    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 n = 0;
    std::size_t i = 0, j = 0;
    for (std::size_t a = 0; a < ES.NumberOfAlleles[m]; a++)
    {
        const Eigen::VectorXd & expectedContributionProfile_mn = expectedContributionProfile_m.row(n);

        if (expectedContributionProfile_mn.sum() > 0)
        {
            reducedExpectedContributionMatrix_m.row(i) = expectedContributionProfile_mn;
            reducedAlleleIndex_m[i] = a;
            i++;
        }
        else
        {
            reducedNoiseIndex_m[j] = a;
            j++;
        }

        n++;
    }

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

void EvolutionaryAlgorithm::Mutation(Individual & I, const ExperimentalSetup & ES, RandomVariates & RV)
{
    std::size_t numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    Eigen::VectorXd E = I.EncodedProfile;
    Eigen::VectorXd encodedMutation = CreateMutationProbability(I, ES);

    // const std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix = I.ReducedExpectedContributionMatrix;
    // const std::vector<Eigen::VectorXd> reducedAlleleIndex = I.ReducedAlleleIndex;
    // const std::vector<Eigen::VectorXd> reducedNoiseIndex = I.ReducedNoiseIndex;
    for (std::size_t m = 0; m < ES.NumberOfMarkers; m++)
    {
        std::size_t j = 0;
        const std::size_t i = 2 * numberOfUnknownContributors * m;
        for (std::size_t k = 0; k < 2 * numberOfUnknownContributors; k++)
        {
            double mutate = RV.generate_uniform_real();
            if (mutate < encodedMutation[i + k])
            {
                int mutationShift = 0;
                if (ES.NumberOfAlleles[m] > 1)
                {
                    while(mutationShift == 0)
                    {
                        mutationShift += RV.generate_uniform_mutation[m]();
                    }
                }

                const double E_k = E[i + k];
                E[i + k] = static_cast<int>(E_k + mutationShift) % static_cast<int>(ES.NumberOfAlleles[m]);
                j++;
            }
        }

        // if (j > 0)
        // {
        //     updateMarkerIndividual(E, reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, m, ES);
        // }
    }

    Individual J(E, ES); // J(E, reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, ES);
    I = J;
}

Eigen::VectorXd expectedContributionMatrixRow(const Eigen::VectorXd & E, const ExperimentalSetup & ES,
                                              const int & m, const std::size_t & k, const std::size_t & n)
{

    const std::size_t & a = E[k];
    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    // Creating genotype matrix of marker m
    Eigen::MatrixXd decodedProfile_m = Eigen::MatrixXd::Zero(ES.NumberOfAlleles[m], ES.NumberOfContributors);
    for (std::size_t i = 0; i < ES.NumberOfAlleles[m]; i++)
    {
        for (std::size_t j = 0; j < ES.NumberOfKnownContributors; j++)
        {
            decodedProfile_m(i, j) = ES.KnownProfiles(ES.PartialSumAlleles[m] + i, j);
        }
    }

    for (std::size_t i = 0; i < numberOfUnknownContributors; i++)
    {
        for (std::size_t j = 0; j <= 1; j++)
        {
            std::size_t l = 2 * m * numberOfUnknownContributors + 2 * i + j;
            decodedProfile_m(E[l], i + ES.NumberOfKnownContributors) += 1;
        }
    }

    // Creating ECM of row n
    std::vector<Eigen::MatrixXd> potentialParents_m = ES.PotentialParents[m];
    Eigen::VectorXd ECM_n = Eigen::VectorXd::Zero(decodedProfile_m.cols());
    for (std::size_t j = 0; j < ECM_n.size(); j++)
    {
        const Eigen::VectorXd & decodedProfile_mj = decodedProfile_m.col(j);

        double potentialParentContribution = ParentStutterContribution(a, ES.LevelsOfStutterRecursion, 1, decodedProfile_mj, potentialParents_m, ES.NumberOfAlleles[m]);
        ECM_n[j] = decodedProfile_mj[a] + potentialParentContribution;
    }

    return ECM_n;
}

void EvolutionaryAlgorithm::HillClimbing(Individual & I, ExperimentalSetup & ES, RandomVariates & RV)
{
    std::size_t numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    for (std::size_t i = 0; i < HillClimbingIterations; i++)
    {
        int stepMarker = RV.generate_uniform_marker();
        int stepContributor = RV.generate_uniform_unknown_contributor();
        int stepBinary = RV.generate_uniform_binary();
        std::size_t k = 2 * stepMarker * numberOfUnknownContributors + 2 * stepContributor + stepBinary;

        // Create n'th row of the decoded and ecm matrix
        std::size_t n = ES.PartialSumAlleles[stepMarker] + I.EncodedProfile[k];
        Eigen::VectorXd expectedContributionMatrixRow_n_i = expectedContributionMatrixRow(I.EncodedProfile, ES, stepMarker, k, n);

        double I_mu_ma = I.SampleParameters[0] * ES.MarkerImbalances[n] * expectedContributionMatrixRow_n_i.transpose() * I.MixtureParameters;

        std::vector< Eigen::VectorXd > surroundings(ES.NumberOfAlleles[stepMarker] - 1);
        Eigen::VectorXd surroundingResiduals = Eigen::VectorXd::Zero(ES.NumberOfAlleles[stepMarker] - 1);
        for (std::size_t j = 1; j < ES.NumberOfAlleles[stepMarker]; j++)
        {
            Eigen::VectorXd I_j = I.EncodedProfile;

            I_j[k] = static_cast<int>(I_j[k] + j) % static_cast<int>(ES.NumberOfAlleles[stepMarker]);
            std::size_t m = ES.PartialSumAlleles[stepMarker] + I_j[k];

            Eigen::VectorXd expectedContributionMatrixRow_n_j = expectedContributionMatrixRow(I_j, ES, stepMarker, k, m);
            double J_mu_ma = I.SampleParameters[0] * ES.MarkerImbalances[m] * expectedContributionMatrixRow_n_j.transpose() * I.MixtureParameters;

            surroundings[j - 1] = I_j;
            surroundingResiduals[j - 1] = std::abs(ES.Coverage[n] - I_mu_ma + ES.Coverage[m] - J_mu_ma);
        }

        Eigen::MatrixXf::Index minIndex;
        double smallestValue = surroundingResiduals.minCoeff(&minIndex);

        // std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix = I.ReducedExpectedContributionMatrix;
        // std::vector<Eigen::VectorXd> reducedAlleleIndex = I.ReducedAlleleIndex;
        // std::vector<Eigen::VectorXd> reducedNoiseIndex = I.ReducedNoiseIndex;
        //
        // updateMarkerIndividual(surroundings[minIndex], reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, stepMarker, ES);

        Individual K(surroundings[minIndex], ES); // K(surroundings[minIndex], reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, ES);
        if (K.Fitness > I.Fitness)
        {
            I = K;
        }
    }
}

Population EvolutionaryAlgorithm::SelectionCrossoverMutation(const Population & P, ExperimentalSetup & ES, RandomVariates & RV)
{
    // Creating new child population
    std::vector<Individual> childPopulation(PopulationSize);
    for (std::size_t i = 0; i < PopulationSize; i++)
    {
        Individual parent = P.Individuals[i];

        // Parent partner selection
        std::size_t partnerIndex = ChoosePartner(P, i, RV);

        // Crossover
        Individual child = Crossover(parent, P.Individuals[partnerIndex], ES, RV);

        // Mutation
        Mutation(child, ES, RV);

        if (AllowParentSurvival)
        {
            // Hill-climbing
            if (HillClimbingIterations != 0)
            {
                HillClimbing(parent, ES, RV);
                RestructingIndividual(parent, ES);
            }

            if (parent.Fitness < child.Fitness)
            {
                // Restructuring
                RestructingIndividual(child, ES);
                childPopulation[i] = child;
            }
            else
            {
                childPopulation[i] = parent;
            }
        }
        else
        {
            // Hill-climbing
            if (HillClimbingIterations != 0)
            {
                HillClimbing(child, ES, RV);
            }

            // Restructuring
            RestructingIndividual(child, ES);
            childPopulation[i] = child;
        }

    }

    Population C(childPopulation);
    return C;
}

bool individualsEqual(Individual & I, Individual & J)
{
    Eigen::VectorXd K = I.EncodedProfile - J.EncodedProfile;
    return (K.sum() < 2e-16);
}

void EvolutionaryAlgorithm::Run(ExperimentalSetup & ES, RandomVariates & RV, const bool & trace)
{
    std::vector<Individual> TI(NumberOfFittestIndividuals);
    std::vector<double> TF(NumberOfFittestIndividuals, -HUGE_VAL);

    std::size_t n = 0;
    std::size_t terminationCounter = 0;
    bool terminate = false;
    while (!terminate)
    {
        // Updating current population
        MutationDecay_t = MutationDecay[n];

        Population C = SelectionCrossoverMutation(CurrentPopulation, ES, RV);
        CurrentPopulation = C;

        // Updating the list of fittest individuals
        std::vector<int> sortedFitness = sortedIndex(C.Fitness);

        std::size_t m = 0;
        bool updateFittest = true;
        while (updateFittest)
        {
            Individual I_m = C.Individuals[sortedFitness[m]];

            double F_m = I_m.Fitness;
            if (F_m > TF[0])
            {
                auto it_front = std::lower_bound(TF.cbegin(), TF.cend(), F_m);
                auto it_end = std::upper_bound(TF.cbegin(), TF.cend(), F_m);
                std::size_t i = it_front - TF.cbegin();
                std::size_t j = it_end - TF.cbegin();

                bool isDuplicate = false;
                if (F_m == TF[j - 1]) {
                    for (std::size_t k = i - 1; k < j; k++)
                    {
                        Eigen::VectorXd K = I_m.EncodedProfile - TI[k].EncodedProfile;
                        double sumAbs = 0.0;
                        for (std::size_t h = 0; h < K.size(); h++) {
                            sumAbs += std::abs(K[h]);
                        }

                        isDuplicate = (sumAbs < 2e-16);
                    }
                }

                if (!isDuplicate)
                {
                    for (std::size_t k = 0; k < j - 1; k++)
                    {
                        TI[k] = TI[k + 1];
                        TF[k] = TF[k + 1];
                    }

                    TI[j - 1] = I_m;
                    TF[j - 1] = F_m;
                }
            }

            m++;
            updateFittest = (m < NumberOfFittestIndividuals) & (F_m > TF[0]);
        }

        // Updates the termination counter
        if (std::abs(C.Fitness[sortedFitness[0]] - C.Fitness.mean()) / std::abs(C.Fitness[sortedFitness[0]]) < ES.Tolerance[0])
        {
            terminationCounter++;
        }
        else
        {
            terminationCounter = 0;
        }

        n++;
        terminate = (terminationCounter >= NumberOfIterationsEqualMinMax) || (n >= NumberOfIterations);

        if (trace)
        {
            Rcpp::Rcout << "\tCurrent iteration: " << n << "\n"
                        << "\t\tPopulation Fitness: " << "\n"
                        << "\t\t  Highest: " << C.Fitness[sortedFitness[0]] << "\n"
                        << "\t\t  Average: " << C.Fitness.mean() << "\n"
                        << "\t\t  Lowest: " << C.Fitness[sortedFitness[sortedFitness.size() - 1]] << "\n"
                        << "\t\tTermination counter: " << terminationCounter << " / " << NumberOfIterationsEqualMinMax << "\n";
        }
    }

    Population FittestMembers(TI);
    FittestMembersOfEntireRun = FittestMembers;
}


#ifndef _MP2_HPP_
#define _MP2_HPP_

#include <boost/mpl/range_c.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/transform.hpp>

#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/as_vector.hpp>

namespace detail {

    namespace mpl = boost::mpl;
    namespace fusion = boost::fusion;

    template<size_t N>
    struct index {
	struct value_ {
	    typedef const int& type;
	    template<class> struct apply { typedef typename value_::type type; };
	};
	typedef typename value_::type value_type;
	typedef typename mpl::copy<mpl::range_c<int,0,N>,
				   mpl::back_inserter<mpl::vector_c<value_type> >
				   >::type v;
	typedef typename fusion::result_of::as_vector<
	    typename mpl::transform <v, value_>::type>::type type;
    };

    template<size_t N, typename T>
    struct  layout {
	typedef typename index<N>::type index_vector;
	size_t element_at(const index_vector &index) const;
    };

}

template<class T, size_t N, typename value_type>
struct tensor_operator;

template< class T, typename value_type>
struct tensor_operator<T, 4, value_type> {
    value_type& operator()(int i, int j, int k, int l) {
	return tensor_[typename T::index_vector(i, j, k, l)];
    }
    const value_type& operator()(int i, int j, int k, int l) const {
	return tensor_[typename T::index_vector(i, j, k, l)];
    }
protected:
    tensor_operator(T &tensor) : tensor_(tensor) {}
    T &tensor_;
};

template<size_t N, typename T = double>
struct tensor : tensor_operator<tensor<N,T>, N, T> {
    typedef T value_type;
    static const size_t rank = N;
    typedef detail::layout<N,T> layout_type;
    typedef typename layout_type::index_vector index_vector;
    value_type& operator[](const index_vector &index);
    const value_type& operator[](const index_vector &index) const;
private:
    layout_type layout_;
};

struct mp2 {
    tensor<4> T;
    void apply() {
	T(0,0,0,0) = 0;
    }
};

#endif /* _MP2_HPP_ */


#include "ros/ros.h"
#include <sstream>
#include "ekfslam/ekfslam.h"
#include <Eigen/Dense>

#define STATE_SIZE 5
#define HZ 30

int main(int argc, char **argv)
{
/* High level manager for the slam node.
Will call a class of slam that has been developed in subfolders to this directory.
Currently only has ekf slam.
 */
	ros::init(argc, argv, "slamNode");

	ros::NodeHandle n;

	ekfslam slam(n,STATE_SIZE);

	ROS_INFO_STREAM("EKF SLAM: LAUNCHED");

	ros::spin();

	// slam.runnable();
	return 0;
}


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

#include <boost/simd/sdk/memory/parameters.hpp>
#include <boost/simd/sdk/memory/is_power_of_2.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/assert.hpp>
#include <cstddef>

namespace boost { namespace simd
{
  /*!
    @brief Align value and address on an arbitrary alignment boundary

    @param value Value to align
    @param alignment Alignment boundary
  **/
  BOOST_FORCEINLINE std::size_t align_on(std::size_t value, std::size_t align)
  {
    BOOST_ASSERT_MSG( boost::simd::is_power_of_2(align)
                    , "Invalid alignment boundary. You tried to align an "
                      "address or a value on a non-power of 2 boundary."
                    );

    return (value+align-1) & ~(align-1);
  }

  /*! @overload **/
  template<class T> BOOST_FORCEINLINE T* align_on(T* ptr, std::size_t align)
  {
    std::size_t v = reinterpret_cast<std::size_t>(ptr);
    return reinterpret_cast<T*>(boost::simd::align_on(v,align));
  }

  /*!
    @brief Align value and address on default alignment boundary

    @param value Value to align
  **/
  template<class T> BOOST_FORCEINLINE T align_on(T value)
  {
    return boost::simd::align_on(value,BOOST_SIMD_CONFIG_ALIGNMENT);
  }
} }

#endif


//          Copyright Gavin Band 2008 - 2012.
// 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 <string>
#include <map>
#include <cmath>
#include <cassert>
#include <boost/function.hpp>
#include <Eigen/Core>
#include "components/SNPSummaryComponent/GenotypeTabulatingCallComparer.hpp"

GenotypeTabulatingCallComparer::GenotypeTabulatingCallComparer():
	m_threshhold( 0.9 )
{}

void GenotypeTabulatingCallComparer::compare(
	Eigen::MatrixXd const& left,
	Eigen::MatrixXd const& right,
	Callback callback
) const {
	// Make table of counts
	int const N = left.rows() ;
	assert( right.rows() == N ) ;
	assert( left.cols() == 3 ) ;
	assert( right.cols() == 3 ) ;

	Eigen::MatrixXd table( 4, 4 ) ;

	for( std::size_t i = 0; i < N; ++i ) {
		Eigen::MatrixXd::Index left_best_call = -1 ;
		Eigen::MatrixXd::Index right_best_call = -1 ;
		double left_best_call_prob = left.row( i ).maxCoeff( &left_best_call ) ;
		double right_best_call_prob = right.row( i ).maxCoeff( &right_best_call ) ;

		if( left_best_call_prob < m_threshhold ) {
			left_best_call = 3 ;
		}
		if( right_best_call_prob < m_threshhold ) {
			right_best_call = 3 ;
		}
		++table( left_best_call, right_best_call ) ;
	}

	for( int g1 = 0; g1 < 4; ++g1 ) {
		std::string const variable_name1 = ( g1 == 3 ) ? "missing" : genfile::string_utils::to_string( g1 ) ;
		for( int g2 = 0; g2 < 4; ++g2 ) {
			std::string const variable_name2 = ( g2 == 3 ) ? "missing" : genfile::string_utils::to_string( g2 ) ;
			callback( variable_name1 + "/" + variable_name2, table( g1, g2 ) ) ;
		}
	}
}


#pragma once

#include <tuple>

#include <boost/mp11.hpp>

namespace distant::memory
{
	namespace detail
	{
		template <std::size_t I, class Tuple>
		struct aligned_offset_impl
		{
			constexpr std::size_t operator()() const noexcept
			{
				using current_element_t = std::tuple_element_t<I, Tuple>;
				using previous_element_t = std::tuple_element_t<I - 1, Tuple>;

				constexpr auto accumulated_offset = aligned_offset_impl<I - 1, Tuple>{}();

				// Check if the offset and size of the previous element gives a correct alignment for the current element.
				constexpr auto alignment_remainder = (accumulated_offset + sizeof(previous_element_t)) % alignof(current_element_t);

				if (alignment_remainder == 0)
					return accumulated_offset + sizeof(previous_element_t);

				// Calculate the distance needed to be meet the alignment requirements of the current element.
				constexpr auto needed_distance = (alignof(current_element_t) - accumulated_offset) % alignof(current_element_t);

				// If the distance matches the alignment, just use the alignment itself (mod == 0 gives 0)
				if (needed_distance % alignof(current_element_t) == 0)
					return accumulated_offset + alignof(current_element_t);

				// In the last case add the needed distance to meet the next element's alignment requirements.
				return accumulated_offset + needed_distance;
			}
		};

		template <class Tuple>
		struct aligned_offset_impl<0, Tuple>
		{
			constexpr std::size_t operator()() const noexcept
			{
				return 0;
			}
		};
	}

// C++17 constexpr lambda version with less? template instantiations
#if 0
	/// @brief Calculate the aligned offset of an element in the given tuple as if it were put into a structure.
	/// @tparam N the index of the element in the tuple.
	/// @tparam Tuple the tuple to consider.
	/// @return the aligned offset.
	/// @note The aligned offset of the \a Nth element in \a Tuple depends on the previous \a N - 1 aligned offsets.
	template <std::size_t N, class Tuple>
	constexpr std::size_t aligned_offset() noexcept
	{
		using namespace boost::mp11;

		using indices_t = mp_pop_front<mp_iota_c<N>>;

		std::size_t accumulated_offset = 0;

		mp_for_each<indices_t>([&accumulated_offset](const auto I)
		{
			using current_element_t = std::tuple_element_t<I, Tuple>;
			using previous_element_t = std::tuple_element_t<I - 1, Tuple>;

			//constexpr auto accumulated_offset = aligned_offset_impl<I - 1, Tuple>{}();

			// Check if the offset and size of the previous element gives a correct alignment for the current element.
			constexpr auto alignment_remainder = (accumulated_offset + sizeof(previous_element_t)) % alignof(current_element_t);

			if (alignment_remainder == 0)
				return accumulated_offset += sizeof(previous_element_t);

			// Calculate the distance needed to be meet the alignment requirements of the current element.
			constexpr auto needed_distance = (alignof(current_element_t) - accumulated_offset) % alignof(current_element_t);

			// If the distance matches the alignment, just use the alignment itself (mod == 0 gives 0)
			if (needed_distance % alignof(current_element_t) == 0)
				return accumulated_offset += alignof(current_element_t);

			// In the last case add the needed distance to meet the next element's alignment requirements.
			return accumulated_offset += needed_distance;
		});

		return accumulated_offset;
	}
#endif
	/// @brief Calculate the aligned offset of an element in the given tuple as if it were put into a structure.
	/// @tparam N the index of the element in the tuple.
	/// @tparam Tuple the tuple to consider.
	/// @return the aligned offset.
	/// @note The aligned offset of the \a Nth element in \a Tuple depends on the previous \a N - 1 aligned offsets.
	template <std::size_t N, class Tuple>
	constexpr std::size_t aligned_offset() noexcept
	{
		return detail::aligned_offset_impl<N, Tuple>{}();
	}

	//template <class >

} // namespace distant::memory


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program is distributed in the hope that it will be useful,            *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of             *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

#include "aslam/calibration/error-terms/ErrorTermAngularVelocity.h"

#include <Eigen/Dense>

using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

  ErrorTermAngularVelocity::ErrorTermAngularVelocity(const aslam::backend::EuclideanExpression& r_w_mr,
                                                     const aslam::backend::EuclideanExpression& r_w_mr_m,
                                                     const Covariance& sigma2_ang_velZ) :
        _r_w_mr(r_w_mr),
        _r_w_mr_m(r_w_mr_m),
        _sigma2_ang_velZ(sigma2_ang_velZ) {
      setInvR(_sigma2_ang_velZ.inverse());
      DesignVariable::set_t dv;
      _r_w_mr.getDesignVariables(dv);
      _r_w_mr_m.getDesignVariables(dv);
      setDesignVariablesIterator(dv.begin(), dv.end());
    }

  ErrorTermAngularVelocity::ErrorTermAngularVelocity(const ErrorTermAngularVelocity& other) :
        ErrorTermFs<1>(other),
        _r_w_mr(other. _r_w_mr),
        _r_w_mr_m(other. _r_w_mr_m),
        _sigma2_ang_velZ(other._sigma2_ang_velZ){
    }

  ErrorTermAngularVelocity& ErrorTermAngularVelocity::operator =
        (const ErrorTermAngularVelocity& other) {
      if (this != &other) {
        ErrorTermFs<1>::operator=(other);
        _r_w_mr = other._r_w_mr;
        _r_w_mr_m = other._r_w_mr_m;
       _sigma2_ang_velZ = other._sigma2_ang_velZ;
      }
      return *this;
    }

    ErrorTermAngularVelocity::~ErrorTermAngularVelocity() {
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    double ErrorTermAngularVelocity::evaluateErrorImplementation() {
      error_t error;
      const double wz = _r_w_mr.toEuclidean()(2);
      const double wzm = _r_w_mr_m.toEuclidean()(2);

      error(0) = wz - wzm;

      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermAngularVelocity::evaluateJacobiansImplementation(JacobianContainer&
        jacobians) {
      Eigen::Matrix<double, 1, 3> Jw = Eigen::Matrix<double, 1, 3>::Zero();
      Eigen::Matrix<double, 1, 3> Jwm = Eigen::Matrix<double, 1, 3>::Zero();

      Jw(2) = 1.0;
      Jwm(2) = 1.0;

      _r_w_mr.evaluateJacobians(jacobians, Jw);
      _r_w_mr_m.evaluateJacobians(jacobians, -Jwm);
    }

  }
}


/*

Copyright (c) 2005-2020, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

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

*/

#ifndef _ABSTRACTLINEARELLIPTICPDE_HPP_
#define _ABSTRACTLINEARELLIPTICPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractLinearPde.hpp"
#include "UblasCustomFunctions.hpp"
#include "ChastePoint.hpp"
#include "Node.hpp"
#include "Element.hpp"
#include <petscvec.h>

/**
 * AbstractLinearEllipticPde class.
 *
 * A general PDE of the form:
 * 0 =   Grad.(DiffusionTerm(x)*Grad(u))
 *     + ComputeConstantInUSourceTerm(x)
 *     + ComputeLinearInUCoeffInSourceTerm(x, u)
 *
 * Parabolic PDEs are be derived from this (AbstractLinearParabolicPde)
 */
template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
class AbstractLinearEllipticPde : public AbstractLinearPde<ELEMENT_DIM, SPACE_DIM>
{
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Serialize the PDE object.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractLinearPde<ELEMENT_DIM, SPACE_DIM> >(*this);
    }

public:

    /**
     * Constructor.
     */
    AbstractLinearEllipticPde()
        : AbstractLinearPde<ELEMENT_DIM, SPACE_DIM>()
    {}

    /**
     * Destructor.
     */
    virtual ~AbstractLinearEllipticPde()
    {}

    /**
     * @return computed constant in u part of the source term, i.e g(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given point.
     *
     * @param rX The point in space
     * @param pElement The element
     */
    virtual double ComputeConstantInUSourceTerm(const ChastePoint<SPACE_DIM>& rX,
                                                Element<ELEMENT_DIM,SPACE_DIM>* pElement)=0;

    /**
     * @return computed coefficient of u in the linear part of the source term, i.e f(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given point in space.
     *
     * @param rX The point in space
     * @param pElement
     */
    virtual double ComputeLinearInUCoeffInSourceTerm(const ChastePoint<SPACE_DIM>& rX,
                                                     Element<ELEMENT_DIM,SPACE_DIM>* pElement)=0;

    /**
     * @return computed diffusion term at a given point. The diffusion tensor should be symmetric and positive definite
     *
     * @param rX The point in space at which the diffusion term is computed.
     * @return A matrix.
     */
    virtual c_matrix<double, SPACE_DIM, SPACE_DIM> ComputeDiffusionTerm(const ChastePoint<SPACE_DIM>& rX)=0;

    /**
     * @return computed constant in u part of the source term, i.e g(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given node.
     *
     * @param rNode the node
     */
    virtual double ComputeConstantInUSourceTermAtNode(const Node<SPACE_DIM>& rNode);

    /**
     * @return computed coefficient of u in the linear part of the source term, i.e f(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given node.
     *
     * @param rNode the node
     */
    virtual double ComputeLinearInUCoeffInSourceTermAtNode(const Node<SPACE_DIM>& rNode);
};

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double AbstractLinearEllipticPde<ELEMENT_DIM, SPACE_DIM>::ComputeConstantInUSourceTermAtNode(const Node<SPACE_DIM>& rNode)
{
    return ComputeConstantInUSourceTerm(rNode.GetPoint(), nullptr);
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double AbstractLinearEllipticPde<ELEMENT_DIM, SPACE_DIM>::ComputeLinearInUCoeffInSourceTermAtNode(const Node<SPACE_DIM>& rNode)
{
    return ComputeLinearInUCoeffInSourceTerm(rNode.GetPoint(), nullptr);
}

#endif //_ABSTRACTLINEARELLIPTICPDE_HPP_


#include "igl/opengl/glfw/Viewer.h"
#include "igl/readOFF.h"
#include "igl/fit_plane.h"
#include "igl/mat_min.h"
#include "DataSet.hpp"
#include "Mesh.hpp"
#include <Eigen/Geometry>

#include <GLFW/glfw3.h>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
#include <imgui/imgui.h>

#include <iostream>
#include <fstream>
#include <sstream>

bool showMeshColor = false;
bool showPoints = true;
int viewer_mesh_level = 0;
Mesh<float> mesh;
Eigen::MatrixXd P, P2, current_target, C;
DataSet *ds;

// Splits example point cloud into two point clouds
template <typename T>
void split_point_cloud(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P1, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P2, const Eigen::Matrix<int, Eigen::Dynamic, 1>& id1, Eigen::Matrix<int, Eigen::Dynamic, 1>& id2)
{
  P1.resize(id1.rows(), 3);
  P2.resize(id2.rows(), 3);

  for (int i = 0; i < id1.rows(); ++i)
    P1.row(i) = P.row(id1(i));

  for (int i = 0; i < id2.rows(); ++i)
    P2.row(i) = P.row(id2(i));
}

template <typename T>
void generate_example_point_cloud(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P, Eigen::Matrix<int, Eigen::Dynamic, 1>& id1, Eigen::Matrix<int, Eigen::Dynamic, 1>& id2)
{
  const int size = 120;
  P.resize(size*size,3);
  id1.resize(size*int(size / 2));
  id2.resize(size*int((size+1) / 2));

  int cntr1 = 0, cntr2 = 0;
  for (int x = 0; x < size; ++x)
  {
    const int multipl = x / 30 + 1;
    for (int y = 0; y < size; ++y)
    {
      T height;

      if (int(y / (size / (5*multipl))) % 2)
        height = 15;
      else
        height = 0;

      Eigen::Matrix<T, 1, 3> row(T(x), height, T(y));
      P.row(x + y*size) = row;

      if (x < size / 2)
      {
        id1(cntr1++) = x + y*size;
      }
      else
      {
        id2(cntr2++) = x + y*size;
      }

    }
  }

}

void reload_viewer_data(igl::opengl::glfw::Viewer &viewer, const Eigen::MatrixXd pc, const Eigen::MatrixXd C, const unsigned int mesh_level, const bool color, const bool showPoints)
{
    viewer.data().clear();

    if (showPoints)
    {
      viewer.data().point_size = 5;
      viewer.data().add_points(pc, C);
    }

    Eigen::MatrixXf vertices;
    Eigen::MatrixXi faces;
    ColorData colorData;
    mesh.get_mesh(mesh_level, vertices, faces, colorData);
    viewer.data().set_mesh(vertices.cast<double>(), faces);

    if (color)
    {
      viewer.data().set_texture(colorData.texture.red, colorData.texture.green, colorData.texture.blue);
      viewer.data().set_uv(colorData.UV);
      viewer.data().set_colors(Eigen::RowVector3d(1.f, 1.f, 1.f)); // https://github.com/libigl/libigl/issues/570
      viewer.core.lighting_factor = 0.5;
      viewer.data().show_texture = true;
      viewer.data().show_lines = false;
    }else
    {
      mesh.get_mesh(mesh_level, vertices, faces);
      viewer.data().set_mesh(vertices.cast<double>(), faces);
      viewer.data().show_texture = false;
      viewer.data().show_lines = true;
    }

    viewer.data().compute_normals();
    viewer.data().invert_normals = true;
}

bool callback_key_down(igl::opengl::glfw::Viewer &viewer, unsigned char key, int /*modifier*/)
{
  // Solve one iteration when 1 is pressed
  if (key == '1')
  {
    mesh.solve(1);
    Eigen::MatrixXf vertices;
    Eigen::MatrixXi faces;
    mesh.get_mesh(viewer_mesh_level, vertices, faces);
    viewer.data().set_mesh(vertices.cast<double>(), faces);
    viewer.data().compute_normals();
    viewer.data().invert_normals = true;
  }

  if (static_cast<int>(key) == 80)
  {
    showPoints = !showPoints;
    reload_viewer_data(viewer, current_target, C, viewer_mesh_level, showMeshColor, showPoints);
  }

  if (static_cast<int>(key) == 67)
  {
    showMeshColor = !showMeshColor;
    reload_viewer_data(viewer, current_target, C, viewer_mesh_level, showMeshColor, showPoints);
  }

  // Move to next point cloud when 2 is pressed
  if (key == '2')
  {

    C.resize(1, 3);
    C << 0.f,0.7f,0.7f;

    if (ds) {
      Eigen::Matrix4d t_camera;
      ds->get_next_point_cloud(current_target, C, t_camera);

      mesh.set_target_point_cloud(current_target.cast<float>().eval(), C.cast<float>().eval());
    }else
    {
      // If example point cloud switch to second part of it
      current_target = P2;
      mesh.set_target_point_cloud(current_target.cast<float>().eval());
    }

    reload_viewer_data(viewer, current_target, C, viewer_mesh_level, showMeshColor, showPoints);
  }

  if (key == '3' && ds)
  {
    C.resize(1, 3);
    C << 0.f,0.7f,0.7f;

    for (int i = 0; i < 10; ++i)
    {
      if (ds) {
        Eigen::Matrix4d t_camera;
        ds->get_next_point_cloud(current_target, C, t_camera);

        mesh.set_target_point_cloud(current_target.cast<float>().eval(), C.cast<float>().eval());
      }else
      {
        current_target = P2;
        mesh.set_target_point_cloud(current_target.cast<float>().eval());
      }

      mesh.solve(20);
    }

      reload_viewer_data(viewer, current_target, C, viewer_mesh_level, showMeshColor, showPoints);

  }

  if (key == '4' && ds)
  {
    // Set Viewer to tight draw loop
    viewer.core.is_animating = !viewer.core.is_animating;
  }

  // Increase displayed mesh resolution
  if (key == '0')
  {
    viewer_mesh_level = viewer_mesh_level >= MESH_LEVELS - 1 ? viewer_mesh_level : viewer_mesh_level + 1;
    reload_viewer_data(viewer, current_target, C, viewer_mesh_level, showMeshColor, showPoints);
  }

  // Reduce displayed mesh resolution
  if (key == '-')
  {
    viewer_mesh_level = viewer_mesh_level < 1 ? viewer_mesh_level : viewer_mesh_level - 1;
    reload_viewer_data(viewer, current_target, C, viewer_mesh_level, showMeshColor, showPoints);
  }

  return true;
}

int main(int argc, char* argv[]) {
    // Read points and normals
    // igl::readOFF(argv[1],P,F,N);

    C.resize(1,3);
    C << 0.f,0.7f,0.7f;

    if (argc > 2)
    {
      std::cout << "Usage: $0" << std::endl;
      return EXIT_SUCCESS;
    } else if (argc > 1) {
      // Load dataset
      std::string folder(argv[1]);
      ds = new DataSet(folder);
      Eigen::Matrix4d t_camera;
      ds->get_next_point_cloud(P, C, t_camera);
      current_target = P;

      mesh.align_to_point_cloud(P.cast<float>().eval());
      mesh.set_target_point_cloud(current_target.cast<float>().eval(), C.cast<float>().eval());
      showMeshColor = true;
    } else {
      // Create example point cloud
      Eigen::VectorXi id1, id2;
      generate_example_point_cloud(P, id1, id2);
      split_point_cloud(P, current_target, P2, id1, id2);

      mesh.align_to_point_cloud(P.cast<float>().eval());
      mesh.set_target_point_cloud(current_target.cast<float>().eval());
    }

    igl::opengl::glfw::Viewer viewer;
    viewer.callback_key_down = callback_key_down;

    viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer & v)->bool
    {
      if (!ds || !viewer.core.is_animating) return false;

      Eigen::Matrix4d t_camera;
      bool cont = ds->get_next_point_cloud(P, C, t_camera);
      if (!cont)
      {
        // end of image sequence
        viewer.core.is_animating = false;
        return false;
      }

      mesh.set_target_point_cloud(P.cast<float>().eval(), C.cast<float>().eval());
      mesh.solve(1);
      reload_viewer_data(viewer, P, C, viewer_mesh_level, showMeshColor, showPoints);

      return false;
    };

    reload_viewer_data(viewer, current_target, C, viewer_mesh_level, showMeshColor, showPoints);
    viewer.core.align_camera_center(current_target);

    igl::opengl::glfw::imgui::ImGuiMenu menu;
    viewer.plugins.push_back(&menu);

    viewer.core.background_color = Eigen::Vector4f(1.0f, 1.0f, 1.0f, 1.0f);

    menu.callback_draw_viewer_menu = [&]()
    {
      ImGui::Text("Iterate: \"1\"");
      ImGui::Text("Next point cloud: \"2\"");
      ImGui::Text("Lots of iterations and point clouds (dataset only): \"3\"");
      ImGui::Text("Play image sequence (%s): \"4\"", viewer.core.is_animating ? "on" : "off");
      ImGui::Text("Change mesh level (%d): 0/+", viewer_mesh_level);
      ImGui::Text("Toggle coloring (%s): C", showMeshColor ? "on" : "off");
      ImGui::Text("Toggle point cloud (%s): P", showPoints ? "on" : "off");
    };

    viewer.launch();

    // cleanup
    mesh.cleanup();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGEBRA_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGEBRA_HPP

#include <boost/geometry/extensions/algebra/core/access.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_dimension.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_system.hpp>
#include <boost/geometry/extensions/algebra/core/coordinate_type.hpp>
#include <boost/geometry/extensions/algebra/core/tags.hpp>

#include <boost/geometry/extensions/algebra/geometries/concepts/vector_concept.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_quaternion_concept.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_matrix_concept.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/check.hpp>

#include <boost/geometry/extensions/algebra/geometries/vector.hpp>
#include <boost/geometry/extensions/algebra/geometries/rotation_quaternion.hpp>
#include <boost/geometry/extensions/algebra/geometries/rotation_matrix.hpp>

#include <boost/geometry/extensions/algebra/algorithms/assign.hpp>
#include <boost/geometry/extensions/algebra/algorithms/convert.hpp>

// experimental
#include <boost/geometry/extensions/algebra/algorithms/clear.hpp>
#include <boost/geometry/extensions/algebra/algorithms/reverse.hpp>

#include <boost/geometry/extensions/algebra/algorithms/translation.hpp>
#include <boost/geometry/extensions/algebra/algorithms/rotation.hpp>

// should be removed, transform() should be used instead
#include <boost/geometry/extensions/algebra/algorithms/transform_geometrically.hpp>

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGEBRA_HPP


/*
 Copyright (c) 2010, The Barbarian Group
 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 "cinder/ip/EdgeDetect.h"
#include "cinder/Surface.h"
#include "cinder/CinderMath.h"

#include <boost/preprocessor/seq.hpp>

namespace cinder { namespace ip {

//     X           Y
// -1  0  1     1  2  1
// -2  0  2     0  0  0
// -1  0  1    -1 -2 -1
// NOTE: this leaves garbage in the top and bottom rows, as well as the left and right columns

template<typename T>
void edgeDetectSobel( const ChannelT<T> &srcChannel, const Area &srcArea, const Vec2i &dstLT, ChannelT<T> *dstChannel )
{
	std::pair<Area,Vec2i> srcDst = clippedSrcDst( srcChannel.getBounds(), srcArea, dstChannel->getBounds(), dstLT );
	const Area &area( srcDst.first );
	const Vec2i &dstOffset( srcDst.second );
	typename CHANTRAIT<T>::Sum sumX, sumY;

	int32_t srcRowBytes = srcChannel.getRowBytes();
	int8_t srcPixelBytes = srcChannel.getIncrement() * sizeof(T);
	int8_t dstPixelBytes = dstChannel->getIncrement() * sizeof(T);
	const T maxValue = CHANTRAIT<T>::max();
	for( int32_t y = 1; y < area.getHeight() - 1; ++y ) {
		const uint8_t *srcLine = reinterpret_cast<const uint8_t*>( srcChannel.getData( area.getX1() + 1, area.getY1() + y ) );
		uint8_t *dstLine = reinterpret_cast<uint8_t*>( dstChannel->getData( dstOffset.x + area.getX1() + 1, dstOffset.y + y ) );
		for( int32_t x = area.getX1() + 1; x < area.getX2() - 1; ++x ) {
//			sumX = -srcLine[-srcRowPixels-srcPixelStride] + srcLine[-srcRowPixels+srcPixelStride] - 2 * srcLine[-srcPixelStride] + 2 * srcLine[srcPixelStride] - srcLine[srcRowPixels-srcPixelStride] + srcLine[srcRowPixels+srcPixelStride];
			sumX = -*(T*)(srcLine-srcRowBytes-srcPixelBytes) + *(T*)(srcLine-srcRowBytes+srcPixelBytes) - 2 * *(T*)(srcLine-srcPixelBytes) + 2 * *(T*)(srcLine+srcPixelBytes) - *(T*)(srcLine+srcRowBytes-srcPixelBytes) + *(T*)(srcLine+srcRowBytes+srcPixelBytes);
//			sumY = srcLine[-srcRowPixels-srcPixelStride] + 2 * srcLine[-srcRowPixels] + srcLine[-srcRowPixels + srcPixelStride]				- srcLine[srcRowPixels-srcPixelStride] - 2 * srcLine[srcRowPixels] - srcLine[srcRowPixels+srcPixelStride];
			sumY = *(T*)(srcLine-srcRowBytes-srcPixelBytes) + 2 * *(T*)(srcLine-srcRowBytes) + *(T*)(srcLine-srcRowBytes+srcPixelBytes) - *(T*)(srcLine+srcRowBytes-srcPixelBytes) - 2 * *(T*)(srcLine+srcPixelBytes) - *(T*)(srcLine+srcRowBytes+srcPixelBytes);
			sumX = static_cast<typename CHANTRAIT<T>::Sum>( math<float>::sqrt( float( sumX * sumX + sumY * sumY ) ) );
			if( sumX > maxValue ) sumX = maxValue;
			*dstLine = static_cast<uint8_t>( sumX );
			dstLine += dstPixelBytes;
			srcLine += srcPixelBytes;
		}
	}
}

template<typename T>
void edgeDetectSobel( const SurfaceT<T> &srcSurface, const Area &srcArea, const Vec2i &dstLT, SurfaceT<T> *dstSurface )
{
	edgeDetectSobel( srcSurface.getChannelRed(), srcArea, dstLT, &dstSurface->getChannelRed() );
	edgeDetectSobel( srcSurface.getChannelGreen(), srcArea, dstLT, &dstSurface->getChannelGreen() );
	edgeDetectSobel( srcSurface.getChannelBlue(), srcArea, dstLT, &dstSurface->getChannelBlue() );
	if( srcSurface.hasAlpha() && dstSurface->hasAlpha() )
		edgeDetectSobel( srcSurface.getChannelAlpha(), srcArea, dstLT, &dstSurface->getChannelAlpha() );
}

template<typename T>
void edgeDetectSobel( const ChannelT<T> &srcChannel, ChannelT<T> *dstChannel )
{
	edgeDetectSobel( srcChannel, srcChannel.getBounds(), Vec2i::zero(), dstChannel );
}

template<typename T>
void edgeDetectSobel( const SurfaceT<T> &srcSurface, SurfaceT<T> *dstSuface )
{
	edgeDetectSobel( srcSurface, srcSurface.getBounds(), Vec2i::zero(), dstSuface );
}

#define edgeDetect_PROTOTYPES(r,data,T)\
	template void edgeDetectSobel( const ChannelT<T> &srcChannel, const Area &srcArea, const Vec2i &dstLT, ChannelT<T> *dstChannel ); \
	template void edgeDetectSobel( const SurfaceT<T> &srcSurface, const Area &srcArea, const Vec2i &dstLT, SurfaceT<T> *dstSurface ); \
	template void edgeDetectSobel( const ChannelT<T> &srcChannel, ChannelT<T> *dstChannel );	\
	template void edgeDetectSobel( const SurfaceT<T> &srcSurface, SurfaceT<T> *dstSurface );

BOOST_PP_SEQ_FOR_EACH( edgeDetect_PROTOTYPES, ~, CHANNEL_TYPES )

} } // namespace cinder::ip


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

  Distributed under the Boost Software License, Version 1.0. (See accompanying
  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#ifndef SPRIG_TMP_BIT_TRAITS_HPP
#define SPRIG_TMP_BIT_TRAITS_HPP

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <cstddef>
#include <boost/mpl/void.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>

namespace sprig {
	namespace tmp {
		//
		// bit_traits
		//
		template<
			typename T1 = boost::mpl::void_,
			typename T2 = boost::mpl::void_,
			typename T3 = boost::mpl::void_,
			typename T4 = boost::mpl::void_,
			typename T5 = boost::mpl::void_,
			typename T6 = boost::mpl::void_,
			typename T7 = boost::mpl::void_,
			typename T8 = boost::mpl::void_
		>
		struct bit_traits {
		public:
			typedef std::size_t value_type;
		private:
			template<typename T, value_type N>
			struct correction
				: public boost::mpl::size_t<
					boost::mpl::if_<
						typename boost::is_same<T, boost::mpl::void_>::type,
						boost::mpl::size_t<0>,
						boost::mpl::size_t<1>
					>::type::value * N
				>
			{};
		public:
			static const value_type value
				= correction<T1, 0x0001>::value
				+ correction<T2, 0x0002>::value
				+ correction<T3, 0x0004>::value
				+ correction<T4, 0x0008>::value
				+ correction<T5, 0x0010>::value
				+ correction<T6, 0x0020>::value
				+ correction<T7, 0x0040>::value
				+ correction<T8, 0x0080>::value
				;
		public:
			template<value_type N>
			struct and_c
				: public boost::mpl::bool_<N & value>
			{};
			template<typename T>
			struct and_
				: public and_c<T::value>
			{};
			template<value_type N>
			struct xor_c
				: public boost::mpl::bool_<N ^ value>
			{};
			template<typename T>
			struct xor_
				: public xor_c<T::value>
			{};
			template<value_type N>
			struct or_c
				: public boost::mpl::bool_<N | value>
			{};
			template<typename T>
			struct or_
				: public xor_c<T::value>
			{};
		};
	}	// namespace tmp
}	// namespace sprig

#endif	// #ifndef SPRIG_TMP_BIT_TRAITS_HPP


#include <ros/ros.h>
#include<nav_msgs/Path.h>
#include<nav_msgs/Odometry.h>
#include<tf/transform_broadcaster.h>
#include<visualization_msgs/Marker.h>
#include<visualization_msgs/MarkerArray.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/PoseWithCovarianceStamped.h>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <vector>
#include <string>
#include <iostream>

#include <utils.hh>
#include "pose_graph_visualization/PoseGraph.h"

using namespace std;

/*
  This node listens to PoseGraph a topic which has
  a PoseArray and Connectivity Matrix. It outputs
  two marker arrays one of which is for the poses
  and the other for the edges between poses.
*/

double refresh_rate;
bool   debug_mode;
Eigen::Vector4d edge_color;
double axis_length;
double edge_thickness;

ros::Publisher  pose_array_publ,
                edge_array_publ;
ros::Subscriber pose_graph_subs;

pose_graph_visualization::PoseGraph pose_graph_msg;
visualization_msgs::Marker pose_array_msg;
visualization_msgs::Marker edge_array_msg;

void publish_pose_array();
void publish_edge_array();

void pose_graph_callback(const pose_graph_visualization::PoseGraph& msg);

void process_inputs(const ros::NodeHandle &n);
void setup_messaging_interface(ros::NodeHandle &n);
void loop(const ros::NodeHandle &n);

int main(int argc, char* argv[]){

	ros::init(argc, argv, "pose_graph_visualization_node");
	ros::NodeHandle n("~");

	process_inputs(n);
	setup_messaging_interface(n);
	loop(n);

	return 1;
}

void process_inputs(const ros::NodeHandle &n){

	n.param("refresh_rate", refresh_rate, 100.0);
	n.param("debug_mode", debug_mode, false);

	n.param("edge_color_r"  , edge_color(0), 1.0);
	n.param("edge_color_g"  , edge_color(1), 0.0);
	n.param("edge_color_b"  , edge_color(2), 1.0);
	n.param("edge_color_a"  , edge_color(3), 1.0);
	n.param("axis_length"   , axis_length  , 1.0);
	n.param("edge_thickness", edge_thickness, 1.0);

	ROS_INFO(" ---------- POSE GRAPH VISUALIZATION ------------");
	ROS_INFO("[refresh_rate] ------ : [%.3f - (doesn't run on fixed rate)]", refresh_rate);
	ROS_INFO("[debug_mode] -------- : [%s]", debug_mode ? "TRUE" : "FALSE");
	ROS_INFO("[edge_color (rgba)] - : [%.3f, %.3f, %.3f, %.3f]", edge_color(0), edge_color(1), edge_color(2), edge_color(3));
	ROS_INFO("[edge_thickness] ---- : [%.3f]", edge_thickness);
	ROS_INFO("[axis_length] ------- : [%.3f]", axis_length);
	ROS_INFO(" ----------------------------------------");
}

void setup_messaging_interface(ros::NodeHandle &n)
{
  pose_array_publ = n.advertise<visualization_msgs::Marker>("pose_graph/poses", 10);
  edge_array_publ = n.advertise<visualization_msgs::Marker>("pose_graph/edges", 10);
  pose_graph_subs = n.subscribe("pose_graph", 10, pose_graph_callback, ros::TransportHints().tcpNoDelay());
}

void loop(const ros::NodeHandle &n)
{
	ros::spin();
}

void pose_graph_callback(const pose_graph_visualization::PoseGraph& msg){
	if(debug_mode)
		ROS_INFO("POSE GRAPH VISUALIZATION : Got PoseGraph Data");

  pose_graph_msg = msg;
	publish_pose_array();
	publish_edge_array();
}

void publish_pose_array(){
	if(debug_mode)
		ROS_INFO("POSE GRAPH VISUALIZATION : Publish Pose Data");

  static bool initialized = false;

  pose_array_msg.header.stamp = ros::Time::now();
  pose_array_msg.header.frame_id = "world";
  pose_array_msg.header.seq++;

  if(initialized == false){
    pose_array_msg.ns = "pose_graph/edges";
    pose_array_msg.id = 0;
    pose_array_msg.type = visualization_msgs::Marker::LINE_LIST;
    pose_array_msg.action = visualization_msgs::Marker::ADD;
    pose_array_msg.pose.position.x = 0;
    pose_array_msg.pose.position.y = 0;
    pose_array_msg.pose.position.z = 0;
    pose_array_msg.pose.orientation.w = 1;
    pose_array_msg.pose.orientation.x = 0;
    pose_array_msg.pose.orientation.y = 0;
    pose_array_msg.pose.orientation.z = 0;
    pose_array_msg.color.r = 1;
    pose_array_msg.color.g = 1;
    pose_array_msg.color.b = 1;
    pose_array_msg.color.a = 1;
    pose_array_msg.scale.x = .05;
    pose_array_msg.scale.y = .05;
    pose_array_msg.scale.z = .05;
    pose_array_msg.lifetime = ros::Duration(0);
    pose_array_msg.frame_locked = true;
  }

  initialized = true;

  int num_poses = pose_graph_msg.poses.size();
  pose_array_msg.points.resize(6 * num_poses);
  pose_array_msg.colors.resize(6 * num_poses);

  Eigen::Matrix3d dcm;

  for(int i = 0 ; i < num_poses ; i++){
    geometry_msgs::Pose pose = pose_graph_msg.poses[i];
    dcm = axis_length * utils::trans::pose2se3(pose).topLeftCorner<3, 3>();

    pose_array_msg.points[6 * i + 0].x = pose.position.x;
    pose_array_msg.points[6 * i + 0].y = pose.position.y;
    pose_array_msg.points[6 * i + 0].z = pose.position.z;
    pose_array_msg.points[6 * i + 1].x = pose.position.x + dcm(0, 0);
    pose_array_msg.points[6 * i + 1].y = pose.position.y + dcm(1, 0);
    pose_array_msg.points[6 * i + 1].z = pose.position.z + dcm(2, 0);
    pose_array_msg.points[6 * i + 2].x = pose.position.x;
    pose_array_msg.points[6 * i + 2].y = pose.position.y;
    pose_array_msg.points[6 * i + 2].z = pose.position.z;
    pose_array_msg.points[6 * i + 3].x = pose.position.x + dcm(0, 1);
    pose_array_msg.points[6 * i + 3].y = pose.position.y + dcm(1, 1);
    pose_array_msg.points[6 * i + 3].z = pose.position.z + dcm(2, 1);
    pose_array_msg.points[6 * i + 4].x = pose.position.x;
    pose_array_msg.points[6 * i + 4].y = pose.position.y;
    pose_array_msg.points[6 * i + 4].z = pose.position.z;
    pose_array_msg.points[6 * i + 5].x = pose.position.x + dcm(0, 2);
    pose_array_msg.points[6 * i + 5].y = pose.position.y + dcm(1, 2);
    pose_array_msg.points[6 * i + 5].z = pose.position.z + dcm(2, 2);

    pose_array_msg.colors[6 * i + 0].r = 1;
    pose_array_msg.colors[6 * i + 0].g = 0;
    pose_array_msg.colors[6 * i + 0].b = 0;
    pose_array_msg.colors[6 * i + 0].a = 1;
    pose_array_msg.colors[6 * i + 1].r = 1;
    pose_array_msg.colors[6 * i + 1].g = 0;
    pose_array_msg.colors[6 * i + 1].b = 0;
    pose_array_msg.colors[6 * i + 1].a = 1;
    pose_array_msg.colors[6 * i + 2].r = 0;
    pose_array_msg.colors[6 * i + 2].g = 1;
    pose_array_msg.colors[6 * i + 2].b = 0;
    pose_array_msg.colors[6 * i + 2].a = 1;
    pose_array_msg.colors[6 * i + 3].r = 0;
    pose_array_msg.colors[6 * i + 3].g = 1;
    pose_array_msg.colors[6 * i + 3].b = 0;
    pose_array_msg.colors[6 * i + 3].a = 1;
    pose_array_msg.colors[6 * i + 4].r = 0;
    pose_array_msg.colors[6 * i + 4].g = 0;
    pose_array_msg.colors[6 * i + 4].b = 1;
    pose_array_msg.colors[6 * i + 4].a = 1;
    pose_array_msg.colors[6 * i + 5].r = 0;
    pose_array_msg.colors[6 * i + 5].g = 0;
    pose_array_msg.colors[6 * i + 5].b = 1;
    pose_array_msg.colors[6 * i + 5].a = 1;
  }

	pose_array_publ.publish(pose_array_msg);
}

void publish_edge_array(){
	if(debug_mode)
		ROS_INFO("POSE GRAPH VISUALIZATION : Publish Edge Data");

  static bool initialized = false;

  edge_array_msg.header.stamp = ros::Time::now();
  edge_array_msg.header.frame_id = "world";
  edge_array_msg.header.seq++;

  if(initialized == false){
    edge_array_msg.ns = "pose_graph/edges";
    edge_array_msg.id = 0;
    edge_array_msg.type = visualization_msgs::Marker::LINE_LIST;
    edge_array_msg.action = visualization_msgs::Marker::ADD;
    edge_array_msg.pose.position.x = 0;
    edge_array_msg.pose.position.y = 0;
    edge_array_msg.pose.position.z = 0;
    edge_array_msg.pose.orientation.w = 1;
    edge_array_msg.pose.orientation.x = 0;
    edge_array_msg.pose.orientation.y = 0;
    edge_array_msg.pose.orientation.z = 0;
    edge_array_msg.color.r = edge_color(0);
    edge_array_msg.color.g = edge_color(1);
    edge_array_msg.color.b = edge_color(2);
    edge_array_msg.color.a = edge_color(3);
    edge_array_msg.scale.x = .03;
    edge_array_msg.scale.y = .03;
    edge_array_msg.scale.z = .03;
    edge_array_msg.lifetime = ros::Duration(0);
    edge_array_msg.frame_locked = true;
  }

  initialized = true;

  int num_edges = pose_graph_msg.edges.size() / 2;
  edge_array_msg.points.resize(2 * num_edges);

  for(int i = 0 ; i < num_edges ; i++){
    int id1, id2;
    id1 = pose_graph_msg.edges[2 * i];
    id2 = pose_graph_msg.edges[2 * i + 1];
    geometry_msgs::Pose pose1 = pose_graph_msg.poses[id1];
    geometry_msgs::Pose pose2 = pose_graph_msg.poses[id2];
    edge_array_msg.points[2 * i].x = pose1.position.x;
    edge_array_msg.points[2 * i].y = pose1.position.y;
    edge_array_msg.points[2 * i].z = pose1.position.z;
    edge_array_msg.points[2 * i + 1].x = pose2.position.x;
    edge_array_msg.points[2 * i + 1].y = pose2.position.y;
    edge_array_msg.points[2 * i + 1].z = pose2.position.z;
  }

	edge_array_publ.publish(edge_array_msg);
}


#ifndef VOFEATURES_HPP_
#define VOFEATURES_HPP_

#include <iostream>
#include <stdio.h>
#include <signal.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>

#include <lcm/lcm-cpp.hpp>
#include <fovis/fovis.hpp>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "imagefeatures.hpp"
#include <pronto_utils/pronto_vis.hpp> // visualize pt clds

#include <lcmtypes/motion_estimate.hpp>
#include <lcmtypes/bot_core_image_t.h>

class VoFeatures
{
public:
  VoFeatures(boost::shared_ptr<lcm::LCM> &lcm_, int image_width_, int image_height_);
  ~VoFeatures();

  void setFeatures(const fovis::FeatureMatch* matches, int num_matches, int64_t utime);

  void setReferenceImage(uint8_t *left_ref_buf){
    left_ref_buf_ = left_ref_buf;
  }
  void setCurrentImage(uint8_t *left_cur_buf){
    left_cur_buf_ = left_cur_buf;
  }

  /*
  void setReferenceImages(uint8_t *left_ref_buf,uint8_t *right_ref_buf){
    left_ref_buf_ = left_ref_buf;
    right_ref_buf_ = right_ref_buf;
  }
  void setCurrentImages(uint8_t *left_cur_buf,uint8_t *right_cur_buf){
    left_cur_buf_ = left_cur_buf;
    right_cur_buf_ = right_cur_buf;
  }
  */

  void setReferenceCameraPose(Eigen::Isometry3d ref_camera_pose){
    ref_camera_pose_ = ref_camera_pose;
  }
  void setCurrentCameraPose(Eigen::Isometry3d cur_camera_pose){
    cur_camera_pose_ = cur_camera_pose;
  }

  void doFeatureProcessing(int reference_or_current);
  void sendImage(std::string channel, int which_image );
private:
  boost::shared_ptr<lcm::LCM> lcm_;
  pronto_vis* pc_vis_;
  int image_width_;
  int image_height_;
  int output_counter_;

  void writeReferenceImages();
  void writeFeatures(std::vector<ImageFeature> features);
  void writePose();
  void sendFeatures(std::vector<ImageFeature> features,
                    std::vector<int> features_indices, std::string channel);
  void sendFeaturesAsCollection(std::vector<ImageFeature> features,
                                std::vector<int> features_indices,
                                int vs_id);

  void drawFeaturesOnImage(cv::Mat &img, int which_image );

  // All the incoming data and state:
  Eigen::Isometry3d ref_camera_pose_, cur_camera_pose_;
  int64_t utime_;
  // pointers to reference image: (only used of visual output):
  uint8_t *left_ref_buf_;
  std::vector<ImageFeature> features_ref_;
  std::vector<int> features_ref_indices_; // 0 outlier, 1 inlier
  // pointers to reference image: (only used of visual output):
  uint8_t *left_cur_buf_;
  std::vector<ImageFeature> features_cur_;
  std::vector<int> features_cur_indices_;

  // no longer used:
  // uint8_t *right_ref_buf_, *right_cur_buf_;
};

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_COMPARE_LESS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_COMPARE_LESS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/meta/as_logical.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(compare_less_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
     using sA0 =  bd::scalar_of_t<A0>;
     BOOST_FORCEINLINE logical<sA0> operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
     {
        for(std::size_t i=0;i<bs::cardinal_of<A0>::value;++i)
        {
          if (a0[i] < a1[i])  return {true};
          if (a1[i] < a0[i])  return {false};
        }
          return {false};
      }
   };

} } }

#endif


/**
 *
 * RenderPipeline
 *
 * Copyright (c) 2014-2016 tobspr <tobias.springer1@gmail.com>
 *
 * 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.
 *
 */

// Only include the pssm helper if actually required
#ifdef RP_REQ_PSSM_HELPER

#include "pssm_helper.h"

#include <Eigen/Dense>

/**
 * @brief Internal method to generate a set of equations
 * @details This generates a set of equations into a given equation system.
 *   Based on a given start point and end point, it inserts an equation to the
 *   system which will, when solved, solve the following equation:
 *   vec * <SOLVED-TRANSFORM> = expected
 *
 * @param eq_system The equation system
 * @param eq_results The target result vector of the equation system
 * @param vec The actual vector to be transformed
 * @param expected The expected output of the transformation
 * @param offset The index of the equation
 */
void generate_equations(Eigen::MatrixXf &eq_system, Eigen::VectorXf &eq_results, const LVector4f &vec, LVector4f expected, size_t offset) {
    size_t write_offset = offset * 4;
    for (size_t row = 0; row < 4; ++row) {
        float expected_coeff = expected.get_cell(row);
        size_t col_offset = row * 4;
        for (size_t col = 0; col < 4; ++col) {
            eq_system(write_offset, col_offset + col) = vec.get_cell(col);
        }
        eq_results(write_offset++) = expected_coeff;
    }
}
/**
 * @brief Finds a projection mat arround the given set of points.
 * @details This methods finds a projection matrix which projects the given set
 *   of frustum points to a unit cube, which can be used as a camera matrix.
 *   The eight points should determine the frustum uniquely.
 *
 *  @param near_ul The Upper-Left point of the frustum on the near plane
 *  @param near_ur The Upper-Right point of the frustum on the near plane
 *  @param near_ll The Lower-Left point of the frustum on the near plane
 *  @param near_lr The Lower-Right point of the frustum on the near plane
 *  @param far_ul The Upper-Left point of the frustum on the far plane
 *  @param far_ur The Upper-Right point of the frustum on the far plane
 *  @param far_ll The Lower-Left point of the frustum on the far plane
 *  @param far_lr The Lower-Right point of the frustum on the far plane
 *
 */
LMatrix4f PSSMHelper::find_projection_mat(
            const LVector4f &near_ul,
            const LVector4f &near_ur,
            const LVector4f &near_ll,
            const LVector4f &near_lr,

            const LVector4f &far_ul,
            const LVector4f &far_ur,
            const LVector4f &far_ll,
            const LVector4f &far_lr) {

    // We have 8*4 = 32 equations, which require 16 coefficients each
    Eigen::MatrixXf equation_system(32, 16);
    Eigen::VectorXf equation_results(32);
    equation_system.fill(0);

    // Generate the equations
    size_t offset = 0;
    generate_equations(equation_system, equation_results, near_ul, LVector4f(-1,  1, 0, 1), offset++);
    generate_equations(equation_system, equation_results, near_ur, LVector4f( 1,  1, 0, 1), offset++);
    generate_equations(equation_system, equation_results, near_ll, LVector4f(-1, -1, 0, 1), offset++);
    generate_equations(equation_system, equation_results, near_lr, LVector4f( 1, -1, 0, 1), offset++);

    generate_equations(equation_system, equation_results, far_ul,  LVector4f(-1,  1, 1, 1), offset++);
    generate_equations(equation_system, equation_results, far_ur,  LVector4f( 1,  1, 1, 1), offset++);
    generate_equations(equation_system, equation_results, far_ll,  LVector4f(-1, -1, 1, 1), offset++);
    generate_equations(equation_system, equation_results, far_lr,  LVector4f( 1, -1, 1, 1), offset++);

    // Solve the equation system
    Eigen::VectorXf solved_system = equation_system.colPivHouseholderQr().solve(equation_results);

    // Construct result matrix and return it. We also need to transpose the matrix.
    LMatrix4f result(
            solved_system(0), solved_system(4), solved_system(8),  solved_system(12),
            solved_system(1), solved_system(5), solved_system(9),  solved_system(13),
            solved_system(2), solved_system(6), solved_system(10), solved_system(14),
            solved_system(3), solved_system(7), solved_system(11), solved_system(15)
        );
    return result;
}

#endif // RP_REQ_PSSM_HELPER