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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-reduction
    Function object implementing prod capabilities

    Returns the product of the elements of the SIMD vector

    @par Semantic:

    For every parameter of pack type T

    @code
    scalar_of_t<T> r = prod(x);
    @endcode

    is similar to:

    @code
    scalar_of_t<T> r = One;
    for(result_type i = 0; i != T::static_size; ++i)
      r *= x[i];
    @endcode

  **/
  scalar_of_t<Value> prod(Value const & v0);
} }
#endif

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

#endif


//!
//! Contains the implementation of an adaptive proposal function
//!
//! \file infer/adaptive.hpp
//! \author Darren Shen
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <random>
#include <functional>
#include <Eigen/Core>
#include "prior/world.hpp"
#include "prior/prior.hpp"
#include "distrib/multigaussian.hpp"
#include <iostream>
#include <glog/logging.h>

namespace stateline
{
  namespace mcmc
  {

    //! A function to bounce the MCMC proposal off the hard boundaries.
    //! This allows the proposal to always move around without getting stuck at
    //! 'walls'
    //!
    //! \param val The proposed value
    //! \param min The minimum bound of theta
    //! \param max The maximum bound of theta
    //!  \returns The new bounced theta definitely in the bounds
    //!
    Eigen::VectorXd bouncyBounds(const Eigen::VectorXd& val,const Eigen::VectorXd& min, const Eigen::VectorXd& max)
    {
      Eigen::VectorXd delta = max - min;
      Eigen::VectorXd result = val;
      Eigen::Matrix<bool, Eigen::Dynamic, 1> tooBig = (val.array() > max.array());
      Eigen::Matrix<bool, Eigen::Dynamic, 1> tooSmall = (val.array() < min.array());
      for (uint i=0; i< result.size(); i++)
      {
        bool big = tooBig(i);
        bool small = tooSmall(i);
        if (big)
        {
          double overstep = val(i)-max(i);
          int nSteps = (int)(overstep /  delta(i));
          double stillToGo = overstep - nSteps*delta(i);
          if (nSteps % 2 == 0)
            result(i) = max(i) - stillToGo;
          else
            result(i) = min(i) + stillToGo;
        }
        if (small)
        {
          double understep = min(i) - val(i);
          int nSteps = (int)(understep / delta(i));
          double stillToGo = understep - nSteps*delta(i);
          if (nSteps % 2 == 0)
            result(i) = min(i) + stillToGo;
          else
            result(i) = max(i) - stillToGo;
        }
      }
      return result;
    }

    //! An adaptive Gaussian proposal function. It randomly varies each value in
    //! the state according to a Gaussian distribution whose variance changes
    //! depending on the acceptance ratio of a chain. It also bounces of the
    //! walls of the hard boundaries given so as not to get stuck in corners.
    //!
    //! \param state The current state of the chain
    //! \param sigma The standard deviation of the distribution (step size of the proposal)
    //! \param qcovL A dummy variable to maintain consistent prototypes w/other proposals
    //! \param min The minimum bound of theta
    //! \param max The maximum bound of theta
    //! \returns The new proposed theta
    //!
    Eigen::VectorXd adaptiveGaussianProposal(const Eigen::VectorXd &state, double sigma,
        const Eigen::MatrixXd& qcovL, const Eigen::VectorXd& min, const Eigen::VectorXd& max)
    {
      // Random number generators
      static std::random_device rd;
      static std::mt19937 generator(rd());
      static std::normal_distribution<> rand; // Standard normal

      // Vary each paramater according to a Gaussian distribution
      Eigen::VectorXd proposal(state.rows());
      for (int i = 0; i < proposal.rows(); i++)
        proposal(i) = state(i) + rand(generator) * sigma;

      //return bouncyBounds(proposal, min, max);
      return proposal;
    };

    //! RS 2018/03/09:  A multivariate Gaussian proposal function.
    //! It turns out random walk proposals made from a Gaussian with correlated
    //! components won't satisfy detailed balance if made across a reflection
    //! boundary, so it's important that we NOT do this, and instead just set
    //! the world prior probability to zero in order to auto-reject the state.
    //!
    //! \param state The current state of the chain
    //! \param sigma Scaling parameter (step size) to apply to chain covariance
    //! \param qcovL Cholesky factor of the covariance of a multivariate Gaussian
    //! \returns The new proposed theta
    //!

    Eigen::VectorXd multiGaussianProposal(
        const Eigen::VectorXd &state, double sigma, const Eigen::MatrixXd& qcovL)
    {
      // Random number generators
      static std::random_device rd;
      static std::mt19937 generator(rd());
      static std::normal_distribution<> rand; // Standard normal

      // Draw from a multivariate Gaussian scaled to unit determinant
      uint nDims = qcovL.rows();
      Eigen::VectorXd zero_mean = 0.0*state;
      double logdetq = qcovL.diagonal().array().log().sum();
      double iScale = exp(-logdetq / nDims);
      VLOG(4) << "logdetq = " << logdetq << ", iScale = " << iScale;

      Eigen::VectorXd randnDraws(nDims);
      for (uint i = 0; i < nDims; i++)
        randnDraws(i) = rand(generator);
      Eigen::VectorXd myprop = state + sigma * iScale * qcovL * randnDraws;
      return myprop;
    };

    //! Crank-Nicolson proposal function.
    //!
    //! \param state The current state of the chain
    //! \param sigma The standard deviation of the distribution (step size of the proposal)
    //! \param qcov A dummy variable to maintain consistent prototypes w/other proposals
    //! \param prior The prior from which we should draw
    //! \returns The new proposed theta
    //!
    Eigen::VectorXd crankNicolsonProposal(const Eigen::VectorXd &state, double sigma,
        const Eigen::MatrixXd& qcov, obsidian::GlobalPrior& prior
    )
    {
      // Random number generators
      static std::random_device rd;
      static std::mt19937 generator(rd());
      static std::normal_distribution<> rand; // Standard normal

      // RS 2018/04/19:  Transform sigma to the Crank-Nicholson "step size"
      // parameter rho.  This will allow the chains to adapt the step size.
      // While technically 0 < rho < 1, mapping -1 < rho < 1 should be good
      // enough since the sign of rho won't affect the proposal density.
      double F = exp(sigma);
      double rho = (F - 1.0/F)/(F + 1.0/F);

      Eigen::VectorXd proposal(state.rows());
      Eigen::VectorXd epsilon_vec = prior.sample(generator);
      for (int i = 0; i < proposal.rows(); i++) {
        double epsilon = epsilon_vec(i);
        proposal(i) = std::sqrt(1 - rho*rho)*state(i) + rho*epsilon;
      }

      return proposal;
    };

    double gaussianProposalPDF(
	    const Eigen::VectorXd& theta, const double sigma,
        const Eigen::VectorXd& thetaMins, const Eigen::VectorXd& thetaMaxs
    )
    {
	  double n = theta.size();
	  Eigen::MatrixXd Sigma = Eigen::MatrixXd::Identity(n, n) * sigma;
	  obsidian::distrib::MultiGaussian input(theta, Sigma);
	  double density = obsidian::distrib::logPDF(theta, input, thetaMins, thetaMaxs);
	  return density;
    };

  }
}


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "../check_arrays_are_close.hpp"
#include "../type_template.hpp"

#include "assembly/assembly_options.hpp"
#include "assembly/boundary_operator.hpp"
#include "assembly/context.hpp"
#include "assembly/grid_function.hpp"
#include "assembly/identity_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"
#include "assembly/surface_normal_independent_function.hpp"

#include "common/scalar_traits.hpp"

#include "grid/grid.hpp"
#include "grid/grid_factory.hpp"

#include "space/piecewise_linear_continuous_scalar_space.hpp"
#include "space/piecewise_constant_scalar_space.hpp"

#include <boost/test/floating_point_comparison.hpp>

using namespace Bempp;

template <typename ValueType_>
class ConstantFunction
{
public:
    typedef ValueType_ ValueType;
    typedef typename ScalarTraits<ValueType>::RealType CoordinateType;

    int argumentDimension() const { return 3; }
    int resultDimension() const { return 1; }

    inline void evaluate(const arma::Col<CoordinateType>& point,
                         arma::Col<ValueType>& result) const {
        result(0) = 2.;
    }
};

template <typename ValueType_>
class SinusoidalFunction
{
public:
    typedef ValueType_ ValueType;
    typedef typename ScalarTraits<ValueType>::RealType CoordinateType;

    int argumentDimension() const { return 3; }
    int resultDimension() const { return 1; }

    inline void evaluate(const arma::Col<CoordinateType>& point,
                         arma::Col<ValueType>& result) const {
        result(0) = point(0);
    }
};

template <typename ValueType_>
class ExponentialFunction
{
public:
    typedef ValueType_ ValueType;
    typedef typename ScalarTraits<ValueType>::RealType CoordinateType;

    int argumentDimension() const { return 3; }
    int resultDimension() const { return 1; }

    inline void evaluate(const arma::Col<CoordinateType>& point,
                         arma::Col<ValueType>& result) const {
        result(0) = std::exp(ValueType(0., point(0)));
    }
};

// Tests

BOOST_AUTO_TEST_SUITE(GridFunction)

BOOST_AUTO_TEST_CASE_TEMPLATE(L2Norm_works_for_constant_function_and_piecewise_constants, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    shared_ptr<Space<BFT> > space(
        new PiecewiseConstantScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));
    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    Bempp::GridFunction<BFT, RT> fun(context, space, space,
                surfaceNormalIndependentFunction(
                    ConstantFunction<RT>()));

    CT norm = fun.L2Norm();
    CT expectedNorm = sqrt(2. * 2. * 4. * M_PI);
    BOOST_CHECK_CLOSE(norm, expectedNorm, 1 /* percent */);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(L2Norm_works_for_sinusoidal_function_and_piecewise_constants, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    shared_ptr<Space<BFT> > space(
        new PiecewiseConstantScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));
    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    Bempp::GridFunction<BFT, RT> fun(context, space, space,
                surfaceNormalIndependentFunction(
                    SinusoidalFunction<RT>()));
    exportToVtk(fun, VtkWriter::CELL_DATA, "fun", "fun");

    CT norm = fun.L2Norm();
    CT expectedNorm = sqrt(4. / 3. * M_PI);
    BOOST_CHECK_CLOSE(norm, expectedNorm, 1 /* percent */);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(L2Norm_works_for_exponential_function_and_piecewise_constants, ResultType, complex_result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    shared_ptr<Space<BFT> > space(
        new PiecewiseConstantScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));
    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    Bempp::GridFunction<BFT, RT> fun(context, space, space,
                surfaceNormalIndependentFunction(
                    ExponentialFunction<RT>()));

    CT norm = fun.L2Norm();
    CT expectedNorm = sqrt(4 * M_PI);
    BOOST_CHECK_CLOSE(norm, expectedNorm, 1 /* percent */);
}

BOOST_AUTO_TEST_SUITE_END()


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include <map>

#include <boost/make_shared.hpp>

#include "rttbBioModel.h"
#include "rttbNTCPLKBModel.h"
#include "rttbBioModelCurve.h"
#include "rttbDvhBasedModels.h"

namespace rttb
{
	namespace models
	{

