// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2016-2018 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_BOX_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_BOX_BOX_HPP

#include <boost/config.hpp>
#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

namespace details
{

template <typename ReturnType>
class cross_track_box_box_generic
{
public :

    template
    <
            typename Box1,
            typename Box2,
            typename Strategy
    >
    ReturnType static inline apply (Box1 const& box1,
                                    Box2 const& box2,
                                    Strategy ps_strategy)
    {

        // this method assumes that the coordinates of the point and
        // the box are normalized

        typedef typename point_type<Box1>::type box_point_type1;
        typedef typename point_type<Box2>::type box_point_type2;

        box_point_type1 bottom_left1, bottom_right1, top_left1, top_right1;
        geometry::detail::assign_box_corners(box1,
                                             bottom_left1, bottom_right1,
                                             top_left1, top_right1);

        box_point_type2 bottom_left2, bottom_right2, top_left2, top_right2;
        geometry::detail::assign_box_corners(box2,
                                             bottom_left2, bottom_right2,
                                             top_left2, top_right2);

        ReturnType lon_min1 = geometry::get_as_radian<0>(bottom_left1);
        ReturnType const lat_min1 = geometry::get_as_radian<1>(bottom_left1);
        ReturnType lon_max1 = geometry::get_as_radian<0>(top_right1);
        ReturnType const lat_max1 = geometry::get_as_radian<1>(top_right1);

        ReturnType lon_min2 = geometry::get_as_radian<0>(bottom_left2);
        ReturnType const lat_min2 = geometry::get_as_radian<1>(bottom_left2);
        ReturnType lon_max2 = geometry::get_as_radian<0>(top_right2);
        ReturnType const lat_max2 = geometry::get_as_radian<1>(top_right2);

        ReturnType const two_pi = math::two_pi<ReturnType>();

        // Test which sides of the boxes are closer and if boxes cross
        // antimeridian
        bool right_wrap;

        if (lon_min2 > 0 && lon_max2 < 0) // box2 crosses antimeridian
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(box2 crosses antimeridian)";
#endif
            right_wrap = lon_min2 - lon_max1 < lon_min1 - lon_max2;
            lon_max2 += two_pi;
            if (lon_min1 > 0 && lon_max1 < 0) // both boxes crosses antimeridian
            {
                lon_max1 += two_pi;
            }
        }
        else if (lon_min1 > 0 && lon_max1 < 0) // only box1 crosses antimeridian
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(box1 crosses antimeridian)";
#endif
            return apply(box2, box1, ps_strategy);
        }
        else
        {
            right_wrap = lon_max1 <= lon_min2
                         ? lon_min2 - lon_max1 < two_pi - (lon_max2 - lon_min1)
                         : lon_min1 - lon_max2 > two_pi - (lon_max1 - lon_min2);

        }

        // Check1: if box2 crosses the band defined by the
        // minimum and maximum longitude of box1; if yes, determine
        // if the box2 is above, below or intersects/is inside box1 and compute
        // the distance (easy in this case)

        bool lon_min12 = lon_min1 <= lon_min2;
        bool right = lon_max1 <= lon_min2;
        bool left = lon_min1 >= lon_max2;
        bool lon_max12 = lon_max1 <= lon_max2;

        if ((lon_min12 && !right)
                || (!left && !lon_max12)
                || (!lon_min12 && lon_max12))
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(up-down)\n";
#endif
            if (lat_min1 > lat_max2)
            {
                return geometry::strategy::distance::services::result_from_distance
                    <
                        Strategy, box_point_type1, box_point_type2
                    >::apply(ps_strategy, ps_strategy.get_distance_strategy()
                               .meridian(lat_min1, lat_max2));
            }
            else if (lat_max1 < lat_min2)
            {
                return geometry::strategy::distance::services::result_from_distance
                    <
                        Strategy, box_point_type1, box_point_type2
                    >::apply(ps_strategy, ps_strategy.get_distance_strategy().
                             meridian(lat_min2, lat_max1));
            }
            else
            {
                //BOOST_GEOMETRY_ASSERT(plat >= lat_min && plat <= lat_max);
                return ReturnType(0);
            }
        }

        // Check2: if box2 is right/left of box1
        // the max lat of box2 should be less than the max lat of box1
        bool bottom_max;

        ReturnType top_common = (std::min)(lat_max1, lat_max2);
        ReturnType bottom_common = (std::max)(lat_min1, lat_min2);

        // true if the closest points are on northern hemisphere
        bool north_shortest = math::abs(top_common) > math::abs(bottom_common)
                || lat_max1 <= lat_min2
                || lat_min1 >= lat_max2;

        if (north_shortest)
        {
            bottom_max = lat_max1 >= lat_max2;
        }
        else
        {
            bottom_max = lat_min1 <= lat_min2;
        }

#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
        std::cout << "(diagonal)";
#endif
        if (bottom_max && !right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(bottom left)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_right2, top_left1, bottom_left1);
            }
            return ps_strategy.apply(bottom_right2, top_left1, bottom_left1);
        }
        if (bottom_max && right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(bottom right)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_left2, top_right1, bottom_right1);
            }
            return ps_strategy.apply(bottom_left2, top_right1, bottom_right1);
        }
        if (!bottom_max && !right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(top left)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_left1, top_right2, bottom_right2);
            }
            return ps_strategy.apply(bottom_left1, top_right2, bottom_right2);
        }
        if (!bottom_max && right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(top right)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_right1, top_left2, bottom_left2);
            }
            return ps_strategy.apply(bottom_right1, top_left2, bottom_left2);
        }
        return ReturnType(0);
    }
};

} //namespace details

/*!
\brief Strategy functor for distance box to box calculation
\ingroup strategies
\details Class which calculates the distance of a box to a box, for
boxes on a sphere or globe
\tparam CalculationType \tparam_calculation
\tparam Strategy underlying point-segment distance strategy, defaults
to cross track
\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}
*/
template
<
    typename CalculationType = void,
    typename Strategy = cross_track<CalculationType>
>
class cross_track_box_box
{
public:
    template <typename Box1, typename Box2>
    struct return_type
        : services::return_type<Strategy,
                                typename point_type<Box1>::type,
                                typename point_type<Box2>::type>
    {};

    typedef typename Strategy::radius_type radius_type;

    inline cross_track_box_box()
    {}

    explicit inline cross_track_box_box(typename Strategy::radius_type const& r)
        : m_ps_strategy(r)
    {}

    inline cross_track_box_box(Strategy const& s)
        : m_ps_strategy(s)
    {}

    // It might be useful in the future
    // to overload constructor with strategy info.
    // crosstrack(...) {}

    template <typename Box1, typename Box2>
    inline typename return_type<Box1, Box2>::type
    apply(Box1 const& box1, Box2 const& box2) const
    {
#if !defined(BOOST_MSVC)
        BOOST_CONCEPT_ASSERT
            (
                (concepts::PointSegmentDistanceStrategy
                    <
                        Strategy,
                        typename point_type<Box1>::type,
                        typename point_type<Box2>::type
                    >)
            );
#endif
        typedef typename return_type<Box1, Box2>::type return_type;
        return details::cross_track_box_box_generic
                                <return_type>::apply(box1, box2, m_ps_strategy);
    }

    inline typename Strategy::radius_type radius() const
    {
        return m_ps_strategy.radius();
    }

private:
    Strategy m_ps_strategy;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType, typename Strategy>
struct tag<cross_track_box_box<CalculationType, Strategy> >
{
    typedef strategy_tag_distance_box_box type;
};

template <typename CalculationType, typename Strategy, typename Box1, typename Box2>
struct return_type<cross_track_box_box<CalculationType, Strategy>, Box1, Box2>
    : cross_track_box_box
        <
            CalculationType, Strategy
        >::template return_type<Box1, Box2>
{};

template <typename CalculationType, typename Strategy>
struct comparable_type<cross_track_box_box<CalculationType, Strategy> >
{
    typedef cross_track_box_box
        <
            CalculationType, typename comparable_type<Strategy>::type
        > type;
};

template <typename CalculationType, typename Strategy>
struct get_comparable<cross_track_box_box<CalculationType, Strategy> >
{
    typedef cross_track_box_box<CalculationType, Strategy> this_strategy;
    typedef typename comparable_type<this_strategy>::type comparable_type;

public:
    static inline comparable_type apply(this_strategy const& strategy)
    {
        return comparable_type(strategy.radius());
    }
};

template <typename CalculationType, typename Strategy, typename Box1, typename Box2>
struct result_from_distance
    <
        cross_track_box_box<CalculationType, Strategy>, Box1, Box2
    >
{
private:
    typedef cross_track_box_box<CalculationType, Strategy> this_strategy;

    typedef typename this_strategy::template return_type
        <
            Box1, Box2
        >::type return_type;

public:
    template <typename T>
    static inline return_type apply(this_strategy const& strategy,
                                    T const& distance)
    {
        Strategy s(strategy.radius());

        return result_from_distance
            <
                Strategy,
                typename point_type<Box1>::type,
                typename point_type<Box2>::type
            >::apply(s, distance);
    }
};

// define cross_track_box_box<default_point_segment_strategy> as
// default box-box strategy for the spherical equatorial coordinate system
template <typename Box1, typename Box2, typename Strategy>
struct default_strategy
    <
        box_tag, box_tag, Box1, Box2,
        spherical_equatorial_tag, spherical_equatorial_tag,
        Strategy
    >
{
    typedef cross_track_box_box
        <
            void,
            typename boost::mpl::if_
                <
                    boost::is_void<Strategy>,
                    typename default_strategy
                        <
                            point_tag, segment_tag,
                            typename point_type<Box1>::type, typename point_type<Box2>::type,
                            spherical_equatorial_tag, spherical_equatorial_tag
                        >::type,
                    Strategy
                >::type
        > type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_BOX_BOX_HPP


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_BA_PCG_HPP__
#define __OPTIMISATION2_BA_PCG_HPP__

#include <libv/lma/time/tictoc.hpp>
#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/ttt/traits/naming.hpp>
#include <libv/lma/lm/omp/omp.hpp>
#include <boost/fusion/include/as_map.hpp>
#include <boost/fusion/include/fold.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/transform.hpp>

#include <iomanip>
#include <exception>

#include "make_type.hpp"
#include "utils.hpp"

namespace lma
{
  template<class T> bool is_zero_or_infinite(T x) { return ((x == 0.0) || (std::isinf(x))); }

  template<class S> struct AssignSameDiagInv
  {
    const S& s;
    AssignSameDiagInv(const S& s_):s(s_){}
    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Pair<Key,Key>,Cont>& obj) const
    {
      obj.second.set_diag_inv(boost::fusion::at_key<Pair<Key,Key>>(s));
    }

    template<class T> void operator()(T&) const {}
  };

  template<class D, class S> void assign_same_diag_inv(D& d, const S& s)
  {
    boost::fusion::for_each(d,AssignSameDiagInv<S>(s));
  }

  struct Minus
  {
    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& obj) const
    { this->operator()(obj.second); }

    template<class T> void operator()(T& obj) const { obj.minus(); }
  };

