/**
    @file
    @author  Alexander Sherikov

    @copyright 2017 Alexander Sherikov. Licensed under the Apache License,
    Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0)

    @brief
*/

#include "utf_common.h"

#include <boost/mpl/vector.hpp>

#include <qpmad/solver.h>
#include <qpmad/testing.h>

//===========================================================================
// SolverObjectiveFixture
//===========================================================================

#define QPMAD_USING_PARENT_FIXTURE(Parent)                                                                             \
    using Parent::x;                                                                                \
    using Parent::H;                                                                                \
    using Parent::H_copy;                                                                                \
    using Parent::h;                                                                                \
    using Parent::A;                                                                                \
    using Parent::Alb;                                                                                \
    using Parent::Aub;                                                                                \
    using Parent::lb;                                                                                \
    using Parent::ub;                                                                                \
    using Parent::solver;                                                                                \
    using Parent::status;                                                                                \
    using Parent::checkObjective;                                                                                \
    using Parent::initRandomHessian;

template <class t_Solver>
class SolverObjectiveFixture
{
public:
    Eigen::VectorXd x;
    Eigen::MatrixXd H;
    Eigen::MatrixXd H_copy;
    Eigen::VectorXd h;
    Eigen::MatrixXd A;
    Eigen::VectorXd Alb;
    Eigen::VectorXd Aub;
    Eigen::VectorXd lb;
    Eigen::VectorXd ub;

    t_Solver solver;
    typename t_Solver::ReturnStatus status;

public:
    void checkObjective()
    {
        status = solver.solve(x, H, h, A, Alb, Aub);

        Eigen::VectorXd tmp;
        if (h.size() > 0)
        {
            tmp = H_copy * x + h;
        }
        else
        {
            tmp = H_copy * x;
        }

        BOOST_CHECK_EQUAL(status, qpmad::Solver::OK);
        BOOST_CHECK(tmp.norm() < g_default_tolerance);
    }

    void initRandomHessian(const qpmad::MatrixIndex size)
    {
        qpmad_utils::getRandomPositiveDefiniteMatrix(H, size);
        H_copy = H;
    }
};

template <int t_primal_size>
using TypeListObjective = boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, t_primal_size, 0, 0> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(objective00, t_Solver, TypeListObjective<50>, SolverObjectiveFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;

    this->initRandomHessian(size);
    this->h.setRandom(size);

    this->checkObjective();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(objective01, t_Solver, TypeListObjective<50>, SolverObjectiveFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;

    this->initRandomHessian(size);
    this->h.setZero(size);

    this->checkObjective();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(objective02, t_Solver, TypeListObjective<50>, SolverObjectiveFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;

    this->initRandomHessian(size);

    this->checkObjective();
}

// qpmad::SolverTemplate<double, 1, 0, 0>  does not compile due to Eigen asserts
BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        objective03,
        t_Solver,
        boost::mpl::vector<qpmad::Solver>,
        SolverObjectiveFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 1;

    this->initRandomHessian(size);
    this->h.setRandom(size);

    this->checkObjective();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        objective04_semidefinite,
        t_Solver,
        TypeListObjective<50>,
        SolverObjectiveFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;

    this->initRandomHessian(size);
    this->H(5, 5) = 0.0;  // break positive definiteness
    this->h.setRandom(size);

    BOOST_CHECK_THROW(this->checkObjective(), std::exception);
}

//===========================================================================
// SolverGeneralEqualitiesFixture
//===========================================================================

template <class t_Solver>
class SolverGeneralEqualitiesFixture : public SolverObjectiveFixture<t_Solver>
{
public:
    QPMAD_USING_PARENT_FIXTURE(SolverObjectiveFixture<t_Solver>)

public:
    void checkGeneralEqualities()
    {
        status = solver.solve(x, H, h, A, Alb, Alb);

        Eigen::MatrixXd Hi = H_copy.inverse();
        Eigen::VectorXd tmp;
        if (h.size() > 0)
        {
            tmp = Hi * A.transpose() * (A * Hi * A.transpose()).inverse() * (Alb + A * Hi * h) - Hi * h;
        }
        else
        {
            tmp = Hi * A.transpose() * (A * Hi * A.transpose()).inverse() * Alb;
        }

        BOOST_CHECK_EQUAL(status, qpmad::Solver::OK);
        BOOST_CHECK(x.isApprox(tmp, g_default_tolerance));
    }
};

template <int t_num_ctr>
using TypeListGeneralEqualities = boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, 50, 0, t_num_ctr> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_equalities00,
        t_Solver,
        TypeListGeneralEqualities<20>,
        SolverGeneralEqualitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;
    qpmad::MatrixIndex num_ctr = 20;

    this->initRandomHessian(size);
    this->h.setRandom(size);

    this->A = Eigen::MatrixXd::Identity(size, size).topRows(num_ctr);
    this->Alb.setRandom(num_ctr);

    this->checkGeneralEqualities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_equalities01,
        t_Solver,
        TypeListGeneralEqualities<20>,
        SolverGeneralEqualitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;
    qpmad::MatrixIndex num_ctr = 20;

    this->initRandomHessian(size);

    this->A = Eigen::MatrixXd::Identity(size, size).topRows(num_ctr);
    this->Alb.setRandom(num_ctr);

    this->checkGeneralEqualities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_equalities02,
        t_Solver,
        TypeListGeneralEqualities<50>,
        SolverGeneralEqualitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;
    qpmad::MatrixIndex num_ctr = 50;

    this->initRandomHessian(size);
    this->h.setRandom(size);

    this->A = Eigen::MatrixXd::Identity(size, size).topRows(num_ctr);
    this->Alb.setRandom(num_ctr);

    this->checkGeneralEqualities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_equalities03,
        t_Solver,
        TypeListGeneralEqualities<51>,
        SolverGeneralEqualitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;
    qpmad::MatrixIndex num_ctr = 51;

    this->initRandomHessian(size);
    this->h.setRandom(size);

    this->A.setRandom(num_ctr, size);
    this->A.topRows(size) += Eigen::MatrixXd::Identity(size, size);
    this->Alb.setRandom(num_ctr);
    this->Alb(num_ctr - 1) += 100.0;

    // Infeasible equalities
    BOOST_CHECK_THROW(
            this->status = this->solver.solve(this->x, this->H, this->h, this->A, this->Alb, this->Alb),
            std::exception);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_equalities04,
        t_Solver,
        TypeListGeneralEqualities<51>,
        SolverGeneralEqualitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 50;
    qpmad::MatrixIndex num_ctr = 51;

    this->initRandomHessian(size);
    this->h.setRandom(size);

    this->A.setZero(num_ctr, size);
    this->A.topRows(size) += Eigen::MatrixXd::Identity(size, size);
    this->A(num_ctr - 1, 0) = 100.0;
    this->Alb.resize(num_ctr);
    this->Alb.setConstant(1.0);
    this->Alb(num_ctr - 1) = 100.0;

    this->status = this->solver.solve(this->x, this->H, this->h, this->A, this->Alb, this->Alb);

    BOOST_CHECK_EQUAL(this->status, qpmad::Solver::OK);
    BOOST_CHECK(this->x.isApprox(Eigen::VectorXd::Ones(size), g_default_tolerance));
}

//===========================================================================
// SolverGeneralInequalitiesFixture
//===========================================================================

template <class t_Solver>
class SolverGeneralInequalitiesFixture : public SolverObjectiveFixture<t_Solver>
{
public:
    QPMAD_USING_PARENT_FIXTURE(SolverObjectiveFixture<t_Solver>)

public:
    Eigen::VectorXd x_ref;

public:
    void checkGeneralInequalities()
    {
        status = solver.solve(x, H, h, A, Alb, Aub);

        BOOST_CHECK_EQUAL(status, qpmad::Solver::OK);
        BOOST_CHECK(x.isApprox(x_ref, g_default_tolerance));
    }
};