		CurveDataType getCurveDoseVSBioModel(BioModel& aModel, double normalisationDose, int aBin,
		                                     double minDose, double maxDose)
		{
			CurveDataType _curveData;

			double minFactor = 0, maxFactor = 0;

			minFactor = minDose / normalisationDose;
			maxFactor = maxDose / normalisationDose;

			double lastValue = 0;

			for (int i = 0; i < aBin; i++)
			{
				double factor = minFactor + i * (maxFactor - minFactor) / (aBin - 1);

				if (lastValue < 1)
				{
					aModel.init(factor);
					BioModelValueType value = aModel.getValue();
					lastValue = value;
					_curveData.insert(CurvePointType(factor * normalisationDose, value));
				}
				else
				{
					_curveData.insert(CurvePointType(factor * normalisationDose, 1));
				}
			}

			return _curveData;
		}

		CurveDataType getCurveEUDVSBioModel(NTCPLKBModel& aModel, DoseCalcType maxFactor,
		                                    DoseCalcType minFactor, int aBin)
		{
			CurveDataType _curveData;
			BioModel::DVHPointer _dvh = aModel.getDVH();

			for (int i = 0; i < aBin; i++)
			{
				DoseCalcType factor = minFactor + i * (maxFactor - minFactor) / (aBin - 1);

				core::DVH variantDVH = core::DVH(_dvh->getDataDifferential(), _dvh->getDeltaD() * factor,
				                                 _dvh->getDeltaV(),
				                                 "temporary", "temporary");

				auto spDVH = boost::make_shared<core::DVH>(variantDVH);
				double eud = getEUD(spDVH, aModel.getA());

				aModel.init(factor);
				BioModelValueType value = aModel.getValue();
				_curveData.insert(CurvePointType(eud, value));
			}

			return _curveData;
		}
	}
}


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2022 Ilia Shirobokov <i.shirobokov@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE fri_test

#include <string>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt4.hpp>
#include <nil/crypto3/algebra/curves/pallas.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/pallas.hpp>
#include <nil/crypto3/algebra/random_element.hpp>

#include <nil/crypto3/math/polynomial/polynomial.hpp>
#include <nil/crypto3/math/polynomial/lagrange_interpolation.hpp>
#include <nil/crypto3/math/algorithms/unity_root.hpp>
#include <nil/crypto3/math/domains/evaluation_domain.hpp>
#include <nil/crypto3/math/algorithms/make_evaluation_domain.hpp>

#include <nil/crypto3/zk/transcript/fiat_shamir.hpp>
#include <nil/crypto3/zk/commitments/polynomial/fri.hpp>
#include <nil/crypto3/zk/commitments/polynomial/batched_fri.hpp>
#include <nil/crypto3/zk/commitments/type_traits.hpp>

using namespace nil::crypto3;

BOOST_AUTO_TEST_SUITE(fri_test_suite)

BOOST_AUTO_TEST_CASE(fri_basic_test) {

    // setup
    using curve_type = algebra::curves::pallas;
    using FieldType = typename curve_type::base_field_type;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t d = 16;

    constexpr static const std::size_t r = boost::static_log2<d>::value;
    constexpr static const std::size_t m = 2;

    typedef zk::commitments::fri<FieldType, merkle_hash_type, transcript_hash_type, m> fri_type;

    static_assert(zk::is_commitment<fri_type>::value);
    static_assert(!zk::is_commitment<merkle_hash_type>::value);

    typedef typename fri_type::proof_type proof_type;
    typedef typename fri_type::params_type params_type;

    params_type params;

    constexpr static const std::size_t d_extended = d;
    std::size_t extended_log = boost::static_log2<d_extended>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(extended_log, r);

    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};
    params.r = r;
    params.D = D;
    params.q = q;
    params.max_degree = d - 1;

    BOOST_CHECK(D[1]->m == D[0]->m / 2);
    BOOST_CHECK(D[1]->get_domain_element(1) == D[0]->get_domain_element(1).squared());
    BOOST_CHECK(params.q.evaluate(D[0]->get_domain_element(1)) == D[0]->get_domain_element(1).squared());

    // commit
    math::polynomial<typename FieldType::value_type> f = {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1};

    merkle_tree_type commit_merkle = fri_type::precommit(f, D[0]);
    std::array<typename FieldType::value_type, 1> evaluation_points = {D[0]->get_domain_element(1).pow(5)};

    // eval
    std::vector<std::uint8_t> init_blob {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript(init_blob);

    proof_type proof = fri_type::proof_eval(f, commit_merkle, params, transcript);

    // verify
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript_verifier(init_blob);

    BOOST_CHECK(fri_type::verify_eval(proof, params, transcript_verifier));

    typename FieldType::value_type verifier_next_challenge = transcript_verifier.template challenge<FieldType>();
    typename FieldType::value_type prover_next_challenge = transcript.template challenge<FieldType>();
    BOOST_CHECK(verifier_next_challenge == prover_next_challenge);
}

BOOST_AUTO_TEST_CASE(fri_fold_test) {

    // fri params
    using curve_type = algebra::curves::mnt4<298>;
    using FieldType = typename curve_type::base_field_type;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t d = 4;
    constexpr static const std::size_t r = boost::static_log2<d>::value;
    constexpr static const std::size_t m = 2;

    typedef zk::commitments::fri<FieldType, merkle_hash_type, transcript_hash_type, m> fri_type;
    typedef typename fri_type::proof_type proof_type;
    typedef typename fri_type::params_type params_type;

    params_type params;
    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};

    std::size_t d_log = boost::static_log2<d>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(d_log, 1);

    params.r = r;
    params.D = D;
    params.q = q;

    math::polynomial<typename FieldType::value_type> f = {1, 3, 4, 3};

    typename FieldType::value_type omega = D[0]->get_domain_element(1);

    typename FieldType::value_type x_next = params.q.evaluate(omega);
    typename FieldType::value_type alpha = algebra::random_element<FieldType>();

    math::polynomial<typename FieldType::value_type> f_next = zk::commitments::detail::fold_polynomial<FieldType>(f, alpha);

    BOOST_CHECK_EQUAL(f_next.degree(), f.degree() / 2);
    std::vector<std::pair<typename FieldType::value_type, typename FieldType::value_type>> interpolation_points {
        std::make_pair(omega, f.evaluate(omega)),
        std::make_pair(-omega, f.evaluate(-omega)),
    };

    math::polynomial<typename FieldType::value_type> interpolant = math::lagrange_interpolation(interpolation_points);
    typename FieldType::value_type x1 = interpolant.evaluate(alpha);
    typename FieldType::value_type x2 = f_next.evaluate(x_next);
    BOOST_CHECK(x1 == x2);
}

BOOST_AUTO_TEST_CASE(fri_steps_count_test) {

    // fri params
    using curve_type = algebra::curves::mnt4<298>;
    using FieldType = typename curve_type::base_field_type;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t d = 16;

    constexpr static const std::size_t r = boost::static_log2<d>::value;
    constexpr static const std::size_t m = 2;

    typedef zk::commitments::fri<FieldType, merkle_hash_type, transcript_hash_type, m> fri_type;
    typedef typename fri_type::proof_type proof_type;
    typedef typename fri_type::params_type params_type;

    params_type params;
    math::polynomial<typename FieldType::value_type> f = {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1};

    constexpr static const std::size_t d_extended = d;
    std::size_t extended_log = boost::static_log2<d_extended>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(extended_log, r);

    params.r = r - 1;
    params.D = D;
    params.q = f;
    params.max_degree = d - 1;

    BOOST_CHECK(D[1]->m == D[0]->m / 2);
    merkle_tree_type commit_merkle = fri_type::precommit(f, D[0]);
    std::array<typename FieldType::value_type, 1> evaluation_points = {D[0]->get_domain_element(1).pow(5)};

    std::vector<std::uint8_t> init_blob {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript(init_blob);

    proof_type proof = fri_type::proof_eval(f, commit_merkle, params, transcript);

    math::polynomial<typename FieldType::value_type> final_polynomial = proof.final_polynomial;
    BOOST_CHECK_EQUAL(proof.final_polynomial.degree(), 1);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(batched_fri_test_suite)

BOOST_AUTO_TEST_CASE(batched_fri_basic_test) {

    // setup
    using curve_type = algebra::curves::pallas;
    using FieldType = typename curve_type::base_field_type;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t d = 16;

    constexpr static const std::size_t r = boost::static_log2<d>::value;
    constexpr static const std::size_t m = 2;
    constexpr static const std::size_t leaf_size = 2;

    typedef zk::commitments::batched_fri<FieldType, merkle_hash_type, transcript_hash_type, m, leaf_size> fri_type;

    static_assert(zk::is_commitment<fri_type>::value);
    static_assert(!zk::is_commitment<merkle_hash_type>::value);

    typedef typename fri_type::proof_type proof_type;
    typedef typename fri_type::params_type params_type;

    params_type params;

    constexpr static const std::size_t d_extended = d;
    std::size_t extended_log = boost::static_log2<d_extended>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(extended_log, r);

    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};
    params.r = r;
    params.D = D;
    params.q = q;
    params.max_degree = d - 1;

    BOOST_CHECK(D[1]->m == D[0]->m / 2);
    BOOST_CHECK(D[1]->get_domain_element(1) == D[0]->get_domain_element(1).squared());
    BOOST_CHECK(params.q.evaluate(D[0]->get_domain_element(1)) == D[0]->get_domain_element(1).squared());

    // commit
    std::array<math::polynomial<typename FieldType::value_type>, leaf_size> f = {{
        {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1},
        {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1}}};

    merkle_tree_type commit_merkle = fri_type::precommit(f, D[0]);
    std::array<typename FieldType::value_type, 1> evaluation_points = {D[0]->get_domain_element(1).pow(5)};

    // eval
    std::vector<std::uint8_t> init_blob {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript(init_blob);

    proof_type proof = fri_type::proof_eval(f, commit_merkle, params, transcript);

    // verify
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript_verifier(init_blob);

    BOOST_CHECK(fri_type::verify_eval(proof, params, transcript_verifier));

    typename FieldType::value_type verifier_next_challenge = transcript_verifier.template challenge<FieldType>();
    typename FieldType::value_type prover_next_challenge = transcript.template challenge<FieldType>();
    BOOST_CHECK(verifier_next_challenge == prover_next_challenge);
}

BOOST_AUTO_TEST_SUITE_END()


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_INTEROP_VTK_BOUNDS_HPP
#define BOOST_COMPUTE_INTEROP_VTK_BOUNDS_HPP

#include <vector>
#include <iterator>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/reduce.hpp>
#include <boost/compute/container/array.hpp>

namespace boost {
namespace compute {

/// Calculates the bounds for the points in the range [\p first, \p last) and
/// stores the result in \p bounds.
///
/// For example, this can be used to implement the GetBounds() method for a
/// vtkMapper subclass.
template<class PointIterator>
inline void vtk_compute_bounds(PointIterator first,
                               PointIterator last,
                               double bounds[6],
                               command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<PointIterator>::value_type T;

    const context &context = queue.get_context();

    // compute min and max point
    array<T, 2> extrema(context);
    reduce(first, last, extrema.begin() + 0, min<T>(), queue);
    reduce(first, last, extrema.begin() + 1, max<T>(), queue);

    // copy results to host buffer
    std::vector<T> buffer(2);
    copy_n(extrema.begin(), 2, buffer.begin(), queue);

    // copy to vtk-style bounds
    bounds[0] = buffer[0][0]; bounds[1] = buffer[1][0];
    bounds[2] = buffer[0][1]; bounds[3] = buffer[1][1];
    bounds[4] = buffer[0][2]; bounds[5] = buffer[1][2];
}

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_INTEROP_VTK_BOUNDS_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_CEPHES_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_CEPHES_HPP_INCLUDED
#include <nt2/toolbox/trigonometric/functions/rem_pio2_cephes.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/scalar/round.hpp>
#include <nt2/include/functions/scalar/fast_toint.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_cephes_, tag::cpu_,
                             (A0),
                             (scalar_ < floating_<A0> > )
                             )
  {
    typedef boost::fusion::tuple<A0,A0,nt2::int32_t>           result_type;
    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<2>(res) =
          nt2::rem_pio2_cephes(a0,
                           boost::fusion::at_c<0>(res),
                           boost::fusion::at_c<1>(res)
                          );
      return res;
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementation when real
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_cephes_, tag::cpu_,
                             (A0),
                             (scalar_< floating_<A0> >)
                             (scalar_< floating_<A0> >)
                             (scalar_< floating_<A0> >)
                             )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& x, A0 & xr, A0& xc) const
    {
      A0 xi =  nt2::round(x*Twoopi<A0>());
      xr = x-xi*Pio2_1<A0>();
      xr -= xi*Pio2_2<A0>();
      xr -= xi*Pio2_3<A0>();
      xc = Zero<A0>();
      return nt2::fast_toint(xi)&3;
    }
  };
} }

