/**
   \file aga.hpp
   \brief asexual genetic algorithm method
   \author Junhua Gu
*/

#ifndef AGA_METHOD
#define AGA_METHOD
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <limits>
#include <cstdlib>
#include <core/opt_traits.hpp>
#include <cassert>
#include <cmath>
#include <ctime>
#include <vector>
#include <algorithm>
/*
 *
*/
#include <iostream>
using std::cout;
using std::endl;

namespace opt_utilities
{

  template <typename rT,typename pT>
  struct vp_pair
  {
    rT v;
    pT p;
  };

  template <typename rT,typename pT>
  class vp_comp
  {
  public:
    bool operator()(const vp_pair<rT,pT>& x1,
		    const vp_pair<rT,pT>& x2)
    {
      return x1.v<x2.v;
    }
  };

  /**
     \brief Implement of the asexual genetic algorithm
     2009A&A...501.1259C
     http://adsabs.harvard.edu/abs/2009arXiv0905.3712C
     \tparam rT return type of the object function
     \tparam pT parameter type of the object function
   */
  template <typename rT,typename pT>
  class aga_method
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
  private:
    int n1,n2,n0;
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;
    rT threshold;
    pT lower_bound;
    pT upper_bound;

    typename element_type_trait<pT>::element_type decay_factor;
    pT reproduction_box;
    std::vector<vp_pair<rT,pT> > samples;
    std::vector<pT> buffer;
    mutable bool bstop;
  private:
    typename element_type_trait<pT>::element_type uni_rand
    (typename element_type_trait<pT>::element_type x1,
     typename element_type_trait<pT>::element_type x2)
    {
      return rand()/(double)RAND_MAX*(x2-x1)+x1;
    }

  private:
    const char* do_get_type_name()const
    {
      return "asexual genetic algorithm";
    }

    rT func(const pT& x)
    {
      assert(p_fo!=0);
      return p_fo->eval(x);
    }

  public:
    aga_method(int _n1,int _n2)
      :n1(_n1),n2(_n2),n0(n1*n2+n1),
       p_fo(0),p_optimizer(0),threshold(1e-4),
       decay_factor(.999),
       samples(n1*n2+n1)
    {
    }

    aga_method()
      :n1(50),n2(20),n0(n1*n2+n1),
       p_fo(0),p_optimizer(0),threshold(1e-4),
       decay_factor(.999),
       samples(n1*n2+n1)
    {
    }

    virtual ~aga_method()
    {
    };

    aga_method(const aga_method<rT,pT>& rhs)
      :n1(rhs.n1),n2(rhs.n2),n0(rhs.n0),
       p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       threshold(rhs.threshold),
       decay_factor(rhs.decay_factor),
       samples(rhs.samples)
    {
    }

    aga_method<rT,pT>& operator=(const aga_method<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      samples=rhs.samples;
      n1=rhs.n1;
      n2=rhs.n2;
      n0=rhs.n0;
    }

    void set_decay_factor(typename element_type_trait<pT>::element_type _decay_factor)
    {
      decay_factor=_decay_factor;
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new aga_method<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      for(size_t i=0;i<samples.size();++i)
	{
	  //  cout<<i<<" ";
	  resize(samples[i].p,get_size(p));
	  //	  std::cout<<samples[i].p.size()<<std::endl;;
	  for(size_t j=0;j<get_size(p);++j)
	    {
	      set_element(samples[i].p,j,
			  uni_rand(get_element(lower_bound,j),
				   get_element(upper_bound,j))
			  );
	    }
	}

    }

    array1d_type do_get_start_point()const
    {
      return array1d_type();
    }

    void do_set_lower_limit(const array1d_type& p)
    {
      opt_eq(lower_bound,p);
    }

    array1d_type do_get_lower_limit()const
    {
      return lower_bound;
    }

    void do_set_upper_limit(const array1d_type& p)
    {
      opt_eq(upper_bound,p);
    }

    array1d_type do_get_upper_limit()const
    {
      return upper_bound;
    }

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    bool iter()
    {
      rT sum2=0;
      rT sum=0;
      for(size_t i=0;i<samples.size();++i)
	{
	  samples[i].v=func(samples[i].p);
	  sum2+=samples[i].v*samples[i].v;
	  sum+=samples[i].v;
	}

      std::sort(samples.begin(),samples.end(),vp_comp<rT,pT>());
      if(sum2/samples.size()-pow(sum/samples.size(),2)<threshold)
	{
	  return false;
	}
      pT lb(get_size(samples[0].p));
      pT ub(get_size(samples[0].p));
      for(int i=0;i<n2&&!bstop;++i)
	{
	  pT p(samples[i].p);
	  for(size_t j=0;j<get_size(p);++j)
	    {
	      if(i==0)
		{
		  ub[j]=p[j];
		  lb[j]=p[j];
		}
	      ub[j]=std::max(ub[j],p[j]);
	      lb[j]=std::min(lb[j],p[j]);

	      set_element(p,j,
			  get_element(p,j)+
			  uni_rand(-get_element(reproduction_box,j),
				   get_element(reproduction_box,j)));
	      if(get_element(p,j)>get_element(upper_bound,j))
		{
		  set_element(p,j,get_element(upper_bound,j));
		}
	      if(get_element(p,j)<get_element(lower_bound,j))
		{
		  set_element(p,j,get_element(lower_bound,j));
		}
	    }
	  buffer[i]=p;
	}
      if(bstop)
	{
	  return false;
	}
      for(int i=0;i<n1&&!bstop;++i)
	{
	  for(int j=0;j<n2&&!bstop;++j)
	    {
	      pT p(samples[i].p);
	      for(size_t k=0;k<get_size(p);++k)
		{
		  set_element(samples[i*n2+j+n1].p,k,
			      (get_element(samples[i].p,k)+
			       get_element(buffer[j],k))/2.);

		  ub[k]=std::max(ub[k],samples[i*n2+j+n1].p[k]);
		  lb[k]=std::min(lb[k],samples[i*n2+j+n1].p[k]);
		}
	    }
	}
      if(bstop)
	{
	  return false;
	}
      double n_per_dim=pow((double)n0,1./get_size(lower_bound));
      for(size_t i=0;i<get_size(reproduction_box);++i)
	{
	  //	  set_element(reproduction_box,i,
	  //get_element(reproduction_box,i)*decay_factor);
	  set_element(reproduction_box,i,
		      (get_element(ub,i)-
		       get_element(ub,i))/n_per_dim);

	}
      return true;
    }

    pT do_optimize()
    {
      bstop=false;
      srand(time(0));
      buffer.resize(n2);
      double n_per_dim=pow((double)n0,1./get_size(lower_bound));
      resize(reproduction_box,get_size(lower_bound));

      for(size_t i=0;i<get_size(lower_bound);++i)
	{

	  set_element(reproduction_box,i,
		      (get_element(upper_bound,i)-
		       get_element(lower_bound,i))/n_per_dim);
	}

      while(iter()&&!bstop){}

      return samples.begin()->p;
    }

    void do_stop()
    {
      bstop=true;
    }

  };

}

#endif
//EOF


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

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// 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_AREA_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HPP