template <int t_num_ctr>
using TypeListGeneralInequalities00 =
        boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, 2, 0, t_num_ctr> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_inequalities00,
        t_Solver,
        TypeListGeneralInequalities00<1>,
        SolverGeneralInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 2;
    qpmad::MatrixIndex num_ctr = 1;

    this->H.setIdentity(size, size);
    this->h.setOnes(size);

    this->A = Eigen::MatrixXd::Identity(size, size).topRows(1);
    this->Alb.resize(num_ctr);
    this->Aub.resize(num_ctr);
    this->Alb(0) = -10;
    this->Aub(0) = -5;

    this->x_ref.resize(size);
    this->x_ref << -5, -1;

    this->checkGeneralInequalities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_inequalities01,
        t_Solver,
        TypeListGeneralInequalities00<3>,
        SolverGeneralInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 2;
    qpmad::MatrixIndex num_ctr = 3;

    this->H.setIdentity(size, size);

    this->A.resize(num_ctr, size);
    this->A << 1, 0, 1, 1, 2, 0.5;
    this->Alb.resize(num_ctr);
    this->Aub.resize(num_ctr);
    this->Alb << 0, 1, 1;
    this->Aub << 0, 1e10, 1e10;

    this->x_ref.resize(size);
    this->x_ref << 0, 2;

    this->checkGeneralInequalities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_inequalities02,
        t_Solver,
        TypeListGeneralInequalities00<3>,
        SolverGeneralInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 2;
    qpmad::MatrixIndex num_ctr = 3;

    this->H.setIdentity(size, size);

    this->A.resize(num_ctr, size);
    this->A << 1, 0, 1, 1, 2, 0.5;
    this->Alb.resize(num_ctr);
    this->Aub.resize(num_ctr);
    this->Alb << 0, -1e10, -1e10;
    this->Aub << 0, -1, -1;

    this->x_ref.resize(size);
    this->x_ref << 0, -2;

    this->checkGeneralInequalities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_inequalities03,
        t_Solver,
        TypeListGeneralInequalities00<3>,
        SolverGeneralInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 2;
    qpmad::MatrixIndex num_ctr = 3;

    this->H.setIdentity(size, size);

    this->A.resize(num_ctr, size);
    this->A << 1, 0, -1, -1, -2, -0.5;
    this->Alb.resize(num_ctr);
    this->Aub.resize(num_ctr);
    this->Alb << 0, -1e10, -1e10;
    this->Aub << 0, -1, -1;

    this->x_ref.resize(size);
    this->x_ref << 0, 2;

    this->checkGeneralInequalities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_inequalities04,
        t_Solver,
        TypeListGeneralInequalities00<3>,
        SolverGeneralInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 2;
    qpmad::MatrixIndex num_ctr = 3;

    this->H.setIdentity(size, size);

    this->A.resize(num_ctr, size);
    this->A << 1, 0, -1, -1, -2, -0.5;
    this->Alb.resize(num_ctr);
    this->Aub.resize(num_ctr);
    this->Alb << 0, 1, 1;
    this->Aub << 0, 1e10, 1e10;

    this->x_ref.resize(size);
    this->x_ref << 0, -2;

    this->checkGeneralInequalities();
}

template <int t_num_ctr>
using TypeListGeneralInequalities01 =
        boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, 20, 0, t_num_ctr> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_inequalities20,
        t_Solver,
        TypeListGeneralInequalities01<20>,
        SolverGeneralInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 20;
    qpmad::MatrixIndex num_ctr = size;

    this->H.setIdentity(size, size);
    this->h.setOnes(size);

    this->A = Eigen::MatrixXd::Identity(num_ctr, size);
    this->Alb.resize(num_ctr);
    this->Aub.resize(num_ctr);
    this->Alb << 1, 2, 3, 4, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
            -0.5;
    this->Aub << 1, 2, 3, 4, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;

    this->x_ref.resize(size);
    this->x_ref << 1.0, 2.0, 3.0, 4.0, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
            -0.5, -0.5, -0.5;

    this->checkGeneralInequalities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        general_inequalities21,
        t_Solver,
        TypeListGeneralInequalities01<21>,
        SolverGeneralInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 20;
    qpmad::MatrixIndex num_ctr = size + 1;

    this->H.setIdentity(size, size);
    this->h.setOnes(size);

    this->A.resize(num_ctr, size);
    this->A.topRows(size) = Eigen::MatrixXd::Identity(size, size);
    this->A.row(num_ctr - 1).setOnes();
    this->Alb.resize(num_ctr);
    this->Aub.resize(num_ctr);
    this->Alb << 1, 2, 3, 4, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -1.5;
    this->Aub << 1, 2, 3, 4, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 1.5;

    this->x_ref.resize(size);
    this->x_ref << 1.0, 2.0, 3.0, 4.0, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875,
            -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875;
    this->checkGeneralInequalities();
}

//===========================================================================
// SolverSimpleInequalitiesFixture
//===========================================================================

template <class t_Solver>
class SolverSimpleInequalitiesFixture : public SolverObjectiveFixture<t_Solver>
{
public:
    QPMAD_USING_PARENT_FIXTURE(SolverObjectiveFixture<t_Solver>)

public:
    Eigen::VectorXd x_ref;

public:
    void checkSimpleInequalities()
    {
        status = solver.solve(x, H, h, lb, ub, A, Alb, Aub);

        BOOST_CHECK_EQUAL(status, qpmad::Solver::OK);
        BOOST_CHECK(x.isApprox(x_ref, g_default_tolerance));
    }
};

template <int t_primal_size>
using TypeListSolverSimpleInequalities00 =
        boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, t_primal_size, 1, 0> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        simple_inequalities00,
        t_Solver,
        TypeListSolverSimpleInequalities00<2>,
        SolverSimpleInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 2;

    this->H.setIdentity(size, size);
    this->h.setOnes(size);

    this->lb.resize(size);
    this->ub.resize(size);
    this->lb << -10, -g_infinity;
    this->ub << -5, g_infinity;

    this->x_ref.resize(size);
    this->x_ref << -5, -1;

    this->checkSimpleInequalities();
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        simple_inequalities20,
        t_Solver,
        TypeListSolverSimpleInequalities00<20>,
        SolverSimpleInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 20;

    this->H.setIdentity(size, size);
    this->h.setOnes(size);

    this->lb.resize(size);
    this->ub.resize(size);
    this->lb << 1, 2, 3, 4, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
            -0.5;
    this->ub << 1, 2, 3, 4, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;

    this->x_ref.resize(size);
    this->x_ref << 1.0, 2.0, 3.0, 4.0, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5,
            -0.5, -0.5, -0.5;

    this->checkSimpleInequalities();
}

using TypeListSolverSimpleInequalities01 = boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, 20, 1, 1> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        simple_inequalities21,
        t_Solver,
        TypeListSolverSimpleInequalities01,
        SolverSimpleInequalitiesFixture<t_Solver>)
{
    qpmad::MatrixIndex size = 20;
    qpmad::MatrixIndex num_eq_ctr = 1;

    this->H.setIdentity(size, size);
    this->h.setOnes(size);

    this->A.resize(num_eq_ctr, size);
    this->A.setOnes();
    this->Alb.resize(num_eq_ctr);
    this->Aub.resize(num_eq_ctr);
    this->Alb << -1.5;
    this->Aub << 1.5;

    this->lb.resize(size);
    this->ub.resize(size);
    this->lb << 1, 2, 3, 4, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5;
    this->ub << 1, 2, 3, 4, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;

    this->x_ref.resize(size);
    this->x_ref << 1.0, 2.0, 3.0, 4.0, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875,
            -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875, -0.71875;
    this->checkSimpleInequalities();

    BOOST_CHECK_EQUAL(1, this->solver.getNumberOfInequalityIterations());

    Eigen::VectorXd dual;
    Eigen::Matrix<qpmad::MatrixIndex, Eigen::Dynamic, 1> indices;
    Eigen::Matrix<bool, Eigen::Dynamic, 1> is_lower;

    this->solver.getInequalityDual(dual, indices, is_lower);
    qpmad::testing::printDualVariables(dual, indices, is_lower);

    BOOST_CHECK_EQUAL(1, dual.size());
}


//
// GraphTools library
// Copyright 2017-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Egor Dolzhenko <edolzhenko@illumina.com>,
//         Peter Krusche <pkrusche@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#include "graphalign/KmerIndexOperations.hh"

#include <list>

#include <boost/algorithm/string.hpp>

#include "graphutils/SequenceOperations.hh"

using std::list;
using std::string;

