/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2015 Andres Hernandez

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/math/fireflyalgorithm.hpp>
#include <ql/math/randomnumbers/sobolrsg.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>
#include <utility>

namespace QuantLib {
    FireflyAlgorithm::FireflyAlgorithm(Size M,
                                       ext::shared_ptr<Intensity> intensity,
                                       ext::shared_ptr<RandomWalk> randomWalk,
                                       Size Mde,
                                       Real mutation,
                                       Real crossover,
                                       unsigned long seed)
    : mutation_(mutation), crossover_(crossover), M_(M), Mde_(Mde), Mfa_(M_ - Mde_),
      intensity_(std::move(intensity)), randomWalk_(std::move(randomWalk)),
      drawIndex_(base_generator_type(seed), uniform_integer(Mfa_, Mde > 0 ? M_ - 1 : M_)),
      rng_(seed) {
        QL_REQUIRE(M_ >= Mde_,
            "Differential Evolution subpopulation cannot be larger than total population");
    }

    void FireflyAlgorithm::startState(Problem &P, const EndCriteria &endCriteria) {
        N_ = P.currentValue().size();
        x_.reserve(M_);
        xI_.reserve(M_);
        xRW_.reserve(M_);
        values_.reserve(M_);
        uX_ = P.constraint().upperBound(P.currentValue());
        lX_ = P.constraint().lowerBound(P.currentValue());
        Array bounds = uX_ - lX_;

        //Random initialization is done by Sobol sequence
        SobolRsg sobol(N_);

        //Prepare containers
        for (Size i = 0; i < M_; i++) {
            const SobolRsg::sample_type::value_type &sample = sobol.nextSequence().value;
            x_.emplace_back(N_, 0.0);
            xI_.emplace_back(N_, 0.0);
            xRW_.emplace_back(N_, 0.0);
            Array& x = x_.back();
            for (Size j = 0; j < N_; j++) {
                //Assign X=lb+(ub-lb)*random
                x[j] = lX_[j] + bounds[j] * sample[j];
            }
            //Evaluate point
            values_.emplace_back(P.value(x), i);
        }

        //init intensity & randomWalk
        intensity_->init(this);
        randomWalk_->init(this);
    }

    EndCriteria::Type FireflyAlgorithm::minimize(Problem &P, const EndCriteria &endCriteria) {
        QL_REQUIRE(!P.constraint().empty(), "Firefly Algorithm is a constrained optimizer");
        EndCriteria::Type ecType = EndCriteria::None;
        P.reset();
        Size iteration = 0;
        Size iterationStat = 0;
        Size maxIteration = endCriteria.maxIterations();
        Size maxIStationary = endCriteria.maxStationaryStateIterations();

        startState(P, endCriteria);

        bool isFA = Mfa_ > 0;
        //Variables for DE
        Array z(N_, 0.0);
        Size indexR1, indexR2;
        uniform_integer::param_type nParam(0, N_ - 1);

        //Set best value & position
        Real bestValue = values_[0].first;
        Size bestPosition = 0;
        for (Size i = 1; i < M_; i++) {
            if (values_[i].first < bestValue) {
                bestPosition = i;
                bestValue = values_[i].first;
            }
        }
        Array bestX = x_[bestPosition];

        //Run optimization
        do {
            iteration++;
            iterationStat++;
            //Check if stopping criteria is met
            if (iteration > maxIteration || iterationStat > maxIStationary)
                break;

            //Divide into two subpopulations
            //First sort values
            std::sort(values_.begin(), values_.end());

            //Differential evolution
            if(Mfa_ < M_){
                Size indexBest = values_[0].second;
                Array& xBest = x_[indexBest];
                for (Size i = Mfa_; i < M_; i++) {
                    if (!isFA) {
                        //Pure DE requires random index
                        indexBest = drawIndex_();
                        xBest = x_[indexBest];
                    }
                    do {
                        indexR1 = drawIndex_();
                    } while(indexR1 == indexBest);
                    do {
                        indexR2 = drawIndex_();
                    } while(indexR2 == indexBest || indexR2 == indexR1);

                    Size index = values_[i].second;
                    Array& x   = x_[index];
                    Array& xR1 = x_[indexR1];
                    Array& xR2 = x_[indexR2];
					Size rIndex = drawIndex_(nParam);
                    for (Size j = 0; j < N_; j++) {
                        if (j == rIndex || rng_.nextReal() <= crossover_) {
                            //Change x[j] according to crossover
                            z[j] = xBest[j] + mutation_*(xR1[j] - xR2[j]);
                        } else {
                            z[j] = x[j];
                        }
                        //Enforce bounds on positions
                        if (z[j] < lX_[j]) {
                            z[j] = lX_[j];
                        }
                        else if (z[j] > uX_[j]) {
                            z[j] = uX_[j];
                        }
                    }
                    Real val = P.value(z);
                    if (val < values_[index].first) {
                        //Accept new point
                        x = z;
                        values_[index].first = val;
                        //mark best
                        if (val < bestValue) {
                            bestValue = val;
                            bestX = x;
                            iterationStat = 0;
                        }
                    }
                }
            }

            //Firefly algorithm
            if(isFA){
                //According to the intensity, determine best global position
                intensity_->findBrightest();

                //Prepare random walk
                randomWalk_->walk();

                //Loop over particles
                for (Size i = 0; i < Mfa_; i++) {
                    Size index = values_[i].second;
                    Array& x   = x_[index];
                    Array& xI  = xI_[index];
                    Array& xRW = xRW_[index];

                    //Loop over dimensions
                    for (Size j = 0; j < N_; j++) {
                        //Update position
                        z[j] = x[j] + xI[j] + xRW[j];
                        //Enforce bounds on positions
                        if (z[j] < lX_[j]) {
                            z[j] = lX_[j];
                        }
                        else if (z[j] > uX_[j]) {
                            z[j] = uX_[j];
                        }
                    }
                    Real val = P.value(z);
                    if(!boost::math::isnan(val))
					{
						//Accept new point
                        x = z;
                        values_[index].first = val;
                        //mark best
                        if (val < bestValue) {
                            bestValue = val;
                            bestX = x;
                            iterationStat = 0;
                        }
					}
                }
            }
        } while (true);
        if (iteration > maxIteration)
            ecType = EndCriteria::MaxIterations;
        else
            ecType = EndCriteria::StationaryPoint;

        //Set result to best point
        P.setCurrentValue(bestX);
        P.setFunctionValue(bestValue);
        return ecType;
    }

    void FireflyAlgorithm::Intensity::findBrightest() {
        //Brightest ignores all others
        Array& xI = (*xI_)[(*values_)[0].second];
        for (Size j = 0; j < N_; j++) {
            xI[j] = 0.0;
        }

        for (Size i = 1; i < Mfa_; i++) {
            //values_ is already sorted
            Size index = (*values_)[i].second;
            const Array& x = (*x_)[index];
            Array& xI = (*xI_)[index];
            for (Size j = 0; j < N_; j++) {
                xI[j] = 0.0;
            }
            Real valueX = (*values_)[i].first;
            for (Size k = 0; k < i - 1; k++){
                const Array& y = (*x_)[(*values_)[k].second];
                Real valueY = (*values_)[k].first;
                Real intensity = intensityImpl(valueX, valueY, distance(x, y));
                xI += intensity*(y - x);
            }
        }
    }
}


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

#include <nt2/exponential/functions/pow_abs.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/log2_em1.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/oneo_16.hpp>
#include <nt2/include/constants/powlargelim.hpp>
#include <nt2/include/constants/powlowlim.hpp>
#include <nt2/include/constants/sixteen.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/frexp.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/if_else_zero.hpp>
#include <nt2/include/functions/simd/if_one_else_zero.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/is_gtz.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/is_odd.hpp>
#include <nt2/include/functions/simd/ldexp.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/oneminus.hpp>
#include <nt2/include/functions/simd/oneplus.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/pow2.hpp>
#include <nt2/include/functions/simd/selinc.hpp>
#include <nt2/include/functions/simd/selsub.hpp>
#include <nt2/include/functions/simd/shift_right.hpp>
#include <nt2/include/functions/simd/signnz.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/toint.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/functions/simd/bitwise_and.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/is_gtz.hpp>
#include <nt2/include/functions/simd/if_zero_else_one.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( pow_abs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      typedef typename meta::as_integer<A0>::type iA0;
      iA0 e;
      A0 ax = nt2::abs(a0);
      A0 x = frexp(ax, e);
      iA0 i  = details::pow_kernel<A0>::select(x);
      A0 z = sqr(x);
      A0 w = details::pow_kernel<A0>::pow1(x, z);
      w = fma(Mhalf<A0>(), z, w);
      w = fma(Log2_em1<A0>(), w, w);
      z = fma(Log2_em1<A0>(), x, w);
      z += x;
      w =  fma(tofloat(-i), Oneo_16<A0>(), tofloat(e));

      A0 ya = reduc(a1);
      A0 yb = a1 - ya;
      A0 W = fma(z, a1, w*yb);
      A0 Wa = reduc(W);
      A0 Wb = W - Wa;
      W = fma(w, ya, Wa);
      Wa = reduc(W);
      A0 u = W - Wa;
      W = Wb + u;
      Wb = reduc(W);
      w = Sixteen<A0>()*(Wa + Wb);

      // Test the power of 2 for over/underflow
      bA0 inf_ret = gt(w, Powlargelim<A0>());
      bA0 zer_ret = lt(w, Powlowlim<A0>());
      e = toint(w);
      Wb = W - Wb;
      bA0 test = is_gtz(Wb);
      e = selinc(test, e);
      Wb = selsub(test, Wb, Oneo_16<A0>());

      z = details::pow_kernel<A0>::pow2(Wb)*Wb;
      i = selinc(is_gtz(e), (e/Sixteen<iA0>()));
      e = (i*Sixteen<iA0>()) - e;
      w =  details::pow_kernel<A0>::twomio16(e);
      z = fma(w, z, w);
      z = ldexp( z, i );
      #ifndef BOOST_SIMD_NO_INFINITIES
      bA0 gtax1 = gt(ax,One<A0>());
      z =  if_else(eq(a1,  Inf<A0>()),if_else_zero(gtax1, Inf<A0>()), z);
      z =  if_else(eq(a1, Minf<A0>()),if_zero_else(gtax1, Inf<A0>()), z);
      z =  if_else(eq(ax, Inf<A0>()),
                   if_else(is_gtz(a1),
                           Inf<A0>(),
                           if_zero_else_one(is_ltz(a1))),
                   z);
      #endif
      z = if_zero_else(zer_ret, z);
      z = if_else(inf_ret, Inf<A0>(), z);
      z = if_else(eq(ax, One<A0>()), ax, z);
      return if_else(is_eqz(a0), if_one_else_zero(is_eqz(a1)), z);

    }
  private :
    static BOOST_FORCEINLINE A0 reduc(const A0& x)
    {
      // Find a multiple of 1/16 that is within 1/16 of x.
      return Oneo_16<A0>()*floor(Sixteen<A0>()*x);
    }
  };
} }