#endif


/*
 *  Copyright 2011-2014 Maxim Milakov
 *
 *  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 "distort_2d_data_transformer.h"

#include "neural_network_exception.h"
#include "data_transformer_util.h"

#include <opencv2/core/core.hpp>
#include <boost/format.hpp>

namespace nnforge
{
	distort_2d_data_transformer::distort_2d_data_transformer(
		float max_absolute_rotation_angle_in_degrees,
		float max_scale_factor,
		float min_shift_right_x,
		float max_shift_right_x,
		float min_shift_down_y,
		float max_shift_down_y,
		bool flip_around_x_axis_allowed,
		bool flip_around_y_axis_allowed,
		float max_stretch_factor)
	{
		generator = rnd::get_random_generator();

		rotate_angle_distribution = nnforge_uniform_real_distribution<float>(-max_absolute_rotation_angle_in_degrees, max_absolute_rotation_angle_in_degrees);
		scale_distribution = nnforge_uniform_real_distribution<float>(1.0F / max_scale_factor, max_scale_factor);
		shift_x_distribution = nnforge_uniform_real_distribution<float>(min_shift_right_x, max_shift_right_x);
		shift_y_distribution = nnforge_uniform_real_distribution<float>(min_shift_down_y, max_shift_down_y);
		flip_around_x_distribution = nnforge_uniform_int_distribution<int>(0, flip_around_x_axis_allowed ? 1 : 0);
		flip_around_y_distribution = nnforge_uniform_int_distribution<int>(0, flip_around_y_axis_allowed ? 1 : 0);
		stretch_distribution = nnforge_uniform_real_distribution<float>(1.0F / max_stretch_factor, max_stretch_factor);
		stretch_angle_distribution = nnforge_uniform_real_distribution<float>(-180.0F, 180.0F);
	}

	distort_2d_data_transformer::~distort_2d_data_transformer()
	{
	}

	void distort_2d_data_transformer::transform(
		const void * data,
		void * data_transformed,
		neuron_data_type::input_type type,
		const layer_configuration_specific& original_config,
		unsigned int sample_id)
	{
		if (type != neuron_data_type::type_byte)
			throw neural_network_exception("distort_2d_data_transformer is implemented for data stored as bytes only");

		if (original_config.dimension_sizes.size() != 2)
			throw neural_network_exception((boost::format("distort_2d_data_transformer is processing 2d data only, data is passed with number of dimensions %1%") % original_config.dimension_sizes.size()).str());

		float rotation_angle = rotate_angle_distribution.min();
		if (rotate_angle_distribution.max() > rotate_angle_distribution.min())
			rotation_angle = rotate_angle_distribution(generator);
		float scale = scale_distribution.min();
		if (scale_distribution.max() > scale_distribution.min())
			scale = scale_distribution(generator);
		float shift_x = shift_x_distribution.min();
		if (shift_x_distribution.max() > shift_x_distribution.min())
			shift_x = shift_x_distribution(generator);
		float shift_y = shift_y_distribution.min();
		if (shift_y_distribution.max() > shift_y_distribution.min())
			shift_y = shift_y_distribution(generator);
		bool flip_around_x_axis = (flip_around_x_distribution.min() == 1);
		if (flip_around_x_distribution.max() > flip_around_x_distribution.min())
			flip_around_x_axis = (flip_around_x_distribution(generator) == 1);
		bool flip_around_y_axis = (flip_around_y_distribution.min() == 1);
		if (flip_around_y_distribution.max() > flip_around_y_distribution.min())
			flip_around_y_axis = (flip_around_y_distribution(generator) == 1);
		float stretch = stretch_distribution.min();
		if (stretch_distribution.max() > stretch_distribution.min())
			stretch = stretch_distribution(generator);
		float stretch_angle = stretch_angle_distribution.min();
		if (stretch_angle_distribution.max() > stretch_angle_distribution.min())
			stretch_angle = stretch_angle_distribution(generator);

		unsigned int neuron_count_per_feature_map = original_config.get_neuron_count_per_feature_map();
		for(unsigned int feature_map_id = 0; feature_map_id < original_config.feature_map_count; ++feature_map_id)
		{
			cv::Mat1b image(static_cast<int>(original_config.dimension_sizes[1]), static_cast<int>(original_config.dimension_sizes[0]), static_cast<unsigned char *>(data_transformed) + (neuron_count_per_feature_map * feature_map_id));

			data_transformer_util::rotate_scale_shift(
				image,
				cv::Point2f(static_cast<float>(image.cols) * 0.5F, static_cast<float>(image.rows) * 0.5F),
				rotation_angle,
				scale,
				shift_x,
				shift_y,
				stretch,
				stretch_angle);

			data_transformer_util::flip(
				image,
				flip_around_x_axis,
				flip_around_y_axis);
		}
	}

 	bool distort_2d_data_transformer::is_deterministic() const
	{
		return false;
	}
}


// Copyright Tom Brinkman 2008. Distributed under the Boost
// Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef _fill_hpp_
#define _fill_hpp_

#include <boost/function.hpp>
#include <boost/gil/gil_all.hpp>

namespace layer
{

template <typename view_t>
struct fill
{
	typedef typename view_t::value_type value_type_t;
	value_type_t color;
	fill(value_type_t color) : color(color) {}

	void operator()(view_t& view)
	{
		boost::gil::fill_pixels(view,color);
	}
};

template <typename view_t>
struct fill_horizontal
{
	typedef boost::function<void (view_t&)> funct_t;
	double value1;
	double value2;
	double minvalue;
	double maxvalue;
	funct_t funct;
	fill_horizontal(funct_t funct, double value1, double value2, double minvalue, double maxvalue) :
		funct(funct), minvalue(minvalue), maxvalue(maxvalue), value1(value1), value2(value2) {}

	void operator()(view_t& view)
	{
		using namespace boost::gil;

		double diff = maxvalue-minvalue;
		if (diff == 0)
			return;

		double rval = 1-((value1-minvalue)/(diff));
		int y1 = (int)(rval*(view.height()-1));

		rval = 1-((value2-minvalue)/(diff));
		int y2 = (int)(rval*(view.height()-1));

		int yt = (std::min)(y1,y2);
		int yb = (std::max)(y1,y2);

		view_t v2 = subimage_view(view,0,yt,view.width(),yb-yt);
		funct(v2);
	}
};

}

#endif


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/at.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/drop_front.hpp>
#include <boost/hana/eval.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/functional/fix.hpp>
#include <boost/hana/functional/iterate.hpp>
#include <boost/hana/fwd/at.hpp>
#include <boost/hana/fwd/empty.hpp>
#include <boost/hana/fwd/prepend.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/is_empty.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/value.hpp>

#include <cstddef>
namespace hana = boost::hana;

struct LazyList;

template <typename X, typename Xs>
struct lazy_cons_type {
    X x;
    Xs xs;
    using hana_tag = LazyList;
};

auto lazy_cons = [](auto x, auto xs) {
    return lazy_cons_type<decltype(x), decltype(xs)>{x, xs};
};

struct lazy_nil_type { using hana_tag = LazyList; };

constexpr lazy_nil_type lazy_nil{};

auto repeat = hana::fix([](auto repeat, auto x) {
    return lazy_cons(x, hana::make_lazy(repeat)(x));
});

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Iterable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct at_impl<LazyList> {
        template <typename Xs, typename N>
        static constexpr auto apply(Xs&& lcons, N const& n) {
            return hana::drop_front(lcons, n).x;
        }
    };

    namespace detail {
        struct eval_tail {
            template <typename Xs>
            constexpr auto operator()(Xs const& lcons) const {
                return hana::eval(lcons.xs);
            }

            constexpr auto operator()(lazy_nil_type const&) const {
                return lazy_nil;
            }
        };
    }

    template <>
    struct drop_front_impl<LazyList> {
        template <typename Xs, typename N>
        static constexpr auto apply(Xs&& lcons, N const&) {
            return hana::iterate<N::value>(detail::eval_tail{}, lcons);
        }
    };

    template <>
    struct is_empty_impl<LazyList> {
        template <typename Xs>
        static constexpr auto apply(Xs const&)
        { return hana::false_c; }

        static constexpr auto apply(lazy_nil_type const&)
        { return hana::true_c; }
    };

    //////////////////////////////////////////////////////////////////////////
    // MonadPlus
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct prepend_impl<LazyList> {
        template <typename Xs, typename X>
        static constexpr auto apply(Xs xs, X x)
        { return lazy_cons(x, hana::make_lazy(xs)); }
    };

    template <>
    struct empty_impl<LazyList> {
        static constexpr auto apply()
        { return lazy_nil; }
    };
}}

int main() {
    BOOST_HANA_CONSTANT_CHECK(!hana::is_empty(repeat(1)));
    BOOST_HANA_CONSTEXPR_CHECK(hana::front(repeat(1)) == 1);
    BOOST_HANA_CONSTEXPR_CHECK(hana::at(repeat(1), hana::size_c<10>) == 1);
}


/*
 *            Copyright 2016 The MUSCET Development Team
 *
 *      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 <votca/xtp/gyration.h>
#include <boost/format.hpp>
#include <votca/tools/elements.h>

using namespace votca::tools;

namespace votca { namespace xtp {

void Density2Gyration::Initialize( tools::Property& options) {
    string key = Identify();

    std::string statestring=options.ifExistsReturnElseThrowRuntimeError<string>(key + ".state");
    _state.FromString(statestring);
    _dostateonly = options.ifExistsReturnElseReturnDefault<bool>(key + ".difference_to_groundstate",false);
    _gridsize = options.ifExistsReturnElseReturnDefault<string>(key+".gridsize","medium");
    _openmp_threads = options.ifExistsReturnElseReturnDefault<int>(key + ".openmp",1);

    // get the path to the shared folders with xml files
    char *votca_share = getenv("VOTCASHARE");
    if(votca_share == NULL) throw std::runtime_error("VOTCASHARE not set, cannot open help files.");
    }

    void Density2Gyration::AnalyzeDensity(Orbitals & orbitals) {
      int threads = 1;
#ifdef _OPENMP
      if (_openmp_threads > 0) omp_set_num_threads(_openmp_threads);
      threads = omp_get_max_threads();
#endif
      CTP_LOG(ctp::logDEBUG, *_log) << "===== Running on " << threads << " threads ===== " << flush;

      std::vector< QMAtom* > Atomlist = orbitals.QMAtoms();
      Eigen::MatrixXd DMAT_tot;
      BasisSet bs;
      bs.LoadBasisSet(orbitals.getDFTbasis());
      AOBasis basis;
      basis.AOBasisFill(bs, Atomlist);
      AnalyzeGeometry(Atomlist);
      std::vector<Eigen::MatrixXd > DMAT;

      // setup numerical integration grid
      NumericalIntegration numway;
      numway.GridSetup(_gridsize, Atomlist, basis);

      if (!_dostateonly) {
        Eigen::MatrixXd DMATGS = orbitals.DensityMatrixFull(_state);
        Gyrationtensor gyro = numway.IntegrateGyrationTensor(DMAT_tot);
        tools::matrix::eigensystem_t system;
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Converting to Eigenframe " << flush;
        gyro.gyration.SolveEigensystem(system);
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating Quaternion " << flush;
        //Eigen::Quaterniond _quaternion = get_quaternion( system );
        // report results
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Reporting " << flush;
        ReportAnalysis(_state.ToLongString(), gyro,system);

      } else{
        // hole density first
        std::vector<Eigen::MatrixXd > DMAT=orbitals.DensityMatrixExcitedState(_state);
        Gyrationtensor gyro_hole = numway.IntegrateGyrationTensor(DMAT[0]);
        tools::matrix::eigensystem_t system_h;
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Converting to Eigenframe " << flush;
        gyro_hole.gyration.SolveEigensystem(system_h);
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Reporting " << flush;
        ReportAnalysis("hole", gyro_hole,system_h);

        // electron density
        Gyrationtensor gyro_electron = numway.IntegrateGyrationTensor(DMAT[1]);
        tools::matrix::eigensystem_t system_e;
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Converting to Eigenframe " << flush;
        gyro_electron.gyration.SolveEigensystem(system_e);
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Reporting " << flush;
        ReportAnalysis("electron", gyro_electron,system_e);
      }
      return;
    }

    void Density2Gyration::AnalyzeGeometry(std::vector<QMAtom*> atoms){

        tools::Elements elements;
        double mass=0.0;
        tools::vec centroid = tools::vec(0.0);
        tools::matrix gyration = tools::matrix(0.0);
        for (QMAtom* atom:atoms){
            double m = elements.getMass(atom->getType());
            const tools::vec & pos =atom->getPos();
            mass+= m;
            centroid+=m*pos;
            gyration+=m*(pos|pos);
        }
        centroid/=mass;
        gyration/=mass;
        gyration-=(centroid|centroid);
        Gyrationtensor gyro;
        gyro.mass=mass;
        gyro.centroid=centroid;
        gyro.gyration=gyration;
        tools::matrix::eigensystem_t system;
        gyration.SolveEigensystem(system);
        ReportAnalysis( "geometry", gyro,system );
    }

    void Density2Gyration::ReportAnalysis(string label,Gyrationtensor gyro, tools::matrix::eigensystem_t system){

            CTP_LOG(ctp::logINFO, *_log) << "---------------- " << label << " ----------------" << flush;
            CTP_LOG(ctp::logINFO, *_log) << (boost::format("  Norm               = %1$9.4f ") % (gyro.mass) ) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid x         = %1$9.4f Ang") % (gyro.centroid.getX()*tools::conv::bohr2ang) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid y         = %1$9.4f Ang") % (gyro.centroid.getY()*tools::conv::bohr2ang) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid y         = %1$9.4f Ang") % (gyro.centroid.getZ()*tools::conv::bohr2ang) ) << flush;

            double RA2 = tools::conv::bohr2ang  *tools::conv::bohr2ang;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xx = %1$9.4f Ang^2") % (gyro.gyration[0][0]*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xy = %1$9.4f Ang^2") % (gyro.gyration[0][1]*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xz = %1$9.4f Ang^2") % (gyro.gyration[0][2]*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor yy = %1$9.4f Ang^2") % (gyro.gyration[1][1]*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor yz = %1$9.4f Ang^2") % (gyro.gyration[1][2]*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor zz = %1$9.4f Ang^2") % (gyro.gyration[2][2]*RA2) ) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D1 = %1$9.4f Ang^2") % (system.eigenvalues[0] *RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D2 = %1$9.4f Ang^2") % (system.eigenvalues[1]*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D3 = %1$9.4f Ang^2") % (system.eigenvalues[2]*RA2) ) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Radius of Gyration = %1$9.4f Ang") % (std::sqrt(system.eigenvalues[0] +system.eigenvalues[1]+ system.eigenvalues[2])*tools::conv::bohr2ang )) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 1 = %1$9.4f ") % (system.eigenvecs[0].getX()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 2 = %1$9.4f ") % (system.eigenvecs[0].getY()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 3 = %1$9.4f ") % (system.eigenvecs[0].getZ()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 1 = %1$9.4f ") % (system.eigenvecs[1].getX()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 2 = %1$9.4f ") % (system.eigenvecs[1].getY()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 3 = %1$9.4f ") % (system.eigenvecs[1].getZ()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 1 = %1$9.4f ") % (system.eigenvecs[2].getX()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 2 = %1$9.4f ") % (system.eigenvecs[2].getY()) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 3 = %1$9.4f ") % (system.eigenvecs[2].getZ()) ) << flush;
            return;
    }

}}


// Copyright (C) 2005, 2006 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
//
//  compilation: mpic++ -O3 -std=c++14 -ldl -lboost_mpi -lboost_serialization -lboost_graph_parallel -lboost_system
#define PBGL_ACCOUNTING

#include <boost/graph/use_mpi.hpp>
#include <boost/config.hpp>
#include <boost/throw_exception.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/graph/distributed/boman_et_al_graph_coloring.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/parallel/distribution.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <iostream>
#include <boost/random.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/accounting.hpp>
#include <fstream>
#include <time.h>
#include <chrono>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

using namespace boost;
using boost::graph::distributed::mpi_process_group;

void
test_distributed_graph_coloring(std::string filename, int superstep)
{
  typedef adjacency_list<vecS,
                         distributedS<mpi_process_group, vecS>,
                         undirectedS> Graph;

  typedef property_map<Graph, vertex_index_t>::type vertex_index_map;

  std::ifstream infile(filename);
  if (!infile) {
    std::cout << "Error loading file" << std::endl;
  }

  std::string line;
  uint32_t nvertices;
  uint32_t nedges;
  uint32_t a;
  uint32_t b;

  std::getline(infile, line);
  std::istringstream iss(line);
  if (iss >> a >> b) {
    nvertices = a;
    nedges = b;
  }

  Graph g(nvertices);

  if (process_id(g.process_group()) == 0) {
    while (std::getline(infile,line)) {
    std::istringstream iss(line);
      if (iss >> a >> b) {
        add_edge(vertex(a,g),vertex(b,g),g);
      }
    }
  }

  // Set up color map
  std::vector<int> colors_vec(num_vertices(g));
  iterator_property_map<int*, vertex_index_map> color(&colors_vec[0],
                                                      get(vertex_index, g));

  auto wtcs = std::chrono::system_clock::now();
  // Run the graph coloring algorithm
  graph::boman_et_al_graph_coloring(g, color, superstep);

  std::chrono::duration<double> wtcduration = std::chrono::system_clock::now() - wtcs;
  if (process_id(g.process_group()) == 0) {
    std::cout << "Finished in " << wtcduration.count() << " seconds [Wall time]" << std::endl;
  }
  if (process_id(g.process_group()) == 0) {
    graph::distributed::boman_et_al_graph_coloring_stats.print(std::cout);
  }

  graph::distributed::boman_et_al_graph_coloring_stats_t stats = graph::distributed::boman_et_al_graph_coloring_stats;

  if (process_id(g.process_group()) == 0) {
    std::ofstream results("results_baseline.csv", std::ios_base::app | std::ios_base::out);
    results << filename <<  "," << num_processes(g.process_group()) << "," << nvertices << "," << nedges << "," << /*boost::graph::accounting::print_time(stats.execution_time)*/ stats.execution_time << "," << -1 << "," << stats.num_colors << "," << superstep << "," << 1 << std::endl;
  }
}

