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

// #include <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename Matrix>
void setup(Matrix& A)
{
    const std::size_t n= num_rows(A);
    A= 1.0;
    mtl::mat::inserter<Matrix, mtl::update_plus<double> > ins(A);

    for (std::size_t i= 0; i < 2 * n; i++) {
	int r= rand()%n, c= rand()%n;
	ins[r][c] << -1;
	ins[r][r] << 1;
    }
}

template <typename At, typename Lt, typename Ut>
void dense_ilu_0(const At& As, const Lt& Ls, const Ut& Us)
{
    mtl::dense2D<double> LU(As);

    const std::size_t n= num_rows(LU);
    for (std::size_t i= 1; i < n; i++)
	for (std::size_t k= 0; k < i; k++) {
	    LU[i][k]/= LU[k][k];
	    for (std::size_t j= k + 1; j < n; j++)
		if (LU[i][j] != 0)
		    LU[i][j]-= LU[i][k] * LU[k][j];
	}
    std::cout << "Factorizing A = \n" << As << "-> LU = \n" << LU;
    // std::cout << "L = \n" << Ls << "\nU = \n" << Us;

    if (std::abs(LU[0][0] - Ls[0][0]) > 0.001) throw "Wrong value in L for sparse ILU(0) factorization";

    if (std::abs(LU[0][1] - Us[0][1]) > 0.001) throw "Wrong value in U for sparse ILU(0) factorization";
}

int main(int, char**)
{
    // For a more realistic example set sz to 1000 or larger
    const int N = 5;

    typedef mtl::dense2D<double>       matrix_type;
    typedef mtl::dense_vector<double>  vector_type;
    matrix_type                        A(N, N);
    setup(A);

    itl::pc::ilu_0<matrix_type>        P(A);

    mtl::dense_vector<double> x(N), x2(N), Px(N), x3(N), x4(N), x5(N);

    for (unsigned i= 0; i < num_rows(x); i++)
	x[i]= i+1;

    std::cout << "A is\n" << A;
    x2= A * x;
    std::cout << "x2= A * x = " << x2 << "\n";

    x3= solve(P, x2);
    std::cout << "solve(P, x2) = " << x3 << " (should be [1,2,..,N])\n";
    if (two_norm(vector_type(x - x3)) > 0.00001) throw "Wrong result";

    // Now test adjoint solve
    x4= trans(A) * x;
    std::cout << "x4= adjoint(A) * x = " << x4 << "\n";

    x5= adjoint_solve(P, x4);
    std::cout << "adjoint_solve(P, x4) = " << x5 << " (should be [1,2,..,N])\n";
    if (two_norm(vector_type(x - x5)) > 0.00001) throw "Wrong result";

    matrix_type LU(A), L(N,N), U(N,N);
    lu(LU, x5);
    L= lower(LU);
    U= upper(LU);
    std::cout<< "L=" << L << "\n";
    std::cout<< "U=" << U << "\n";
    dense_ilu_0(A, L, U);

    return 0;
}


// Copyright (c) 2015-2018, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of gepetto-viewer-corba.
// gepetto-viewer-corba 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.
//
// gepetto-viewer-corba 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// gepetto-viewer-corba. If not, see <http://www.gnu.org/licenses/>.

#ifndef __se3_se3_hpp__
#define __se3_se3_hpp__
#include <Eigen/Geometry>

namespace se3
{
/* Type returned by the "se3Action" and "se3ActionInverse" functions. */
namespace internal
{
template<typename D>
struct ActionReturn { typedef D Type; };
}
/** The rigid transform aMb can be seen in two ways:
*
* - given a point p expressed in frame B by its coordinate vector Bp, aMb
* computes its coordinates in frame A by Ap = aMb Bp.
* - aMb displaces a solid S centered at frame A into the solid centered in
* B. In particular, the origin of A is displaced at the origin of B: $^aM_b
* ^aA = ^aB$.
* The rigid displacement is stored as a rotation matrix and translation vector by:
* aMb (x) = aRb*x + aAB
* where aAB is the vector from origin A to origin B expressed in coordinates A.
*/
template<typename _Scalar, int _Options>
class SE3Tpl
{
public:
typedef _Scalar Scalar;
enum { Options = _Options };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef Eigen::Quaternion<Scalar,Options> Quaternion;
//typedef MotionTpl<Scalar,Options> Motion;
//typedef ForceTpl<Scalar,Options> Force;
//typedef ActionTpl<Scalar,Options> Action;
enum { LINEAR = 0, ANGULAR = 3 };
public:
// Constructors
SE3Tpl() : rot(), trans() {}
template<typename M3,typename v3>
SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
: rot(R), trans(p) {}
SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
template<typename S2, int O2>
SE3Tpl( const SE3Tpl<S2,O2> clone )
: rot(clone.rotation()),trans(clone.translation()) {}
template<typename S2, int O2>
SE3Tpl & operator= (const SE3Tpl<S2,O2> & other)
{
rot = other.rotation ();
trans = other.translation ();
return *this;
}
const Matrix3 & rotation() const { return rot; }
const Vector3 & translation() const { return trans; }
Matrix3 & rotation() { return rot; }
Vector3 & translation() { return trans; }
void rotation(const Matrix3 & R) { rot=R; }
void translation(const Vector3 & p) { trans=p; }
static SE3Tpl Identity()
{
return SE3Tpl(1);
}
static SE3Tpl Random()
{
Eigen::Quaternion<Scalar,Options> q(Vector4::Random());
q.normalize();
return SE3Tpl(q.matrix(),Vector3::Random());
}
Eigen::Matrix<Scalar,4,4,Options> toHomogeneousMatrix() const
{
Eigen::Matrix<Scalar,4,4,Options> M;
M.template block<3,3>(0,0) = rot;
M.template block<3,1>(0,3) = trans;
M.template block<1,3>(3,0).setZero();
M(3,3) = 1;
return M;
}
/// Vb.toVector() = bXa.toMatrix() * Va.toVector()
Matrix6 toActionMatrix() const
{
Matrix6 M;
M.template block<3,3>(ANGULAR,ANGULAR)
= M.template block<3,3>(LINEAR,LINEAR) = rot;
M.template block<3,3>(ANGULAR,LINEAR).setZero();
M.template block<3,3>(LINEAR,ANGULAR)
= skew(trans) * M.template block<3,3>(ANGULAR,ANGULAR);
return M;
}
/// aXb = bXa.inverse()
SE3Tpl inverse() const
{
return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
}
void disp(std::ostream & os) const
{
os << " R =\n" << rot << std::endl
<< " p =\n" << trans.transpose() << std::endl;
}
/* --- GROUP ACTIONS ON M6, F6 and I6 --- */
/// ay = aXb.act(by)
template<typename D> typename internal::ActionReturn<D>::Type act (const D & d) const
{ return d.se3Action(*this); }
/// by = aXb.actInv(ay)
template<typename D> typename internal::ActionReturn<D>::Type actInv(const D & d) const
{ return d.se3ActionInverse(*this); }
Vector3 act (const Vector3& p) const { return (rot*p+trans).eval(); }
Vector3 actInv(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); }
SE3Tpl act (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
SE3Tpl actInv (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot,
rot.transpose()*(m2.trans-trans));}
/* --- OPERATORS -------------------------------------------------------- */
operator Matrix4() const { return toHomogeneousMatrix(); }
operator Matrix6() const { return toActionMatrix(); }
SE3Tpl operator*(const SE3Tpl & m2) const { return this->act(m2); }
friend std::ostream & operator << (std::ostream & os,const SE3Tpl & X)
{ X.disp(os); return os; }
public:
private:
Matrix3 rot;
Vector3 trans;
};
typedef SE3Tpl<float,0> SE3;
} // namespace se3
#endif // ifndef __se3_se3_hpp__


/*******************************************************************************
 * Reverse Polish Notation calculator.                                         *
 * Copyright (c) 2007-2009, Samuel Fredrickson <kinghajj@gmail.com>            *
 * 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 HOLDER ``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 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.                                                                     *
 ******************************************************************************/