#endif


/*
This code is based on the original code by Quentin "Leph" Rouxel and Team Rhoban.
The original files can be found at:
https://github.com/Rhoban/model/
*/
#ifndef FOOTSTEP_HPP
#define FOOTSTEP_HPP

#include <Eigen/Dense>
#include <cmath>
#include <stdexcept>
#include "bitbots_splines/Angle.h"

namespace bitbots_quintic_walk {

/**
 * Footstep
 *
 * Manage humanoid footstep
 * generation and state
 */
class Footstep
{
    public:

        enum SupportFoot {
            LeftSupportFoot,
            RightSupportFoot,
        };

        /**
         * Initialization with lateral
         * foot distance and support foot
         */
        Footstep(
            double footDistance,
            bool isLeftSupportFoot = true);

        /**
         * Set the lateral foot
         * distance parameters
         */
        void setFootDistance(double footDistance);
        double getFootDistance();

        /**
         * Reset to neutral position the current
         * step (not the integrated odometry)
         */
        void reset(bool isLeftSupportFoot);

        // reset odometry
        void resetInWorld(bool isLeftSupportFoot);

        /**
         * Current support foot
         */
        bool isLeftSupport() const;

        /**
         * Starting position of current flying
         * foot in support foot frame
         */
        const Eigen::Vector3d& getLast() const;

        /**
         * Target pose of current flying
         * foot in support foot frame
         */
        const Eigen::Vector3d& getNext() const;

        /**
         * Returns the odometry change of the current step.
         */
        const Eigen::Vector3d& getOdom() const;

        /**
         * Left and right, current or next pose
         * of foot in world initial frame
         */
        const Eigen::Vector3d& getLeft() const;
        const Eigen::Vector3d& getRight() const;

        /**
         * Set the target pose of current support foot
         * during next support phase and update support foot.
         * The target foot pose diff is given with respect to
         * next support foot pose (current flying foot target).
         */
        void stepFromSupport(const Eigen::Vector3d& diff);

        /**
         * Set target pose of current support foot
         * using diff orders.
         * Zero vector means in place walking.
         * Special handle of lateral and turn step
         * to avoid foot collision.
         */
        void stepFromOrders(const Eigen::Vector3d& diff);

    private:

        /**
         * Static lateral distance
         * between the feet
         */
        double _footDistance;

        /**
         * Current support foot
         * (left or right)
         */
        bool _isLeftSupportFoot;

        /**
         * Pose diff [dx, dy, dtheta]
         * from support foot to flying foot
         * last and next position
         */
        Eigen::Vector3d _supportToLast;
        Eigen::Vector3d _supportToNext;

        /**
         * Pose integration of left
         * and right foot in initial frame.
         * Set at "future" state taking into account
         * next expected fot pose.
         */
        Eigen::Vector3d _leftInWorld;
        Eigen::Vector3d _rightInWorld;

        /**
         * Add to given pose the given diff
         * expressed in pose frame and
         * return the integrated added pose
         */
        Eigen::Vector3d poseAdd(
            const Eigen::Vector3d& pose,
            const Eigen::Vector3d& diff) const;

        /**
         * Compute and return the delta from
         * (zero+diff) to (zero) in
         * (zero+diff) frame.
         */
        Eigen::Vector3d diffInv(
            const Eigen::Vector3d& diff) const;
};

}
#endif


#define CF_USE_TIMER 1
#include "main.h"
#include "arap_material.h"
#include "hcheck.h"
#include "neohookean_material.h"
#include "tet_elastic_body.h"
#include "typedefs.h"
#include "utils.h"

#include <mkl.h>
#include <Eigen/PardisoSupport>

#include <cstdio>
#include <memory>

using namespace baseline;

bool baseline::g_hessian_proj = true;
double baseline::g_hessian_diag_reg = 0;
using SparseMat = materials::ElasticBody<3, double>::SparseMat;

namespace {

//! change g_hessian_proj in the scope of this class and restore to old value
class ChangeHeissianProj : public cf::NonCopyable {
    const bool m_old;

public:
    explicit ChangeHeissianProj(bool v) : m_old{g_hessian_proj} {
        g_hessian_proj = v;
    }
    ~ChangeHeissianProj() { g_hessian_proj = m_old; }
};

// compute stiffness matrix K for current configuration
void get_stiffmat_and_force(
        const materials::TetMesh<double>&
                tet_mesh,  // the tet mesh with rest configuration
        const materials::TetElasticBody<double>& tet_def_body,
        const CoordMat& vertices,  // the vertices coordinates in current
                                   // configuration (size 3 * N)
        const CoordMat* f_ext,     // static external force
        const std::vector<bool>& index_mask,  // the index mask, which obtained
                                              // from set_boundary_conditions
        SparseMat* K,                         // output: the stiffness matrix
        Eigen::VectorXd& force_elastic  // output: the elastic force in current
                                        // configuration
) {
    const size_t rows = tet_mesh.vertex().rows();
    const size_t cols = tet_mesh.vertex().cols();
    const size_t total_dim = rows * cols;

    SparseMat K_full;

    if (K) {
        K_full = tet_def_body.compute_stiffness_matrix(vertices);

        if (g_hessian_diag_reg) {
            SparseMat regularizer(K_full.rows(), K_full.cols());
            regularizer.setIdentity();

            // add a regularization term on the diagonal
            K_full += regularizer * g_hessian_diag_reg;
        }
    }

    Eigen::Matrix3Xd force_elastic_full = tet_def_body.compute_force(vertices);
    if (f_ext) {
        force_elastic_full += *f_ext;
    }

    // trim K_full and force_elastic_full with vertex_mask
    std::unique_ptr<int[]> index_mapping(new int[total_dim]);
    int dofs = 0;
    for (size_t i = 0; i < total_dim; i++) {
        index_mapping[i] = dofs;
        if (index_mask[i]) {
            dofs++;
        }
    }

    if (K) {
        std::vector<Eigen::Triplet<double>> triplet_list;
        triplet_list.reserve(K_full.nonZeros());
        for (int k = 0; k < K_full.outerSize(); k++) {
            for (SparseMat::InnerIterator it(K_full, k); it; ++it) {
                if (index_mask[it.row()] && index_mask[it.col()]) {
                    triplet_list.push_back(Eigen::Triplet<double>(
                            index_mapping[it.row()], index_mapping[it.col()],
                            it.value()));
                }
            }
        }
        K->resize(dofs, dofs);
        K->setZero();
        K->makeCompressed();
        K->setFromTriplets(triplet_list.begin(), triplet_list.end());
    }

    force_elastic = Eigen::VectorXd::Zero(dofs);
    for (size_t i = 0; i < total_dim; i++) {
        if (index_mask[i])
            force_elastic[index_mapping[i]] = force_elastic_full(i % 3, i / 3);
    }
}

std::unique_ptr<materials::Material<3, double>> make_material(
        const MaterialDesc& desc) {
    std::unique_ptr<materials::Material<3, double>> ret;
    switch (desc.energy) {
        case MaterialDesc::NEOHOOKEAN_C:
            ret.reset(new materials::CompressibleNeohookeanMaterial<3, double>(
                    desc.young, desc.poisson));
            break;
        case MaterialDesc::NEOHOOKEAN_I:
            ret.reset(
                    new materials::IncompressibleNeohookeanMaterial<3, double>(
                            desc.young, desc.poisson));
            break;
        case MaterialDesc::ARAP:
            ret.reset(new materials::ARAPElasticityMaterial<3, double>(
                    desc.young, desc.poisson));
            break;
        default:
            cf_assert(0, "bad material");
    }
    return ret;
}

void check_hessian() {
    static bool enable = getenv("FEA_CHECK");
    if (!enable) {
        return;
    }
    static bool done = false;
    if (!done) {
        ChangeHeissianProj disable_proj{false};
        materials::CompressibleNeohookeanMaterial<3, double> m0(1.2, 0.45);
        materials::check_hessian("neohookean_c", m0);
        materials::IncompressibleNeohookeanMaterial<3, double> m1(1.2, 0.45);
        materials::check_hessian("neohookean_i", m1);
        materials::ARAPElasticityMaterial<3, double> m2(1.2, 0.45);
        materials::check_hessian("arap", m2);
        done = true;
    }
}

class SparseSolver {
    bool m_self_adj;
    Eigen::PardisoLLT<SparseMat, Eigen::Upper> m_solver_llt;
    Eigen::PardisoLU<SparseMat> m_solver_lu;

public:
    explicit SparseSolver(bool self_adj) : m_self_adj{self_adj} {}