#include <boost/geometry/formulas/area_formulas.hpp>
#include <boost/geometry/srs/sphere.hpp>
#include <boost/geometry/strategies/area.hpp>
#include <boost/geometry/strategies/spherical/get_radius.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Spherical area calculation
\ingroup strategies
\details Calculates area on the surface of a sphere using the trapezoidal rule
\tparam RadiusTypeOrSphere \tparam_radius_or_sphere
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
}
*/
template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
{
    // Enables special handling of long segments
    static const bool LongSegment = false;

public:
    template <typename Geometry>
    struct result_type
        : strategy::area::detail::result_type
            <
                Geometry,
                CalculationType
            >
    {};

    template <typename Geometry>
    class state
    {
        friend class spherical;

        typedef typename result_type<Geometry>::type return_type;

    public:
        inline state()
            : m_sum(0)
            , m_crosses_prime_meridian(0)
        {}

    private:
        template <typename RadiusType>
        inline return_type area(RadiusType const& r) const
        {
            return_type result;
            return_type radius = r;

            // Encircles pole
            if(m_crosses_prime_meridian % 2 == 1)
            {
                size_t times_crosses_prime_meridian
                        = 1 + (m_crosses_prime_meridian / 2);

                result = return_type(2)
                         * geometry::math::pi<return_type>()
                         * times_crosses_prime_meridian
                         - geometry::math::abs(m_sum);

                if(geometry::math::sign<return_type>(m_sum) == 1)
                {
                    result = - result;
                }

            } else {
                result =  m_sum;
            }

            result *= radius * radius;

            return result;
        }

        return_type m_sum;

        // Keep track if encircles some pole
        size_t m_crosses_prime_meridian;
    };

public :

    // For backward compatibility reasons the radius is set to 1
    inline spherical()
        : m_radius(1.0)
    {}

    template <typename RadiusOrSphere>
    explicit inline spherical(RadiusOrSphere const& radius_or_sphere)
        : m_radius(strategy_detail::get_radius
                    <
                        RadiusOrSphere
                    >::apply(radius_or_sphere))
    {}

    template <typename PointOfSegment, typename Geometry>
    inline void apply(PointOfSegment const& p1,
                      PointOfSegment const& p2,
                      state<Geometry>& st) const
    {
        if (! geometry::math::equals(get<0>(p1), get<0>(p2)))
        {
            typedef geometry::formula::area_formulas
                <
                    typename result_type<Geometry>::type
                > area_formulas;

            st.m_sum += area_formulas::template spherical<LongSegment>(p1, p2);

            // Keep track whenever a segment crosses the prime meridian
            if (area_formulas::crosses_prime_meridian(p1, p2))
            {
                st.m_crosses_prime_meridian++;
            }
        }
    }

    template <typename Geometry>
    inline typename result_type<Geometry>::type
        result(state<Geometry> const& st) const
    {
        return st.area(m_radius);
    }

private :
    typename strategy_detail::get_radius
        <
            RadiusTypeOrSphere
        >::type m_radius;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <>
struct default_strategy<spherical_equatorial_tag>
{
    typedef strategy::area::spherical<> type;
};

// Note: spherical polar coordinate system requires "get_as_radian_equatorial"
template <>
struct default_strategy<spherical_polar_tag>
{
    typedef strategy::area::spherical<> type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HPP


/// @brief Eigen wrappers for the exact CCD methods.

#pragma once

#include <Eigen/Core>

namespace IPC {
namespace ExactCCD {

/// @brief Methods of geometrically exact continous collision detection.
enum Method {
    NONE, ///< @brief Do not use exact CCD.
    ROOT_PARITY, ///< @brief Root parity method of Brochu et al. [2012].
    BSC, ///< @brief Bernstein sign classification method of Tang et al. [2014].
    RATIONAL_ROOT_PARITY ///< @brief Teseo's reimplementation of Brochu et al. [2012] using rationals
};

/**
 * @brief Detect collisions between two edges as they move.
 *
 * Looks for collisions between edges (q0start, p0start) and (q1start, p1start)
 * as they move towards (q0end, p0end) and (q1end, p1end). Returns true if the
 * edges collide.
 *
 * @param[in]  q0start  Start position of the first edge's first vertex.
 * @param[in]  p0start  Start position of the first edge's second vertex.
 * @param[in]  q1start  Start position of the second edge's first vertex.
 * @param[in]  p1start  Start position of the second edge's second vertex.
 * @param[in]  q0end    End position of the first edge's first vertex.
 * @param[in]  p0end    End position of the first edge's second vertex.
 * @param[in]  q1end    End position of the second edge's first vertex.
 * @param[in]  p1end    End position of the second edge's second vertex.
 * @param[in]  method   Method of exact CCD.
 *
 * @returns True if the edges collide.
 */
bool edgeEdgeCCD(
    const Eigen::Vector3d& q0start,
    const Eigen::Vector3d& p0start,
    const Eigen::Vector3d& q1start,
    const Eigen::Vector3d& p1start,
    const Eigen::Vector3d& q0end,
    const Eigen::Vector3d& p0end,
    const Eigen::Vector3d& q1end,
    const Eigen::Vector3d& p1end,
    const Method method);

/**
 * @brief Detect collisions between a vertex and a triangular face.
 *
 * Looks for collisions between the vertex q0start and the face
 * (q1start, q2start, q3start) as they move towards q0end and
 * (q1end, q2end, q3end). Returns true if the vertex and face collide.
 *
 * @param[in]  q0start  Start position of the vertex.
 * @param[in]  q1start  Start position of the first vertex of the face.
 * @param[in]  q2start  Start position of the second vertex of the face.
 * @param[in]  q3start  Start position of the third vertex of the face.
 * @param[in]  q0end    End position of the vertex.
 * @param[in]  q1end    End position of the first vertex of the face.
 * @param[in]  q2end    End position of the second vertex of the face.
 * @param[in]  q3end    End position of the third vertex of the face.
 * @param[in]  method   Method of exact CCD.
 *
 * @returns  True if the vertex and face collide.
 */
bool vertexFaceCCD(
    const Eigen::Vector3d& q0start,
    const Eigen::Vector3d& q1start,
    const Eigen::Vector3d& q2start,
    const Eigen::Vector3d& q3start,
    const Eigen::Vector3d& q0end,
    const Eigen::Vector3d& q1end,
    const Eigen::Vector3d& q2end,
    const Eigen::Vector3d& q3end,
    const Method method);

}
} // namespace IPC::ExactCCD


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL 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 "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/projection.hpp>
#include <opencv2/sfm/triangulation.hpp>
#include <opencv2/sfm/fundamental.hpp>
#include <opencv2/sfm/numeric.hpp>
#include <opencv2/sfm/conditioning.hpp>

// libmv headers
#include "libmv/multiview/fundamental.h"

#include <iostream>
using namespace std;

namespace cv
{
namespace sfm
{
  template<typename T>
  void
  projectionsFromFundamental( const Mat_<T> &F,
                              Mat_<T> P1,
                              Mat_<T> P2 )
  {
    P1 << 1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1, 0;

    Vec<T,3> e2;
    cv::SVD::solveZ(F.t(), e2);

    Mat_<T> P2cols = skew(e2) * F;
    for(char j=0;j<3;++j) {
      for(char i=0;i<3;++i)
        P2(j,i) = P2cols(j,i);
      P2(j,3) = e2(j);
    }

  }

  void
  projectionsFromFundamental( InputArray _F,
                              OutputArray _P1,
                              OutputArray _P2 )
  {
    const Mat F = _F.getMat();
    const int depth = F.depth();
    CV_Assert(F.cols == 3 && F.rows == 3 && (depth == CV_32F || depth == CV_64F));

    _P1.create(3, 4, depth);
    _P2.create(3, 4, depth);

    Mat P1 = _P1.getMat(),  P2 = _P2.getMat();

    // type
    if( depth == CV_32F )
    {
      projectionsFromFundamental<float>(F, P1, P2);
    }
    else
    {
      projectionsFromFundamental<double>(F, P1, P2);
    }

  }

  template<typename T>
  void
  fundamentalFromProjections( const Mat_<T> &P1,
                              const Mat_<T> &P2,
                              Mat_<T> F )
  {
    Mat_<T> X[3];
    vconcat( P1.row(1), P1.row(2), X[0] );
    vconcat( P1.row(2), P1.row(0), X[1] );
    vconcat( P1.row(0), P1.row(1), X[2] );

    Mat_<T> Y[3];
    vconcat( P2.row(1), P2.row(2), Y[0] );
    vconcat( P2.row(2), P2.row(0), Y[1] );
    vconcat( P2.row(0), P2.row(1), Y[2] );

    Mat_<T> XY;
    for (int i = 0; i < 3; ++i)
      for (int j = 0; j < 3; ++j)
      {
        vconcat(X[j], Y[i], XY);
        F(i, j) = determinant(XY);
      }
  }

  void
  fundamentalFromProjections( InputArray _P1,
                              InputArray _P2,
                              OutputArray _F )
  {
    const Mat P1 = _P1.getMat(), P2 = _P2.getMat();
    const int depth = P1.depth();
    CV_Assert((P1.cols == 4 && P1.rows == 3) && P1.rows == P2.rows && P1.cols == P2.cols);
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == P2.depth());

    _F.create(3, 3, depth);

    Mat F = _F.getMat();

    // type
    if( depth == CV_32F )
    {
      fundamentalFromProjections<float>(P1, P2, F);
    }
    else
    {
      fundamentalFromProjections<double>(P1, P2, F);
    }

  }

  template<typename T>
  void
  normalizedEightPointSolver( const Mat_<T> &_x1,
                              const Mat_<T> &_x2,
                              Mat_<T> _F )
  {
    libmv::Mat x1, x2;
    libmv::Mat3 F;

    cv2eigen(_x1, x1);
    cv2eigen(_x2, x2);

    libmv::NormalizedEightPointSolver(x1, x2, &F);

    eigen2cv(F, _F);
  }

  void
  normalizedEightPointSolver( InputArray _x1, InputArray _x2, OutputArray _F )
  {
    const Mat x1 = _x1.getMat(), x2 = _x2.getMat();
    const int depth = x1.depth();
    CV_Assert(x1.dims == 2 && x1.dims == x2.dims && (depth == CV_32F || depth == CV_64F));

    _F.create(3, 3, depth);

    Mat F = _F.getMat();

    // type
    if( depth == CV_32F )
    {
      normalizedEightPointSolver<float>(x1, x2, F);
    }
    else
    {
      normalizedEightPointSolver<double>(x1, x2, F);
    }

  }

  template<typename T>
  void
  relativeCameraMotion( const Mat_<T> &R1,
                        const Mat_<T> &t1,
                        const Mat_<T> &R2,
                        const Mat_<T> &t2,
                        Mat_<T> R,
                        Mat_<T> t )
  {
    R = R2 * R1.t();
    t = t2 - R * t1;
  }

  void
  relativeCameraMotion( InputArray _R1, InputArray _t1, InputArray _R2,
                        InputArray _t2, OutputArray _R, OutputArray _t )
  {
    const Mat R1 = _R1.getMat(), t1 = _t1.getMat(), R2 = _R2.getMat(), t2 = _t2.getMat();
    const int depth = R1.depth();
    CV_Assert((R1.cols == 3 && R1.rows == 3) && (R1.size() == R2.size()));
    CV_Assert((t1.cols == 1 && t1.rows == 3) && (t1.size() == t2.size()));
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R2.depth() && depth == t1.depth() && depth == t2.depth());

    _R.create(3, 3, depth);
    _t.create(3, 1, depth);

    Mat R = _R.getMat(), t = _t.getMat();

    // type
    if( depth == CV_32F )
    {
      relativeCameraMotion<float>(R1, t1, R2, t2, R, t);
    }
    else
    {
      relativeCameraMotion<double>(R1, t1, R2, t2, R, t);
    }

  }

  template<typename T>
  void
  motionFromEssential( const Mat_<T> &_E,
                       std::vector<Mat> &_Rs,
                       std::vector<Mat> &_ts )
  {
    libmv::Mat3 E;
    std::vector < libmv::Mat3 > Rs;
    std::vector < libmv::Vec3 > ts;

    cv2eigen(_E, E);

    libmv::MotionFromEssential(E, &Rs, &ts);

    _Rs.clear();
    _ts.clear();

    int n = Rs.size();
    CV_Assert(ts.size() == n);

    for ( int i = 0; i < n; ++i )
    {
      Mat_<T> R_temp, t_temp;

      eigen2cv(Rs[i], R_temp);
      _Rs.push_back(R_temp);

      eigen2cv(ts[i], t_temp);
      _ts.push_back(t_temp);
    }

  }

  void
  motionFromEssential( InputArray _E, OutputArrayOfArrays _Rs,
                       OutputArrayOfArrays _ts )
  {
    const Mat E = _E.getMat();
    const int depth = E.depth(), cn = 4;
    CV_Assert(E.cols == 3 && E.rows == 3 && (depth == CV_32F || depth == CV_64F));

    _Rs.create(cn, 1, depth);
    _ts.create(cn, 1, depth);
    for (int i = 0; i < cn; ++i)
    {
      _Rs.create(Size(3,3), depth, i);
      _ts.create(Size(3,1), depth, i);
    }

    std::vector<Mat> Rs, ts;
    _Rs.getMatVector(Rs);
    _ts.getMatVector(ts);

    // type
    if( depth == CV_32F )
    {
      motionFromEssential<float>(E, Rs, ts);
    }
    else
    {
      motionFromEssential<double>(E, Rs, ts);
    }

    for (int i = 0; i < cn; ++i)
    {
      Rs[i].copyTo(_Rs.getMatRef(i));
      ts[i].copyTo(_ts.getMatRef(i));
    }

  }

  template<typename T>
  int motionFromEssentialChooseSolution( const std::vector<Mat> &Rs,
                                         const std::vector<Mat> &ts,
                                         const Mat_<T> &K1,
                                         const Mat_<T> &x1,
                                         const Mat_<T> &K2,
                                         const Mat_<T> &x2 )
  {
    Mat_<T> P1, P2, R1 = Mat_<T>::eye(3,3);

    T val = static_cast<T>(0.0);
    Vec<T,3> t1(val, val, val);

    projectionFromKRt(K1, R1, t1, P1);

    std::vector<Mat_<T> > points2d;
    points2d.push_back(x1);
    points2d.push_back(x2);

    for ( int i = 0; i < 4; ++i )
    {
      const Mat_<T> R2 = Rs[i];
      const Vec<T,3> t2 = ts[i];
      projectionFromKRt(K2, R2, t2, P2);

      std::vector<Mat_<T> > Ps;
      Ps.push_back(P1);
      Ps.push_back(P2);

      Vec<T,3> X;
      triangulatePoints(points2d, Ps, X);

      T d1 = depth(R1, t1, X);
      T d2 = depth(R2, t2, X);

      // Test if point is front to the two cameras.
      if ( d1 > 0 && d2 > 0 )
      {
        return i;
      }
    }

    return -1;
  }

  int motionFromEssentialChooseSolution( InputArrayOfArrays _Rs,
                                         InputArrayOfArrays _ts,
                                         InputArray _K1,
                                         InputArray _x1,
                                         InputArray _K2,
                                         InputArray _x2 )
  {
    std::vector<Mat> Rs, ts;
    _Rs.getMatVector(Rs);
    _ts.getMatVector(ts);
    const Mat K1 = _K1.getMat(), x1 = _x1.getMat(), K2 = _K2.getMat(), x2 = _x2.getMat();
    const int depth = K1.depth();
    CV_Assert( Rs.size() == 4 && ts.size() == 4 );
    CV_Assert((K1.cols == 3 && K1.rows == 3) && (K1.size() == K2.size()));
    CV_Assert((x1.cols == 1 && x1.rows == 2) && (x1.size() == x2.size()));
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == K2.depth() && depth == x1.depth() && depth == x2.depth());

    int solution = 0;

    // type
    if( depth == CV_32F )
    {
      solution = motionFromEssentialChooseSolution<float>(Rs, ts, K1, x1, K2, x2);
    }
    else
    {
      solution = motionFromEssentialChooseSolution<double>(Rs, ts, K1, x1, K2, x2);
    }

    return solution;
  }

  template<typename T>
  void
  fundamentalFromEssential( const Mat_<T> &E,
                            const Mat_<T> &K1,
                            const Mat_<T> &K2,
                            Mat_<T> F )
  {
    F = K2.inv().t() * E * K1.inv();
  }

  void
  fundamentalFromEssential( InputArray _E,
                            InputArray _K1,
                            InputArray _K2,
                            OutputArray _F )
  {
    const Mat E = _E.getMat(), K1 = _K1.getMat(), K2 = _K2.getMat();
    const int depth =  E.depth();
    CV_Assert(E.cols == 3 && E.rows == 3 && E.size() == _K1.size() && E.size() == _K2.size() && (depth == CV_32F || depth == CV_64F));

    _F.create(3, 3, depth);

    Mat F = _F.getMat();

    // type
    if( depth == CV_32F )
    {
      fundamentalFromEssential<float>(E, K1, K2, F);
    }
    else
    {
      fundamentalFromEssential<double>(E, K1, K2, F);
    }

  }

  template<typename T>
  void
  essentialFromFundamental( const Mat_<T> &F,
                            const Mat_<T> &K1,
                            const Mat_<T> &K2,
                            Mat_<T> E )
  {
    E = K2.t() * F * K1;
  }

  void
  essentialFromFundamental( InputArray _F,
                            InputArray _K1,
                            InputArray _K2,
                            OutputArray _E )
  {
    const Mat F = _F.getMat(), K1 = _K1.getMat(), K2 = _K2.getMat();
    const int depth =  F.depth();
    CV_Assert(F.cols == 3 && F.rows == 3 && F.size() == _K1.size() && F.size() == _K2.size() && (depth == CV_32F || depth == CV_64F));

    _E.create(3, 3, depth);

    Mat E = _E.getMat();

    // type
    if( depth == CV_32F )
    {
      essentialFromFundamental<float>(F, K1, K2, E);
    }
    else
    {
      essentialFromFundamental<double>(F, K1, K2, E);
    }
  }

  template<typename T>
  void
  essentialFromRt( const Mat_<T> &_R1,
                   const Mat_<T> &_t1,
                   const Mat_<T> &_R2,
                   const Mat_<T> &_t2,
                   Mat_<T> _E )
  {
    libmv::Mat3 E;
    libmv::Mat3 R1, R2;
    libmv::Vec3 t1, t2;

    cv2eigen( _R1, R1 );
    cv2eigen( _t1, t1 );
    cv2eigen( _R2, R2 );
    cv2eigen( _t2, t2 );

    libmv::EssentialFromRt( R1, t1, R2, t2, &E );

    eigen2cv( E, _E );
  }

  void
  essentialFromRt( InputArray _R1,
                   InputArray _t1,
                   InputArray _R2,
                   InputArray _t2,
                   OutputArray _E )
  {
    const Mat R1 = _R1.getMat(), t1 = _t1.getMat(), R2 = _R2.getMat(), t2 = _t2.getMat();
    const int depth = R1.depth();
    CV_Assert((R1.cols == 3 && R1.rows == 3) && (R1.size() == R2.size()));
    CV_Assert((t1.cols == 1 && t1.rows == 3) && (t1.size() == t2.size()));
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R2.depth() && depth == t1.depth() && depth == t2.depth());

    _E.create(3, 3, depth);

    Mat E = _E.getMat();

    // type
    if( depth == CV_32F )
    {
      essentialFromRt<float>(R1, t1, R2, t2, E);
    }
    else
    {
      essentialFromRt<double>(R1, t1, R2, t2, E);
    }

  }

  template<typename T>
  void
  normalizeFundamental( const Mat_<T> &F, Mat_<T> F_normalized )
  {
    F_normalized = F * (1.0/norm(F,NORM_L2));  // Frobenius Norm

    if ( F_normalized(2,2) < 0 )
    {
        F_normalized *= -1;
    }
  }

  void
  normalizeFundamental( InputArray _F,
                        OutputArray _F_normalized )
  {
    const Mat F = _F.getMat();
    const int depth =  F.depth();
    CV_Assert(F.cols == 3 && F.rows == 3 && (depth == CV_32F || depth == CV_64F));

    _F_normalized.create(3, 3, depth);

    Mat F_normalized = _F_normalized.getMat();

    // type
    if( depth == CV_32F )
    {
      normalizeFundamental<float>(F, F_normalized);
    }
    else
    {
      normalizeFundamental<double>(F, F_normalized);
    }
  }

  template<typename T>
  void
  computeOrientation( const Mat_<T> &x1,
                      const Mat_<T> &x2,
                      Mat_<T> R,
                      Mat_<T> t,
                      T s )
  {
    Mat_<T> rr, rl, rt, lt;
    normalizePoints(x1, rr, rt);
    normalizePoints(x2, rl, lt);

    Mat_<T> rrBar, rlBar, rVar, lVar;
    meanAndVarianceAlongRows(rr, rrBar, rVar);
    meanAndVarianceAlongRows(rl, rlBar, lVar);

    Mat_<T> rrp, rlp;
    rrp = rr - repeat(rrBar, x1.rows, x1.cols);
    rlp = rl - repeat(rlBar, x2.rows, x2.cols);

    // TODO: finish implementation
    // https://github.com/vrabaud/sfm_toolbox/blob/master/sfm/computeOrientation.m#L44
  }

  void
  computeOrientation( InputArrayOfArrays _x1,
                      InputArrayOfArrays _x2,
                      OutputArray _R,
                      OutputArray _t,
                      double s )
  {
    const Mat x1 = _x1.getMat(), x2 = _x2.getMat();
    const int depth =  x1.depth();
    CV_Assert(x1.size() == x2.size() && (depth == CV_32F || depth == CV_64F));

    _R.create(3, 3, depth);
    _t.create(3, 1, depth);

    Mat R = _R.getMat(), t = _t.getMat();

    // type
    if( depth == CV_32F )
    {
      computeOrientation<float>(x1, x2, R, t, s);
    }
    else
    {
      computeOrientation<double>(x1, x2, R, t, s);
    }
  }

} /* namespace sfm */
} /* namespace cv */


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <complex>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <utility> // std::move

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"

