/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <iostream>
#include <memory>

#include <BayesFilters/BootstrapCorrection.h>
#include <BayesFilters/DrawParticles.h>
#include <BayesFilters/GaussianLikelihood.h>
#include <BayesFilters/InitSurveillanceAreaGrid.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/Resampling.h>
#include <BayesFilters/SIS.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>
#include <BayesFilters/utils.h>
#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class SISSimulation : public SIS
{
public:
    SISSimulation
    (
        unsigned int num_particle,
        std::size_t state_size,
        unsigned int simulation_steps,
        std::unique_ptr<ParticleSetInitialization> initialization,
        std::unique_ptr<PFPrediction> prediction,
        std::unique_ptr<PFCorrection> correction,
        std::unique_ptr<Resampling> resampling
    ) noexcept :
        SIS(num_particle, state_size, std::move(initialization), std::move(prediction), std::move(correction), std::move(resampling)),
        simulation_steps_(simulation_steps)
    { }

protected:
    bool runCondition() override
    {
        if (getFilteringStep() < simulation_steps_)
            return true;
        else
            return false;
    }

private:
    unsigned int simulation_steps_;
};

int main()
{
    std::cout << "Running a SIS particle filter on a simulated target." << std::endl;
    std::cout << "Data is logged in the test folder with prefix testSIS." << std::endl;

    /* A set of parameters needed to run a SIS particle filter in a simulated environment. */
    double surv_x = 1000.0;
    double surv_y = 1000.0;
    unsigned int num_particle_x = 100;
    unsigned int num_particle_y = 100;
    unsigned int num_particle = num_particle_x * num_particle_y;
    Vector4d initial_state(10.0f, 0.0f, 10.0f, 0.0f);
    unsigned int simulation_time = 100;
    std::size_t state_size = 4;

    /* Step 1 - Initialization */
    /* Initialize initialization class. */
    std::unique_ptr<ParticleSetInitialization> grid_initialization = utils::make_unique<InitSurveillanceAreaGrid>(surv_x, surv_y, num_particle_x, num_particle_y);

    /* Step 2 - Prediction */
    /* Step 2.1 - Define the state model */
    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<StateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Step 2.2 - Define the prediction step */
    /* Initialize the particle filter prediction step and pass the ownership of the state model. */
    std::unique_ptr<PFPrediction> pf_prediction = utils::make_unique<DrawParticles>();
    pf_prediction->setStateModel(std::move(wna));

    /* Step 3 - Correction */
    /* Step 3.1 - Define where the measurement are originated from (either simulated or from a real process) */
    /* Initialize simulaterd target model with a white noise acceleration. */
    std::unique_ptr<StateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_state, simulation_time);
    simulated_state_model->enable_log(".", "testSIS");

    /* Initialize a measurement model (a linear sensor reading x and y coordinates). */
    std::unique_ptr<MeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model));
    simulated_linear_sensor->enable_log(".", "testSIS");

    /* Step 3.3 - Define the likelihood model */
    /* Initialize the the exponential likelihood, a PFCorrection decoration of the particle filter correction step. */
    std::unique_ptr<LikelihoodModel> exp_likelihood = utils::make_unique<GaussianLikelihood>();

    /* Step 3.4 - Define the correction step */
    /* Initialize the particle filter correction step and pass the ownership of the measurement model. */
    std::unique_ptr<PFCorrection> pf_correction = utils::make_unique<BoostrapCorrection>();
    pf_correction->setLikelihoodModel(std::move(exp_likelihood));
    pf_correction->setMeasurementModel(std::move(simulated_linear_sensor));

    /* Step 4 - Resampling */
    /* Initialize a resampling algorithm */
    std::unique_ptr<Resampling> resampling = utils::make_unique<Resampling>();

    /* Step 5 - Assemble the particle filter */
    std::cout << "Constructing SIS particle filter..." << std::flush;
    SISSimulation sis_pf(num_particle, state_size, simulation_time, std::move(grid_initialization), std::move(pf_prediction), std::move(pf_correction), std::move(resampling));
    sis_pf.enable_log(".", "testSIS");
    std::cout << "done!" << std::endl;

    /* Step 6 - Prepare the filter to be run */
    std::cout << "Booting SIS particle filter..." << std::flush;
    sis_pf.boot();
    std::cout << "completed!" << std::endl;

    /* Step 7 - Run the filter and wait until it is closed */
    /* Note that since this is a simulation, the filter will end upon simulation termination */
    std::cout << "Running SIS particle filter..." << std::flush;
    sis_pf.run();
    std::cout << "waiting..." << std::flush;
    if (!sis_pf.wait())
        return EXIT_FAILURE;
    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


#ifndef OTHELLO_AI_QLEARNINGPLAYER_HPP
#define OTHELLO_AI_QLEARNINGPLAYER_HPP

//Boost headers:
#include <boost/random.hpp>
//FANN headers:
#include <fann/doublefann.h>
#include <fann/fann_cpp.h>
//Othello headers:
#include <othello/ai/ILearningPlayer.hpp>
#include <othello/game/Game.hpp>

namespace othello
{

    namespace ai
    {

        ////////////////////////////////////////////////////////////////
        /// \class QLearningPlayer
        ///
        /// \brief A learning AI player that plays moves using Q
        ///        Learning and a multilayer perceptron neural network
        ///
        ////////////////////////////////////////////////////////////////
        class QLearningPlayer : public ILearningPlayer
        {
            private:

                ////////////////////////////////////////////////////////////////
                /// \brief Whether the player is in training mode
                ///
                ////////////////////////////////////////////////////////////////
                bool training;

                ////////////////////////////////////////////////////////////////
                /// \brief A mersenne_twister_engine for generating random
                ///        numbers
                ///
                ////////////////////////////////////////////////////////////////
                boost::random::mt19937 randomNumberGenerator;

                ////////////////////////////////////////////////////////////////
                /// \brief Static multilayer perceptron network
                ///
                /// The network is static so that multiple agents share the same
                /// network (for self-learning)
                ///
                ////////////////////////////////////////////////////////////////
                static FANN::neural_net mlp;

                ////////////////////////////////////////////////////////////////
                /// \struct TurnState
                ///
                /// \brief Structure representing the state of the player at a
                ///        certain turn in the game
                ///
                ////////////////////////////////////////////////////////////////
                struct TurnState
                {

                    ////////////////////////////////////////////////////////////////
                    /// \brief The state of the board as an input into the NN
                    ///
                    ////////////////////////////////////////////////////////////////
                    std::array<fann_type, game::Board::BOARD_SIZE * game::Board::BOARD_SIZE> input;

                    ////////////////////////////////////////////////////////////////
                    /// \brief The index in output of the valid move with the
                    ///        highest intensity
                    ///
                    ////////////////////////////////////////////////////////////////
                    std::size_t playedMove;

                    ////////////////////////////////////////////////////////////////
                    /// \brief The Q value of the state
                    ///
                    ////////////////////////////////////////////////////////////////
                    fann_type QVal;

                    ////////////////////////////////////////////////////////////////
                    /// \brief Class constructor
                    ///
                    ////////////////////////////////////////////////////////////////
                    TurnState(std::array<fann_type, game::Board::BOARD_SIZE * game::Board::BOARD_SIZE> input,
                              const std::size_t& playedMove, const fann_type& QVal)
                            : input(input), playedMove(playedMove), QVal(QVal) {}

                };

                ////////////////////////////////////////////////////////////////
                /// \brief A vector containing the turn states of the on-going
                ///        game
                ///
                ////////////////////////////////////////////////////////////////
                std::vector<TurnState> states;

                ////////////////////////////////////////////////////////////////
                /// \brief The discount factor
                ///
                ////////////////////////////////////////////////////////////////
                const fann_type discountFactor = 1.0;

                ////////////////////////////////////////////////////////////////
                /// \brief The probability for the network to pick a random
                ///        action
                ///
                ////////////////////////////////////////////////////////////////
                fann_type epsilon = 0.1;

                ////////////////////////////////////////////////////////////////
                /// \brief The amount that epsilon should change every training
                ///        cycle
                ///
                ////////////////////////////////////////////////////////////////
                fann_type deltaEpsilon;

            public:

                ////////////////////////////////////////////////////////////////
                /// \brief Class constructor
                ///
                /// \param training Whether the player should be in training
                ///        mode
                /// \param seed The seed to be used for random numbers
                /// \param numCycles The number of training cycles. Can be 0.
                /// \param numHiddenLayers The number of hidden layers
                /// \param numHiddenNeurons The number of neurons in each hidden
                ///        layer
                /// \param discountFactor The discount factor (how much it
                ///        discounts later rewards)
                /// \param learningRate How fast the values in the neural
                ///        network change
                /// \param epsilon The probability of the player to select
                ///        random moves during testing. Decreases linerarly over
                ///        the course of training
                ///
                ////////////////////////////////////////////////////////////////
                QLearningPlayer(const bool& training, const unsigned int& seed,
                        const unsigned int& numCycles, const unsigned int& numHiddenLayers,
                        const unsigned int& numHiddenNeurons, const fann_type& discountFactor,
                        const fann_type& learningRate, const fann_type& epsilon);

                ////////////////////////////////////////////////////////////////
                /// \brief Function that is called when the player should make a
                ///        move. This function uses a neural network to make a
                ///        decision
                ///
                /// \param game A const reference to the game to make a move in
                /// \param player The index of this player in the game. (0 is
                ///        player 1, 1 is player 2)
                /// \param possibleMoves A vector of the possible moves
                ///
                /// \return A const pointer to a const move that will be played
                ///         by the current player. This pointer must point to a
                ///         move in possibleMoves
                ///
                ////////////////////////////////////////////////////////////////
                const game::Move* makeMove(const game::Game& game, const uint8_t& player,
                                           const std::vector<game::Move>& possibleMoves) override;

                ////////////////////////////////////////////////////////////////
                /// \brief Virtual function that is called when a game finishes
                ///
                ////////////////////////////////////////////////////////////////
                void gameFinished(const game::Game&, const uint8_t&) override;

                ////////////////////////////////////////////////////////////////
                /// \brief Function to either enable or disable training the
                ///        player
                ///
                /// \param trainingMode Whether to enable or disable training
                ///
                ////////////////////////////////////////////////////////////////
                void setTraining(const bool& trainingMode) override;

        };

    }

}

#endif //OTHELLO_AI_QLEARNINGPLAYER_HPP


#include "catch.hpp"

#include <vector>

#include <libArrhenius/Integration/ArrheniusIntegral.hpp>

#include "fakeit.hpp"

using namespace libArrhenius;
using namespace libArrhenius::Constants;

TEST_CASE("ArrheniusIntegral Usage", "[integral]")
{
  double              tau = 2;
  double              dt  = tau / 20;
  size_t              N   = 4 * tau / dt;
  std::vector<double> t(N), T(N);

  for (size_t i = 0; i < t.size(); i++) {
    t[i] = dt * i;
    T[i] = 310;
    if (t[i] > tau / 2) T[i] = 100 + 310;
    if (t[i] > tau + tau / 2) T[i] = 310;
  }

  double A, Ea, Omega;

  A  = 3.1e99;
  Ea = 6.28e5;

  SECTION("Trapezoid")
  {
    ArrheniusIntegral<double> Arr(A, Ea);

    Omega = Arr(N, t.data(), T.data());
    CHECK(Omega == Approx(A * exp(-Ea / (MKS::R * 410)) * tau +
                          A * exp(-Ea / (MKS::R * 310)) * 3 * tau));

    A  = 2.00e30;
    Ea = 2.00e5;

    Arr.setA(A);
    Arr.setEa(Ea);

    Omega = Arr(N, t.data(), T.data());

    CHECK(Omega == Approx(A * exp(-Ea / (MKS::R * 410)) * tau +
                          A * exp(-Ea / (MKS::R * 310)) * 3 * tau)
                       .epsilon(0.0001));

    // this will result in a number too small for double
    A  = 2.00e30;
    Ea = 2.00e10;

    Arr.setA(A);
    Arr.setEa(Ea);

    Omega = Arr(N, t.data(), T.data());

    CHECK(Omega == 0);

    // this will result in a number too large for double
    A  = 2.00e300;
    Ea = -2.00e5;

    Arr.setA(A);
    Arr.setEa(Ea);

    Omega = Arr(N, t.data(), T.data());

    CHECK(Omega == std::numeric_limits<double>::infinity());
  }
}

