//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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_LINALG_FUNCTIONS_SCALAR_MNORM_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_SCALAR_MNORM_HPP_INCLUDED
#include <nt2/linalg/functions/mnorm.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/ismatrix.hpp>
#include <nt2/include/functions/isvector.hpp>
#include <nt2/include/functions/svd.hpp>
#include <nt2/include/functions/globalnorm2.hpp>
#include <nt2/include/functions/mnorm1.hpp>
#include <nt2/include/functions/mnorminf.hpp>
#include <nt2/include/functions/mnormfro.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/core/container/dsl/forward.hpp>
#include <nt2/core/functions/table/details/is_definitely_vector.hpp>
#include <boost/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/static_assert.hpp>

namespace nt2 {  namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( mnorm_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef typename meta::as_real<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return nt2::abs(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( mnorm_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef typename meta::as_real<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return nt2::abs(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( mnorm_, tag::cpu_
                            , (A0)
                            , ((ast_<A0, nt2::container::domain>))
                            )
  {
    typedef typename A0::value_type                   type_t;
    typedef typename meta::as_real<type_t>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      BOOST_ASSERT_MSG(nt2::ismatrix(a0), "a0 is not a matrix");
      typedef typename details::is_vector<typename A0::extent_type>::type choice_t;
      return eval(a0, choice_t());
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, boost::mpl::true_ const &) const
    {
      return globalnorm2(a0);
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, boost::mpl::false_ const &) const
    {
      return svd(a0)(1);
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, nt2::meta::indeterminate_ const &) const
    {
      if (isvector(a0))
        return globalnorm2(a0);
      else
        return svd(a0)(1);
    }

  };

  BOOST_DISPATCH_IMPLEMENT  ( mnorm_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              (scalar_<arithmetic_<A1> >)
                            )
  {
    typedef typename A0::value_type                   type_t;
    typedef typename meta::as_real<type_t>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const & a1) const
    {
      BOOST_ASSERT_MSG(nt2::ismatrix(a0), "a0 is not a matrix");
      BOOST_ASSERT_MSG((a1 == Two<A1>()) ||
                       (a1 == One<A1>()) ||
                       (a1 == Inf<A1>()) ||
                       (a1 == Mone<A1>()),
                       "mnorm is not defined for this parameters setting");
      if (a1 == Two<A1>()) return nt2::mnorm(a0);
      if (a1 == One<A1>()) return nt2::mnorm1(a0);
      if (a1 == Inf<A1>()) return nt2::mnorminf(a0);
      return nt2::mnormfro(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( mnorm_, tag::cpu_,
                                     (A0)(A1),
                                     ((ast_<A0, nt2::container::domain>))
                                     (unspecified_<A1>)
                                     )
  {
    typedef typename A0::value_type                   type_t;
    typedef typename meta::as_real<type_t>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const &a0, A1 const&) const
    {
      return eval(a0, A1());
    }

  private:
    BOOST_FORCEINLINE result_type eval(A0 const &a0
                                      , nt2::meta::as_<tag::Inf> const&) const
    {
      return nt2::mnorminf(a0);
    }
    BOOST_FORCEINLINE result_type eval(A0 const &a0
                                      , nt2::meta::as_<tag::inf_> const&) const
    {
      return nt2::mnorminf(a0);
    }
    BOOST_FORCEINLINE result_type eval(A0 const &a0
                                      , nt2::meta::as_<tag::One> const&) const
    {
      return nt2::mnorm1(a0);
    }
    BOOST_FORCEINLINE result_type eval(A0 const &a0
                                      , nt2::meta::as_<tag::one_> const&) const
    {
      return nt2::mnorm1(a0);
    }
    BOOST_FORCEINLINE result_type eval(A0 const &a0
                                      , nt2::meta::as_<tag::Two> const&) const
    {
      return mnorm(a0);
    }
    BOOST_FORCEINLINE result_type eval(A0 const &a0
                                      , nt2::meta::as_<tag::two_> const&) const
    {
      return mnorm(a0);
    }
    BOOST_FORCEINLINE result_type eval(A0 const &a0
                                      , nt2::meta::as_<tag::fro_> const&) const
    {
      return mnormfro(a0);
    }
  };

   // Selects globalnorm from static norm value
  BOOST_DISPATCH_IMPLEMENT  ( mnorm_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              (mpl_integral_< scalar_< fundamental_<A1> > >)
                            )
  {
    typedef typename A0::value_type                   type_t;
    typedef typename meta::as_real<type_t>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const &a0, A1 const&) const
    {
      // outside of Inf,  Minf,  One and Two no hope
      BOOST_ASSERT_MSG(nt2::ismatrix(a0), "a0 is not a matrix");
      BOOST_STATIC_ASSERT_MSG((A1::value == 2) ||
                              (A1::value == 1) ||
                              (A1::value == 0) ||
                              (A1::value == -1)
                             , "Norm value must be 1 2 0 (inf_) or -1 (fro_)" );
      return eval(a0, A1());
    }

    BOOST_FORCEINLINE result_type eval( A0 const &a0
                                      , boost::mpl::int_<2> const&
                                      ) const
    {
      return nt2::mnorm2(a0);
    }
    BOOST_FORCEINLINE result_type eval( A0 const &a0
                                      , boost::mpl::int_<1> const&
                                      ) const
    {
      return nt2::mnorm1(a0);
    }
    BOOST_FORCEINLINE result_type eval( A0 const &a0
                                      , boost::mpl::int_<-1> const&
                                      ) const
    {
      return nt2::mnormfro(a0);
    }
    BOOST_FORCEINLINE result_type eval( A0 const &a0
                                      , boost::mpl::int_<0> const&
                                      ) const
    {
      return nt2::mnorminf(a0);
    }
  };

} }

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RWLIBS_ALGORITHMS_NULLSPACEPROJECTION_HPP
#define RWLIBS_ALGORITHMS_NULLSPACEPROJECTION_HPP

#include <rw/kinematics/State.hpp>
#include <rw/math/Q.hpp>

#include <Eigen/Core>

namespace rw { namespace kinematics {
    class Frame;
}}    // namespace rw::kinematics
namespace rw { namespace models {
    class Device;
}}    // namespace rw::models

namespace rwlibs { namespace algorithms {

    /**
     * @brief Performs a projection in the null space of the device Jacobian to move joints away
     * from singularities.
     *
     * Given a device with redundant degrees of freedom, the null space of the Jacobian can be used
     * to move joints away from their limits. The problem of finding an optimal correction is
     * formulated as a quadratic optimization problem in which joint position, velocity and
     * acceleration limits are formulated as in the QP/XQP method.
     *
     * The basic NullSpaceProjection assumes all 6 degrees of freedom of the tool needs to be
     * constrainted.
     */
    class NullSpaceProjection
    {
      public:
        /**
         * @brief Construct NullSpaceProjection
         * @param device [in] Device to consider
         * @param controlFrame [in] Frame for which to calculate the Jacobian
         * @param state [in] State giving the assembly of the workcell
         * @param dt [in] Time step size
         */
        NullSpaceProjection (rw::models::Device* device, rw::kinematics::Frame* controlFrame,
                             const rw::kinematics::State& state, double dt);

        /**
         * @brief Destructor
         */
        virtual ~NullSpaceProjection ();

        /**
         * @brief Solves to give a joint motion moving away from joint limits while satisfying the
         * main task.
         *
         * Usage:
         * \code
         * NullSpaceProjection nps(device, device->getEnd(), state);
         * ...
         * ...
         * \\input Current configuration q, current velocity dq and desired new velocity dq1
         * Q qns = nps.solve(q, dq, dq1);
         * dq = dq1+qns;
         * \endcode
         *
         * @param q [in] Configuration of the device
         * @param dqcurrent [in] The current velocity
         * @param dq1 [in] The new velocity calculated e.g. by the XQPController
         */
        rw::math::Q solve (const rw::math::Q& q, const rw::math::Q& dqcurrent,
                           const rw::math::Q& dq1);

        /**
         * @brief Enumeration used to specify frame associated with the projection
         */
        enum ProjectionFrame {
            BaseFrame = 0, /** Robot Base Frame */
            ControlFrame   /**The Frame specified as the controlFrame*/
        };

        /**
         * @brief Specifies an initial projection of the Jacobian before calculating the null-space
         *
         * Given a projection matric \f$P\f$ it is multiplied with the device Jacobian as $\f$P
         * J\f$. This can be used to ignore degrees of freedom such as tool rool.
         *
         * \see XQPController::setProjection
         *
         * Usage: Setup for system ignoring tool roll
         * \code
         * XQPController* xqp = new XQPController(device, device->getEnd(), state, dt)
         * Eigen::Matrix<double,5,6 P;
         * for (int i = 0; i<5; i++)
         *     P(i,i) = 1;
         * xqp->setProjection(P, XQPController::ControlFrame);
         * \endcode
         *
         * @param P [in] The projection matrix
         * @param space [in] The space in which to apply the projection
         */
        void setProjection (const Eigen::MatrixXd& P, ProjectionFrame space);

        /**
         * @brief Sets the threshold for the joint limits
         *
         * Given an upper and a lower bound \f$upper\f$ and \f$lower\f$ the proximity of the joint
         * limits is defined as \f$upper-\tau (upper-lower)\f$ where \f$\tau\f$ is the threshold
         * specified here.
         *
         * @param threshold [in] Relative threshold for the joint limits
         */
        void setThreshold (double threshold);

        /**
         * @brief Sets the weight of the joint limits
         *
         * @param w [in] Weight of the joint limit
         */
        void setJointLimitsWeight (double w);

      private:
        /**
         * Calculate gradient for joint limit cost function
         */
        rw::math::Q getGradient (const rw::math::Q& q);

        /**
         * Calculate velocity limits associated with position, velocity and acceleration limits as
         * in the QPController
         */
        void calculateVelocityLimits (rw::math::Q& lower, rw::math::Q& upper, const rw::math::Q& q,
                                      const rw::math::Q& dq);

        rw::models::Device* _device;
        rw::kinematics::Frame* _controlFrame;
        rw::kinematics::State _state;
        int _dof;
        double _dt;

        rw::math::Q _qlower;
        rw::math::Q _qupper;
        rw::math::Q _dqlimit;
        rw::math::Q _ddqlimit;
        rw::math::Q _thresholdLower;
        rw::math::Q _thresholdUpper;
        Eigen::MatrixXd _P;
        ProjectionFrame _space;

        double _weightJointLimits;
    };

}}    // namespace rwlibs::algorithms

#endif /*RWLIBS_ALGORITHMS_NULLSPACEPROJECTION_HPP*/


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * 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.
 */

///
/// \file mod_prony_reduction.hpp
///
/// Sparse approximation of exponential sum by the modified Prony method.
///

#ifndef MXPFIT_MOD_PRONY_REDUCTION_HPP
#define MXPFIT_MOD_PRONY_REDUCTION_HPP

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <Eigen/SVD>

#include <mxpfit/exponential_sum.hpp>

namespace mxpfit
{

///
/// ### ModPronyReduction
///
/// \brief Find a truncated exponential sum function with smaller number of
///        terms by the modified Prony's method
///
/// \tparam T  Scalar type of exponential sum function.
///
/// For a given exponential sum function with real exponents,
///
/// \f[
///   f(t)=\sum_{j=1}^{n} c_{j}^{} e^{-a_{j}^{} t}, \quad
///   (a_{j} > 0),
/// \f]
///
/// and prescribed accuracy \f$\epsilon > 0,\f$ this class calculates truncated
/// exponential \f$\hat{f}(t)\f$ sum such that
///
/// \f[
///   \hat{f}(t)=\sum_{j=1}^{k} \hat{c}_{j}^{}e^{-\hat{a}_{j}^{} t}, \quad
///   \left| f(t)-\hat{f}(t) \right| < \epsilon,
/// \f]
///
/// where \f$k \leq n.\f$ Exponents are assumed to be real and sorted in
/// ascending order.
///
template <typename T>
class ModPronyReduction
{
public:
    using Scalar     = T;
    using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
    using Index      = Eigen::Index;

    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using ResultType = ExponentialSum<Scalar>;

    ///
    /// Compute truncated exponential sum \f$ \hat{f}(t) \f$
    ///
    /// \tparam DerivedF type of exponential sum inheriting ExponentialSumBase
    ///
    /// \param[in] orig original exponential sum function, \f$ f(t) \f$
    /// \param[in] threshold  prescribed accuracy \f$0 < \epsilon \ll 1\f$
    ///
    /// \return An instance of ExponentialSum represents \f$\hat{f}(t)\f$
    ///
    template <typename DerivedF>
    static ResultType compute(const ExponentialSumBase<DerivedF>& orig,
                              RealScalar threshold);
};

template <typename T>
template <typename DerivedF>
typename ModPronyReduction<T>::ResultType
ModPronyReduction<T>::compute(const ExponentialSumBase<DerivedF>& fn,
                              RealScalar threshold)
{
    using Eigen::numext::real;
    //
    // Express the exponential sum whose exponents are less than a threshold
    // (set to 1 here) with the linear combinations of fewer exponential
    // functions.
    //
    // Note that exponents are all positive, and sorted in ascending order.
    //
    Index m0 = 0;
    for (; m0 < fm.size(); ++m0)
    {
        if (fn.exponent(m0) >= T(1))
        {
            break;
        }
    }

    VectorType h(2 * m0);
    const auto w_small = fn.weights().head(m0);   // View
    const auto p_small = fn.exponents().head(m0); // View
    h(0)               = w_small.sum();
    h(1)               = -(w_small * p_small).sum();
    Index m            = 1;
    auto factorial     = T(1);
    for (; m < m0; ++m)
    {
        h(2 * m + 0) = (w_small * p_small.pow(2 * m + 0)).sum();
        h(2 * m + 1) = -(w_small * p_small.pow(2 * m + 1)).sum();
        factorial *= T((2 * m) * (2 * m + 1));
        if (-h(2 * m + 1) / factorial < eps)
        {
            // Taylor expansion converges with the tolerance eps.
            ++m;
            break;
        }
    }

    //
    // Construct a Hankel matrix from the sequence h, and solve the linear
    // equation, H q = b, with b = -h(m:2m-1).
    //
    MatrixType H(m, m);
    for (Index i = 0; i < m; ++i)
    {
        H.col(i) = h.segment(i, m);
    }
    VectorType b(-h.segment(m, m));
    VectorType q(H.colPivHouseholderQr().solve(b));

    //
    // Find the roots of the Prony polynomial,
    //
    // q(z) = \sum_{k=0}^{m-1} q_k z^{k}.
    //
    // The roots of q(z) can be obtained as the eigenvalues of the companion
    // matrix,
    //
    //     (0  0  ...  0 -p[0]  )
    //     (1  0  ...  0 -p[1]  )
    // C = (0  1  ...  0 -p[2]  )
    //     (.. .. ...  .. ..    )
    //     (0  0  ...  1 -p[m-1])
    //
    MatrixType& companion = H;
    companion.setZeros();
    companion.diagonal(-1).setOnes();
    companion.col(m - 1) = -q;
    Eigen::EigenSolver<MatrixType> es(companion,
                                      /*compute eigenvectors*/ false);
    // Vector gamma(arma::real(arma::eig_gen(companion)));

    // --- Update exponents & weights
    const Index keep = fn.size() - m0;
    ResultType ret(keep + m);
    ret.exponents().head(m)    = -es.eigenvalues().real();
    ret.exponents().tail(keep) = fn.exponents().tail(keep);
    //
    // Construct Vandermonde matrix from gamma
    //
    MatrixType V(2 * m, m);
    for (Index i = 0; i < m; ++i)
    {
        const auto z = real(gamma(i));
        V(0, i)      = T(1);
        for (Index j = 1; j < V.rows(); ++j)
        {
            V(j, i) = V(j - 1, i) * z; // z[i]**j
        }
    }
    //
    // Solve overdetermined Vandermonde system,
    //
    // V(0:2m-1,0:m-1) w(0:m-1) = h(0:2m-1)
    //
    // by the least square method.
    //
    ret.weights().head(m)    = V.colPivHouseholderQr().solve(h.head(2 * m));
    ret.weights().tail(keep) = fn.weights().tail(keep);

    return ret;
}
/// \internal
///
/// ### reduce_terms_with_small_exponents
///
/// Find shorter exponential sum approximation by removing terms with small
/// exponents using the modified Prony's method.
///
template <typename T>
GaussianSum<T> reduce_terms_with_small_exponents(const GaussianSum<T>& gs_orig,
                                                 T eps)
{
    using UIndex  = arma::uword;
    using SUIndex = arma::sword;
    using Vector  = arma::Col<T>;
    using Matrix  = arma::Mat<T>;
    //
    // Express the sum of Gaussians whose exponents are less than a threshold
    // (set to 1 here) with the linear combinations of fewer Gaussians.
    //
    // Note that exponents are all positive, and sorted in ascending order.
    //
    UIndex m0 = 0;
    for (; m0 < gs_orig.size(); ++m0)
    {
        if (gs_orig.exponent(m0) >= T(1))
        {
            break;
        }
    }

    Vector h(2 * m0);
    const auto w_small = gs_orig.weights().head(m0);   // View
    const auto p_small = gs_orig.exponents().head(m0); // View
    h(0)               = arma::sum(w_small);
    h(1)               = -arma::sum(w_small % p_small);
    UIndex m           = 1;
    auto factorial     = T(1);
    for (; m < m0; ++m)
    {
        h(2 * m + 0) = arma::sum(w_small % arma::pow(p_small, 2 * m + 0));
        h(2 * m + 1) = -arma::sum(w_small % arma::pow(p_small, 2 * m + 1));
        factorial *= T((2 * m) * (2 * m + 1));
        if (-h(2 * m + 1) / factorial < eps)
        {
            // Taylor expansion converges with the tolerance eps.
            ++m;
            break;
        }
    }

    //
    // Construct a Hankel matrix from the sequence h, and solve the linear
    // equation, H q = b, with b = -h(m:2m-1).
    //
    Matrix H(m, m);
    for (UIndex i = 0; i < m; ++i)
    {
        H.col(i) = h.subvec(i, i + m - 1);
    }
    Vector b(-h.subvec(m, 2 * m - 1));
    Vector q(arma::solve(H, b));
    //
    // Find the roots of the Prony polynomial,
    //
    // q(z) = \sum_{k=0}^{m-1} q_k z^{k}.
    //
    // The roots of q(z) can be obtained as the eigenvalues of the companion
    // matrix,
    //
    //     (0  0  ...  0 -p[0]  )
    //     (1  0  ...  0 -p[1]  )
    // C = (0  1  ...  0 -p[2]  )
    //     (.. .. ...  .. ..    )
    //     (0  0  ...  1 -p[m-1])
    //
    Matrix companion(m, m, arma::fill::zeros);
    companion.diag(-1).ones();
    companion.col(m - 1) = -q;
    Vector gamma(arma::real(arma::eig_gen(companion)));

    // --- Update exponents & weights
    auto keep = gs_orig.size() - m0;
    GaussianSum<T> gs(keep + m);
    gs.exponents_unsafe().head(m)    = -gamma;
    gs.exponents_unsafe().tail(keep) = gs_orig.exponents().tail(keep);
    //
    // Construct Vandermonde matrix from gamma
    //
    Matrix V(2 * m, m);
    for (UIndex i = 0; i < m; ++i)
    {
        const auto z = gamma(i);
        V(0, i)      = T(1);
        for (UIndex j = 1; j < V.n_rows; ++j)
        {
            V(j, i) = V(j - 1, i) * z; // z[i]**j
        }
    }
    //
    // Solve overdetermined Vandermonde system,
    //
    // V(0:2m-1,0:m-1) w(0:m-1) = h(0:2m-1)
    //
    // by the least square method.
    //
    gs.weights_unsafe().head(m)    = arma::solve(V, h.head(2 * m));
    gs.weights_unsafe().tail(keep) = gs_orig.weights().tail(keep);

    return gs;
}

} // namespace mxpfit

#endif /* MXPFIT_MOD_PRONY_REDUCTION_HPP */


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>

int
main()
{
  using namespace boost;
  typedef adjacency_list<vecS, vecS, directedS, no_property,
    property< edge_weight_t, int, property< edge_weight2_t, int > > > Graph;
  const int V = 5;
  typedef std::pair < int, int >Edge;
  Edge edge_array[] =
    { Edge(0, 1), Edge(0, 4), Edge(0, 2), Edge(1, 3), Edge(1, 4),
    Edge(2, 1), Edge(3, 2), Edge(3, 0), Edge(4, 3)
  };
  const std::size_t E = sizeof(edge_array) / sizeof(Edge);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle the iterator constructor
  Graph g(V);
  for (std::size_t j = 0; j < E; ++j)
    add_edge(edge_array[j].first, edge_array[j].second, g);
#else
  Graph g(edge_array, edge_array + E, V);
#endif

  property_map < Graph, edge_weight_t >::type w = get(edge_weight, g);
  int weights[] = { 3, -4, 8, 1, 7, 4, -5, 2, 6 };
  int *wp = weights;

  graph_traits < Graph >::edge_iterator e, e_end;
  for (boost::tie(e, e_end) = edges(g); e != e_end; ++e)
    w[*e] = *wp++;

  std::vector < int >d(V, std::numeric_limits < int >::max());
  int D[V][V];
  johnson_all_pairs_shortest_paths(g, D, distance_map(&d[0]));

  std::cout << "     ";
  for (int k = 0; k < V; ++k)
    std::cout << std::setw(5) << k;
  std::cout << std::endl;
  for (int i = 0; i < V; ++i) {
    std::cout << i << " -> ";
    for (int j = 0; j < V; ++j) {
      if (D[i][j] > 20 || D[i][j] < -20)
        std::cout << std::setw(5) << "inf";
      else
        std::cout << std::setw(5) << D[i][j];
    }
    std::cout << std::endl;
  }

  std::ofstream fout("figs/johnson-eg.dot");
  fout << "digraph A {\n"
    << "  rankdir=LR\n"
    << "size=\"5,3\"\n"
    << "ratio=\"fill\"\n"
    << "edge[style=\"bold\"]\n" << "node[shape=\"circle\"]\n";

  graph_traits < Graph >::edge_iterator ei, ei_end;
  for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    fout << source(*ei, g) << " -> " << target(*ei, g)
      << "[label=" << get(edge_weight, g)[*ei] << "]\n";

  fout << "}\n";
  return 0;
}


//
// utils.hpp
//
// Copyright (c) 2018 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#ifndef UTILS
#define UTILS

#include<stdlib.h>
#include<math.h>
#include<cmath>
#include<iostream>
#include<vector>
#include<numeric>
#include<memory>
#include<random>
#include<iomanip>
#include<fstream>
#include<limits>
#include <boost/lexical_cast.hpp>
#include<Eigen/Dense>
#include<Eigen/Core>
#include<Eigen/LU>
#include <Eigen/Cholesky>

template<class T>
void readCSV(std::string filename, std::vector<std::vector<T> > &matrix, std::vector<std::string> *rowNamesPointer=nullptr, std::vector<std::string> *columnNamesPointer=nullptr){
    matrix.clear();
    if(rowNamesPointer != nullptr){
        (*rowNamesPointer).clear();
    }
    if(columnNamesPointer != nullptr){
        (*columnNamesPointer).clear();
    }
    std::ifstream inputText(filename);
    if(!inputText){
        std::cout<<"Cannot open Csvfile";
        exit(1);
    }
    std::string str;
    // row
    for(int i=0; std::getline(inputText,str); i++){
        std::vector<T> vec;
        std::string token;
        std::istringstream stream(str);

        //column name
        if(i==0 && columnNamesPointer!=nullptr){
            //column
            for(int j=0; std::getline(stream,token,','); j++){
                if(j==0 && rowNamesPointer!=nullptr)continue;
                (*columnNamesPointer).push_back(token);
            }
            continue;
        }
        //column
        for(int j=0; std::getline(stream,token,','); j++){
            if(j==0 && rowNamesPointer!=nullptr)(*rowNamesPointer).push_back(token);
            else{
                try{
                    vec.push_back(boost::lexical_cast<T>(token));
                }catch(...){
                    std::cout<<"row:"<<i<<" column:"<<j<<" value:"<<token<<" error";
                    exit(0);
                }
            }
        }
        matrix.push_back(vec);
    }
}

template<typename T>
void parseCSV2Eigen(std::string filename, Eigen::Matrix<T, Eigen::Dynamic, 1> &eigenVector, std::vector<std::string> *rowNamesPointer=nullptr, std::vector<std::string> *columnNamesPointer=nullptr){
    std::vector<std::vector<T> > matrix;
    readCSV<T>(filename, matrix, rowNamesPointer, columnNamesPointer);
    eigenVector = Eigen::Matrix<T, Eigen::Dynamic, 1>(matrix.size());
    for (int i = 0; i < matrix.size(); i++){
        eigenVector(i) = matrix[i][0];
    }
}

template<typename T>
void parseCSV2Eigen(std::string filename, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &eigenMatrix, std::vector<std::string> *rowNamesPointer=nullptr, std::vector<std::string> *columnNamesPointer=nullptr){
    std::vector<std::vector<T> > matrix;
    readCSV<T>(filename, matrix, rowNamesPointer, columnNamesPointer);
    eigenMatrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(matrix.size(), matrix[0].size());
    for (int i = 0; i < matrix.size(); i++){
        eigenMatrix.row(i) = Eigen::Matrix<T, Eigen::Dynamic, 1>::Map(&matrix[i][0], matrix[0].size());
    }
}

template<class T>
void convertUnique(const std::vector<T> &v) {
    std::sort(v.begin(), v.end());
    v.erase(std::unique(v.begin(), v.end()), v.end());
}

template<typename T>
double logdet(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &matrix){
    return 2.0 * Eigen::LLT<Eigen::MatrixXd>(matrix).matrixL().toDenseMatrix().diagonal().array().log().sum();
}

template<typename T>
double logSumExp(const Eigen::Matrix<T, Eigen::Dynamic, 1> &logVector){
    double constant(logVector.maxCoeff());
    double sumexp((logVector.array() - constant).array().exp().sum());
    return log(sumexp) + constant;
}

template<typename T>
Eigen::VectorXd normalizeWithLogSumExp(const Eigen::Matrix<T, Eigen::Dynamic, 1> &logVector){
    Eigen::VectorXd resVector = (logVector.array() - logVector.maxCoeff()).array().exp();
    return resVector/resVector.sum();
}

template<typename T>
void eraseVectorElement(std::vector<T> &vector, unsigned int idx){
    vector.erase(vector.begin() + idx);
}

const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", "\n");

template<typename T>
void outputEigenMatrix(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &matrix, std::string filename){
    std::ofstream stream(filename);
    stream<<matrix.format(CSVFormat);
    stream.close();
 }

template<typename T>
void outputVectorEigenVector(const std::vector<Eigen::Matrix<T, Eigen::Dynamic, 1>> &vectorVector, std::string filename){
    std::ofstream stream(filename);
    for(int i=0; i<vectorVector.size(); i++){
        // Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> tempMatrix(vectorVector[i].transpose());
        // stream<<tempMatrix.format(CSVFormat);
        stream<<vectorVector[i].transpose().format(CSVFormat)<<std::endl;
        // std::cout<<tempMatrix.format(CSVFormat);
    }
    stream.close();
 }

template<typename T>
void outputEigenMatrixForDebug(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &matrix, std::string msg){
    std::ofstream stream;
    stream.open("./check", std::ios::app);
    stream<<msg<<std::endl;
    stream<<matrix.format(CSVFormat);
    stream<<std::endl;
    stream.close();
}

template<typename T>
void outputVector(const std::vector<T> &vector, std::string filename){
    std::ofstream stream;
    stream.open(filename, std::ios::out);
    stream<<std::setprecision(std::numeric_limits<double>::max_digits10);
    for(int i=0;i<vector.size();i++){
        stream<<vector[i];
        stream<<std::endl;
    }
    stream.close();
}

#endif


/*
 Deterministic Bayesian Sparse Linear Mixed Model (DBSLMM)
 Copyright (C) 2019  Sheng Yang and Xiang Zhou

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

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

 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <vector>
#include <string>

#include <iostream>
#include <fstream>
#include <sstream>
#include <sys/stat.h>
#include <sys/types.h>
#include <armadillo>
#include "../include/calc_asymptotic_variance.hpp"
#include "../include/dbslmm.hpp"

using namespace std;
using namespace arma;

int main(int argc, char * argv[])
{
  DBSLMM cDB;
  PARAM cPar;
  cDB.Assign(argc, argv, cPar);

  //int nchr = 22; //number of chromosomes
  int nchr = 1;
  //initialize a arma::field to store outputs for var calcs!

  arma::field < arma::mat> training(5000, 5); //5000 is always bigger than the number of LD BLocks in the genome
  arma::field < arma::mat> test(5000, 5);
  double sigma2_s = cPar.h / (double)cPar.nsnp;
  unsigned int row_total = 0; //initialize counter for number of rows in field
  for (int i = 0; i < nchr; ++i){
    //int i = 0;
    int chr = i + 1;
    std::string filetr ("Chr" + std::to_string(chr) + "_training.dat");
    std::string filete ("Chr" + std::to_string(chr) + "_test.dat");
    arma::field <arma::mat > tr;
    arma::field <arma::mat > te;
    tr.load(filetr);
    cout << filetr << endl;
    cout << "number of elements in tr: " << tr.n_elem << endl;
    cout << "number of rows in tr: " << tr.n_rows << endl;
    cout << "number of columns in tr: " << tr.n_cols << endl;

    te.load(filete);
    cout << filete << endl;
    cout << "number of elements in te: " << te.n_elem << endl;
    cout << "number of rows in te: " << te.n_rows << endl;
    cout << "number of columns in te: " << te.n_cols << endl;

    unsigned int rows_in_block = te.n_rows;
    training.rows(row_total, row_total+rows_in_block - 1) = tr;
    test.rows(row_total, row_total+rows_in_block - 1) = te;
    row_total = row_total + rows_in_block;

//    training.row(i) = assembleMatrices(tr);//store in a two-dimensional field
//    test.row(i) = assembleMatrices(te);
  }
  training = training.rows(0, row_total - 1);
  cout << "training has this many rows: " << row_total << endl;
  test = test.rows(0, row_total - 1);
  cout << "test has this many rows: " << row_total << endl;
  //var calcs here!
  //1. assemble genome-wide matrices from "training" & "test"
  arma::field < arma::mat > mats_training = assembleMatrices(training);
  arma::field < arma::mat > mats_test = assembleMatrices(test);
  //2. input matrices to calc_asymptotic_variance
  cout << "Starting asymptotic variance calculations..." << endl;
  arma::mat vv = calc_asymptotic_variance(mats_training(2), //Sigma_ll
                                          arma::trans(mats_training(1)), // Sigma_ls
                                          mats_training(0), //Sigma_ss
                                          training.col(0),
                                          sigma2_s,
                                          cPar.n,
                                          mats_test(4), //X_l
                                          mats_test(3)); // X_s
  //3. write diagonal of var to a csv file
  arma::vec vd = diagvec(vv);
  vd.save("out.csv", csv_ascii);

  return EXIT_SUCCESS;
}


#include "akumuli_def.h"
#include "anomalydetector.h"
#include "hashfnfamily.h"
#include "queryprocessor_framework.h"

#include <random>
#include <stdexcept>

#include <boost/exception/all.hpp>

namespace Akumuli {
namespace QP {

//                          //
//      CountingSketch      //
//                          //

struct CountingSketch {
    HashFnFamily const& hashes_;
    const u32 N;
    const u32 K;
    double sum_;
    std::vector<std::vector<double>> tables_;

    CountingSketch(HashFnFamily const& hf)
        : hashes_(hf)
        , N(hf.N)
        , K(hf.K)
        , sum_(0.0)
    {
        for (u32 i = 0u; i < N; i++) {
            std::vector<double> row;
            row.resize(K, 0.0);
            tables_.push_back(std::move(row));
        }
    }

    CountingSketch(CountingSketch const& cs)
        : hashes_(cs.hashes_)
        , N(cs.N)
        , K(cs.K)
        , sum_(cs.sum_)
    {
        for (auto ixrow = 0u; ixrow < N; ixrow++) {
            std::vector<double> row;
            row.resize(K, 0.0);
            std::vector<double> const& rcs = cs.tables_[ixrow];
            for (auto col = 0u; col < K; col++) {
                row[col] = rcs[col];
            }
            tables_.push_back(std::move(row));
        }
    }

    void _update_sum() {
        sum_ = 0.0;
        for (auto val: tables_[0]) {
            sum_ += val;
        }
    }

    void add(u64 id, double value) {
        sum_ += value;
        for (u32 i = 0; i < N; i++) {
            // calculate hash from id to K
            u32 hash = hashes_.hash(i, id);
            tables_[i][hash] += value;
        }
    }

    //! Second moment estimator
    double estimateF2() const {
        std::vector<double> results;
        auto f = 1./(K - 1);
        for (u32 i = 0u; i < N; i++) {
            double rowsum = std::accumulate(tables_[i].begin(), tables_[i].end(), 0.0, [](double acc, double val) {
                return acc + val*val;
            });
            double res = K*f*sqrt(rowsum) - f*sum_*sum_;
            results.push_back(res);
        }
        std::sort(results.begin(), results.end());
        return results[N/2];
    }

    //! Unbiased value estimator
    double estimate(u64 id) const {
        std::vector<double> results;
        for (u32 i = 0u; i < N; i++) {
            u32 hash = hashes_.hash(i, id);
            double value = tables_[i][hash];
            double estimate = (value - sum_/K)/(1. - 1./K);
            results.push_back(estimate);
        }
        std::sort(results.begin(), results.end());
        return results[N/2];
    }

    //! current sketch <- absolute difference between two arguments
    void diff(CountingSketch const& lhs, CountingSketch const& rhs) {
        for (auto ixrow = 0u; ixrow < N; ixrow++) {
            std::vector<double>& row = tables_[ixrow];
            std::vector<double> const& lrow = lhs.tables_[ixrow];
            std::vector<double> const& rrow = rhs.tables_[ixrow];
            for (auto col = 0u; col < K; col++) {
                row[col] = std::fabs(lrow[col] - rrow[col]);
            }
        }
        _update_sum();
    }

    //! Add sketch
    void add(CountingSketch const& val) {
        for (auto ixrow = 0u; ixrow < N; ixrow++) {
            std::vector<double>& row = tables_[ixrow];
            std::vector<double> const& rval = val.tables_[ixrow];
            for (auto col = 0u; col < K; col++) {
                row[col] = row[col] + rval[col];
            }
        }
        _update_sum();
    }

    //! Substract sketch
    void sub(CountingSketch const& val) {
        for (auto ixrow = 0u; ixrow < N; ixrow++) {
            std::vector<double>& row = tables_[ixrow];
            std::vector<double> const& rval = val.tables_[ixrow];
            for (auto col = 0u; col < K; col++) {
                row[col] = row[col] - rval[col];
            }
        }
        _update_sum();
    }

    //! Multiply sketch by value
    void mul(double value) {
        for (auto ixrow = 0u; ixrow < N; ixrow++) {
            std::vector<double>& row = tables_[ixrow];
            for (auto col = 0u; col < K; col++) {
                row[col] *= value;
            }
        }
        _update_sum();
    }

    //! Multiply by another sketch
    void mul(CountingSketch const& value) {
        for (auto ixrow = 0u; ixrow < N; ixrow++) {
            std::vector<double>& row = tables_[ixrow];
            std::vector<double> const& rval = value.tables_[ixrow];
            for (auto col = 0u; col < K; col++) {
                row[col] = row[col] * rval[col];
            }
        }
        _update_sum();
    }

    //! Divide by another sketch
    void div(CountingSketch const& value) {
        for (auto ixrow = 0u; ixrow < N; ixrow++) {
            std::vector<double>& row = tables_[ixrow];
            std::vector<double> const& rval = value.tables_[ixrow];
            for (auto col = 0u; col < K; col++) {
                row[col] = row[col] / rval[col];
            }
        }
        _update_sum();
    }
};

//                          //
//      PreciseCounter      //
//                          //

struct PreciseCounter {
    std::unordered_map<u64, double> table_;

    //! C-tor. Parameter `hf` is unused for the sake of interface unification.
    PreciseCounter(HashFnFamily const& hf) {
    }

    PreciseCounter(PreciseCounter const& cs)
        : table_(cs.table_)
    {
    }

    void add(u64 id, double value) {
        table_[id] += value;
    }

    //! Unbiased value estimator
    double estimate(u64 id) const {
        auto it = table_.find(id);
        if (it != table_.end()) {
            return it->second;
        }
        return 0.;
    }

    //! Second moment estimator
    double estimateF2() const {
        double sum = std::accumulate(table_.begin(), table_.end(), 0.0,
                                     [](double acc, std::pair<u64, double> pval) {
            return acc + pval.second*pval.second;
        });
        return sqrt(sum);
    }

    //! current sketch <- absolute difference between two arguments
    void diff(PreciseCounter const& lhs, PreciseCounter const& rhs) {
        const std::unordered_map<u64, double> *small, *large;
        if (lhs.table_.size() < rhs.table_.size()) {
            small = &lhs.table_;
            large = &rhs.table_;
        } else {
            small = &rhs.table_;
            large = &lhs.table_;
        }
        table_.clear();
        // Scan largest
        for (auto it = large->begin(); it != large->end(); it++) {
            auto small_it = small->find(it->first);
            double val = 0.;
            if (small_it != small->end()) {
                val = small_it->second;
            }
            table_[it->first] = std::fabs(it->second - val);
        }
    }

    //! Add sketch
    void add(PreciseCounter const& val) {
        for(auto it = val.table_.begin(); it != val.table_.end(); it++) {
            table_[it->first] += it->second;
        }
    }

    //! Substract sketch
    void sub(PreciseCounter const& val) {
        for(auto it = val.table_.begin(); it != val.table_.end(); it++) {
            table_[it->first] -= it->second;
        }
    }

    //! Multiply sketch by value
    void mul(double value) {
        for(auto it = table_.begin(); it != table_.end(); it++) {
            it->second *= value;
        }
    }

    //! Multiply
    void mul(PreciseCounter const& val) {
        for(auto it = val.table_.begin(); it != val.table_.end(); it++) {
            table_[it->first] *= it->second;
        }
    }

    //! Divide
    void div(PreciseCounter const& val) {
        for(auto it = val.table_.begin(); it != val.table_.end(); it++) {
            table_[it->first] /= it->second;
        }
    }
};

//                              //
//      SMASlidingWindow        //
//                              //

static double checked_inv(u32 depth) {
    if (depth == 0) {
        NodeException err("Sliding window depth can't be zero.");
        BOOST_THROW_EXCEPTION(err);
    }
    return 1.0/depth;
}

//! Simple moving average implementation
template<class Frame>
struct SMASlidingWindow {
    typedef std::unique_ptr<Frame> PFrame;
    PFrame             sma_;
    const u32          depth_;
    const double       mul_;
    std::deque<PFrame> queue_;

    SMASlidingWindow(u32 depth)
        : depth_(depth)
        , mul_(checked_inv(depth))
    {
    }

    void add(PFrame sketch) {
        if (!sma_) {
            sma_.reset(new Frame(*sketch));
            queue_.push_back(std::move(sketch));
        } else {
            sma_->add(*sketch);
            queue_.push_back(std::move(sketch));
            if (queue_.size() > depth_) {
                auto removed = std::move(queue_.front());
                queue_.pop_front();
                sma_->sub(*removed);
            }
        }
    }

    PFrame forecast() const {
        PFrame res;
        if (queue_.size() < depth_) {
            // return empty response
            return std::move(res);
        }
        res.reset(new Frame(*sma_));
        res->mul(mul_);
        return std::move(res);
    }
};

//                              //
//      EWMASlidingWindow       //
//                              //

//! Exponentialy weighted moving average implementation
template<class Frame>
struct EWMASlidingWindow {
    typedef std::unique_ptr<Frame> PFrame;
    PFrame               ewma_;
    const double         decay_;
    int                  counter_;

    EWMASlidingWindow(double alpha)
        : decay_(alpha)
        , counter_(0)
    {
    }

    void add(PFrame sketch) {
        if (!ewma_) {
            ewma_.reset(new Frame(*sketch));
            counter_ = 1;
        } else if (counter_ < 10) {
            ewma_->add(*sketch);
            counter_++;
            if (counter_ == 10) {
                ewma_->mul(0.1);
            }
        } else {
            sketch->mul(decay_);
            ewma_->mul(1.0 - decay_);
            ewma_->add(*sketch);
        }
    }

    PFrame forecast() const {
        PFrame res;
        if (counter_ < 10) {
            // return empty response
            return std::move(res);
        }
        res.reset(new Frame(*ewma_));
        return std::move(res);
    }
};

//                                          //
//      DoubleExpSmoothingSlidingWindow     //
//                                          //

//! Holt-Winters moving average implementation
template<class Frame>
struct DoubleExpSmoothingSlidingWindow {
    typedef std::unique_ptr<Frame> PFrame;
    PFrame               baseline_;
    PFrame               slope_;
    const double         alpha_;
    const double         beta_;
    int                  counter_;

    /** C-tor
      * @param alpha smoothing coefficient
      */
    DoubleExpSmoothingSlidingWindow(double alpha, double beta)
        : alpha_(alpha)
        , beta_(beta)
        , counter_(0)
    {
    }

    void add(PFrame value) {
        switch(counter_) {
        case 0:
            std::swap(baseline_, value);
            counter_ = 1;
            break;
        case 1:
            slope_.reset(new Frame(*value));
            slope_->sub(*baseline_);
            baseline_ = std::move(value);
            counter_ = 2;
            break;
        default: {
                PFrame old_baseline(new Frame(*baseline_));
                PFrame old_slope = std::move(slope_);
                // Calculate new baseline
                {
                    PFrame new_baseline = std::move(value);
                    new_baseline->mul(alpha_);
                    old_baseline->add(*old_slope);
                    old_baseline->mul(1.0 - alpha_);
                    new_baseline->add(*old_baseline);
                    std::swap(new_baseline, baseline_);
                    std::swap(new_baseline, old_baseline);
                }
                // Calculate new slope
                slope_.reset(new Frame(*baseline_));
                slope_->sub(*old_baseline);
                slope_->mul(beta_);
                old_slope->mul(1.0 - beta_);
                slope_->add(*old_slope);
                break;
            }
        };
    }

    PFrame forecast() const {
        PFrame res;
        if (counter_ < 2) {
            // return empty response
            return std::move(res);
        }
        res.reset(new Frame(*baseline_));
        res->add(*slope_);
        return std::move(res);
    }
};

//                                      //
//      HoltWintersSlidingWindow        //
//                                      //

/** Holt-Winters implementation.
  * http://static.usenix.org/events/lisa00/full_papers/brutlag/brutlag_html/
  */
template<class Frame>
struct HoltWintersSlidingWindow {
    typedef std::unique_ptr<Frame> PFrame;
    PFrame               baseline_;
    PFrame               slope_;
    std::deque<PFrame>   seasonal_;
    const double         alpha_;
    const double         beta_;
    const double         gamma_;
    int                  counter_;
    int                  period_;