/*******************************************************************************
 * Calculator.cpp - non-command Calculator methods.                            *
 ******************************************************************************/

#include "rpn.h"
#include <boost/tokenizer.hpp>
#include <sstream>
using namespace boost;
using namespace std;
using namespace RPN;

void Calculator::Eval(string s)
{
    typedef tokenizer< char_separator<char> > Tokens;
    char_separator<char> sep(" \t\n");
    Tokens tokens(s, sep);

    if(!HasStack()) return;

    for(Tokens::iterator tok = tokens.begin();
        tok != tokens.end() && status == Continue;
        ++tok)
    {
        Commands::iterator  foundCommand   = commands.find(*tok);
        Operators::iterator foundOperator  = operators.find(*tok);
        Variables::iterator foundVariable  = variables.find(*tok);
        Value val;

        // if the token is a number, push it.
        if(istringstream(*tok) >> val)
            CurrentStack().push_front(val);

        // if the token is a command, perform it.
        else if(foundCommand != commands.end())
        {
            const Command& command = foundCommand->second;
            vector<string> args;
            args.reserve(command.NumArgs());

            // collect a the tokens that will be the arguments to the command.
            while(tok != tokens.end() && args.size() != command.NumArgs())
                args.push_back(*++tok);

            // only perform a command if we can give it enough arguments.
            if(args.size() == command.NumArgs())
                command.Perform(*this, args);
        }

        // if the token is an operator and that stack has at least two items,
        // then perform that operator.
        else if(foundOperator != operators.end() && StackSize() > 1)
        {
            Value b = TopmostItem(); CurrentStack().pop_front();
            Value a = TopmostItem(); CurrentStack().pop_front();
            CurrentStack().push_front(foundOperator->second(a, b));
        }

        // if the token is a variable, push the variable onto the stack.
        else if(foundVariable != variables.end())
            CurrentStack().push_front(foundVariable->second);

        // otherwise, if the stack has at least one item, set a new variable
        // whose name is the token and value is the top item.
        else variables[*tok] = TopmostItem();
    }
}

// displays the top item of the stack if there is one.
// I tried to write this as a friend operator<<(), but I got errors for
// accessing private data, which is what friend functions are supposed to be
// able to do!
void Calculator::Display() const
{
    Print(TopmostItem());
}


//
// Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH.
// Copyright (c) 2015-2017, University of Bremen
// 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.
//
#ifndef __MAPS_OCCUPANCY_PATCH_HPP_
#define __MAPS_OCCUPANCY_PATCH_HPP_

#include <cmath>

#include <boost/serialization/access.hpp>
#include <boost/serialization/nvp.hpp>

namespace maps { namespace grid
{

class OccupancyPatch
{
    float log_odds;

public:
    OccupancyPatch(double initial_probability) : log_odds(logodds(initial_probability)) {}
    OccupancyPatch(float initial_log_odds = 0.f) : log_odds(initial_log_odds) {}
    virtual ~OccupancyPatch() {}

    double getPropability() const
    {
	return probability(log_odds);
    }

    float getLogOdds() const
    {
	return log_odds;
    }

    bool isOccupied(double occupied_tresshold = 0.8) const
    {
	return probability(log_odds) >= occupied_tresshold;
    }

    bool isFreeSpace(double not_occupied_tresshold = 0.3) const
    {
	return probability(log_odds) < not_occupied_tresshold;
    }

    void updatePropability(double update_prob, double min_prob = 0.1192, double max_prob = 0.971)
    {
	updateLogOdds(logodds(update_prob), logodds(min_prob), logodds(max_prob));
    }

    void updateLogOdds(float update_logodds, float min = -2.f, float max = 3.5f)
    {
	log_odds += update_logodds;
	if(log_odds < min)
	    log_odds = min;
	else if(log_odds > max)
	    log_odds = max;
    }

    bool operator==(const OccupancyPatch& other) const
    {
	return this == &other;
    }

    // compute log-odds from probability
    static inline float logodds(double probability)
    {
	return (float)log(probability / (1.0 - probability));
    }

    // compute probability from log-odds
    static inline double probability(double logodds)
    {
	return 1.0 - ( 1.0 / (1.0 + exp(logodds)));
    }

protected:

    /** Grants access to boost serialization */
    friend class boost::serialization::access;

    /** Serializes the members of this class*/
    template <typename Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
	ar & BOOST_SERIALIZATION_NVP(log_odds);
    }
};

}  //namespace grid
}  //namespace maps

#endif  //__MAPS_OCCUPANCY_PATCH_HPP_


/* test_fisher_f_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id: test_fisher_f_distribution.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/fisher_f_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::fisher_f_distribution<>
#define BOOST_RANDOM_ARG1 m
#define BOOST_RANDOM_ARG2 n
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0.0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0.0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN 0.0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS (1.0, 2.1)
#define BOOST_RANDOM_TEST2_PARAMS (10.0, 10.0)

#include "test_distribution.ipp"


//  ================================================================
//  Created by Gregory Kramida on 1/30/19.
//  Copyright (c) 2019 Gregory Kramida
//  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.
//  ================================================================
#pragma once

//stdlib

//libs
#include <Eigen/Eigen>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "../math/typedefs.hpp"

namespace eig = Eigen;

namespace tsdf {

math::MatrixXuc generate_TSDF_3D_EWA_image_visualization(
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& depth_image,
		float depth_unit_ratio,
		const eig::Tensor<float, 3>& field,
		const eig::Matrix3f& camera_intrinsic_matrix,
		const eig::Matrix4f& camera_pose = eig::Matrix4f::Identity(4, 4),
		const eig::Vector3i& array_offset =
				[] {eig::Vector3i default_offset; default_offset << -64, -64, 64; return default_offset;}(),
		float voxel_size = 0.004,
		int scale=20,
		float tsdf_threshold = 0.1f,
		float gaussian_covariance_scale = 1.0f);

eig::MatrixXf sampling_area_heatmap_2D_EWA_image(int image_y_coordinate,
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& depth_image,
		float depth_unit_ratio,
		const eig::Matrix3f& camera_intrinsic_matrix,
		const eig::Matrix4f& camera_pose,
		const eig::Vector3i& array_offset,
		int field_size,
		float voxel_size,
		int narrow_band_width_voxels,
		float gaussian_covariance_scale);

} // namespace tsdf


#ifndef __MECHANISM_KALMAN_ALGORITHM__
#define __MECHANISM_KALMAN_ALGORITHM__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <pr2_algorithms/algorithm_base.hpp>
#include <limits>
#include <stdexcept>

namespace manipulation_algorithms{
typedef Eigen::Matrix<double, 6, 1> Vector6d;

  /**
    Class that implements the joint position estimator used
    in the mechanism identification problem.
  **/
  class KalmanEstimator : public AlgorithmBase
  {
  public:
    KalmanEstimator();
    ~KalmanEstimator();

    /**
      Estimate the joint location from the measured reaction forces.
      Neglects inertial effects and friction.

      @param p_e1 The position of the rod piece end-effector.
      @param x_dot_e1 The rod piece end-effector twist.
      @param p_e2 The position of the surface piece end-effector.
      @param wrench_e2 The measured wrench of the surface piece end-effector.
      @param dt The elapsed time between estimate steps.
      @output The joint position estimate pc.
    **/
    Eigen::Vector3d estimate(const Eigen::Vector3d &p_e1, const Vector6d &x_dot_e1, const Eigen::Vector3d &p_e2, const Vector6d &wrench_e2, double dt);

    /**
      Use a constant gain instead of computing the Kalman gain, effectively becoming a simple linear observer.
      Not executing the integration steps in the Kalman gain computation might help when the measurements are affected by bias.

      @param p_e1 The position of the rod piece end-effector.
      @param x_dot_e1 The rod piece end-effector twist.
      @param p_e2 The position of the surface piece end-effector.
      @param wrench_e2 The measured wrench of the surface piece end-effector.
      @param dt The elapsed time between estimate steps.
      @output The joint position estimate pc.
    **/
    Eigen::Vector3d estimateConstant(const Eigen::Vector3d &p_e1, const Vector6d &x_dot_e1, const Eigen::Vector3d &p_e2, const Vector6d &wrench_e2, double dt);

    /**
      Initialize the estimator

      @param pc The initial joint position estimate.
    **/
    void initialize(const Eigen::Vector3d &pc);
    void setObserverGain(double gain) {constant_gain_ = gain;}
    virtual bool getParams(const ros::NodeHandle &n);

  private:
    Eigen::Vector3d pc_;
    Eigen::Matrix3d P_, Q_, R_;
    double constant_gain_;
  };
}
#endif