namespace graphtools
{
list<string> extractKmersFromAllPositions(const string& sequence, int32_t kmer_len)
{
    list<string> kmers;
    for (size_t pos = 0; pos + kmer_len <= sequence.length(); ++pos)
    {
        string kmer = sequence.substr(pos, static_cast<std::size_t>(kmer_len));
        boost::to_upper(kmer);
        kmers.push_back(kmer);
    }
    return kmers;
}

int32_t countKmerMatches(const KmerIndex& kmer_index, const std::string& seq)
{
    const list<string> kmers = extractKmersFromAllPositions(seq, kmer_index.kmerLength());
    int32_t num_kmer_matches = 0;

    for (const string& kmer : kmers)
    {
        if (kmer_index.numPaths(kmer) != 0)
        {
            ++num_kmer_matches;
        }
    }
    return num_kmer_matches;
}

bool checkIfForwardOriented(const KmerIndex& kmer_index, const std::string& sequence)
{
    const int32_t num_forward_matches = countKmerMatches(kmer_index, sequence);
    const int32_t num_revcomp_matches = countKmerMatches(kmer_index, reverseComplement(sequence));
    return num_forward_matches >= num_revcomp_matches;
}

/**
 * Find minimum kmer length that covers each node with a unique kmer
 * @param graph  a graph
 * @param min_unique_kmers_per_edge  min number of unique kmers to cover each edge
 * @param min_unique_kmers_per_node  min number of unique kmers to cover each node
 * @return
 */
int findMinCoveringKmerLength(Graph const* graph, size_t min_unique_kmers_per_edge, size_t min_unique_kmers_per_node)
{
    for (int32_t k = 10; k < 64; ++k)
    {
        KmerIndex index(*graph, k);

        bool any_below = false;
        for (NodeId node_id = 0; node_id != graph->numNodes(); ++node_id)
        {
            if (index.numUniqueKmersOverlappingNode(node_id) < min_unique_kmers_per_node)
            {
                any_below = true;
                break;
            }
            // this will enumerate all edges
            for (const auto succ : graph->successors(node_id))
            {
                if (index.numUniqueKmersOverlappingEdge(node_id, succ) < min_unique_kmers_per_edge)
                {
                    any_below = true;
                    break;
                }
            }
            if (any_below)
            {
                break;
            }
        }
        if (any_below)
        {
            continue;
        }

        return k;
    }
    return -1;
}
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <iostream>

#include <Eigen/Core>

#include "bi_operators/CollocationIntegrator.hpp"
#include "green/DerivativeTypes.hpp"
#include "cavity/GePolCavity.hpp"
#include "utils/Molecule.hpp"
#include "green/Vacuum.hpp"
#include "solver/IEFSolver.hpp"
#include "green/SphericalDiffuse.hpp"
#include "TestingMolecules.hpp"

SCENARIO("Test solver for the IEFPCM for a point charge in a spherical diffuse environment and a GePol cavity", "[solver][iefpcm][iefpcm_diffuse-gepol-point][anisotropic]")
{
    GIVEN("An isotropic environment modelled as a spherical diffuse permittivity")
    {
        Eigen::Vector3d origin = (Eigen::Vector3d() << 68.0375, -21.1234, 56.6198).finished();
        double eps1 = 78.39;
        double eps2 = 78.39;
        double center = 100.0;
        double width = 5.0;
        Vacuum<> gf_i;
        bool symm = true;

        double charge = 8.0;
        double totalASC = - charge * (eps1 - 1) / eps1;
        /*! \class IEFSolver
         *  \test \b pointChargeDiffuseGePol tests IEFSolver using a point charge with a GePol cavity and a spherical diffuse interface
         *  The spherical diffuse interface is centered at the origin, while the point charge is away from the origin.
         */
        WHEN("the spherical diffuse layer is centered at the origin and the charge is away from the origin")
        {
            Molecule point = dummy<0>(2.929075493, origin);
            double area = 1.0;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius);

            SphericalDiffuse<> gf_o(eps1, eps2, width, center, Eigen::Vector3d::Zero(), 3);
            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge, origin);
            for (int i = 0; i < size; ++i) {
                INFO("fake_mep(" << i << ") = " << fake_mep(i));
            }
            THEN("the apparent surface charge is")
            {
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                double totalFakeASC = fake_asc.sum();
                for (int i = 0; i < size; ++i) {
                    INFO("fake_asc(" << i << ") = " << fake_asc(i));
                }

                CAPTURE(totalASC);
                CAPTURE(totalFakeASC);
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        AND_WHEN("the spherical diffuse layers is centered away from the origin and the charge is at the origin")
        {
            Molecule point = dummy<0>(2.929075493);
            double area = 1.0;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity = GePolCavity(point, area, probeRadius, minRadius);

            SphericalDiffuse<> gf_o(eps1, eps2, width, center, origin, 3);

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);
            for (int i = 0; i < size; ++i) {
                INFO("fake_mep(" << i << ") = " << fake_mep(i));
            }
            THEN("the apparent surface charge is")
            {
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                double totalFakeASC = fake_asc.sum();
                for (int i = 0; i < size; ++i) {
                    INFO("fake_asc(" << i << ") = " << fake_asc(i));
                }

                CAPTURE(totalASC);
                CAPTURE(totalFakeASC);
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }
    }
}


#include <boost/graph/plod_generator.hpp>


/*!
@file
Forward declares `boost::hana::sum`.

@copyright Louis Dionne 2013-2016
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_HANA_FWD_SUM_HPP
#define BOOST_HANA_FWD_SUM_HPP

#include <boost/hana/config.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/fwd/integral_constant.hpp>

BOOST_HANA_NAMESPACE_BEGIN
    //! Compute the sum of the numbers of a structure.
    //! @ingroup group-Foldable
    //!
    //! More generally, `sum` will take any foldable structure containing
    //! objects forming a Monoid and reduce them using the Monoid's binary
    //! operation. The initial state for folding is the identity of the
    //! Monoid. It is sometimes necessary to specify the Monoid to use;
    //! this is possible by using `sum<M>`. If no Monoid is specified,
    //! the structure will use the Monoid formed by the elements it contains
    //! (if it knows it), or `integral_constant_tag<int>` otherwise. Hence,
    //! @code
    //!     sum<M>(xs) = fold_left(xs, zero<M or inferred Monoid>(), plus)
    //!     sum<> = sum<integral_constant_tag<int>>
    //! @endcode
    //!
    //! For numbers, this will just compute the sum of the numbers in the
    //! `xs` structure.
    //!
    //!
    //! @note
    //! The elements of the structure are not actually required to be in the
    //! same Monoid, but it must be possible to perform `plus` on any two
    //! adjacent elements of the structure, which requires each pair of
    //! adjacent element to at least have a common Monoid embedding. The
    //! meaning of "adjacent" as used here is that two elements of the
    //! structure `x` and `y` are adjacent if and only if they are adjacent
    //! in the linearization of that structure, as documented by the Iterable
    //! concept.
    //!
    //!
    //! Why must we sometimes specify the `Monoid` by using `sum<M>`?
    //! -------------------------------------------------------------
    //! This is because sequence tags like `tuple_tag` are not parameterized
    //! (by design). Hence, we do not know what kind of objects are in the
    //! sequence, so we can't know a `0` value of which type should be
    //! returned when the sequence is empty. Therefore, the type of the
    //! `0` to return in the empty case must be specified explicitly. Other
    //! foldable structures like `hana::range`s will ignore the suggested
    //! Monoid because they know the tag of the objects they contain. This
    //! inconsistent behavior is a limitation of the current design with
    //! non-parameterized tags, but we have no good solution for now.
    //!
    //!
    //! Example
    //! -------
    //! @include example/sum.cpp
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto sum = see documentation;
#else
    template <typename T, typename = void>
    struct sum_impl : sum_impl<T, when<true>> { };

    template <typename M>
    struct sum_t;

    template <typename M = integral_constant_tag<int>>
    constexpr sum_t<M> sum{};
#endif
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_FWD_SUM_HPP


#pragma once
#include <vector>
#include <map>
#include <memory>

#include "Variable.hpp"

#include "PysimTypes.hpp"

#include "boost/variant.hpp"

#include <Eigen/Dense>

namespace pysim {

struct VariablePrivate{
    std::map<std::string, double* > scalars;
    std::map<std::string, pysim::vector* > vectors;
    std::map<std::string, Eigen::MatrixXd* > matrices;
    std::map<std::string, std::string> descriptions;
};

}


//
// Created by jfajkowski on 19.01.19.
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE tests

#include <boost/test/unit_test.hpp>
#include "classifier.h"

BOOST_AUTO_TEST_CASE(row_test) {
    Vector a(std::vector<double>({10.0, 0.0, 0.0}));
    Vector b(std::vector<double>({0.0, 10.0, 0.0}));

    Vector expected = Vector(std::vector<double>({10.0, 10.0, 0.0}));
    Vector actual = a + b;
    BOOST_CHECK(actual == expected);

    expected = Vector(std::vector<double>({10.0, 10.0, 10.0}));
    actual += Vector(std::vector<double>({0.0, 0.0, 10.0}));
    BOOST_CHECK(actual == expected);

    expected = Vector(std::vector<double>({1.0, 1.0, 1.0}));
    actual /= 10.0;
    BOOST_CHECK(actual == expected);

    expected = Vector(std::vector<double>({1.0, 0.0, 0.0}));
    actual = a / 10.0;
    BOOST_CHECK(actual == expected);
}

BOOST_AUTO_TEST_CASE(matrix_test) {
    Matrix m;
    m.addCol(std::vector<double>({1.0, 0.0, 0.0}));
    m.addCol(std::vector<double>({0.0, 0.0, 1.0}));
    std::cout << m << std::endl;
    std::cout << m.getCol(0).normalize() << std::endl;
}

BOOST_AUTO_TEST_CASE(classifier_test) {
    Matrix X;
    X.addRow(std::vector<double>({1.0, 0.0}));
    X.addRow(std::vector<double>({0.0, 1.0}));

    Matrix y_true;
    y_true.addRow(std::vector<double>({1.0, 0.0}));
    y_true.addRow(std::vector<double>({0.0, 1.0}));

    Classifier classifier;
    classifier.fit(X, y_true);
    Matrix y_pred = classifier.predict(X);

    for (size_t i = 0; i < 2; ++i) {
        BOOST_CHECK(y_pred.getRow(i) == y_true.getRow(i));
    }
}


#include <boost/version.hpp>
#if ((BOOST_VERSION / 100) % 1000) >= 53
#include <boost/thread/mutex.hpp>
#include <boost/thread/lock_guard.hpp>
#endif

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

#include "opencv2/imgcodecs.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/videoio.hpp"
#include <opencv2/highgui.hpp>
#include <opencv2/video.hpp>

#include <ros/console.h>

namespace image_topic_to_image {

class ProcessStreamNodelet : public nodelet::Nodelet
{
  // ROS communication
  boost::shared_ptr<image_transport::ImageTransport> it_in_,it_out_;
  image_transport::Subscriber image_sub_;
  int queue_size_;
  cv_bridge::CvImageConstPtr source_ptr_;

  boost::mutex callback_mutex_;
  image_transport::Publisher image_pub_;

  cv::Mat image_sum_;
  bool image_sum_initialized_;

  virtual void onInit();

  void imageCb(const sensor_msgs::ImageConstPtr& image_msg);

};

void ProcessStreamNodelet::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();
  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  ros::NodeHandle nh_out(nh,"accumulated");
  it_in_.reset(new image_transport::ImageTransport(nh));
  it_out_.reset(new image_transport::ImageTransport(nh_out));

  // read parameters
  private_nh.param("queue_size",queue_size_,5);

  image_sum_initialized_ = false;

  image_pub_ = it_out_->advertise("image_raw",1);
  image_sub_ = it_in_->subscribe("/camera/blob_out/image_raw",queue_size_,&ProcessStreamNodelet::imageCb,this);
}

void ProcessStreamNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg)
{
  // get a cv::Mat view of the source data
  {
    boost::lock_guard<boost::mutex> lock(callback_mutex_);
    source_ptr_ = cv_bridge::toCvShare(image_msg,sensor_msgs::image_encodings::BGR8);
  }

  // input image
  cv::Mat image_input;
  cv::cvtColor(source_ptr_->image,image_input,CV_BGR2GRAY);

  if (image_sum_initialized_)
  {
    cv::add(image_sum_,image_input,image_sum_,cv::noArray(),CV_64F);
  }
  else
  {
    image_sum_ = image_input;
    image_sum_.convertTo(image_sum_,CV_64F);
    image_sum_initialized_ = true;
  }

  cv::Mat image_sum_normalized;
  normalize(image_sum_,image_sum_normalized,0,255,cv::NORM_MINMAX,CV_8U);

  // output image
  cv::Mat image_output;
  applyColorMap(image_sum_normalized,image_output,cv::COLORMAP_JET);

  sensor_msgs::ImagePtr image_msg_ptr = cv_bridge::CvImage(std_msgs::Header(),"bgr8",image_output).toImageMsg();
  image_pub_.publish(image_msg_ptr);

}

} // namespace image_topic_to_image

// register nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( image_topic_to_image::ProcessStreamNodelet,nodelet::Nodelet)


/********************************************************************************
Copyright (c) 2015, TRACLabs, Inc.
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice,
       this list of conditions and the following disclaimer.

    2. 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.

    3. Neither the name of the copyright holder 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 TRAC_IK_HPP
#define TRAC_IK_HPP

#include <trac_ik/nlopt_ik.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <boost/thread.hpp>
#include <boost/asio.hpp>

namespace TRAC_IK {

  enum SolveType { Speed, Distance, Manip1, Manip2 };

  class TRAC_IK
  {
  public:
    TRAC_IK(const KDL::Chain& _chain, const KDL::JntArray& _q_min, const KDL::JntArray& _q_max, double _maxtime=0.005, double _eps=1e-5, SolveType _type=Speed);

    ~TRAC_IK();

    static double JointErr(const KDL::JntArray& arr1, const KDL::JntArray& arr2) {
      double err = 0;
      for (uint i=0; i<arr1.data.size(); i++) {
        err += pow(arr1(i) - arr2(i),2);
      }

      return err;
    }

    int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist& bounds=KDL::Twist::Zero());

  private:
    KDL::Chain chain;
    KDL::ChainJntToJacSolver jacsolver;
    double eps;
    double maxtime;
    SolveType solvetype;

    NLOPT_IK::NLOPT_IK nl_solver;
    KDL::ChainIkSolverPos_TL iksolver;

    boost::posix_time::ptime start_time;

    bool runKDL(const KDL::JntArray &q_init, const KDL::Frame &p_in);

    bool runNLOPT(const KDL::JntArray &q_init, const KDL::Frame &p_in);

    void normalize_seed(const KDL::JntArray& seed, KDL::JntArray& solution);
    void normalize_limits(const KDL::JntArray& seed, KDL::JntArray& solution);

    std::vector<double> lb, ub;

    std::vector<KDL::BasicJointType> types;

    boost::mutex mtx_;
    std::vector<KDL::JntArray> solutions;
    std::vector<std::pair<double,uint> >  errors;

    boost::asio::io_service io_service;
    boost::thread_group threads;
    boost::asio::io_service::work work;
    KDL::Twist bounds;

    bool unique_solution(const KDL::JntArray& sol);

    inline static double fRand(double min, double max)
    {
      double f = (double)rand() / RAND_MAX;
      return min + f * (max - min);
    }

    double manipPenalty(const KDL::JntArray&);
    double ManipValue1(const KDL::JntArray&);
    double ManipValue2(const KDL::JntArray&);

    inline bool myEqual(const KDL::JntArray& a, const KDL::JntArray& b) {
      return (a.data-b.data).isZero(1e-4);
    }

  };

}

#endif


/**
 * \ file GainMaxExpanderFilter.cpp
 */

#include <iostream>

#include <ATK/Dynamic/GainMaxExpanderFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

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

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_softness_test )
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  filter.set_softness(0.5);
  BOOST_CHECK_EQUAL(filter.get_softness(), 0.5);
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_softness_range_test )
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  BOOST_CHECK_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_maxreduc_test )
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  filter.set_max_reduction(0.5);
  BOOST_CHECK_EQUAL(filter.get_max_reduction(), 0.5);
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_maxreduc_db_test )
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  filter.set_max_reduction_db(20);
  BOOST_CHECK_EQUAL(filter.get_max_reduction(), 100);
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_maxreduc_range_test )
{
  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter;
  BOOST_CHECK_THROW(filter.set_max_reduction(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_0_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(outdata[i], 0.f, 1.f);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_threshold_2_ratio_2_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.set_max_reduction(0.001);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.700553358, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_threshold_2_ratio_4_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.389224231, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_threshold_2_ratio_4_max_reduction_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainMaxExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(0.0001);
  filter.set_max_reduction(.1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_GE(outdata[i], .1 - std::numeric_limits<float>::epsilon());
  }
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_absolute_pose_with_known_orientation.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/rotation.h>
#include <vector>

#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/position_from_two_rays.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

// Rotate the correspondences to be in the world space instead of camera
// coordinates.
void RotateCorrespondences(
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    const Eigen::Vector3d& camera_orientation,
    std::vector<FeatureCorrespondence2D3D>* rotated_correspondences) {
  Eigen::Matrix3d camera_to_world_rotation;
  ceres::AngleAxisToRotationMatrix(
      camera_orientation.data(),
      ceres::ColumnMajorAdapter3x3(camera_to_world_rotation.data()));
  camera_to_world_rotation.transposeInPlace();

  rotated_correspondences->resize(normalized_correspondences.size());
  for (int i = 0; i < normalized_correspondences.size(); i++) {
    const Eigen::Vector3d feature_ray =
        camera_to_world_rotation *
        normalized_correspondences[i].feature.homogeneous();
    (*rotated_correspondences)[i].feature = feature_ray.hnormalized();
    (*rotated_correspondences)[i].world_point =
        normalized_correspondences[i].world_point;
  }
}

// An estimator for computing the absolute pose from 2 feature
// correspondences. The feature correspondences should be normalized by the
// focal length with the principal point at (0, 0) and rotated into the same
// coordinate system as the points.
class AbsolutePoseWithKnownOrientationEstimator
    : public Estimator<FeatureCorrespondence2D3D, Eigen::Vector3d> {
 public:
  AbsolutePoseWithKnownOrientationEstimator() {}

  // 2 correspondences are needed to determine the absolute position.
  double SampleSize() const { return 2; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence2D3D>& correspondences,
      std::vector<Eigen::Vector3d>* absolute_positions) const {
    Eigen::Vector3d position;
    if (!PositionFromTwoRays(correspondences[0].feature,
                             correspondences[0].world_point,
                             correspondences[1].feature,
                             correspondences[1].world_point,
                             &position)) {
      return false;
    }

    absolute_positions->emplace_back(position);
    return true;
  }

  // The error for a correspondences given an absolute position. This is the
  // squared reprojection error.
  double Error(const FeatureCorrespondence2D3D& correspondence,
               const Eigen::Vector3d& absolute_position) const {
    // The reprojected point is computed as X - c where c is the position, and X
    // is the 3D point.
    const Eigen::Vector2d reprojected_feature =
        (correspondence.world_point - absolute_position).hnormalized();
    return (reprojected_feature - correspondence.feature).squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(AbsolutePoseWithKnownOrientationEstimator);
};

}  // namespace

// Estimates the absolute pose with known orientation. It is assumed that the 2D
// features in the 2D-3D correspondences are normalized by the camera
// intrinsics. Returns true if the position could be successfully estimated and
// false otherwise. The quality of the result depends on the quality of the
// input data.
bool EstimateAbsolutePoseWithKnownOrientation(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const Eigen::Vector3d& camera_orientation,
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    Eigen::Vector3d* camera_position,
    RansacSummary* ransac_summary) {
  // Rotate the correspondences.
  std::vector<FeatureCorrespondence2D3D> rotated_correspondences;
  RotateCorrespondences(
      normalized_correspondences, camera_orientation, &rotated_correspondences);

  AbsolutePoseWithKnownOrientationEstimator absolute_pose_estimator;
  std::unique_ptr<
      SampleConsensusEstimator<AbsolutePoseWithKnownOrientationEstimator> >
      ransac = CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, absolute_pose_estimator);
  // Estimate the absolute pose.
  return ransac->Estimate(
      rotated_correspondences, camera_position, ransac_summary);
}

}  // namespace theia


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/FileIO.h>
#include <vw/Image.h>
#include <vw/Cartography.h>
#include <photk/Macros.h>
#include <photk/Common.h>
#include <boost/foreach.hpp>

using namespace vw;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

struct Options : photk::BaseOptions {
  Options() : nodata(std::numeric_limits<double>::max()), threshold(-1), percent(-1) {}
  // Input
  std::vector<std::string> input_files;
  double nodata, threshold, percent;
  bool feather;
};

// Operational Code
template <class PixelT>
void shadow_mask_nodata( Options& opt,
                         std::string input,
                         std::string output ) {
  double session_nodata = opt.nodata;
  if ( session_nodata == std::numeric_limits<double>::max() ) {
    DiskImageResource *rsrc = DiskImageResource::open(input);
    if ( rsrc->has_nodata_read() ) {
      session_nodata = rsrc->nodata_read();
      vw_out() << "\t--> Using nodata value: " << session_nodata << "\n";
    } else {
      vw_throw( ArgumentErr() << "Missing user supplied nodata value" );
    }
    delete rsrc;
  }
  typedef typename PixelChannelType<PixelT>::type ChannelT;
  typedef typename MaskedPixelType<PixelT>::type PMaskT;

  cartography::GeoReference georef;
  cartography::read_georeference(georef, input);
  DiskImageView<PixelT> input_image(input);
  ImageViewRef<PMaskT> masked_input =
    create_mask(input_image, boost::numeric_cast<ChannelT>(session_nodata));

  double session_threshold = opt.threshold;
  if ( opt.threshold < 0 && opt.percent < 0 ) {
    ChannelRange<ChannelT> helper;
    session_threshold = 0.1*helper.max();
  } else if ( opt.percent > 0 ) {
    ChannelT min, max;
    min_max_channel_values( masked_input, min, max );
    session_threshold = min + boost::numeric_cast<ChannelT>((float(max-min)*opt.percent)/100.0f);
  }

  vw_out() << "\t--> Using threshold value: " << session_threshold << "\n";

  ImageViewRef<PixelT> result =
    apply_mask(intersect_mask(masked_input,create_mask(threshold(input_image,boost::numeric_cast<ChannelT>(session_threshold)))),boost::numeric_cast<ChannelT>(session_nodata));

  photk::write_gdal_georeferenced_image( output, result, georef, opt,
                                       TerminalProgressCallback("photometrytk","Writing:") );
}

template <class PixelT>
class ThresholdAlphaFunctor : public UnaryReturnSameType {
  typedef typename vw::PixelChannelType<PixelT>::type channel_type;
  double m_threshold;
public:
  ThresholdAlphaFunctor( double t ) : m_threshold(t) {}

  template <class ArgT>
  inline ArgT operator()( ArgT const& arg ) const {
    ArgT copy = arg;
    if ( PixelGray<channel_type>(copy) <= m_threshold ) {
      copy.a() = 0;
    }
    return copy;
  }
};

template <class PixelT>
struct MultiplyAlphaFunctor : public vw::ReturnFixedType<PixelT> {
  template <class Pixel2T>
  inline PixelT operator()( PixelT const& arg1, Pixel2T const& arg2 ) const {
    typedef typename CompoundChannelType<PixelT>::type ChannelT;
    PixelT copy = arg1;
    copy.a() = boost::numeric_cast<ChannelT>(float(copy.a()) * arg2);
    if ( !boost::is_floating_point<typename CompoundChannelType<PixelT>::type>::value &&
         copy.a() == 0 && arg1.a() != 0 && arg2 != 0 )
      copy.a() = 1;
    return copy;
  }
};

template <class PixelT>
void shadow_mask_alpha( Options& opt,
                        std::string input,
                        std::string output ) {
  typedef typename PixelChannelType<PixelT>::type ChannelT;
  typedef typename PixelWithoutAlpha<PixelT>::type PixelNoAT;

  cartography::GeoReference georef;
  cartography::read_georeference(georef, input);
  DiskImageView<PixelT> input_image(input);
  ChannelRange<ChannelT> helper;

  double session_threshold = opt.threshold;
  if ( opt.threshold < 0 && opt.percent < 0 ) {
    session_threshold = 0.1*helper.max();
  } else if ( opt.percent > 0 ) {
    ChannelT min, max;
    min_max_channel_values( input_image, min, max );
    session_threshold = min + boost::numeric_cast<ChannelT>((float(max-min)*opt.percent)/100.0f);
  }

  vw_out() << "\t--> Using threshold value: " << session_threshold << "\n";

  if ( opt.feather ) {
    ImageViewRef<PixelNoAT> invert_holes =
      threshold(apply_mask(alpha_to_mask(input_image),helper.max()),
                ChannelT(session_threshold),0,helper.max());
    ImageView<int32> dist = grassfire(invert_holes);
    ImageViewRef<double> alpha_mod =
      clamp(channel_cast<double>(dist)/40.0,1.0);
    ImageViewRef<PixelT> result = per_pixel_filter( input_image, alpha_mod, MultiplyAlphaFunctor<PixelT>() );

    photk::write_gdal_georeferenced_image( output, result, georef, opt,
                                         TerminalProgressCallback("photometrytk","Writing:"));
  } else {
    ImageViewRef<PixelT> result =
      per_pixel_filter( input_image, ThresholdAlphaFunctor<PixelT>(session_threshold) );

    photk::write_gdal_georeferenced_image( output, result, georef, opt,
                                         TerminalProgressCallback("photometrytk","Writing:"));
  }
}

// Code for handing input
void handle_arguments( int argc, char *argv[], Options& opt ) {
  po::options_description general_options("");
  general_options.add_options()
    ("nodata-value", po::value(&opt.nodata), "Value that is nodata in the input image. Not used if input has alpha.")
    ("threshold,t", po::value(&opt.threshold), "Intensity value used to detect shadows.")
    ("percent,p", po::value(&opt.percent), "Percent value of valid pixels used to detect shadows. This is an alternative to threshold option.")
    ("feather", "Feather pre-existing alpha around the newly masked shadows. Only for images with alpha already.");
  general_options.add( photk::BaseOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("input-files", po::value(&opt.input_files));

  po::positional_options_description positional_desc;
  positional_desc.add("input-files", -1);

  std::string usage = "Usage: " + std::string(argv[0]) + " [options] <image_files>\n";
  po::variables_map vm =
    photk::check_command_line( argc, argv, opt, general_options,
                             positional, positional_desc, usage );

  opt.feather = vm.count("feather");
  if ( opt.input_files.empty() )
    vw_throw( ArgumentErr() << "Missing input files!\n"
              << usage << general_options );
}

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

  Options opt;
  try {
    handle_arguments( argc, argv, opt );

    BOOST_FOREACH( const std::string& input, opt.input_files ) {

      // Determining the format of the input
      DiskImageResource *rsrc = DiskImageResource::open(input);
      ChannelTypeEnum channel_type = rsrc->channel_type();
      PixelFormatEnum pixel_format = rsrc->pixel_format();
      delete rsrc;

      vw_out() << "Loading: " << input << "\n";

      fs::path input_path(input);
      std::string output = "./"+input_path.stem()+"_shdw" + input_path.extension();
      vw_out() << "Output = [" << output << "]\n";

      switch (pixel_format) {
      case VW_PIXEL_GRAY:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          shadow_mask_nodata<PixelGray<uint8> >( opt, input, output ); break;
        case VW_CHANNEL_INT16:
          shadow_mask_nodata<PixelGray<int16> >( opt, input, output ); break;
        case VW_CHANNEL_UINT16:
          shadow_mask_nodata<PixelGray<uint16> >( opt, input, output ); break;
        default:
          shadow_mask_nodata<PixelGray<float32> >( opt, input, output ); break;
        }
        break;
      case VW_PIXEL_GRAYA:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          shadow_mask_alpha<PixelGrayA<uint8> >( opt, input, output ); break;
        case VW_CHANNEL_INT16:
          shadow_mask_alpha<PixelGrayA<int16> >( opt, input, output ); break;
        case VW_CHANNEL_UINT16:
          shadow_mask_alpha<PixelGrayA<uint16> >( opt, input, output ); break;
        default:
          shadow_mask_alpha<PixelGrayA<float32> >( opt, input, output ); break;
        }
        break;
      case VW_PIXEL_RGB:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          shadow_mask_nodata<PixelRGB<uint8> >( opt, input, output ); break;
        case VW_CHANNEL_INT16:
          shadow_mask_nodata<PixelRGB<int16> >( opt, input, output ); break;
        case VW_CHANNEL_UINT16:
          shadow_mask_nodata<PixelRGB<uint16> >( opt, input, output ); break;
        default:
          shadow_mask_nodata<PixelRGB<float32> >( opt, input, output ); break;
        }
        break;
      default:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          shadow_mask_alpha<PixelRGBA<uint8> >( opt, input, output ); break;
        case VW_CHANNEL_INT16:
          shadow_mask_alpha<PixelRGBA<int16> >( opt, input, output ); break;
        case VW_CHANNEL_UINT16:
          shadow_mask_alpha<PixelRGBA<uint16> >( opt, input, output ); break;
        default:
          shadow_mask_alpha<PixelRGBA<float32> >( opt, input, output ); break;
        }
        break;
      }
    }
  } PHOTK_STANDARD_CATCHES;

  return 0;
}