    HoltWintersSlidingWindow(double alpha, double beta, double gamma, int period)
        : alpha_(alpha)
        , beta_(beta)
        , gamma_(gamma)
        , counter_(0)
        , period_(period)
    {
    }

    void add(PFrame value) {
        if (counter_ == 0) {
            baseline_.reset(new Frame(*value));
            seasonal_.push_back(std::move(value));
        } else if (counter_ == 1) {
            slope_.reset(new Frame(*value));
            slope_->sub(*baseline_);
            baseline_.reset(new Frame(*value));
            seasonal_.push_back(std::move(value));
        } else if (counter_ < period_) {
            seasonal_.push_back(std::move(value));
        } else {
            PFrame seasonal = std::move(seasonal_.front());
            PFrame old_baseline;
            seasonal_.pop_front();
            // Calculate baseline
            {
                PFrame new_baseline(new Frame(*value));
                PFrame old_slope(new Frame(*slope_));
                new_baseline->sub(*seasonal);
                new_baseline->mul(alpha_);
                old_slope->add(*baseline_);
                old_slope->mul(1.0 - alpha_);
                new_baseline->add(*old_slope);
                old_baseline = std::move(baseline_);
                std::swap(new_baseline, baseline_);
            }
            // Calculate slope
            {
                PFrame new_slope(new Frame(*baseline_));
                PFrame old_slope = std::move(slope_);
                new_slope->sub(*old_baseline);
                new_slope->mul(beta_);
                old_slope->mul(1.0 - beta_);
                new_slope->add(*old_slope);
                std::swap(new_slope, slope_);
            }
            // Calculate seasonality
            {
                value->sub(*baseline_);
                value->mul(gamma_);
                seasonal->mul(1.0 - gamma_);
                value->add(*seasonal);
                seasonal_.push_back(std::move(value));
            }
        }
        counter_++;
    }