  template<class A> struct Dot
  {
    typedef double result_type;
    const A& a;

    Dot(const A& a_):a(a_){}

    template<class Key,class Cont, template<class,class> class Pair> result_type operator()(const result_type& prev, const Pair<Key,Cont>& pair) const
    { return this->operator()<Cont>(prev,pair.second,boost::fusion::at_key<Key>(a)); }

    template<class Cont1, class Cont2> result_type operator()(const result_type& result, const Cont1& cont, const Cont2& b) const
    {
      return result + cont.compute_dot(b);
    }
  };

  template<class A> Dot<A> make_dot(const A& a) { return Dot<A>(a); }

  template<class P, class A, class B> struct ProdDiag21
  {
    P& p;
    const A& a;
    const B& b;
    ProdDiag21(P& p_, const A& a_, const B& b_):p(p_),a(a_),b(b_){}

    template<class Key> void operator()(ttt::wrap<Key>)
    {

      auto& residu = bf::at_key<Key>(p);
      const auto& refa = bf::at_key<bf::pair<Key,Key>>(a);
//       clement(a);
//       std::cout << ttt::name<Key>() << std::endl;
//       std::cout << ttt::name<A>() << std::endl;
      const auto& refb = bf::at_key<Key>(b);
      residu.resize(refb.size());
//       #pragma omp parallel for if(use_omp())
      for(auto i = residu.first() ; i < residu.size() ; ++i)
        residu(i) = refa(i,0) * refb(i);
//       prod_trig_sup(residu,refa,refb);
    }
  };

  template<class Scalar, class V> struct DecScalarProdV
  {
    const Scalar scalar;
    const V& v;

    DecScalarProdV(const Scalar& scalar_, const V& v_):scalar(scalar_),v(v_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref = boost::fusion::at_key<Key>(v);
      result.resize(ref.size());
//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) -= scalar * ref(i);
    }
  };

  template<class Scalar, class V> struct IncScalarProdV
  {
    const Scalar scalar;
    const V& v;

    IncScalarProdV(const Scalar& scalar_, const V& v_):scalar(scalar_),v(v_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref = boost::fusion::at_key<Key>(v);
      result.resize(ref.size());
//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) += scalar * ref(i);
    }
  };

  //     p = -y + beta * p;
  template<class A, class B, class C> struct MinusPlusScalarProdV
  {
    const A& a;
    const B b;
    const C& c;
    MinusPlusScalarProdV(const A& a_, const B& b_, const C& c_):a(a_),b(b_),c(c_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref1 = boost::fusion::at_key<Key>(a);
      const auto& ref2 = boost::fusion::at_key<Key>(c);

//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) =  - ref1(i) + b * ref2(i);
    }
  };

  template<class A, class B, class C> struct PlusScalarProdV
  {
    const A& a;
    const B b;
    const C& c;
    PlusScalarProdV(const A& a_, const B& b_, const C& c_):a(a_),b(b_),c(c_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref1 = boost::fusion::at_key<Key>(a);
      const auto& ref2 = boost::fusion::at_key<Key>(c);

//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) =  ref1(i) + b * ref2(i);
    }
  };

  template<class A, class B> struct AssignSame2
  {
    A& a;
    const B& b;
    AssignSame2(A& a_, const B& b_):a(a_),b(b_){}

    template<class Key> void operator()(ttt::wrap<Key>)
    {
      auto& refa = boost::fusion::at_key<Key>(a);
      const auto& refb = boost::fusion::at_key<Key>(b);

      if (refa.size() != refb.size())
      {
        std::cout << " DIFF ! " << ttt::name<Key>() << std::endl;
        std::cout << refa.name() << std::endl;
        std::cout << refb.name() << std::endl;
        std::cout << refa.size() << " , " << refb.size() << std::endl;
        getchar();
      }
      refa = refb;

      assert(!refa.is_invalid());

    }
  };

  template<class B, class Z> struct BMINUSZ
  {
    const B& b;
    const Z& z;
    BMINUSZ(const B& b_, const Z& z_):b(b_),z(z_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref1 = boost::fusion::at_key<Key>(b);
      const auto& ref2 = boost::fusion::at_key<Key>(z);

//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) =  ref1(i) - ref2(i);
    }
  };

  template<class Scalar, class V> IncScalarProdV<Scalar,V> incscalarprodv(const Scalar& a, const V& v) { return IncScalarProdV<Scalar,V>(a,v); }

  template<class Scalar, class V> DecScalarProdV<Scalar,V> decscalarprodv(const Scalar& a, const V& v) { return DecScalarProdV<Scalar,V>(a,v); }

  template<class A, class B, class C> MinusPlusScalarProdV<A,B,C> minus_plus_scalar_prod_v(const A& a, const B& b, const C& c){ return MinusPlusScalarProdV<A,B,C>(a,b,c); }

  template<class A, class B, class C> PlusScalarProdV<A,B,C> plus_scalar_prod_v(const A& a, const B& b, const C& c){ return PlusScalarProdV<A,B,C>(a,b,c); }

  template<class A, class B> AssignSame2<A,B> assign_same2(A& a, const B& b){ return AssignSame2<A,B>(a,b); }

  template<class B, class Z> BMINUSZ<B,Z> b_minus_z(const B& b, const Z& z) { return BMINUSZ<B,Z>(b,z); }
  struct SquaredNorm
  {
    typedef double result_type;
    template<class Key, class Cont, template<class,class> class Pair> result_type operator()(const result_type& prev, const Pair<Key,Cont>& obj) const
    {
//       std::cout << " Norm : " << prev << " " << obj.second.squaredNorm() << std::endl;
      return prev + obj.second.squaredNorm();
    }
  };

  template<class Tuple> double norm(const Tuple& t)
  {
    return std::sqrt(bf::fold(t,0.0,SquaredNorm()));
  }

  template<class Tuple> double dot(const Tuple& t)
  {
    return bf::fold(t,0.0,make_dot(t));
  }

  struct PcgConfig
  {
    double seuil;
    size_t max_iteration;
    size_t frequence;
    PcgConfig(double s = 0.9999, size_t m = 100, size_t f = -1):seuil(s),max_iteration(m),frequence(f){}
  };

  struct PCG : PcgConfig
  {
    PCG(PcgConfig config):PcgConfig(config) {}