#include <vi/ea.h>

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include <cstdio>
#include <iostream>
#include <random>
#include <string>

// Merciful C++ gods, please absolve me of these sins

#define BUILD_SYSTEM(PARENT_SELECTION, ADULT_SELECTION) \
    vi::ea::build_system( \
        std::default_random_engine{std::random_device{}()}, \
        vi::ea::dynamic_bit_vector_creator{ \
            variables["problem_size"].as<unsigned>(), \
            variables["problem_size"].as<unsigned>()}, \
        PARENT_SELECTION, \
        vi::ea::reproduction::sexual{ \
            variables["mutation_rate"].as<double>(), \
            variables["crossover_rate"].as<double>(), \
            variables["crossover_points"].as<unsigned>()}, \
        ADULT_SELECTION, \
        variables["population_size"].as<unsigned>(), \
        [] (const auto& genotype) \
        { \
            unsigned sum = 0; \
            for (vi::ea::dynamic_bit_vector::size_type i = 0; i != target_value.size(); ++i) \
            { \
                if (target_value[i] == genotype[i]) \
                { \
                    ++sum; \
                } \
            } \
            if (sum == genotype.size()) \
            { \
                solution_found = true; \
            } \
            return static_cast<double>(sum) / static_cast<double>(genotype.size()); \
        })

namespace po = boost::program_options;
po::variables_map variables{};

vi::ea::dynamic_bit_vector target_value{};
bool solution_found = false;

template <typename system_type>
void run_system(system_type&& system)
{
    unsigned generation;
    double fitness_mean, fitness_std_dev;
    typename system_type::individual_type const* best_individual;

    while (true)
    {
        std::tie(generation, fitness_mean, fitness_std_dev, best_individual) = system.stats();

        std::printf("%d %f %f %f %s\n",
                    generation,
                    best_individual->fitness,
                    fitness_mean,
                    fitness_std_dev,
                    boost::lexical_cast<std::string>(best_individual->genotype).c_str());

        if (generation >= variables["generations"].as<unsigned>())
        {
            break;
        }

        system.evolve();
    }
}

int main(int argc, char** argv)
{
    try
    {
        po::options_description description{"Options"};
        description.add_options()
            ("adult_selection", po::value<std::string>()->default_value("full"), "Adult selection (full/mixed/over)")
            ("child_count", po::value<unsigned>()->default_value(150), "Child count used in mixed/over adult selection")
            ("crossover_points", po::value<unsigned>()->default_value(1), "Crossover points")
            ("crossover_rate", po::value<double>()->default_value(1.0), "Crossover rate")
            ("epsilon", po::value<double>()->default_value(0.1), "Tournament probability of selecting random winner")
            ("generations", po::value<unsigned>()->default_value(1000), "Generation count")
            ("group_size", po::value<unsigned>()->default_value(10), "Tournament group size")
            ("mutation_rate", po::value<double>()->default_value(0.001), "Mutation rate")
            ("parent_selection", po::value<std::string>()->default_value("proportionate"), "Parent selection (proportionate/rank/sigma/tournament)")
            ("population_size", po::value<unsigned>()->default_value(100), "Population size")
            ("problem_size", po::value<unsigned>()->default_value(40), "Problem size")
            ("random_target", "Random target string")
            ("rank_max", po::value<double>()->default_value(1.5), "Rank selection pressure ('max')");

        po::store(po::parse_command_line(argc, argv, description), variables);

        target_value = vi::ea::dynamic_bit_vector{variables["problem_size"].as<unsigned>()};

        if (variables.count("random_target"))
        {
            auto random_generator   = std::default_random_engine{std::random_device{}()};
            auto value_distribution = std::bernoulli_distribution{};

            for (vi::ea::dynamic_bit_vector::size_type i = 0; i != target_value.size(); ++i)
            {
                target_value[i] = value_distribution(random_generator);
            }
        }
        else
        {
            target_value.set();
        }

        const auto adult_selection  = variables["adult_selection"].as<std::string>();
        const auto parent_selection = variables["parent_selection"].as<std::string>();

        if (adult_selection == "full" and parent_selection == "proportionate")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::fitness_proportionate{},
                vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "full" and parent_selection == "rank")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::rank{variables["rank_max"].as<double>()},
                vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "full" and parent_selection == "sigma")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::sigma{},
                vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "full" and parent_selection == "tournament")
        {
            auto parent_selection = vi::ea::parent_selection::tournament{
                variables["group_size"].as<unsigned>(),
                variables["epsilon"].as<double>()};

            run_system(BUILD_SYSTEM(parent_selection, vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "mixed" and parent_selection == "proportionate")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::fitness_proportionate{},
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "mixed" and parent_selection == "rank")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::rank{variables["rank_max"].as<double>()},
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "mixed" and parent_selection == "sigma")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::sigma{},
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "mixed" and parent_selection == "tournament")
        {
            auto parent_selection = vi::ea::parent_selection::tournament{
                variables["group_size"].as<unsigned>(),
                variables["epsilon"].as<double>()};

            run_system(BUILD_SYSTEM(
                parent_selection,
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "proportionate")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::fitness_proportionate{},
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "rank")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::rank{variables["rank_max"].as<double>()},
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "sigma")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::sigma{},
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "tournament")
        {
            auto parent_selection = vi::ea::parent_selection::tournament{
                variables["group_size"].as<unsigned>(),
                variables["epsilon"].as<double>()};

            run_system(BUILD_SYSTEM(
                parent_selection,
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
    }
    catch (const po::error& error)
    {
        std::cerr << "error: " << error.what() << std::endl;
        return EXIT_FAILURE;
    }

    return solution_found ? 1 : 0;
}


#include "CALPHADConcSolverBinary.h"
#include "CALPHADFunctions.h"
#include "CALPHADSpeciesPhaseGibbsEnergy.h"
#include "PhysicalConstants.h"

#include <chrono>

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <iomanip>
#include <iostream>
#include <string>

#include <omp.h>

namespace pt = boost::property_tree;

typedef std::chrono::high_resolution_clock Clock;

int main(int argc, char* argv[])
{
    const int N = 10000000;

#ifdef _OPENMP
    printf("Compiled by an OpenMP-compliant implementation.\n");
#endif

    std::cout << "Run test with " << omp_get_max_threads() << " threads"
              << std::endl;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadAuNi.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    double temperature = 1450.;

    CalphadDataType LmixPhaseL[4][MAX_POL_T_INDEX];
    CalphadDataType LmixPhaseA[4][MAX_POL_T_INDEX];

    {
        std::string dbnamemixL("LmixPhaseL");
        pt::ptree Lmix0_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix0_db, LmixPhaseL);
    }
    {
        std::string dbnamemixA("LmixPhaseA");
        pt::ptree Lmix1_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix1_db, LmixPhaseA);
    }

    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseL[2];
    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseA[2];

    {
        pt::ptree& species0_db = calphad_db.get_child("SpeciesA");
        std::string dbnameL("PhaseL");
        std::string dbnameA("PhaseA");

        g_species_phaseL[0].initialize("L0", species0_db.get_child(dbnameL));
        g_species_phaseA[0].initialize("A0", species0_db.get_child(dbnameA));

        pt::ptree& speciesB_db = calphad_db.get_child("SpeciesB");
        g_species_phaseL[1].initialize("L1", speciesB_db.get_child(dbnameL));
        g_species_phaseA[1].initialize("A1", speciesB_db.get_child(dbnameA));
    }

    CalphadDataType fA[2];
    fA[0] = g_species_phaseL[0].fenergy(temperature);
    fA[1] = g_species_phaseA[0].fenergy(temperature);
    // std::cout<<"fA[0]="<<fA[0]<<", fA[1]="<<fA[1]<<std::endl;

    CalphadDataType fB[2];
    fB[0] = g_species_phaseL[1].fenergy(temperature);
    fB[1] = g_species_phaseA[1].fenergy(temperature);
    // std::cout<<"fB[0]="<<fB[0]<<", fB[1]="<<fB[1]<<std::endl;

    CalphadDataType Lmix_L[4];
    for (int i = 0; i < 4; i++)
        Lmix_L[i] = LmixPhaseL[i][0] + temperature * LmixPhaseL[i][1];
    // for(int i=0;i<4;i++)std::cout<<"Lmix_L["<<i<<"]="<<Lmix_L[i]<<std::endl;

    CalphadDataType Lmix_A[4];
    for (int i = 0; i < 4; i++)
        Lmix_A[i] = LmixPhaseA[i][0] + temperature * LmixPhaseA[i][1];
    // for(int i=0;i<4;i++)std::cout<<"Lmix_A["<<i<<"]="<<Lmix_A[i]<<std::endl;

    const double RTinv
        = 1.0 / (Thermo4PFM::gas_constant_R_JpKpmol * temperature);

    double sol[2] = { 0.5, 0.5 };

    double deviation = 1.e-4;

    double* xhost = new double[2 * N];
    for (int i = 0; i < 2 * N; i++)
    {
        xhost[i] = -1.;
    }

    // Host solve
    {
        short* nits = new short[N];
        auto t1     = Clock::now();

#pragma omp parallel for
        for (int i = 0; i < N; i++)
        {
            if (!omp_is_initial_device()) abort();

            xhost[2 * i]     = sol[0];
            xhost[2 * i + 1] = sol[1];
            double hphi      = 0.5 + (i % 100) * deviation;
            double c0        = 0.3;
            Thermo4PFM::CALPHADConcSolverBinary solver;
            solver.setup(c0, hphi, RTinv, Lmix_L, Lmix_A, fA, fB);
            nits[i] = solver.ComputeConcentration(&xhost[2 * i], 1.e-8, 50);
        }
        auto t2 = Clock::now();
        long int usec
            = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1)
                  .count();
        std::cout << "Host time/us/solve:   " << (double)usec / (double)N
                  << std::endl;

        std::cout << std::setprecision(12);

        int n = N > 20 ? 20 : N;

        for (int i = 0; i < n; i++)
        {
            std::cout << "Host: x=" << xhost[2 * i] << "," << xhost[2 * i + 1]
                      << std::endl;
            std::cout << "nits=" << nits[i] << std::endl;
        }
        delete[] nits;
    }

    // Device solve
    {
        double* xdev = new double[2 * N];
        for (int i = 0; i < 2 * N; i++)
        {
            xdev[i] = -1;
        }

        short* nits = new short[N];

        auto t1 = Clock::now();

// clang-format off
#pragma omp target map(to : sol) \
                   map(tofrom : xdev) \
                   map(to : fA, fB, Lmix_L, Lmix_A) \
                   map(to : RTinv) \
                   map(from : nits)                                                     \
// clang-format on
        {
#pragma omp teams distribute parallel for
            for (int i = 0; i < N; i++)
            {
                // if( omp_is_initial_device() ) abort();
                xdev[2 * i]     = sol[0];
                xdev[2 * i + 1] = sol[1];

                double hphi = 0.5 + (i % 100) * deviation;
                double c0   = 0.3;
                class Thermo4PFM::CALPHADConcSolverBinary solver;
                solver.setup(c0, hphi, RTinv, Lmix_L, Lmix_A, fA, fB);
                nits[i] = solver.ComputeConcentration(&xdev[2 * i], 1.e-8, 50);
            }
        }

        auto t2 = Clock::now();
        long int usec
            = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1)
                  .count();
        std::cout << "Device time/us/solve: " << (double)usec / (double)N
                  << std::endl;

        double tol = 1.e-8;
        for (int i = 0; i < N; i++)
        {
            if (std::abs(xdev[2 * i] - xhost[2 * i]) > tol
                || std::abs(xdev[2 * i + 1] - xhost[2 * i + 1]) > tol || N < 20)
            {
                std::cout << "Device: x=" << xdev[2 * i] << ","
                          << xdev[2 * i + 1] << std::endl;
                std::cout << "Difference: " << xdev[2 * i] - xhost[2 * i]
                          << ", " << xdev[2 * i + 1] - xhost[2 * i + 1]
                          << std::endl;
                std::cout << "nits[" << i << "]=" << nits[i] << std::endl;
            }
        }
        delete[] nits;
        delete[] xdev;
    }

    delete[] xhost;
}