    PFrame forecast() const {
        PFrame res;
        if (counter_ < period_) {
            // return empty response
            return std::move(res);
        }
        res.reset(new Frame(*baseline_));
        res->add(*slope_);
        res->add(*seasonal_.back());
        return std::move(res);
    }
};

//                                  //
//      AnomalyDetectorPipeline     //
//                                  //

template<
    class Frame,                        // Frame type
    template<class F> class FMethod     // Forecasting method type
>
struct AnomalyDetectorPipeline : AnomalyDetectorIface {
    typedef FMethod<Frame>                  FcastMethod;
    typedef std::unique_ptr<Frame>          PFrame ;
    typedef std::unique_ptr<FcastMethod>    PSlidingWindow;

    HashFnFamily                hashes_;
    const u32              N;
    const u32              K;
    PFrame                      current_;
    PFrame                      error_;
    double                      F2_;
    double                      threshold_;
    PSlidingWindow              sliding_window_;

    AnomalyDetectorPipeline(u32 N, u32 K, double threshold, PSlidingWindow swindow)
        : hashes_(N, K)
        , N(N)
        , K(K)
        , F2_(0.0)
        , threshold_(threshold)
        , sliding_window_(std::move(swindow))
    {
        current_.reset(new Frame(hashes_));
    }