    void set_self_adj(bool self_adj) { m_self_adj = self_adj; }

    Eigen::VectorXd solve(const SparseMat& A, const Eigen::VectorXd& b) {
        Eigen::VectorXd r;
        Eigen::ComputationInfo info;
        if (m_self_adj) {
            r = m_solver_llt.compute(A).solve(b);
            info = m_solver_llt.info();
        } else {
            r = m_solver_lu.compute(A).solve(b);
            info = m_solver_lu.info();
        }
        if (info) {
            std::string info_message;
            if (info == 1) {
                info_message = "NumericalIssue";
            } else if (info == 2) {
                info_message = "NoConvergence";
            } else if (info == 3) {
                info_message = "InvalidInput";
            } else {
                info_message = "UnknownError";
            }
            throw std::runtime_error{std::move(info_message)};
        }
        return r;
    }
};

//! compute A'A with MKL builtin
void compute_aTa(SparseMat& dst, SparseMat& A) {
    cf_assert(A.isCompressed() && A.IsRowMajor);
    sparse_matrix_t A_mkl, ATA_mkl;
    {
        int* rows_start = A.outerIndexPtr();
        int* rows_end = A.outerIndexPtr() + 1;
        int* col_indx = A.innerIndexPtr();
        auto err = mkl_sparse_d_create_csr(&A_mkl, SPARSE_INDEX_BASE_ZERO,
                                           A.rows(), A.cols(), rows_start,
                                           rows_end, col_indx, A.valuePtr());
        cf_assert(err == SPARSE_STATUS_SUCCESS);
    }

    CF_DEFER(std::bind(mkl_sparse_destroy, A_mkl));

    auto status = mkl_sparse_syrk(SPARSE_OPERATION_TRANSPOSE, A_mkl, &ATA_mkl);
    cf_assert(status == SPARSE_STATUS_SUCCESS,
              "failed to compute A'A: status=%d", static_cast<int>(status));
    CF_DEFER(std::bind(mkl_sparse_destroy, ATA_mkl));

    sparse_index_base_t ATA_idx;
    MKL_INT r, c, *ATA_ia0, *ATA_ia1, *ATA_ja;
    double* ATA_a;
    status = mkl_sparse_d_export_csr(ATA_mkl, &ATA_idx, &r, &c, &ATA_ia0,
                                     &ATA_ia1, &ATA_ja, &ATA_a);
    cf_assert(status == SPARSE_STATUS_SUCCESS,
              "failed to export A'A: status=%d", static_cast<int>(status));
    cf_assert(r == c && r == A.rows());
    cf_assert(ATA_idx == SPARSE_INDEX_BASE_ZERO);
    cf_assert(ATA_ia1 == ATA_ia0 + 1);
    dst.makeCompressed();
    dst = Eigen::Map<const Eigen::SparseMatrix<double, Eigen::RowMajor>>{
            r, c, ATA_ia0[r], ATA_ia0, ATA_ja, ATA_a};
    cf_assert(dst.isCompressed());
}

double rms(const Eigen::VectorXd& v) {
    return std::sqrt(v.squaredNorm() / v.size());
}

}  // anonymous namespace

baseline::Stat baseline::solve_energy_min(
        const IndexMat& elements, const CoordMat& vtx_init,
        const CoordMat& vtx_dst, const CoordMat* f_ext, const MaskMat& bnd_mask,
        const MaterialDesc& material_desc, double gtol_refine,
        const IterCallback& iter_callback) {
    check_hessian();
    auto material = make_material(material_desc);
    materials::TetMesh<double> tet_mesh{vtx_init, elements};
    materials::TetElasticBody<double> tet_def_body(*material, {}, {}, tet_mesh);

    CoordMat vertices = vtx_dst;
    SparseMat hessian;
    Eigen::VectorXd grad;
    std::vector<bool> index_mask(bnd_mask.cols() * 3);
    for (int i = 0; i < bnd_mask.cols(); ++i) {
        for (int j = 0; j < 3; ++j) {
            index_mask[i * 3 + j] = !bnd_mask(j, i);
        }
    }

    Stat stat;
    cf::Timer timer;
    timer.start();

    constexpr double gtol = 1e-6, xtol = 1e-6, ls_c1 = 0.2;
    SparseSolver solver{g_hessian_proj};

    auto get_energy = [&tet_def_body, f_ext, &vtx_init](const CoordMat& vtx) {
        auto e = tet_def_body.compute_energy(vtx);
        if (f_ext) {
            Eigen::Map<const Eigen::VectorXd> v0_flat{vtx_init.data(),
                                                      vtx_init.size()},
                    v1_flat{vtx.data(), vtx.size()},
                    f_flat{f_ext->data(), f_ext->size()};
            e += (v0_flat - v1_flat).dot(f_flat);
        }
        return e;
    };

    Eigen::Matrix<double, 3, Eigen::Dynamic> new_vertices;
    for (;;) {
        get_stiffmat_and_force(tet_mesh, tet_def_body, vertices, f_ext,
                               index_mask, &hessian, grad);
        // this is actually the negative grad since it is the force
        ++stat.nr_iter;
        cf::Timer timer_newton;
        timer_newton.start();
        Eigen::VectorXd u = solver.solve(hessian, grad);

        double step = 1, energy = get_energy(vertices),
               c1_g_p = -ls_c1 * u.dot(grad), new_energy,
               dx_base = u.norm() / (new_vertices.norm() + 1);
        if (g_hessian_proj) {
            cf_assert(c1_g_p < 0);
        } else {
            if (c1_g_p >= 0) {
                c1_g_p = 0;
            }
        }
        int linear_search_cnt = 0;
        for (;; ++linear_search_cnt) {
            new_vertices = vertices;

            // get the final positions and shove them in u
            int cnt = 0;
            for (int i = 0; i < vertices.cols(); i++) {
                for (int j = 0; j < 3; j++) {
                    if (index_mask[i * 3 + j]) {
                        new_vertices(j, i) += u(cnt++) * step;
                    }
                }
            }

            new_energy = get_energy(new_vertices);
            if (new_energy < energy + step * c1_g_p) {
                break;
            }
            step /= 2;
            if (dx_base * step < xtol) {
                new_vertices = vertices;
                break;
            }
        }

        timer_newton.stop();
        stat.tot_newton_time += timer_newton.time();

        double grad_rms = rms(grad),
               df = (energy - new_energy) / (new_energy + 1),
               dx = dx_base * step;

        vertices = new_vertices;

        printf("\riter=%d grms=%g df=%g dx=%g step=%g f=%g cnt=%d  ",
               stat.nr_iter, grad_rms, df, dx, step, energy, linear_search_cnt);
        fflush(stdout);
        if (iter_callback && !iter_callback(vertices)) {
            break;
        }
        if (grad_rms < gtol || dx < xtol) {
            stat.df = df;
            stat.dx = dx;
            stat.grad_rms = grad_rms;
            stat.energy = energy;
            break;
        }
    }
    printf("\n");
    auto postprocess = [&]() {
        timer.stop();
        stat.tot_time = timer.time();
        stat.vtx = vertices;
    };

    if (stat.grad_rms <= gtol_refine) {
        postprocess();
        return stat;
    }

    printf("run refinement: grad_rms=%g gtol_refine=%g\n", stat.grad_rms,
           gtol_refine);

    ChangeHeissianProj disable_proj{false};
    solver.set_self_adj(false);
    get_stiffmat_and_force(tet_mesh, tet_def_body, vertices, f_ext, index_mask,
                           &hessian, grad);
    for (;;) {
        ++stat.nr_iter;
        ++stat.nr_iter_refine;
        cf::Timer timer_newton;
        timer_newton.start();
        Eigen::VectorXd u = solver.solve(hessian, grad);
        {
            int cnt = 0;
            for (int i = 0; i < vertices.cols(); i++) {
                for (int j = 0; j < 3; j++) {
                    if (index_mask[i * 3 + j]) {
                        vertices(j, i) += u(cnt++);
                    }
                }
            }
        }
        timer_newton.stop();
        stat.tot_newton_time += timer_newton.time();

        get_stiffmat_and_force(tet_mesh, tet_def_body, vertices, f_ext,
                               index_mask, &hessian, grad);

        double grad_rms = rms(grad);
        printf("\riter=%d grms=%g ", stat.nr_iter, grad_rms);
        fflush(stdout);
        if (iter_callback && !iter_callback(vertices)) {
            break;
        }
        if (grad_rms < gtol_refine || stat.nr_iter_refine >= 20) {
            stat.grad_rms_refine = grad_rms;
            break;
        }
    }
    printf("\n");
    postprocess();
    return stat;
}