TEST_CASE("ArrheniusIntegral Large Profile", "[integral]")
{
  double A, Ea, Omega;

  A  = 3.1e99;
  Ea = 6.28e5;

  ArrheniusIntegral<double> Arr(A, Ea);

  // create a data set that will trigger parallelization.
  double              tau = 2;
  double              dt  = tau / 200;
  size_t              N   = 1000;
  std::vector<double> t(N), T(N);

  SECTION("Constant Temperature")
  {
    for (size_t i = 0; i < t.size(); i++) {
      t[i] = dt * i;
      T[i] = 310;
      if (t[i] > tau / 2) T[i] = 100 + 310;
      if (t[i] > tau + tau / 2) T[i] = 310;
    }

    double Exact = A * exp(-Ea / (MKS::R * 410)) * tau + A * exp(-Ea / (MKS::R * 310)) * 3 * tau;

    SECTION("Trapezoid")
    {
      ArrheniusIntegral<double, Trapezoid> Arr(A, Ea);
      Omega = Arr(N, t.data(), T.data());
      CHECK(Omega == Approx(Exact));
    }
    SECTION("Exponential Integral")
    {
      ArrheniusIntegral<double, ExponentialIntegral> Arr(A, Ea);
      Omega = Arr(N, t.data(), T.data());
      CHECK(Omega == Approx(Exact).epsilon(0.05));
    }
  }

  SECTION("Linear Temperature")
  {
    for (size_t i = 0; i < t.size(); i++) {
      t[i] = dt * i;
      T[i] = 310 + t[i];
    }

    double t0 = t[0];
    double t1 = t[t.size()-1];
    double T0 = T[0];
    double T1 = T[T.size()-1];
    double Exact = A*(t1-t0)/(T1-T0)*( T1*boost::math::expint(2,Ea/(MKS::R * T1)) - T0*boost::math::expint(2,Ea/(MKS::R * T0)) );

    SECTION("Trapezoid")
    {
      ArrheniusIntegral<double, Trapezoid> Arr(A, Ea);
      Omega = Arr(N, t.data(), T.data());
      CHECK(Omega == Approx(Exact));
    }
    SECTION("Exponential Integral")
    {
      ArrheniusIntegral<double, ExponentialIntegral> Arr(A, Ea);
      Omega = Arr(N, t.data(), T.data());
      CHECK(Omega == Approx(Exact));
    }
  }
}

#include <boost/multiprecision/cpp_dec_float.hpp>
using namespace boost::multiprecision;