    void add(u64 id, double value) {
        current_->add(id, value);
    }

    //! Returns true if series is anomalous (approx)
    bool is_anomaly_candidate(u64 id) const {
        if (error_) {
            double estimate = error_->estimate(id);
            return estimate > F2_;
        }
        return false;
    }

    void move_sliding_window() {
        PFrame forecast = std::move(sliding_window_->forecast());
        if (forecast) {
            error_ = std::move(calculate_error(forecast, current_));
            F2_ = sqrt(error_->estimateF2())*threshold_;
        }
        sliding_window_->add(std::move(current_));
        current_.reset(new Frame(hashes_));
    }

    PFrame calculate_error(const PFrame &forecast, const PFrame &actual) {
        PFrame res;
        res.reset(new Frame(hashes_));
        res->diff(*forecast, *actual);
        return std::move(res);
    }
};

template<class Window, class Detector>
std::unique_ptr<AnomalyDetectorIface> create_detector(u32 N,
                                                      u32 K,
                                                      double threshold,
                                                      u32 window_size)
{
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(window_size));
    result.reset(new Detector(N, K, threshold, std::move(window)));
    return std::move(result);
}

//! Create approximate anomaly detector based on simple moving-average smothing
std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_approx_sma(u32 N,
                                           u32 K,
                                           double threshold,
                                           u32 window_size)
{
    typedef AnomalyDetectorPipeline<CountingSketch, SMASlidingWindow>   Detector;
    typedef SMASlidingWindow<CountingSketch>                            Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(window_size));
    result.reset(new Detector(N, K, threshold, std::move(window)));
    return std::move(result);
}