    template<class Tag, class Container, class Delta>
    void operator()(const Container& cont, Delta& delta, const Tag&)
    {
      //Pour la notation et la méthode cf Méthode numérique appliquées, A. Gourdin, M. Boumahrat : page 258,259
      static const bool disp = false;

      typedef typename Tag::second_type Float;
      typedef typename Container::OptimizeKeys ListeParametre;
      typedef typename mpl::transform<
				      ListeParametre,
				      MakeTupleTable<mpl::_1,mpl::_1,Tag>
				     >::type ListeDiag;
      typedef typename mpl::transform<ListeParametre, VectorToPairStruct<mpl::_1,Tag> >::type ListeVector;
      typedef typename br::as_map<ListeVector>::type TupleResidu;
      typedef typename br::as_map<ListeDiag>::type TupleDiag;

      TupleDiag C;
      TupleResidu r,S,p,x;

      Float r0y0 = 0,r1y1 = 0;
      Float residualNorm2=std::numeric_limits<Float>::max(),residualNorm2Initial = 0;

      //r0 = b
      r = cont.B();// - mat * x;

      residualNorm2Initial = norm(r);//bf::fold(residual,0.0,dot(residual));
//       std::cout << " Initial norm : " << residualNorm2Initial << std::endl;

      if (residualNorm2Initial==0) return;

//       cont.get_preconditionner(C);

      assign_same_diag_inv(C,cont.A());// return le preconditionner inversé

      //p = C * r
//       bf::for_each(p,prod_diag_21(C,r));
      mpl::for_each<ListeParametre,ttt::wrap<mpl::_1>>(ProdDiag21<TupleResidu,TupleDiag,TupleResidu>(p,C,r));

      // S = p
      S = p;

      size_t max_element = nb_element(r);
      max_iteration = std::min(max_iteration,max_element);

      //if (cpt > max_element) std::cerr << __FILE__ << " : " << __LINE__ <<  " cpt > max_element : " << cpt << " > " << max_element << std::endl;

//       std::cout << " S \n " << to_vect(S).transpose() << std::endl;
      // r0y0 = r * S
      r0y0 = bf::fold(r,0.0,make_dot(S));
//       std::cout << " r0y0 " << r0y0 << std::endl;

//       std::cout << " Diff r0y0" << std::abs(to_vect(r).dot(to_vect(S)) - r0y0) <<  std::endl;
//       r0y0 = to_vect(r).dot(to_vect(S));
//       assert(  to_vect(r).dot(to_vect(S)) == r0y0 );
      if (r0y0==0) {std::cerr << " r0y0 = " << r0y0 << std::endl;return;}

      std::size_t it = 0;
// /*
      for( ; it < max_iteration ; ++it)
      {
        TupleResidu Ap;
        // AP = s * p
        // on fait le calcul ailleurs pour gérer les deux cas : S implicite ou explicite
        cont.prodAP(Ap,p);
// 	mpl::for_each<typename Container::TypesA,ttt::wrap<mpl::_1>>(prod_ap_p(Ap,cont.A(),p));

        // denom = p * Ap
        Float denom = bf::fold(p,0.0,make_dot(Ap));

//         std::cout << " Diff denom " << std::abs(to_vect(p).dot(to_vect(Ap)) - denom) <<  std::endl;
//         denom = to_vect(p).dot(to_vect(Ap));

        if (std::isinf(denom)) throw INF_ERROR("PCG denom");

        if (std::isnan(denom)) { std::cerr << color.bold() << color.red() <<  " PCG : p.dot(Ap) == NAN " << color.reset() << std::endl; throw NAN_ERROR("PCG: isnan(p*Ap)"); }
        if (denom==0) { std::cerr << color.bold() << color.red() <<  " PCG : p.dot(Ap) == 0 " << color.reset() << std::endl; throw NAN_ERROR("PCG: p*Ap==0"); }

//         std::cout << " numerateur   " << r0y0 << std::endl;
//         std::cout << " denominateur " << denom << std::endl;

        const Float alpha = r0y0 / denom;

        if (std::isinf(denom)) throw INF_ERROR("PCG alpha");

        if (it !=0 && it%frequence==0)
        {
//         si r dérive, on peut utiliser (mais c'est plus lent!) :
//         r = b - Ax
          TupleResidu Ax;
          cont.prodAP(Ax,x);
          bf::for_each(r,b_minus_z(cont.B(),Ax));
        }
        else
        {
        // r -= alpha * Ap
          bf::for_each(r,decscalarprodv(alpha,Ap));
        }

        residualNorm2 = norm(r);
//         std::cout << alpha << " ";
        // x += alpha * p
//         std::cout << to_vect(x) << std::endl << std::endl;
//         std::cout << to_vect(p) << std::endl;
//         auto x2 = (to_vect(x)  + alpha * to_vect(p)).eval();
        bf::for_each(x,incscalarprodv(alpha,p));

//         std::cout << (to_vect(x) - x2).transpose() << std::endl;
//         std::cout << " residu : " << residualNorm2 << std::endl;

        if (std::abs(residualNorm2-residualNorm2Initial)/residualNorm2Initial > seuil && residualNorm2<residualNorm2Initial)
        {
//           std::cout << " break condition " << std::endl;
          break;
        }

        // S = invM * r
// //         bf::for_each(S,prod_diag_21(C,r));
	  mpl::for_each<ListeParametre,ttt::wrap<mpl::_1>>(ProdDiag21<TupleResidu,TupleDiag,TupleResidu>(S,C,r));

        // r1y1 = r.dot(S);
        r1y1 = bf::fold(r,0.0,make_dot(S));

//         std::cout << " Diff r1y1 " << std::abs(to_vect(r).dot(to_vect(S)) - r1y1) <<  std::endl;
//         r1y1 = to_vect(r).dot(to_vect(S));
        //if (r1y1>r0y0) break;
        if(is_zero_or_infinite(r1y1)) {std::cout << " r1y1 is shitted ..." << std::endl;break;}//throw ZeroOrInfiniteError("PCG r1y1");

//         if(disp) std::cout << color.magenta() << " r1y1 = " << r1y1 << color.reset() << std::endl;

        const Float beta = r1y1 / r0y0;

        if(disp) std::cout << color.magenta() << " beta = " << beta << color.reset() << std::endl;

        if (is_zero_or_infinite(beta)) ZeroOrInfiniteError("PCG beta");
        // p = S + beta * p;
        bf::for_each(p,plus_scalar_prod_v(S,beta,p));

        r0y0 = r1y1;

//         std::cout << " critère : " << residualNorm2 << " / " << residualNorm2Initial << " = " << residualNorm2 / residualNorm2Initial << std::endl;
//         if (residualNorm2 / residualNorm2Initial < 0.1) break;
//         std::cout << " residual[" << it << "]: " << r1y1 << std::endl;
      }

//      std::cout << " nb iteration = " << it << " / " << max_iteration << std::endl;
//       std::cout <<  "final residual : " << residualNorm2 << std::endl;
//     std::cout << " FINAL [" << it << "]: " << r1y1 << std::endl;
      mpl::for_each<ListeParametre,ttt::wrap<mpl::_1>>(assign_same2(delta,x));
//       std::cout << " my pcg     :  " << to_matv(x).transpose() << std::endl;

    }
  };
}// eon

namespace ttt
{
  template<> struct Name<lma::PCG> { static std::string name(){ return "SSparsePCG"; } };
}

/*
void solve(const MatD& S, VecD& delta, const VecD& E, const std::size_t& nb_camera)
{
  const std::size_t size = nb_camera * C;

  double seuil = 1e-16;   //à choisir entre 1e-8 et 1e-16 (influe sur le nombre d'itérations du PCG et sa précision)

  double residual, residual_first;

  VecD r = -E;
  VecD x = VecD::Zero(size);
  residual_first = r.dot(r);

  VectorCC invMv(nb_camera);
  for(std::size_t i = 0 ; i < nb_camera ; ++i)
    invMv[i] = S.inverse();

  VecD y = VecD::Zero(size);

  for(std::size_t i = 0 ; i < nb_camera ; ++i)
    BlockC1::view(y,i*C,0) = invMv[i] * BlockC1::view(r,i*C,0);

  VecD p = - y;
  double r0y0 = r.dot(y), r1y1;

  for(std::size_t it = 0 ; it < size ; ++it)
  {
    VecD Ap = S * p;

    double temp = p.dot(Ap) ;

    if (std::isnan(temp)) { V_TXT(" PCG : p.dot(Ap) == NAN "); break; }

    const double alpha = r0y0 / temp;

    x += alpha * p;
    r += alpha * Ap;
    residual = r.dot(r);

    if ( residual < seuil * residual_first ) {break;}

    for(std::size_t i = 0 ; i < nb_camera ; ++i)
      BlockC1::view(y,i*C,0) = invMv[i] * BlockC1::view(r,i*C,0);

    r1y1 = r.dot(y);

    const double beta = r1y1 / r0y0;
    p = -y + beta * p;

    r0y0 = r1y1;
  }
  delta = x;
}
*/

#endif


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <Eigen/Geometry>
#include <comma/math/compare.h>
#include "range_bearing_elevation.h"

namespace snark {

Eigen::Vector2d bearing::to_cartesian( const double radians, const double radius )
{
    return Eigen::Vector2d( radius * std::sin( radians ), radius * std::cos( radians ) );
}

double bearing::from_cartesian( const double x, const double y )
{
    double radians = ( M_PI / 2 ) - std::atan2( y, x );
    return radians < 0 ? radians + 2 * M_PI : radians;
}

bearing_elevation::bearing_elevation() : bearing_( 0 ), elevation_( 0 ) {}

bearing_elevation::bearing_elevation( double b, double e ) : bearing_( bearing( b ) ), elevation_( elevation( e ) ) {}

double bearing_elevation::bearing() const { return bearing_; }

double bearing_elevation::elevation() const { return elevation_; }

double bearing_elevation::b() const { return bearing(); }

double bearing_elevation::e() const { return elevation(); }

double bearing_elevation::b( double b ) { return bearing( b ); }

double bearing_elevation::e( double e ) { return elevation( e ); }

static double mod_( double t, double m ) // quick and dirty, because std::fmod is ridiculously slow
{
    int r = std::abs( t / m );
    return comma::math::less( t, 0 ) ? ( t + m * r ) : ( t - m * r );
}

double bearing_elevation::bearing( double t )
{
    double b( mod_( t, ( double )( M_PI * 2 ) ) ); //double b( std::fmod( t, ( double )( M_PI * 2 ) ) );
    if( !comma::math::less( b, M_PI ) ) { b -= ( M_PI * 2 ); }
    else if( comma::math::less( b, -M_PI ) ) { b += ( M_PI * 2 ); }
    bearing_ = b;
    return bearing_;
}

double bearing_elevation::elevation( double t )
{
    double e( mod_( t, ( double )( M_PI * 2 ) ) ); //double e( std::fmod( t, ( double )( M_PI * 2 ) ) );
    if( comma::math::less( e, 0 ) ) { e += M_PI * 2; }
    if( !comma::math::less( e, M_PI / 2 ) )
    {
        if( !comma::math::less( e, M_PI ) )
        {
            if( !comma::math::less( e, M_PI * 3 / 2 ) )
            {
                e = e - M_PI * 2;
            }
            else
            {
                e = M_PI - e;
                bearing( bearing() + M_PI );
            }
        }
        else
        {
            e = M_PI - e;
            bearing( bearing() + M_PI );
        }
    }
    elevation_ = e;
    return elevation_;
}

range_bearing_elevation::range_bearing_elevation() { range( 0 ); }

range_bearing_elevation::range_bearing_elevation( double r, double b, double e )
{
    bearing_elevation_.bearing( b );
    bearing_elevation_.elevation( e );
    range( r );
}

double range_bearing_elevation::range( double t )
{
    if( !comma::math::less( t, 0 ) )
    {
        range_ = t;
    }
    else
    {
        range_ = -t;
        bearing_elevation_.bearing( -bearing_elevation_.bearing() );
        bearing_elevation_.elevation( -bearing_elevation_.elevation() );
    }
    return range_;
}

Eigen::Vector3d range_bearing_elevation::to_cartesian() const
{
    double xy_projection( range() * std::cos( elevation() ) );
    return ::Eigen::Matrix< double, 3, 1 >( xy_projection * std::cos( bearing() )
                                    , xy_projection * std::sin( bearing() )
                                    , range() * std::sin( elevation() ) );
}

const range_bearing_elevation& range_bearing_elevation::from_cartesian( double x, double y, double z )
{
    return from_cartesian( Eigen::Vector3d( x, y, z ) );
}

const range_bearing_elevation& range_bearing_elevation::from_cartesian( const Eigen::Vector3d& v )
{
    range_ = v.norm();
    if( comma::math::equal( range_, 0 ) ) { bearing_elevation_ = snark::bearing_elevation( 0, 0 ); return *this; }
    const Eigen::AngleAxis< double >& a =  Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( v, Eigen::Vector3d( 0, 0, 1 ) ) );
    double e = M_PI / 2 - a.angle();
    Eigen::AngleAxis< double > c( -e, a.axis() );
    const Eigen::Matrix3d& r = c.toRotationMatrix();
    Eigen::AngleAxis< double > d( Eigen::Quaternion< double >::FromTwoVectors( Eigen::Vector3d( 1, 0, 0 ), r * v ) );
    bearing_elevation_.bearing( d.angle() * ( d.axis().z() < 0 ? -1 : 1 ) );
    bearing_elevation_.elevation( e );
    return *this;
}