TEST_CASE("ArrheniusIntegral With Boost.Multiprecision", "[integral]")
{
  // doesn't seem to work on Ubuntu with boost 1.57
  // complains about the convert_to<>() calls...
  cpp_dec_float_100              tau = 2;
  cpp_dec_float_100              dt  = tau / 20;
  size_t                         N   = (4 * tau / dt).convert_to<size_t>();
  std::vector<cpp_dec_float_100> t(N), T(N);

  for (size_t i = 0; i < t.size(); i++) {
    t[i] = dt * i;
    T[i] = 310;
    if (t[i] > tau / 2) T[i] = 100 + 310;
    if (t[i] > tau + tau / 2) T[i] = 310;
  }

  cpp_dec_float_100 A, Ea, Omega;

  A  = 3.1e99;
  Ea = 6.28e5;

  ArrheniusIntegral<cpp_dec_float_100> Arr(A, Ea);

  Omega = Arr(N, t.data(), T.data());
  CHECK(Omega.convert_to<double>() ==
        Approx((A * exp(-Ea / (MKS::R * 410)) * tau +
                A * exp(-Ea / (MKS::R * 310)) * 3 * tau)
                   .convert_to<double>()));

  A  = 2.00e30;
  Ea = 2.00e5;

  Arr.setA(A);
  Arr.setEa(Ea);

  Omega = Arr(N, t.data(), T.data());

  CHECK(Omega.convert_to<double>() ==
        Approx((A * exp(-Ea / (MKS::R * 410)) * tau +
                A * exp(-Ea / (MKS::R * 310)) * 3 * tau)
                   .convert_to<double>())
            .epsilon(0.001));

  A  = 2.00e30;
  Ea = 2.00e10;

  Arr.setA(A);
  Arr.setEa(Ea);

  Omega = Arr(N, t.data(), T.data());

  CHECK(Omega > 0);

  // this will result in a number too large for double
  A  = 2.00e300;
  Ea = -2.00e5;

  Arr.setA(A);
  Arr.setEa(Ea);

  Omega = Arr(N, t.data(), T.data());

  CHECK(Omega != std::numeric_limits<cpp_dec_float_100>::infinity());
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "LevenbergMarquardt.h"
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>

namespace Scine {
namespace Utils {

void LevenbergMarquardt::optimize(Eigen::VectorXd& parameters, UpdateFunctionManagerBase& updateFunctionManager) {
  LMFunctor functor(updateFunctionManager);
  functor.n = static_cast<int>(parameters.size());
  functor.m = functor.updateFunctionManager_.getNumberOfDataPoints(parameters);
  Eigen::LevenbergMarquardt<LMFunctor, double> lm(functor);
  // Set maximum number of function evaluations if it was set to a sensible value
  if (maxFuncEval > 0)
    lm.parameters.maxfev = maxFuncEval;
  lm.minimize(parameters);
  // Calculate and update covariance matrix if desired
  if (calculateCovarianceMatrix) {
    auto hessian = lm.fjac.transpose() * lm.fjac;
    auto inverseHessian = hessian.inverse();
    auto variance = (1.0 / (functor.m - functor.n + 1.0)) * lm.fvec.squaredNorm();
    covarianceMatrix_ = variance * inverseHessian;
  }
}

LevenbergMarquardt::LMFunctor::LMFunctor(UpdateFunctionManagerBase& updateFunctionManager)
  : updateFunctionManager_(updateFunctionManager) {
}

// Compute 'm' errors, one for each data point, for the given parameter values in 'x'
int LevenbergMarquardt::LMFunctor::operator()(const Eigen::VectorXd& parameters, Eigen::VectorXd& fvec) const {
  updateFunctionManager_.updateErrors(parameters, fvec);
  return 0;
}

// Compute the Jacobian of the errors
int LevenbergMarquardt::LMFunctor::df(const Eigen::VectorXd& parameters, Eigen::MatrixXd& fjac) const {
  updateFunctionManager_.updateJacobian(parameters, fjac);
  return 0;
}

// Returns 'm', the number of values.
int LevenbergMarquardt::LMFunctor::values() const {
  return m;
}

// Returns 'n', the number of inputs.
int LevenbergMarquardt::LMFunctor::inputs() const {
  return n;
}

const Eigen::MatrixXd& LevenbergMarquardt::getCovarianceMatrix() {
  return covarianceMatrix_;
}

} // namespace Utils
} // namespace Scine


//==================================================================================================
/**
  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_ARCH_COMMON_DETAIL_SCALAR_EXPO_BASE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_EXPO_BASE_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/is_nan.hpp>
#endif
#include <boost/simd/arch/common/detail/scalar/expo_reduction.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace detail
{
  template< typename A0
          , typename Tag
          , typename Style
          , typename base_A0 = bd::scalar_of_t<A0>
          >
  struct exponential
  {};

  template<typename A0, typename Tag>
  struct exponential< A0, Tag, tag::not_simd_type, double>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax) where a is 1, 2 or ten depending on Tag
    static BOOST_FORCEINLINE A0 expa(A0 a0) BOOST_NOEXCEPT
    {
      if (reduc_t::isgemaxlog(a0)) return Inf<A0>();
      if (reduc_t::isleminlog(a0)) return Zero<A0>();
     #ifndef BOOST_SIMD_NO_INVALIDS
      if (is_nan(a0)) return a0;
     #endif
      A0 hi = Zero<A0>(), lo = Zero<A0>(), x = Zero<A0>();
      A0 k = reduc_t::reduce(a0, hi, lo, x);
      A0 c = reduc_t::approx(x);
      c = reduc_t::finalize(x, c, hi, lo);
      return  ldexp(c, toint(k));
    }
  };

  template<typename A0, typename Tag>
  struct exponential< A0, Tag, tag::not_simd_type, float>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax) where a is 1, 2 or ten depending on Tag
    static BOOST_FORCEINLINE A0 expa(A0 a0) BOOST_NOEXCEPT
    {

      if (reduc_t::isgemaxlog(a0)) return Inf<A0>();
      if (reduc_t::isleminlog(a0)) return Zero<A0>();
    #ifndef BOOST_SIMD_NO_INVALIDS
      if (is_nan(a0)) return a0;
    #endif
      A0 x;
      A0 k = reduc_t::reduce(a0, x);
      x = reduc_t::approx(x);
      return  ldexp(x, toint(k));
    }
  };
} } }
#endif


/** @file
    @brief Implementation

    @date 2015-2020

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
// Copyright 2019-2020 Collabora, Ltd.
//
// 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 <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

#if 0
template <typename T>
inline void dumpKalmanDebugOuput(const char name[], const char expr[],
                                 T const &value) {
    std::cout << "\n(Kalman Debug Output) " << name << " [" << expr << "]:\n"
              << value << std::endl;
}

#define FLEXKALMAN_DEBUG_OUTPUT(Name, Value)                                   \
    dumpKalmanDebugOuput(Name, #Value, Value)
#endif

// Internal Includes
#include "FlexKalman/AbsoluteOrientationMeasurement.h"
#include "FlexKalman/AbsolutePositionMeasurement.h"
#include "FlexKalman/FlexibleKalmanFilter.h"
#include "FlexKalman/FlexibleUnscentedCorrect.h"
#include "FlexKalman/PoseStateExponentialMap.h"

#include <catch2/catch.hpp>

#include "ContentsInvalid.h"

using State = flexkalman::pose_exp_map::State;
using flexkalman::AbsoluteOrientationMeasurement;
using flexkalman::AbsolutePositionMeasurement;

static void dumpState(State const &state, const char msg[], size_t iteration) {
    std::cout << "\n"
              << msg << " (iteration " << iteration << "):\n"
              << state.stateVector().transpose() << std::endl;
}

template <typename PM, typename M>
static void runFilterAndCheck(State &state,
                              flexkalman::ProcessModelBase<PM> &processModel,
                              flexkalman::MeasurementBase<M> &meas, double dt,
                              size_t iteration) {
    INFO("Iteration " << iteration);
    INFO("prediction step");
    flexkalman::predict(state, processModel, dt);
    dumpState(state, "After prediction", iteration);
    REQUIRE_FALSE(stateContentsInvalid(state));
    REQUIRE_FALSE(covarianceContentsInvalid(state));

    REQUIRE_FALSE(
        covarianceContentsInvalid(meas.derived().getCovariance(state)));

    INFO("correction step");
    flexkalman::correctUnscented(state, meas.derived());
    dumpState(state, "After correction", iteration);
    REQUIRE_FALSE(stateContentsInvalid(state));
    REQUIRE_FALSE(covarianceContentsInvalid(state));
}

TEMPLATE_TEST_CASE("ProcessModelStability", "",
                   flexkalman::pose_exp_map::ConstantVelocityProcessModel) {

    using ProcessModel = TestType;
    State state;
    ProcessModel processModel;
    std::size_t iteration = 0;

    dumpState(state, "Initial state", iteration);

    SECTION("IdentityAbsoluteOrientationMeasurement") {
        auto meas = AbsoluteOrientationMeasurement{
            Eigen::Quaterniond::Identity(),
            Eigen::Vector3d(0.00001, 0.00001, 0.00001)};
        for (iteration = 0; iteration < 100; ++iteration) {
            runFilterAndCheck(state, processModel, meas, 0.1, iteration);
        }
        // Can't use isApprox to compare to zero vector
        CHECK(state.position().isMuchSmallerThan(0.001));
        /// @todo check that it's roughly identity
    }
    SECTION("IdentityAbsolutePositionMeasurement") {
        auto meas = AbsolutePositionMeasurement{
            Eigen::Vector3d::Zero(), Eigen::Vector3d::Constant(0.000007)};
        for (iteration = 0; iteration < 100; ++iteration) {
            runFilterAndCheck(state, processModel, meas, 0.1, iteration);
        }
        /// @todo check that it's roughly identity
    }
    SECTION("AbsolutePositionMeasurementXlate111") {
        auto meas = AbsolutePositionMeasurement{
            Eigen::Vector3d::Constant(1), Eigen::Vector3d::Constant(0.000007)};
        for (iteration = 0; iteration < 100; ++iteration) {
            runFilterAndCheck(state, processModel, meas, 0.1, iteration);
        }
        /// @todo check that it's roughly identity orientation, position of 1,
        /// 1, 1
    }

    SECTION("AbsoluteOrientationMeasurementConstantAngVel") {
        auto angleAxis = Eigen::AngleAxisd(0.001, Eigen::Vector3d::UnitX());
        for (iteration = 0; iteration < 100; ++iteration) {
            angleAxis.angle() += 0.001;
            auto meas = AbsoluteOrientationMeasurement{
                Eigen::Quaterniond(angleAxis),
                Eigen::Vector3d(0.00001, 0.00001, 0.00001)};
            runFilterAndCheck(state, processModel, meas, 0.1, iteration);
        }
        // Can't use isApprox to compare to zero vector
        CHECK(state.position().isMuchSmallerThan(0.001));

        INFO("Should almost reach the most recent measurement");
        CHECK(state.rotationVector()[0] ==
              Approx(angleAxis.angle()).epsilon(0.1));
        CHECK(state.rotationVector()[1] == Approx(0).margin(0.01));
        CHECK(state.rotationVector()[2] == Approx(0).margin(0.01));
        INFO("We shouldn't overshoot")
        CHECK(state.rotationVector().norm() < angleAxis.angle());
    }
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson::devroye::detail::int_mean.hpp                        	//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_INT_MEAN_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_INT_MEAN_HPP_ER_2010
#include <boost/range.hpp>
#include <boost/math/special_functions/modf.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/exponential_distribution.hpp>

#include <boost/random/poisson_ext/devroye/detail/math.hpp>
#include <boost/random/poisson_ext/devroye/detail/parameters.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace detail{

    // Samples from the poisson distribution with an integer mean
    //
    // Source : The computer generation of poisson random variables. L. Devroye,
    // Computing 26, Springer-Verlag, 1981
    //
    // Parameter     Description
    // Step4         A class that models the concept by the same name
    // Int           An integer type
    // T             A float type
    // P             An error handling policy
	//
    // Expression                 Requirement
    // int_mean<Step4,Int,T,P>    Public base of Step4
	//
    // Concept Step4:
    // int_mean<Step4,Int,T,P> is a public base, and
    // Expression    Result type   Side effect
    //  accept()     bool          if true, y is set to the value to be returned
    //
    // The numbers on the right are those of the lines of the Fortran listing
    // given in the reference above.
    template<typename Step4,typename Int,typename T,typename P>
    class int_mean : public devroye::detail::parameters<Int,T,P>
        {

		typedef devroye::detail::math<Int,T,P> ma_;
        typedef devroye::detail::parameters<Int,T,P> super1_;

		public:

        typedef T input_type;
        typedef Int result_type;

        input_type mean() const { return ma_::to_float(this->int_mean_); }
        void reset() { }

		int_mean():super1_(){}
        explicit int_mean(const result_type& m)
        	:super1_(m),int_mean_(m){
            	BOOST_ASSERT(this->int_mean_val()>=0);
            }

        int_mean (const int_mean& that)
        	:super1_(that),
            int_mean_(that.int_mean_),
			u_(that.u_),
        	x_(that.x_),
        	y_(that.y_),
        	v_(that.v_)
            {}

        int_mean& operator=(const int_mean& that){
        	if(that!=this){
				static_cast<super1_&>(*this) = that;
            	this->int_mean_ = (that.int_mean_);
				this->u_ = (that.u_);
        		this->x_ = (that.x_);
        		this->y_ = (that.y_);
        		this->v_ = (that.v_);
            }
			return (*this);
		}

  		template<class U>
  		result_type operator()(U& urng)
  		{
			result_type result =  this->step1(urng);
            BOOST_ASSERT(result >= 0);
            return result;
        }

            // --- I/O --- //

            template<class CharT, class Traits>
            friend std::basic_ostream<CharT,Traits>&
            operator<<(
                std::basic_ostream<CharT,Traits>& os,
                const int_mean& pd
            )
            {
                os 	<< "devroye("
            		<< pd.int_mean_val()
            		<< ','
            		<< pd.u()
            		<< ','
            		<< pd.x()
            		<< ','
            		<< pd.y()
            		<< ','
            		<< pd.v()
            		<< ')';
                return os;
            }

            template<class CharT, class Traits>
            friend std::basic_istream<CharT,Traits>&
            operator>>(
                std::basic_istream<CharT,Traits>& is,
                int_mean& pd
            )
            {
            	result_type new_mean;
                is	>> std::ws
            		>> new_mean
            		>> std::ws
                	>> std::ws 	// u
            		>> std::ws
                	>> std::ws	// x
            		>> std::ws
                	>> std::ws	// y
            		>> std::ws
                	>> std::ws	// v
            		>> std::ws;
				static_cast<Step4&>(pd) = Step4(new_mean);
            	return is;
            }

        // ----------- //

        protected:

		template<typename U>
		inline result_type step1(U& urng){
        	// TODO max recursion policy

            // line 10 : case mean = 0;

            this->u_ = urng();												//13
            if(this->u()>this->p1()){
            	// Reconciliation with Fortran listing :
                // ptail = p2 + p3 = 1-p1.
                // u' ~ 1-u. (u'<ptail) <=> (1-u<p2+p3) <=> (u > p1)
            	return this->step3(urng);
            }else{
                return this->step2(urng);
            }
		}

		template<typename U>
		inline result_type step2(U& urng){
        	input_type z = this->random_z(urng); 							//15
        	this->x_ = ( this->sd1() * z ) + super1_::loc1();
            if(
            	(this->x()>this->delta())
                	||  (this->x()< (-this->m1()))
            ){
            	return this->step1(urng);
            }else{
            	// y is rounded away from zero
                if(ma_::is_strictly_negative(this->x())){
                    this->y_ = ma_::floor(this->x());
                }else{
                    this->y_ = ma_::ceil(this->x());
                }
                input_type e  = this->random_exp1(urng);
                this->v_ = -( e + ma_::pow(z,2)/ma_::to_float(2) ) + this->c1();
                return this->step4(urng);
            }
        }

		template<typename U>
		inline result_type step3(U& urng){

            // Reconciliation with Fortran listing :
            // pbody = p3 = 1-(p1+p2)
            // u' ~ 1-u. (u'<pbody) <=> (1-u<p3) <=> (u > p1+p2)
        	if(this->u()>this->p1()+this->p2()){
            	this->y_ = this->int_mean_val();
                return this->y();
            }else{
            	input_type e1 = this->random_exp1(urng);
            	input_type e2 = this->random_exp1(urng);
            	this->x_ = this->delta() + e1 / this->shape2();				//51
            	this->y_ = ma_::to_int( ma_::ceil(this->x()) );
            	this->v_ = - (
                	e2 + this->shape2() * (ma_::to_float(1) + this->x())
                );
            	return this->step4(urng);
            }
        }

		template<typename U>
		inline result_type step4(U& urng){
            Step4& derived = static_cast<Step4&>(*this);
            if(derived.accept()){
            	return this->y();
            }else{
				return this->step1(urng);
            }
		}

		template<typename U>
		input_type random_z(U& urng)const{
        	typedef boost::normal_distribution<input_type> d_;
            static d_ d = d_(ma_::to_float(0),ma_::to_float(1));
            return d(urng);
        }

		template<typename U>
		input_type random_exp1(U& urng)const{
        	typedef boost::exponential_distribution<input_type> d_;
            static d_ d = d_(ma_::to_float(1));
            return d(urng);
        }

		// Parameter
		result_type int_mean_;

        // These are quantities that vary throughout sampling
		input_type	u_;
        input_type 	x_;
        result_type y_;
        input_type 	v_;

        const result_type& 	int_mean_val()const{ return this->int_mean_; }
        const input_type& 	u()const{ return this->u_; }
        const input_type& 	x()const{ return this->x_; }
        const result_type& 	y()const{ return this->y_; }
        const input_type& 	v()const{ return this->v_; }

	};

}// detail
}// devroye
}// poisson
}// random
}// boost

#endif


#include <vector>

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/flow_transformer_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe {

template <typename Dtype>
void FlowTransformerLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {

	string prefix = "\t\tFlow Transformer Layer:: LayerSetUp: \t";

	std::cout<<prefix<<"Getting output_H_ and output_W_"<<std::endl;
	output_H_ = bottom[0]->shape(2);
	output_W_ = bottom[0]->shape(3);
	std::cout<<prefix<<"output_H_ = "<<output_H_<<", output_W_ = "<<output_W_<<std::endl;

	std::cout<<prefix<<"Getting pre-defined parameters"<<std::endl;

	// check the validation for the parameter theta
	CHECK(bottom[1]->shape(2) == bottom[0]->shape(2)) << "The third dimension (height) of the flow field and " <<
			"U should be the same" << std::endl;
	CHECK(bottom[1]->shape(3) == bottom[0]->shape(3)) << "The forth dimension (width) of the flow field and " <<
			"U should be the same" << std::endl;

	// initialize the matrix for output grid
	std::cout<<prefix<<"Initializing the matrix for output grid"<<std::endl;

	//2 channel image, 1 channel each for U and V of flow field
	vector<int> shape_output(4);
	shape_output[0] = bottom[0]->shape(0);
	shape_output[1] = 2;
	shape_output[2] = output_H_;
	shape_output[3] = output_W_;
	output_grid.Reshape(shape_output);

	Dtype* data = output_grid.mutable_cpu_data();
	for(int n=0;n<bottom[0]->shape(0);n++){
	for(int r=0;r<output_H_;r++){
		for(int c=0;c<output_W_;c++){
			data[output_grid.offset(n,0,r,c)] = c;
			data[output_grid.offset(n,1,r,c)] = r;
		}
	}
	}

	// initialize the matrix for input grid
	std::cout<<prefix<<"Initializing the matrix for input grid"<<std::endl;

	vector<int> shape_input(4);
	shape_input[0] = bottom[0]->shape(0);
	shape_input[1] = 2;
	shape_input[2] = output_H_;
	shape_input[3] = output_W_;
	input_grid.Reshape(shape_input);

	std::cout<<prefix<<"Initialization finished."<<std::endl;
}

template <typename Dtype>
void FlowTransformerLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
	string prefix = "\t\tFlow Transformer Layer:: Reshape: \t";

	if(global_debug) std::cout<<prefix<<"Starting!"<<std::endl;

	N = bottom[0]->shape(0);
	C = bottom[0]->shape(1);
	H = bottom[0]->shape(2);
	W = bottom[0]->shape(3);

	// reshape V
	vector<int> shape(4);

	shape[0] = N;
	shape[1] = C;
	shape[2] = output_H_;
	shape[3] = output_W_;

	top[0]->Reshape(shape);

	// reshape dTheta_tmp
	vector<int> dTheta_tmp_shape(4);

	dTheta_tmp_shape[0] = N;
	dTheta_tmp_shape[1] = 2;
	dTheta_tmp_shape[2] = 3;
	dTheta_tmp_shape[3] = output_H_ * output_W_ * C;

	dTheta_tmp.Reshape(dTheta_tmp_shape);

	// init all_ones_2
	vector<int> all_ones_2_shape(1);
	all_ones_2_shape[0] = output_H_ * output_W_ * C;
	all_ones_2.Reshape(all_ones_2_shape);

	// reshape full_theta
	vector<int> full_theta_shape(2);
	full_theta_shape[0] = N;
	full_theta_shape[1] = 6;
	full_theta.Reshape(full_theta_shape);

	if(global_debug) std::cout<<prefix<<"Finished."<<std::endl;
}

template <typename Dtype>
Dtype FlowTransformerLayer<Dtype>::transform_forward_cpu(const Dtype* pic, Dtype px, Dtype py) {
	/*
	so i think this does the bilinear interpolation for a single pixel
	*/

	bool debug = false;

	string prefix = "\t\tFlow Transformer Layer:: transform_forward_cpu: \t";

	if(debug) std::cout<<prefix<<"Starting!\t"<<std::endl;
	if(debug) std::cout<<prefix<<"(px, py) = ("<<px<<", "<<py<<")"<<std::endl;

	Dtype res = (Dtype)0.;

	Dtype x = px; Dtype y = py;

	if(debug) std::cout<<prefix<<"(x, y) = ("<<x<<", "<<y<<")"<<std::endl;

	int m, n; Dtype w;

	m = floor(x); n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"1: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	m = floor(x) + 1; n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"2: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	m = floor(x); n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"3: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	m = floor(x) + 1; n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"4: (m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));
		res += w * pic[m * W + n];
		if(debug) std::cout<<prefix<<"w = "<<w<<", pic[m, n] = "<<pic[m * W + n]<<std::endl;
	}

	if(debug) std::cout<<prefix<<"Finished. \tres = "<<res<<std::endl;

	return res;
}