//! Create precise anomaly detector based on simple moving-average smothing
std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_precise_sma(double threshold,
                                            u32 window_size)
{
    typedef AnomalyDetectorPipeline<PreciseCounter, SMASlidingWindow>   Detector;
    typedef SMASlidingWindow<PreciseCounter>                            Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(window_size));
    result.reset(new Detector(1, 8, threshold, std::move(window)));
    return std::move(result);
}

//! Create approximate anomaly detector based on simple moving-average smothing or EWMA
std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_approx_ewma(u32 N,
                                            u32 K,
                                            double threshold,
                                            double alpha)
{
    typedef AnomalyDetectorPipeline<CountingSketch, EWMASlidingWindow>  Detector;
    typedef EWMASlidingWindow<CountingSketch>                           Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(alpha));
    result.reset(new Detector(N, K, threshold, std::move(window)));
    return std::move(result);
}

//! Create precise anomaly detector based on simple moving-average smothing or EWMA
std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_precise_ewma(double threshold,
                                             double alpha)
{
    typedef AnomalyDetectorPipeline<PreciseCounter, EWMASlidingWindow>  Detector;
    typedef EWMASlidingWindow<PreciseCounter>                           Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(alpha));
    result.reset(new Detector(1, 8, threshold, std::move(window)));
    return std::move(result);
}

//! Create precise anomaly detector based on simple moving-average smothing or EWMA
std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_precise_double_exp_smoothing(
                                             double threshold,
                                             double alpha,
                                             double beta)
{
    typedef AnomalyDetectorPipeline<PreciseCounter, DoubleExpSmoothingSlidingWindow>  Detector;
    typedef DoubleExpSmoothingSlidingWindow<PreciseCounter>                           Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(alpha, beta));
    result.reset(new Detector(1, 8, threshold, std::move(window)));
    return std::move(result);
}

std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_approx_double_exp_smoothing(
                                         u32 N,
                                         u32 K,
                                         double threshold,
                                         double alpha,
                                         double beta)
{
    typedef AnomalyDetectorPipeline<CountingSketch, DoubleExpSmoothingSlidingWindow>  Detector;
    typedef DoubleExpSmoothingSlidingWindow<CountingSketch>                           Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(alpha, beta));
    result.reset(new Detector(N, K, threshold, std::move(window)));
    return std::move(result);
}

//! Create precise anomaly detector based on simple moving-average smothing or EWMA
std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_precise_holt_winters(
                                             double threshold,
                                             double alpha,
                                             double beta,
                                             double gamma,
                                             int period)
{
    typedef AnomalyDetectorPipeline<PreciseCounter, HoltWintersSlidingWindow>         Detector;
    typedef HoltWintersSlidingWindow<PreciseCounter>                                  Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(alpha, beta, gamma, period));
    result.reset(new Detector(1, 8, threshold, std::move(window)));
    return std::move(result);
}

//! Create precise anomaly detector based on simple moving-average smothing or EWMA
std::unique_ptr<AnomalyDetectorIface>
    AnomalyDetectorUtil::create_approx_holt_winters(
                                             u32 N,
                                             u32 K,
                                             double threshold,
                                             double alpha,
                                             double beta,
                                             double gamma,
                                             int period)
{
    typedef AnomalyDetectorPipeline<PreciseCounter, HoltWintersSlidingWindow>         Detector;
    typedef HoltWintersSlidingWindow<PreciseCounter>                                  Window;
    std::unique_ptr<AnomalyDetectorIface> result;
    std::unique_ptr<Window> window(new Window(alpha, beta, gamma, period));
    result.reset(new Detector(N, K, threshold, std::move(window)));
    return std::move(result);
}

}
}


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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/numerical_integrations.h>
#include <votca/ctp/logger.h>
#include <votca/xtp/espfit.h>
#include <votca/xtp/aomatrix.h>
#include <votca/tools/linalg.h>
//#include <boost/progress.hpp>

#include <math.h>
#include <votca/tools/constants.h>

using namespace votca::tools;