#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_COM_INTERVALTYPES
#include "com_intervaltypes.h"
#define INCLUDED_COM_INTERVALTYPES
#endif

// Library headers.
#ifndef INCLUDED_SSTREAM
#include <sstream>
#define INCLUDED_SSTREAM
#endif
#ifndef INCLUDED_COM_MATH
#include "com_math.h"
#define INCLUDED_COM_MATH
#endif
#include <boost/spirit/include/classic_core.hpp>
#ifndef INCLUDED_BOOST_ALGORITHM_STRING_TRIM
#include <boost/algorithm/string/trim.hpp>
#define INCLUDED_BOOST_ALGORITHM_STRING_TRIM
#endif
#ifndef INCLUDED_VECTOR
#include <vector>
#define INCLUDED_VECTOR
#endif
#ifndef INCLUDED_STRING
#include <string>
#define INCLUDED_STRING
#endif

// PCRaster library headers.

// Module headers.

/*!
  \file
  This file contains the implementation of all classes
  derived from Interval
*/

//------------------------------------------------------------------------------
// DEFINITION OF INTERVAL MEMBERS
//------------------------------------------------------------------------------

template<typename R>
com::Interval<R>::Interval()
{
}

template<typename R>
com::Interval<R>::~Interval()
{
}

//! sorting criteria for Interval objects
/*! this Interval object is smaller than \a rhs if
 *  all values represented by this interval are smaller
 *  than all values represented in \a rhs.
 *  In other words they do not overlap
 */