#include "SW.hpp"

#include "union_find.hpp"

#include <cmath>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <boost/timer/timer.hpp>

namespace ising{

Worker::Worker(alps::Parameters const& params):
  SuperClass(params),
  mcs_(params),
  T_(evaluate("T", params)),
  beta_(1.0/T_),
  J_(params.value_or_default("J", 1.0)),
  nsites_(num_sites()),
  nbonds_(num_bonds()),
  spins_(nsites_, 1),
  V2_(nsites_*nsites_)
{
  init(params);
}

void Worker::init(alps::Parameters const& params)
{
  if(params.defined("MODEL")){
    std::string modelname = params["MODEL"];
    if(modelname ==  std::string("Ising")){
      ene_const_ = nbonds_ * J_;
      q_ = 2;
      J_ *= 2;
    }else if(modelname == std::string("Potts")){
      q_ = evaluate("q", params);
      ene_const_ = 0.0;
    }else{
      std::cerr << modelname << " model is not implemented.";
      exit(127);
    }
  }else{
    std::cerr << "parameter \"MODEL\" is not defined.";
    exit(127);
  }
  negspin_ = 1.0 / (q_-1);
  p_ = -boost::math::expm1(-beta_*J_);
  m2_coeff_ = negspin_;
  m4_coeff_ = (1.0 + negspin_*negspin_*negspin_)/q_;
  ene_coeff_ = J_ / p_;
}

void Worker::init_observables(alps::Parameters const&, alps::ObservableSet& obs)
{
  obs << alps::RealObservable("Time");
  obs << alps::RealObservable("Speed");
  obs << alps::RealObservable("Number of Sites");
  obs << alps::RealObservable("Magnetization^2");
  obs << alps::RealObservable("Magnetization^4");
  obs << alps::RealObservable("|Magnetization|");
  obs << alps::RealObservable("Energy");
  obs << alps::RealObservable("Susceptibility");
  obs << alps::RealObservable("Activated Bonds");
  obs << alps::RealObservable("Activated Bonds^2");
}

void Worker::run(alps::ObservableSet& obs)
{
  boost::timer::cpu_timer tm;

  ++mcs_;

  // make clusters

  int activated = 0;
  std::vector<union_find::Node> nodes(nsites_);
  foreach(bond_descriptor b, bonds()){
    const int lsite = source(b);
    const int rsite = target(b);
    if( spins_[lsite] == spins_[rsite] && random_01() < p_){
      union_find::unify(nodes, lsite, rsite);
      ++activated;
    }
  }

  // flip spins

  double ma = 0.0;
  std::vector<int> cluster_size2;
  std::vector<int> cluster_spin;
  int id = 0;
  for(int site=0; site<nsites_; ++site){
    int ri = union_find::root_index(nodes, site);
    union_find::Node &root = nodes[ri];
    if(root.id == -1){
      root.id = id++;
      cluster_size2.push_back(root.size*root.size);
      cluster_spin.push_back(q_ * random_01());
    }
    spins_[site] = cluster_spin[root.id];
    ma += spins_[site] == 0 ? 1.0 : -negspin_;
  }

  if(!is_thermalized()){
    return;
  }

  // measure

  const int nc = cluster_size2.size();
  double m2 = 0.0;
  double m4 = 0.0;
  for(int ci=0; ci < nc; ++ci){
    m4 += m4_coeff_ * cluster_size2[ci] * cluster_size2[ci];
    m4 += 6 * cluster_size2[ci] * m2;
    m2 += m2_coeff_ * cluster_size2[ci];
  }
  const double V2 = nsites_ * nsites_;
  m2 /= V2;
  m4 /= (V2*V2);

  double ene = ene_const_ - ene_coeff_ * activated;

  obs["Magnetization^2"] << m2;
  obs["Magnetization^4"] << m4;
  obs["|Magnetization|"] << std::abs(ma) / nsites_;
  obs["Susceptibility"] << m2 * nsites_ *  beta_;
  obs["Number of Sites"] << 1.0 * nsites_;
  obs["Energy"] << ene;
  obs["Activated Bonds"] << 1.0*activated;
  obs["Activated Bonds^2"] << 1.0*activated*activated;

  const double sec = tm.elapsed().wall * 1.0e-9;
  obs["Time"] << sec;
  obs["Speed"] << 1.0/sec;
}

void Evaluator::evaluate(alps::ObservableSet& obs) const
{
  const double T = alps::evaluate("T", params_);
  const double beta = 1.0/T;

  alps::RealObsevaluator m2 = obs["Magnetization^2"];
  alps::RealObsevaluator m4 = obs["Magnetization^4"];
  alps::RealObsevaluator ma = obs["|Magnetization|"];
  alps::RealObsevaluator nsites = obs["Number of Sites"];
  alps::RealObsevaluator n = obs["Activated Bonds"];
  alps::RealObsevaluator n2 = obs["Activated Bonds^2"];

  alps::RealObsevaluator binder = m4 / (m2*m2);
  binder.rename("Binder Ratio");
  obs.addObservable(binder);

  alps::RealObsevaluator csus = (m2 - ma * ma) * nsites * beta;
  csus.rename("Connected Susceptibility");
  obs.addObservable(csus);

  const double J = static_cast<double>(params_.value_or_default("J", 1.0)) * (params_["MODEL"]==std::string("Ising") ? 2.0:1.0);

  const double bj = -beta*J;
  const double coeff = bj / (boost::math::expm1(bj));
  alps::RealObsevaluator spec = (n2 - n*n + std::exp(bj)*n) * coeff * coeff;
  spec.rename("Specific Heat");
  obs.addObservable(spec);
}

} // end of namespace ising


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_TREVC_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_TREVC_HPP