namespace votca { namespace xtp {
    namespace ub = boost::numeric::ublas;

void Espfit::Fit2Density(std::vector< ctp::QMAtom* >& _atomlist, ub::matrix<double> &_dmat, AOBasis &_basis,BasisSet &bs,string gridsize) {

    // setting up grid
    Grid _grid;
    _grid.setAtomlist(&_atomlist);
    _grid.setupCHELPgrid();
    //_grid.printGridtoxyzfile("grid.xyz");
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() <<  " Done setting up CHELPG grid with " << _grid.getsize() << " points " << endl;

    // Calculating nuclear potential at gridpoints

    ub::vector<double> _ESPatGrid = ub::zero_vector<double>(_grid.getsize());

    AOOverlap overlap;
    overlap.Fill(_basis);
    ub::vector<double> DMATasarray=_dmat.data();
    ub::vector<double> AOOasarray=overlap.Matrix().data();
    double N_comp=0.0;
    #pragma omp parallel for reduction(+:N_comp)
    for ( unsigned _i =0; _i < DMATasarray.size(); _i++ ){
            N_comp =N_comp+ DMATasarray(_i)*AOOasarray(_i);
        }

    NumericalIntegration numway;

    numway.GridSetup(gridsize,&bs,_atomlist,&_basis);
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculate Densities at Numerical Grid with gridsize "<<gridsize  << flush;
    double N=numway.IntegrateDensity(_dmat);
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculated Densities at Numerical Grid, Number of electrons is "<< N << flush;

    if(std::abs(N-N_comp)>0.001){
        CTP_LOG(ctp::logDEBUG, *_log) <<"=======================" << flush;
        CTP_LOG(ctp::logDEBUG, *_log) <<"WARNING: Calculated Densities at Numerical Grid, Number of electrons "<< N <<" is far away from the the real value "<< N_comp<<", you should increase the accuracy of the integration grid."<< flush;
        N=N_comp;
        CTP_LOG(ctp::logDEBUG, *_log) <<"WARNING: Electronnumber set to "<< N << flush;
        CTP_LOG(ctp::logDEBUG, *_log) <<"=======================" << flush;
    }

    double netcharge=getNetcharge( _atomlist,N );

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating ESP at CHELPG grid points"  << flush;
    //boost::progress_display show_progress( _grid.getsize() );
    #pragma omp parallel for
    for ( int i = 0 ; i < _grid.getsize(); i++){
        _ESPatGrid(i)=numway.IntegratePotential(_grid.getGrid()[i]*tools::conv::nm2bohr);
        //++show_progress;
    }

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Electron contribution calculated"  << flush;
    if (!_do_Transition){
    ub::vector<double> _NucPatGrid = EvalNuclearPotential(  _atomlist,  _grid );
    _ESPatGrid += _NucPatGrid;
    }

    std::vector< tools::vec > _fitcenters;

    for ( unsigned j = 0; j < _atomlist.size(); j++){
       tools::vec _pos=_atomlist[j]->getPos()*tools::conv::ang2nm;
      _fitcenters.push_back(_pos);
    }

    std::vector<double> _charges = FitPartialCharges(_fitcenters,_grid, _ESPatGrid, netcharge);

    //Write charges to qmatoms
    for ( unsigned _i =0 ; _i < _atomlist.size(); _i++){
        _atomlist[_i]->charge=_charges[_i];
    }
    return;
    }

ub::vector<double> Espfit::EvalNuclearPotential(std::vector< ctp::QMAtom* >& _atoms, Grid _grid) {
    ub::vector<double> _NucPatGrid = ub::zero_vector<double>(_grid.getsize());

    double Znuc=0.0;
    const std::vector< vec >& _gridpoints = _grid.getGrid();
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating ESP of nuclei at CHELPG grid points" << flush;

    for (unsigned i = 0; i < _gridpoints.size(); i++) {
        for (unsigned j = 0; j < _atoms.size(); j++) {
            vec posatom=_atoms[j]->getPos()*tools::conv::ang2nm;
            if (_ECP) {
                Znuc = _elements.getNucCrgECP(_atoms[j]->type);
            } else {
                Znuc = _elements.getNucCrg(_atoms[j]->type);
            }
            double dist_j = tools::abs(_gridpoints[i]-posatom) * tools::conv::nm2bohr;
            _NucPatGrid(i) += Znuc / dist_j;
        }

    }
    return _NucPatGrid;
}

double Espfit::getNetcharge( std::vector< ctp::QMAtom* >& _atoms, double N ){
    double netcharge=0.0;
    if( std::abs(N)<0.05){
        //CTP_LOG(ctp::logDEBUG, *_log) << "Number of Electrons is "<<N<< " transitiondensity is used for fit"  << flush;
        _do_Transition=true;
    }
    else{
    double Znuc_ECP = 0.0;
    double Znuc=0.0;
    for ( unsigned j = 0; j < _atoms.size(); j++){
           Znuc_ECP += _elements.getNucCrgECP(_atoms[j]->type);
           Znuc+= _elements.getNucCrg(_atoms[j]->type);
    }

    if (_ECP){
        if (std::abs(Znuc_ECP-N)<4){
            CTP_LOG(ctp::logDEBUG, *_log) <<"Number of Electrons minus ECP_Nucleus charge is "<<Znuc_ECP-N<< " you use ECPs, sounds good"  << flush;
        }
        else if (std::abs(Znuc-N)<4){
            CTP_LOG(ctp::logDEBUG, *_log) <<"Number of Electrons minus real Nucleus charge is "<<Znuc-N<< " you are sure you want ECPs?"  << flush;
        }
        else{
            CTP_LOG(ctp::logDEBUG, *_log) <<"Warning: Your molecule is highly ionized and you want ECPs, sounds interesting" << flush;
        }
        netcharge=Znuc_ECP-N;
    }
    else{
        if (std::abs(Znuc-N)<4){
            CTP_LOG(ctp::logDEBUG, *_log) <<"Number of Electrons minus Nucleus charge is "<<Znuc-N<< " you probably do not use ECPs, if you do use ECPs please use the option. Otherwise you are fine"  << flush;
        }
        else if(std::abs(Znuc_ECP-N)<4){
            CTP_LOG(ctp::logDEBUG, *_log) <<"Number of Electrons minus ECP_Nucleus charge is "<<Znuc_ECP-N<< " you probably use ECPs, if you do use ECPs please use the option to switch on"  << flush;
        }
        else{
            CTP_LOG(ctp::logDEBUG, *_log) <<"Warning: Your molecule is highly ionized and you use real core potentials, sounds interesting" << flush;
        }
        netcharge = Znuc-N;
    }
    _do_Transition=false;
    }

    netcharge=round(netcharge);
    CTP_LOG(ctp::logDEBUG, *_log) <<"Netcharge constrained to " << netcharge<< flush;

    return netcharge;
}

void Espfit::Fit2Density_analytic(std::vector< ctp::QMAtom* >& _atomlist, ub::matrix<double> &_dmat,AOBasis &_basis) {
     double Nm2Bohr=tools::conv::nm2bohr;
     double A2nm=tools::conv::ang2nm;
    // setting up grid
    Grid _grid;
    _grid.setAtomlist(&_atomlist);
    _grid.setupCHELPgrid();

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() <<  " Done setting up CHELPG grid with " << _grid.getsize() << " points " << endl;
    // Calculating nuclear potential at gridpoints

    ub::vector<double> _ESPatGrid = ub::zero_vector<double>(_grid.getsize());

    AOOverlap overlap;
    overlap.Fill(_basis);
    const ub::vector<double> DMATasarray=_dmat.data();
    const ub::vector<double> AOOasarray=overlap.Matrix().data();
    double N=0.0;
    #pragma omp parallel for reduction(+:N)
    for ( unsigned _i =0; _i < DMATasarray.size(); _i++ ){
            N =N+ DMATasarray(_i)*AOOasarray(_i);
        }

    double netcharge=getNetcharge( _atomlist,N );
    if(!_do_Transition){
        ub::vector<double> _NucPatGrid = EvalNuclearPotential(  _atomlist,  _grid);
        _ESPatGrid += _NucPatGrid;
    }

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating ESP at CHELPG grid points"  << flush;
    #pragma omp parallel for
    for ( int i = 0 ; i < _grid.getsize(); i++){
        // AOESP matrix
         AOESP _aoesp;
         _aoesp.Fill(_basis, _grid.getGrid()[i]*Nm2Bohr);
        const ub::vector<double> AOESPasarray=_aoesp.Matrix().data();

        for ( unsigned _i =0; _i < DMATasarray.size(); _i++ ){
            _ESPatGrid(i) -= DMATasarray(_i)*AOESPasarray(_i);
        }
    }

    std::vector< tools::vec > _fitcenters;

          for ( unsigned j = 0; j < _atomlist.size(); j++){
             tools::vec _pos=A2nm*_atomlist[j]->getPos();
            _fitcenters.push_back(_pos);
          }
    std::vector<double> _charges = FitPartialCharges(_fitcenters,_grid, _ESPatGrid, netcharge);

    //Write charges to qmatoms
        for ( unsigned _i =0 ; _i < _atomlist.size(); _i++){
            _atomlist[_i]->charge=_charges[_i];
        }
    return;
    }

std::vector<double> Espfit::FitPartialCharges( std::vector< tools::vec >& _fitcenters, Grid& _grid, ub::vector<double>& _potential, double& _netcharge ){
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Setting up Matrices for fitting of size "<< _fitcenters.size()+1 <<" x " << _fitcenters.size()+1<< flush;

    const std::vector< tools::vec >& _gridpoints=_grid.getGrid();

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Using "<< _fitcenters.size() <<" Fittingcenters and " << _gridpoints.size()<< " Gridpoints."<< flush;

    ub::matrix<double> _Amat = ub::zero_matrix<double>(_fitcenters.size()+1,_fitcenters.size()+1);
    ub::matrix<double> _Bvec = ub::zero_matrix<double>(_fitcenters.size()+1,1);
    //boost::progress_display show_progress( _fitcenters.size() );
    // setting up _Amat
    #pragma omp parallel for
    for ( unsigned _i =0 ; _i < _Amat.size1()-1; _i++){
        for ( unsigned _j=_i; _j<_Amat.size2()-1; _j++){
            for ( unsigned _k=0; _k < _gridpoints.size(); _k++){
                double dist_i = tools::abs(_fitcenters[_i]-_gridpoints[_k])*tools::conv::nm2bohr;
                double dist_j = tools::abs(_fitcenters[_j]-_gridpoints[_k])*tools::conv::nm2bohr;

                 _Amat(_i,_j) += 1.0/dist_i/dist_j;
            }
            _Amat(_j,_i) = _Amat(_i,_j);
        }
    }

    for ( unsigned _i =0 ; _i < _Amat.size1(); _i++){
      _Amat(_i,_Amat.size1()-1) = 1.0;
      _Amat(_Amat.size1()-1,_i) = 1.0;
    }
    _Amat(_Amat.size1()-1,_Amat.size1()-1) = 0.0;

    // setting up Bvec
    #pragma omp parallel for
    for ( unsigned _i =0 ; _i < _Bvec.size1()-1; _i++){
        for ( unsigned _k=0; _k < _gridpoints.size(); _k++){
                double dist_i = tools::abs(_fitcenters[_i]-_gridpoints[_k])*tools::conv::nm2bohr;
                _Bvec(_i,0) += _potential(_k)/dist_i;
        }
       }

    _Bvec(_Bvec.size1()-1,0) = _netcharge; //netcharge!!!!
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << "  Inverting Matrices "<< flush;
    // invert _Amat
    ub::matrix<double> _Amat_inverse = ub::zero_matrix<double>(_fitcenters.size()+1,_fitcenters.size()+1);

    if(_do_svd){
        int notfittedatoms=linalg_invert_svd( _Amat , _Amat_inverse,_conditionnumber);
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << "SVD Done. "<<notfittedatoms<<" could not be fitted and are set to zero."<< flush;
    }
    else{
        linalg_invert( _Amat , _Amat_inverse);
    }

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Inverting Matrices done."<< flush;
    //_Amat.resize(0,0);

    ub::matrix<double> _charges = ub::prod(_Amat_inverse,_Bvec);

    std::vector<double> _result;
    for ( unsigned _i = 0; _i < _charges.size1(); _i++ ){
        _result.push_back(_charges(_i,0));
    }

    double _sumcrg = 0.0;
    for ( unsigned _i =0 ; _i < _fitcenters.size(); _i++){

        //CTP_LOG(ctp::logDEBUG, *_log) << " Center " << _i << " FitCharge: " << _result[_i] << "pos " << _fitcenters[_i] << flush;
        _sumcrg += _result[_i];
    }

    CTP_LOG(ctp::logDEBUG, *_log) << " Sum of fitted charges: " << _sumcrg << flush;

    // get RMSE
    double _rmse = 0.0;
    double _totalPotSq = 0.0;
    for ( unsigned _k=0 ; _k < _gridpoints.size(); _k++ ){
        double temp = 0.0;
        for ( unsigned _i=0; _i < _fitcenters.size(); _i++ ){
            double dist =  tools::abs(_gridpoints[_k]-_fitcenters[_i])*tools::conv::nm2bohr;
            temp += _result[_i]/dist;
        }
        _rmse += (_potential(_k) - temp)*(_potential(_k) - temp);
        _totalPotSq += _potential(_k)*_potential(_k);
    }
    CTP_LOG(ctp::logDEBUG, *_log) << " RMSE of fit:  " << sqrt(_rmse/_gridpoints.size()) << flush;
    CTP_LOG(ctp::logDEBUG, *_log) << " RRMSE of fit: " << sqrt(_rmse/_totalPotSq) << flush;

    return _result;
   }

}}


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