template<typename R>
bool com::Interval<R>::less(const Interval<R>& rhs) const
{
  if (max()==rhs.min()) {
    // really < if one side is exclusive
    return (!valid(max())) || (!rhs.valid(rhs.min()));
  }
  return max() < rhs.min();
}

template<typename R>
bool com::Interval<R>::operator==(const Interval& rhs)const
{
  return
   min()==rhs.min()&&
   max()==rhs.max()&&
   valid(min())==rhs.valid(rhs.min())&&
   valid(max())==rhs.valid(rhs.max());
}

//! lowest value representable
template<typename R>
R com::Interval<R>::minLimit()
{
    return NumericLimits<R>::minValue();
}

//! highest value representable
template<typename R>
R com::Interval<R>::maxLimit()
{
  return NumericLimits<R>::maxValue();
}

template<typename R>
R com::Interval<R>::centre() const
{
  return (min()+max())/2;
}

template<typename R>
void com::Interval<R>::print(std::ostream& stream) const
{
  if (min()==max()) {
     // single value
     stream << min();
     return;
  }
  stream << std::string(valid(min()) ? "[" : "<");
  if (min()!=minLimit())
    stream<<min();
  stream << ",";
  if (max()!=maxLimit())
    stream<<max();
  stream << std::string(valid(max()) ? "]" : ">");
}