// const range_bearing_elevation& range_bearing_elevation::from_cartesian( const Eigen::Vector3d& xyz )
// {
//     return from_cartesian( xyz[0], xyz[1], xyz[2] );
// }
//
// const range_bearing_elevation& range_bearing_elevation::from_cartesian( double x, double y, double z )
// { // todo: use rotation matrices instead!
//     long double projection_square ( x * x +  y * y );
//     if ( comma::math::equal( projection_square, 0 ) )
//     {
//         if ( comma::math::equal( z, 0 ) )
//         {
//             range_ = 0;
//             bearing_elevation_.bearing( 0 );
//             bearing_elevation_.elevation( 0 );
//             return *this;
//         }
//         range_ = std::abs( z );
//         bearing_elevation_.bearing( 0 );
//         elevation( comma::math::less( z, 0 ) ? -M_PI / 2 : M_PI / 2 );
//         return *this;
//     }
//     long double range_square( projection_square + z * z );
//     long double lr( std::sqrt( range_square ) );
//     long double elevation_ = 0;
//     if ( !comma::math::equal( z, 0 ) )
//     {
//         long double r = z / lr;
//         if ( comma::math::less( ( long double )( 1.0 ), r ) ) { r = 1; }
//         else if ( comma::math::less( r, ( long double ) ( -1.0 ) ) ) { r = -1; }
//         elevation_ = std::asin( r );
//     }
//     long double r = x / std::sqrt( projection_square );
//     if ( comma::math::less( ( long double )( 1.0 ), r ) ) { r = 1; }
//     else if ( comma::math::less( r, ( long double ) ( -1.0 ) ) ) { r = -1; }
//     long double bearing_ = std::acos( r );
//     if ( comma::math::less( y, 0 ) ) { bearing_ = M_PI * 2 - bearing_; }
//     range_ = lr;
//     bearing( bearing_ );
//     elevation( elevation_ );
//     return *this;
// }

Eigen::AngleAxis< double > great_circle_angle_axis( const bearing_elevation& lhs, const bearing_elevation& rhs )
{
    Eigen::Vector3d a = rbe( 1, lhs.bearing(), lhs.elevation() ).to_cartesian();
    Eigen::Vector3d b = rbe( 1, rhs.bearing(), rhs.elevation() ).to_cartesian();
    return Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( a, b ) );
}

} // namespace snark {


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

#pragma once

/// @file
/// Code for dealing with Green's functions for 1 dimension in real
/// space.

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <boost/filesystem.hpp>
#include <dynamical_matrix.hpp>
#include <qpoint_grid.hpp>
#include <vasp_io.hpp>

namespace alma {
/// Objects of this class enable the calling code to compute 1d
/// Green's functions along particular directions in a bulk
/// material.
class Green1d_factory {
public:
    /// Number of q points in the grid.
    const std::size_t nqpoints;
    /// Number of degrees of freedom in the unit cell.
    const std::size_t ndof;
    /// Basic constructor.
    ///
    /// @param[in] structure - description of the crystal lattice
    /// @param[in] q0 - starting point in reciprocal space, in
    /// Cartesian coordinates
    /// @param[in] normal - direction of the path in reciprocal
    /// space, expressed in direct coordinates (only integers are
    /// allowed)
    /// @param[in] nqpoints_ - number of divisions of the segment
    /// @param[in] builder - factory of dynamical matrices
    Green1d_factory(const Crystal_structure& structure,
                    const Eigen::Ref<const Eigen::VectorXd>& q0,
                    const Eigen::Ref<const Eigen::VectorXi>& normal,
                    const std::size_t nqpoints_,
                    const Dynamical_matrix_builder& builder);
    /// Obtain the Green's function for a particular angular frequency.
    ///
    /// @param[in] omega - angular frequency [rad / ps]
    /// @param[in] ncells - number of unit cells to include in the
    /// supercell
    /// @return the Green's function matrix
    Eigen::MatrixXcd build(double omega, std::size_t ncells) const;

    /// Obtain the scattering rate caused by a diagonal perturbation
    /// proportional to the frequency squared
    ///
    /// @param[in] q - wave number of the incident phonon, as a number
    /// between 0 and 2 * pi
    /// @param[in] omega - frequency of the incident phonon [rad / ps]
    /// @param[in] wfin - wave function of the incident phonon over a
    /// single unit cell
    /// @param[in] factors - coefficient of omega ** 2 for each atom
    /// @return a scattering rate in ps^{-1}
    double calc_w0_dmass(
        double q,
        double omega,
        const Eigen::Ref<const Eigen::VectorXcd>& wfin,
        const Eigen::Ref<const Eigen::VectorXd>& factors) const;

private:
    /// Projection of the origin over the direction of interest,
    /// in the interval [0, 2 * pi).
    const double offset;
    /// Wave numbers at which the spectrum is sampled, from 0 to 2 *
    /// pi.
    Eigen::ArrayXd qgrid;
    /// Angular frequencies squared for all modes [rad ** 2 / ps ** 2].
    ///
    /// The first index runs over branches, the second over q points.
    Eigen::ArrayXXd Egrid;
    /// Directional derivative of omega ** 2 along the direction of
    /// interest.
    ///
    /// The first index runs over branches, the second over q points.
    Eigen::ArrayXXd dEgrid;
    /// Wave functions (over a single unit cell) for all modes.
    ///
    /// The main vector index runs over q points.
    std::vector<Eigen::MatrixXcd> wfgrid;
    /// Extend all wave functions at a particular q point from a single
    /// unit
    /// cell to a supercell.
    ///
    /// @param[in] iq - index of the q point
    /// @param[in] ncells - supercell size
    inline Eigen::MatrixXcd get_superwfs(std::size_t iq,
                                         std::size_t ncells) const;

    /// Compute the integration weights of all q points and all
    /// branches for a given energy.
    ///
    /// @param[in] omega - angular frequency squared [rad ** 2 / ps ** 2]
    /// @return the weights as a complex vector
    Eigen::MatrixXcd compute_weights(double omega2) const;
};
} // namespace alma


#include <sstream>
#include <Eigen/Geometry>
#include "Geometry.h"

#include <iostream>

using namespace Eigen;

floatOptional Geometry::intersect(const Ray &r) {
    if (transformed) {
        Vector4f newOrigin;
        newOrigin << r.origin(), 1;
        Vector4f newDir;
        newDir << r.direction(), 0;

        newOrigin = inverseModelMatrix * newOrigin;
        newDir = inverseModelMatrix * newDir;
        const Ray transformedR = Ray(newOrigin.head(3), newDir.head(3));
        return objectIntersect(transformedR);
    }
    else {
        return objectIntersect(r);
    }
}

Eigen::Vector3f Geometry::normalAtPoint(const Eigen::Vector3f &p) {
    if (transformed) {
        Vector4f localP, n;
        localP << p, 1;
        localP = inverseModelMatrix * localP;
        n << objectNormal(localP.head(3)), 0;
        n = inverseModelMatrix.transpose() * n;
        return n.head(3).normalized();
    }
    else {
        return objectNormal(p);
    }
}

Eigen::Vector3f Geometry::minVector(const Eigen::Vector3f &a, const Eigen::Vector3f &b) {
    Vector3f minVec;
    minVec.x() = std::min(a.x(), b.x());
    minVec.y() = std::min(a.y(), b.y());
    minVec.z() = std::min(a.z(), b.z());
    return minVec;
}

Eigen::Vector3f Geometry::maxVector(const Eigen::Vector3f &a, const Eigen::Vector3f &b) {
    Vector3f maxVec;
    maxVec.x() = std::max(a.x(), b.x());
    maxVec.y() = std::max(a.y(), b.y());
    maxVec.z() = std::max(a.z(), b.z());
    return maxVec;
}

void Geometry::worldSpaceBoundingBox(Eigen::Vector3f &min, Eigen::Vector3f &max) {
    // clockwise on bottom, then top
    Vector4f v[8];
    // bottom vertices
    v[0] << min, 1;
    v[1] << max.x(), min.y(), min.z(), 1;
    v[2] << max.x(), min.y(), max.z(), 1;
    v[3] << min.x(), min.y(), max.z(), 1;
    // top vertices
    v[4] << min.x(), max.y(), min.z(), 1;
    v[5] << max.x(), max.y(), min.z(), 1;
    v[6] << max, 1;
    v[7] << min.x(), max.y(), max.z(), 1;

    min << INFINITY, INFINITY, INFINITY;
    max << -INFINITY, -INFINITY, -INFINITY;
    for (int i = 0; i < 8; i++) {
        v[i] = modelMatrix * v[i];
        min = minVector(min, v[i].head(3));
        max = maxVector(max, v[i].head(3));
    }
}

void Geometry::boundingBox(Eigen::Vector3f &min, Eigen::Vector3f &max) {
    objectBoundingBox(min, max);
    if (transformed) {
        worldSpaceBoundingBox(min, max);
    }
}

Eigen::Vector3f Geometry::getCenter() {
    Vector4f localCenter;
    localCenter << objectCenter(), 1;
    if (transformed) {
        localCenter = modelMatrix * localCenter;
    }
    return localCenter.head(3);
}

std::string Geometry::to_string() {
    std::stringstream str;
    str << "- Color: " << formatVector(pigment) << "\n";
    str << "- Material:\n";
    str << "  - Ambient: " << finish.ambient << "\n";
    str << "  - Diffuse: " << finish.diffuse << "\n";
    str << "  - Specular: " << finish.specular << "\n";
    str << "  - Roughness: " << finish.roughness << "\n";
    str << "  - Metallic: " << finish.metallic << "\n";
    str << "  - IOR: " << finish.ior << "\n";
    str << "  - Reflection: " << finish.reflection << "\n";
    str << "  - Refraction: " << finish.filter << "\n";

    return str.str();
}

void Geometry::scale(const Eigen::Vector3f s) {
    Transform<float, 3, Projective> sc;
    sc = Scaling(s);
    modelMatrix = sc.matrix() * modelMatrix;
    transformed = true;
}