// This code will convert multiple cartesian state vectors
// into TLE and check where exactly the Atom is failing. The cartesian
// vectors are generated by converting randomly generated keplerian elements.
// The conversion is achieved through the pykep library of ESA.

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <execinfo.h>

#include <boost/exception/info.hpp>
#include <libsgp4/Globals.h>
#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>

#include "CppProject/TleGen.hpp"
#include "CppProject/randomGen.hpp"

typedef double Real;
typedef std::vector< Real > Vector;
typedef std::vector< Real > Vector6;
typedef std::vector< Real > Vector3;
typedef std::vector< Real > Vector2;
typedef std::vector < std::vector < Real > > Vector2D;

int main(void)
{
    // some constants values are defined here
    const double km2m = 1000; // conversion from km to m
    // earth radius
    const double EarthRadius = kXKMPER * km2m; // unit m
    const double EarthDiam = 2 * EarthRadius;
    // grav. parameter 'mu' of earth
    // const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2
    const int bypass = true; // keep this true, this just enables executing an alternate code snippet that takes care of thrown exceptions from ATOM

    if(bypass == false){
        // initialize input parameters for the function generating random orbital elements. Description can be
        // found in randomKepElem.hpp for each of the following parameters.
        const Vector2 range_a      = { (EarthDiam+100000), (EarthDiam+1000000) };
        const Vector2 range_e      = { 0, 1 };
        const Vector2 range_i      = { 0, sml::convertDegreesToRadians( 180.0 ) };
        const Vector2 range_raan   = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const Vector2 range_w      = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const Vector2 range_E      = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const int limit            = 100;
        Vector2D randKep( limit, std::vector< Real >( 6 ) );
        Vector semiAxis( limit ); // storage for random semi major axis values
        Vector eccentricity( limit ); // storage for random eccentricity values
        Vector inclination( limit ); // storage for random inclination values
        Vector RAAN( limit ); // storage for random RAAN values
        Vector AOP( limit ); // storage for random AOP values
        Vector EA( limit ); // storage for random EA values

        // call the function to generate random keplerian orbital elements. Values are stored in randKep in a 2D
        // vector format. A single row represents one set of orbital elements, arranged in the same order as the
        // input argument order of the elements.
        randomGen::randomGen( range_a, limit, semiAxis );
        randomGen::randomGen( range_e, limit, eccentricity );
        randomGen::randomGen( range_i, limit, inclination );
        randomGen::randomGen( range_raan, limit, RAAN );
        randomGen::randomGen( range_w, limit, AOP );
        randomGen::randomGen( range_E, limit, EA );
        for(int i = 0; i < limit; i++)
        {
            randKep[ i ][ 0 ] = semiAxis[ i ];
            randKep[ i ][ 1 ] = eccentricity[ i ];
            randKep[ i ][ 2 ] = inclination[ i ];
            randKep[ i ][ 3 ] = RAAN[ i ];
            randKep[ i ][ 4 ] = AOP[ i ];
            randKep[ i ][ 5 ] = EA[ i ];
        }

        // remove orbital elements where the radius of perigee is inside earth
        Vector2D randKepElem( limit, std::vector< Real >( 6 ) );
        int newLimit = limit; // whenever a row of cartesian (keplerian) elements is removed, the limit will have to be changed.
        Real radiusPerigee = 0; // variable storing the radius of Perigee for an orbit. this is used in our checking condition
        std::vector< int > rowsToDelete; // stores the row numbers which have to be deleted
        int insideCounter = 0; // to count the number of times radius of perigee is inside the Earth.
        int outsideCounter = 0; // to count the number of times the radius of perigee is beyond a cetain upper limit
        int newIndex = 0; // index counter for the second 2D vector
        for(int j = 0; j < newLimit; j++)
        {
            radiusPerigee = randKep[ j ][ 0 ] * (1 - randKep[ j ][ 1 ]);
            if(radiusPerigee <= EarthRadius )
            {
                insideCounter = insideCounter + 1; // counter increments everytime the condition is true
            }
            if(radiusPerigee >= (EarthRadius+2000000))
            {
                outsideCounter = outsideCounter + 1;
            }
            if(radiusPerigee > EarthRadius && radiusPerigee < (EarthRadius+2000000)) // this is obv not the most efficient way
            {
                randKepElem[ newIndex ] = randKep[ j ];
                newIndex++;
            }
        }
        randKepElem.erase( randKepElem.begin() + newIndex, randKepElem.end() ); // delete the left over rows in the final random keplerian element vector
        std::vector< std::vector < Real > > ().swap(randKep); // create an empty vector with no memory allocated to it ...
        // ... and swap it with the vector which you want to delete and deallocate the memory
        std::cout << "Inside counter value = " << insideCounter << std::endl;
        std::cout << "Outside counter value = " << outsideCounter << std::endl;
        std::cout << "Usefull sets left = " << limit - (insideCounter + outsideCounter) << std::endl;
        newLimit = randKepElem.size();
        std::cout << "Number of final rows in randKepElem 2D vector = " << newLimit << std::endl;

        // file storage
        std::ofstream tlefile;
        tlefile.open("TLEfile.csv", std::ofstream::app);
        tlefile << "Solver Status Summary" << "," << "Iteration Count" << "," << "semi-major axis [km]" << "," << "eccentricity" << ",";
        tlefile << "Inclination [deg]" << "," << "RAAN [deg]" << "," << "AOP [deg]" << "," << "Eccentric Anomaly [deg]" << std::endl;
        // Generate the TLEs for the final random set of orbital elements
        std::string SolverStatus;
        int IterationCount;
        // some other bookkeeping variables, these are not used in the convert to tle function
        std::size_t findSuccess;
        for(int i = 0; i < newLimit; i++)
        {
            IterationCount = 0;
            TleGen::TleGen( randKepElem[ i ], SolverStatus, IterationCount ); // giving one set of orbital elements one by one to the tle generator
            findSuccess = SolverStatus.find("success");
            if(findSuccess == std::string::npos){
                std::cout << "TLE Conversion Failed" << std::endl;
                tlefile << "Failed" << "," << IterationCount << ",";
                tlefile << ( randKepElem[ i ][ 0 ]/1000 ) << ",";
                tlefile << randKepElem[ i ][ 1 ] << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 2 ] ) << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 3 ] ) << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 4 ] ) << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 5 ] ) << std::endl;
            }
            else{
                std::cout << "TLE conversion success" << std::endl;
                tlefile << "Success" << "," << IterationCount << ",";
                tlefile << ( randKepElem[ i ][ 0 ]/1000 ) << ",";
                tlefile << randKepElem[ i ][ 1 ] << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 2 ] ) << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 3 ] ) << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 4 ] ) << ",";
                tlefile << sml::convertRadiansToDegrees( randKepElem[ i ][ 5 ] ) << std::endl;
            }

        }
        tlefile << std::endl << std::endl;
        tlefile.close();
    }
    else{
            const int newLimit = 1000; // number of random elementss to be generated
            Vector2D randKepElem( newLimit, std::vector < Real >( 6 ) ); // general 2D vector to store the randomly generated keplerian elements in one place
            int indexer = 0; // used to keep track of which orbit or set of random orbital elements is being run in the simulation
            Vector RadiusOfPerigee( newLimit ); // to store radius of perigee value for each random orbit generated

            std::ofstream tlefile; // output file handle
            tlefile.open( "TLEfile3.csv", std::ofstream::app ); // open the file and append the data
            tlefile << "Solver Status Summary" << "," << "Iteration Count" << "," << "semi-major axis [km]" << "," << "SMA Seed" << ",";
            tlefile << "eccentricity" << "," << "ECC Seed" << ",";
            tlefile << "Inclination [deg]" << "," << "INC Seed" << ",";
            tlefile << "RAAN [deg]" << "," << "RAAN Seed" << ",";
            tlefile << "AOP [deg]" << "," << "AOP Seed" << ",";
            tlefile << "Eccentric Anomaly [deg]" << "," << "EA Seed" << "," << "Radius of Perigee [km]" << std::endl;

            // bookkeeping variables, these are not used in the convert to tle function
            std::size_t findSuccess;

            Vector2 range_a = { (EarthDiam + 10000 * km2m), (EarthDiam + 90000 * km2m) }; // range for semi major axis
            Vector2 range_e = { 0.0, 1.0 }; // range for eccentricity
            Vector2 range_i = { 0.0, 180.0 }; // range for inclination
            Vector2 range_raan = { 0.0, 360.0 }; // range for raan
            Vector2 range_w = { 0.0, 360.0 }; // range for argument of perigee
            Vector2 range_EA = { 0.0, 360.0 }; // range for eccentric anomaly

            Vector eccentricity( newLimit ); // init. vector to store eccentricity values
            Vector semiAxis( newLimit ); // initialize vector of size newLimit
            Vector inclination( newLimit ); // vector to store random inclination values
            Vector raan( newLimit ); // vector to store random raan values
            Vector aop( newLimit ); // vector to store random aop values
            Vector EA( newLimit ); // vector to store random EA values

            // seed values for each orbital element, to be used in the pseudo random element generator algorithm. These seed values can be used to
            // regenerate the psuedo random sequence at any given time in future.
            const int semiSeed = 400;
            const int eccSeed = 400;
            const int incSeed = 400;
            const int raanSeed = 300;
            const int aopSeed = 200;
            const int eaSeed = 100;

            // random value generation
            randomGen::randomGenWithSeed( range_a, newLimit, semiAxis, semiSeed );
            randomGen::randomGenWithSeed( range_e, newLimit, eccentricity, eccSeed );
            randomGen::randomGenWithSeed( range_i, newLimit, inclination, incSeed );
            randomGen::randomGenWithSeed( range_raan, newLimit, raan, raanSeed );
            randomGen::randomGenWithSeed( range_w, newLimit, aop, aopSeed );
            randomGen::randomGenWithSeed( range_EA, newLimit, EA, eaSeed );

            for(int i = 0; i < newLimit; i++)
            {
                randKepElem[ i ][ 0 ] = semiAxis[ i ]; // semi major axis
                randKepElem[ i ][ 1 ] = eccentricity[ i ]; // eccentricity
                randKepElem[ i ][ 2 ] = sml::convertDegreesToRadians( inclination[ i ] ); // inclination
                randKepElem[ i ][ 3 ] = sml::convertDegreesToRadians( raan[ i ] ); // RAAN
                randKepElem[ i ][ 4 ] = sml::convertDegreesToRadians( aop[ i ] ); // AOP
                randKepElem[ i ][ 5 ] = sml::convertDegreesToRadians( EA[ i ] ); // EA
                RadiusOfPerigee[ i ] = semiAxis[ i ] * ( 1 - eccentricity[ i ] );
            }
            while( indexer < newLimit)
            {
                std::string SolverStatus;
                int IterationCount;
                try
                {
                    indexer++;
                    // std::cout << "indexer = " << indexer << std::endl;
                    // std::cout << "Radius of Perigee = " << RadiusOfPerigee[ indexer - 1 ]/1000 << std::endl;
                    // std::cout << "Semi Axis = " << randKepElem[ indexer - 1 ][ 0 ]/1000 << std::endl;
                    // std::cout << "Eccentricity = " << randKepElem[ indexer - 1 ][ 1 ] << std::endl;
                    // std::cout << "Inclination = " << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 2 ] ) << std::endl;

                    TleGen::TleGen( randKepElem[ indexer - 1 ], SolverStatus, IterationCount );
                    // std::cout << SolverStatus << std::endl;
                    findSuccess = SolverStatus.find("success");
                    if(findSuccess == std::string::npos){
                            // std::cout << "TLE Conversion Failed" << std::endl;
                            tlefile << "Failed" << "," << IterationCount << ",";
                            tlefile << ( randKepElem[ indexer - 1 ][ 0 ]/1000 ) << "," << semiSeed << ",";
                            tlefile << randKepElem[ indexer - 1 ][ 1 ] << "," << eccSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 2 ] ) << "," << incSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 3 ] ) << "," << raanSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 4 ] ) << "," << aopSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 5 ] ) << "," << eaSeed << ",";
                            tlefile << RadiusOfPerigee[ indexer - 1 ]/1000 << std::endl;
                    }
                    else{
                            // std::cout << "TLE conversion success" << std::endl;
                            tlefile << "Success" << "," << IterationCount << ",";
                            tlefile << ( randKepElem[ indexer - 1 ][ 0 ]/1000 ) << "," << semiSeed << ",";
                            tlefile << randKepElem[ indexer - 1 ][ 1 ] << "," << eccSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 2 ] ) << "," << incSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 3 ] ) << "," << raanSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 4 ] ) << "," << aopSeed << ",";
                            tlefile << sml::convertRadiansToDegrees( randKepElem[ indexer - 1 ][ 5 ] ) << "," << eaSeed << ",";
                            tlefile << RadiusOfPerigee[ indexer - 1 ]/1000 << std::endl;
                    }
                }
                catch(const std::exception& err)
                {
                    // std::cout << "Error Caught = ";
                    // std::cout << err.what() << std::endl;
                    tlefile << "Exception Caught = " << err.what() << "," << ",";
                    tlefile << semiAxis[ indexer - 1 ]/1000 << "," << semiSeed << ",";
                    tlefile << eccentricity[ indexer - 1 ] << "," << eccSeed << ",";
                    tlefile << inclination[ indexer - 1 ] << "," << incSeed << ",";
                    tlefile << raan[ indexer - 1 ] << "," << raanSeed << ",";
                    tlefile << aop[ indexer - 1 ] << "," << aopSeed << ",";
                    tlefile << EA[ indexer - 1 ] << "," << eaSeed << ",";
                    tlefile << RadiusOfPerigee[ indexer - 1 ]/1000 << std::endl;
                }
            }
            tlefile << std::endl << std::endl;
            tlefile.close();
        }
   return EXIT_SUCCESS;
}