baseline::Stat baseline::solve_force_equ_levmar(
        const IndexMat& elements, const CoordMat& vtx_init,
        const CoordMat& f_ext, const MaskMat& bnd_mask,
        const MaterialDesc& material_desc, double gtol,
        const IterCallback& iter_callback) {
    check_hessian();
    cf_assert(!g_hessian_proj);
    auto material = make_material(material_desc);
    materials::TetMesh<double> tet_mesh{vtx_init, elements};
    materials::TetElasticBody<double> tet_def_body(*material, {}, {}, tet_mesh);

    CoordMat vertices = vtx_init, new_vertices;
    SparseMat jacobian, damped;
    Eigen::VectorXd force, new_force, eqn_rhs;
    std::vector<bool> index_mask(bnd_mask.cols() * 3);
    for (int i = 0; i < bnd_mask.cols(); ++i) {
        for (int j = 0; j < 3; ++j) {
            index_mask[i * 3 + j] = !bnd_mask(j, i);
        }
    }

    Stat stat;
    cf::Timer timer;

    auto postprocess = [&]() {
        timer.stop();
        printf("\n");
        stat.tot_time = timer.time();
        stat.vtx = vertices;
    };

    timer.start();

    constexpr int max_iters = 1000;
    constexpr double damp_coeff_k = 10,
                     damp_coeff_min = std::numeric_limits<double>::epsilon();
    SparseSolver solver{true};
    double damp_coeff = 1e-4;
    Eigen::VectorXd delta, diag;
    for (;;) {
        ++stat.nr_iter;
        get_stiffmat_and_force(tet_mesh, tet_def_body, vertices, &f_ext,
                               index_mask, &jacobian, force);

        cf::Timer timer_newton;
        timer_newton.start();
        double energy = rms(force), new_energy;
        compute_aTa(damped, jacobian);
        eqn_rhs = jacobian.transpose() * force;
        diag = damped.diagonal();

        int search_cnt = 0;
        for (; ++search_cnt;) {
            damped.diagonal() = diag * (1 + damp_coeff);
            delta = solver.solve(damped, eqn_rhs);

            new_vertices = vertices;
            {
                // get the final positions and shove them in u
                int cnt = 0;
                for (int i = 0; i < vertices.cols(); i++) {
                    for (int j = 0; j < 3; j++) {
                        if (index_mask[i * 3 + j]) {
                            new_vertices(j, i) += delta(cnt++);
                        }
                    }
                }
            }

            timer_newton.stop();
            try {
                get_stiffmat_and_force(tet_mesh, tet_def_body, new_vertices,
                                       &f_ext, index_mask, nullptr, new_force);
                new_energy = rms(new_force);
            } catch (materials::NumericalError&) {
                new_energy = energy * 1.1;
            }
            timer_newton.start();
            if (new_energy < energy) {
                damp_coeff =
                        std::max(damp_coeff / damp_coeff_k, damp_coeff_min);
                break;
            } else {
                damp_coeff *= damp_coeff_k;
            }

            if (search_cnt >= 50) {
                // no progress can be made ...
                stat.grad_rms = energy;
                stat.dx = -1;
                postprocess();
                return stat;
            }
        }

        timer_newton.stop();
        stat.tot_newton_time += timer_newton.time();

        auto dx = delta.norm() / (new_vertices.norm() + 1);
        printf("\riter=%d grms=%g dg=%g dx=%g lambda=%g/%.1g cnt=%d  ",
               stat.nr_iter, new_energy, energy - new_energy, dx, damp_coeff,
               damp_coeff_min, search_cnt);
        fflush(stdout);

        vertices = new_vertices;
        energy = new_energy;

        if (iter_callback && !iter_callback(vertices)) {
            break;
        }
        if (energy < gtol || stat.nr_iter >= max_iters) {
            stat.dx = dx;
            stat.grad_rms = energy;
            break;
        }
    }
    postprocess();
    return stat;
}


/*
 * Copyright (C) 2014 Mellnik
 *
 * 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 <boost/chrono.hpp>
#include "cryptopp/hex.h"
#include "cryptopp/osrng.h"
#include "cryptopp/whrlpool.h"
#include "cryptopp/pwdbased.h"

#include "pbkdf2.h"

Pbkdf2::Pbkdf2(const char *key, unsigned iterations, CallbackData *callback) :
	h_Key(key),
	h_Iterations(iterations),
	cData(callback),
	h_Worker(PBKDF2_GENERATE),
	h_Equal(false),
	h_ExecTime(0)
{

}

Pbkdf2::Pbkdf2(const char *key, const char *hash, const char *salt, unsigned iterations, CallbackData *callback) :
	h_Key(key),
	h_Hash(hash),
	h_Salt(salt),
	h_Iterations(iterations),
	cData(callback),
	h_Worker(PBKDF2_VALIDATE),
	h_Equal(false),
	h_ExecTime(0)
{

}

Pbkdf2::~Pbkdf2()
{
	delete cData;
}

void Pbkdf2::Work()
{
	static const unsigned _PBKDF2_BYTE_ = 512 / 8;

	boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now();

	CryptoPP::SecByteBlock byte_Derived(_PBKDF2_BYTE_);
	CryptoPP::PKCS5_PBKDF2_HMAC<CryptoPP::Whirlpool> pbkdf2;

	if(h_Worker == PBKDF2_GENERATE)
	{
		CryptoPP::SecByteBlock byte_Salt(_PBKDF2_BYTE_);
		CryptoPP::AutoSeededRandomPool RNG;
		RNG.GenerateBlock(byte_Salt, byte_Salt.size());
		CryptoPP::ArraySource(byte_Salt, byte_Salt.size(), true, new CryptoPP::HexEncoder(new CryptoPP::StringSink(h_Salt)));

		pbkdf2.DeriveKey(byte_Derived, byte_Derived.size(), 0x0, reinterpret_cast<const byte *>(h_Key.data()), h_Key.size(), reinterpret_cast<const byte *>(h_Salt.data()), h_Salt.size(), h_Iterations, 0);
		CryptoPP::ArraySource(byte_Derived, byte_Derived.size(), true, new CryptoPP::HexEncoder(new CryptoPP::StringSink(h_Hash)));
	}
	else if(h_Worker == PBKDF2_VALIDATE)
	{
		pbkdf2.DeriveKey(byte_Derived, byte_Derived.size(), 0x0, reinterpret_cast<const byte *>(h_Key.data()), h_Key.size(), reinterpret_cast<const byte *>(h_Salt.data()), h_Salt.size(), h_Iterations, 0);

		CryptoPP::SecByteBlock byte_Validate(_PBKDF2_BYTE_);
		CryptoPP::StringSource(h_Hash, true, new CryptoPP::HexDecoder(new CryptoPP::ArraySink(byte_Validate, byte_Validate.size())));

		// Length-constant comparison.
		unsigned diff = _PBKDF2_BYTE_ ^ _PBKDF2_BYTE_;
		for(unsigned i = 0; i < _PBKDF2_BYTE_; ++i)
		{
			diff |= byte_Derived[i] ^ byte_Validate[i];
		}
		h_Equal = diff == 0;
	}

	boost::chrono::steady_clock::duration duration = boost::chrono::steady_clock::now() - start;
	h_ExecTime = static_cast<unsigned>(boost::chrono::duration_cast<boost::chrono::milliseconds>(duration).count());
}


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

  @copyright 2012-2015 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_CONSTANT_LOG10_2HI_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LOG10_2HI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant log10_2hi.This constant is coupled with Log10_2lo and is
    used in the float logarithms computations
    We have abs( double(log10_2hi<float>())+double(Log10_2lo<float>()) - Log10(2.0) < 3e-11

    @par Semantic:

    @code
    T r = log10_2hi<T>();
    @endcode

**/
  template<typename T> T Log10_2hi();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant log10_2hi.This constant is coupled with Log10_2lo and is
      used in the float logarithms computations
      We have abs( double(log10_2hi<float>())+double(Log10_2lo<float>()) - Log10(2.0) < 3e-11

      Generate the  constant log10_2hi.

      @return The Log10_2hi constant for the proper type
    **/
    const boost::dispatch::functor<tag::log10_2hi_> log10_2hi = {};
  }
} }
#endif