void Geometry::rotate(const Eigen::Vector3f r) {
    Transform<float, 3, Projective> rot;
    rot = AngleAxisf(r.z() * M_PI / 180, Vector3f::UnitZ())
        * AngleAxisf(r.y() * M_PI / 180, Vector3f::UnitY())
        * AngleAxisf(r.x() * M_PI / 180, Vector3f::UnitX());
    modelMatrix = rot.matrix() * modelMatrix;
    transformed = true;
}

void Geometry::translate(const Eigen::Vector3f t) {
    Matrix4f tr = Matrix4f::Identity();
    tr.col(3).head(3) = t;
    modelMatrix = tr * modelMatrix;
    transformed = true;
}

void Geometry::finalizeTransform() {
    if (transformed) {
        inverseModelMatrix = modelMatrix.inverse();
    }
}

Ray Geometry::getTransformedRay(const Ray &r) {
    if (transformed) {
        Vector4f newOrigin;
        newOrigin << r.origin(), 1;
        Vector4f newDir;
        newDir << r.direction(), 0;

        newOrigin = modelMatrix * newOrigin;
        newDir = modelMatrix * newDir;
        return Ray(newOrigin.head(3), newDir.head(3));
    }
    else {
        return r;
    }
}


/*
 * Copyright 2009-2011 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/csg/map.h>
#include <votca/csg/topology.h>
#include <iostream>
#include <votca/tools/matrix.h>
#include <votca/tools/tokenizer.h>
#include <numeric>
#include <boost/lexical_cast.hpp>

namespace votca { namespace csg {

using namespace std;

Map::~Map()
{
    vector<BeadMap *>::iterator iter;
    for(iter=_maps.begin();iter!=_maps.end();++iter)
        delete (*iter);
    _maps.clear();
}

void Map::Apply()
{
    vector<BeadMap *>::iterator iter;
    for(iter=_maps.begin();iter!=_maps.end();++iter)
        (*iter)->Apply();
}

void Map_Sphere::Initialize(Molecule *in, Bead *out, Property *opts_bead, Property *opts_map) {
    BeadMap::Initialize(in, out, opts_bead, opts_map);

    vector<string> beads;
    vector<double> weights;
    vector<double> fweights;

    // get the beads
    string s(_opts_bead->get("beads").value());
    Tokenizer tok_beads(s, " \n\t");
    tok_beads.ToVector(beads);

    // get vector of weights
    Tokenizer tok_weights(_opts_map->get("weights").value(), " \n\t");
    tok_weights.ConvertToVector<double>(weights);

    // check weather weights and # beads matches
    if (beads.size() != weights.size())
        throw runtime_error(string("number of subbeads in " +
                opts_bead->get("name").as<string>()
                + " and number of weights in map "
                + opts_map->get("name").as<string>() + " do not match"));

    // normalize the weights
    double norm = 1./ std::accumulate(weights.begin(), weights.end(), 0.);

    transform(weights.begin(), weights.end(), weights.begin(), bind2nd(multiplies<double>(), norm));
    // get the d vector if exists or initialize same as weights
    vector<double> d;
    if(_opts_map->exists("d")) {
        Tokenizer tok_weights(_opts_map->get("d").value(), " \n\t");
        tok_weights.ConvertToVector(d);
        // normalize d coefficients
        norm = 1./std::accumulate(d.begin(), d.end(), 0.);
        transform(d.begin(), d.end(), d.begin(), bind2nd(multiplies<double>(), norm));
    } else {
        // initialize force-weights with weights
        d.resize(weights.size());
        copy(weights.begin(), weights.end(), d.begin());
    }

    // check weather number of d coeffs is correct
    if (beads.size() != d.size()) {
        throw runtime_error(string("number of subbeads in " +
            opts_bead->get("name").as<string>()
            + " and number of d-coefficients in map "
            + opts_map->get("name").as<string>() + " do not match"));
    }

    fweights.resize(weights.size());
    // calculate force weights by d_i/w_i
    for(size_t i=0; i<weights.size(); ++i) {
        if(weights[i] == 0 && d[i]!=0) {
            throw runtime_error(
                "A d coefficient is nonzero while weights is zero in mapping "
                + opts_map->get("name").as<string>());
        }
        if(weights[i] != 0)
            fweights[i] = d[i] / weights[i];
        else
            fweights[i] = 0;
    }

    for (size_t i = 0; i < beads.size(); ++i) {
        int iin = in->getBeadByName(beads[i]);
        if (iin < 0)
            throw std::runtime_error(string("mapping error: molecule " + beads[i] + " does not exist"));
        AddElem(in->getBead(iin), weights[i], fweights[i]);
    }
}

void Map_Sphere::Apply()
{
    vector<element_t>::iterator iter;
    vec cg(0., 0., 0.), f(0.,0.,0.), vel(0.,0.,0.);
    bool bPos, bVel, bF;
    bPos=bVel=bF=false;
    _out->ParentBeads().clear();

    // the following is needed for pbc treatment
    Topology *top = _out->getParent();
    double max_dist = 0.5*top->ShortestBoxSize();
    vec r0 = vec(0,0,0);
    string name0;
    int id0;
    if(_matrix.size() > 0) {
        if(_matrix.front()._in->HasPos()) {
            r0=_matrix.front()._in->getPos();
            name0 = _matrix.front()._in->getName();
            id0 = _matrix.front()._in->getId();
        }
    }

    double M = 0;

    for(iter = _matrix.begin(); iter != _matrix.end(); ++iter) {
        Bead *bead = iter->_in;
        _out->ParentBeads().push_back(bead->getId());
        M+=bead->getM();
        if(bead->HasPos()) {
            vec r = top->BCShortestConnection(r0, bead->getPos());
            if(abs(r) > max_dist) {
                cout << r0 << " " << bead->getPos() << endl;
                throw std::runtime_error("coarse-grained bead is bigger than half the box \n (atoms "
                        + name0 + " (id " + boost::lexical_cast<string>(id0+1) + ")" + ", " + bead->getName() + " (id " + boost::lexical_cast<string>(bead->getId()+1) + ")" +  +" , molecule "
                        + boost::lexical_cast<string>(bead->getMolecule()->getId()+1) + ")" );
            }
            cg += (*iter)._weight * (r+r0);
            bPos=true;
        }
        if(bead->HasVel()) {
            vel += (*iter)._weight * bead->getVel();
            bVel = true;
        }
        if(bead->HasF()) {
            f += (*iter)._force_weight * bead->getF();
            bF = true;
        }
    }
    _out->setM(M);
    if(bPos)
        _out->setPos(cg);
    if(bVel)
        _out->setVel(vel);
    if(bF)
        _out->setF(f);
}

/// \todo implement this function
void Map_Ellipsoid::Apply()
{
    vector<element_t>::iterator iter;
    vec cg(0., 0., 0.), c(0., 0., 0.), f(0.,0.,0.), vel(0.,0.,0.);
    matrix m(0.);
     bool bPos, bVel, bF;
    bPos=bVel=bF=false;

    // the following is needed for pbc treatment
    Topology *top = _out->getParent();
    double max_dist = 0.5*top->ShortestBoxSize();
    vec r0 = vec(0,0,0);
    if(_matrix.size() > 0) {
        if(_matrix.front()._in->HasPos()) {
            r0=_matrix.front()._in->getPos();
        }
    }

    int n;
    n = 0;
    _out->ParentBeads().clear();
    for(iter = _matrix.begin(); iter != _matrix.end(); ++iter) {
       Bead *bead = iter->_in;
       _out->ParentBeads().push_back(bead->getId());
       if(bead->HasPos()) {
            vec r = top->BCShortestConnection(r0, bead->getPos());
            if(abs(r) > max_dist)
                throw std::runtime_error("coarse-grained bead is bigger than half the box");
            cg += (*iter)._weight * (r+r0);
            bPos=true;
        }
        if(bead->HasVel() == true) {
            vel += (*iter)._weight * bead->getVel();
            bVel = true;
        }
        if(bead->HasF()) {
            /// \todo fix me, right calculation should be F_i = m_cg / sum(w_i) * sum(w_i/m_i*F_i)
            //f += (*iter)._weight * _in->getBeadF((*iter)._in);
            f += (*iter)._force_weight * bead->getF();
            bF = true;
        }

        if((*iter)._weight>0 && bead->HasPos()) {
            c += bead->getPos();
            n++;
        }
    }

    if(bPos)
        _out->setPos(cg);
    if(bVel)
        _out->setVel(vel);
    if(bF)
        _out->setF(f);

    if(!_matrix[0]._in->HasPos()) {
        _out->setU(vec(1.0,0,0));
        _out->setV(vec(.0,1,0));
        _out->setW(vec(.0,0,1));
        return;
    }

    // calculate the tensor of gyration
    c=c/(double)n;
    for(iter = _matrix.begin(); iter != _matrix.end(); ++iter) {
        if((*iter)._weight == 0) continue;
        Bead *bead = iter->_in;
            vec v = bead->getPos() - c;
            //v = vec(1, 0.5, 0) * 0.*(drand48()-0.5)
            //    + vec(0.5, -1, 0) * (drand48()-0.5)
            //    + vec(0, 0, 1) * (drand48()-0.5);

            //Normalize the tensor with 1/number_of_atoms_per_bead
            m[0][0] += v.getX()*v.getX()/(double)_matrix.size();
            m[0][1] += v.getX()*v.getY()/(double)_matrix.size();
            m[0][2] += v.getX()*v.getZ()/(double)_matrix.size();
            m[1][1] += v.getY()*v.getY()/(double)_matrix.size();
            m[1][2] += v.getY()*v.getZ()/(double)_matrix.size();
            m[2][2] += v.getZ()*v.getZ()/(double)_matrix.size();

    }
    m[1][0] = m[0][1];
    m[2][0] = m[0][2];
    m[2][1] = m[1][2];

    // calculate the eigenvectors
    matrix::eigensystem_t es;
    m.SolveEigensystem(es);

    //vec eigenv1=es.eigenvecs[0];
    //vec eigenv2=es.eigenvecs[1];
    //vec eigenv3=es.eigenvecs[2];

/*    _out->seteigenvec1(eigenv1);
    _out->seteigenvec2(eigenv2);
    _out->seteigenvec3(eigenv3);
  */

    vec u = es.eigenvecs[0];
    vec v = _matrix[1]._in->getPos() - _matrix[0]._in->getPos();
    v.normalize();

    _out->setV(v);

    vec w = _matrix[2]._in->getPos() - _matrix[0]._in->getPos();
    w.normalize();

    if((v^w)*u < 0) u=vec(0.,0.,0.)-u;
    _out->setU(u);

    //write out w
    w=u^v;
    w.normalize();
    _out->setW(w);

    //out.BeadV(_out) = v;

    //out.BeadW(_out) = es.eigenvecs[2];
}

}}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    LevenbergMarquardtOptimizer.cpp
 * @brief   A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme
 * @author  Richard Roberts
 * @author  Frank Dellaert
 * @author  Luca Carlone
 * @date    Feb 26, 2012
 */

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/internal/LevenbergMarquardtState.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <boost/optional.hpp>
#include <boost/range/adaptor/map.hpp>