#include "dune/common/stdstreams.hh"
#include "dune/grid/sgrid.hh"
#include "dune/grid/uggrid.hh"
#include "dune/grid/common/gridinfo.hh"

#include "fem/assemble.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
#include "fem/functional_aux.hh"
#include "fem/coarsening.hh"
#include "fem/norms.hh"

#include "linalg/umfpack_solve.hh"

#include "timestepping/limexWithoutJens.hh"
#include "timestepping/semieuler.hh"
#include "timestepping/extrapolation.hh"

#include "io/vtk.hh"

#include "integrate.hh"
#include "movingsource.hh"

//#include "io/lossystorageDUNE.hh"
#include "io/lossystorageWithoutTemporalPred.hh"
// #include "io/lossystorage.hh"

using namespace Kaskade;

template <class Grid>
std::unique_ptr<Grid> RefineGrid(int refinements, int heapSize=500)
{
  Grid::setDefaultHeapSize(heapSize);
  Dune::GridFactory<Grid> factory;
  Dune::FieldVector<double,2> v;
  // vertices
  v[0]=0; v[1]=0;
  factory.insertVertex(v);
  v[0]=1; v[1]=0;
  factory.insertVertex(v);
  v[0]=1; v[1]=1;
  factory.insertVertex(v);
  v[0]=0; v[1]=1;
  factory.insertVertex(v);
  // elements
  std::vector<unsigned int> vid(3);
  vid[0]=0; vid[1]=1; vid[2]=2;
  factory.insertElement(Dune::GeometryType(Dune::GeometryType::simplex,2),vid);
  vid[0]=0; vid[1]=2; vid[2]=3;
  factory.insertElement(Dune::GeometryType(Dune::GeometryType::simplex,2),vid);
  std::unique_ptr<Grid> grid( factory.createGrid() ) ;
  grid->globalRefine(refinements);
  return grid;
}