#include <complex>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>
// #include <boost/numeric/bindings/traits/std_vector.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Compute eigenvectors of Schur matrix (computed by gees).
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * trevc() computes the eigenvectors using the Schur factorization
     *
     * Let  A = U * S * herm(U), then trecv computes the eigenvectors of
     * S, and may optionally apply them to U.
     *
     * To compute the Schur factorization, see gees.
     */

    namespace detail {
      inline
      void trevc (char const side, char const howmny, const logical_t* select, int const n,
                 float* t, int const ldt, float* vl, int const ldvl, float* vr, int const ldvr,
		 int const mm, int& m, float* work, int& info)
      {
        LAPACK_STREVC (&side, &howmny, select, &n, t, &ldt, vl, &ldvl, vr, &ldvr, &mm, &m, work, &info);
      }

      inline
      void trevc (char const side, char const howmny, const logical_t* select, int const n,
                 double* t, int const ldt, double* vl, int const ldvl, double* vr, int const ldvr,
		 int const mm, int& m, double* work, int& info)
      {
        LAPACK_DTREVC (&side, &howmny, select, &n, t, &ldt, vl, &ldvl, vr, &ldvr, &mm, &m, work, &info);
      }

      inline
      void trevc (char const side, char const howmny, const logical_t* select, int const n,
                 traits::complex_f* t, int const ldt, traits::complex_f* vl, int const ldvl, traits::complex_f* vr, int const ldvr,
                 int const mm, int& m, traits::complex_f* work, int& info)
      {
        LAPACK_CTREVC (&side, &howmny, select, &n, traits::complex_ptr(t), &ldt, traits::complex_ptr(vl), &ldvl,
			traits::complex_ptr(vr), &ldvr, &mm, &m, traits::complex_ptr(work+n), traits::complex_ptr(work), &info);
      }

      inline
      void trevc (char const side, char const howmny, const logical_t* select, int const n,
                  traits::complex_d* t, int const ldt, traits::complex_d* vl, int const ldvl, traits::complex_d* vr, int const ldvr,
		  int const mm, int& m, traits::complex_d* work, int& info)
      {
        LAPACK_ZTREVC (&side, &howmny, select, &n, traits::complex_ptr(t), &ldt,
      	               traits::complex_ptr(vl), &ldvl, traits::complex_ptr(vr), &ldvr,
		       &mm, &m, traits::complex_ptr(work+n), traits::complex_ptr(work), &info);
      }

    }

    // Compute Schur factorization with Schur vectors
    template <typename MatrT, typename VL, typename VR, typename Work>
    inline
    int trevc (char const side, char const howmny, MatrT& t, VL& vl, VR& vr, Work& work) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrT>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<VL>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<VR>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (t);
      assert (n == traits::matrix_size2 (t));
      assert (n == traits::matrix_size1 (vl));
      assert (n == traits::matrix_size2 (vl));
      assert (n == traits::matrix_size1 (vr));
      assert (n == traits::matrix_size2 (vr));
      assert (3*n <= traits::vector_size (work));

      logical_t* select=0;

      int mm=n;
      int m;
      int info;
      detail::trevc (side, howmny, select, n,
                    traits::matrix_storage (t),
                    traits::leading_dimension (t),
                    traits::matrix_storage (vl),
                    traits::leading_dimension (vl),
                    traits::matrix_storage (vr),
                    traits::leading_dimension (vr),
		    mm,
		    m,
                    traits::vector_storage (work),
                    info);
      return info;
    }

  }

}}}

#endif


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

    @copyright 2016 NumScale SAS

  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_FUNCTION_ATANPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ATANPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse tangent in \f$\pi\f$ multiples.

    @par Header <boost/simd/function/atanpi.hpp>

    @par Note

      For every parameter of floating type `atanpi(x)`
      returns the arc @c r in the interval  \f$[-0.5, 0.5[\f$
      such that <tt>tanpi(r) == x</tt>.

    @see atan2, atan2d, atand, atan, tanpi

    @par Example:

      @snippet atanpi.cpp atanpi

    @par Possible output:

      @snippet atanpi.txt atanpi

  **/
  IEEEValue atanpi(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/atanpi.hpp>
#include <boost/simd/function/simd/atanpi.hpp>

#endif


/**
 * \ file Chebyshev2Filter.cpp
 */

#include <ATK/EQ/Chebyshev2Filter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

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

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2LowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev2LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.533158264494777));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2LowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev2LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8413951416367915));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2LowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev2LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.38439870861360015));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2LowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev2LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8411929081798066));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2HighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev2HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.9999904942771862));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2HighPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev2HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8413951415475837));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2HighPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev2HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.9999976499956404));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2HighPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev2HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.9993415485050974));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev2BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8413951423132554));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev2BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8337339391384047));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev2BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.8335118338740579));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev2BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8413950585465586));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandStopCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev2BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8413951519207091));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandStopCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev2BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.9998108684766274));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandStopCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev2BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.999863033497463));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev2BandStopCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev2BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8413938019260473));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


/*
    Copyright 2012 Chung-Lin Wen, Davide Anastasia
    Use, modification and distribution are subject to the Boost Software License,
    Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt).
*/

/*************************************************************************************************/

#ifndef BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_XYZ_HPP
#define BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_XYZ_HPP

////////////////////////////////////////////////////////////////////////////////////////
/// \file xyz.hpp
/// \brief Support for CIE XYZ color space
/// \author Chung-Lin Wen, Davide Anastasia \n
///
/// \date 2012 \n
///
////////////////////////////////////////////////////////////////////////////////////////

#include <boost/cast.hpp>