template<typename R>
std::ostream& com::operator<<(
    std::ostream& stream,
    const Interval<R>& i)
{
  i.print(stream);
  return stream;
}

template<typename R>
com::GreaterThan<R> *com::GreaterThan<R>::createClone() const
{
  return new GreaterThan(*this);
}

template<typename R>
std::string com::GreaterThan<R>::msg() const
{
  std::ostringstream str;
  str << "greater than " << this->d_minV << " (>"<<this->d_minV<<")";
  return str.str();
}

template<typename R>
std::string com::GreaterThanEqualTo<R>::msg() const
{
  std::ostringstream str;
  str << "greater than or equal to " << this->d_minV << " (>="<<this->d_minV<<")";
  return str.str();
}

template<typename R>
com::GreaterThanEqualTo<R> *com::GreaterThanEqualTo<R>::createClone() const
{
  return new GreaterThanEqualTo(*this);
}

template<typename R>
std::string com::LessThan<R>::msg() const
{
  std::ostringstream str;
  str << "less than " << this->d_maxV << " (<"<<this->d_maxV<<")";
  return str.str();
}

template<typename R>
com::LessThan<R> *com::LessThan<R>::createClone() const
{
  return new LessThan(*this);
}

template<typename R>
std::string com::LessThanEqualTo<R>::msg() const
{
  std::ostringstream str;
  str << "less than or equal to " << this->d_maxV << " (<="<<this->d_maxV<<")";
  return str.str();
}

template<typename R>
com::LessThanEqualTo<R> *com::LessThanEqualTo<R>::createClone() const
{
  return new LessThanEqualTo(*this);
}

template<typename R>
std::string com::EqualTo<R>::msg() const
{
  std::ostringstream str;
  str << "equal to " << d_thisValue << " (="<<d_thisValue<<")";
  return str.str();
}

template<typename R>
com::EqualTo<R> *com::EqualTo<R>::createClone() const
{
  return new EqualTo(*this);
}

template<typename R>
std::string com::AnythingInterval<R>::msg() const
{
  return "anything";
}

template<typename R>
com::AnythingInterval<R> *com::AnythingInterval<R>::createClone() const
{
  return new AnythingInterval(*this);
}

//! ctor
/*!
 * \throws
 *   com::BadIntervalFormat if:
 *

 *
lower value is higher than high value: [2,1]

 *
lower value is equal to high value but conflicting or empty like
 *       conflict:[2,2> empty:<2,2> ok:[2,2]

 *

 */
template<typename R>
com::BetweenLimits<R>::BetweenLimits(
   const LowerLimit<R>&  lowerLimit,
   const UpperLimit<R>&  upperLimit):
    d_lowerLimit(lowerLimit.createClone()),
    d_upperLimit(upperLimit.createClone())
{
  // normal expected case:
  R min(d_lowerLimit->min());
  R max(d_upperLimit->max());

  if (min < max)
    return;

  // most certain errors, except the case [2,2]
  if ( min==max && valid(min) && valid(max) )
    return;

  clean();
  throw com::BadIntervalFormat(
        "lower value is higher than high value");
}

//! dtor
template<typename R>
com::BetweenLimits<R>::~BetweenLimits()
{
  clean();
}

template<typename R>
void com::BetweenLimits<R>::clean()
{
  delete d_lowerLimit;
  d_lowerLimit=0;
  delete d_upperLimit;
  d_upperLimit=0;
}

template<typename R>
com::BetweenLimits<R>::BetweenLimits(const BetweenLimits& rhs)
  : Interval<R>(rhs),
    d_lowerLimit(rhs.d_lowerLimit->createClone()),
    d_upperLimit(rhs.d_upperLimit->createClone())
{
}

template<typename R>
com::BetweenLimits<R> *com::BetweenLimits<R>::createClone() const
{
  return new BetweenLimits(*this);
}

template<typename R>
com::BetweenLimits<R>& com::BetweenLimits<R>::operator=(
                   const BetweenLimits<R>& rhs )
{
  if (this != &rhs) {
    delete d_lowerLimit;
    d_lowerLimit=rhs.d_lowerLimit->createClone();
    delete d_upperLimit;
    d_upperLimit=rhs.d_upperLimit->createClone();
  }
  return *this;
}