struct InitialValue
{
  typedef double Scalar;
  static int const components = 1;
  typedef Dune::FieldVector<Scalar,components> ValueType;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
                  Dune::FieldVector<typename Cell::ctype,Cell::dimension> const& localCoordinate) const
  {
    Dune::FieldVector<typename Cell::ctype,Cell::dimensionworld> x = cell.geometry().global(localCoordinate);

	double vx = x[0]-0.5;
	double vy = x[1]-0.75;

    return 0.8*exp(-80.0*(vx*vx + vy*vy));
  }

private:
  int component;
};

int main(int argc, char *argv[])
  {
    int const dim = 2;
    int refinements = 7, order = 1, extrapolOrder = 1, maxSteps = 10, heapSize = 1000 ;
    double dt = 0.1, maxDT = 1.0, T = 10.0, rTolT = 1.0e-2, aTolT = 1.0e-2, rTolX = 1.0e-4, aTolX = 1.0e-4, writeInterval = 1.0;

    typedef Dune::UGGrid<dim> Grid;
    std::unique_ptr<Grid> grid( RefineGrid<Grid>(refinements,heapSize) );

    std::cout << "Grid: " << grid->size(0) << " " << grid->size(1) << " " << grid->size(2) << std::endl << std::endl;

    GridManager<Grid> gridManager(std::move(grid));

    // construct involved spaces and define equation
    typedef Grid::LeafGridView LeafView;
    typedef FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> > H1Space;

    H1Space temperatureSpace(gridManager,gridManager.grid().leafView(),order);

    typedef boost::fusion::vector<H1Space const*> Spaces;
    Spaces spaces(&temperatureSpace);

    typedef boost::fusion::vector<VariableDescription<0,1,0> > VariableDescriptions;
    std::string varNames[1] = { "u" };

    typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
    VariableSet variableSet(spaces,varNames);

    typedef MovingSourceEquation<double,VariableSet> Equation;
    Equation Eq;

    std::vector<VariableSet::VariableSet> solutions;

    // prepare lossy storage
    std::vector<double> times ;

    int coarseLevel = 0 ;
    double qTol = 1e-5 ;   // quantization error tolerance

    typedef FEFunctionSpace<ContinuousLagrangeMapper<double,Grid::LevelGridView> > HierarchicH1Space;
    typedef UniformQuantizationPolicy<VariableSet,Grid> QuantizationPolicy ;
    typedef LossyStorage<Grid,VariableSet,HierarchicH1Space,QuantizationPolicy> Storage ;

    QuantizationPolicy quantizationPolicy ;
    Storage lossyStorage( gridManager, variableSet, coarseLevel, qTol, true, quantizationPolicy ) ;

    // integrate equation
    Eq.time(0);
    VariableSet::VariableSet x(variableSet);
    Eq.scaleInitialValue<0>(InitialValue(0),x);

    x = integrate(gridManager,Eq,variableSet,spaces,gridManager.grid(),
		  dt,maxDT,T,maxSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
		  std::back_inserter(solutions),writeInterval,x,DirectType::UMFPACK,times,lossyStorage);

    // decode files & check error
    // decode backwards in time (necessary if temporal prediction/differential
    // encoding is used)
    std::cout << "\n\nckecking error:\n" ;

    VariableSet::VariableSet state_data(variableSet) ;
    L2Norm l2 ;

    std::ostringstream fn ;
    fn.width(3);  fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    for( int t = times.size()-1 ; t >= 0 ; t-- )
    {
      fn << "graph/quant" ;
      fn << t ;
      fn.flush();
      lossyStorage.decode( gridManager, state_data, fn.str() ) ;

      // write reconstructed states as vtu-files
//       writeVTKFile(gridManager.grid().leafView(), variableSet, state_data, fn.str());

      fn.clear() ; fn.str("");

      // compute and print L^\infty error
      state_data -= solutions[t] ;
      std::vector<double> foo( gridManager.grid().size(dim) ) ;
      state_data.write( foo.begin() ) ;
      double absQuantErr = fabs(*std::max_element(foo.begin(),foo.end(),abscompare)) ;
      std::cout << "t = " << times[t] << "\tL^\\infty = " << absQuantErr ;
      absQuantErr = sqrt(l2.square( boost::fusion::at_c<0>(state_data.data) ));
      std::cout << "\tL^2 = " << absQuantErr << "\n" ;
    }

    return 0;

//     std::cout << times.size() << " timesteps.\n" ;
//     for( int i = 0 ; i < times.size() ; i++ ) std::cout << times[i] << "   " ;
//     std::cout << "\n\n" ;
//     std::cout << "Solution vector contains " << solutions.size() << " entries.\n" ;
//
//     int windowSize = 2 ;
//
//     for( int i = 0 ; i < times.size()-windowSize ; i++ )
//     {
//
//       VariableSet::VariableSet a = solutions[i+1] , b = solutions[i+0] ;
//       a -= solutions[i+0] ; a *= 1.0/(times[i+1]-times[i+0]) ;
//
//       VariableSet::VariableSet pred = b ;
//       pred.axpy(times[i+2]-times[i+0],a);
//
//       typedef Grid::LeafGridView LeafGridView;
//       LeafGridView leafView = gridManager.grid().leafView();
//       std::ostringstream fn ;
//       fn.width(3);  fn.fill('0');
//       fn.setf(std::ios_base::right,std::ios_base::adjustfield);
//       fn << "graph/pred" ;
//       fn << i+2 ;
//       fn.flush();
//       writeVTKFile(leafView,variableSet,pred,fn.str() );
//
//       lossyStorage.encode( pred, fn.str() ) ;
//
//       fn.str("") ;
//       fn << "graph/diff" ;
//       fn << i+2 ;
//       fn.flush();
//       pred -= solutions[i+2] ;
//       writeVTKFile(leafView,variableSet,pred,fn.str());
//
//       lossyStorage.encode( pred, fn.str() ) ;
//     }
  }


/*    Copyright (c) 2010-2016, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      160406    M. Van den Broeck Creation
 *      <YYMMDD>  <author name>     <comment>
 *
 *    References
 *      Vittaldev, V. (2010). The Unified State Model: Derivation and application in astrodynamics
 *          and navigation. Master's thesis, Delft University of Technology.
 *      <Second reference>
 *
 *    Notes
 *
 */

#include <cmath>

#include <boost/exception/all.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{