//  Copyright (c) 2018 Robert Ramey
//
// 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)

#include <iostream>

#include <boost/safe_numerics/safe_integer.hpp>

int main(){
    std::cout << "example 4: ";
    std::cout << "implicit conversions change data values" << std::endl;
    std::cout << "Not using safe numerics" << std::endl;

    // problem: implicit conversions change data values
    try{
        signed int   a{-1};
        unsigned int b{1};
        std::cout << "a is " << a << " b is " << b << '\n';
        if(a < b){
            std::cout << "a is less than b\n";
        }
        else{
            std::cout << "b is less than a\n";
        }
        std::cout << "error NOT detected!" << std::endl;
    }
    catch(const std::exception &){
        // never arrive here - just produce the wrong answer!
        std::cout << "error detected!" << std::endl;
        return 1;
    }

    // solution: replace int with safe<int> and unsigned int with safe<unsigned int>
    std::cout << "Using safe numerics" << std::endl;
    try{
        using namespace boost::safe_numerics;
        safe<signed int>   a{-1};
        safe<unsigned int> b{1};
        std::cout << "a is " << a << " b is " << b << '\n';
        if(a < b){
            std::cout << "a is less than b\n";
        }
        else{
            std::cout << "b is less than a\n";
        }
        std::cout << "error NOT detected!" << std::endl;
        return 1;
    }
    catch(const std::exception & e){
        // never arrive here - just produce the correct answer!
        std::cout << e.what() << std::endl;
        std::cout << "error detected!" << std::endl;
    }
    return 0;
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// you should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_CLUSTERING_HH
#define GRAPH_CLUSTERING_HH

#include "config.h"

#include "hash_map_wrap.hh"
#include <boost/mpl/if.hpp>

#ifdef _OPENMP
#include "omp.h"
#endif

#ifndef __clang__
#include <ext/numeric>
using __gnu_cxx::power;
#else
template <class Value>
Value power(Value value, int n)
{
    return pow(value, n);
}
#endif

namespace graph_tool
{
using namespace boost;
using namespace std;

// calculates the number of triangles to which v belongs
template <class Graph, class VProp>
pair<int,int>
get_triangles(typename graph_traits<Graph>::vertex_descriptor v, VProp& mark,
              const Graph& g)
{
    size_t triangles = 0;

    for (auto n : adjacent_vertices_range(v, g))
    {
        if (n == v)
            continue;
        mark[n] = true;
    }

    for (auto n : adjacent_vertices_range(v, g))
    {
        if (n == v)
            continue;
        for (auto n2 : adjacent_vertices_range(n, g))
        {
            if (n2 == n)
                continue;
            if (mark[n2])
                ++triangles;
        }
    }

    for (auto n : adjacent_vertices_range(v, g))
        mark[n] = false;

    size_t k = out_degree(v, g);
    if (graph_tool::is_directed(g))
        return make_pair(triangles, (k * (k - 1)));
    else
        return make_pair(triangles / 2, (k * (k - 1)) / 2);
}

// retrieves the global clustering coefficient
struct get_global_clustering
{
    template <class Graph>
    void operator()(const Graph& g, double& c, double& c_err) const
    {
        size_t triangles = 0, n = 0;
        vector<bool> mask(num_vertices(g), false);

        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(mask) reduction(+:triangles, n)
        parallel_vertex_loop_no_spawn
                (g,
                 [&](auto v)
                 {
                     auto temp = get_triangles(v, mask, g);
                     triangles += temp.first;
                     n += temp.second;
                 });
        c = double(triangles) / n;

        // "jackknife" variance
        c_err = 0.0;
        double cerr = 0.0;
        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(mask) reduction(+:cerr)
        parallel_vertex_loop_no_spawn
                (g,
                 [&](auto v)
                 {
                     auto temp = get_triangles(v, mask, g);
                     double cl = double(triangles - temp.first) /
                         (n - temp.second);
                     cerr += power(c - cl, 2);
                 });
        c_err = sqrt(cerr);
    }
};

// sets the local clustering coefficient to a property
struct set_clustering_to_property
{
    template <class Graph, class ClustMap>
    void operator()(const Graph& g, ClustMap clust_map) const
    {
        typedef typename property_traits<ClustMap>::value_type c_type;
        vector<bool> mask(num_vertices(g), false);

        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(mask)
        parallel_vertex_loop_no_spawn
            (g,
             [&](auto v)
             {
                 auto triangles = get_triangles(v, mask, g);
                 double clustering = (triangles.second > 0) ?
                     double(triangles.first)/triangles.second :
                     0.0;
                 clust_map[v] = c_type(clustering);
             });
    }

    template <class Graph>
    struct get_undirected_graph
    {
        typedef typename mpl::if_
           <std::is_convertible<typename graph_traits<Graph>::directed_category,
                                directed_tag>,
            const undirected_adaptor<Graph>,
            const Graph& >::type type;
    };
};

} //graph-tool namespace

#endif // GRAPH_CLUSTERING_HH