namespace boost{ namespace gil {

/// \addtogroup ColorNameModel
/// \{
namespace xyz_color_space
{
/// \brief x Color Component
struct x_t {};
/// \brief y Color Component
struct y_t {};
/// \brief z Color Component
struct z_t {};
}
/// \}

/// \ingroup ColorSpaceModel
typedef mpl::vector3< xyz_color_space::x_t
                    , xyz_color_space::y_t
                    , xyz_color_space::z_t
                    > xyz_t;

/// \ingroup LayoutModel
typedef layout<xyz_t> xyz_layout_t;

GIL_DEFINE_ALL_TYPEDEFS( 32f, xyz );

/// \ingroup ColorConvert
/// \brief RGB to XYZ
/// Link
/// \note rgb_t is assumed to be sRGB D65
template <>
struct default_color_converter_impl< rgb_t, xyz_t >
{
private:
    GIL_FORCEINLINE
    bits32f inverse_companding(bits32f sample) const
    {
        if ( sample > 0.04045f )
        {
            return powf((( sample + 0.055f ) / 1.055f ), 2.4f);
        }
        else
        {
            return ( sample / 12.92f );
        }
    }

public:
    template <typename P1, typename P2>
    void operator()( const P1& src, P2& dst ) const
    {
        using namespace xyz_color_space;

        bits32f red(
                    inverse_companding(
                        channel_convert<bits32f>( get_color( src, red_t() ))
                        )
                    );
        bits32f green(
                    inverse_companding(
                        channel_convert<bits32f>( get_color( src, green_t() ))
                        )
                    );
        bits32f blue(
                    inverse_companding(
                        channel_convert<bits32f>( get_color( src, blue_t() ))
                        )
                    );

        get_color( dst, x_t() ) =
                red * 0.4124564f +
                green * 0.3575761f +
                blue * 0.1804375f;
        get_color( dst, y_t() ) =
                red * 0.2126729f +
                green * 0.7151522f +
                blue * 0.0721750f;
        get_color( dst, z_t() ) =
                red * 0.0193339f +
                green * 0.1191920f +
                blue * 0.9503041f;
    }
};

/// \ingroup ColorConvert
/// \brief XYZ to RGB
template <>
struct default_color_converter_impl<xyz_t,rgb_t>
{
private:
    GIL_FORCEINLINE
    bits32f companding(bits32f sample) const
    {
        if ( sample > 0.0031308f )
        {
            return ( 1.055f * powf( sample, 1.f/2.4f ) - 0.055f );
        }
        else
        {
            return ( 12.92f * sample );
        }
    }

public:
    template <typename P1, typename P2>
    void operator()( const P1& src, P2& dst) const
    {
        using namespace xyz_color_space;

        // Note: ideally channel_convert should be compiled out, because xyz_t
        // is bits32f natively only
        bits32f x( channel_convert<bits32f>( get_color( src, x_t() ) ) );
        bits32f y( channel_convert<bits32f>( get_color( src, y_t() ) ) );
        bits32f z( channel_convert<bits32f>( get_color( src, z_t() ) ) );

        get_color(dst,red_t())  =
                channel_convert<typename color_element_type<P2, red_t>::type>(
                    companding( x *  3.2404542f +
                                y * -1.5371385f +
                                z * -0.4985314f )
                    );
        get_color(dst,green_t()) =
                channel_convert<typename color_element_type<P2, green_t>::type>(
                    companding( x * -0.9692660f +
                                y *  1.8760108f +
                                z *  0.0415560f )
                    );
        get_color(dst,blue_t()) =
                channel_convert<typename color_element_type<P2, blue_t>::type>(
                    companding( x *  0.0556434f +
                                y * -0.2040259f +
                                z *  1.0572252f )
                    );
    }
};

} // namespace gil
} // namespace boost

#endif // BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_XYZ_HPP


#pragma once

#include <Eigen/Core>
#include <autodiff/autodiff_types.hpp>

namespace ccd {
namespace autogen {

    template <typename T>
    using Vector2T = Eigen::Matrix<T, 2, 1>;

    /**
    * Compute the volume of intersection for an edge given a time
    * of intersection (toi) and position of intersection (alpha)
    *
    * \f$V = (1-\tau_I)\sqrt{\epsilon^2 \|e(\tau_I)\|^2 + (U_{ij} \cdot
    * e(\tau_I)^\perp)^2}\f$
    *
    *     @param V_{ijk}          : Vertices positions.
    *     @param U_{ijk}          : Vertices displacements.
    *     @param epsilon          : The time scale used for minimal volume.
    *
    * @return                     : The space-time interference volume.
    */
    template <typename T>
    T space_time_collision_volume(
        const Eigen::Vector2d& Vi,
        const Eigen::Vector2d& Vj,
        const Vector2T<T>& Ui,
        const Vector2T<T>& Uj,
        const T& toi, const T& alpha,
        const double epsilon);

}
}

#include "auto_collision_volume.ipp"


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Ilias Khairullin <ilias@nil.foundation>
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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 CRYPTO3_HASH_POSEIDON_MDS_MATRIX_HPP
#define CRYPTO3_HASH_POSEIDON_MDS_MATRIX_HPP

#include <nil/crypto3/algebra/matrix/matrix.hpp>
#include <nil/crypto3/algebra/matrix/math.hpp>
#include <nil/crypto3/algebra/matrix/operators.hpp>
#include <nil/crypto3/algebra/vector/vector.hpp>
#include <nil/crypto3/algebra/vector/math.hpp>
#include <nil/crypto3/algebra/vector/operators.hpp>

#include <nil/crypto3/hash/detail/poseidon/poseidon_policy.hpp>

#include <boost/assert.hpp>

namespace nil {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<typename FieldType, std::size_t Arity, std::size_t PartRounds>
                struct poseidon_mds_matrix {
                    typedef poseidon_policy<FieldType, Arity, PartRounds> policy_type;
                    typedef typename FieldType::value_type element_type;

                    constexpr static const std::size_t state_words = policy_type::state_words;
                    constexpr static const std::size_t half_full_rounds = policy_type::half_full_rounds;
                    constexpr static const std::size_t part_rounds = policy_type::part_rounds;

                    typedef algebra::matrix<element_type, state_words, state_words> mds_matrix_type;
                    typedef algebra::vector<element_type, state_words> state_vector_type;
                    typedef algebra::vector<element_type, state_words - 1> substate_vector_type;
                    typedef algebra::matrix<element_type, state_words - 1, state_words - 1> mds_submatrix_type;

                    inline void product_with_mds_matrix(state_vector_type &A_vector) const {
                        A_vector = algebra::vectmatmul(A_vector, mds_matrix);
                    }

                    constexpr void product_with_inverse_mds_matrix_noalias(const state_vector_type &A_vector_in,
                                                                           state_vector_type &A_vector_out) const {
                        A_vector_out = algebra::vectmatmul(A_vector_in, mds_matrix_inverse);
                    }

                    inline void product_with_equivalent_mds_matrix_init(state_vector_type &A_vector,
                                                                        std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number == half_full_rounds,
                                         "wrong using: product_with_equivalent_mds_matrix_init");
                        A_vector = algebra::vectmatmul(A_vector, get_M_i());
                    }

                    inline void product_with_equivalent_mds_matrix(state_vector_type &A_vector,
                                                                   std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number >= half_full_rounds &&
                                             round_number < half_full_rounds + part_rounds,
                                         "wrong using: product_with_equivalent_mds_matrix");
                        const std::size_t matrix_number_base = part_rounds - (round_number - half_full_rounds) - 1;
                        const substate_vector_type &v = get_v(matrix_number_base);
                        state_vector_type temp_vector;
                        element_type A_0 = A_vector[0];
                        temp_vector[0] = get_M_0_0();
                        for (std::size_t i = 1; i < state_words; i++) {
                            temp_vector[i] = get_w_hat(matrix_number_base)[i - 1];
                        }
                        A_vector[0] = algebra::dot(A_vector, temp_vector);
                        for (std::size_t i = 1; i < state_words; i++) {
                            A_vector[i] = A_0 * v[i - 1] + A_vector[i];
                        }
                    }

                    // private:
#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                    constexpr
#endif
                    inline mds_matrix_type generate_mds_matrix() {
                        mds_matrix_type new_mds_matrix;
                        for (std::size_t i = 0; i < state_words; i++) {
                            for (std::size_t j = 0; j < state_words; j++) {
                                new_mds_matrix[i][j] = element_type(i + j + state_words).inversed();
                            }
                        }
                        return new_mds_matrix;
                    }

                    struct equivalent_mds_matrix_type {
                        typedef std::array<substate_vector_type, part_rounds> subvectors_array;

#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                        constexpr
#endif
                        equivalent_mds_matrix_type(const mds_matrix_type &mds_matrix) :
                            M_i(algebra::get_identity<element_type, state_words>()), w_hat_list(), v_list(), M_0_0() {
                            mds_matrix_type M_mul(mds_matrix);
                            mds_submatrix_type M_hat_inverse;
                            substate_vector_type M_mul_column_slice;

                            for (std::size_t i = 0; i < part_rounds; i++) {
                                M_hat_inverse =
                                    algebra::inverse(algebra::submat<state_words - 1, state_words - 1>(M_mul, 1, 1));
                                w_hat_list[i] = algebra::matvectmul(
                                    M_hat_inverse, algebra::slice<state_words - 1>(M_mul.column(0), 1));
                                v_list[i] = algebra::slice<state_words - 1>(M_mul.row(0), 1);
                                for (std::size_t j = 1; j < state_words; j++) {
                                    for (std::size_t k = 1; k < state_words; k++) {
                                        M_i[j][k] = M_mul[j][k];
                                    }
                                }
                                M_mul = algebra::matmul(mds_matrix, M_i);
                            }
                            M_0_0 = mds_matrix[0][0];
                        }

                        mds_matrix_type M_i;
                        subvectors_array w_hat_list;
                        subvectors_array v_list;
                        element_type M_0_0;
                    };

                    inline const substate_vector_type &get_w_hat(std::size_t w_hat_number) const {
                        return equivalent_mds_matrix.w_hat_list[w_hat_number];
                    }
                    inline const substate_vector_type &get_v(std::size_t v_number) const {
                        return equivalent_mds_matrix.v_list[v_number];
                    }
                    inline const element_type &get_M_0_0() const {
                        return equivalent_mds_matrix.M_0_0;
                    }
                    inline const mds_matrix_type &get_M_i() const {
                        return equivalent_mds_matrix.M_i;
                    }

#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                    constexpr
#endif
                    poseidon_mds_matrix() :
                        mds_matrix(generate_mds_matrix()), mds_matrix_inverse(algebra::inverse(mds_matrix)),
                        equivalent_mds_matrix(mds_matrix) {
                    }

                    mds_matrix_type mds_matrix;
                    mds_matrix_type mds_matrix_inverse;
                    equivalent_mds_matrix_type equivalent_mds_matrix;
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_HASH_POSEIDON_MDS_MATRIX_HPP


#include <cmath>
#include <cstdio>
#include <vector>

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

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <ros/ros.h>

#include <simple_fiducial_mapping/mapping.h>
#include <simple_fiducial_mapping/ceres_backend.h>

namespace simple_fiducial_mapping
{
bool areObservationsGoodForSlam(const std::map<size_t, Eigen::Vector3d>& observations)
{
  if (observations.size() < 3)
  {
    ROS_ERROR_STREAM("We got less than 3 observations.");
    return false;
  }

  // Project observations onto ideal image plane.
  std::vector<Eigen::Vector2d> ideal_image_plane_points;
  for (const auto& observation_pair : observations)
  {
    const Eigen::Vector3d& position = observation_pair.second;
    if (position.z() >= OBSERVATION_MIN_DISTANCE)
      // Throw out any observations too close to the camera (they have high error after reprojection).
      ideal_image_plane_points.push_back(Eigen::Vector2d(position.x() / position.z(), position.y() / position.z()));
  }

  if (ideal_image_plane_points.size() < 3)
  {
    ROS_ERROR_STREAM("The number of ideal image plane points is less than 3.");
    return false;
  }

  // Unit vectors along which we'll check point spread.
  const std::vector<Eigen::Vector2d> direction_vectors{
    { cos(0.0), sin(0.0) }, { cos(M_PI / 4.0), sin(M_PI / 4.0) }, { cos(M_PI / 2.0), sin(M_PI / 2.0) },
  };

  // Spread must be large enough along all of our direction vectors.
  for (const auto direction : direction_vectors)
  {
    double min_val = direction.dot(ideal_image_plane_points[0]);
    double max_val = direction.dot(ideal_image_plane_points[0]);
    for (const auto& point : ideal_image_plane_points)
    {
      double dot_val = direction.dot(point);
      if (dot_val < min_val)
        min_val = dot_val;
      if (dot_val > max_val)
        max_val = dot_val;
    }
    double spread = max_val - min_val;
    // Intentionally written as a negative check so that a spread of NaN will return false.
    if (!(spread > OBSERVATION_MIN_SPREAD))
    {
      ROS_ERROR_STREAM(
          "The current spread: " << spread << " is less than the observation min spread: " << OBSERVATION_MIN_SPREAD);
      return false;
    }
  }

  return true;
}

std::map<size_t, Eigen::Vector3d>
tagDetectionsToObservations(const apriltags2_ros::AprilTagDetectionArray& detections_msg)
{
  std::map<size_t, Eigen::Vector3d> observations;
  for (const auto& detection : detections_msg.detections)
  {
    assert(detection.id.size() == 1);
    size_t fiducial_id = detection.id[0];
    const auto& position_msg = detection.pose.pose.pose.position;
    observations[fiducial_id] = Eigen::Vector3d(position_msg.x, position_msg.y, position_msg.z);
  }
  return observations;
}

bool localize(const std::map<size_t, Eigen::Vector3d>& observations, const Eigen::Isometry3d& camera_pose_guess,
              double max_rmse, Eigen::Isometry3d* computed_camera_pose, MapGraph* map_graph)
{
  if (!areObservationsGoodForSlam(observations))
  {
    ROS_ERROR("Observations are not spread out; not localizing");
    return false;
  }

  Eigen::Quaterniond orientation_guess(camera_pose_guess.rotation());
  Eigen::Vector3d position_guess = camera_pose_guess.translation();

  ROS_DEBUG_STREAM("Localizing from " << observations.size() << " observations");

  size_t camera_pose_id =
      addCameraPose(0, CameraPose{ position_guess, orientation_guess, false /* is_constant */ }, map_graph);
  for (const auto& observation_pair : observations)
  {
    size_t fiducial_id = observation_pair.first;
    Eigen::Vector3d fiducial_position = observation_pair.second;
    addObservation(camera_pose_id, fiducial_id, fiducial_position, map_graph);
  }
  OptimizationInfo localization_optimization_info = bundleAdjust(20, map_graph);
  double rmse = sqrt(computeCartesianMeanSquareError(localization_optimization_info));
  ROS_INFO("Localized with RMSE of %f", rmse);
  if (rmse > max_rmse)
  {
    return false;
  }

  const CameraPose& camera_pose = map_graph->camera_poses.at(camera_pose_id);
  *computed_camera_pose = Eigen::Translation3d(camera_pose.position) * camera_pose.orientation;

  removeCameraPose(camera_pose_id, map_graph);

  return true;
}

}  // namespace simple_fiducial_mapping


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../public/KMeans.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidDataSet.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/FluidTensor.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <queue>
#include <string>

namespace fluid {
namespace algorithm {

class SKMeans : public KMeans
{

public:
  void train(const FluidDataSet<std::string, double, 1>& dataset, index k,
             index maxIter)
  {
    using namespace Eigen;
    using namespace _impl;
    assert(!mTrained || (dataset.pointSize() == mDims && mK == k));
    MatrixXd dataPoints = asEigen<Matrix>(dataset.getData());
    MatrixXd dataPointsT = dataPoints.transpose();
    if (mTrained) { mAssignments = assignClusters(dataPointsT);}
    else
    {
      mK = k;
      mDims = dataset.pointSize();
      initMeans(dataPoints);
    }

    while (maxIter-- > 0)
    {
      mEmbedding = mMeans.matrix() * dataPointsT;
      auto assignments = assignClusters(mEmbedding);
      if (!changed(assignments)) { break; }
      else
        mAssignments = assignments;
      updateEmbedding();
      computeMeans(dataPoints);
    }
    mTrained = true;
  }