#include <cmath>
#include <fstream>
#include <iostream>
#include <limits>
#include <string>

using namespace std;

namespace gtsam {

using boost::adaptors::map_values;
typedef internal::LevenbergMarquardtState State;

/* ************************************************************************* */
LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
                                                         const Values& initialValues,
                                                         const LevenbergMarquardtParams& params)
    : NonlinearOptimizer(
          graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues),
                                                  params.lambdaInitial, params.lambdaFactor))),
      params_(LevenbergMarquardtParams::EnsureHasOrdering(params, graph)) {}

LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
                                                         const Values& initialValues,
                                                         const Ordering& ordering,
                                                         const LevenbergMarquardtParams& params)
    : NonlinearOptimizer(
          graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues),
                                                  params.lambdaInitial, params.lambdaFactor))),
      params_(LevenbergMarquardtParams::ReplaceOrdering(params, ordering)) {}

/* ************************************************************************* */
void LevenbergMarquardtOptimizer::initTime() {
  startTime_ = boost::posix_time::microsec_clock::universal_time();
}

/* ************************************************************************* */
double LevenbergMarquardtOptimizer::lambda() const {
  auto currentState = static_cast<const State*>(state_.get());
  return currentState->lambda;
}

/* ************************************************************************* */
int LevenbergMarquardtOptimizer::getInnerIterations() const {
  auto currentState = static_cast<const State*>(state_.get());
  return currentState->totalNumberInnerIterations;
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const {
  return graph_.linearize(state_->values);
}

/* ************************************************************************* */
GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
    const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal) const {
  gttic(damp);
  auto currentState = static_cast<const State*>(state_.get());

  if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED)
    std::cout << "building damped system with lambda " << currentState->lambda << std::endl;

  if (params_.diagonalDamping)
    return currentState->buildDampedSystem(linear, sqrtHessianDiagonal);
  else
    return currentState->buildDampedSystem(linear);
}

/* ************************************************************************* */
// Log current error/lambda to file
inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
  auto currentState = static_cast<const State*>(state_.get());

  if (!params_.logFile.empty()) {
    ofstream os(params_.logFile.c_str(), ios::app);
    boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
    os << /*inner iterations*/ currentState->totalNumberInnerIterations << ","
        << 1e-6 * (currentTime - startTime_).total_microseconds() << ","
        << /*current error*/ currentError << "," << currentState->lambda << ","
        << /*outer iterations*/ currentState->iterations << endl;
  }
}

/* ************************************************************************* */
bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
                                            const VectorValues& sqrtHessianDiagonal) {
  auto currentState = static_cast<const State*>(state_.get());
  bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA);

#ifdef GTSAM_USING_NEW_BOOST_TIMERS
  boost::timer::cpu_timer lamda_iteration_timer;
  lamda_iteration_timer.start();
#else
  boost::timer lamda_iteration_timer;
  lamda_iteration_timer.restart();
#endif

  if (verbose)
    cout << "trying lambda = " << currentState->lambda << endl;

  // Build damped system for this lambda (adds prior factors that make it like gradient descent)
  auto dampedSystem = buildDampedSystem(linear, sqrtHessianDiagonal);

  // Try solving
  double modelFidelity = 0.0;
  bool step_is_successful = false;
  bool stopSearchingLambda = false;
  double newError = numeric_limits<double>::infinity(), costChange;
  Values newValues;
  VectorValues delta;

  bool systemSolvedSuccessfully;
  try {
    // ============ Solve is where most computation happens !! =================
    delta = solve(dampedSystem, params_);
    systemSolvedSuccessfully = true;
  } catch (const IndeterminantLinearSystemException&) {
    systemSolvedSuccessfully = false;
  }

  if (systemSolvedSuccessfully) {
    if (verbose)
      cout << "linear delta norm = " << delta.norm() << endl;
    if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA)
      delta.print("delta");

    // cost change in the linearized system (old - new)
    double newlinearizedError = linear.error(delta);

    double linearizedCostChange = currentState->error - newlinearizedError;
    if (verbose)
      cout << "newlinearizedError = " << newlinearizedError
           << "  linearizedCostChange = " << linearizedCostChange << endl;

    if (linearizedCostChange >= 0) {  // step is valid
      // update values
      gttic(retract);
      // ============ This is where the solution is updated ====================
      newValues = currentState->values.retract(delta);
      // =======================================================================
      gttoc(retract);

      // compute new error
      gttic(compute_error);
      if (verbose)
        cout << "calculating error:" << endl;
      newError = graph_.error(newValues);
      gttoc(compute_error);

      if (verbose)
        cout << "old error (" << currentState->error << ") new (tentative) error (" << newError
             << ")" << endl;

      // cost change in the original, nonlinear system (old - new)
      costChange = currentState->error - newError;

      if (linearizedCostChange >
          1e-20) {  // the (linear) error has to decrease to satisfy this condition
        // fidelity of linearized model VS original system between
        modelFidelity = costChange / linearizedCostChange;
        // if we decrease the error in the nonlinear system and modelFidelity is above threshold
        step_is_successful = modelFidelity > params_.minModelFidelity;
        if (verbose)
          cout << "modelFidelity: " << modelFidelity << endl;
      }  // else we consider the step non successful and we either increase lambda or stop if error
         // change is small

      double minAbsoluteTolerance = params_.relativeErrorTol * currentState->error;
      // if the change is small we terminate
      if (std::abs(costChange) < minAbsoluteTolerance) {
        if (verbose)
          cout << "abs(costChange)=" << std::abs(costChange)
               << "  minAbsoluteTolerance=" << minAbsoluteTolerance
               << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl;
        stopSearchingLambda = true;
      }
    }
  } // if (systemSolvedSuccessfully)

  if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
// do timing
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
    double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall;
#else
    double iterationTime = lamda_iteration_timer.elapsed();
#endif
    if (currentState->iterations == 0)
      cout << "iter      cost      cost_change    lambda  success iter_time" << endl;

    cout << boost::format("% 4d % 8e   % 3.2e   % 3.2e  % 4d   % 3.2e") % currentState->iterations %
                newError % costChange % currentState->lambda % systemSolvedSuccessfully %
                iterationTime << endl;
  }

  if (step_is_successful) {
    // we have successfully decreased the cost and we have good modelFidelity
    // NOTE(frank): As we return immediately after this, we move the newValues
    // TODO(frank): make Values actually support move. Does not seem to happen now.
    state_ = currentState->decreaseLambda(params_, modelFidelity, std::move(newValues), newError);
    return true;
  } else if (!stopSearchingLambda) {  // we failed to solved the system or had no decrease in cost
    if (verbose)
      cout << "increasing lambda" << endl;
    State* modifiedState = static_cast<State*>(state_.get());
    modifiedState->increaseLambda(params_); // TODO(frank): make this functional with Values move

    // check if lambda is too big
    if (modifiedState->lambda >= params_.lambdaUpperBound) {
      if (params_.verbosity >= NonlinearOptimizerParams::TERMINATION ||
          params_.verbosityLM == LevenbergMarquardtParams::SUMMARY)
        cout << "Warning:  Levenberg-Marquardt giving up because "
                "cannot decrease error with maximum lambda" << endl;
      return true;
    } else {
      return false;  // only case where we will keep trying
    }
  } else {  // the change in the cost is very small and it is not worth trying bigger lambdas
    if (verbose)
      cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
    return true;
  }
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::iterate() {
  auto currentState = static_cast<const State*>(state_.get());

  gttic(LM_iterate);

  // Linearize graph
  if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED)
    cout << "linearizing = " << endl;
  GaussianFactorGraph::shared_ptr linear = linearize();

  if(currentState->totalNumberInnerIterations==0) { // write initial error
    writeLogFile(currentState->error);

    if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
      cout << "Initial error: " << currentState->error
           << ", values: " << currentState->values.size() << std::endl;
    }
  }

  // Only calculate diagonal of Hessian (expensive) once per outer iteration, if we need it
  VectorValues sqrtHessianDiagonal;
  if (params_.diagonalDamping) {
    sqrtHessianDiagonal = linear->hessianDiagonal();
    for (Vector& v : sqrtHessianDiagonal | map_values) {
      v = v.cwiseMax(params_.minDiagonal).cwiseMin(params_.maxDiagonal).cwiseSqrt();
    }
  }

  // Keep increasing lambda until we make make progress
  while (!tryLambda(*linear, sqrtHessianDiagonal)) {
    auto newState = static_cast<const State*>(state_.get());
    writeLogFile(newState->error);
  }

  return linear;
}

} /* namespace gtsam */


/*
 * kaskadeBridge.hh
 *
 *  Created on: 06.12.2013
 *      Author: Lars Lubkoll, Anton Schiela
 */

#ifndef KASKADE_BRIDGE_HH_
#define KASKADE_BRIDGE_HH_

#include <boost/signals2.hpp>
#include <boost/bind.hpp>

#include "algorithm/abstract_interface.hh"
#include "algorithm/dune_bridge.hh"
#include "algorithm/newton_bridge.hh"

namespace Kaskade
{
  template <class Value>
  void assignIfNegative(Value& val, Value newVal)
  {
    if(val < 0) val = newVal;
  }

  namespace Bridge
  {
    template <class Linearization>
    class ConnectedLinearization : public Linearization, public AbstractFlushConnection
    {
    public:
      template <typename... Args>
      ConnectedLinearization(const Args&... args) : Linearization(args...)
      {}