int test_main(int argc, char* argv[])
{
  std::string filename;
  int superstep;

  mpi::environment env(argc, argv);
  mpi::communicator comm;

  if (argc < 3) {
    if (comm.rank() == 0) {
      std::cout << "invalid number of arguments";
    }
    boost::mpi::environment::abort(-1);
  }

  if (comm.rank() == 0) {
    std::cout << "Processing file: " << argv[1] << std::endl;
  }

  filename = argv[1];

  std::istringstream bla(argv[2]);
  if (!(bla >> superstep)) {
    if (comm.rank() == 0) {
      std::cout << "invalid superstep" << std::endl;
    }
    boost::mpi::environment::abort(-1);
  }

  test_distributed_graph_coloring(filename, superstep);

  return 0;
}


//
// Created by lss on 2021/1/23.
//
#include "psopt.h"
#include "ros/ros.h"
#include <chrono>
#include "iomanip"
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include "offb_posctl/controlstate.h"
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <thread>
#include <math.h>
#include <stdlib.h>
#include <FILTER.h>

using namespace Eigen;
using namespace std;

geometry_msgs::PoseStamped pos_ref;         //无人机参考位置

mavros_msgs::State current_state;           //无人机当前状态(mode arm)
sensor_msgs::Imu   imu_drone;               //读入的无人机的IMU信息 包括姿态角和线加速度

geometry_msgs::PoseStamped  pos_drone;      //读入的无人机当前位置
geometry_msgs::PoseStamped  pos_drone_last; //读入的无人机上一次位置
geometry_msgs::PoseStamped  fused_drone; //融合了vicon数据的无人机位姿，注意是NED坐标下的，需要翻转

geometry_msgs::TwistStamped vel_drone;      //读入的无人机当前速度

geometry_msgs::Vector3 acc_receive;         //读入的无人机线加速度
geometry_msgs::Vector3 angle_receive;       //读入的无人机姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon_qua;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromviconinit;  //读入的vicon计算的sgement initial 姿态（欧拉角）
geometry_msgs::Vector3 fusedangle_receive;

geometry_msgs::Quaternion orientation_target;   //发给无人机的姿态指令

geometry_msgs::Vector3 angle_des;            //线性模型输出的理想值
//geometry_msgs::Vector3 angle_dis;            //DOB控制器估计的扰动值
geometry_msgs::Vector3 angle_target;            //经DOB控制器作用后的实际系统输入值
geometry_msgs::Vector3 vel_target, vel_read, vel_read2;
geometry_msgs::Vector3 currentRPYangle;   //欧拉角
geometry_msgs::Vector3 filteredPlaneVelmsg;   //欧拉角
// debug data
geometry_msgs::Vector3 vel_vicon;
const float MAX_POSITION_MEASURE_ERROR = 1;
Eigen::Quaterniond current_quaternion(0,0,0,0);

bool pose_initialized = false;
bool vel_initialized = false;
int posvel_updatesynflag= false;// the variable to make sure the pos and vel used in ocp is synchronized, In reality, we can put pos and vel in one topic to avoid this problem