#include <boost/simd/constant/definition/log10_2hi.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#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_FIX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FIX_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing fix capabilities

    Computes the truncation toward @ref Zero of its parameter.

    This is a convenience alias of @ref trunc
  **/
  Value fix(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/trunc.hpp>
#include <boost/simd/function/simd/fix.hpp>

#endif


/**
 * @file set_cover_long_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-02-25
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_orlib_sc.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/set_cover_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/greedy/set_cover/set_cover.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/algorithm/cxx11/all_of.hpp>

#include <iterator>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>

BOOST_AUTO_TEST_CASE(set_cover_long) {
    const std::string test_dir = paal::system::get_test_data_dir("SC_ORLIB");
    using paal::system::build_path;

    paal::parse(build_path(test_dir, "cases.txt"),
                [&](const std::string &fname, std::istream &is_test_cases) {
        LOGLN("TEST " << fname);
        std::ifstream ifs(build_path(test_dir, "/cases/" + fname));
        assert(ifs.good());

        int sample_result;
        is_test_cases >> sample_result;
        auto sc = paal::read_ORLIB_SC(ifs);
        auto &costs_of_sets = sc.first;
        auto &elements_of_sets = sc.second;
        auto sets = boost::irange(size_t(0), costs_of_sets.size());
        auto set_to_elements =
            paal::utils::make_array_to_functor(elements_of_sets);
        std::vector<int> result;
        auto element_index = paal::utils::identity_functor{};
        auto cost = paal::greedy::set_cover(
            sets, paal::utils::make_array_to_functor(costs_of_sets),
            set_to_elements, std::back_inserter(result), element_index);
        double approximation_ratio = set_cover_result_check(sets,set_to_elements,result);
        check_result_compare_to_bound(cost, sample_result, approximation_ratio);
    });
}


// Copyright (c) 2020 Patrick Hart, Julian Bernhard,
// Klemens Esterle, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_ML_COMMONS_COMMONS_HPP_
#define BARK_ML_COMMONS_COMMONS_HPP_

#include <memory>
#include <tuple>
#include <vector>
#include <Eigen/Dense>

namespace bark_ml {
namespace commons {

template<typename T>
inline const T Norm(const T& val, const T& min_val, const T& max_val) {
  return (val - min_val) / (max_val-min_val);
}

}  // namespace commons
}  // namespace bark_ml

#endif  // BARK_ML_COMMONS_COMMONS_HPP_


/*
 * Copyright 2014.  Los Alamos National Security, LLC. This material was produced
 * under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National
 * Laboratory (LANL), which is operated by Los Alamos National Security, LLC
 * for the U.S. Department of Energy. The U.S. Government has rights to use,
 * reproduce, and distribute this software.  NEITHER THE GOVERNMENT NOR
 * LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED,
 * OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE.  If software is
 * modified to produce derivative works, such modified software should be
 * clearly marked, so as not to confuse it with the version available from LANL.
 *
 * 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.
 *
 *  Additionally, 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 Los Alamos National Security, LLC, Los Alamos
 *       National Laboratory, LANL, the U.S. Government, nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * LibParty, Version 1.x, Copyright number C114122, LA-CC-14-086
 *
 * Authors:
 *    Matthew Kinsey, kinsey@lanl.gov
 *    Verinder Rana, vrana@lanl.gov
 *    Bob Robey, XCP-2, LANL, brobey@lanl.gov
 *    Jeremy Sauer, EES-16, LANL, jsauer@lanl.gov
 *    Jon Reisner, XCP-4 LANL, reisner@lanl.gov
 */

#include "libparty.hh"
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
//#include <boost/mpi.hpp>
//#include "libparty.h"
#include "MersenneTwister.hh"

inline double gaussian(double r, double sigma) {
  double val;
  double cons = 1.0/sigma/sqrt(2.0*3.1415);
  val = cons * exp(-(r*r)/(2*sigma*sigma));
  return val;
}
part_sys_soa::part_sys_soa( int npart )
{
  MTRand mtrand;
  double sphere_outerr = 10.0;
  for( int i = 0; i < npart; i++)
  {
    id.push_back(i);
//    double r, prob;
//    bool keep = false;
//    do {
//    x.push_back(sphere_outerr - mtrand.rand(2*sphere_outerr));
//    y.push_back(sphere_outerr - mtrand.rand(2*sphere_outerr));
//    z.push_back(sphere_outerr - mtrand.rand(2*sphere_outerr));
//      r = sqrt(p.pos[0]*p.pos[0] + p.pos[1]*p.pos[1] + p.pos[2]*p.pos[2]);
//      prob = gaussian(r,5.0);
//      if(mtrand.rand() < prob) keep = true;
//    } while (!keep);
//    } while(false);
//
//        p.mass = 1.;
//        p.nn_h = 0.5;
//        p.vel[0] = 0.2 - mtrand.rand(0.4);
//        p.vel[1] = 0.2 - mtrand.rand(0.4);
//        p.vel[2] = 0.2 - mtrand.rand(0.4);
//        p.del = false;
//        partv.push_back(p);
     }

  x.resize(npart,0.0);
  y.resize(npart,0.0);
  z.resize(npart,0.0);
  vx.resize(npart,0.0);
  vy.resize(npart,0.0);
  vz.resize(npart,0.0);
  mass.resize(npart,1.0);
  nn_h.resize(npart,1.);
  rho.resize(npart,0.0);
  ghost.resize(npart,0);

#ifndef USE_MIC
  //boost::mpi::environment env();
  if(boost::mpi::environment::initialized()){
    mproc = world.rank();
    nprocs = world.size();
  } else {
#endif
    mproc = 0;
    nprocs = 1;
#ifndef USE_MIC
  }
#endif
  nn_cutoff = pow(10.0,-6);
  max_nn_h = pow((1/(4*3.1415*nn_cutoff)),(1.0/3.0));
}

void part_sys_soa::initgauss()
{
  MTRand mtrand = MTRand(1337);
  double sphere_outerr = 10.0;
  for( int i = 0; i < x.size(); i++)
  {
    double r, prob;
    bool keep = false;
    do {
      x[i] = sphere_outerr - mtrand.rand(2*sphere_outerr);
      y[i] = sphere_outerr - mtrand.rand(2*sphere_outerr);
      z[i] = sphere_outerr - mtrand.rand(2*sphere_outerr);
      r = sqrt(x[i]*x[i] + y[i]*y[i] + z[i]*z[i]);
      prob = gaussian(r,8.0);
      if(mtrand.rand() < prob) keep = true;
    } while (!keep);
 //    } while(false);
//    //
//    //        p.mass = 1.;
//    //        p.nn_h = 0.5;
    vx[i] = 0.2 - mtrand.rand(0.4);
    vy[i] = 0.2 - mtrand.rand(0.4);
    vz[i] = 0.2 - mtrand.rand(0.4);
//    //        p.del = false;
//    //        partv.push_back(p);
   }
}

void part_sys_soa::periodic2d(bbox b) {
  for(int i = 0; i < x.size(); ++i) {
    if(x[i] < b[0]) x[i] += b[1]-b[0];
    if(x[i] > b[1]) x[i] -= b[1]-b[0];
    if(y[i] < b[2]) y[i] += b[3]-b[2];
    if(y[i] > b[3]) y[i] -= b[3]-b[2];
  }
}
/*
void part_sys_soa::step( double dt )
{
  int numpart = x.size();
  double* cx = &x[0];
  double* cy = &y[0];
  double* cz = &z[0];
  double* cvx = &vx[0];
  double* cvy = &vy[0];
  double* cvz = &vz[0];
  short int* cg = &ghost[0];

  for( int proc = 0; proc < nprocs; ++proc) {
    if(mproc == proc && x.size() != 0) {
#pragma offload target(mic:0) in(cvx, cvy, cvz, cg : length(numpart) ) in(dt) inout(cx,cy,cz : length(numpart) alloc_if(1) free_if(1) )
#ifdef _OPENMP
#pragma omp parallel for
#endif
      for( int i = 0; i < numpart; ++i) {
        if(cg[i]) continue;
        cx[i] += cvx[i] * dt;
        cy[i] += cvy[i] * dt;
        cz[i] += cvz[i] * dt;
        }
      }
  }
}
*/
void part_sys_soa::step( double dt )
{
  int numpart = x.size();
#ifdef _OPENMP
#pragma omp parallel for
#endif
for( int i = 0; i < numpart; ++i) {
if(ghost[i]) continue;
//#pragma omp parallel for
x[i] += vx[i] * dt;
//}
//#pragma omp parallel for
//for( int i = 0; i < numpart; ++i) {
////#pragma omp parallel for
y[i] += vy[i] * dt;
//}

//#pragma omp parallel for
//for( int i = 0; i < numpart; ++i) {
////#pragma omp parallel for
z[i] += vz[i] * dt;
}
}

std::ostream& operator<< (std::ostream &out, const part_sys_soa& p)
{
  for( int i = 0; i < p.x.size(); ++i) {
    out << "(" << p.x[i] << "," << p.y[i] << "," << p.z[i] << ")" << std::endl;
  }
  return out;
}

// Sync bboxes between all ranks
void part_sys_soa::syncbboxes(bbox lbox) {
  bboxes.resize(nprocs);
#ifndef USE_MIC
  boost::mpi::all_gather(world, lbox,bboxes);
#endif
  have_bboxes = true;
}

bool checkbbox_soa( double posx, double posy, double posz, bbox b) {
  return (posx > b[0]) && (posx < b[1]) && \
         (posy > b[2]) && (posy < b[3]) && \
         (posz > b[4]) && (posz < b[5]);
}

// return the rank that a particle p lives on (sync bboxes first)
int part_sys_soa::checkproc( int pind ) {
  if (checkbbox_soa(x[pind], y[pind], z[pind], bboxes[mproc])) {
    return mproc;
  } else {
    for(int proc=0; proc < nprocs; proc++) {
      if (checkbbox_soa(x[pind], y[pind], z[pind], bboxes[proc])) {
        return proc;
      }
    }
    return -1;
  }
}

void part_sys_soa::boostsend() {
#ifndef USE_MIC
  //Setup Send
  std::vector<std::vector<part> > sendpart;
  sendpart.resize(nprocs);

  for(int p=0; p < nprocs; p++) {
//    part temp;
    if( p == mproc ) {
      for( int i = 0; i < x.size(); ++i ) {
//        if((*it).proc != p) {
        if(ghost[i]) continue;
        int proc = checkproc(i);
        if(proc!=p) {
          if ( (proc >= 0) && (proc < nprocs)) {
            sendpart[proc].push_back(poppart(i));
          }
          //erasepart(i);
//          ghost[i] = true;
        }
      }
      //partv.erase(std::remove_if(partv.begin(), partv.end(), delpart), partv.end());
    }
  }

  // Setup recv
  std::vector<std::vector<part> > recvpart;
  recvpart.resize(nprocs);

  // let boost do its mpi magic
  boost::mpi::all_to_all(world, sendpart, recvpart);

  // Push the recved parts into partv
  int numrecv = 0;
  for(int q=0; q < nprocs; q++){
    if( recvpart[q].size() > 0){
      int recv_size = recvpart[q].size();
      for ( int i = 0; i < recv_size; ++i) {
        pushpart(recvpart[q][i]);
      }
    }
    numrecv += recvpart[q].size();
  }
  //if(verbose > 0)
  //  std::cout << "recved: " << numrecv << std::endl;
  //}
#endif
}

void part_sys_soa::sync_ghosts (double buff)
{
 clearghosts();
//  int gnum=0;
//  for(int i = ghost.size(); i > 0; --i) {
//    if(ghost[i]) {
//      erasepart(i);
//      gnum++;
//    }
//  }
//  for(std:vector<bool>::iterator it = ghost.begin(); it < ghost.end();) {
//  std:vector<bool>::iterator it = ghost.begin();
//  while(it != ghost.end()) {
//    if(*it) erasepart(i)
//    it++;
//  }
//  std::cout << "erased " << gnum << std::endl;
  //clearghosts();

  std::vector<double> buffsizes;

#ifndef USE_MIC
  boost::mpi::all_gather(world, buff, buffsizes);
#endif

  std::vector<bbox> buffboxes;
  buffboxes.resize(nprocs);
  std::vector<std::vector<part>> sendpart;
  sendpart.resize(nprocs);
  for(int proc=0; proc < nprocs; proc++)
  {
    if(proc == mproc) continue;
    buffboxes[proc].resize(6);
    for(int d=0; d<6; d=d+2) {
      buffboxes[proc][d] = bboxes[proc][d] - buffsizes[proc];
      buffboxes[proc][d+1] = bboxes[proc][d+1] + buffsizes[proc];
    }
    for( int p = 0; p < x.size(); ++p ) {
      if(checkbbox_soa(x[p], y[p], z[p], buffboxes[proc])) {
        sendpart[proc].push_back(poppart(p));
      }
    }
  }

  std::vector<std::vector<part> > recvpart;
  recvpart.resize(nprocs);

#ifndef USE_MIC
  boost::mpi::all_to_all(world, sendpart, recvpart);
#endif

  int numrecv = 0;
  for(int q=0; q < nprocs; q++){
    if( q == mproc) continue;
    if( recvpart[q].size() > 0){
      int recv_size = recvpart[q].size();
      for ( int i = 0; i < recv_size; ++i) {
        pushpart(recvpart[q][i],true);
      }
    }
    numrecv += recvpart[q].size();
  }
//  std::cout << "recved " << numrecv << std::endl;
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#pragma once

#include <cmath>

#include <Eigen/Geometry>

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

namespace kindr {

/*! \class AngleAxis
 * \brief Implementation of an angle axis rotation based on Eigen::AngleAxis
 *
 *  The following two typedefs are provided for convenience:
 *   - \ref AngleAxisD "AngleAxisPD" for primitive type double
 *   - \ref AngleAxisF "AngleAxisPF" for primitive type float
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *
 *  \ingroup rotations
 */
template<typename PrimType_>
class AngleAxis : public RotationBase<AngleAxis<PrimType_>> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::AngleAxis<PrimType_> Base;

  /*! Data container
   */
  Base angleAxis_;

 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;
  /*! \brief The axis type is a 3D vector.
   */
  typedef Eigen::Matrix<PrimType_, 3, 1> Vector3;

  /*! \brief All four parameters stored in a vector [angle; axis]
   */
  typedef Eigen::Matrix<PrimType_, 4, 1> Vector4;

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

  /*! \brief Constructor using four scalars.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param angle     rotation angle
   *  \param v1      first entry of the rotation axis vector
   *  \param v2      second entry of the rotation axis vector
   *  \param v3      third entry of the rotation axis vector
   */
  AngleAxis(Scalar angle, Scalar v1, Scalar v2, Scalar v3)
    : angleAxis_(angle,Vector3(v1,v2,v3)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using angle and axis.
   * In debug mode, an assertion is thrown if the rotation vector has not unit length.
   * \param angle   rotation angle
   * \param axis     rotation vector with unit length (Eigen vector)
   */
  AngleAxis(Scalar angle, const Vector3& axis)
    : angleAxis_(angle,axis) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using a 4x1matrix.
   * In debug mode, an assertion is thrown if the rotation vector has not unit length.
   * \param vector     4x1-matrix with [angle; axis]
   */
  AngleAxis(const Vector4& vector)
    : angleAxis_(vector(0),vector.template block<3,1>(1,0)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using Eigen::AngleAxis.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param other   Eigen::AngleAxis<PrimType_>
   */
  explicit AngleAxis(const Base& other) // explicit on purpose
    : angleAxis_(other) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit AngleAxis(const RotationBase<OtherDerived_>& other)
    : angleAxis_(internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation()) {
  }

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

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

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  AngleAxis inverted() const {
    Base inverse = this->toImplementation().inverse();
    inverse.angle() = -inverse.angle();
    inverse.axis() = -inverse.axis();
    return AngleAxis(inverse);
  }

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

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

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

  /*! \brief Returns the rotation angle.
   *  \returns rotation angle (scalar)
   */
  inline Scalar angle() const {
    return angleAxis_.angle();
  }

  /*! \brief Sets the rotation angle.
   */
  inline void setAngle(Scalar angle) {
    angleAxis_.angle() = angle;
  }

  /*! \brief Returns the rotation axis.
   *  \returns rotation axis (vector)
   */
  inline const Vector3& axis() const {
    return angleAxis_.axis();
  }

  /*! \brief Sets the rotation axis.
   */
  inline void setAxis(const Vector3& axis) {
    angleAxis_.axis() = axis;
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Sets the rotation axis.
   */
  inline void setAxis(Scalar v1, Scalar v2, Scalar v3) {
    angleAxis_.axis() = Vector3(v1,v2,v3);
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Sets angle-axis from a 4x1-matrix
   */
  inline void setVector(const Vector4& vector) {
    this->setAngle(vector(0));
    this->setAxis(vector.template block<3,1>(1,0));
  }

  /*! \returns the angle and axis in a 4x1 vector [angle; axis].
   */
  inline Vector4 vector() const {
    Vector4 vector;
    vector(0) = angle();
    vector.template block<3,1>(1,0) = axis();
    return vector;
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  AngleAxis& setIdentity() {
    this->setAngle(static_cast<Scalar>(0));
    this->setAxis(static_cast<Scalar>(1), static_cast<Scalar>(0), static_cast<Scalar>(0));
    return *this;
  }

  /*! \brief Returns a unique angle axis rotation with angle in [0,pi].
   *  This function is used to compare different rotations.
   *  \returns copy of the angle axis rotation which is unique
   */
  AngleAxis getUnique() const {
    const Scalar anglePosNegPi = kindr::wrapPosNegPI(angle());
    AngleAxis aa(anglePosNegPi, axis()); // first wraps angle into [-pi,pi)
    if(aa.angle() > 0.0)  {
        return aa;
    }
    else if(aa.angle() < 0.0) {
      if(aa.angle() != -M_PI) {
        return AngleAxis(-aa.angle(),-aa.axis());
      }
      else { // angle == -pi, so axis must be viewed further, because -pi,axis does the same as -pi,-axis
        if(aa.axis()[0] < 0.0) {
          return AngleAxis(-aa.angle(),-aa.axis());
        }
        else if(aa.axis()[0] > 0) {
          return AngleAxis(-aa.angle(),aa.axis());
        }
        else { // v1 == 0

          if(aa.axis()[1] < 0.0) {
            return AngleAxis(-aa.angle(),-aa.axis());
          }
          else if(aa.axis()[1] > 0) {
            return AngleAxis(-aa.angle(),aa.axis());
          }
          else { // v2 == 0

            if(aa.axis()[2] < 0.0) { // v3 must be -1 or 1
              return AngleAxis(-aa.angle(),-aa.axis());
            }
            else  {
              return AngleAxis(-aa.angle(),aa.axis());
            }
          }
        }
      }
    }
    else { // angle == 0
      return AngleAxis();
    }
  }

  /*! \brief Modifies the angle axis rotation such that the lies angle in [0,pi).
   *  \returns reference
   */
  AngleAxis& setUnique() {
    *this = getUnique();
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using RotationBase<AngleAxis<PrimType_>>::operator*;

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const AngleAxis& a) {
    out << a.angle() << ", " << a.axis().transpose();
    return out;
  }
};

//! \brief Active angle axis rotation with double primitive type
typedef AngleAxis<double>  AngleAxisPD;
//! \brief Active angle axis rotation with float primitive type
typedef AngleAxis<float>  AngleAxisPF;
//! \brief Passive angle axis rotation with double primitive type
typedef AngleAxis<double> AngleAxisD;
//! \brief Passive angle axis rotation with float primitive type
typedef AngleAxis<float> AngleAxisF;

namespace internal {

template<typename PrimType_>
class get_scalar<AngleAxis<PrimType_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_>
class get_matrix3X<AngleAxis<PrimType_>>{
 public:
  typedef int  IndexType;

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

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<AngleAxis<DestPrimType_>, AngleAxis<SourcePrimType_>> {
 public:
  inline static AngleAxis<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& a) {
    return AngleAxis<DestPrimType_>(a.toImplementation().template cast<DestPrimType_>());
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<AngleAxis<DestPrimType_>, RotationVector<SourcePrimType_>> {
 public:
  inline static AngleAxis<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
    typedef typename RotationVector<SourcePrimType_>::Scalar Scalar;

    // Cast to destination primitive type
    const Eigen::Matrix<DestPrimType_, 3, 1> vector = rotationVector.toImplementation().template cast<DestPrimType_>();
    const DestPrimType_ norm = vector.norm();

    // Check for identity rotation (we cannot divide by zero norm).
    if (norm < internal::NumTraits<Scalar>::dummy_precision()) {
      return AngleAxis<DestPrimType_>();
    }
    return AngleAxis<DestPrimType_>(norm, vector.normalized());
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<AngleAxis<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
 public:
  inline static AngleAxis<DestPrimType_> convert(const RotationQuaternion<SourcePrimType_>& q) {
    // bad prescion !
    return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(q.toImplementation().template cast<DestPrimType_>()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<AngleAxis<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
 public:
  inline static AngleAxis<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& rotationMatrix) {
    // Bad precision!
   return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(rotationMatrix.toImplementation().template cast<DestPrimType_>()));

  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<AngleAxis<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
 public:
  inline static AngleAxis<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
    // bad precision!
    return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(
        Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX()) *
        Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
        Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ())));
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<AngleAxis<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
 public:
  inline static AngleAxis<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
    // Bad precision!
    return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ()) *
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX())));
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Multiplication Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_>
class ComparisonTraits<AngleAxis<PrimType_>, AngleAxis<PrimType_>> {
 public:
  inline static bool isEqual(const AngleAxis<PrimType_>& a, const AngleAxis<PrimType_>& b){
    const double tolPercent = 0.01;
    return compareRelative(a.angle(), b.angle(), tolPercent) &&
        compareRelative(a.axis().x(), b.axis().x(), tolPercent) &&
        compareRelative(a.axis().y(), b.axis().y(), tolPercent) &&
        compareRelative(a.axis().z(), b.axis().z(), tolPercent);
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_>
class FixingTraits<AngleAxis<PrimType_>> {
 public:
  inline static void fix(AngleAxis<PrimType_>& aa) {
    aa.setAxis(aa.axis().normalized());
  }
};

} // namespace internal
} // namespace kindr


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#pragma once

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/quaternions/Quaternion.hpp"
#include "kindr/rotations/RotationBase.hpp"

namespace kindr {

/*!  \class RotationQuaternion
 *  \brief Implementation of quaternion rotation based on Eigen::Quaternion
 *
 *  The following four typedefs are provided for convenience:
 *   - \ref RotationQuaternionD "RotationQuaternionD" for primitive type double
 *   - \ref RotationQuaternionF "RotationQuaternionF" for primitive type float
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *
 *  \ingroup rotations
 */
template<typename PrimType_>
class RotationQuaternion : public RotationBase<RotationQuaternion<PrimType_>> {
 private:
  /*! \brief The base type.
   */
  typedef UnitQuaternion<PrimType_> Base;

  /*! \brief The data container
   */
  Base rotationQuaternion_;
 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef typename Base::Implementation Implementation;

  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  //! the imaginary type, i.e., Eigen::Quaternion<>
  typedef Eigen::Matrix<PrimType_,3,1> Imaginary;

  //! quaternion as 4x1 matrix: [w; x; y; z]
  typedef Eigen::Matrix<PrimType_,4,1> Vector4;

  /*! \brief Default constructor using identity rotation.
   */
  RotationQuaternion()
    : rotationQuaternion_(Implementation::Identity()) {
  }

  /*! \brief Constructor using four scalars.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param w     first entry of the quaternion = cos(phi/2)
   *  \param x     second entry of the quaternion = n1*sin(phi/2)
   *  \param y     third entry of the quaternion = n2*sin(phi/2)
   *  \param z     fourth entry of the quaternion = n3*sin(phi/2)
   */
  RotationQuaternion(Scalar w, Scalar x, Scalar y, Scalar z)
    : rotationQuaternion_(w,x,y,z) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using real and imaginary part.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param real   real part (PrimType_)
   *  \param imag   imaginary part (Eigen::Matrix<PrimType_,3,1>)
   */
  RotationQuaternion(Scalar real, const Imaginary& imag)
    : rotationQuaternion_(real,imag(0),imag(1),imag(2)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-2), "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using Eigen::Matrix<PrimType_,4,1>.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param other   Eigen::Matrix<PrimType_,4,1>
   */
  RotationQuaternion(const Vector4 & vec)
    : rotationQuaternion_(vec(0),vec(1),vec(2),vec(3)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), 1e-2, "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using Eigen::Quaternion<PrimType_>.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param other   Eigen::Quaternion<PrimType_>
   */
  explicit RotationQuaternion(const Implementation& other)
    : rotationQuaternion_(other.w(), other.x(), other.y(), other.z()) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), 1e-2, "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using UnitQuaternion.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param other   UnitQuaternion
   */
  explicit RotationQuaternion(const Base& other)
    : rotationQuaternion_(other.w(), other.x(), other.y(), other.z()) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, rotationQuaternion_.norm(), static_cast<Scalar>(1), 1e-2, "Input quaternion has not unit length.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit RotationQuaternion(const RotationBase<OtherDerived_>& other)
    : rotationQuaternion_(internal::ConversionTraits<RotationQuaternion, OtherDerived_>::convert(other.derived()).toImplementation()) {
  }

  inline Scalar w() const {
    return rotationQuaternion_.w();
  }

  inline Scalar x() const {
    return rotationQuaternion_.x();
  }

  inline Scalar y() const {
    return rotationQuaternion_.y();
  }

  inline Scalar z() const {
    return rotationQuaternion_.z();
  }

  inline Scalar& w() {
    return rotationQuaternion_.w();
  }

  inline Scalar& x() {
    return rotationQuaternion_.x();
  }

  inline Scalar& y() {
    return rotationQuaternion_.y();
  }

  inline Scalar& z() {
    return rotationQuaternion_.z();
  }

  inline Scalar real() const {
    return this->toUnitQuaternion().real();
  }

  inline Imaginary imaginary() const {
    return this->toUnitQuaternion().imaginary();
  }

  inline Vector4 vector() const {
    Vector4 vector4;
    vector4 << w(), x(), y(), z();
    return vector4;
  }

  inline void setValues(Scalar w, Scalar x, Scalar y, Scalar z) {
    rotationQuaternion_.w() = w;
    rotationQuaternion_.x() = x;
    rotationQuaternion_.y() = y;
    rotationQuaternion_.z() = z;
  }

  inline void setParts(Scalar real, const Imaginary& imag) {
    rotationQuaternion_.w() = real;
    rotationQuaternion_.x() = imag(0);
    rotationQuaternion_.y() = imag(1);
    rotationQuaternion_.z() = imag(2);
  }

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

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

  Base& toUnitQuaternion() {
     return static_cast<Base&>(rotationQuaternion_);
   }

  const Base& toUnitQuaternion() const {
     return static_cast<const Base&>(rotationQuaternion_);
   }

  /*! \brief Assignment operator using a UnitQuaternion.
   *  \param quat   UnitQuaternion
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator =(const UnitQuaternion<PrimTypeIn_>& quat) {
    this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
    return *this;
  }

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

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator =(const RotationQuaternion<PrimTypeIn_>& other) {
    this->toImplementation() = other.toImplementation().template cast<PrimType_>();
    return *this;
  }

  /*! \brief Bracket operator which assigns a UnitQuaternion to the RotationQuaternion.
   *  \param quat   UnitQuaternion
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator ()(const UnitQuaternion<PrimTypeIn_>& quat) {
    this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
    return *this;
  }

  /*! \brief Bracket operator which assigns a Quaternion to the RotationQuaternion.
   *  In debug mode, an assertion is thrown if the quaternion has not unit length.
   *  \param quat   Quaternion
   *  \returns reference
   */
  template<typename PrimTypeIn_>
  RotationQuaternion& operator ()(const Quaternion<PrimTypeIn_>& quat) {
    this->toImplementation() = Implementation(quat.w(),quat.x(),quat.y(),quat.z());
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input quaternion has not unit length.");
    return *this;
  }

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

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  RotationQuaternion inverted() const {
    return RotationQuaternion(this->toUnitQuaternion().inverted());
  }

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

  /*! \brief Returns the conjugated of the quaternion.
   *  \returns conjugated of the quaternion
   */
  RotationQuaternion conjugated() const {
    return RotationQuaternion(this->toUnitQuaternion().conjugated());
  }

  /*! \brief Conjugates of the quaternion.
   *  \returns reference
   */
  RotationQuaternion& conjugate() {
    *this = conjugated();
    return *this;
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  RotationQuaternion& setIdentity() {
    rotationQuaternion_.w() = static_cast<Scalar>(1);
    rotationQuaternion_.x() = static_cast<Scalar>(0);
    rotationQuaternion_.y() = static_cast<Scalar>(0);
    rotationQuaternion_.z() = static_cast<Scalar>(0);
    return *this;
  }

  /*! \brief Returns a unique quaternion rotation with w > 0.
   *  This function is used to compare different rotations.
   *  \returns copy of the quaternion rotation which is unique
   */
  RotationQuaternion getUnique() const {
    if(this->w() > 0) {
      return *this;
    } else if (this->w() < 0){
      return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
    } else { // w == 0

      if(this->x() > 0) {
        return *this;
      } else if (this->x() < 0){
        return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
      } else { // x == 0

        if(this->y() > 0) {
          return *this;
        } else if (this->y() < 0){
          return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
        } else { // y == 0

          if(this->z() > 0) { // z must be either -1 or 1 in this case
            return *this;
          } else {
            return RotationQuaternion(-this->w(),-this->x(),-this->y(),-this->z());
          }
        }
      }
    }
  }

  /*! \brief Returns the quaternion matrix Qleft: q*p = Qleft(q)*p
   *  This function can be used to get the derivative of the concatenation with respect to the right quaternion.
   *  \returns the quaternion matrix Qleft
   */
  Eigen::Matrix<PrimType_,4,4> getQuaternionMatrix() const {
    Eigen::Matrix<PrimType_,4,4> Qleft;
    Qleft(0,0) =  w();      Qleft(0,1) = -x();      Qleft(0,2) = -y();      Qleft(0,3) = -z();
    Qleft(1,0) =  x();      Qleft(1,1) =  w();      Qleft(1,2) = -z();      Qleft(1,3) =  y();
    Qleft(2,0) =  y();      Qleft(2,1) =  z();      Qleft(2,2) =  w();      Qleft(2,3) = -x();
    Qleft(3,0) =  z();      Qleft(3,1) = -y();      Qleft(3,2) =  x();      Qleft(3,3) =  w();

    return Qleft;
  }

  /*! \brief Returns the quaternion matrix Qright: q*p = Qright(p)*q
   *  This function can be used to get the derivative of the concatenation with respect to the left quaternion.
   *  \returns the quaternion matrix Qright
   */
  Eigen::Matrix<PrimType_,4,4> getConjugateQuaternionMatrix() const {
    Eigen::Matrix<PrimType_,4,4> Qright;
    Qright(0,0) =  w();      Qright(0,1) = -x();      Qright(0,2) = -y();      Qright(0,3) = -z();
    Qright(1,0) =  x();      Qright(1,1) =  w();      Qright(1,2) =  z();      Qright(1,3) = -y();
    Qright(2,0) =  y();      Qright(2,1) = -z();      Qright(2,2) =  w();      Qright(2,3) =  x();
    Qright(3,0) =  z();      Qright(3,1) =  y();      Qright(3,2) = -x();      Qright(3,3) =  w();

    return Qright;
  }

  /*! \brief Returns the global quaternion diff matrix H: GlobalAngularVelocity = 2*H*qdiff, qdiff = 0.5*H^T*GlobalAngularVelocity
   *  \returns the global quaternion diff matrix H
   */
  Eigen::Matrix<PrimType_,3,4> getGlobalQuaternionDiffMatrix() const {
    Eigen::Matrix<PrimType_,3,4> H;
      H(0,0) = -x();      H(0,1) =  w();      H(0,2) = -z();      H(0,3) =  y();
      H(1,0) = -y();      H(1,1) =  z();      H(1,2) =  w();      H(1,3) = -x();
      H(2,0) = -z(); H(2,1) = -y(); H(2,2) = x(); H(2,3) = w();
    return H;
  }

  /*! \brief Returns the local quaternion diff matrix HBar: LocalAngularVelocity = 2*HBar*qdiff, qdiff = 0.5*HBar^T*LocalAngularVelocity
   *  \returns the local quaternion diff matrix HBar
   */
  Eigen::Matrix<PrimType_,3,4> getLocalQuaternionDiffMatrix() const {
    Eigen::Matrix<PrimType_,3,4> HBar;
      HBar(0,0) = -x();      HBar(0,1) =  w();      HBar(0,2) =  z();      HBar(0,3) = -y();
      HBar(1,0) = -y();      HBar(1,1) = -z();      HBar(1,2) =  w();      HBar(1,3) =  x();
      HBar(2,0) = -z();      HBar(2,1) = y();       HBar(2,2) = -x();       HBar(2,3) = w();
    return HBar;
  }

  /*! \brief Modifies the quaternion rotation such that w >= 0.
   *  \returns reference
   */
  RotationQuaternion& setUnique() {
    *this = getUnique();
    return *this;
  }

  /*! \brief Returns the norm of the quaternion.
   *  The RotationQuaternion should always have unit length.
   *  \returns norm of the quaternion
   */
  Scalar norm() {
    return rotationQuaternion_.norm();
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because QuaternionBase provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using RotationBase<RotationQuaternion<PrimType_>> ::operator*;

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because QuaternionBase provides also an operator==.
   *  \returns true if two rotations are similar.
   */
  using RotationBase<RotationQuaternion<PrimType_>> ::operator==;

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationQuaternion& rquat) {
    out << rquat.toUnitQuaternion();
    return out;
  }
};

//! \brief Passive quaternion rotation with double primitive type
typedef RotationQuaternion<double> RotationQuaternionPD;
//! \brief Passive quaternion rotation with float primitive type
typedef RotationQuaternion<float> RotationQuaternionPF;

//! \brief Passive quaternion rotation with double primitive type
typedef RotationQuaternion<double> RotationQuaternionD;
//! \brief Passive quaternion rotation with float primitive type
typedef RotationQuaternion<float> RotationQuaternionF;

namespace internal {

template<typename PrimType_>
class get_scalar<RotationQuaternion<PrimType_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_>
class get_matrix3X<RotationQuaternion<PrimType_>>{
 public:
  typedef int IndexType;

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

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationQuaternion<DestPrimType_>, AngleAxis<SourcePrimType_>> {
 public:
  inline static RotationQuaternion<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& aa) {
    return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(aa.toImplementation().template cast<DestPrimType_>()));
  }
};

template <typename Scalar_ = double>
inline bool isLessThenEpsilons4thRoot(Scalar_ x){
  static const Scalar_ epsilon4thRoot = pow(std::numeric_limits<Scalar_>::epsilon(), 1.0/4.0);
  return x < epsilon4thRoot;
}

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationQuaternion<DestPrimType_>, RotationVector<SourcePrimType_>> {
 public:
  inline static RotationQuaternion<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
    typedef typename RotationQuaternion<DestPrimType_>::Scalar Scalar;
    typedef typename RotationQuaternion<DestPrimType_>::Imaginary Imaginary;
//    const Scalar v = rotationVector.toImplementation().norm();
//    Scalar real;
//    Imaginary imaginary;
//    if (v < internal::NumTraits<Scalar>::dummy_precision()) {
//      real = 1.0;
//      imaginary= 0.5*rotationVector.toImplementation().template cast<DestPrimType_>();
//    }
//    else {
//      real = cos(v/2);
//      imaginary = sin(v/2)/v*rotationVector.toImplementation().template cast<DestPrimType_>();
//    }
//    return RotationQuaternion<DestPrimType_>(real, imaginary);

    Scalar theta = (Scalar)rotationVector.toImplementation().norm();

    // na is 1/theta sin(theta/2)
    double na;
    if(isLessThenEpsilons4thRoot(theta))
    {
        const Scalar one_over_48 = 1.0/48.0;
        na = 0.5 + (theta * theta) * one_over_48;
    }
    else
    {
        na = sin(theta*0.5) / theta;
    }
    Imaginary axis = rotationVector.toImplementation().template cast<Scalar>()*na;
    Scalar ct = cos(theta*0.5);
    return RotationQuaternion<DestPrimType_>(ct, axis[0],axis[1],axis[2]);
//    return Eigen::Vector4d(axis[0],axis[1],axis[2],ct);

  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationQuaternion<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
 public:
  inline static RotationQuaternion<DestPrimType_> convert(const RotationQuaternion<SourcePrimType_>& q) {
    return RotationQuaternion<DestPrimType_>(q.toImplementation().template cast<DestPrimType_>());
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationQuaternion<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
 public:
  inline static RotationQuaternion<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& rotationMatrix) {
    return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(rotationMatrix.toImplementation().template cast<DestPrimType_>()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationQuaternion<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
 public:
  inline static RotationQuaternion<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
    return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX()) *
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ())));
  }
};

template<typename DestPrimType_, typename SourcePrimType_>
class ConversionTraits<RotationQuaternion<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
 public:
  inline static RotationQuaternion<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
    return RotationQuaternion<DestPrimType_>(Eigen::Quaternion<DestPrimType_>(
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ()) *
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
      Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX())));
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Multiplication Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_>
class ComparisonTraits<RotationQuaternion<PrimType_>, RotationQuaternion<PrimType_>> {
 public:
   inline static bool isEqual(const RotationQuaternion<PrimType_>& a, const RotationQuaternion<PrimType_>& b){
     return a.toImplementation().w() ==  b.toImplementation().w() &&
            a.toImplementation().x() ==  b.toImplementation().x() &&
            a.toImplementation().y() ==  b.toImplementation().y() &&
            a.toImplementation().z() ==  b.toImplementation().z();
   }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_>
class FixingTraits<RotationQuaternion<PrimType_>> {
 public:
  inline static void fix(RotationQuaternion<PrimType_>& q) {
    q.toImplementation().normalize();
  }
};

} // namespace internal

} // namespace kindr