      virtual ~ConnectedLinearization() { changed(); flushconn.disconnect(); }

      virtual void flush() { changed(); Linearization::flush(); }

      virtual void connectToSignalForFlush(boost::signals2::signal<void ()>& sig)
      {
        if(flushconn.connected()) flushconn.disconnect();
        flushconn=sig.connect(boost::bind(&ConnectedLinearization<Linearization>::flush, this));
      }

      boost::signals2::signal<void ()> changed;
      boost::signals2::connection flushconn;
    };

    /// Bridge::Linearization class that uses a VariationalFunctionalAssembler to create linear systems
    /** Implements AbstractLinearization */
    template<class Functional>
    class KaskadeLinearization : public AbstractLinearization, public SparseLinearSystem
    {
    public:

      static const int nThreads = 32;

      typedef typename Functional::AnsatzVars::VariableSet DomainElement;
      typedef typename Functional::TestVars::VariableSet ImageElement;
      typedef typename Functional::Scalar Scalar;
      typedef LinearizationAt<Functional> Implementation;
      typedef VariationalFunctionalAssembler<Implementation> Assembler;
      typedef typename DomainElement::Descriptions::template CoefficientVectorRepresentation<>::type CoefficientVector;
      typedef Dune::LinearOperator<CoefficientVector, CoefficientVector> OperatorType;
      typedef OperatorType Operator;

      /*      KaskadeLinearization()
      : x(0), fu(nullptr), lin(*fu,x), ass(nullptr), xptr(nullptr)
      {
        flush();
      }

      /// Creation of a linearization for a functional fu at x_
      KaskadeLinearization(Functional const& fu_)
      : x(0), fu(&fu_), lin(*fu,x), ass(nullptr), xptr(nullptr)
      {
        flush();
      }*/

      /// Creation of a linearization for a functional fu at x_
      KaskadeLinearization(Functional const& fu_, DomainElement const& x_)
        : x(x_), fu(&fu_), lin(*fu,x), ass(new Assembler(x.descriptions.spaces)), xptr(new Vector<DomainElement>(x))
      {
        flush();
      }

      /// Creation of a linearization for a functional fu at x_
      KaskadeLinearization(Functional const& fu_, DomainElement const& x_, std::shared_ptr<Assembler> const& ass_)
        : x(x_), fu(&fu_), lin(*fu,x), ass(ass_), xptr(new Vector<DomainElement>(x))
      {
        flush();
      }

      KaskadeLinearization(KaskadeLinearization const& other)
        : x(other.x), fu(other.fu), lin(*fu,x), ass(other.ass), xptr(new Vector<DomainElement>(x))
      {
      }

      virtual ~KaskadeLinearization() {}

      /// Number of columns of components [cbegin, cend)
      int cols(int cbegin=0, int cend=-1) const
      {
        assignIfNegative(cend,nColBlocks());
        assert(cbegin<cend);
        return x.descriptions.degreesOfFreedom(cbegin,cend);
      }

      /// Number of rows of components [rbegin, rend)
      int rows(int rbegin=0, int rend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assert(rbegin < rend);
        return x.descriptions.degreesOfFreedom(rbegin,rend);
      }

      void precompute() {
        doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);
      }

      /// write blocks of the hessian matrix into mat
      void getMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin, int rend, int cbegin, int cend) const
      {
        doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);
        mat = ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend,cbegin,cend);

        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          MatrixAsTriplet<Scalar> matD;
          getDiscreteMatrixBlocks(matD,rbegin,rend,cbegin,cend);
          mat+=matD;
        }
      }

      /// write components of the gradient into rhs
      void getRHSBlocks(std::vector<Scalar>& rhs, int rbegin, int rend) const
      {
        doAssemble(Assembler::VALUE | Assembler::RHS);
        rhs.resize(rows(rbegin,rend),0.0);
        ass->toSequence(rbegin,rend,rhs.begin());
        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          addDiscreteRHSBlocks(rhs,rbegin,rend);
        }
      }

      /// return number of columns
      int nColBlocks() const { return Functional::AnsatzVars::noOfVariables; }

      /// return number of rows
      int nRowBlocks() const { return Functional::TestVars::noOfVariables; }

      /// return point of linearization
      AbstractFunctionSpaceElement const& getOrigin() const
      {
        return *xptr;
      }

      void setOrigin(AbstractFunctionSpaceElement const& x_)
      {
        x = Bridge::getImpl<DomainElement>(x_);
        xptr.reset(new Vector<DomainElement>(x));

        if(ass == nullptr)
        {
          ass.reset(new Assembler(x.descriptions.spaces));
        }
      }

      /// return the implementation
      Implementation const& getLinImpl() const {return lin; }

      /// flush all data, gathered so far
      void flush() { ass->flush( Assembler::VALUE | Assembler::RHS | Assembler::MATRIX ); }

      /// return whether x is in the domain of definition
      bool inDomain(DomainElement const& x) { return InDomainTraits<Functional,DomainElement>::inDomain(x); }

      /// return the current value Functional(Origin)
      double eval() const
      {
        doAssemble(Assembler::VALUE);
        Scalar value =ass->functional();
        if(DiscreteBlockTraits<Functional>::anyPresent)
          value += DiscreteBlockTraits<Functional>::getValue(*fu,x);
        return value;
      }

      double getValue() const { return eval(); }

      void evald(AbstractFunctionSpaceElement& v, int rbegin, int rend) const
      {
        assignIfNegative(rend,nRowBlocks());
        assert(rbegin<=rend);
        std::vector<Scalar> rhs(rows(rbegin,rend),0.0);
        getRHSBlocks(rhs,rbegin,rend);
        dynamic_cast<Vector<ImageElement>& >(v).read(rhs,rbegin,rend);
      }

      void ddxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        d2axpy(1.0,y,x,rbegin,rend,cbegin,cend);
      }

      void d2axpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result,rbegin,rend);
      }

      void ddtxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        d2taxpy(1.0,y,x,rbegin,rend,cbegin,cend);
      }

      void d2taxpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());

        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,rbegin,rend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,cbegin,cend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.transpose().axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result,cbegin,cend);
      }

      double evalL1norm() const
      {
        doAssemble(Assembler::VALUE);
        Scalar value =0;//ass.fL1norm();
        if(DiscreteBlockTraits<Functional>::anyPresent)
          value += std::fabs(DiscreteBlockTraits<Functional>::getValue(*fu,x));
        return value;
      }

      Assembler const& getValidAssembler() const
      {
        doAssemble((Assembler::VALUE | Assembler::RHS | Assembler::MATRIX));
        return *ass;
      }

      Functional const& getFunctional() const
      {
        return *fu;
      }

    protected:
      void addDiscreteRHSBlocks(std::vector<Scalar>& rhs, int rbegin, int rend) const
      {
        for(int i=rbegin; i<rend; ++i)
        {
          std::vector<Scalar> rhsD;
          DiscreteBlockTraits<Functional>::getRHSBlock(rhsD,*fu,x,i);
          if(rhsD.size() > 0)
          {
            assert(rhsD.size() == rows(i,i+1));
            for(int k=rows(rbegin,i), l=0;k<rows(rbegin,i+1);++k, ++l)
              rhs[k]+=rhsD[l];
          }
        }
      }

      void getDiscreteMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin, int rend, int cbegin, int cend) const
      {
        for(int i=rbegin; i<rend; ++i)
          for(int j=cbegin; j<cend; ++j)
          {
            MatrixAsTriplet<Scalar> matD;
            DiscreteBlockTraits<Functional>::getMatrixBlock(matD, *fu,x,i,j);
            matD.shiftIndices(rows(rbegin,i),cols(cbegin,j));
            mat+=matD;
          }
      }

      void doAssemble(int flags) const
      {
        int toDoFlag= ((~ass->valid()) & flags);
        if(toDoFlag!=0) ass->assemble(lin,toDoFlag,nThreads);
      }

      DomainElement x;
      Functional const* fu;
      Implementation lin;
      mutable std::shared_ptr<Assembler> ass;
      std::unique_ptr<Vector<DomainElement> > xptr;
    };

    template <class Functional> using ConnectedKaskadeLinearization = ConnectedLinearization<KaskadeLinearization<Functional> >;

    template<class Functional, int stateId=1, int adjointId=2>
    class NormalStepLinearization : public ConnectedKaskadeLinearization<Functional>
    {
      typedef ConnectedKaskadeLinearization<Functional> Base;
      typedef typename Base::Scalar Scalar;
      typedef typename Base::Assembler Assembler;
      typedef typename Base::DomainElement DomainElement;
      typedef typename Base::ImageElement ImageElement;
      using Base::nRowBlocks;
      using Base::nColBlocks;
    public:
      NormalStepLinearization() : Base() {}

      /// Creation of a linearization for a functional fu at x_
      NormalStepLinearization(Functional const& fu, DomainElement const& x) : Base(fu,x) {}

      /// Creation of a linearization for a functional fu at x_
      NormalStepLinearization(Functional const& fu, DomainElement const& x, std::shared_ptr<Assembler> const& assembler) : Base(fu,x,assembler) {}

      NormalStepLinearization(Base const& other) : Base(other) {}

      /// write blocks of the hessian matrix into mat
      void getMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        this->doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);
        if(rbegin==stateId && rend==stateId+1 && cbegin==stateId && cend==stateId+1)
        {
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,adjointId,adjointId+1,adjointId,adjointId+1);
          return;
        }

        if(rbegin<=stateId && rend>stateId && cbegin<=stateId && cend > stateId)
        {
          size_t rows0 = 0, cols0 = 0, rows1 = 0, cols1 = 0;
          MatrixAsTriplet<Scalar> tmp;
          if(rbegin < stateId)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,stateId,cbegin,cend);
            rows0 = tmp.nrows();
            mat += tmp;
          }
          if(cbegin < stateId)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,stateId,stateId+1,cbegin,stateId);
            cols0 = tmp.ncols();
            tmp.shiftIndices(rows0,0);
            mat += tmp;
          }
          tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,adjointId,adjointId+1,adjointId,adjointId+1);
          rows1 = tmp.nrows(), cols1 = tmp.ncols();
          tmp.shiftIndices(rows0,cols0);
          mat += tmp;
          if(cend > stateId+1)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,stateId,stateId+1,stateId+1,cend);
            tmp.shiftIndices(rows0,cols0+cols1);
            mat += tmp;
          }
          if(rend > stateId+1)
          {
            tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,stateId+1,rend,cbegin,cend);
            tmp.shiftIndices(rows0+rows1,0);
            mat += tmp;
          }

        }
        else
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend,cbegin,cend);

        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          MatrixAsTriplet<Scalar> matD;
          this->getDiscreteMatrixBlocks(matD,rbegin,rend,cbegin,cend);
          mat+=matD;
        }
      }

      void ddxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        assert(cbegin<=cend);
        assert(rbegin<=rend);
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const& >(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement>& >(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument);
        dynamic_cast<Vector<ImageElement>& >(y).read(result,rbegin,rend);
      }

      void d2axpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if( rbegin == adjointId && rend == adjointId+1 && cbegin == adjointId && cend == adjointId+1 ) return;
        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result, rbegin, rend);
      }
    };

    template<class Functional, int stateId=1, int adjointId=2>
    class TangentialStepLinearization : public ConnectedKaskadeLinearization<Functional>
    {
      typedef ConnectedKaskadeLinearization<Functional> Base;
      typedef typename Base::Scalar Scalar;
      typedef typename Base::Assembler Assembler;
      typedef typename Base::DomainElement DomainElement;
      typedef typename Base::ImageElement ImageElement;
      using Base::nRowBlocks;
      using Base::nColBlocks;
    public:
      TangentialStepLinearization() : Base() {}

      /// Creation of a linearization for a functional fu at x_
      TangentialStepLinearization(Functional const& fu, DomainElement const& x) : Base(fu,x) {}

      /// Creation of a linearization for a functional fu at x_
      TangentialStepLinearization(Functional const& fu, DomainElement const& x, std::shared_ptr<Assembler> const& assembler) : Base(fu,x,assembler) {}

      TangentialStepLinearization(Base const& other) : Base(other) {}

      /// write blocks of the hessian matrix into mat
      void getMatrixBlocks(MatrixAsTriplet<Scalar>& mat, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        this->doAssemble(Assembler::VALUE | Assembler::RHS | Assembler::MATRIX);

        if(rend > adjointId && cend > adjointId)
        {
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend-1,cbegin,cend-1);
          size_t rows = mat.nrows(), cols = mat.ncols();
          auto tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rend-1,rend,cbegin,cend-1);
          tmp.shiftIndices(rows,0);
          mat += tmp;
          tmp = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend-1,cend-1,cend);
          tmp.shiftIndices(0,cols);
          mat += tmp;
        }
        else
          mat = this->ass->template get<MatrixAsTriplet<Scalar> >(false,rbegin,rend,cbegin,cend);

        if(DiscreteBlockTraits<Functional>::anyPresent)
        {
          MatrixAsTriplet<Scalar> matD;
          this->getDiscreteMatrixBlocks(matD,rbegin,rend,cbegin,cend);
          mat+=matD;
        }
      }

      void ddxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if(rbegin==adjointId && rend==adjointId+1 && cbegin==adjointId && cend==adjointId+1) return;
        assert(cbegin<=cend);
        assert(rbegin<=rend);
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const& >(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement>& >(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument);
        dynamic_cast<Vector<ImageElement>& >(y).read(result,rbegin,rend);
      }

      void d2axpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
      {
        assignIfNegative(rend,nRowBlocks());
        assignIfNegative(cend,nColBlocks());
        if( rbegin == adjointId && rend == adjointId+1 && cbegin == adjointId && cend == adjointId+1 ) return;
        assert( cbegin <= cend );
        assert( rbegin <= rend );
        std::vector<Scalar> result, argument;
        dynamic_cast<Vector<ImageElement> const&>(x).write(argument,cbegin,cend);
        dynamic_cast<Vector<ImageElement> const&>(y).write(result,rbegin,rend);
        MatrixAsTriplet<Scalar> mat;
        getMatrixBlocks(mat,rbegin,rend,cbegin,cend);
        mat.axpy(result, argument, a);
        dynamic_cast<Vector<ImageElement>&>(y).read(result,rbegin,rend);
      }
    };

    //template <class Functional, int stateId=1, int adjointId=2> using NormalStepLinearization = ConnectedLinearization<NormalStepKaskadeLinearization<Functional,stateId,adjointId> >;
    //template <class Functional, int stateId=1, int adjointId=2> using TangentialStepLinearization = ConnectedLinearization<TangentialStepKaskadeLinearization<Functional,stateId,adjointId> >;

  }
}