template <typename Dtype>
void FlowTransformerLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
    const vector<Blob<Dtype>*>& top) {

	string prefix = "\t\tFlow Transformer Layer:: Forward_cpu: \t";

	/*
	CHECK(false) << "Don't use the CPU implementation! If you really want to, delete the" <<
			" CHECK in st_layer.cpp file. Line number: 240-241." << std::endl;
	*/

	if(global_debug) std::cout<<prefix<<"Starting!"<<std::endl;

	const Dtype* U = bottom[0]->cpu_data();
	const Dtype* flows = bottom[1]->cpu_data();

	const Dtype* output_grid_data = output_grid.cpu_data();

	Dtype* input_grid_data = input_grid.mutable_cpu_data();
	Dtype* V = top[0]->mutable_cpu_data();

	caffe_set(input_grid.count(), (Dtype)0, input_grid_data);
	caffe_set(top[0]->count(), (Dtype)0, V);

	//combine flow with grid
	caffe_add(input_grid.count(),output_grid_data,flows,input_grid_data);

	// for each input
	for(int i = 0; i < N; ++i) {
		//coordinates is a matrix of coordinates on the original image
		//to sample pixel values from
		//Dtype* coordinates = input_grid_data + (output_H_ * output_W_ * 2) * i;
		//Dtype* coordinates = input_grid_data + input_grid.offset(i,0,0,0);

		//do affine transformation
		//caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasTrans, output_H_ * output_W_, 2, 3, (Dtype)1.,
		//      output_grid_data, theta + 6 * i, (Dtype)0., coordinates);

		Dtype px, py;

		//for each pixel on the output, find the interpolated pixel value
		for(int s = 0; s < output_H_; ++s){
			for(int t = 0; t < output_W_; ++t) {
				//px,py should be the sample coordinates on the source img

				py = input_grid_data[input_grid.offset(i,0,s,t)];
				px = input_grid_data[input_grid.offset(i,1,s,t)];

				for(int j = 0; j < C; ++j){
					//do interpolation
					V[top[0]->offset(i, j, s, t)] = transform_forward_cpu(
							U + bottom[0]->offset(i, j, 0, 0), px, py);
				}
			}
		}
	}

	if(global_debug) std::cout<<prefix<<"Finished."<<std::endl;
}

template <typename Dtype>
void FlowTransformerLayer<Dtype>::transform_backward_cpu(Dtype dV, const Dtype* U, const Dtype px,
		const Dtype py, Dtype* dU, Dtype& dpx, Dtype& dpy) {
	/*
	U is the input image
	V is out output image, dV is local gradient from output
	*/

	bool debug = false;

	string prefix = "\t\tFlow Transformer Layer:: transform_backward_cpu: \t";

	if(debug) std::cout<<prefix<<"Starting!"<<std::endl;

	//Dtype x = (px + 1) / 2 * H; Dtype y = (py + 1) / 2 * W;
	Dtype x = px; Dtype y = py;
	if(debug) std::cout<<prefix<<"(x, y) = ("<<x<<", "<<y<<")"<<std::endl;

	int m, n; Dtype w;

	m = floor(x); n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		if(to_compute_dU_) dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	m = floor(x) + 1; n = floor(y); w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		if(to_compute_dU_) dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	m = floor(x); n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		if(to_compute_dU_) dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	m = floor(x) + 1; n = floor(y) + 1; w = 0;
	if(debug) std::cout<<prefix<<"(m, n) = ("<<m<<", "<<n<<")"<<std::endl;

	if(m >= 0 && m < H && n >= 0 && n < W) {
		w = max(0, 1 - abs(x - m)) * max(0, 1 - abs(y - n));

		if(to_compute_dU_) dU[m * W + n] += w * dV;

		if(abs(x - m) < 1) {
			if(m >= x) {
				dpx += max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx += "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			} else {
				dpx -= max(0, 1 - abs(y - n)) * U[m * W + n] * dV;// * H / 2;
				if(debug) std::cout<<prefix<<"dpx -= "<<max(0, 1 - abs(y - n))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<H / 2<<std::endl;
			}
		}

		if(abs(y - n) < 1) {
			if(n >= y) {
				dpy += max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy += "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			} else {
				dpy -= max(0, 1 - abs(x - m)) * U[m * W + n] * dV;// * W / 2;
				if(debug) std::cout<<prefix<<"dpy -= "<<max(0, 1 - abs(x - m))<<" * "<<U[m * W + n]<<" * "<<dV<<" * "<<W / 2<<std::endl;
			}
		}
	}

	if(debug) std::cout<<prefix<<"Finished."<<std::endl;
}

template <typename Dtype>
void FlowTransformerLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
    const vector<bool>& propagate_down,
    const vector<Blob<Dtype>*>& bottom) {

		string prefix = "\t\tFlow Transformer Layer:: Backward_cpu: \t";

		//CHECK(false) << "Don't use the CPU implementation! If you really want to, delete the" <<
		//		" CHECK in st_layer.cpp file. Line number: 420-421." << std::endl;

		if(global_debug) std::cout<<prefix<<"Starting!"<<std::endl;

		const Dtype* dV = top[0]->cpu_diff();
		Dtype* input_grid_data = input_grid.mutable_cpu_data();
		//const Dtype* output_grid_data = output_grid.cpu_data(); // remove if you don't need to recalc the input grid
		const Dtype* U = bottom[0]->cpu_data();

		Dtype* dU = bottom[0]->mutable_cpu_diff(); //we won't be setting this
		//Dtype* dTheta = bottom[1]->mutable_cpu_diff();
		//const Dtype* flows = bottom[1]->cpu_data(); // remove if you don't need to recalc the input grid
		Dtype* flow_diff = bottom[1]->mutable_cpu_diff();

		caffe_set(bottom[0]->count(), (Dtype)0, dU);
		//caffe_set(bottom[1]->count(), (Dtype)0, dTheta);
		caffe_set(input_grid.count(), (Dtype)0, flow_diff);

		//calc input grid data, recalc incase blob is reset between forward and backward
		//caffe_add(input_grid.count(),output_grid_data,flows,input_grid_data);

		for(int i = 0; i < N; ++i) {

			//const Dtype* coordinates = input_grid_data + (output_H_ * output_W_ * 2) * i;
			//Dtype* coordinates_diff = input_grid_diff + (output_H_ * output_W_ * 2) * i;

			const Dtype* curInputGrid = &input_grid_data[input_grid.offset(i,0,0,0)];
			//since its just element wise addition, we are
			//just going to set the diff directly to the flow
			Dtype* curFlowDiff = &flow_diff[bottom[1]->offset(i,0,0,0)];

			//int row_idx;
			Dtype px, py, delta_dpx, delta_dpy;

			for(int s = 0; s < output_H_; ++s)
				for(int t = 0; t < output_W_; ++t) {

					//row_idx = output_W_ * s + t;	//this means fix!

					px = curInputGrid[input_grid.offset(0,1,s,t)];
					py = curInputGrid[input_grid.offset(0,0,s,t)];

					for(int j = 0; j < C; ++j) {

						delta_dpx = delta_dpy = (Dtype)0.;

						transform_backward_cpu(dV[top[0]->offset(i, j, s, t)], U + bottom[0]->offset(i, j, 0, 0),
								px, py, dU + bottom[0]->offset(i, j, 0, 0), delta_dpx, delta_dpy);

						curFlowDiff[bottom[1]->offset(0,1,s,t)] += delta_dpx;
						curFlowDiff[bottom[1]->offset(0,0,s,t)] += delta_dpy;
					}

					/*
					dpx = curFlowDiff[row_idx * 2];
					dpy = curFlowDiff[row_idx * 2 + 1];

					dTheta[6 * i] += dpx * (s * 1.0 / output_H_ * 2 - 1);
					dTheta[6 * i + 1] += dpx * (t * 1.0 / output_W_ * 2 - 1);
					dTheta[6 * i + 2] += dpx;
					dTheta[6 * i + 3] += dpy * (s * 1.0 / output_H_ * 2 - 1);
					dTheta[6 * i + 4] += dpy * (t * 1.0 / output_W_ * 2 - 1);
					dTheta[6 * i + 5] += dpy;
					*/
				}
		}

		if(global_debug) std::cout<<prefix<<"Finished."<<std::endl;
}

#ifdef CPU_ONLY
STUB_GPU(FlowTransformerLayer);
#endif

INSTANTIATE_CLASS(FlowTransformerLayer);
REGISTER_LAYER_CLASS(FlowTransformer);

}  // namespace caffe


#include "./../../header/graphcomponent.h"
//STL
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iterator>
//Boost

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list_io.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/copy.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