int timeconsumption=0;
float t_end=2;
int pointnumber=10;// the number is almost always 20. It less, the accuracy won't be enough, if more, the time consumpiton will be too large.
int controlfreq=50;

float px_ini = -3.0;
float pz_ini = 0.0;
float vx_ini = -0.1;
float vz_ini = 0.0;
float theta_ini = 0.0;
float rate_ini = 0.0;
float vxPlane_ini=0.0,vyPlane_ini=0.0,vzPlane_ini=0.0;
float aircoeffx=1.5,aircoeffz=0.35;
float carPlaneDistance=3.0;
offb_posctl::controlstate controlstate_msg;
bool currentupdateflag= false;
FILTER filterDroneVelx(150);
sensor_msgs::Imu drone_imu;

Alg  algorithm;
Sol  solution;
Prob problem;
double py0=0,pz0=0.5,phi0=0,vy0=0,vz0=0,omegax0=0,thrust0=9.8,tau0=0;
double pyf=2,pzf=2,phif=1.57,vyf=0.2*sin(phif),vzf=-0.2*cos(phif),omegaxf=0.0,thrustf=9.8*cos(phif),tauf=0;
////x1=y,x2=z,x3=phi,x4=vy,x5=vz,x6=omega,x7=thrust,x8=tau//

MatrixXd x  ;
MatrixXd u  ;
MatrixXd t  ;
MatrixXd x_refined  = MatrixXd::Zero(8,500);

adouble endpoint_cost(adouble* initial_states, adouble* final_states,
                      adouble* parameters,adouble& t0, adouble& tf,
                      adouble* xad, int iphase, Workspace* workspace)
{
    return tf;
}

//////////////////////////////////////////////////////////////////////////
///////////////////  Define the integrand (Lagrange) cost function  //////
//////////////////////////////////////////////////////////////////////////

adouble integrand_cost(adouble* states, adouble* controls, adouble* parameters,
                       adouble& time, adouble* xad, int iphase, Workspace* workspace)
{
    adouble x7 = states[ 6 ];
    adouble x8 = states[ 7 ];
//    adouble udf = controls[ 0 ];
//    adouble udtau = controls[ 1 ];
    return  0.5*x7*x7+0.5*x8*x8;
//    return 0;
}

//////////////////////////////////////////////////////////////////////////
///////////////////  Define the DAE's ////////////////////////////////////
//////////////////////////////////////////////////////////////////////////

void dae(adouble* derivatives, adouble* path, adouble* states,
         adouble* controls, adouble* parameters, adouble& time,
         adouble* xad, int iphase, Workspace* workspace)
{
    adouble x1dot, x2dot, x3dot,x4dot, x5dot, x6dot,x7dot, x8dot;

    adouble x1 = states[ 0 ];
    adouble x2 = states[ 1 ];
    adouble x3 = states[ 2 ];
    adouble x4 = states[ 3 ];
    adouble x5 = states[ 4 ];
    adouble x6 = states[ 5 ];
    adouble x7 = states[ 6 ];
    adouble x8 = states[ 7 ];

    adouble udf = controls[ 0 ];
    adouble udtau = controls[ 1 ];

    x1dot = x4;
    x2dot = x5;
    x3dot = x6;
    x4dot = -x7*sin(x3)-x4*0.1;
    x5dot = x7*cos(x3)-9.8-x5*0.1;
    x6dot = x8;
    x7dot = udf;
    x8dot = udtau;

    derivatives[ 0 ] = x1dot;
    derivatives[ 1 ] = x2dot;
    derivatives[ 2 ] = x3dot;
    derivatives[ 3 ] = x4dot;
    derivatives[ 4 ] = x5dot;
    derivatives[ 5 ] = x6dot;
    derivatives[ 6 ] = x7dot;
    derivatives[ 7 ] = x8dot;
}

////////////////////////////////////////////////////////////////////////////
///////////////////  Define the events function ////////////////////////////
////////////////////////////////////////////////////////////////////////////

void events(adouble* e, adouble* initial_states, adouble* final_states,
            adouble* parameters,adouble& t0, adouble& tf, adouble* xad,
            int iphase, Workspace* workspace)
{
    adouble x10 = initial_states[ 0 ];
    adouble x20 = initial_states[ 1 ];
    adouble x30 = initial_states[ 2 ];
    adouble x40 = initial_states[ 3 ];
    adouble x50 = initial_states[ 4 ];
    adouble x60 = initial_states[ 5 ];
    adouble x70 = initial_states[ 6 ];
    adouble x80 = initial_states[ 7 ];

    adouble x1f = final_states[ 0];
    adouble x2f = final_states[ 1];
    adouble x3f = final_states[ 2];
    adouble x4f = final_states[ 3];
    adouble x5f = final_states[ 4];
    adouble x6f = final_states[ 5];
    adouble x7f = final_states[ 6];
    adouble x8f = final_states[ 7];

    e[ 0 ] = x10;
    e[ 1 ] = x20;
    e[ 2 ] = x30;
    e[ 3 ] = x40;
    e[ 4 ] = x50;
    e[ 5 ] = x60;
    e[ 6 ] = x70;
    e[ 7 ] = x80;

    e[ 8 ] = x1f;
    e[ 9 ] = x2f;
    e[ 10 ] = x3f;
    e[ 11 ] = x4f;
    e[ 12 ] = x5f;
    e[ 13 ] = x6f;
    e[ 14 ] = x7f;
    e[ 15 ] = x8f;
}

void linkages( adouble* linkages, adouble* xad, Workspace* workspace)
{
    // No linkages as this is a single phase problem
}

void do_process()
{
    chrono::time_point<chrono::steady_clock> begin_time = chrono::steady_clock::now();

    problem.phases(1).bounds.lower.events   	<< py0,  pz0,  phi0,  vy0,  vz0, omegax0, thrust0, tau0, pyf, pzf, phif, vyf,vzf,omegaxf,thrustf,tauf;
    problem.phases(1).bounds.upper.events   	<< py0,  pz0,  phi0,  vy0,  vz0, omegax0, thrust0, tau0, pyf, pzf, phif, vyf,vzf,omegaxf,thrustf,tauf;

    problem.phases(1).guess.controls = u;
    problem.phases(1).guess.states = x;
    problem.phases(1).guess.time = t;

    psopt(solution, problem, algorithm);
    chrono::time_point<chrono::steady_clock> end_time = chrono::steady_clock::now();
    int timeconsumption = chrono::duration_cast<chrono::milliseconds>(end_time - begin_time).count();
    cout << "time consume mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm: " << timeconsumption<< endl;

    if (solution.error_flag) return;

    x = solution.get_states_in_phase(1);
    u = solution.get_controls_in_phase(1);
    t = solution.get_time_in_phase(1);

}

void interpolation()
{
    int interpolationpoints=floor(t(0,pointnumber-1)*controlfreq)+1;
    int leftnode=0,rightnode=1;
    double leftweight=0,rightweight=0;
    for(int i=0;i<interpolationpoints;i++)
    {
        if(t(0,leftnode)<=(double)i/controlfreq && (double)i/controlfreq<=t(0,rightnode))
        {
            leftweight=(t(0,rightnode)-(double)i/controlfreq)/(t(0,rightnode)-t(0,leftnode));
            rightweight=((double)i/controlfreq-t(0,leftnode))/(t(0,rightnode)-t(0,leftnode));
            for(int j=0;j<8;j++)
            {
                x_refined(j,i)=x(j,leftnode)*leftweight+x(j,rightnode)*rightweight;
            }
        } else if((double)i/controlfreq>(t.row(0))(rightnode))
        {
            while((double)i/controlfreq>(t.row(0))(rightnode))
            {
                leftnode=rightnode;
                rightnode++;
            }
            leftweight=(t(0,rightnode)-(double)i/controlfreq)/(t(0,rightnode)-t(0,leftnode));
            rightweight=((double)i/controlfreq-t(0,leftnode))/(t(0,rightnode)-t(0,leftnode));
            for(int j=0;j<8;j++)
            {
                x_refined(j,i)=x(j,leftnode)*leftweight+x(j,rightnode)*rightweight;
            }
        }
    }
}

// rostopic subscribe
void state_cb(const mavros_msgs::State::ConstPtr &msg){
    current_state = *msg;
}

bool hasGotImu = false;
void droneImu_cb(const sensor_msgs::Imu::ConstPtr &msg)
{
    drone_imu=*msg;
    rate_ini=drone_imu.angular_velocity.y;
}

void dronerpy_cb(const geometry_msgs::Vector3::ConstPtr &msg)
{
    currentRPYangle =*msg;
    theta_ini=currentRPYangle.y;
//    std::cout <<"currentRPYangle-----------theta_ini:"<<theta_ini<<endl;
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){ //由ConstPtr可以看到msg是指针的引用，因此*msg相当于对被引用的指针对象取值
    if (pose_initialized== false)
    {
        pos_drone_last=*msg;
        pose_initialized=true;
        return;
    }
    if(fabs((*msg).pose.position.x - pos_drone_last.pose.position.x) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.y - pos_drone_last.pose.position.y) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.z - pos_drone_last.pose.position.z) < MAX_POSITION_MEASURE_ERROR)
    {
        pos_drone_last = pos_drone;
        pos_drone = *msg;
        current_quaternion=Eigen::Quaterniond(pos_drone.pose.orientation.w,pos_drone.pose.orientation.x,pos_drone.pose.orientation.y,pos_drone.pose.orientation.z);
       if(pose_initialized&&vel_initialized)
       {
           if (fabs(pos_drone.header.stamp.nsec-vel_drone.header.stamp.nsec)<30e6)
           {
               posvel_updatesynflag=true;
           }
       }
    } else
    {
        pos_drone_last=*msg;
    }
}

void plane_vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg){
    vel_drone = *msg;
//    vxPlane_ini=filterDroneVelx.filter(vel_drone.twist.linear.x);
    vxPlane_ini=(vel_drone.twist.linear.x);
    filteredPlaneVelmsg.x=vxPlane_ini;
    vyPlane_ini=vel_drone.twist.linear.y;
    vzPlane_ini=vel_drone.twist.linear.z;
}

void relative_postwist_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    px_ini=(*msg).pose.pose.position.x;
    pz_ini=(*msg).pose.pose.position.z;
    vx_ini=(*msg).twist.twist.linear.x;
    vz_ini=(*msg).twist.twist.linear.z;
    currentupdateflag=true;