  void encode(RealMatrixView data, RealMatrixView out,
                 double alpha = 0.25) const
  {
    using namespace Eigen;
    MatrixXd points = _impl::asEigen<Matrix>(data).transpose();
    MatrixXd embedding = (mMeans.matrix() * points).array() - alpha;
    embedding = (embedding.array() > 0).select(embedding, 0).transpose();
    out <<= _impl::asFluid(embedding);
  }

private:

  void initMeans(Eigen::MatrixXd& dataPoints)
  {
    using namespace Eigen;
    mMeans = ArrayXXd::Zero(mK, mDims);
    mAssignments =
        ((0.5 + (0.5 * ArrayXd::Random(dataPoints.rows()))) * (mK - 1))
            .round()
            .cast<int>();
    mEmbedding = MatrixXd::Zero(mK, dataPoints.rows());
    for (index i = 0; i < dataPoints.rows(); i++)
      mEmbedding(mAssignments(i), i) = 1;
    computeMeans(dataPoints);
  }

  void updateEmbedding()
  {
    for (index i = 0; i < mAssignments.cols(); i++)
    {
      double val = mEmbedding(mAssignments(i), i);
      mEmbedding.col(i).setZero();
      mEmbedding(mAssignments(i), i) = val;
    }
  }

  Eigen::VectorXi assignClusters(Eigen::MatrixXd& embedding) const
  {
    Eigen::VectorXi assignments = Eigen::VectorXi::Zero(embedding.cols());
    for (index i = 0; i < embedding.cols(); i++)
    {
      Eigen::VectorXd::Index maxIndex;
      embedding.col(i).maxCoeff(&maxIndex);
      assignments(i) = static_cast<int>(maxIndex);
    }
    return assignments;
  }

  void computeMeans(Eigen::MatrixXd& dataPoints)
  {
    mMeans = mEmbedding * dataPoints;
    mMeans.matrix().rowwise().normalize();
  }

private:
  Eigen::MatrixXd mEmbedding;
};
} // namespace algorithm
} // namespace fluid


#include "tracker.hpp"

#include <Eigen/Dense>

using khmot::Observation;
using khmot::Tracker;

int main()
{
  Observation obs;
  obs.kalmanObs.state << 0.1, 0.2, 0.3, 0.0, 0.0, 0.0;
  obs.kalmanObs.covariance = Eigen::MatrixXd::Identity(6, 6);
  obs.kalmanObs.timestamp = 0.0;
  std::vector<Observation> v;
  v.push_back(obs);

  Tracker t;
  for (int i = 0; i < 10; ++i) {
    t.update(v, static_cast<double>(i));
    v[0].kalmanObs.state(0) += 0.001;
    v[0].kalmanObs.timestamp += 1.0;
  }

  return 0;
}


/**
 * @file    SchurComplementDenseSolver.cpp
 * @brief   Abstract linear solver class of Ax = b
 * @author  Jing Dong
 * @date    Jun 24, 2019
 */

#include <minisam/linear/SchurComplementDenseSolver.h>
#include <minisam/utils/Timer.h>

#include <Eigen/Cholesky>

namespace minisam {

/* ************************************************************************** */
LinearSolverStatus SchurComplementDenseSolver::solve(
    const Eigen::SparseMatrix<double>& AtA, const Eigen::VectorXd& Atb,
    Eigen::VectorXd& x) {
  // static auto sr_timer = global_timer().getTimer("* Schur solve reduced");
  // static auto se_timer = global_timer().getTimer("* Schur solve elimiated");

  const int rs_until_idx = sc_ordering_->reducedSysUntilIndex();

  Eigen::MatrixXd H_reduced_lower;
  Eigen::SparseMatrix<double> Her;
  Eigen::SparseMatrix<double> He_inv_lower;
  Eigen::VectorXd g_reduced;

  // build reduced system  H_reduced * x_reduced = g_reduced
  LinearSolverStatus build_status = buildReducedSystem_(
      AtA, Atb, H_reduced_lower, Her, He_inv_lower, g_reduced);

  if (build_status != LinearSolverStatus::SUCCESS) {
    return build_status;
  }

  // sr_timer->tic();

  // reduced solution
  Eigen::VectorXd x_reduced;
  LinearSolverStatus reduced_status =
      reduced_solver_->solve(H_reduced_lower, g_reduced, x_reduced);

  if (reduced_status != LinearSolverStatus::SUCCESS) {
    return reduced_status;
  }

  // sr_timer->toc();
  // se_timer->tic();

  Eigen::VectorXd x_elimiated =
      He_inv_lower.selfadjointView<Eigen::Lower>() *
      (Atb.tail(AtA.rows() - rs_until_idx) - Her * x_reduced);

  // se_timer->toc();

  x = Eigen::VectorXd(x_reduced.size() + x_elimiated.size());
  x << x_reduced, x_elimiated;

  return LinearSolverStatus::SUCCESS;
}

/* ************************************************************************** */
LinearSolverStatus SchurComplementDenseSolver::buildReducedSystem_(
    const Eigen::SparseMatrix<double>& AtA_lower, const Eigen::VectorXd& Atb,
    Eigen::MatrixXd& H_reduced_lower, Eigen::SparseMatrix<double>& Her,
    Eigen::SparseMatrix<double>& He_inv_lower, Eigen::VectorXd& g_reduced) {
  // static auto br1_timer = global_timer().getTimer("* Schur build reduced");

  const int rs_until_idx = sc_ordering_->reducedSysUntilIndex();

  He_inv_lower = AtA_lower.block(rs_until_idx, rs_until_idx,
                                 AtA_lower.rows() - rs_until_idx,
                                 AtA_lower.rows() - rs_until_idx);
  Her = AtA_lower.block(rs_until_idx, 0, AtA_lower.rows() - rs_until_idx,
                        rs_until_idx);
  He_inv_lower.makeCompressed();
  Her.makeCompressed();

  H_reduced_lower =
      Eigen::MatrixXd(AtA_lower.block(0, 0, rs_until_idx, rs_until_idx));

  // br1_timer->tic();

  // block inversed based method
  for (size_t i = 0; i < sc_ordering_->eliminatedVariableSize(); i++) {
    const int var_pos = sc_ordering_->eliminatedVariablePosition(i);
    const int var_dim = sc_ordering_->eliminatedVariableDim(i);

    // br11_timer->tic();
    const Eigen::LDLT<Eigen::MatrixXd, Eigen::Lower> ldlt(
        He_inv_lower.block(var_pos, var_pos, var_dim, var_dim));
    if (ldlt.info() != Eigen::Success) {
      return LinearSolverStatus::RANK_DEFICIENCY;
    }
    const Eigen::MatrixXd Hs_block_inv =
        ldlt.solve(Eigen::MatrixXd::Identity(var_dim, var_dim));
    // br11_timer->toc();
    // replace with inverse
    // br12_timer->tic();
    for (int ii = 0; ii < var_dim; ii++) {
      for (int ij = 0; ij < var_dim - ii; ij++) {
        double* value_ptr = He_inv_lower.valuePtr() +
                            He_inv_lower.outerIndexPtr()[ii + var_pos] + ij;
        *value_ptr = Hs_block_inv(ii, ii + ij);
      }
    }
    // br12_timer->toc();
  }

  H_reduced_lower -=
      Her.transpose() * (He_inv_lower.selfadjointView<Eigen::Lower>() * Her);

  // br1_timer->toc();

  g_reduced.noalias() =
      Atb.head(rs_until_idx) -
      Her.transpose() * (He_inv_lower.selfadjointView<Eigen::Lower>() *
                         Atb.tail(AtA_lower.rows() - rs_until_idx));

  return LinearSolverStatus::SUCCESS;
}

}  // namspace minisam