void GraphComponent::read_graph_file(std::string filename, theGraph &g)
{
    std::ifstream infile(filename);
    v_p id = get(&VertexProperty::index, g);
    typedef graph_traits<theGraph>::vertex_descriptor Vertex;
    typedef graph_traits<theGraph>::vertices_size_type size_type;
    size_type n_vertices;
    infile >> n_vertices; // read in number of vertices
    std::vector<Vertex_t> vertex_set(n_vertices);
    for (size_type i = 0; i < n_vertices; ++i)
        vertex_set[i] = add_vertex(g);

    size_type u, v;
    while (infile >> u)
        if (infile >> v)
            add_edge(vertex_set[u], vertex_set[v], EdgeProperty(""), g);
        else
            break;
    //initialize the nums for the DFS
    boost::graph_traits<theGraph>::vertex_iterator vi, viend;
    int vnum = 0;
    for (boost::tie(vi, viend) = vertices(g); vi != viend; ++vi)
        id[*vi] = vnum++;
}

void GraphComponent::print_graph_file(theGraph &graph)
{
    v_p id = get(&VertexProperty::index, graph);

    property_map<theGraph, std::string EdgeProperty::*>::type
        name = get(&EdgeProperty::name, graph);
    graph_traits<theGraph>::vertex_iterator i, end;
    graph_traits<theGraph>::out_edge_iterator ei, edge_end;
    for (boost::tie(i, end) = vertices(graph); i != end; ++i)
    {
        std::cout << id[*i] + 1 << " ";
        for (boost::tie(ei, edge_end) = out_edges(*i, graph); ei != edge_end; ++ei)
            std::cout << " -" << name[*ei] << "-> " << id[target(*ei, graph)] + 1 << "  ";
        std::cout << std::endl;
    }
    // print_edges(theGraph, id);
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/SVD>

template<typename MatrixType> void upperbidiag(const MatrixType& m)
{
  const typename MatrixType::Index rows = m.rows();
  const typename MatrixType::Index cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef Matrix<typename MatrixType::RealScalar, MatrixType::RowsAtCompileTime,  MatrixType::ColsAtCompileTime> RealMatrixType;

  MatrixType a = MatrixType::Random(rows,cols);
  internal::UpperBidiagonalization<MatrixType> ubd(a);
  RealMatrixType b(rows, cols);
  b.setZero();
  b.block(0,0,cols,cols) = ubd.bidiagonal();
  MatrixType c = ubd.householderU() * b * ubd.householderV().adjoint();
  VERIFY_IS_APPROX(a,c);
}

void test_upperbidiagonalization()
{
  for(int i = 0; i < g_repeat; i++) {
   CALL_SUBTEST_1( upperbidiag(MatrixXf(3,3)) );
   CALL_SUBTEST_2( upperbidiag(MatrixXd(17,12)) );
   CALL_SUBTEST_3( upperbidiag(MatrixXcf(20,20)) );
   CALL_SUBTEST_4( upperbidiag(MatrixXcd(16,15)) );
   CALL_SUBTEST_5( upperbidiag(Matrix<float,6,4>()) );
   CALL_SUBTEST_6( upperbidiag(Matrix<float,5,5>()) );
   CALL_SUBTEST_7( upperbidiag(Matrix<double,4,3>()) );
  }
}


#define BOOST_TEST_MODULE "lex_integer_test"
#include <toml/lexer.hpp>
#include <boost/test/included/unit_test.hpp>
#include <iostream>
#include <iomanip>
#include "lex_aux.hpp"

using namespace toml;
using namespace detail;

BOOST_AUTO_TEST_CASE(test_decimal_correct)
{
    TOML_LEX_CHECK_ACCEPT(lex_integer, "1234",        "1234"       );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "+1234",       "+1234"      );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "-1234",       "-1234"      );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0",           "0"          );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "1_2_3_4",     "1_2_3_4"    );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "+1_2_3_4",    "+1_2_3_4"   );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "-1_2_3_4",    "-1_2_3_4"   );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "123_456_789", "123_456_789");
}

BOOST_AUTO_TEST_CASE(test_decimal_invalid)
{
    TOML_LEX_CHECK_ACCEPT(lex_integer, "123+45",  "123");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "123-45",  "123");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "01234",   "0");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "123__45", "123");

    TOML_LEX_CHECK_REJECT(lex_integer, "_1234");
}

BOOST_AUTO_TEST_CASE(test_hex_correct)
{
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xDEADBEEF",  "0xDEADBEEF" );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xdeadbeef",  "0xdeadbeef" );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xDEADbeef",  "0xDEADbeef" );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xDEAD_BEEF", "0xDEAD_BEEF");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xdead_beef", "0xdead_beef");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xdead_BEEF", "0xdead_BEEF");

    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xFF",     "0xFF"    );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0x00FF",   "0x00FF"  );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0x0000FF", "0x0000FF");
}

BOOST_AUTO_TEST_CASE(test_hex_invalid)
{
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xAPPLE",     "0xA");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xDEAD+BEEF", "0xDEAD");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0xDEAD__BEEF", "0xDEAD");

    TOML_LEX_CHECK_REJECT(lex_hex_int, "0x_DEADBEEF");
    TOML_LEX_CHECK_REJECT(lex_hex_int, "0x+DEADBEEF");
    TOML_LEX_CHECK_REJECT(lex_hex_int, "-0xFF"      );
    TOML_LEX_CHECK_REJECT(lex_hex_int, "-0x00FF"    );

    TOML_LEX_CHECK_ACCEPT(lex_integer, "0x_DEADBEEF", "0" );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0x+DEADBEEF", "0" );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "-0xFF"      , "-0" );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "-0x00FF"    , "-0" );
}

BOOST_AUTO_TEST_CASE(test_oct_correct)
{
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o777",    "0o777"  );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o7_7_7",  "0o7_7_7");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o007",    "0o007"  );

}

BOOST_AUTO_TEST_CASE(test_oct_invalid)
{
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o77+7", "0o77");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o1__0", "0o1");

    TOML_LEX_CHECK_REJECT(lex_oct_int, "0o800" );
    TOML_LEX_CHECK_REJECT(lex_oct_int, "-0o777");
    TOML_LEX_CHECK_REJECT(lex_oct_int, "0o+777");
    TOML_LEX_CHECK_REJECT(lex_oct_int, "0o_10" );

    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o800",  "0");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "-0o777", "-0");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o+777", "0");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0o_10",  "0");
}

BOOST_AUTO_TEST_CASE(test_bin_correct)
{
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0b10000",    "0b10000"   );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0b010000",   "0b010000"  );
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0b01_00_00", "0b01_00_00");
    TOML_LEX_CHECK_ACCEPT(lex_integer, "0b111111",   "0b111111"  );
}

BOOST_AUTO_TEST_CASE(test_bin_invalid)
{
    TOML_LEX_CHECK_ACCEPT(lex_bin_int, "0b11__11", "0b11");
    TOML_LEX_CHECK_ACCEPT(lex_bin_int, "0b11+11" , "0b11");

    TOML_LEX_CHECK_REJECT(lex_bin_int, "-0b10000");
    TOML_LEX_CHECK_REJECT(lex_bin_int, "0b_1111" );
}


#include <fstream>
#include <assert.h>
#include <stdlib.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graph_traits.hpp>

void printUsageAndExit()
{
  printf("%s", "Usage:./rmatg x\n");
  printf("%s", "x is the size of the graph, x>32 (Boost generator hang if x<32)\n");
  exit(0);
}

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

  // RMAT paper http://snap.stanford.edu/class/cs224w-readings/chakrabarti04rmat.pdf
  // Boost doc on RMAT http://www.boost.org/doc/libs/1_49_0/libs/graph_parallel/doc/html/rmat_generator.html

  typedef boost::adjacency_list<boost::mapS, boost::vecS, boost::directedS> Graph;
  typedef boost::unique_rmat_iterator<boost::minstd_rand, Graph> RMATGen;

  if (argc < 2) printUsageAndExit();
  int size = atoi (argv[1]);
  if (size<32) printUsageAndExit();
  assert (size > 31 && size < INT_MAX);
  const unsigned num_edges = 16 * size;
  /************************
   * RMAT Gen
   ************************/
  std::cout << "generating ... "<<'\n';
  // values of a,b,c,d are from the graph500.
  boost::minstd_rand gen;
  Graph g(RMATGen(gen, size, num_edges, 0.57, 0.19, 0.19, 0.05, true), RMATGen(), size);
  assert (num_edges == boost::num_edges(g));

  /************************
   * Print
   ************************/
  boost::graph_traits<Graph>::edge_iterator edge, edge_end;
  std::cout << "vertices : "      << boost::num_vertices(g) <<'\n';
  std::cout << "edges : "         << boost::num_edges(g) <<'\n';
  std::cout << "average degree : "<< static_cast<float>(boost::num_edges(g))/boost::num_vertices(g)<< '\n';

  // Print in matrix coordinate real general format
  std::cout << "writing ... "<<'\n';
  std::stringstream tmp;
  tmp <<"local_test_data/rmat_graph_" << size << ".mtx";
  const std::string filename = tmp.str();
  std::ofstream fout(tmp.str().c_str()) ;
  if (argv[2]==NULL)
  {
    // Power law out degree with random weights
    fout << "%%MatrixMarket matrix coordinate real general\n";
    fout << boost::num_vertices(g) <<' '<< boost::num_vertices(g)  <<' '<< boost::num_edges(g) << '\n';
    float val;
    for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
    {
      val = (rand()%10)+(rand()%100)*(1e-2f);
      fout << boost::source(*edge, g) << ' ' << boost::target(*edge, g)<< ' ' << val << '\n';
    }
  }
  else if (argv[2][0]=='i')
  {
    // Power law in degree (ie the transpose will have a power law)
    // -- Edges only --
    // * Wraning * edges will be unsorted, use sort_edges.cpp to sort the dataset.
    fout << boost::num_vertices(g) <<' '<< boost::num_edges(g) << '\n';
    for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
      fout <<boost::target(*edge, g)<< ' ' << boost::source(*edge, g) << '\n';
  }
  else if (argv[2][0]=='o')
  {
    // Power law out degree
    // -- Edges only --
    fout << boost::num_vertices(g) <<' '<< boost::num_edges(g) << '\n';
    for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
      fout << boost::source(*edge, g) << ' ' << boost::target(*edge, g)<< '\n';
  }
  else printUsageAndExit();
  fout.close();
  std::cout << "done"<<'\n';
  return 0;
}


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file k_cut_long_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-08-20
 */

#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/greedy/k_cut/k_cut.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/test/unit_test.hpp>