//    std::cout <<"ocpsolveriniconditons-----------px_ini:  " << px_ini <<"   vx_ini:  " << vx_ini<<"  pz_ini:  " << pz_ini <<"   vz_ini:  " << vz_ini<<
//    "   theta_ini:  " << theta_ini<< "   rate_ini:  " << rate_ini<<std::endl;

}
int main( int argc, char ** argv)
{
    ros::init(argc, argv, "minimizetime_control");
    ros::NodeHandle nh;
    ros::Rate rate(100);// it is the rate to check and process callback functions. the high rate is to avoid the latency to accept the pos and vel

    ros::Subscriber drone_imu_sub=nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data",10,droneImu_cb);
    ros::Subscriber plane_rpy_sub = nh.subscribe<geometry_msgs::Vector3>("drone/current_rpy",10,dronerpy_cb);
    ros::Subscriber car_position_sub = nh.subscribe<nav_msgs::Odometry>("current_relative_postwist",10,relative_postwist_cb); //车的pos+twist
    ros::Subscriber plane_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/local_position/velocity_local", 10, plane_vel_cb); //twist

    ros::Publisher controlstate_pub=nh.advertise<offb_posctl::controlstate>("ocp/control_state",10);
    ros::Publisher planeVel_pub=nh.advertise<geometry_msgs::Vector3>("filteredPlaneVel",10);

/////////////////////////////////// construct the ocp problem
    problem.name        		        = "minimize time Problem";
    problem.outfilename                 = "aggrecup.txt";
    problem.nphases   			        = 1;
    problem.nlinkages                   = 0;
    psopt_level1_setup(problem);

    problem.phases(1).nstates   		= 8;
    problem.phases(1).ncontrols 		= 2;
    problem.phases(1).nevents   		= 16;
    problem.phases(1).npath     		= 0;
    problem.phases(1).nodes                     << pointnumber;
//    problem.phases(1).nodes=(RowVectorXi(2)<<7,9).finished();
    psopt_level2_setup(problem, algorithm);

    problem.phases(1).bounds.lower.states   	<<  -1,  0.2,  -1.57, -10, -10,-10,  0,   -10;
    problem.phases(1).bounds.upper.states   	<<   3,  10,    1.57,  10,  10, 10,  19.6, 10;

    problem.phases(1).bounds.lower.controls 	<< -15, -15;
    problem.phases(1).bounds.upper.controls 	<<  15,  15;

    problem.phases(1).bounds.lower.StartTime    = 0.0;
    problem.phases(1).bounds.upper.StartTime    = 0.0;

    problem.phases(1).bounds.lower.EndTime      = 0.0;
    problem.phases(1).bounds.upper.EndTime      = 10.0;

    problem.integrand_cost 	= &integrand_cost;
    problem.endpoint_cost 	= &endpoint_cost;
    problem.dae 		    = &dae;
    problem.events 		    = &events;
    problem.linkages		= &linkages;

    MatrixXd x0(8,pointnumber-1) ;
    MatrixXd u0(2,pointnumber-1) ;
    MatrixXd t0(1,pointnumber-1) ;

    x0.row(0) = linspace(0.0,1.0, pointnumber-1);
    x0.row(1) = linspace(0.0,1.0, pointnumber-1);
    x0.row(2) = linspace(0.0,1.0, pointnumber-1);
    x0.row(3) = linspace(0.0,1.0, pointnumber-1);
    x0.row(4) = linspace(0.0,1.0, pointnumber-1);
    x0.row(5) = linspace(0.0,1.0, pointnumber-1);
    x0.row(6) = linspace(0.0,1.0, pointnumber-1);
    x0.row(7) = linspace(0.0,1.0, pointnumber-1);

    u0.row(0) = linspace(0.0,1.0, pointnumber-1);
    u0.row(1) = linspace(0.0,1.0, pointnumber-1);

    t0.row(0) = linspace(0.0,2.0, pointnumber-1);

    algorithm.nlp_method                  = "IPOPT";
    algorithm.scaling                     = "automatic";
    algorithm.derivatives                 = "automatic";
    algorithm.nlp_iter_max                = 1000;
    algorithm.nlp_tolerance               = 1.e-3;
    algorithm.collocation_method          = "trapezoidal";
    algorithm.print_level                 =0;
/////////////////////////////////// construct the ocp problem

    problem.phases(1).bounds.lower.events   	<< py0,  pz0,  phi0,  vy0,  vz0, omegax0, thrust0, tau0, pyf, pzf, phif, vyf,vzf,omegaxf,thrustf,tauf;
    problem.phases(1).bounds.upper.events   	<< py0,  pz0,  phi0,  vy0,  vz0, omegax0, thrust0, tau0, pyf, pzf, phif, vyf,vzf,omegaxf,thrustf,tauf;

    problem.phases(1).guess.controls = u0;
    problem.phases(1).guess.states = x0;
    problem.phases(1).guess.time = t0;
    psopt(solution, problem, algorithm);

    if (solution.error_flag) return 0;

    x = solution.get_states_in_phase(1);
    u = solution.get_controls_in_phase(1);
    t = solution.get_time_in_phase(1);
    cout<<x<<endl;

    while (ros::ok())
    {
        ros::spinOnce();// to examine the queues of the callback functions once
        if(currentupdateflag)
        {
            do_process();
//            cout<<"solution.error_flag:  "<<solution.error_flag<<endl;
            if(!solution.error_flag)
            {
                interpolation();
                controlstate_msg.discrepointpersecond=controlfreq;
                controlstate_msg.inicounter=1;// the first value is current state, we should use at least next state
                controlstate_msg.arraylength=floor(t(0,pointnumber-1)*controlfreq)+1;// becasue the first point's index starts from 0.
                controlstate_msg.thrustarray.clear();
                controlstate_msg.phiarray.clear();
                controlstate_msg.tauarray.clear();
                controlstate_msg.stateYarray.clear();
                controlstate_msg.stateZarray.clear();
                controlstate_msg.stateVYarray.clear();
                controlstate_msg.stateVZarray.clear();
                for(int i=0;i<controlstate_msg.arraylength;i++)
                {
                    controlstate_msg.thrustarray.push_back(x_refined(6,i));
                    controlstate_msg.phiarray.push_back(x_refined(2,i));
                    controlstate_msg.thetaarray.push_back(0);
                    controlstate_msg.stateXarray.push_back(0);
                    controlstate_msg.stateYarray.push_back(x_refined(0,i));
                    controlstate_msg.stateZarray.push_back(x_refined(1,i));
                    controlstate_msg.stateVYarray.push_back(x_refined(3,i));
                    controlstate_msg.stateVZarray.push_back(x_refined(4,i));
                    controlstate_msg.tauarray.push_back(x_refined(7,i));
                }
//                cout<<"controlstate_msg.arraylength----!!!!:"<<controlstate_msg.arraylength<<"  time:"<<t(0,pointnumber-1)<<endl;
                controlstate_pub.publish(controlstate_msg);
//                cout<<"controlstate_msg.stateXarray[0]-----------fffffff:"<<controlstate_msg.stateYarray[0]<<endl;
            }
            currentupdateflag= false;
//            planeVel_pub.publish(filteredPlaneVelmsg);
        }
//        rate.sleep(); // comment this to achieve the maximize refresh frequency
    }

    return 0;
}


/**
 * @example waveq3d/test/reflection_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/netcdf/netcdf_files.h>
#include <usml/waveq3d/waveq3d.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <boost/foreach.hpp>
#include <boost/progress.hpp>

BOOST_AUTO_TEST_SUITE(reflection_test)

using namespace boost ;
using namespace unit_test ;
using namespace numeric::ublas ;
using namespace usml::netcdf ;
using namespace usml::waveq3d ;

/**
 * @ingroup waveq3d_test
 * @{
 */

// TODO: reflect_flat_test disabled until reverb model delivered

/**
 * Monitor callbacks from reflection model.
 */
class reflection_callback {

public:
    eigenverb_collection* _collection ;
    double time ;
    double latitude ;
    bool surface ;
    bool bottom ;
    size_t surf_count ;
    size_t bot_count;

    /**
     * Initialize counter.
     */
    reflection_callback( eigenverb_collection* collection )
        : _collection(collection), time(0.0), latitude(),
          surface(false), bottom(false), surf_count(0), bot_count(0)
    {}

    virtual ~reflection_callback() {}

    bool check_count( size_t& old_count ) {
        bottom = surface = false ;
        if( surf_count != _collection->eigenverbs(1).size() ) {
            surf_count = _collection->eigenverbs(1).size() ;
            surface = true ;
        }
        if( bot_count != _collection->eigenverbs(0).size() ) {
            bot_count = _collection->eigenverbs(0).size() ;
            bottom = true ;
        }
        if( old_count != (surf_count + bot_count) ) {
            old_count = surf_count + bot_count ;
            if( surface ) {
                eigenverb_list elist = _collection->eigenverbs(1) ;
                eigenverb_list::iterator it = elist.end() ;
                --it ;
                latitude = it->position.latitude() ;
                time = it->time ;
            }
            else if( bottom ) {
                eigenverb_list elist = _collection->eigenverbs(0) ;
                eigenverb_list::iterator it = elist.end() ;
                --it ;
                latitude = it->position.latitude() ;
                time = it->time ;
            }
        }
        return (surface || bottom) ;
    }

};

/**
 * Bottom and surface reflection in a flat bottomed isovelocity ocean.
 * Constructs a geometry in which the changes in latitude
 * and travel time between bounces can be calculated analytically.
 * The following equations predict the path of a downwardly
 * steered ray, given a \f$ \Delta \theta \f$ = latitude change
 * between the surface and the bottom.
 * \f[
 *      s^2 = R_1^2 + R_2^2 - 2 R_1 R_2 cos( \Delta \theta )
 * \f]\f[
 *      sin(\gamma_s) = \frac{ R_1^2 - R_2^2 + s^2 }
 *                         { 2 R_1 s }
 * \f]\f[
 *      \tau = s / c
 * \f]
 * where:
 *      - \f$ R_1 \f$ = radius to ocean surface
 *      - \f$ R_2 \f$ = radius to ocean bottom
 *      - \f$ R_2 - R_1 \f$ = ocean depth
 *      - \f$ \Delta\theta \f$ = latitude change between
 *        the surface and the bottom.
 *      - \f$ s \f$ = path length from surface to bottom
 *      - \f$ \gamma_s \f$ = grazing angle at surface
 *        = ray launch angle
 *      - \f$ \gamma_b  \f$ = grazing angle at bottom
 *        = \f$ \gamma_s - \Delta\theta \f$
 *      - \f$ c      \f$ = sound speed
 *      - \f$ \tau   \f$ = travel time between the surface and the bottom.
 *
 * Selecting \f$ \gamma \f$ to make the latitude change exactly 0.1 degrees
 * yields the following test values.
 *
 *      - \f$ R_1 \f$ = 6378101.030201019 m
 *      - \f$ R_2 \f$ = \f$ R_1 \f$ - 1000 m
 *      - \f$ \Delta \theta \f$ = 0.1 deg
 *      - \f$ s   \f$ = 11,175.841460125 m
 *      - \f$ \gamma_s \f$ = 5.183617057 deg
 *      - \f$ \gamma_b \f$ = 5.083617057 deg
 *      - \f$ c      \f$ = 1500 m/sec
 *      - \f$ \tau   \f$ = 7.450560973 sec
 *
 * Uses a 100 millisecond time step so that the time and location of the
 * collision can be compared to the analytic values without breaking
 * into the guts of the reflection model.
 */