template<typename R>
bool com::BetweenLimits<R>::valid(R v) const
{
  return d_lowerLimit->valid(v) && d_upperLimit->valid(v);
}

template<typename R>
bool  com::BetweenLimits<R>::operator<(R v) const
{
  return d_upperLimit->operator<(v);
}

template<typename R>
bool  com::BetweenLimits<R>::operator>(R v) const
{
  return d_lowerLimit->operator>(v);
}

template<typename R>
std::string com::BetweenLimits<R>::msg() const
{
  return d_lowerLimit->msg()+" and "+d_upperLimit->msg();
}

#ifndef INCLUDED_BOOST_OPTIONAL
#include <boost/optional.hpp>
#define INCLUDED_BOOST_OPTIONAL
#endif
#ifndef INCLUDED_BOOST_BIND
#include <boost/bind.hpp>
#define INCLUDED_BOOST_BIND
#endif

namespace com {
   typedef boost::optional<double> OD;

static void spiritParser(
     const std::string& str,
     std::vector<char>& ranges,
      OD& singleValue,
      OD& low,
      OD& high
     )
  {
    // laatste hand parser (in commentaar) zit in revisie 1.14
    using namespace boost::spirit::classic;

   /* normal EBNF
      ( real
        | ( '[' | '<' ) real? ',' real? (']'|'>')
      )
   */
   // TODO model this like spirt's predefined action assign
   //      to elimninate this macro
#define RESET(x) boost::bind(&OD::reset,&x,_1)

   // TODO how te define trailing space correctly in parser?
   std::string copyStr(str);
   boost::algorithm::trim(copyStr);

   bool correct=parse(copyStr.begin(),copyStr.end(),
        //  Begin grammar
          (   real_p[RESET(singleValue)]
              | (
                 ( ch_p('[')[append(ranges)] |
                   ch_p('<')[append(ranges)] )
                    >> (!real_p[RESET(low)]) >> ','
                    >>  !real_p[RESET(high)]
                    >>
                 ( ch_p(']')[append(ranges)] |
                   ch_p('>')[append(ranges)] )
                )
          )
        ,
        //  End grammar
        space_p).full;
  if (!correct)
      throw com::BadIntervalFormat("illegal key format");
 }
}

//! create from a string formatted as a PCRaster lookup key element
/*!
 *  \throws com::BadIntervalFormat in case of error
 */
template<typename R>
com::Interval<R> * com::createIntervalFromLookupTableKey (
    const std::string& str)
{
   std::vector<char>   ranges;
   OD singleValue, low, high;

   //  either float or double
   DEVELOP_PRECOND(!std::numeric_limits<R>::is_integer);

   spiritParser(str,ranges,singleValue,low,high);

   if (!low && !high) {
     if (!singleValue)
       return new com::AnythingInterval<R>(); // infinity
     return new com::EqualTo<R>((R)*singleValue);
   }

   PRECOND(ranges.size() == 2);
   char lowRange  = ranges[0];
   char highRange = ranges[1];

   com::UpperLimit<R>* h(0);
   com::LowerLimit<R>* l(0);

   if (high) {
    if (highRange == ']')
     h = new com::LessThanEqualTo<R>((R)*high);  // ... ,h]
    else {
     POSTCOND(highRange == '>');
     h = new com::LessThan<R>((R)*high);         // ... ,h>
    }
    if (!low)
      return h; // < inf, ...
   }

   if (low) {
    if (lowRange == '[')
     l = new com::GreaterThanEqualTo<R>((R)*low); // [l, ...
    else {
     POSTCOND(lowRange == '<');
     l = new com::GreaterThan<R>((R)*low);        // <l, ...
    }
    if (!h)
      return l; // ... , inf >
   }

   PRECOND(l && h);
   com::Interval<R> *v(0);
   try {
    v=new com::BetweenLimits<R>(*l,*h);
   } catch(...) {
    delete l;
    delete h;
    throw;
   }
   delete l;
   delete h;
   return v;
}

namespace com {
  template class Interval<double>;
  template class UpperLimit<double>;
  template class GreaterThanEqualTo<double>;
  template class GreaterThan<double>;
  template class LessThanEqualTo<double>;
  template class LessThan<double>;
  template class LowerLimit<double>;
  template class EqualTo<double>;
  template class AnythingInterval<double>;
  template class BetweenLimits<double>;
  template Interval<double>* createIntervalFromLookupTableKey (const std::string& str);
  Interval<double> * createClone(Interval<double> const* c)
  { return c->createClone(); }
  void deleteClone(Interval<double> const* c) { delete c; }
  template std::ostream& operator<<(std::ostream& stream, const Interval<double>& i);
}
namespace com {
  template class Interval<float>;
  template class UpperLimit<float>;
  template class GreaterThanEqualTo<float>;
  template class GreaterThan<float>;
  template class LessThanEqualTo<float>;
  template class LessThan<float>;
  template class LowerLimit<float>;
  template class EqualTo<float>;
  template class AnythingInterval<float>;
  template class BetweenLimits<float>;
  template Interval<float>* createIntervalFromLookupTableKey (const std::string& str);
  template std::ostream& operator<<(std::ostream& stream, const Interval<float>& i);

  Interval<float> * createClone(Interval<float> const* c)
  { return c->createClone(); }
  void deleteClone(Interval<float> const* c) { delete c; }
}
/*! be sure to implements maxLimit()+minLimit()==0
 *  test in IntervalTest::testLimit
 */


// 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, London, UK.

// 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_NSPHERE_STRATEGIES_CARTESIAN_NSPHERE_IN_BOX_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_CARTESIAN_NSPHERE_IN_BOX_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/within.hpp>

namespace boost { namespace geometry { namespace strategy
{

namespace within
{

struct nsphere_within_range
{
    template <typename ContainedValue, typename ContainingValue>
    static inline bool apply(ContainedValue const& ed_min
                           , ContainedValue const& ed_max
                           , ContainingValue const& ing_min
                           , ContainingValue const& ing_max)
    {
        return ing_min < ed_min && ed_max < ing_max;
    }
};

struct nsphere_covered_by_range
{
    template <typename ContainedValue, typename ContainingValue>
    static inline bool apply(ContainedValue const& ed_min
                           , ContainedValue const& ed_max
                           , ContainingValue const& ing_min
                           , ContainingValue const& ing_max)
    {
        return ing_min <= ed_min && ed_max <= ing_max;
    }
};

template
<
    typename SubStrategy,
    typename NSphere,
    typename Box,
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct relate_nsphere_box_loop
{
    static inline bool apply(NSphere const& sphere, Box const& box)
    {
        if (! SubStrategy::apply(
                get<Dimension>(sphere) - get_radius<0>(sphere),
                get<Dimension>(sphere) + get_radius<0>(sphere),
                get<min_corner, Dimension>(box),
                get<max_corner, Dimension>(box))
            )
        {
            return false;
        }

        return relate_nsphere_box_loop
            <
                SubStrategy,
                NSphere, Box,
                Dimension + 1, DimensionCount
            >::apply(sphere, box);
    }
};

template
<
    typename SubStrategy,
    typename NSphere,
    typename Box,
    std::size_t DimensionCount
>
struct relate_nsphere_box_loop<SubStrategy, NSphere, Box, DimensionCount, DimensionCount>
{
    static inline bool apply(NSphere const& , Box const& )
    {
        return true;
    }
};

template
<
    typename NSphere,
    typename Box,
    typename SubStrategy = nsphere_within_range
>
struct nsphere_in_box
{
    static inline bool apply(NSphere const& nsphere, Box const& box)
    {
        return relate_nsphere_box_loop
            <
                SubStrategy,
                NSphere, Box,
                0, dimension<NSphere>::type::value
            >::apply(nsphere, box);
    }
};

} // namespace within

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace within { namespace services
{

template <typename NSphere, typename Box>
struct default_strategy
    <
        nsphere_tag, box_tag,
        nsphere_tag, areal_tag,
        cartesian_tag, cartesian_tag,
        NSphere, Box
    >
{
    typedef within::nsphere_in_box<NSphere, Box, within::nsphere_within_range> type;
};

}} // namespace within::services

namespace covered_by { namespace services
{

template <typename NSphere, typename Box>
struct default_strategy
    <
        nsphere_tag, box_tag,
        nsphere_tag, areal_tag,
        cartesian_tag, cartesian_tag,
        NSphere, Box
    >
{
    typedef within::nsphere_in_box<NSphere, Box, within::nsphere_covered_by_range> type;
};

}} // namespace covered_by::services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_CARTESIAN_NSPHERE_IN_BOX_HPP


#include <ros/ros.h>
#include <frontiers.h>
#include <neighbors.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <frontiers_msgs/LocalGeofenceRequest.h>
#include <Eigen/Dense>
#include <frontiers_msgs/FindFrontiers.h>
#include <geometry_msgs/Point.h>

namespace frontiers_debug_node
{
	ros::Publisher marker_pub;
	void neighbor_callback (const frontiers_msgs::VoxelMsg::ConstPtr& voxel)
	{
		LazyThetaStarOctree::unordered_set_pointers neighbors;
		octomath::Vector3 center_coords(voxel->xyz_m.x, voxel->xyz_m.y, voxel->xyz_m.z);
		float resolution = 0.5;
		LazyThetaStarOctree::generateNeighbors_frontiers_pointers( neighbors, center_coords, voxel->size, resolution);
		visualization_msgs::MarkerArray marker_array;
		// rviz_interface::build_neighbor_array(neighbors, marker_array);

        visualization_msgs::Marker marker;
        marker.header.frame_id = "/map";
        marker.header.stamp = ros::Time::now();
        marker.ns = "frontier_candidate";
        marker.id = 10;
        marker.type = visualization_msgs::Marker::CUBE;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.position.x = center_coords.x();
        marker.pose.position.y = center_coords.y();
        marker.pose.position.z = center_coords.z();
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;
        marker.scale.x = resolution;
        marker.scale.y = resolution;
        marker.scale.z = resolution;
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 1.0f;
        marker.color.a = 1.0;
        marker.lifetime = ros::Duration();
        marker_array.markers.push_back(marker);

        marker_pub.publish(marker_array);
	}