#endif /* BRIDGE_HH_ */


/**
 * Dumps out a csv file with information on ring hypergraphs
 */

#include <ostream>
#include <istream>
#include <iostream>
#include <chrono>
#include <sstream>
#include <vector>

#include <boost/hana.hpp>
#include <hypergraph/approx.hpp>
#include <generators/generators.hpp>
#include <hypergraph/certificate.hpp>

namespace hana = boost::hana;

struct InputInfo {
  BOOST_HANA_DEFINE_STRUCT(
      InputInfo,
      (size_t, num_vertices),
      (size_t, num_edges),
      (double, radius),
      (uint64_t, seed),
      (double, epsilon),

      (size_t, min_cut_value),
      (size_t, eps_cut_value),
      (size_t, size_before),
      (size_t, size_after),
      (size_t, p1),
      (size_t, p2)
  );
  /*
  size_t num_vertices;
  size_t num_edges;
  double radius;
  double epsilon;
  // Suboptimality factor of cut found by running the approximate algorithm
  double suboptimality_factor;
  size_t size_before;
  // Size after sparsifying with the cut value found by the approximate algorithm
  size_t size_after;
  // Size of partition 1
  size_t p1;
  // Size of partition 2
  size_t p2;
   */

  static std::string header() {
    std::stringstream s;
    hana::for_each(InputInfo{}, hana::fuse([&s](const auto &name, const auto &member) {
      s << name.c_str() << ",";
    }));
    std::string str = s.str();
    return str.substr(0, str.size() - 1);
  }
};

// Comma delimit
template<typename T>
std::string comma_delimit(const T &data) {
  std::stringstream s;
  hana::for_each(data, hana::fuse([&s](const auto &name, const auto &member) {
    s << member << ",";
  }));
  std::string str = s.str();
  return str.substr(0, str.size() - 1);
}

std::ostream &operator<<(std::ostream &out, const InputInfo &info) {
  out << comma_delimit(info) << std::endl;
  return out;
}

/*
std::istream &operator>>(std::istream &in, InputInfo &info) {
  std::string line;
  std::getline(in, line);
  std::stringstream line_stream(line);
  auto map = hana::to<hana::map_tag>(info);
  hana::for_each(info, hana::fuse([&line_stream, &map](auto name, auto member) {
    std::string field;
    std::getline(line_stream, field, ',');
    std::stringstream field_stream(field);
    decltype(member) m;
    field_stream >> m;
    map[name] = m;
  }));

  info.num_vertices = map[BOOST_HANA_STRING("num_vertices")];
  info.num_edges = map[BOOST_HANA_STRING("num_edges")];

  return in;
}
 */

struct Timer {
  using TimePoint = decltype(std::chrono::high_resolution_clock::now());

  Timer() {
    start = std::chrono::high_resolution_clock::now();
  }

  auto stop() {
    return std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::high_resolution_clock::now() - start).count();
  }
private:
  TimePoint start;
};

// Compare just the input parts of InputInfo
struct CompareInput {
  static auto to_tuple(const InputInfo &a) {
    return std::make_tuple(a.num_vertices, a.num_edges, a.radius);
  }

  bool operator()(const InputInfo &a, const InputInfo &b) const {
    return to_tuple(a) < to_tuple(b);
  }
};

hypergraphlib::HypergraphCut<size_t> memoized_cut(const InputInfo &info, const hypergraphlib::Hypergraph &hypergraph) {
  static std::map<InputInfo, hypergraphlib::HypergraphCut<size_t>, CompareInput> cuts;

  auto it = cuts.find(info);
  if (it == cuts.end()) {
    hypergraphlib::Hypergraph h(hypergraph);
    const auto cut = hypergraphlib::MW_min_cut(h);
    cuts.insert({info, cut});
    return cut;
  }
  return it->second;
}

int main() {
  std::vector<InputInfo> infos;

  for (size_t num_vertices = 100; num_vertices <= 500; num_vertices += 25) {
    for (size_t num_edges = 100; num_edges <= num_vertices * 30; num_edges += 250) {
      for (double radius = 5; radius <= 90; radius += 5) {
        for (double epsilon: {0.1, 0.2, 0.4, 0.8, 1., 2., 4., 8., 16., 32., 64., 128.}) {
          for (int i = 0; i < 10; ++i) {
            std::mt19937_64 rd;
            std::uniform_int_distribution<uint64_t> dis;
            uint64_t seed;
            InputInfo info = {
                .num_vertices = num_vertices,
                .num_edges = num_edges,
                .radius = radius,
                .seed = dis(rd),
                .epsilon = epsilon
            };
            infos.emplace_back(std::move(info));
          }
        }
      }
    }
  }

  std::cout << InputInfo::header() << std::endl;

  for (auto &info : infos) {
    RandomRingConstantEdgeHypergraph gen(info.num_vertices, info.num_edges, info.radius, 777);
    auto[h, _] = gen.generate();
    // TODO do this before in generate or something
    h.remove_singleton_and_empty_hyperedges();
    const hypergraphlib::Hypergraph hypergraph(h);

    const auto cut = memoized_cut(info, hypergraph);
    hypergraphlib::Hypergraph temp2(hypergraph);
    const auto eps_cut = hypergraphlib::approximate_minimizer(temp2, info.epsilon);
    hypergraphlib::KTrimmedCertificate k(hypergraph);
    auto hypergraph_after = k.certificate(eps_cut.value);

    info.eps_cut_value = eps_cut.value;
    info.min_cut_value = cut.value;
    info.size_before = hypergraph.size();
    info.size_after = hypergraph_after.size();
    info.p1 = cut.partitions.at(0).size();
    info.p2 = cut.partitions.at(1).size();

    std::cout << info;
  }
}