BOOST_AUTO_TEST_CASE( reflect_flat_test ) {
    cout << "=== reflection_test: reflect_flat_test ===" << endl ;
    try {
        const double src_lat = 45.0; // default to 45 degrees

        // initialize propagation model

        // Ensure static (global scope of test framework) set
        wposition::compute_earth_radius(src_lat) ;
        const double c0 = 1500.0 ;
        profile_model*  profile = new profile_linear(c0) ;
        boundary_model* surface = new boundary_flat() ;
        boundary_model* bottom  = new boundary_flat(1000.0) ;
        ocean_model ocean( surface, bottom, profile ) ;

        seq_log freq( 10.0, 10.0, 1 ) ;
        wposition1 pos( 45.0, -45.0, 0.0 ) ;
        seq_linear de( -5.183617057, 0.0, 1 ) ;  // steer down
        seq_linear az( 0.0, 0.0, 1 ) ;           // north
        const double time_step = 0.1 ;           // 100 msec
        const double max_time = 60.0 ;           // maximum travel time

        wave_queue wave( ocean, freq, pos, de, az, time_step ) ;
        eigenverb_collection verb_collection( 0 ) ;
        wave.add_eigenverb_listener( &verb_collection ) ;
        reflection_callback callback( &verb_collection ) ;
        size_t old_count = callback.surf_count + callback.bot_count ;
        double max_time_error = 0.0 ;
        double max_lat_error = 0.0 ;

        // initialize output to spreadsheet file

        const char* name = USML_TEST_DIR "/waveq3d/test/reflect_flat_test.csv" ;
        std::ofstream os(name) ;
        cout << "Writing tables to " << name << endl ;

        os << "t,"
           << "lat,lng,alt,"
           << "de,az,bot,srf,cst,"
           << "r,theta,phi,"
           << "rd,thd,phid,"
           << "mu,eta,nu,"
           << "mud,etad,nud,"
           << "c,dcdz"
           << endl ;
        os << std::scientific << std::showpoint << std::setprecision(18) ;

        cout << "time step = " << time_step << " secs" << endl ;

        // propagate rays to stimulate bottom and surface reflections

        while ( wave.time() < max_time ) {

            // write to spreadsheet file

            wvector1 ndir( wave.curr()->ndirection, 0, 0 ) ;
            double de, az ;
            ndir.direction( &de, &az ) ;

            os << wave.time() << ','
               << wave.curr()->position.latitude(0,0) << ','
               << wave.curr()->position.longitude(0,0) << ','
               << wave.curr()->position.altitude(0,0) << ','
               << de << "," << az << ","
               << wave.curr()->surface(0,0) << ','
               << wave.curr()->bottom(0,0) << ','
               << wave.curr()->caustic(0,0) << ','
               << wave.curr()->position.rho(0,0) << ','
               << wave.curr()->position.theta(0,0) << ','
               << wave.curr()->position.phi(0,0) << ','
               << wave.curr()->pos_gradient.rho(0,0) << ','
               << wave.curr()->pos_gradient.theta(0,0) << ','
               << wave.curr()->pos_gradient.phi(0,0) << ','
               << wave.curr()->ndirection.rho(0,0) << ','
               << wave.curr()->ndirection.theta(0,0) << ','
               << wave.curr()->ndirection.phi(0,0) << ','
               << wave.curr()->ndir_gradient.rho(0,0) << ','
               << wave.curr()->ndir_gradient.theta(0,0) << ','
               << wave.curr()->ndir_gradient.phi(0,0) << ','
               << wave.curr()->sound_speed(0,0) << ','
               << wave.curr()->sound_gradient.rho(0,0) << endl ;

            // move wavefront to next time step

            wave.step() ;

            // check location and time of reflections against analytic result

            if( callback.check_count( old_count ) ) {
                double predict_time = old_count * 7.450560973 ;
                double current_time = callback.time ;
                double predict_lat = 45.0 + old_count * 0.1 ;
                double current_lat = callback.latitude ;

                if ( callback.surface ) cout << "surface" ;
                if ( callback.bottom ) cout << "bottom " ;
                cout << " reflection at t=" << current_time
                     << " lat=" << current_lat
                     << endl ;

                double time_error = abs( current_time - predict_time ) ;
                max_time_error = max( time_error, max_time_error ) ;
                BOOST_CHECK_SMALL( time_error, 1e-4 ) ;

                double lat_error = abs( current_lat - predict_lat ) ;
                max_lat_error = max( lat_error, max_lat_error ) ;
                BOOST_CHECK_SMALL( lat_error, 1e-6 ) ;
            }
        }
        cout << "wave propagates for " << wave.time() << " secs" << endl
             << "max_time_error = " << max_time_error << " secs "
             << "max_lat_error = " << max_lat_error  << " deg " << endl ;
    } catch ( std::exception* except ) {
        BOOST_ERROR( except->what() ) ;
    }
}

/**
 * Modified version of the reflect_flat_test() in which the bottom
 * has a 1 degree up-slope in the latitude direction.  This change should
 * create a 2 degree increase in grazing angle for each reflection that
 * hits the bottom.
 */
BOOST_AUTO_TEST_CASE( reflect_slope_test ) {
    cout << "=== reflection_test: reflect_slope_test ===" << endl ;
    try {

        // initialize propagation model

        const double c0 = 1500.0 ;
        profile_model*  profile = new profile_linear(c0) ;
        boundary_model* surface = new boundary_flat() ;

        wposition1 slope_ref( 45.1, -45.0, 0.0 ) ;
        reflect_loss_model* slope_loss = new reflect_loss_rayleigh( reflect_loss_rayleigh::SILT ) ;
        boundary_model* bottom  = new boundary_slope(
            slope_ref, 1000.0, to_radians(1.0), 0.0, slope_loss ) ;

        ocean_model ocean( surface, bottom, profile ) ;

        seq_log freq( 10.0, 10.0, 1 ) ;
        wposition1 pos( 45.0, -45.0, 0.0 ) ;
        seq_linear de( -5.175034664, 0.0, 1 ) ;  // steer down
        seq_linear az( 0.0, 0.0, 1 ) ;           // north
        const double time_step = 0.001 ;

        wave_queue wave( ocean, freq, pos, de, az, time_step ) ;

        // initialize output to spreadsheet file

        const char* name = USML_TEST_DIR "/waveq3d/test/reflect_slope_test.csv" ;
        std::ofstream os(name) ;
        cout << "Writing tables to " << name << endl ;

        os << "t,"
           << "lat,lng,alt,"
           << "de,az,bot,surf,"
           << "r,theta,phi,"
           << "rd,thd,phid,"
           << "mu,eta,nu,"
           << "mud,etad,nud,"
           << "c,dcdz"
           << endl ;
        os << std::scientific << std::showpoint << std::setprecision(18) ;

        cout << "time step = " << time_step << " secs" << endl ;

        // propagate rays to stimulate bottom and surface reflections

        int bounce = 0 ;
        double old_de = de(0) ;
        while ( wave.time() < 25.0 ) {

            // write to spreadsheet file

            wvector1 ndir( wave.curr()->ndirection, 0, 0 ) ;
            double de, az ;
            ndir.direction( &de, &az ) ;

            os << wave.time() << ','
               << wave.curr()->position.latitude(0,0) << ','
               << wave.curr()->position.longitude(0,0) << ','
               << wave.curr()->position.altitude(0,0) << ','
               << de << "," << az << ","
               << wave.curr()->surface(0,0) << ','
               << wave.curr()->bottom(0,0) << ','
               << wave.curr()->position.rho(0,0) << ','
               << wave.curr()->position.theta(0,0) << ','
               << wave.curr()->position.phi(0,0) << ','
               << wave.curr()->pos_gradient.rho(0,0) << ','
               << wave.curr()->pos_gradient.theta(0,0) << ','
               << wave.curr()->pos_gradient.phi(0,0) << ','
               << wave.curr()->ndirection.rho(0,0) << ','
               << wave.curr()->ndirection.theta(0,0) << ','
               << wave.curr()->ndirection.phi(0,0) << ','
               << wave.curr()->ndir_gradient.rho(0,0) << ','
               << wave.curr()->ndir_gradient.theta(0,0) << ','
               << wave.curr()->ndir_gradient.phi(0,0) << ','
               << wave.curr()->sound_speed(0,0) << ','
               << wave.curr()->sound_gradient.rho(0,0) << endl ;

            // move wavefront to next time step

            wave.step() ;

            // check angle change for each reflection

            if ( old_de * de < 0.0 ) {
                ++bounce ;

                if ( old_de < 0.0 ) {
                    cout << "bottom  reflection at t=" << wave.time()
                         << " lat=" << wave.curr()->position.latitude(0,0)
                         << " old de=" << old_de << " new de=" << de
                         << " diff=" << (de+old_de)
                         << endl ;
                    BOOST_CHECK_SMALL( 2.0-(de+old_de), 0.001 ) ;
                } else {
                    cout << "surface reflection at t=" << wave.time()
                         << " lat=" << wave.curr()->position.latitude(0,0)
                         << " old de=" << old_de << " new de=" << de
                         << " diff=" << (old_de+de)
                         << endl ;
                    BOOST_CHECK_SMALL( old_de+de, 0.001 ) ;
                }
            }
            old_de = de ;
        }
        cout << "wave propagates for " << wave.time() << " secs" << endl ;

    } catch ( std::exception* except ) {
        BOOST_ERROR( except->what() ) ;
    }
}

/**
 * Reflect rays off of the ETOPO1 bottom near the Malta escarpment.
 * Ray path starts from 35:59N 16:00E, D/E=-20 deg AZ=270 deg.
 * A run for 90 seconds should propagate up the slope, turn around
 * and head back down the slope.  Because of slope features in the
 * latitude direction, the ray does not stay in the east-west plane.
 *
 * When the gcc -ffast-math compiler option is turned off, these results are
 * expected to be accurate to at least 0.09 percent.  With fast-math turned on,
 * the accuracy of the lat/long drops to 5e-5 percent, and about 6 meters
 * are lost on the accuracy in depth.
 */