const int nu_vertices = 500;
const int nu_edges = 1000*100;
const int seed = 43;
const int parts = 20;
const int max_edge_weight_in_components = 10000;
const int max_edge_weight_between_components = 10;
BOOST_AUTO_TEST_CASE(KCut) {
    LOGLN("wertices: " << nu_vertices << " edges: " << nu_edges);
    LOGLN("parts: " << parts);
    //generate graph
    std::vector<std::pair<int, int>> edges_p;
    std::vector<long long> cost_edges;
    std::srand(seed);
    long long cost_cut_oncomponents = 0;
    {
        int source, target, edge_cost, nu_edges_copy = nu_edges;
        while (--nu_edges_copy) {
            source = rand() % nu_vertices;
            target = rand() % nu_vertices;
            edges_p.push_back(std::make_pair(source, target));
            if (source % parts == target % parts) {
                edge_cost = (rand() % max_edge_weight_in_components);
            } else {
                edge_cost = (rand() % max_edge_weight_between_components);
                cost_cut_oncomponents += edge_cost;
            }
            cost_edges.push_back(edge_cost);
        }
    }
    boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                    boost::no_property,
                    boost::property < boost::edge_weight_t, int>
                    > graph(edges_p.begin(), edges_p.end(), cost_edges.begin(), nu_vertices);
    //solve
    std::vector<std::pair<int, int>> vertices_parts;
    long long cost_cut = paal::greedy::k_cut(graph, parts, back_inserter(vertices_parts));
    //print result
    LOGLN("cost cut: " << cost_cut);
    std::vector<int> vertices_to_parts;
    vertices_to_parts.resize(vertices_parts.size());
    for (auto i:vertices_parts) {
        LOG(i.first << "(" << i.second << "), ");
        vertices_to_parts[i.first] = i.second;
    }
    LOGLN("");
    //verificate result
    auto weight = get(boost::edge_weight, graph);
    long long cost_cut_verification = 0;
    auto all_edges = edges(graph);
    for (auto edge : boost::make_iterator_range(all_edges)) {
        if (vertices_to_parts[source(edge, graph)] != vertices_to_parts[target(edge, graph)])
            cost_cut_verification += weight(edge);
    }
    BOOST_CHECK_EQUAL(cost_cut, cost_cut_verification);
    LOGLN("Number of parts: " << parts);
    //estimate aproximation ratio
    check_result_compare_to_bound(cost_cut_verification, cost_cut_oncomponents,
                    2.0 - 2.0 / double(parts), paal::utils::less_equal(),
                    0LL, "cut cost on components: ");
}


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

#include <pch.hpp>

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/tools/test.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/special_functions/ulp.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/debug_adaptor.hpp>
#include <iostream>
#include <iomanip>

#ifdef BOOST_MSVC
#pragma warning(disable:4127)
#endif

template <class T>
bool is_normalized_value(const T& val)
{
   //
   // Returns false if value has guard digits that are non-zero
   //
   boost::intmax_t shift = std::numeric_limits<T>::digits - ilogb(val) - 1;
   T shifted = scalbn(val, shift);
   return floor(shifted) == shifted;
}

template <class T>
void test_value(const T& val, const char* name)
{
   using namespace boost::math;
   T upper = tools::max_value<T>();
   T lower = -upper;

   std::cout << "Testing type " << name << " with initial value " << val << std::endl;

   BOOST_CHECK_EQUAL(float_distance(float_next(val), val), -1);
   BOOST_CHECK(float_next(val) > val);
   BOOST_CHECK_EQUAL(float_distance(float_prior(val), val), 1);
   BOOST_CHECK(float_prior(val) < val);
   BOOST_CHECK_EQUAL(float_distance((boost::math::nextafter)(val, upper), val), -1);
   BOOST_CHECK((boost::math::nextafter)(val, upper) > val);
   BOOST_CHECK_EQUAL(float_distance((boost::math::nextafter)(val, lower), val), 1);
   BOOST_CHECK((boost::math::nextafter)(val, lower) < val);
   BOOST_CHECK_EQUAL(float_distance(float_next(float_next(val)), val), -2);
   BOOST_CHECK_EQUAL(float_distance(float_prior(float_prior(val)), val), 2);
   BOOST_CHECK_EQUAL(float_distance(float_prior(float_prior(val)), float_next(float_next(val))), 4);
   BOOST_CHECK_EQUAL(float_distance(float_prior(float_next(val)), val), 0);
   BOOST_CHECK_EQUAL(float_distance(float_next(float_prior(val)), val), 0);
   if (is_normalized_value(val))
   {
      BOOST_CHECK_EQUAL(float_prior(float_next(val)), val);
      BOOST_CHECK_EQUAL(float_next(float_prior(val)), val);
   }
   BOOST_CHECK_EQUAL(float_distance(float_advance(val, 4), val), -4);
   BOOST_CHECK_EQUAL(float_distance(float_advance(val, -4), val), 4);
   if(std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present))
   {
      BOOST_CHECK_EQUAL(float_distance(float_advance(float_next(float_next(val)), 4), float_next(float_next(val))), -4);
      BOOST_CHECK_EQUAL(float_distance(float_advance(float_next(float_next(val)), -4), float_next(float_next(val))), 4);
   }
   if (is_normalized_value(val))
   {
      if (val > 0)
      {
         T n = val + ulp(val);
         T fn = float_next(val);
         if (n > fn)
         {
            BOOST_CHECK_LE(ulp(val), boost::math::tools::min_value<T>());
         }
         else
         {
            BOOST_CHECK_EQUAL(fn, n);
         }
      }
      else if (val == 0)
      {
         BOOST_CHECK_GE(boost::math::tools::min_value<T>(), ulp(val));
      }
      else
      {
         T n = val - ulp(val);
         T fp = float_prior(val);
         if (n < fp)
         {
            BOOST_CHECK_LE(ulp(val), boost::math::tools::min_value<T>());
         }
         else
         {
            BOOST_CHECK_EQUAL(fp, n);
         }
      }
   }
}

template <class T>
void test_values(const T& val, const char* name)
{
   static const T a = boost::lexical_cast<T>("1.3456724e22");
   static const T b = boost::lexical_cast<T>("1.3456724e-22");
   static const T z = 0;
   static const T one = 1;
   static const T radix = std::numeric_limits<T>::radix;

   std::cout << "Testing type " << name << std::endl;

   T den = (std::numeric_limits<T>::min)() / 4;
   if(den != 0)
   {
      std::cout << "Denormals are active\n";
   }
   else
   {
      std::cout << "Denormals are flushed to zero.\n";
   }

   test_value(a, name);
   test_value(T(-a), name);
   test_value(b, name);
   test_value(T(-b), name);
   test_value(T(b / 3), name);
   test_value(T(-b / 3), name);
   test_value(boost::math::tools::epsilon<T>(), name);
   test_value(T(-boost::math::tools::epsilon<T>()), name);
   test_value(boost::math::tools::min_value<T>(), name);
   test_value(T(-boost::math::tools::min_value<T>()), name);
   if (std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present) && ((std::numeric_limits<T>::min)() / 2 != 0))
   {
      test_value(z, name);
      test_value(T(-z), name);
   }
   test_value(one, name);
   test_value(T(-one), name);
   test_value(radix, name);
   test_value(T(-radix), name);

   if(std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present) && ((std::numeric_limits<T>::min)() / 2 != 0))
   {
      test_value(std::numeric_limits<T>::denorm_min(), name);
      test_value(T(-std::numeric_limits<T>::denorm_min()), name);
      test_value(T(2 * std::numeric_limits<T>::denorm_min()), name);
      test_value(T(-2 * std::numeric_limits<T>::denorm_min()), name);
   }

   static const int primes[] = {
      11,     13,     17,     19,     23,     29,
      31,     37,     41,     43,     47,     53,     59,     61,     67,     71,
      73,     79,     83,     89,     97,    101,    103,    107,    109,    113,
      127,    131,    137,    139,    149,    151,    157,    163,    167,    173,
      179,    181,    191,    193,    197,    199,    211,    223,    227,    229,
      233,    239,    241,    251,    257,    263,    269,    271,    277,    281,
      283,    293,    307,    311,    313,    317,    331,    337,    347,    349,
      353,    359,    367,    373,    379,    383,    389,    397,    401,    409,
      419,    421,    431,    433,    439,    443,    449,    457,    461,    463,
   };

   for(unsigned i = 0; i < sizeof(primes)/sizeof(primes[0]); ++i)
   {
      T v1 = val;
      T v2 = val;
      for(int j = 0; j < primes[i]; ++j)
      {
         v1 = boost::math::float_next(v1);
         v2 = boost::math::float_prior(v2);
      }
      BOOST_CHECK_EQUAL(boost::math::float_distance(v1, val), -primes[i]);
      BOOST_CHECK_EQUAL(boost::math::float_distance(v2, val), primes[i]);
      BOOST_CHECK_EQUAL(boost::math::float_advance(val, primes[i]), v1);
      BOOST_CHECK_EQUAL(boost::math::float_advance(val, -primes[i]), v2);
   }
   if(std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_infinity))
   {
      BOOST_CHECK_EQUAL(boost::math::float_prior(std::numeric_limits<T>::infinity()), (std::numeric_limits<T>::max)());
      BOOST_CHECK_EQUAL(boost::math::float_next(-std::numeric_limits<T>::infinity()), -(std::numeric_limits<T>::max)());
      BOOST_MATH_CHECK_THROW(boost::math::float_prior(-std::numeric_limits<T>::infinity()), std::domain_error);
      BOOST_MATH_CHECK_THROW(boost::math::float_next(std::numeric_limits<T>::infinity()), std::domain_error);
      if(boost::math::policies:: BOOST_MATH_OVERFLOW_ERROR_POLICY == boost::math::policies::throw_on_error)
      {
         BOOST_MATH_CHECK_THROW(boost::math::float_prior(-(std::numeric_limits<T>::max)()), std::overflow_error);
         BOOST_MATH_CHECK_THROW(boost::math::float_next((std::numeric_limits<T>::max)()), std::overflow_error);
      }
      else
      {
         BOOST_CHECK_EQUAL(boost::math::float_prior(-(std::numeric_limits<T>::max)()), -std::numeric_limits<T>::infinity());
         BOOST_CHECK_EQUAL(boost::math::float_next((std::numeric_limits<T>::max)()), std::numeric_limits<T>::infinity());
      }
   }
}

BOOST_AUTO_TEST_CASE( test_main )
{
   // Very slow, but debuggable:
   //test_values(boost::multiprecision::number<boost::multiprecision::debug_adaptor<boost::multiprecision::cpp_dec_float_50::backend_type> >(0), "cpp_dec_float_50");

   // Faster, but no good for diagnising the cause of any issues:
   test_values(boost::multiprecision::cpp_dec_float_50(0), "cpp_dec_float_50");
}


//
// Created by dch on 13/03/17.
//

#include <cuNDArray_fileio.h>
#include <cuNDArray_math.h>

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

#include "mssim.h"

namespace po = boost::program_options;

using namespace Gadgetron;

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

    std::string image_file;
    std::string reference_file;
    float sigma;
    po::options_description desc("Allowed options");

    desc.add_options()
            ("help", "produce help message")
            ("image",po::value<std::string>(&image_file))
            ("gold",po::value<std::string>(&reference_file))
            ("sigma",po::value<float>(&sigma)->default_value(1.5f));

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

    auto image = read_nd_array<float>(image_file.c_str());
    auto gold = read_nd_array<float>(reference_file.c_str());

    auto dims = *image->get_dimensions();
    size_t phases = dims[3];

    std::vector<float> results;

    std::vector<size_t> dims3d(dims.begin(),dims.end()-1);

    size_t elements = std::accumulate(dims3d.begin(),dims3d.end(),1,std::multiplies<size_t>());

    for (size_t phase = 0; phase < phases; phase++){
        hoNDArray<float> image_view(dims3d,image->get_data_ptr()+phase*elements);
        hoNDArray<float> ref_view(dims3d,gold->get_data_ptr()+phase*elements);

        cuNDArray<float> cu_image(image_view);
        cuNDArray<float> cu_ref(ref_view);

        results.push_back(mssim(&cu_image,&cu_ref,floatd3(sigma,sigma,sigma)));
//
//        cu_image -= cu_ref;
//
//        results.push_back(nrm2(&cu_image));

    }

    float sum = std::accumulate(results.begin(),results.end(),0.0f)/results.size();

    std::cout << sum << std::endl;
//
//    for (auto r : results)
//        std::cout << r <<  " ";

}