namespace orbital_element_conversions
{

//! Convert Keplerian elements to Unified State Model elements.
Eigen::Matrix< double, 7, 1 > convertKeplerianToUnifiedStateModelElements(
        const basic_mathematics::Vector6d& keplerianElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;

    // Declaring eventual output vector.
    Eigen::Matrix< double, 7, 1 > convertedUnifiedStateModelElements = Eigen::Matrix< double, 7, 1 >::Zero( );

    // Define the tolerance of a singularity
    double singularityTolerance = 1.0e-15; // Based on tolerance chosen in
                                           // orbitalElementConversions.cpp in Tudat Core.

    // If eccentricity is outside range [0,inf)
    if ( keplerianElements( eccentricityIndex ) < 0.0 )
    {
        //Define the error message
        std::stringstream errorMessage;
        errorMessage << "Eccentricity is expected in range [0,inf)\n"
                     << "Specified eccentricity: " << keplerianElements( eccentricityIndex ) << std::endl;

        // Throw exception
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If inclination is outside range [0,PI]
    if ( ( keplerianElements( inclinationIndex ) < 0.0 ) || ( keplerianElements( inclinationIndex ) > PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "Inclination is expected in range [0," << PI << "]\n"
                     << "Specified inclination: " << keplerianElements( inclinationIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If argument of pericenter is outside range [0,2.0 * PI]
    if ( ( keplerianElements( argumentOfPeriapsisIndex ) < 0.0 ) || ( keplerianElements( argumentOfPeriapsisIndex ) >
                                                                      2.0 * PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "RAAN is expected in range [0," << 2.0 * PI << "]\n"
                     << "Specified inclination: " << keplerianElements( argumentOfPeriapsisIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If right ascension of ascending node is outside range [0,2.0 * PI]
    if ( ( keplerianElements( longitudeOfAscendingNodeIndex ) < 0.0 ) ||
         ( keplerianElements( longitudeOfAscendingNodeIndex ) > 2.0 * PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "RAAN is expected in range [0," << 2.0 * PI << "]\n"
                     << "Specified inclination: " << keplerianElements( longitudeOfAscendingNodeIndex ) << " rad."
                     << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If true anomaly is outside range [0,2.0 * PI]
    if ( ( keplerianElements( trueAnomalyIndex ) < 0.0 ) || ( keplerianElements( trueAnomalyIndex ) > 2.0 * PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "RAAN is expected in range [0," << 2.0 * PI << "]\n"
                     << "Specified inclination: " << keplerianElements( trueAnomalyIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If inclination is zero and the right ascension of ascending node is non-zero
    if ( ( std::fabs( keplerianElements( inclinationIndex ) ) < singularityTolerance ) &&
         ( std::fabs( keplerianElements( longitudeOfAscendingNodeIndex ) ) > singularityTolerance ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the inclination is zero, the right ascending node should be zero by definition\n"
                     << "Specified right ascension of ascending node: " <<
                        keplerianElements( longitudeOfAscendingNodeIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If eccentricity is zero and the argument of pericenter is non-zero
    if ( ( std::fabs( keplerianElements( eccentricityIndex ) ) < singularityTolerance ) &&
         ( std::fabs( keplerianElements( argumentOfPeriapsisIndex ) ) > singularityTolerance ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the eccentricity is zero, the argument of pericenter should be zero by definition\n"
                     << "Specified argument of pericenter: " <<
                        keplerianElements( argumentOfPeriapsisIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If semi-major axis is negative and the eccentricity is smaller or equal to one
    if ( ( keplerianElements( semiMajorAxisIndex ) < 0.0 ) && ( keplerianElements( eccentricityIndex ) <= 1.0 ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the semi-major axis is negative, the eccentricity should be larger than one\n"
                     << "Specified semi-major axis: " << keplerianElements( semiMajorAxisIndex ) << " m.\n"
                     << "Specified eccentricity: " << keplerianElements( eccentricityIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If semi-major axis is positive and the eccentricity is larger than one
    if ( ( keplerianElements( semiMajorAxisIndex ) > 0.0 ) && ( keplerianElements( eccentricityIndex ) > 1.0 ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the semi-major axis is positive, the eccentricity should be smaller than or equal to one\n"
                     << "Specified semi-major axis: " << keplerianElements( semiMajorAxisIndex ) << " m.\n"
                     << "Specified eccentricity: " << keplerianElements( eccentricityIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    //Else, nothing wrong and continue

    // Compute the C hodograph element of the Unified State Model
    if ( std::fabs( keplerianElements( eccentricityIndex ) - 1.0) < singularityTolerance )
            // parabolic orbit -> semi-major axis is not defined
    {
        convertedUnifiedStateModelElements( CHodographIndex ) =
                std::sqrt( centralBodyGravitationalParameter / keplerianElements( semiLatusRectumIndex ) );
    }
    else
    {
        convertedUnifiedStateModelElements( CHodographIndex ) =
                std::sqrt( centralBodyGravitationalParameter / ( keplerianElements( semiMajorAxisIndex )
                                                  * ( 1 - keplerianElements( eccentricityIndex ) *
                                                      keplerianElements( eccentricityIndex ) ) ) );
    }

    // Calculate the additional R hodograph parameter
    double RHodographElement = keplerianElements( eccentricityIndex ) *
            convertedUnifiedStateModelElements( CHodographIndex );

    // Compute the Rf1 hodograph element of the Unified State Model
    convertedUnifiedStateModelElements( Rf1HodographIndex ) =
            - RHodographElement * std::sin( keplerianElements( longitudeOfAscendingNodeIndex )
                                            + keplerianElements( argumentOfPeriapsisIndex ) );

    // Compute the Rf2 hodograph element of the Unified State Model
    convertedUnifiedStateModelElements( Rf2HodographIndex ) =
              RHodographElement * std::cos( keplerianElements( longitudeOfAscendingNodeIndex )
                                            + keplerianElements( argumentOfPeriapsisIndex ) );

    // Calculate the additional argument of longitude u
    double argumentOfLongitude = keplerianElements( argumentOfPeriapsisIndex ) +
            keplerianElements( trueAnomalyIndex );

    // Compute the epsilon1 quaternion of the Unified State Model
    convertedUnifiedStateModelElements( epsilon1QuaternionIndex ) =
            std::sin( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::cos( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) - argumentOfLongitude ) );

    // Compute the epsilon2 quaternion of the Unified State Model
    convertedUnifiedStateModelElements( epsilon2QuaternionIndex ) =
            std::sin( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::sin( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) - argumentOfLongitude ) );

    // Compute the epsilon3 quaternion of the Unified State Model
    convertedUnifiedStateModelElements( epsilon3QuaternionIndex ) =
            std::cos( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::sin( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) + argumentOfLongitude ) );

    // Compute the eta quaternion of the Unified State Model
    convertedUnifiedStateModelElements( etaQuaternionIndex ) =
            std::cos( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::cos( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) + argumentOfLongitude ) );

    // Give back result
    return convertedUnifiedStateModelElements;

}

//! Convert Unified State Model elements to Keplerian elements.
basic_mathematics::Vector6d convertUnifiedStateModelToKeplerianElements(
        const Eigen::Matrix< double, 7, 1 >& unifiedStateModelElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;

    // Declaring eventual output vector.
    basic_mathematics::Vector6d convertedKeplerianElements = basic_mathematics::
            Vector6d::Zero( 6 );

    // Define the tolerance of a singularity
    double singularityTolerance = 1.0e-15; // Based on tolerance chosen in
                                           // orbitalElementConversions.cpp in Tudat Core.

    // Declare auxiliary parameters before using them in the if loop
    double cosineLambda = 0.0;
    double sineLambda = 0.0;
    double lambdaFromSineAndCosine = 0.0;

    // Check whether the Unified State Model elements are within expected limits
    // If inclination is zero and the right ascension of ascending node is non-zero
    const double normOfQuaternionElements = std::sqrt( std::pow( unifiedStateModelElements( epsilon1QuaternionIndex ), 2 ) +
                                                       std::pow( unifiedStateModelElements( epsilon2QuaternionIndex ), 2 ) +
                                                       std::pow( unifiedStateModelElements( epsilon3QuaternionIndex ), 2 ) +
                                                       std::pow( unifiedStateModelElements( etaQuaternionIndex ), 2 ) );

    if ( std::fabs( normOfQuaternionElements - 1.0 ) > singularityTolerance )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "The norm of the quaternion should be equal to one.\n"
                     << "Norm of the specified quaternion is: " << normOfQuaternionElements << " ." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    //Else, nothing wrong and continue

    // Compute auxiliary parameters cosineLambda, sineLambda and Lambda
    if ( ( std::fabs( unifiedStateModelElements( epsilon3QuaternionIndex ) ) < singularityTolerance )
        && ( std::fabs( unifiedStateModelElements( etaQuaternionIndex ) ) < singularityTolerance ) )
            // pure-retrograde orbit -> inclination  = pi
    {
        //Define the error message
        std::stringstream errorMessage;
        errorMessage << "Pure-retrograde orbit (inclination = pi).\n"
                     << "Unified State Model elements cannot be transformed to Kepler elements." << std::endl;

        // Throw exception
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    else
    {
        cosineLambda = ( unifiedStateModelElements( etaQuaternionIndex ) *
                                unifiedStateModelElements( etaQuaternionIndex ) -
                                unifiedStateModelElements( epsilon3QuaternionIndex ) *
                                unifiedStateModelElements( epsilon3QuaternionIndex ) )
                / ( unifiedStateModelElements( epsilon3QuaternionIndex ) *
                    unifiedStateModelElements( epsilon3QuaternionIndex ) +
                    unifiedStateModelElements( etaQuaternionIndex ) *
                    unifiedStateModelElements( etaQuaternionIndex ) );
        sineLambda = ( 2.0 *
                              unifiedStateModelElements( epsilon3QuaternionIndex ) *
                              unifiedStateModelElements( etaQuaternionIndex ) )
                / ( unifiedStateModelElements( epsilon3QuaternionIndex ) *
                    unifiedStateModelElements( epsilon3QuaternionIndex ) +
                    unifiedStateModelElements( etaQuaternionIndex ) *
                    unifiedStateModelElements( etaQuaternionIndex ) );
        lambdaFromSineAndCosine = std::atan2( sineLambda, cosineLambda );
    }

    // Compute auxiliary parameters auxiliaryParameter1 and auxiliaryParameter2
    double auxiliaryParameter1 = unifiedStateModelElements( Rf1HodographIndex ) * cosineLambda +
            unifiedStateModelElements( Rf2HodographIndex ) * sineLambda;
    double auxiliaryParameter2 = unifiedStateModelElements( CHodographIndex ) -
            unifiedStateModelElements( Rf1HodographIndex ) * sineLambda +
            unifiedStateModelElements( Rf2HodographIndex ) * cosineLambda;

    // Compute auxiliary R hodograph parameter
    double RHodographElement = std::sqrt( unifiedStateModelElements( Rf1HodographIndex )
                                          * unifiedStateModelElements( Rf1HodographIndex )
                                          + unifiedStateModelElements( Rf2HodographIndex )
                                          * unifiedStateModelElements( Rf2HodographIndex ));

    // Compute eccentricity
    convertedKeplerianElements( eccentricityIndex ) =
            RHodographElement / unifiedStateModelElements( CHodographIndex );

    // Compute semi-major axis or, in case of a parabolic orbit, the semi-latus rectum.
    if ( std::fabs( convertedKeplerianElements( eccentricityIndex ) - 1.0 ) < singularityTolerance )
            // parabolic orbit -> semi-major axis is not defined. Use semi-latus rectum instead.
    {
        convertedKeplerianElements( semiLatusRectumIndex ) = centralBodyGravitationalParameter /
                ( unifiedStateModelElements( CHodographIndex ) * unifiedStateModelElements( CHodographIndex ) );
    }
    else
    {
        convertedKeplerianElements( semiMajorAxisIndex ) =
                centralBodyGravitationalParameter /
                ( 2.0 * unifiedStateModelElements( CHodographIndex ) * auxiliaryParameter2 -
                    ( auxiliaryParameter1 * auxiliaryParameter1 + auxiliaryParameter2 * auxiliaryParameter2 ) );
    }

    // Compute inclination
    convertedKeplerianElements( inclinationIndex ) =
            std::acos( 1.0 - 2.0 * ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                     unifiedStateModelElements( epsilon1QuaternionIndex ) +
                                     unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                     unifiedStateModelElements( epsilon2QuaternionIndex ) ) );
        // This acos is always defined correctly because the inclination is always below pi rad.

    // Compute longitude of ascending node
    if ( ( ( std::fabs( unifiedStateModelElements( epsilon1QuaternionIndex ) ) < singularityTolerance )
           && ( std::fabs( unifiedStateModelElements( epsilon2QuaternionIndex ) ) < singularityTolerance ) ) ||
         ( ( std::fabs( unifiedStateModelElements( epsilon3QuaternionIndex ) ) < singularityTolerance )
         && ( std::fabs( unifiedStateModelElements( etaQuaternionIndex ) ) < singularityTolerance ) ) )
            // pure-prograde or pure-retrograde orbit
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0; // by definition
    }
    else
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) =
                std::atan2( ( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                unifiedStateModelElements( epsilon3QuaternionIndex ) +
                                unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                unifiedStateModelElements( etaQuaternionIndex ) )
                           / ( std::sqrt( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                               unifiedStateModelElements( epsilon1QuaternionIndex ) +
                                               unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                               unifiedStateModelElements( epsilon2QuaternionIndex ) ) *
                                          ( unifiedStateModelElements( etaQuaternionIndex ) *
                                               unifiedStateModelElements( etaQuaternionIndex ) +
                                               unifiedStateModelElements( epsilon3QuaternionIndex ) *
                                               unifiedStateModelElements( epsilon3QuaternionIndex ) ) ) ) ),
                            ( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                             unifiedStateModelElements( etaQuaternionIndex ) -
                             unifiedStateModelElements( epsilon2QuaternionIndex ) *
                             unifiedStateModelElements( epsilon3QuaternionIndex ) )
                        / ( std::sqrt( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                            unifiedStateModelElements( epsilon1QuaternionIndex ) +
                                            unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                            unifiedStateModelElements( epsilon2QuaternionIndex ) ) *
                                       ( unifiedStateModelElements( etaQuaternionIndex ) *
                                            unifiedStateModelElements( etaQuaternionIndex ) +
                                            unifiedStateModelElements( epsilon3QuaternionIndex ) *
                                            unifiedStateModelElements( epsilon3QuaternionIndex ) ) ) ) ) );

        // Round off small values of the right ascension of ascending node to zero
        if ( std::fabs( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
        }
        // Ensure the longitude of ascending node is positive
        while ( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) < 0.0 )
                // Because of the previous if loop, if the longitude of ascending node is smaller than 0, it will
                // always be smaller than -singularityTolerance
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) =
                    convertedKeplerianElements( longitudeOfAscendingNodeIndex ) + 2.0 * PI;
        }
    }

    // Compute true anomaly and argument of periapsis
    if ( std::fabs( RHodographElement ) < singularityTolerance ) // circular orbit
    {
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0; // by definition
        convertedKeplerianElements( trueAnomalyIndex ) =
                lambdaFromSineAndCosine - convertedKeplerianElements( longitudeOfAscendingNodeIndex );

        // Round off small theta to zero
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }

        // Ensure the true anomaly is positive
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
                // Because of the previous if loop, if the true anomaly is smaller than zero, it will always be smaller than
                // -singularityTolerance
        {
            convertedKeplerianElements( trueAnomalyIndex ) =
                    convertedKeplerianElements( trueAnomalyIndex ) + 2.0 * PI;
        }
    }
    else
    {
        convertedKeplerianElements( trueAnomalyIndex ) =
                std::atan2( ( auxiliaryParameter1 / RHodographElement ),
                            ( ( auxiliaryParameter2 - unifiedStateModelElements( CHodographIndex ) )
                           / RHodographElement ) );

        // Round off small theta to zero
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }

        // Ensure the true anomaly is positive
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
            // Because of the previous if loop, if the true anomaly is smaller than zero, it will always
            // be smaller than -singularityTolerance
        {
            convertedKeplerianElements( trueAnomalyIndex ) =
                    convertedKeplerianElements( trueAnomalyIndex ) + 2.0 * PI;
        }

        convertedKeplerianElements( argumentOfPeriapsisIndex ) =
                lambdaFromSineAndCosine -
                convertedKeplerianElements( longitudeOfAscendingNodeIndex ) -
                convertedKeplerianElements( trueAnomalyIndex );

        // Round off small omega to zero
        if ( std::fabs( convertedKeplerianElements( argumentOfPeriapsisIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0;
        }

        // Ensure the argument of periapsis is positive
        while ( convertedKeplerianElements( argumentOfPeriapsisIndex ) < 0.0 )
            // Because of the previous if loop, if the argument of pericenter is smaller than zero,
            // it will be smaller than -singularityTolerance
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) =
                    convertedKeplerianElements( argumentOfPeriapsisIndex ) + 2.0 * PI;
        }
    }

    // Give back result
    return convertedKeplerianElements;
}

} // close namespace orbital_element_conversions

} // close namespace tudat


/**

\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_ALGO_LM_MANY_CLASSES_LEVMAR_HPP__
#define __OPTIMISATION2_ALGO_LM_MANY_CLASSES_LEVMAR_HPP__

#include <boost/fusion/include/for_each.hpp>
#include <boost/mpl/copy_if.hpp>
#include <libv/lma/lm/ba/bas.hpp>
#include <libv/lma/lm/ba/initialize.hpp>
#include <libv/lma/lm/ba/fill_hessien.hpp>
#include <libv/lma/lm/bundle/cost_and_save.hpp>
#include <libv/lma/lm/container/map_container.hpp>
#include <libv/lma/lm/container/container.hpp>
#include <libv/lma/lm/function/function.hpp>
#include <libv/lma/lm/trait/use_estimator.hpp>
#include <libv/lma/time/tictoc.hpp>

namespace lma
{
  template<class F> struct MakeResidualVector : bf::pair<F,AlignVector<std::pair<typename Function<F>::ErreurType,bool>>> {};

  template<> struct MakeResidualVector<mpl::_1>{};

  template<class A, class A_, class B, class B_, class C, class C_>
  void prod_v_v_h(VectorColumn<A,A_>& a, const VectorColumn<B,B_>& b, const DiagonalTable<C,C_>& c)
  {
    for(auto i = c.first() ; i < c.size() ; ++i)
      a(i) += b(i) * c(i);
  }

  template<class A, class A_, class B, class B_, class _>
  void prod_v_v_h(VectorColumn<B,B_>& r, const VectorColumn<A,A_>& delta, const Table<A,B,_>& h)
  {
    for(auto i = h.first() ; i < h.size() ; ++i)
      for(auto j = h.first(i) ; j < h.size(i) ; ++j)
        r(h.indice(i,j)) += delta(i) * h(i,j);
  }

  template<class A, class A_, class B, class B_, class _>
  void prod_v_v_h_t(VectorColumn<A,A_>& r, const VectorColumn<B,B_>& delta, const Table<A,B,_>& h)
  {
    for(auto i = h.first() ; i < h.size() ; ++i)
      for(auto j = h.first(i) ; j < h.size(i) ; ++j)
        r(i) += delta(h.indice(i,j)) * transpose(h(i,j));
  }

  template<class A, class B, class C, class C_>
  void prod_v_v_h_dispatch(A& r, const B& delta, const DiagonalTable<C,C_>& c)
  {
    prod_v_v_h(bf::at_key<C>(r),bf::at_key<C>(delta),c);
  }

  template<class A, class B, class C1, class C2, class _>
  void prod_v_v_h_dispatch(A& r, const B& delta, const Table<C1,C2,_>& c)
  {
    prod_v_v_h(bf::at_key<C2>(r),bf::at_key<C1>(delta),c);
    prod_v_v_h_t(bf::at_key<C1>(r),bf::at_key<C2>(delta),c);
  }

  struct GetMax
  {
    double& lambda;
    GetMax(double& d):lambda(d){}
    template<class Table> void operator()(const Table& table){ lambda = std::max(lambda,table.max_lambda());}
  };

  struct ComputeScale
  {
    double &scale;
    double λ
    ComputeScale(double& scale_, double& lambda_):scale(scale_),lambda(lambda_){}
    template<class Delta, class Jte> void operator() (const Delta& delta, const Jte& jte)
    {
      for(auto i = delta.first() ; i < delta.size() ; ++i)
        for(size_t k = 0 ; k < delta.I; ++k)
          scale += (delta(i)[k] * (lambda * delta(i)[k] + jte(i)[k]));
    };
  };

  template<class Policy> struct LevMar : Policy
  {
    typedef LevMar<Policy> type;

    typedef utils::Tic<false> Tic;
    typedef typename Policy::Bundle Bundle;
    typedef typename Policy::Ba Ba;
    typedef typename Ba::Keys Keys;

    Ba ba_;
    const Ba& ba() const { return ba_; }

    double residual_evaluations;
    double jacobian_evaluations;
    double norm_eq_;
    double preprocess;

    double prev_lambda;
    double erreur_,previous_erreur_;
    double nb_used_fonctor;

    typedef MultiContainer<typename Bundle::ListFunction,MakeResidualVector<mpl::_1>> ContainerMapErreur;

    utils::Tic<true> free_tic;

    ContainerMapErreur map_erreur;

    static std::string name(){ return ttt::name<LevMar<Policy>>(); }

    template<class Config>
    LevMar(Config config):Policy(config),prev_lambda(-1.0),erreur_(-1.0),previous_erreur_(-1.0)
    {
      residual_evaluations = 0;
      preprocess = norm_eq_ = jacobian_evaluations = 0;
    }

    typedef typename
    mpl::copy_if<
                  typename Bundle::ListFunction,
                  detail::IsMEstimator<mpl::_1>
                >::type MEstimatorList;

    typedef typename
      br::as_map<
                  typename mpl::transform<
                                          MEstimatorList,
                                          bf::pair<mpl::_1,double>
                                         >::type
                >::type Meds;
    Meds meds;//bf::tuple< pair<F1,double>, pair<F2,double> ...>

    void init(Bundle& bundle_)
    {
      free_tic.tic();
      bundle_.update();
      initialize(bundle_,ba_);
      Policy::init(bundle_,ba_);
      cost_and_save_mad<Meds>(bundle_,meds);
      preprocess = free_tic.toc();
    }

    void restore_erreur()
    {
      assert(erreur_!=-1.0);//! l'erreur a déjà été calculé au moins une fois
      erreur_ = previous_erreur_;
    }

    double get_erreur() const
    {
      assert(erreur_!=-1.0);
      return erreur_;
    }

    std::pair<double,int> compute_erreur(const Bundle& bundle_)
    {
      free_tic.tic();
      if (erreur_!=-1.0)
        previous_erreur_ = erreur_;
      std::tie(erreur_,nb_used_fonctor) =  cost_and_save(bundle_,map_erreur,meds);
      if (erreur_==-1)
	     std::cerr << " LMA::compute_erreur " << erreur_ << " " << previous_erreur_ << std::endl;
      assert(erreur_!=-1.0);
      residual_evaluations += free_tic.toc();
      return {erreur_,nb_used_fonctor};
    }

    double compute_scale(double lambda) const
    {
      double scale = 0.0;
      ComputeScale cs(scale,lambda);
      for_each<MetaBinary<typename Ba::Keys>>(std::tie(ba_.delta,ba_.jte),cs);
      return scale;
    }

    double init_lambda()
    {
      double lambda = 0;
      GetMax gm(lambda);
//       for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),[&lambda](auto& table){ lambda = std::max(lambda,table.max_lambda());});
      for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),gm);
      return lambda * 1e-5;
    }

//     double prediction(const Bundle& bundle_)
//     {
//       double pred=0;
//       double alpha=0;
//       typename Ba::Vectors v = ba_.jte;
//
//       bf::for_each(v,[](auto& pair){ pair.second.set_zero(); });
//       bf::for_each(ba_.h,[&](auto& pair){prod_v_v_h_dispatch(v,ba_.delta,pair.second);});
//       for_each<MetaBinary<typename Ba::Keys>>(std::tie(v,ba_.delta),[&alpha](auto& a, auto& b){
//         for(auto i = a.first() ; i < a.size() ; ++i)
//           alpha += a(i)*b(i);
//       });
//       alpha /= 2.0;
//       std::cout << " alpha : " << alpha << std::endl;
// //       abort();
//       bf::for_each(ba_.jacob,
//         [&](auto& pair)
//         {
//           typedef typename std::decay<decltype(pair)>::type Pair;
//           typedef typename Pair::first_type Obs;
//           auto& jacobs = pair.second;
//           TooN::Vector<2,double> vec = TooN::Ones;
//           vec *= alpha;
//
//           for(size_t iobs = 0 ; iobs < jacobs.size() ; ++iobs)
//           {
//             bf::for_each(jacobs.at(iobs),
//               [&]
//               (auto& pair_block_jacob)
//               {
//                 typedef typename std::decay<decltype(pair_block_jacob)>::type PairBlockJacob;
//                 typedef typename PairBlockJacob::first_type ParamType;
//                 auto& block = pair_block_jacob.second;
//                 auto& container_delta = bf::at_key<ParamType>(ba_.delta);
//                 typedef typename std::decay<decltype(container_delta)>::type::MatrixTag MatrixTag;
//                 const auto& map_indice = bundle_.spi2.indices(ttt::Indice<Obs>(iobs));// vector<Indice,...>
//                 auto& indice = *bf::find<ttt::Indice<ParamType>>(map_indice);
//
//                 pred +=
//                         squared_norm(make_view(map_erreur.template at_key<Obs>().at(iobs).first,ttt::wrap<MatrixTag>())
//                         +
//                         block * container_delta(indice)
//                         + vec);
//               });
//           }
//         });
//       // now I'm damned
//       return pred;
//     }

//     void compute(Bundle& bundle_, double& lambda, bool recalcul, double& pred)
//     {
//       set_zero_(ba_);
//       Policy::init_zero();
//
//       if (recalcul)
//       {
//         bf::for_each(ba_.h,detail::SetZero());
//         bf::for_each(ba_.jte,detail::SetZero());
//         // compute H = JtJ & Jte
//         detail::fill_hessien_residu33<Derivator<typename Policy::MatrixTag>>(bundle_,ba_,map_erreur);
//         Policy::save_h(ba_.h);
//       }
//       else
//       {
//         Policy::reload_h(ba_.h,-prev_lambda);
//       }
//
//       prev_lambda = lambda;
//       for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),LambdaDiag(lambda));
//       Policy::solve(ba_,bundle_);
//
//       pred = prediction(bundle_);
//
//       for_each<MetaBinary<typename Ba::Keys>>(std::tie(bundle_.opt_container.map(),ba_.delta),Correct());
//     }

    void compute(Bundle& bundle_, double& lambda, bool recalcul)
    {
      Tic tic("compute all");
      assert(bundle_.nb_obs()!=0);
      try
      {
        set_zero_(ba_);
        Policy::init_zero();

        free_tic.tic();
        Tic tic_h("compute H");
        if (recalcul)
        {
          bf::for_each(ba_.h,detail::SetZero());
          bf::for_each(ba_.jte,detail::SetZero());
          // compute H = JtJ & Jte
          detail::fill_hessien<typename Policy::MatrixTag>(bundle_,ba_,map_erreur,meds);
          Policy::save_h(ba_.h);
        }
        else
        {
          Policy::reload_h(ba_.h,-prev_lambda);
        }
        tic_h.disp();
        jacobian_evaluations += free_tic.toc();

        free_tic.tic();

        // H += Eye(H)*lambda
        if (lambda==-1)
        {
          lambda = init_lambda();
        }

        prev_lambda = lambda;
        for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),LambdaDiag(lambda));
        Tic tic_schur("SolveDelta");

//         clement(ba_.h);
//         std::cout << ba_.h << std::endl;
//           auto a = to_mat<typename Ba::Keys,typename Ba::ListeHessien>(ba_.h,size_tuple<mpl::size<typename Ba::Keys>::value>(ba_.delta));

//         auto u = Blocker<2,2>::view(a,0,0);
//         auto v = Blocker<2,2>::view(a,2,2);
//         auto w = Blocker<2,2>::view(a,0,2);
//
//         std::cout << " A :\n" << a << std::endl << std::endl << std::endl;
//         std::cout << " V :\n" << v << std::endl;
//         std::cout << " W :\n" << w << std::endl;
//
//         auto b = to_matv(ba_.jte());
//         auto ea = Blocker<2,1>::view(b,0,0);
//         auto eb = Blocker<2,1>::view(b,2,0);
//         std::cout << " jte : " << b.transpose() << std::endl;
//         std::cout << " ea  : " << ea.transpose() << std::endl;
//         std::cout << " eb  : " << eb.transpose() << std::endl;
//
//         auto y = w*v.inverse();
//         auto s = u - y * w.transpose();
//         auto e = ea - y * eb;
//
//         auto da = (s.template selfadjointView<Eigen::Upper>().llt().solve(e)).eval();
//         auto db = v.inverse()*(eb - w.transpose() * da);
//
//         std::cout << " S : \n" << s << std::endl;
//         std::cout << std::endl;
//
//         std::cout << da.transpose() << " " << db.transpose() << std::endl;

        Policy::solve(ba_,bundle_);

//         std::cout << to_matv(ba_.delta()).transpose() << std::endl;
//         std::cout << std::endl;
        tic_schur.disp();

        for_each<MetaBinary<typename Ba::Keys>>(std::tie(bundle_.opt_container.map(),ba_.delta),Correct());

        norm_eq_ += free_tic.toc();
      }
      catch(NAN_ERROR& e)
      {
        std::cout << " Optimization failure : " <<  e.what() << std::endl;
      }
      tic.disp();
    }
  };
}

namespace ttt
{
  template<class A> struct Name< lma::LevMar<A> > { static std::string name(){ return "LevMar<" + ttt::name<A>() + ">";} };
}

#endif


#pragma once

#include <iostream>
#include <experimental/filesystem>
#include <fstream>
#include <random>

#define EIGEN_DONT_PARALLELIZE 1

#include <Eigen/Dense>

// Function: read_mnist_label
inline auto read_mnist_label(const std::experimental::filesystem::path& path) {

  // Helper lambda.
  auto reverse_int = [](int i) {
    unsigned char c1, c2, c3, c4;
    c1 = i         & 255;
    c2 = (i >> 8)  & 255;
    c3 = (i >> 16) & 255;
    c4 = (i >> 24) & 255;
    return ((int)c1 << 24) + ((int)c2 << 16) + ((int)c3 << 8) + c4;
  };

  // Read the image.
  std::ifstream ifs(path, std::ios::binary);

  if(!ifs) {
    assert(false);
  }

  int magic_number = 0;
  int num_imgs = 0;

  ifs.read((char*)&magic_number, sizeof(magic_number));
  magic_number = reverse_int(magic_number);

  ifs.read((char*)&num_imgs, sizeof(num_imgs));
  num_imgs = reverse_int(num_imgs);

  Eigen::VectorXi labels(num_imgs);
  for (int i = 0; i<num_imgs; ++i) {
    unsigned char temp = 0;  // must use unsigned
    ifs.read((char*)&temp, sizeof(temp));
    labels[i] = static_cast<int>(temp);
  }
  return labels;
}

inline auto read_mnist_image(const std::experimental::filesystem::path& path) {

  // Helper lambda.
  auto reverse_int = [] (int i) {
    unsigned char c1, c2, c3, c4;
    c1 = i         & 255;
    c2 = (i >> 8)  & 255;
    c3 = (i >> 16) & 255;
    c4 = (i >> 24) & 255;
    return ((int)c1 << 24) + ((int)c2 << 16) + ((int)c3 << 8) + c4;
  };

  // Read the image.
  std::ifstream ifs(path, std::ios::binary);

  if(!ifs) {
    assert(false);
  }

  int magic_number = 0;
  int num_imgs = 0;
  int num_rows = 0;
  int num_cols = 0;

  ifs.read((char*)&magic_number, sizeof(magic_number));
  magic_number = reverse_int(magic_number);

  ifs.read((char*)&num_imgs, sizeof(num_imgs));
  num_imgs = reverse_int(num_imgs);

  ifs.read((char*)&num_rows, sizeof(num_rows));
  num_rows = reverse_int(num_rows);

  ifs.read((char*)&num_cols, sizeof(num_cols));
  num_cols = reverse_int(num_cols);

  Eigen::MatrixXf images(num_imgs, num_rows*num_cols);

  for(int i = 0; i < num_imgs; ++i) {
    for(int r = 0; r < num_rows; ++r) {
      for(int c = 0; c < num_cols; ++c) {
        unsigned char p = 0;  // must use unsigned
        ifs.read((char*)&p, sizeof(p));
        images(i, r*num_cols + c) = static_cast<float>(p);
      }
    }
  }

  for(int i=0; i<images.rows(); i++) {
    for(int j=0; j<images.cols(); j++) {
      images(i, j) /= 255.0;
    }
  }
  return images;
}

inline auto time_diff(
  std::chrono::time_point<std::chrono::high_resolution_clock> &t1,
  std::chrono::time_point<std::chrono::high_resolution_clock> &t2
) {
  return std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count();
}

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

enum class Activation {
  NONE,
  RELU,
  SIGMOID
};

// Procedure: sigmoid
inline void sigmoid(Eigen::MatrixXf& x) {
  x = ((1.0f + (-x).array().exp()).inverse()).matrix();
}

// Procedure: relu
inline void relu(Eigen::MatrixXf& x) {
  for(int j=0; j<x.cols(); ++j) {
    for(int i=0; i<x.rows(); ++i) {
      if(x(i, j) <= 0.0f) {
        x(i, j) = 0.0f;
      }
    }
  }
}

inline void activate(Eigen::MatrixXf& mat, Activation act) {
  switch(act) {
    case Activation::NONE:  return;
    case Activation::SIGMOID: sigmoid(mat); return;
    case Activation::RELU: relu(mat); return;
  };
}

// Function: drelu
inline void drelu(Eigen::MatrixXf& x) {
  for(int j=0; j<x.cols(); ++j) {
    for(int i=0; i<x.rows(); ++i) {
      x(i, j) = x(i, j) > 0.0f ? 1.0f : 0.0f;
    }
  }
}

// Function: dsigmoid
inline void dsigmoid(Eigen::MatrixXf& x) {
  x = x.array() * (1 - x.array());
}

inline void deactivate(Eigen::MatrixXf& mat, Activation act) {
  switch(act) {
    case Activation::NONE:    mat = Eigen::MatrixXf::Ones(mat.rows(), mat.cols()); return;
    case Activation::SIGMOID: dsigmoid(mat); return ;
    case Activation::RELU:    drelu(mat); return;
  };
}

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

struct MNIST_DNN {
  // Ctor
  MNIST_DNN() = default;

  void add_layer(size_t in_degree, size_t out_degree, Activation act) {
    acts.emplace_back(act);
    Ys.emplace_back().resize(batch_size, out_degree);
    Ws.push_back(Eigen::MatrixXf::Random(in_degree, out_degree));
    Bs.push_back(Eigen::MatrixXf::Random(1, out_degree));

    dW.emplace_back().resize(in_degree, out_degree);
    dB.emplace_back().resize(1, out_degree);
  }

  void forward(size_t layer, const Eigen::MatrixXf& mat) {
    Ys[layer] = mat * Ws[layer] + Bs[layer].replicate(mat.rows(), 1);
    activate(Ys[layer], acts[layer]);
  }

  void loss(const Eigen::VectorXi& labels) {
    delta = Ys.back();
    delta = (delta - delta.rowwise().maxCoeff().replicate(1, delta.cols())).array().exp().matrix();
    delta = delta.cwiseQuotient(delta.rowwise().sum().replicate(1, delta.cols()));
    for(size_t i=beg_row, j=0; j<batch_size; i++, j++) {
      delta(j, labels[i]) -= 1.0;
    }
  }

  void backward(size_t layer, const Eigen::MatrixXf& Xin) {
    deactivate(Ys[layer], acts[layer]);
    delta = delta.cwiseProduct(Ys[layer]);
    dB[layer] = delta.colwise().sum();
    dW[layer] = Xin.transpose() * delta;

    if(layer > 0) {
      delta = delta * Ws[layer].transpose();
    }
  }

  void update(size_t layer) {
    Ws[layer] -= lrate*(dW[layer] + decay*Ws[layer]);
    Bs[layer] -= lrate*(dB[layer] + decay*Bs[layer]);
  }

  // Testing images # = 10000 x 784 (28 x 28)
  void validate(Eigen::MatrixXf& test_images, Eigen::VectorXi& test_labels) {
    Eigen::MatrixXf res = test_images;
    //auto t1 = std::chrono::high_resolution_clock::now();
    for(size_t i=0; i<acts.size(); i++) {
      res = res * Ws[i] + Bs[i].replicate(res.rows(), 1);
      if(acts[i] == Activation::RELU) {
        relu(res);
      }
      else if(acts[i] == Activation::SIGMOID) {
        sigmoid(res);
      }
    }
    //auto t2 = std::chrono::high_resolution_clock::now();
    //std::cout << "Infer runtime: " << time_diff(t1, t2) << " ms\n";

    size_t correct_num {0};
    for(int k=0; k<res.rows(); k++) {
      int pred ;
      res.row(k).maxCoeff(&pred);
      if(pred == test_labels[k]) {
        correct_num ++;
      }
    }
    //std::cout << "Accuracy: " << correct_num << '/' << res.rows() << '\n';
  }

  // Parameter functions ------------------------------------------------------
  auto& epoch_num(unsigned e) {
    epoch = e;
    return *this;
  }
  auto& batch(size_t b) {
    batch_size = b;
    //assert(images->rows()%batch_size == 0);
    return *this;
  }
  auto& learning_rate(float l) {
    lrate = l;
    return *this;
  }

  auto num_layers() const {
    return acts.size();
  }

  std::vector<Eigen::MatrixXf> Ys;
  std::vector<Eigen::MatrixXf> Ws;
  std::vector<Eigen::MatrixXf> Bs;
  std::vector<Eigen::MatrixXf> dW;
  std::vector<Eigen::MatrixXf> dB;

  std::vector<Activation> acts;

  // Training images # = 60000 x 784 (28 x 28)
  //Eigen::MatrixXf* images;
  //Eigen::VectorXi* labels;
  Eigen::MatrixXf delta;

  int beg_row {0};

  float lrate {0.01f};
  float decay {0.01f};

  unsigned epoch {0};
  size_t batch_size {1};
};

inline Eigen::MatrixXf IMAGES;
inline Eigen::VectorXi LABELS;
inline Eigen::MatrixXf TEST_IMAGES;
inline Eigen::VectorXi TEST_LABELS;

// A workable DNN setting:
//   BATCH = 100 (or 50~100)
//   NUM_ITERATIONS = 60000/BATCH
//   dnn.add_layer(784, 100, Activation::RELU);
//   dnn.add_layer(100, 30, Activation::RELU);
//   dnn.add_layer(30, 10, Activation::NONE);

// The total number of training data = 60000
inline constexpr size_t BATCH {60};
inline constexpr size_t NUM_ITERATIONS {60000/BATCH};
inline constexpr size_t NUM_DNNS {10};

inline void init_dnn(MNIST_DNN& dnn, float rate) {
  //dnn.batch(BATCH).learning_rate(0.001);
  //dnn.add_layer(784, 100, Activation::RELU);
  //dnn.add_layer(100, 30, Activation::RELU);
  //dnn.add_layer(30, 10, Activation::NONE);

  dnn.batch(BATCH).learning_rate(rate);
  dnn.add_layer(784, 64, Activation::RELU);
  dnn.add_layer(64, 32, Activation::RELU);
  dnn.add_layer(32, 16, Activation::RELU);
  dnn.add_layer(16, 8, Activation::RELU);
  dnn.add_layer(8, 10, Activation::NONE);
}

inline void forward_task(MNIST_DNN& D,
  Eigen::MatrixXf& mat,
  Eigen::VectorXi& vec) {

  for(size_t i=0; i<D.acts.size(); i++) {
    if(i == 0){
      D.forward(i, mat.middleRows(D.beg_row, D.batch_size));
    }
    else {
      D.forward(i, D.Ys[i-1]);
    }
  }

  D.loss(vec);
}

inline void backward_task(MNIST_DNN& D, size_t i, Eigen::MatrixXf& mat) {
  if(i > 0) {
    //D.backward(i, D.Ys[i-1].transpose());
    D.backward(i, D.Ys[i-1]);
  }
  else {
    //D.backward(i, mat.middleRows(D.beg_row, D.batch_size).transpose());
    D.backward(i, mat.middleRows(D.beg_row, D.batch_size));

    D.beg_row += D.batch_size;
    if(D.beg_row >= IMAGES.rows()) {
      D.beg_row = 0;
    }
  }
}

inline void shuffle(Eigen::MatrixXf& mat, Eigen::VectorXi& vec) {

  static thread_local std::mt19937 gen(0);

  Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> p(mat.rows());
  p.setIdentity();
  std::shuffle(p.indices().data(), p.indices().data() + p.indices().size(), gen);

  mat = p * mat;
  vec = p * vec;
}

inline void report_runtime(std::chrono::time_point<std::chrono::high_resolution_clock>& t1) {
  auto t2 = std::chrono::high_resolution_clock::now();
  std::cout << "One iteration runtime: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << " ms\n";
  t1 = std::chrono::high_resolution_clock::now();
}

inline float rand_rate() {
  return static_cast<float>(rand())/static_cast<float>(RAND_MAX);
}

void run_tbb(const unsigned, const unsigned);
void run_taskflow(const unsigned, const unsigned);
void run_omp(const unsigned, const unsigned);
void run_sequential(unsigned, unsigned);