        bool fillLocalGeofence(Eigen::Vector3d start, Eigen::Vector3d end, int range, double flyby_length, double local_fence_side)
        {
                double extension = local_fence_side - flyby_length;
                geometry_msgs::Point geofence_max, geofence_min;
                geofence_min.x = 0;
                geofence_min.y = 0;
                geofence_min.z = 0;
                geofence_max.x = 10;
                geofence_max.y = 10;
                geofence_max.z = 10;

                frontiers_msgs::FindFrontiers           frontier_srv;

                Eigen::Vector3d direction = end - start;
                if(direction.norm() <= 0.01)
                {
                        ROS_ERROR_STREAM("Still have to do this! start " << start << "; end " << end);
                }
                Eigen::Vector3d ortho = Eigen::Vector3d::UnitZ().cross(direction);
                ortho.normalize();
                direction.normalize();
                Eigen::Vector3d e = start - (direction * extension/2);
                Eigen::Vector3d a = e + (ortho * range);
                Eigen::Vector3d b = a + (direction * local_fence_side);
                Eigen::Vector3d c = e - (ortho * range);
                Eigen::Vector3d d = c + (direction * local_fence_side);

                // Geofence
                // Min
                frontier_srv.request.min.x = std::max(std::min({a.x(), b.x(), c.x(), d.x()}), geofence_min.x);
                frontier_srv.request.min.y = std::max(std::min({a.y(), b.y(), c.y(), d.y()}), geofence_min.y);
                frontier_srv.request.min.z = std::max(std::min({start.z(), end.z()}), geofence_min.z);
                // Max
                frontier_srv.request.max.x = std::min(std::max({a.x(), b.x(), c.x(), d.x()}), geofence_max.x);
                frontier_srv.request.max.y = std::min(std::max({a.y(), b.y(), c.y(), d.y()}), geofence_max.y);
                frontier_srv.request.max.z = std::min(frontier_srv.request.min.z+ std::abs(start.z() - end.z()) + range, geofence_max.z);

                octomath::Vector3 a_o(a.x(), a.y(), a.z());
                octomath::Vector3 e_o(e.x(), e.y(), e.z());
                octomath::Vector3 b_o(b.x(), b.y(), b.z());
                octomath::Vector3 c_o(c.x(), c.y(), c.z());
                octomath::Vector3 d_o(d.x(), d.y(), d.z());
                visualization_msgs::MarkerArray marker_array;
                visualization_msgs::Marker marker;
                octomath::Vector3 start_o(start.x(), start.y(), start.z());
                rviz_interface::build_waypoint(start_o, 0.1, 0,   10, marker, 5);
                marker.ns = "start";
                marker_array.markers.push_back( marker );
                octomath::Vector3 end_o(end.x(), end.y(), end.z());
                rviz_interface::build_waypoint(end_o, 0.1, 0,   11, marker, 5);
                marker.ns = "end";
                marker_array.markers.push_back( marker );

                rviz_interface::build_waypoint(e_o, 0.5, 0,   1, marker, 1);
                marker.ns = "e";
                marker_array.markers.push_back( marker );
                rviz_interface::build_waypoint(a_o, 0.5, 0,   1, marker, 1);
                marker.ns = "a";
                marker_array.markers.push_back( marker );
                rviz_interface::build_waypoint(b_o, 0.5, 250, 2, marker, 1);
                marker.ns = "b";
                marker_array.markers.push_back( marker );
                rviz_interface::build_waypoint(c_o, 0.5, 0  , 3, marker, 9);
                marker.ns = "c";
                marker_array.markers.push_back( marker );
                rviz_interface::build_waypoint(d_o, 0.5, 250, 4, marker, 9);
                marker.ns = "d";
                marker_array.markers.push_back( marker );
                rviz_interface::build_arrow_type(start_o, end_o, marker_array, 20, true);
                octomath::Vector3 end_ortho(start_o.x()+ortho.x(), start_o.y()+ortho.y(), start_o.z()+ortho.z());
                rviz_interface::build_arrow_type(start_o, end_ortho, marker_array, 21, false);

                ROS_INFO_STREAM("");
                // ROS_INFO_STREAM("Start = (" << start.x() << ", " << start.y() << ", " << start.z() << ")");
                // ROS_INFO_STREAM("End = (" << end.x() << ", " << end.y() << ", " << end.z() << ")");
                // ROS_INFO_STREAM("End Ortho = (" << end_ortho.x() << ", " << end_ortho.y() << ", " << end_ortho.z() << ")");

                ROS_INFO_STREAM("a = (" << a.x() << ", " << a.y() << ", " << a.z() << ")");
                ROS_INFO_STREAM("b = (" << b.x() << ", " << b.y() << ", " << b.z() << ")");

                ROS_INFO_STREAM("c = (" << c.x() << ", " << c.y() << ", " << c.z() << ")");
                ROS_INFO_STREAM("d = (" << d.x() << ", " << d.y() << ", " << d.z() << ")");

                // ROS_INFO_STREAM("Direction = (" << direction.x() << ", " << direction.y() << ", " << direction.z() << ")");
                // ROS_INFO_STREAM("Ortho = (" << ortho.x() << ", " << ortho.y() << ", " << ortho.z() << ")");

                ROS_INFO_STREAM("Geofence min " << geofence_min);
                ROS_INFO_STREAM("Geofence max " << geofence_max);

                visualization_msgs::Marker marker_2;
                octomath::Vector3 g_min(geofence_min.x, geofence_min.y, geofence_min.z);
                octomath::Vector3 g_max(geofence_max.x, geofence_max.y, geofence_max.z);
                rviz_interface::build_geofence (g_min, g_max, marker_2, 111, "global", 1, 0, 0);
                marker_array.markers.push_back( marker_2 );

                // ROS_INFO_STREAM( "std::max(std::min({" << a.x() << ", " << b.x() << ", " << c.x() << ", " << d.x() << "}), " << geofence_min.x << ")" );
                // ROS_INFO_STREAM("Request " << frontier_srv.request);
                visualization_msgs::Marker marker_3;
                octomath::Vector3 min(frontier_srv.request.min.x, frontier_srv.request.min.y, frontier_srv.request.min.z);
                octomath::Vector3 max(frontier_srv.request.max.x, frontier_srv.request.max.y, frontier_srv.request.max.z);
                rviz_interface::build_geofence (min, max, marker_3, 100, "local", 1, 1, 0);
                marker_array.markers.push_back( marker_3 );
                marker_pub.publish(marker_array);
        }

        void localFence_callback (const frontiers_msgs::LocalGeofenceRequest::ConstPtr& request)
        {

                Eigen::Vector3d start(request->start.x, request->start.y, request->start.z);
                Eigen::Vector3d end(request->end.x, request->end.y, request->end.z);
                fillLocalGeofence(start, end, request->range, request->flyby_length, request->local_fence_side);
        }
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "ltstar_debug_node");
	ros::NodeHandle nh;
	frontiers_debug_node::marker_pub = nh.advertise<visualization_msgs::MarkerArray>("debug_markers", 1);
	ros::Subscriber neighbor_sub = nh.subscribe<frontiers_msgs::VoxelMsg>("/neighbor_generation", 10, frontiers_debug_node::neighbor_callback);
        ros::Subscriber geofence_sub = nh.subscribe<frontiers_msgs::LocalGeofenceRequest>("/local_geofence_request", 10, frontiers_debug_node::localFence_callback);
	ros::spin();
}