BOOST_AUTO_TEST_CASE( reflect_grid_test ) {
    const char* csvname = USML_TEST_DIR "/waveq3d/test/reflect_grid_test.csv" ;
    const char* ncname = USML_TEST_DIR "/waveq3d/test/reflect_grid_test.nc" ;
    cout << "=== reflection_test: reflect_grid_test ===" << endl ;
    try {

        // define scenario parameters

        const double c0 = 1500.0 ;  // speed of sound

        const double lat1 = 35.5 ;  // Mediterranean sea
        const double lat2 = 36.5 ;  // malta escarpment
        const double lng1 = 15.25 ; // south-east of Sicily
        const double lng2 = 16.25 ;
        wposition::compute_earth_radius( (lat1+lat2)/2.0 ) ;

        wposition1 pos( 35.983333333, 16.0, -10.0 ) ;
        seq_linear de( -20.0, 1.0, 1 ) ;    // down
        seq_linear az( 270.0, 1.0, 1 ) ;    // west
        const double time_step = 0.1 ;
        const double time_max = 80.0 ;

        seq_log freq( 3000.0, 1.0, 1 ) ;

        // load bathymetry from ETOPO1 database

        cout << "load bathymetry" << endl ;
        boundary_model* bottom = new boundary_grid<double,2>( new netcdf_bathy(
    	    USML_DATA_DIR "/bathymetry/ETOPO1_Ice_g_gmt4.grd",
            lat1, lat2, lng1, lng2 ) ) ;

        // combine sound speed and bathymetry into ocean model

        profile_model*  profile = new profile_linear(c0) ;
        boundary_model* surface = new boundary_flat() ;
        ocean_model ocean( surface, bottom, profile ) ;

        // initialize output to spreadsheet file

        std::ofstream os(csvname) ;
        cout << "writting tables to " << csvname << endl ;

        os << "t,"
           << "lat,lng,alt,"
           << "de,az,surf,bot,"
           << "r,theta,phi,"
           << "rd,thd,phid,"
           << "mu,eta,nu,"
           << "mud,etad,nud,"
           << "c,dcdz"
           << endl ;
        os << std::scientific << std::showpoint << std::setprecision(18) ;

        cout << "time step = " << time_step << " secs" << endl ;

        // propagate rays & record to netCDF file

        wave_queue wave( ocean, freq, pos, de, az, time_step ) ;
        cout << "Writing wavefronts to " << ncname << endl ;
        wave.init_netcdf( ncname ) ;
        wave.save_netcdf() ;
        while ( wave.time() < time_max ) {

            // move wavefront to next time step

            wave.step() ;
            wave.save_netcdf() ;

            // write to spreadsheet file

            wvector1 ndir( wave.curr()->ndirection, 0, 0 ) ;
            double de, az ;
            ndir.direction( &de, &az ) ;

            os << wave.time() << ','
               << wave.curr()->position.latitude(0,0) << ','
               << wave.curr()->position.longitude(0,0) << ','
               << wave.curr()->position.altitude(0,0) << ','
               << de << "," << az << ","
               << wave.curr()->surface(0,0) << ','
               << wave.curr()->bottom(0,0) << ','
               << wave.curr()->position.rho(0,0) << ','
               << wave.curr()->position.theta(0,0) << ','
               << wave.curr()->position.phi(0,0) << ','
               << wave.curr()->pos_gradient.rho(0,0) << ','
               << wave.curr()->pos_gradient.theta(0,0) << ','
               << wave.curr()->pos_gradient.phi(0,0) << ','
               << wave.curr()->ndirection.rho(0,0) << ','
               << wave.curr()->ndirection.theta(0,0) << ','
               << wave.curr()->ndirection.phi(0,0) << ','
               << wave.curr()->ndir_gradient.rho(0,0) << ','
               << wave.curr()->ndir_gradient.theta(0,0) << ','
               << wave.curr()->ndir_gradient.phi(0,0) << ','
               << wave.curr()->sound_speed(0,0) << ','
               << wave.curr()->sound_gradient.rho(0,0) << endl ;
        }
        wave.close_netcdf() ;
        cout << "wave propagates for " << wave.time() << " secs" << endl ;

        // compare to prior runs

        #ifdef __FAST_MATH__
            const double position_accuracy = 5e-4 ;
        #else
            const double position_accuracy = 0.09 ;
        #endif
        BOOST_CHECK_CLOSE( wave.curr()->position.latitude(0,0), 36.169253160619995, position_accuracy ) ;
        BOOST_CHECK_CLOSE( wave.curr()->position.longitude(0,0), 16.012084836798909, position_accuracy ) ;

//        #ifdef __FAST_MATH__
//            BOOST_CHECK_SMALL( wave.curr()->position.altitude(0,0)+566.97501238062978, 6.0 ) ;
//        #else
//            BOOST_CHECK_SMALL( wave.curr()->position.altitude(0,0)+566.97501238062978, 1e-6 ) ;
//        #endif

    } catch ( std::exception* except ) {
        BOOST_ERROR( except->what() ) ;
    }
}

BOOST_AUTO_TEST_CASE( reflect_interp_spd_acc_test ){
    const char* csvname = USML_TEST_DIR "/waveq3d/test/reflect_interp_test.csv" ;
    cout << "=== reflection_test: reflect_interp_spd_acc_test ===" << endl ;

    // define scenario parameters

    const double c0 = 1500.0 ;  // speed of sound

    const double lat1 = 35.5 ;  // Mediterranean sea
    const double lat2 = 36.5 ;  // malta escarpment
    const double lng1 = 15.25 ; // south-east of Sicily
    const double lng2 = 16.25 ;
    wposition::compute_earth_radius( (lat1+lat2)/2.0 ) ;

    wposition1 pos( 35.983333333, 16.0, -10.0 ) ;
    seq_linear de( -20.0, 1.0, 1 ) ;    // down
    seq_linear az( 270.0, 1.0, 1 ) ;    // west
    const double time_step = 0.1 ;
    const double time_max = 80.0 ;

    seq_log freq( 3000.0, 1.0, 1 ) ;

    // load bathymetry from ETOPO1 database

    cout << "load bathymetry" << endl ;
    boundary_model* bottom = new boundary_grid<double,2>( new netcdf_bathy(
        USML_DATA_DIR "/bathymetry/ETOPO1_Ice_g_gmt4.grd",
        lat1, lat2, lng1, lng2 ) ) ;

    // combine sound speed and bathymetry into ocean model

    profile_model*  profile = new profile_linear(c0) ;
    boundary_model* surface = new boundary_flat() ;
    ocean_model ocean( surface, bottom, profile ) ;

    std::ofstream os(csvname) ;
    cout << "Writing tables to " << csvname << endl ;

    os << "t,"
       << "lat,lng,alt,"
       << "de,az,surf,bot,"
       << "r,theta,phi,"
       << "rd,thd,phid,"
       << "mu,eta,nu,"
       << "mud,etad,nud,"
       << "c,dcdz"
       << endl ;
    os << std::scientific << std::showpoint << std::setprecision(18) ;

    cout << "time step = " << time_step << " secs" << endl ;

    // propagate rays & record to netCDF file

    wave_queue wave( ocean, freq, pos, de, az, time_step ) ;
    while ( wave.time() < time_max ) {
        wave.step() ;

        wvector1 ndir( wave.curr()->ndirection, 0, 0 ) ;
        double de, az ;
        ndir.direction( &de, &az ) ;

        os << wave.time() << ','
           << wave.curr()->position.latitude(0,0) << ','
           << wave.curr()->position.longitude(0,0) << ','
           << wave.curr()->position.altitude(0,0) << ','
           << de << "," << az << ","
           << wave.curr()->surface(0,0) << ','
           << wave.curr()->bottom(0,0) << ','
           << wave.curr()->position.rho(0,0) << ','
           << wave.curr()->position.theta(0,0) << ','
           << wave.curr()->position.phi(0,0) << ','
           << wave.curr()->pos_gradient.rho(0,0) << ','
           << wave.curr()->pos_gradient.theta(0,0) << ','
           << wave.curr()->pos_gradient.phi(0,0) << ','
           << wave.curr()->ndirection.rho(0,0) << ','
           << wave.curr()->ndirection.theta(0,0) << ','
           << wave.curr()->ndirection.phi(0,0) << ','
           << wave.curr()->ndir_gradient.rho(0,0) << ','
           << wave.curr()->ndir_gradient.theta(0,0) << ','
           << wave.curr()->ndir_gradient.phi(0,0) << ','
           << wave.curr()->sound_speed(0,0) << ','
           << wave.curr()->sound_gradient.rho(0,0) << endl ;
   }
}

/**
 *
 */
BOOST_AUTO_TEST_CASE( bounce_threshold_test ) {
    cout << "=== reflection_test: bounce_threshold_test ===" << endl ;

    double depth = 1000.0 ;
    double c0 = 1500.0 ;
    double dt = 0.1 ;
    double max_time = 10.0 ;

    seq_linear freq( 900.0, 1.0, 1) ;
    seq_linear de(-90.0, 1.0, 90.0) ;
    seq_linear az(0.0, 30.0, 360.0) ;
    wposition1 src( 0.0, 0.0, -500.0 ) ;

    boundary_model* surface = new boundary_flat() ;
    boundary_model* bottom = new boundary_flat( depth, new reflect_loss_constant(0.0) ) ;
    profile_model* profile = new profile_linear( c0 ) ;
    ocean_model ocean( surface, bottom, profile ) ;

    wposition target(1, 1, 0.01, 0.0, -500.0) ;
    eigenray_collection loss( freq, src, de, az, dt, &target ) ;
    eigenverb_collection reverb( 0 ) ;
    wave_queue wave( ocean, freq, src, de, az, dt, &target ) ;
    wave.add_eigenray_listener(&loss) ;
    wave.add_eigenverb_listener(&reverb) ;
    wave.max_surface(3) ;
    wave.max_bottom(5) ;

    cout << "propagating for " << max_time << " seconds" << endl ;
    while ( wave.time() < max_time ) {
        wave.step() ;
    }

    eigenray_list* elist = loss.eigenrays(0,0) ;
    cout << "propagation complete, " << elist->size() << " rays were produced" << endl ;
    BOOST_FOREACH( eigenray e, *elist ) {
        BOOST_CHECK( e.bottom <= wave.max_bottom() ) ;
        BOOST_CHECK( e.surface <= wave.max_surface() ) ;
    }
    cout << "Checking eigenerbs for bottom interface" << endl ;
    eigenverb_list vlist = reverb.eigenverbs( eigenverb::BOTTOM ) ;
    BOOST_FOREACH( eigenverb v, vlist ) {
        BOOST_CHECK( v.bottom <= wave.max_bottom() ) ;
        BOOST_CHECK( v.surface <= wave.max_surface() ) ;
    }
    cout << "Checking eigenverbs for surface interface" << endl ;
    vlist = reverb.eigenverbs( eigenverb::SURFACE ) ;
    BOOST_FOREACH( eigenverb v, vlist ) {
        BOOST_CHECK( v.bottom <= wave.max_bottom() ) ;
        BOOST_CHECK( v.surface <= wave.max_surface() ) ;
    }

}
/// @}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/pack.hpp>
#include <algorithm>

#include <simd_test.hpp>

STF_CASE_TPL( "Check ratio constant behavior for packs", STF_NUMERIC_TYPES )
{
  namespace bs =  boost::simd;
  using pack_t  = bs::pack<T>;
  using hpack_t = bs::pack<T,pack_t::static_size/2>;
  using dpack_t = bs::pack<T,pack_t::static_size*2>;

  {
    auto are_correct = [](T e) { return e == bs::Ratio<T,7,21>(); };

    auto pack  = bs::Ratio<pack_t ,7,21>();
    auto hpack = bs::Ratio<hpack_t,7,21>();
    auto dpack = bs::Ratio<dpack_t,7,21>();

    STF_EXPECT(( std::all_of(pack.begin() , pack.end(), are_correct ) ));
    STF_EXPECT(( std::all_of(hpack.begin(),hpack.end(), are_correct ) ));
    STF_EXPECT(( std::all_of(dpack.begin(),dpack.end(), are_correct ) ));
  }
  {
    auto are_correct = [](T e) { return e == bs::Ratio<T,7>(); };

    auto pack  = bs::Ratio<pack_t,7>();
    auto hpack = bs::Ratio<hpack_t,7>();
    auto dpack = bs::Ratio<dpack_t,7>();

    STF_EXPECT(( std::all_of(pack.begin() , pack.end(), are_correct ) ));
    STF_EXPECT(( std::all_of(hpack.begin(),hpack.end(), are_correct ) ));
    STF_EXPECT(( std::all_of(dpack.begin(),dpack.end(), are_correct ) ));
  }
}

STF_CASE_TPL( "Check unit ratio constant behavior for packs", STF_NUMERIC_TYPES )
{
  namespace bs =  boost::simd;
  using pack_t  = bs::pack<T>;
  using hpack_t = bs::pack<T,pack_t::static_size/2>;
  using dpack_t = bs::pack<T,pack_t::static_size*2>;

  auto are_correct = [](T e) { return e == bs::Ratio<T,7>(); };

  auto pack  = bs::Ratio<pack_t ,7>();
  auto hpack = bs::Ratio<hpack_t,7>();
  auto dpack = bs::Ratio<dpack_t,7>();

  STF_EXPECT(( std::all_of(pack.begin() , pack.end(), are_correct ) ));
  STF_EXPECT(( std::all_of(hpack.begin(),hpack.end(), are_correct ) ));
  STF_EXPECT(( std::all_of(dpack.begin(),dpack.end(), are_correct ) ));
}