/*
 * @Description: tf监听模块
 * @Author: Ren Qian
 * @Date: 2020-02-06 16:10:31
 */
#include "lidar_localization/tf_listener/tf_listener.hpp"

#include <Eigen/Geometry>

namespace lidar_localization {
TFListener::TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id)
    :nh_(nh), base_frame_id_(base_frame_id), child_frame_id_(child_frame_id) {
}

bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) {
    try {
        tf::StampedTransform transform;
        listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform);
        TransformToMatrix(transform, transform_matrix);
        return true;
    } catch (tf::TransformException &ex) {
        return false;
    }
}

bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {
    Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());

    double roll, pitch, yaw;
    tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
    Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());

    // 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();

    return true;
}
}


#ifndef CT_TYPES_EIGEN_HPP
#define CT_TYPES_EIGEN_HPP
#include "../reflect.hpp"
#include "TArrayView.hpp"
#include <Eigen/Core>
#include <array>

namespace ct
{
    template <class T, int ROWS, int COLS, int OPTS, int MAX_ROWS, int MAX_COLS>
    struct ReflectImpl<Eigen::Matrix<T, ROWS, COLS, OPTS, MAX_ROWS, MAX_COLS>, void>
    {
        using DataType = Eigen::Matrix<T, ROWS, COLS, OPTS, MAX_ROWS, MAX_COLS>;
        using this_t = ReflectImpl<DataType, void>;
        static constexpr StringView getTypeName() { return GetName<DataType>::getName(); }

        static std::array<Eigen::Index, 2> shape(const DataType& data) { return {data.rows(), data.cols()}; }

        static void reshape(DataType& data, const std::array<Eigen::Index, 2> shape)
        {
            data.resize(shape[0], shape[1]);
        }

        static TArrayView<const T> getData(const DataType& mat) { return {mat.data(), ROWS * COLS}; }

        static TArrayView<T> getDataMutable(DataType& mat) { return {mat.data(), ROWS * COLS}; }

        REFLECT_STUB
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, shape, &this_t::shape)
            PROPERTY(data, &this_t::getData, &this_t::getDataMutable)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, size)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, colStride)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, rowStride)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, cols)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, rows)
        REFLECT_INTERNAL_END;
        static constexpr Indexer<NUM_FIELDS - 1> end() { return Indexer<NUM_FIELDS - 1>(); }
    };

    template <class T, int OPTS, int MAX_ROWS, int MAX_COLS>
    struct ReflectImpl<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, OPTS, MAX_ROWS, MAX_COLS>, void>
    {
        using DataType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, OPTS, MAX_ROWS, MAX_COLS>;
        using this_t = ReflectImpl<DataType, void>;
        static constexpr StringView getTypeName() { return GetName<DataType>::getName(); }

        static std::array<Eigen::Index, 2> shape(const DataType& data) { return {data.rows(), data.cols()}; }

        static void reshape(DataType& data, const std::array<Eigen::Index, 2> shape)
        {
            data.resize(shape[0], shape[1]);
        }

        static TArrayView<const T> getData(const DataType& mat)
        {
            return {mat.data(), static_cast<size_t>(mat.rows() * mat.cols())};
        }

        static TArrayView<T> getDataMutable(DataType& mat)
        {
            return {mat.data(), static_cast<size_t>(mat.rows() * mat.cols())};
        }

        REFLECT_STUB
            PROPERTY(shape, &this_t::shape, &this_t::reshape)
            PROPERTY(data, &this_t::getData, &this_t::getDataMutable)
            PROPERTY(size)
            PROPERTY(colStride)
            PROPERTY(rowStride)
            PROPERTY(cols)
            PROPERTY(rows)
        REFLECT_INTERNAL_END;
        static constexpr Indexer<NUM_FIELDS - 1> end() { return ct::Indexer<NUM_FIELDS - 1>(); }
    };

    template <typename T, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
    struct ReflectImpl<Eigen::Array<T, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, void>
    {
        using DataType = Eigen::Array<T, _Rows, _Cols, _Options, _MaxRows, _MaxCols>;
        using this_t = ReflectImpl<DataType, void>;
        static constexpr StringView getTypeName() { return GetName<DataType>::getName(); }

        static TArrayView<const T> getData(const DataType& arr)
        {
            return {arr.data(), static_cast<size_t>(arr.rows() * arr.cols())};
        }

        static TArrayView<T> getDataMutable(DataType& arr)
        {
            return {arr.data(), static_cast<size_t>(arr.rows() * arr.cols())};
        }

        REFLECT_STUB
            PROPERTY(data, &this_t::getData, &this_t::getDataMutable)
        REFLECT_INTERNAL_END;
        static constexpr Indexer<NUM_FIELDS - 1> end() { return Indexer<NUM_FIELDS - 1>(); }
    };

    DECL_NAME(Eigen::MatrixXf);
    DECL_NAME(Eigen::Matrix2f);
    DECL_NAME(Eigen::Matrix3f);
    DECL_NAME(Eigen::Matrix4f);

    DECL_NAME(Eigen::MatrixXd);
    DECL_NAME(Eigen::Matrix2d);
    DECL_NAME(Eigen::Matrix3d);
    DECL_NAME(Eigen::Matrix4d);

} // namespace ct

#endif // CT_EIGEN_HPP


#include <blitz/array.h>
#include <blitz/funcs.h>

using namespace blitz;

int main()
{
    Array<int,1> A(4), B(4);
    Array<float,1> C(4);

    A = 1, 2, 3, 5;
    B = 2, 2, 2, 7;

    C = A / B;
    cout << C << endl;

#ifdef BZ_NEW_EXPRESSION_TEMPLATES
    C = A / cast<float>(B);
#else
    C = A / cast(B, float());
#endif
    cout << C << endl;

    return 0;
}


//
// Created by Amir Masoud Abdol on 2019-06-17.
//

#define BOOST_TEST_MODULE MetaAnalysis Test
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include <armadillo>
#include <iostream>
#include <fstream>

#include "ExperimentSetup.h"
#include "Experiment.h"
#include "Researcher.h"
#include "HackingStrategy.h"
#include "Journal.h"
#include "ResearchStrategy.h"
#include "PersistenceManager.h"

#include "sam.h"

#include "test_fixtures.h"

using namespace arma;
using namespace sam;
using namespace std;

using json = nlohmann::json;

bool FLAGS::VERBOSE = false;
bool FLAGS::PROGRESS = false;
bool FLAGS::DEBUG = false;
bool FLAGS::UPDATECONFIG = false;

BOOST_FIXTURE_TEST_SUITE( meta_analysis_strategy, SampleResearch )

    BOOST_AUTO_TEST_CASE( fixed_effect_model )
    {

        float r_estimate = 0.7766;

        nobs = 20;
        mean = 0.5;
        var = 2.;
        cov = 0.75;

        j_conf["max_pubs"] = 20;

        initResearch();

        runSampleSimulation();

        PersistenceManager::Writer pubwriter("/Users/amabdol/meta_pubs.csv");

        FixedEffectEstimator fixed_model;
        std::cout << "Fixed: " << std::endl;
        std::cout << fixed_model.estimate(publications[0]);

        auto fe = fixed_model.estimate(publications[0]);

        BOOST_CHECK_SMALL(fe[0] - r_estimate, 0.001);

        RandomEffectEstimator random_model;
        std::cout << "Random: " << std::endl;
        std::cout << random_model.estimate(publications[0]);

        pubwriter.write(publications[0]);

    }

BOOST_AUTO_TEST_SUITE_END()


#include <ros/ros.h>
#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <quadrotor_msgs/TRPYCommand.h>
#include <quadrotor_simulator/Quadrotor.h>

typedef struct _ControlInput
{
  double rpm[4];
} ControlInput;

typedef struct _Command
{
  float thrust;
  float roll, pitch, yaw;
  bool enable_motors;
  float kR[3];
  float kOm[3];
} Command;

static Command command;

void stateToOdomMsg(const QuadrotorSimulator::Quadrotor::State &state, nav_msgs::Odometry &odom);
void quadToImuMsg(const QuadrotorSimulator::Quadrotor &quad, sensor_msgs::Imu &imu);

static ControlInput getControl(const QuadrotorSimulator::Quadrotor &quad, const Command &cmd)
{
  const double _kf = quad.getPropellerThrustCoefficient();
  const double _km = quad.getPropellerMomentCoefficient();
  const double kf = _kf;
  const double km = _km/_kf*kf;

  const double d = quad.getArmLength();
  const Eigen::Matrix3f J = quad.getInertia().cast<float>();
  const float I[3][3] = {{J(0,0), J(0,1), J(0,2)},
                         {J(1,0), J(1,1), J(1,2)},
                         {J(2,0), J(2,1), J(2,2)}};
  const QuadrotorSimulator::Quadrotor::State state = quad.getState();

  float R11 = state.R(0,0);
  float R12 = state.R(0,1);
  float R13 = state.R(0,2);
  float R21 = state.R(1,0);
  float R22 = state.R(1,1);
  float R23 = state.R(1,2);
  float R31 = state.R(2,0);
  float R32 = state.R(2,1);
  float R33 = state.R(2,2);

  float Om1 = state.omega(0);
  float Om2 = state.omega(1);
  float Om3 = state.omega(2);

  float Rd11 = cos(cmd.yaw)*cos(cmd.pitch);
  float Rd12 = cos(cmd.yaw)*sin(cmd.pitch)*sin(cmd.roll) - cos(cmd.roll)*sin(cmd.yaw);
  float Rd13 = sin(cmd.yaw)*sin(cmd.roll) + cos(cmd.yaw)*cos(cmd.roll)*sin(cmd.pitch);
  float Rd21 = cos(cmd.pitch)*sin(cmd.yaw);
  float Rd22 = cos(cmd.yaw)*cos(cmd.roll) + sin(cmd.yaw)*sin(cmd.pitch)*sin(cmd.roll);
  float Rd23 = cos(cmd.roll)*sin(cmd.yaw)*sin(cmd.pitch) - cos(cmd.yaw)*sin(cmd.roll);
  float Rd31 = -sin(cmd.pitch);
  float Rd32 = cos(cmd.pitch)*sin(cmd.roll);
  float Rd33 = cos(cmd.pitch)*cos(cmd.roll);

  float Psi = 0.5f*(3.0f - (Rd11*R11 + Rd21*R21 + Rd31*R31 +
                            Rd12*R12 + Rd22*R22 + Rd32*R32 +
                            Rd13*R13 + Rd23*R23 + Rd33*R33));

  float force = 0;
  if(Psi < 1.0f) // Position control stability guaranteed only when Psi < 1
    force = cmd.thrust;

  float eR1 = 0.5f*(R12*Rd13 - R13*Rd12 + R22*Rd23 - R23*Rd22 + R32*Rd33 - R33*Rd32);
  float eR2 = 0.5f*(R13*Rd11 - R11*Rd13 - R21*Rd23 + R23*Rd21 - R31*Rd33 + R33*Rd31);
  float eR3 = 0.5f*(R11*Rd12 - R12*Rd11 + R21*Rd22 - R22*Rd21 + R31*Rd32 - R32*Rd31);

  float eOm1 = Om1;
  float eOm2 = Om2;
  float eOm3 = Om3;

  float in1 = Om2*(I[2][0]*Om1 + I[2][1]*Om2 + I[2][2]*Om3) - Om3*(I[1][0]*Om1 + I[1][1]*Om2 + I[1][2]*Om3);
  float in2 = Om3*(I[0][0]*Om1 + I[0][1]*Om2 + I[0][2]*Om3) - Om1*(I[2][0]*Om1 + I[2][1]*Om2 + I[2][2]*Om3);
  float in3 = Om1*(I[1][0]*Om1 + I[1][1]*Om2 + I[1][2]*Om3) - Om2*(I[0][0]*Om1 + I[0][1]*Om2 + I[0][2]*Om3);

  float M1 = -cmd.kR[0]*eR1 - cmd.kOm[0]*eOm1 + in1;
  float M2 = -cmd.kR[1]*eR2 - cmd.kOm[1]*eOm2 + in2;
  float M3 = -cmd.kR[2]*eR3 - cmd.kOm[2]*eOm3 + in3;

  float w_sq[4];
  w_sq[0] = force/(4*kf) - M2/(2*d*kf) + M3/(4*km);
  w_sq[1] = force/(4*kf) + M2/(2*d*kf) + M3/(4*km);
  w_sq[2] = force/(4*kf) + M1/(2*d*kf) - M3/(4*km);
  w_sq[3] = force/(4*kf) - M1/(2*d*kf) - M3/(4*km);

  ControlInput control;
  for(int i = 0; i < 4; i++)
  {
    if(cmd.enable_motors)
    {
      if(w_sq[i] < 0)
        w_sq[i] = 0;

      control.rpm[i] = sqrtf(w_sq[i]);
    }
    else
    {
      control.rpm[i] = 0;
    }
  }
  return control;
}

static void cmd_callback(const quadrotor_msgs::TRPYCommand::ConstPtr &cmd)
{
  command.thrust = cmd->thrust;
  command.roll = cmd->roll;
  command.pitch = cmd->pitch;
  command.yaw = cmd->yaw;
  command.enable_motors = cmd->aux.enable_motors;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "quadrotor_simulator_so3");

  ros::NodeHandle n("~");

  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 100);
  ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 100);
  ros::Subscriber cmd_sub = n.subscribe("cmd", 100, &cmd_callback, ros::TransportHints().tcpNoDelay());

  double simulation_rate;
  n.param("rate/simulation", simulation_rate, 1000.0);
  ROS_ASSERT(simulation_rate > 0);

  double odom_rate;
  n.param("rate/odom", odom_rate, 100.0);
  const ros::Duration odom_pub_duration(1/odom_rate);

  std::string quad_name;
  n.param("quadrotor_name", quad_name, std::string("quadrotor"));

  double kR[3], kOm[3];
  n.param("gains/rot/x", kR[0], 1.0);
  n.param("gains/rot/y", kR[1], 1.0);
  n.param("gains/rot/z", kR[2], 1.0);
  n.param("gains/ang/x", kOm[0], 0.13);
  n.param("gains/ang/y", kOm[1], 0.13);
  n.param("gains/ang/z", kOm[2], 0.1);
  command.kR[0] = kR[0], command.kR[1] = kR[1], command.kR[2] = kR[2];
  command.kOm[0] = kOm[0], command.kOm[1] = kOm[1], command.kOm[2] = kOm[2];

  QuadrotorSimulator::Quadrotor quad;
  QuadrotorSimulator::Quadrotor::State state = quad.getState();
  n.param("initial_pos/x", state.x(0), 0.0);
  n.param("initial_pos/y", state.x(1), 0.0);
  n.param("initial_pos/z", state.x(2), 0.0);
  quad.setState(state);

  ros::Rate r(simulation_rate);
  const double simulation_dt = 1/simulation_rate;

  command.enable_motors = false;

  ControlInput control;

  nav_msgs::Odometry odom_msg;
  sensor_msgs::Imu imu_msg;
  odom_msg.header.frame_id = "/simulator";
  odom_msg.child_frame_id = "/" + quad_name;
  imu_msg.header.frame_id = "/" + quad_name;

  ros::Time next_odom_pub_time = ros::Time::now();
  while(n.ok())
  {
    ros::spinOnce();

    control = getControl(quad, command);
    //ROS_INFO("command_thrust: %f, %f, %f, %f\nrpm: %f, %f, %f, %f", command.thrust, command.roll, command.pitch, command.yaw, control.rpm[0], control.rpm[1], control.rpm[2], control.rpm[3]);
    quad.setInput(control.rpm[0], control.rpm[1], control.rpm[2], control.rpm[3]);
    quad.step(simulation_dt);

    ros::Time tnow = ros::Time::now();

    if(tnow >= next_odom_pub_time)
    {
      next_odom_pub_time += odom_pub_duration;
      state = quad.getState();
      stateToOdomMsg(state, odom_msg);
      quadToImuMsg(quad, imu_msg);
      odom_msg.header.stamp = tnow;
      imu_msg.header.stamp = tnow;
      odom_pub.publish(odom_msg);
      imu_pub.publish(imu_msg);
    }

    r.sleep();
  }

  return 0;
}

void stateToOdomMsg(const QuadrotorSimulator::Quadrotor::State &state, nav_msgs::Odometry &odom)
{
  odom.pose.pose.position.x = state.x(0);
  odom.pose.pose.position.y = state.x(1);
  odom.pose.pose.position.z = state.x(2);

  Eigen::Quaterniond q(state.R);
  odom.pose.pose.orientation.x = q.x();
  odom.pose.pose.orientation.y = q.y();
  odom.pose.pose.orientation.z = q.z();
  odom.pose.pose.orientation.w = q.w();

  odom.twist.twist.linear.x = state.v(0);
  odom.twist.twist.linear.y = state.v(1);
  odom.twist.twist.linear.z = state.v(2);

  odom.twist.twist.angular.x = state.omega(0);
  odom.twist.twist.angular.y = state.omega(1);
  odom.twist.twist.angular.z = state.omega(2);
}

void quadToImuMsg(const QuadrotorSimulator::Quadrotor &quad, sensor_msgs::Imu &imu)
{
  const QuadrotorSimulator::Quadrotor::State state = quad.getState();
  Eigen::Quaterniond q(state.R);
  imu.orientation.x = q.x();
  imu.orientation.y = q.y();
  imu.orientation.z = q.z();
  imu.orientation.w = q.w();

  imu.angular_velocity.x = state.omega(0);
  imu.angular_velocity.y = state.omega(1);
  imu.angular_velocity.z = state.omega(2);

  const double kf = quad.getPropellerThrustCoefficient();
  const double m = quad.getMass();
  const Eigen::Vector3d &external_force = quad.getExternalForce();
  const double g = quad.getGravity();
  const double thrust = kf*state.motor_rpm.square().sum();
  Eigen::Vector3d acc;
  if(state.x(2) < 1e-4)
  {
    acc = state.R*(external_force/m + Eigen::Vector3d(0,0,g));
  }
  else
  {
    acc = thrust/m*Eigen::Vector3d(0,0,1) + state.R*external_force/m;
  }

  imu.linear_acceleration.x = acc(0);
  imu.linear_acceleration.y = acc(1);
  imu.linear_acceleration.z = acc(2);
}


#pragma once

#include <numeric>

#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/cumsum.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/function/sum.hpp>

#include "source.hpp"

typedef boost::simd::pack<double> pack_double;

template<typename tag>
double balanced_sum(source_1d<tag> &f)
{
    double res = 0.;
    while (f.can_procure(1, res) && !f.is_aligned()) {
        f.procure(res);
    }

    pack_double stack[62];
    size_t p = 0;
    for (size_t iteration = 0; f.can_procure(4, stack[0]); ++iteration) {
        pack_double v = boost::simd::Zero<pack_double>();
        f.procure(v);
        f.procure(v);
        pack_double w = boost::simd::Zero<pack_double>();
        f.procure(w);
        f.procure(w);
        v += w;
        size_t bitmask = 1;
        for (; iteration & bitmask; bitmask <<= 1, --p) {
            v += stack[p - 1];
        }
        stack[p++] = v;
    }
    pack_double vsum = boost::simd::Zero<pack_double>();
    for (size_t i = p; i > 0; --i) {
        vsum += stack[i - 1];
    }
    res = std::accumulate(vsum.begin(), vsum.end(), res);
    while (f.can_procure(1, res)) {
        f.procure(res);
    }
    return res;
}

template<typename tag>
double twin_balanced_sum(source_2d<tag>& f)
{
    double stat1 = 0.;
    double stat2 = 0.;
    while (f.can_procure(1, stat1) && !f.is_aligned()) {
        f.procure(stat1, stat2);
    }

    pack_double stack1[62];
    pack_double stack2[62];
    size_t p = 0;
    for (size_t iteration = 0; f.can_procure(4, stack1[0]); ++iteration) {
        pack_double v1 = boost::simd::Zero<pack_double>();
        pack_double v2 = boost::simd::Zero<pack_double>();
        f.procure(v1, v2);
        f.procure(v1, v2);
        pack_double w1 = boost::simd::Zero<pack_double>();
        pack_double w2 = boost::simd::Zero<pack_double>();
        f.procure(w1, w2);
        f.procure(w1, w2);
        v1 += w1;
        v2 += w2;
        size_t bitmask = 1;
        for (; iteration & bitmask; bitmask <<= 1, --p) {
            v1 += stack1[p - 1];
            v2 += stack2[p - 1];
        }
        stack1[p] = v1;
        stack2[p++] = v2;
    }
    pack_double vsum1 = boost::simd::Zero<pack_double>();
    pack_double vsum2 = boost::simd::Zero<pack_double>();
    for (size_t i = p; i > 0; --i) {
        vsum1 += stack1[i - 1];
        vsum2 += stack2[i - 1];
    }
    stat1 = std::accumulate(vsum1.begin(), vsum1.end(), stat1);
    stat2 = std::accumulate(vsum2.begin(), vsum2.end(), stat2);
    while (f.can_procure(1, stat1)) {
        f.procure(stat1, stat2);
    }
    return f.result(stat1, stat2);
}

inline void kahan_update(double &accumulator, double &compensator, double value)
{
    double const new_accumulator = accumulator + value;
    double const first_option = (accumulator - new_accumulator) + value;
    double const second_option = (value - new_accumulator) + accumulator;
    if (std::abs(accumulator) > std::abs(value)) {
        compensator += first_option;
    } else {
        compensator += second_option;
    }
    accumulator = new_accumulator;
}

template<typename tag>
void cum_sum(source_1d<tag> &f)
{
    double accumulator = 0.;
    double compensator = 0.;
    double value = 0.;
    while (f.can_procure(1, value) && !f.is_aligned()) {
        f.procure(value);
        kahan_update(accumulator, compensator, value);
        f.feed(accumulator + compensator);
    }
    pack_double pack_value1 = boost::simd::Zero<pack_double>();
    pack_double pack_value2 = boost::simd::Zero<pack_double>();
    while (f.can_procure(2, pack_value1)) {
        f.procure(pack_value1);
        f.procure(pack_value2);
        pack_double const pack_cum_sum1 = boost::simd::cumsum(pack_value1);
        pack_double const pack_cum_sum2 = boost::simd::cumsum(pack_value2);
        pack_double const pack_feed_slice1 = pack_cum_sum1 + (accumulator + compensator);
        f.feed(pack_feed_slice1);
        kahan_update(accumulator, compensator, pack_cum_sum1[pack_double::static_size - 1]);
        pack_double const pack_feed_slice2 = pack_cum_sum2 + (accumulator + compensator);
        f.feed(pack_feed_slice2);
        kahan_update(accumulator, compensator, pack_cum_sum2[pack_double::static_size - 1]);
    }
    value = 0.;
    while (f.can_procure(1, accumulator)) {
        f.procure(value);
        kahan_update(accumulator, compensator, value);
        f.feed(accumulator + compensator);
    }
}