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

#include <votca/xtp/gwbse.h>

#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <votca/xtp/aomatrix.h>
#include <votca/xtp/threecenters.h>
// #include <votca/xtp/logger.h>
#include <votca/xtp/qmpackagefactory.h>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <votca/tools/linalg.h>

using boost::format;
using namespace boost::filesystem;

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

        // +++++++++++++++++++++++++++++ //
        // MBPT MEMBER FUNCTIONS         //
        // +++++++++++++++++++++++++++++ //

        void GWBSE::BSE_qp_setup(){
            _eh_qp = ub::zero_matrix<real_gwbse>( _bse_size , _bse_size );
            BSE_Add_qp2H( _eh_qp );
            return;
        }

        void GWBSE::BSE_solve_triplets(){

            // add full QP Hamiltonian contributions to free transitions

            ub::matrix<real_gwbse> _bse=_eh_d;

            linalg_eigenvalues(  _bse, _bse_triplet_energies, _bse_triplet_coefficients, _bse_nmax);
            return;
        }

        void GWBSE::Solve_nonhermitian(ub::matrix<double>& H, ub::matrix<double>& LT) {

            // remove stuff from Cholesky and Calculated L^T,, because more efficient for mat prods
            #pragma omp parallel for
            for (unsigned i = 0; i < LT.size1(); i++) {
                for (unsigned j = i + 1; j < LT.size1(); j++) {
                    LT(i, j) = LT(j, i);
                    LT(j, i) = 0;
                }
            }

            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Removed non referenced part of Cholesky decompostion" << flush;
            // determine H = L^T(A-B)L
            ub::matrix<double> _temp = ub::prod(H, ub::trans(LT));
            H= ub::prod(LT, _temp);
            _temp.resize(0, 0);
            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Calculated H = L^T(A+B)L " << flush;

            // solve eigenvalue problem: HR_l = eps_l^2 R_l
            ub::vector<double> _eigenvalues;
            ub::matrix<double> _eigenvectors;

            linalg_eigenvalues(H, _eigenvalues, _eigenvectors, _bse_nmax);
            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Solved HR_l = eps_l^2 R_l " << flush;
            H.resize(0, 0);
            // reconstruct real eigenvalues eps_l = sqrt(eps_l^2)
            _bse_singlet_energies.resize(_bse_nmax);
            for (int _i = 0; _i < _bse_nmax; _i++) {
                _bse_singlet_energies(_i) = sqrt(_eigenvalues(_i)); // store positive energies in orbitals objects
            }

            // reconstruct real eigenvectors X_l = 1/2 [sqrt(eps_l) (L^T)^-1 + 1/sqrt(eps_l)L ] R_l
            //                               Y_l = 1/2 [sqrt(eps_l) (L^T)^-1 - 1/sqrt(eps_l)L ] R_l

            // determine inverse of L^T
            ub::matrix<double> _cholesky_transposed_invert;
            ub::matrix<double> L = ub::trans(LT);
            linalg_invert(LT, _cholesky_transposed_invert);

            int dim = L.size1();
            _bse_singlet_coefficients.resize(dim, _bse_nmax); // resonant part (_X_evec)
            _bse_singlet_coefficients_AR.resize(dim, _bse_nmax); // anti-resonant part (_Y_evec)

            for (int _i = 0; _i < _bse_nmax; _i++) {
                //real_gwbse sqrt_eval = sqrt(_eigenvalues(_i));
                double sqrt_eval = sqrt(_bse_singlet_energies(_i));
                // get l-th reduced EV
                ub::matrix<double> _reduced_evec = ub::project(_eigenvectors, ub::range(0, dim), ub::range(_i, _i + 1)); // potentially col<->row

                ub::matrix<double> _transform = 0.5 * (sqrt_eval * _cholesky_transposed_invert + 1.0 / sqrt_eval * L);
                ub::project(_bse_singlet_coefficients, ub::range(0, dim), ub::range(_i, _i + 1)) = ub::prod(_transform, _reduced_evec);
                _transform = 0.5 * (sqrt_eval * _cholesky_transposed_invert - 1.0 / sqrt_eval * L);
                ub::project(_bse_singlet_coefficients_AR, ub::range(0, dim), ub::range(_i, _i + 1)) = ub::prod(_transform, _reduced_evec);
            }
            return;
        }

      void GWBSE::BSE_solve_singlets_BTDA(){

        // For details of the method, see EPL,78(2007)12001,
        // Nuclear Physics A146(1970)449, Nuclear Physics A163(1971)257.

          // setup resonant (A) and RARC blocks (B)
          // TOCHECK: Isn't that memory overkill here? _A and _B are never needed again?
           // ub::matrix<real_gwbse> _A = _eh_d + 2.0 * _eh_x;
           // ub::matrix<real_gwbse> _B = _eh_d2 + 2.0 * _eh_x;
          ub::matrix<double> _ApB = _eh_d + _eh_d2 + 4.0 * _eh_x;
          ub::matrix<double> _AmB = _eh_d - _eh_d2;

          // calculate Cholesky decomposition of A-B = LL^T. It throws an error if not positive definite
            //(A-B) is not needed any longer and can be overwritten

          bool positive_definite=true;
          CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << " Trying Cholesky decomposition of KAA-KAB" << flush;
          try
            {
            linalg_cholesky_decompose( _AmB );
            }
            catch (const std::runtime_error& error)
            {
                positive_definite=false;
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() <<error.what()<<endl;
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "Trying Cholesky decomposition of KAA+KAB" << flush;
                linalg_cholesky_decompose( _ApB );
                _AmB = _eh_d - _eh_d2;
            }

          if(positive_definite){
              Solve_nonhermitian(_ApB,_AmB);
          }
          else{
              Solve_nonhermitian(_AmB,_ApB);
          }

          return;
      }

      void GWBSE::BSE_Add_qp2H( ub::matrix<real_gwbse>& qp ){

          #pragma omp parallel for
            for ( size_t _v1 = 0 ; _v1 < _bse_vtotal ; _v1++){
                for ( size_t _c1 = 0 ; _c1 < _bse_ctotal ; _c1++){
                    size_t _index_vc = _bse_ctotal * _v1 + _c1;
                    // diagonal
                    qp( _index_vc , _index_vc ) += _vxc(_c1 + _bse_vtotal ,_c1 + _bse_vtotal ) - _vxc(_v1,_v1);
                    // v->c
                    for ( size_t _c2 = 0 ; _c2 < _bse_ctotal ; _c2++){
                        size_t _index_vc2 = _bse_ctotal * _v1 + _c2;
                        if ( _c1 != _c2 ){
                            qp( _index_vc , _index_vc2 ) += _vxc(_c1+ _bse_vtotal ,_c2 + _bse_vtotal );
                        }
                    }

                    // c-> v
                    for ( size_t _v2 = 0 ; _v2 < _bse_vtotal ; _v2++){
                        size_t _index_vc2 = _bse_ctotal * _v2 + _c1;
                        if ( _v1 != _v2 ){
                            qp( _index_vc , _index_vc2 ) -= _vxc(_v1,_v2);
                        }
                    }
                }
            }
            return;
      }

      void GWBSE::BSE_solve_singlets(){

            ub::matrix<real_gwbse> _bse = _eh_d + 2.0 * _eh_x;

            // _bse_singlet_energies.resize(_bse_singlet_coefficients.size1());
            linalg_eigenvalues(_bse, _bse_singlet_energies, _bse_singlet_coefficients, _bse_nmax);
            return;
        }

        void GWBSE::BSE_d_setup ( TCMatrix& _Mmn){
            // gwbasis size
            size_t _gwsize = _Mmn[_homo].size1();

            // messy procedure, first get two matrices for occ and empty subbparts
            // store occs directly transposed
            ub::matrix<real_gwbse> _storage_v = ub::zero_matrix<real_gwbse>(  _bse_vtotal * _bse_vtotal , _gwsize );
            #pragma omp parallel for
            for ( size_t _v1 = 0; _v1 < _bse_vtotal; _v1++){
                const ub::matrix<real_gwbse>& Mmn = _Mmn[_v1 + _bse_vmin ];
                for ( size_t _v2 = 0; _v2 < _bse_vtotal; _v2++){
                    size_t _index_vv = _bse_vtotal * _v1 + _v2;
                    for ( size_t _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++) {
                        _storage_v( _index_vv , _i_gw ) = Mmn( _i_gw , _v2 + _bse_vmin );
                    }
                }
            }

            ub::matrix<real_gwbse> _storage_c = ub::zero_matrix<real_gwbse>( _gwsize, _bse_ctotal * _bse_ctotal );
            #pragma omp parallel for
            for ( size_t _c1 = 0; _c1 < _bse_ctotal; _c1++){
                const ub::matrix<real_gwbse>& Mmn = _Mmn[_c1 + _bse_cmin];
                for ( size_t _c2 = 0; _c2 < _bse_ctotal; _c2++){
                    size_t _index_cc = _bse_ctotal * _c1 + _c2;
                    for ( size_t _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++) {
                        _storage_c( _i_gw , _index_cc ) = Mmn( _i_gw , _c2 + _bse_cmin );
                    }
                }
            }

            if ( ! _do_bse_singlets )  _Mmn.Cleanup();

            // store elements in a vtotal^2 x ctotal^2 matrix
            // cout << "BSE_d_setup 1 [" << _storage_v.size1() << "x" << _storage_v.size2() << "]\n" << std::flush;
            ub::matrix<real_gwbse> _storage_prod = ub::prod( _storage_v , _storage_c );

            // now patch up _storage for screened interaction
            #pragma omp parallel for
            for ( size_t _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++ ){
                if (_ppm_weight(_i_gw) < 1.e-9) {
                    for ( size_t _v = 0 ; _v < (_bse_vtotal* _bse_vtotal) ; _v++){
                        _storage_v( _v , _i_gw ) = 0;
                    }
                    for ( size_t _c = 0 ; _c < (_bse_ctotal*_bse_ctotal) ; _c++){
                        _storage_c( _i_gw , _c ) =0;
                    }

                }else{
                    double _ppm_factor = sqrt( _ppm_weight( _i_gw ));
                    for ( size_t _v = 0 ; _v < (_bse_vtotal* _bse_vtotal) ; _v++){
                        _storage_v( _v , _i_gw ) = _ppm_factor * _storage_v(_v , _i_gw );
                    }
                    for ( size_t _c = 0 ; _c < (_bse_ctotal*_bse_ctotal) ; _c++){
                        _storage_c( _i_gw , _c ) = _ppm_factor * _storage_c( _i_gw , _c  );
                    }
                }
            }

            // multiply and subtract from _storage_prod

            _storage_prod -= ub::prod( _storage_v , _storage_c );

            // free storage_v and storage_c
            _storage_c.resize(0,0);
            _storage_v.resize(0,0);

            // finally resort into _eh_d
            // can be limited to upper diagonal !
            _eh_d = ub::zero_matrix<real_gwbse>( _bse_size , _bse_size );
            #pragma omp parallel for
            for ( size_t _v1 = 0 ; _v1 < _bse_vtotal ; _v1++){
                for ( size_t _v2 = 0 ; _v2 < _bse_vtotal ; _v2++){
                    size_t _index_vv = _bse_vtotal * _v1 + _v2;

                    for ( size_t _c1 = 0 ; _c1 < _bse_ctotal ; _c1++){
                        size_t _index_vc1 = _bse_ctotal * _v1 + _c1 ;

                        for ( size_t _c2 = 0 ; _c2 < _bse_ctotal ; _c2++){
                            size_t _index_vc2 = _bse_ctotal * _v2 + _c2 ;
                            size_t _index_cc  = _bse_ctotal * _c1 + _c2;

                            _eh_d( _index_vc1 , _index_vc2 ) = -_storage_prod( _index_vv , _index_cc );
                        }
                    }
                }
            }

            return;
        }

         void GWBSE::BSE_d2_setup ( TCMatrix& _Mmn){
            // gwbasis size
            size_t _gwsize = _Mmn[_homo].size1();

            // messy procedure, first get two matrices for occ and empty subbparts
            // store occs directly transposed
            ub::matrix<real_gwbse> _storage_cv = ub::zero_matrix<real_gwbse>(  _bse_vtotal * _bse_ctotal , _gwsize );
            #pragma omp parallel for
            for ( size_t _c1 = 0; _c1 < _bse_ctotal; _c1++){
                const ub::matrix<real_gwbse>& Mmn = _Mmn[_c1 + _bse_cmin ];
                for ( size_t _v2 = 0; _v2 < _bse_vtotal; _v2++){
                    size_t _index_cv = _bse_vtotal * _c1 + _v2;
                    for ( size_t _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++) {
                        _storage_cv( _index_cv , _i_gw ) = Mmn( _i_gw , _v2 + _bse_vmin );
                    }
                }
            }

            ub::matrix<real_gwbse> _storage_vc = ub::zero_matrix<real_gwbse>( _gwsize, _bse_vtotal * _bse_ctotal );
            #pragma omp parallel for
            for ( size_t _v1 = 0; _v1 < _bse_vtotal; _v1++){
                const ub::matrix<real_gwbse>& Mmn = _Mmn[_v1 + _bse_vmin];
                for ( size_t _c2 = 0; _c2 < _bse_ctotal; _c2++){
                    size_t _index_vc = _bse_ctotal * _v1 + _c2;
                    for ( size_t _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++) {
                        _storage_vc( _i_gw , _index_vc ) = Mmn( _i_gw , _c2 + _bse_cmin );
                    }
                }
            }

            if ( ! _do_bse_singlets )  _Mmn.Cleanup();

            // store elements in a vtotal^2 x ctotal^2 matrix
            ub::matrix<real_gwbse> _storage_prod = ub::prod( _storage_cv , _storage_vc );

            // now patch up _storage for screened interaction
            #pragma omp parallel for
            for ( size_t _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++ ){
                if (_ppm_weight(_i_gw) < 1.e-9) {
                    for ( size_t _v = 0 ; _v < (_bse_vtotal* _bse_ctotal) ; _v++){
                    _storage_vc(  _i_gw , _v ) =0;
                }
                for ( size_t _c = 0 ; _c < (_bse_ctotal*_bse_vtotal) ; _c++){
                    _storage_cv( _c, _i_gw  ) = 0;
                }
                }else{
                double _ppm_factor = sqrt( _ppm_weight( _i_gw ));
                for ( size_t _v = 0 ; _v < (_bse_vtotal* _bse_ctotal) ; _v++){
                    _storage_vc(  _i_gw , _v ) = _ppm_factor * _storage_vc( _i_gw , _v );
                }
                for ( size_t _c = 0 ; _c < (_bse_ctotal*_bse_vtotal) ; _c++){
                    _storage_cv( _c, _i_gw  ) = _ppm_factor * _storage_cv( _c , _i_gw  );
                }
                    }
            }

            // multiply and subtract from _storage_prod
            _storage_prod -= ub::prod( _storage_cv , _storage_vc );

            // free storage_v and storage_c
            _storage_cv.resize(0,0);
            _storage_vc.resize(0,0);
            // finally resort into _eh_d
            // can be limited to upper diagonal !
            _eh_d2 = ub::zero_matrix<real_gwbse>( _bse_size , _bse_size );
            #pragma omp parallel for
            for ( size_t _v1 = 0 ; _v1 < _bse_vtotal ; _v1++){
                for ( size_t _v2 = 0 ; _v2 < _bse_vtotal ; _v2++){
                    for ( size_t _c1 = 0 ; _c1 < _bse_ctotal ; _c1++){
                        size_t _index_v1c1 = _bse_ctotal * _v1 + _c1 ;

                        size_t _index_c1v2 =_bse_vtotal * _c1 + _v2;

                        for ( size_t _c2 = 0 ; _c2 < _bse_ctotal ; _c2++){
                            size_t _index_v2c2 = _bse_ctotal * _v2 + _c2 ;
                            size_t _index_v1c2 = _bse_ctotal * _v1 + _c2;

                            _eh_d2( _index_v1c1 , _index_v2c2 ) = -_storage_prod( _index_c1v2 , _index_v1c2 );

                        }
                    }
                }
            }
         return;
        }

        void GWBSE::BSE_x_setup( TCMatrix& _Mmn){

            /* unlike the fortran code, we store eh interaction directly in
             * a suitable matrix form instead of a four-index array
             */

            // gwbasis size
            size_t _gwsize = _Mmn[_homo].size1();

            // get a different storage for 3-center integrals we need
            //cout<< "Starting to set up "<< endl;
            ub::matrix<real_gwbse> _storage = ub::zero_matrix<real_gwbse>( _gwsize , _bse_size);
            //cout<< "Storage set up"<< endl;

            // occupied levels
            #pragma omp parallel for
            for ( size_t _v = 0; _v < _bse_vtotal ; _v++ ){
                // cout << " act threads: " << omp_get_thread_num( ) << " total threads " << omp_get_num_threads( ) << " max threads " << omp_get_max_threads( ) <<endl;
                ub::matrix<real_gwbse>& Mmn = _Mmn[_v + _bse_vmin];
                // empty levels
                for (size_t _c =0 ; _c < _bse_ctotal ; _c++ ){
                    size_t _index_vc = _bse_ctotal * _v + _c ;
                    for (size_t _i_gw = 0 ; _i_gw < _gwsize ; _i_gw++ ){
                        _storage( _i_gw, _index_vc ) = Mmn( _i_gw, _c + _bse_cmin);
                    }
                }
            }

            _Mmn.Cleanup();
            // with this storage, _eh_x is obtained by matrix multiplication
	    _eh_x = ub::prod( ub::trans( _storage ), _storage );
            return;
        }

    }

};


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_FormFactorSquared.hpp
//! \author Alex Robinson
//! \brief  The form factor squared declaration
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_FORM_FACTOR_SQUARED_HPP
#define MONTE_CARLO_FORM_FACTOR_SQUARED_HPP

// Boost Includes
#include <boost/units/quantity.hpp>

// FRENSIE Includes
#include "Utility_InverseSquareCentimeterUnit.hpp"
#include "Utility_InverseCentimeterUnit.hpp"

namespace MonteCarlo{

//! The form factor squared class
class FormFactorSquared
{

public:

  //! The form factor independent quantity type
  typedef boost::units::quantity<Utility::Units::InverseCentimeter> ArgumentQuantity;

  //! The form factor squared independent quantity type
  typedef boost::units::quantity<Utility::Units::InverseSquareCentimeter> SquaredArgumentQuantity;

  //! Default constructor
  FormFactorSquared()
  { /* ... */ }

  //! Destructor
  virtual ~FormFactorSquared()
  { /* ... */ }

  //! Evaluate the form factor squared
  virtual double evaluate( const SquaredArgumentQuantity square_argument ) const = 0;

  //! Evaluate the form factor squared
  double evaluate( const ArgumentQuantity argument ) const;

  //! Sample from the form factor squared
  virtual SquaredArgumentQuantity sample() const = 0;

  //! Sample from the form factor squared in a subrange
  virtual SquaredArgumentQuantity sampleInSubrange( const SquaredArgumentQuantity square_arg ) const = 0;

  //! Return the max form factor squared value
  virtual double getMaxValue() const = 0;

  //! Return the min form factor squared value
  virtual double getMinValue() const = 0;

  //! Return the lower bound of the square argument
  virtual SquaredArgumentQuantity getLowerBoundOfSquaredArgument() const = 0;

  //! Return the upper bound of the square argument
  virtual SquaredArgumentQuantity getUpperBoundOfSquaredArgument() const = 0;
};

// Evaluate the form factor squared
/*! \details This method is provided for convenience. Instead of passing in
 * a squared argument the original argument can be passed in and it will be
 * squared before evaluating the squared form factor.
 */
inline double FormFactorSquared::evaluate(
                                        const ArgumentQuantity argument ) const
{
  return this->evaluate( argument*argument );
}

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_FORM_FACTOR_SQUARED_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_FormFactorSquared.hpp
//---------------------------------------------------------------------------//


// Copyright 2016 by Glukhov V. O. All Rights Reserved.
//
// 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 "pooling_layer.hpp"
#include <armadillo>
#include <cmath>

namespace cnn
{
	namespace nn
	{
		void MaxPoolingLayer::Forward(std::shared_ptr<arma::Cube<double>> input)
		{
			using namespace arma;

			input_ = input;
			uword output_height = (input_->n_rows - kernel_size_.height);
			uword output_width = (input_->n_cols - kernel_size_.width);

#ifndef NDEBUG
			assert(output_height % stride_ == 0);
			assert(output_width % stride_ == 0);
			//TODO: for release build in case of working in another thread
			//TODO: we should use std::exception_ptr for thread-safety
#endif
			output_height = output_height / stride_ + 1;
			output_width = output_width / stride_ + 1;
			if (!receptiveField_) {
				receptiveField_ = std::make_shared<Cube<double>>(output_height, output_width,
				                                                input_->n_slices, fill::zeros);
			} else {
				receptiveField_->set_size(output_height, output_width, input_->n_slices);
				receptiveField_->zeros();
			}

			connectIndexes_.set_size(input_->n_rows, input_->n_cols, input_->n_slices);
			connectIndexes_.zeros();
			double maxVal;
			uword rowIdx, colIdx;

			for (uword d = 0; d < input_->n_slices; ++d) {
				uword out_col = 0;
				for (uword c = 0; c < input_->n_cols; c += stride_) {
					uword out_row = 0;
					for (uword r = 0; r < input_->n_rows; r += stride_) {
						maxVal = input_->slice(d)(span(r, r + kernel_size_.height - 1),
						                          span(c, c + kernel_size_.height - 1)).max(rowIdx, colIdx);
						(*receptiveField_)(out_row, out_col, d) = maxVal;
						connectIndexes_(r + rowIdx, c + colIdx, d) = 1;
						++out_row;
					}
					++out_col;
				}
			}

			// currently common to use the activation function after convolution layer
			// instead of a subsample layer
			if (!activFunc_) {
				output_ = receptiveField_;
			} else {
				if (!output_) {
					output_ = std::make_shared<Cube<double>>(output_height, output_width,
					                                        input_->n_slices);
				} else {
					output_->set_size(output_height, output_width, input_->n_slices);
				}
				activFunc_->Compute(receptiveField_, output_);
			}
		}

		std::pair<tensor4d, tensor4d> MaxPoolingLayer::Backward(
			const std::shared_ptr<arma::Cube<double>>& prevLocalLoss)
		{
			using namespace arma;
#ifndef NDEBUG
			assert(prevLocalLoss);
#endif
			// top layer was 1d. we need reshape error to 3d
			if (prevLocalLoss->n_slices == 1 && prevLocalLoss->n_cols == 1) {
				(*prevLocalLoss) = unvectorise(prevLocalLoss->get_ref(), output_->n_rows,
				                               output_->n_cols, output_->n_slices);
			}

			if (!localLoss_) {
				localLoss_ = std::make_shared<Cube<double>>(
					input_->n_rows, input_->n_cols, input_->n_slices, fill::zeros);
			} else {
				localLoss_->set_size(input_->n_rows, input_->n_cols, input_->n_slices);
				localLoss_->zeros();
			}
			uword rowIdx, colIdx;
			for (uword d = 0; d < input_->n_slices; ++d) {
				uword lossCol = 0;
				for (uword c = 0; c < input_->n_cols; c += stride_) {
					uword lossRow = 0;
					for (uword r = 0; r < input_->n_rows; r += stride_) {
						// from top to bottom propagates only connected losses
						connectIndexes_.slice(d)(span(r, r + kernel_size_.height - 1),
						                         span(c, c + kernel_size_.height - 1)
						               ).max(rowIdx, colIdx);
						(*localLoss_)(r + rowIdx, c + colIdx, d) = (*prevLocalLoss)(
							lossRow, lossCol, d);
						++lossRow;
					}
					++lossCol;
				}
			}

			if (activFunc_) {
				localLoss_->transform([&] (double value) {
					if (!value) {
						return value;
					} else
						return activFunc_->Derivative(value);
				});
			}

			// pool layer doesn't has weights
			return
					std::make_pair(tensor4d(), tensor4d());
		}

		std::pair<tensor4d, tensor4d> MaxPoolingLayer::Backward2nd(
			const std::shared_ptr<arma::Cube<double>>& prevLocalLoss)
		{
			using namespace arma;
#ifndef NDEBUG
			assert(prevLocalLoss);
#endif
			// top layer was 1d. we need reshape error to 3d
			if (prevLocalLoss->n_slices == 1 && prevLocalLoss->n_cols == 1) {
				(*prevLocalLoss) = unvectorise(prevLocalLoss->get_ref(), output_->n_rows,
											   output_->n_cols, output_->n_slices);
			}

			if (!localLoss_) {
				localLoss_ = std::make_shared<Cube<double>>(
					input_->n_rows, input_->n_cols, input_->n_slices, fill::zeros);
			} else {
				localLoss_->set_size(input_->n_rows, input_->n_cols, input_->n_slices);
				localLoss_->zeros();
			}
			uword rowIdx, colIdx;
			for (uword d = 0; d < input_->n_slices; ++d) {
				uword lossCol = 0;
				for (uword c = 0; c < input_->n_cols; c += stride_) {
					uword lossRow = 0;
					for (uword r = 0; r < input_->n_rows; r += stride_) {
						// from top to bottom propagates only connected losses
						connectIndexes_.slice(d)(span(r, r + kernel_size_.height - 1),
												 span(c, c + kernel_size_.height - 1)
												 ).max(rowIdx, colIdx);
						(*localLoss_)(r + rowIdx, c + colIdx, d) = (*prevLocalLoss)(
							lossRow, lossCol, d);
						++lossRow;
					}
					++lossCol;
				}
			}

			if (activFunc_) {
				localLoss_->transform([&](double value) {
					if (!value) {
						return value;
					} else
						return std::pow(activFunc_->Derivative(value), 2);
				});
			}

			// pool layer doesn't has weights
			return std::make_pair(tensor4d(), tensor4d());
		}
	}
}


/******************************************************************************

  Point-to-point implementation of the ICP registration algorithm for 3D point
  clouds.

  Copyright (c) 2012
  Alexander Rukletsov <rukletsov@gmail.com>
  Dzmitry Hlindzich <hlindzich@gmail.com>
  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.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.

*******************************************************************************/

#ifndef ICP_3D_HPP_507CF525_CC8A_4499_80D2_E8C8644603F5_
#define ICP_3D_HPP_507CF525_CC8A_4499_80D2_E8C8644603F5_

#include <vector>
#include <utility>
#include <boost/array.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/assert.hpp>

#include "bo/core/vector.hpp"
#include "bo/core/kdtree.hpp"
#include "bo/math/mean.hpp"
#include "bo/math/transformation_3d.hpp"
#include "bo/math/blas_extensions.hpp"
#include "bo/math/blas_conversions.hpp"

/* Usage example:

    std::vector<Vertex> std_source();
    ... // Initialize the source point cloud here.

    boost::shared_ptr<std::vector<Vertex> > std_target_ptr = boost::shared_ptr<std::vector<Vertex> >
    (new std::vector<Vertex>());
    ... // Initialize the target point cloud here.

    // Create an ICP registrator.
    bo::registration::ICP3D<float> icp(std_source, std_target_ptr,
    bo::distances::euclidean_distance<float, 3>);

    // Initialize auxiliary variables.
    // Iteration counter.
    std::size_t iteration = 0;
    // Norm of the transformation difference.
    float epsilon = std::numeric_limits<float>::max();
    // Distance between the source and target point clouds.
    float distance = std::numeric_limits<float>::max();
    // Transformation matrices.
    bo::math::matrix<float> m1;
    bo::math::matrix<float> m2;

    // Iteratively register two point clouds.
    while (iteration < max_allowed_iterations && epsilon >= min_transformation_epsilon)
    {
        // Cache the transformation from the previous iteration.
        m1 = m2;

        // Perform a new ICP registration step and update the current distance
        // between the source and target.
        distance = icp.next();

        // Update the current transformation.
        m2 = icp.current_transformation().matrix();

        // Update the norm of the transformation difference.
        if (iteration > 0)
        {
            bo::math::matrix<float> mdif = m2 - m1;
            epsilon = bo::math::l1_norm(mdif);
        }

        ++iteration;
    }

    // The final transformation.
    bo::Transformation3D<float> final_transformation = icp.current_transformation();

    // The corresponding final transformation matrix.
    bo::math::matrix<float> final_matrix = final_transformation.matrix();
*/

namespace bo {
namespace registration {

template <typename RealType>
class ICP3D: public boost::noncopyable
{
public:
    typedef Vector<RealType, 3> Point3D;
    typedef std::vector<Point3D> PointCloud;
    typedef boost::shared_ptr<PointCloud> PointCloudPtr;
    typedef boost::function<RealType (Point3D, Point3D)> Metric;
    typedef math::Transformation3D<RealType> Transformation;

public:
    ICP3D(const PointCloud& source, PointCloudPtr target, Metric dist_fun,
          bool is_preprocess = true);

    RealType next();
    Transformation current_transformation() const;
    const PointCloud& current_cloud() const;
    const PointCloud& current_correspondence() const;

private:

    typedef bo::KDTree<3, Point3D, std::pointer_to_binary_function
        <const Point3D&, std::size_t, RealType> > Point3DTree;
    static RealType point_bac(const Point3D& p, std::size_t k);

    void overlay_();
    Point3D centroid_(PointCloud* cloud) const;
    // Attention: the elements cloud2[i] must correspond to the elements cloud1[i].
    math::matrix<RealType> cross_covariance_(PointCloud* cloud1, PointCloud* cloud2,
        const Point3D& centroid1, const Point3D& centroid2) const;
    // Attention: the elements cloud2[i] must correspond to the elements cloud1[i].
    RealType distance_(PointCloud* cloud1, PointCloud* cloud2) const;
    void update_current_transform_and_cloud_(const Transformation& m);

private:
    PointCloudPtr target_cloud_;
    PointCloud current_cloud_;
    PointCloud corresp_cloud_;
    Transformation current_trans_;
    Point3DTree tree_;
    Metric dist_fun_;
};

// Includes computing a kd-tree for the target point cloud.
template <typename RealType>
ICP3D<RealType>::ICP3D(const PointCloud& source, PointCloudPtr target,
    Metric dist_fun, bool is_preprocess):
    current_cloud_(source), target_cloud_(target), dist_fun_(dist_fun),
    current_trans_(),
    tree_(target->begin(), target->end(), std::ptr_fun(ICP3D<RealType>::point_bac))
{
    if (is_preprocess)
        overlay_();
}

template <typename RealType>
RealType ICP3D<RealType>::next()
{
    // Calculate the mass center of the current point cloud.
    Point3D current_centroid = centroid_(&current_cloud_);

    // Update the correspondence.
    corresp_cloud_.clear();
    for (typename PointCloud::const_iterator it = current_cloud_.begin(); it != current_cloud_.end(); ++it)
    {
        std::pair<typename Point3DTree::const_iterator, RealType> closest = tree_.find_nearest(*it);
        corresp_cloud_.push_back(*closest.first);
    }

    // Calculate the mass center of the corresponding points.
    Point3D corresp_centroid = centroid_(&corresp_cloud_);

    // Calculate the cross covariance for the current points and the target ones.
    math::matrix<RealType> Spx = cross_covariance_(&current_cloud_, &corresp_cloud_, current_centroid,
        corresp_centroid);

    math::matrix<RealType> SpxT = trans(Spx);

    // Create the asymmetrical matrix.
    math::matrix<RealType> Apx = Spx - SpxT;

    // Calculate the matrix trace.
    RealType traceSpx = Spx(0, 0) + Spx(1, 1) + Spx(2, 2);

    math::matrix<RealType> Bpx = Spx + SpxT -
        traceSpx * math::identity_matrix<RealType>(3);

    // Create the 4x4 matrix.
    math::matrix<RealType> Qpx(4, 4);

    // Fill in the matrix.
    // Block 1.
    Qpx(0, 0) = traceSpx;
    // Block 2.
    Qpx(0, 1) = Apx(1, 2);
    Qpx(0, 2) = Apx(2, 0);
    Qpx(0, 3) = Apx(0, 1);
    // Block 3.
    Qpx(1, 0) = Apx(1, 2);
    Qpx(2, 0) = Apx(2, 0);
    Qpx(3, 0) = Apx(0, 1);
    // Block 4.
    math::subrange(Qpx, 1, 4, 1, 4) = math::subrange(Bpx, 0, 3, 0, 3);

    math::eigen_symmetric(Qpx);

    // Quaternion that defines the optimal rotation.
//    Vector<RealType, 4> quaternion = math::to_bo_vector(math::column(Qpx, 3));
    math::bounded_vector<RealType, 4> col(math::column(Qpx, 3));
    Vector<RealType, 4> quaternion = math::to_bo_vector(col);

    // Optimal translation.
    Point3D translation = corresp_centroid - Transformation(quaternion) * current_centroid;

    // Create the optimal transformation.
    Transformation optimal_trans(quaternion, translation);

    // Update the current point cloud and the transformation.
    update_current_transform_and_cloud_(optimal_trans);

    return distance_(&current_cloud_, &corresp_cloud_);
}

template <typename RealType>
typename ICP3D<RealType>::Transformation ICP3D<RealType>::current_transformation() const
{
    return current_trans_;
}

template <typename RealType>
const typename ICP3D<RealType>::PointCloud& ICP3D<RealType>::current_cloud() const
{
    return current_cloud_;
}

template <typename RealType>
const typename ICP3D<RealType>::PointCloud& ICP3D<RealType>::current_correspondence() const
{
    return corresp_cloud_;
}

template <typename RealType>
void ICP3D<RealType>::overlay_()
{
    // Cloud shift using centroids.

    Point3D translation = centroid_(target_cloud_.get()) - centroid_(&current_cloud_);

    Transformation shift_transform(translation);

    // Update the current point cloud and the transformation.
    update_current_transform_and_cloud_(shift_transform);
}

template <typename RealType>
typename ICP3D<RealType>::Point3D ICP3D<RealType>::centroid_(PointCloud* cloud) const
{
    BOOST_ASSERT(cloud->size() > 0);
    Point3D mass_center = math::mean(*cloud);

    return mass_center;
}

template <typename RealType>
math::matrix<RealType> ICP3D<RealType>::cross_covariance_(PointCloud* cloud1, PointCloud* cloud2,
    const Point3D& centroid1, const Point3D& centroid2) const
{
    const std::size_t n = cloud1->size();

    BOOST_ASSERT(cloud2->size() == n && n > 0);

    math::matrix<RealType> m = math::zero_matrix<RealType>(3, 3);
    math::matrix<RealType> p(3, 1);
    math::matrix<RealType> x(1, 3);

    for (std::size_t i = 0; i < n; ++i)
    {
        Point3D a = cloud1->at(i);
        Point3D b = cloud2->at(i);

        p(0, 0) = a[0] - centroid1[0];
        p(1, 0) = a[1] - centroid1[1];
        p(2, 0) = a[2] - centroid1[2];

        x(0, 0) = b[0] - centroid2[0];
        x(0, 1) = b[1] - centroid2[1];
        x(0, 2) = b[2] - centroid2[2];

        // Accumulate the elements.
        // Warning: type overflow is possible here!
        m = m + math::prod(p, x);
    }

    m = m / n;

    return m;
}

template <typename RealType>
RealType ICP3D<RealType>::distance_(PointCloud* cloud1, PointCloud* cloud2) const
{
    const std::size_t n = cloud1->size();

    BOOST_ASSERT(cloud2->size() == n);

    RealType sum(0);

    for (std::size_t i = 0; i < n; ++i)
    {
        Point3D a = cloud1->at(i);
        Point3D b = cloud2->at(i);
        sum += dist_fun_(a, b);
    }

    return sum;
}

// Point3D brackets accessor.
template <typename RealType>
inline RealType ICP3D<RealType>::point_bac(const Point3D& p, std::size_t k)
{
    return p[k];
}

template <typename RealType>
void ICP3D<RealType>::update_current_transform_and_cloud_(const Transformation& m)
{
    // Update the current transformation.
    current_trans_ = m * current_trans_;

    // Update the current point cloud.
    for (typename PointCloud::iterator it = current_cloud_.begin(); it != current_cloud_.end(); ++it)
    {
        *it = m * (*it);
    }
}

} // namespace registration
} // namespace bo

#endif // ICP_3D_HPP_507CF525_CC8A_4499_80D2_E8C8644603F5_


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__OCP_TO_QP_HPP_
#define SMOOTH__FEEDBACK__OCP_TO_QP_HPP_

/**
 * @file
 * @brief Formulate optimal control problem as a quadratic program
 */

#include <Eigen/Core>
#include <smooth/diff.hpp>

#include "collocation/mesh.hpp"
#include "collocation/mesh_function.hpp"
#include "ocp.hpp"
#include "qp.hpp"

namespace smooth::feedback {

// \cond
namespace detail {

/**
 * @brief Working memory for ocp_to_qp
 */
struct OcpToQpWorkmemory
{
  MeshValue<1> cr_out;   /// @brief output of mesh_eval
  MeshValue<2> int_out;  /// @brief output of mesh_integrate
};

/**
 * @brief Allocate a qp for ocp_to_qp_update()
 *
 * @param[out] qp quadratic program to allocate
 * @param[out] work working memory to allocate
 * @param[in] ocp input problem
 * @param[in] mesh time discretization
 */
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_allocate(
  QuadraticProgramSparse<double> & qp,
  OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;

  using X = typename ocp_t::X;
  using U = typename ocp_t::U;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  // [x0 x1 ... xN u0 u1 ... uN-1]

  const auto N = mesh.N_colloc();

  const auto xvar_L = Nx * (N + 1);
  const auto uvar_L = Nu * N;

  const auto dcon_L  = Nx * N;
  const auto crcon_L = ocp_t::Ncr * N;
  const auto cecon_L = ocp_t::Nce;

  const auto dcon_B  = 0u;
  const auto crcon_B = dcon_L;
  const auto cecon_B = crcon_B + crcon_L;

  const auto Nvar = xvar_L + uvar_L;
  const auto Ncon = dcon_L + crcon_L + cecon_L;

  // resize qp
  qp.P.resize(Nvar, Nvar);
  qp.q.setZero(Nvar);
  qp.A.resize(Ncon, Nvar);
  qp.l.setZero(Ncon);
  qp.u.setZero(Ncon);

  // sparsity pattern of A (row-major)
  Eigen::VectorXi A_pattern = Eigen::VectorXi::Zero(Ncon);
  for (auto ival = 0u, I0 = 0u; ival < mesh.N_ivals(); I0 += mesh.N_colloc_ival(ival), ++ival) {
    const auto Ki = mesh.N_colloc_ival(ival);  // number of nodes in interval
    A_pattern.segment(dcon_B + I0 * Nx, Ki * Nx) +=
      Eigen::VectorXi::Constant(Ki * Nx, Nx + Ki + Nu);
  }
  A_pattern.segment(crcon_B, crcon_L).setConstant(Nx + Nu);
  A_pattern.segment(cecon_B, cecon_L).setConstant(2 * Nx);
  qp.A.reserve(A_pattern);

  // sparsity pattern of P
  Eigen::VectorXi P_pattern = Eigen::VectorXi::Zero(Nvar);
  for (auto i = 0u; i < N; ++i) {
    for (auto j = 0u; j < Nx; ++j) { P_pattern(Nx * i + j) = j + 1; }
  }
  for (auto j = 0u; j < Nx; ++j) { P_pattern(Nx * N + j) = Nx + j + 1; }
  for (auto i = 0u; i < N; ++i) {
    for (auto j = 0u; j < Nu; ++j) { P_pattern(Nx * (N + 1) + Nu * i + j) = Nx + (j + 1); }
  }
  qp.P.reserve(P_pattern);

  // compute work stuff once to allocate pattern
  const double tf = 1.;
  auto xslin      = mesh.all_nodes() | transform([&](double) { return Identity<X>(); });
  auto uslin      = mesh.all_nodes() | transform([&](double) { return Identity<U>(); });

  work.int_out.lambda.setConstant(1, 1);
  mesh_eval<1, DT>(work.cr_out, mesh, ocp.cr, 0, tf, xslin, uslin);       // allocates work.cr_out
  mesh_integrate<2, DT>(work.int_out, mesh, ocp.g, 0, tf, xslin, uslin);  // allocates work.int_out

  work.cr_out.dF.makeCompressed();
  work.int_out.dF.makeCompressed();
  work.int_out.d2F.makeCompressed();
}

/// @brief ocp_to_qp_update: cost part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_cost(
  QuadraticProgramSparse<double> & qp,
  OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;
  static constexpr auto Nq = ocp_t::Nq;
  const double t0          = 0.;

  static_assert(ocp_t::Nq == 1, "exactly one integral supported in ocp_to_qp");

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N      = mesh.N_colloc();
  const auto xvar_L = Nx * (N + 1);
  const auto uvar_L = Nu * N;
  const auto xvar_B = 0u;
  const auto uvar_B = xvar_L;

  ////////////////////////
  //// ZERO VARIABLES ////
  ////////////////////////

  set_zero(qp.P);
  qp.q.setZero();

  //////////////////////////////
  //// LINEARIZATION POINTS ////
  //////////////////////////////

  const X xl0 = xl_fun(0.);
  const X xlf = xl_fun(tf);

  auto xslin = mesh.all_nodes() | transform([&](double t) { return xl_fun(t0 + (tf - t0) * t); });
  auto uslin = mesh.all_nodes() | transform([&](double t) { return ul_fun(t0 + (tf - t0) * t); });

  const Eigen::Vector<double, 1> ql{1.};

  ///////////////////////
  //// INTEGRAL COST ////
  ///////////////////////

  const auto & [th, dth, d2th] = diff::dr<2, DT>(ocp.theta, wrt(tf, xl0, xlf, ql));

  const Eigen::Vector<double, Nx> qo_x0 = dth.middleCols(1, Nx).transpose();
  const Eigen::Vector<double, Nx> qo_xf = dth.middleCols(1 + Nx, Nx).transpose();
  const Eigen::Vector<double, Nq> qo_q  = dth.middleCols(1 + 2 * Nx, Nq).transpose();

  mesh_integrate<2, DT>(work.int_out, mesh, ocp.g, 0, tf, xslin, uslin);

  // clang-format off
  block_add(qp.P, 0, 0, work.int_out.d2F.block(2, 2, xvar_L + uvar_L, xvar_L + uvar_L), qo_q.x(), true);

  qp.q.segment(xvar_B, xvar_L) = qo_q.x() * work.int_out.dF.middleCols(2, xvar_L).transpose();
  qp.q.segment(uvar_B, uvar_L) = qo_q.x() * work.int_out.dF.middleCols(2 + xvar_L, uvar_L).transpose();
  // clang-format on

  ///////////////////////
  //// ENDPOINT COST ////
  ///////////////////////

  block_add(qp.P, 0, 0, d2th.block(1, 1, Nx, Nx), 0.5, true);                      // d2q / dx0x0
  block_add(qp.P, 0, Nx * N, d2th.block(1, 1 + Nx, Nx, Nx), 0.5, true);            // d2q / dx0xf
  block_add(qp.P, Nx * N, Nx * N, d2th.block(1 + Nx, 1 + Nx, Nx, Nx), 0.5, true);  // d2q / dxfxf

  qp.q.segment(0, Nx) += qo_x0;       // dq / dx0
  qp.q.segment(Nx * N, Nx) += qo_xf;  // dq / dxf
}

/// @brief ocp_to_qp_update: dyn part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_dyn(
  QuadraticProgramSparse<double> & qp,
  [[maybe_unused]] OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using utils::zip;
  using namespace std::views;
  using ocp_t = typename std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;
  const double t0          = 0.;

  static_assert(ocp_t::Nq == 1, "exactly one integral supported in ocp_to_qp");

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N      = mesh.N_colloc();
  const auto xvar_L = Nx * (N + 1);
  const auto dcon_L = Nx * N;
  const auto xvar_B = 0u;
  const auto uvar_B = xvar_L;
  const auto dcon_B = 0u;

  ////////////////////////
  //// ZERO VARIABLES ////
  ////////////////////////

  set_zero(qp.A.middleRows(dcon_B, dcon_L));

  /////////////////////////////////
  //// COLLOCATION CONSTRAINTS ////
  /////////////////////////////////

  for (auto ival = 0u, M = 0u; ival < mesh.N_ivals(); M += mesh.N_colloc_ival(ival), ++ival) {
    const auto Ki = mesh.N_colloc_ival(ival);  // number of nodes in interval

    const auto [alpha, Dus] = mesh.interval_diffmat_unscaled(ival);

    // in each interval the collocation constraint is
    // [A0 x0 ... Ak-1 xk-1 0]  + [B0 u0 ... Bk-1 uk-1] + [E0 ... Ek-1] = alpha * X Dus

    for (const auto & [i, tau_i] : zip(iota(0u, Ki), mesh.interval_nodes(ival))) {
      const auto t_i             = t0 + (tf - t0) * tau_i;             // unscaled time
      const auto & [xl_i, dxl_i] = diff::dr<1, DT>(xl_fun, wrt(t_i));  // x-lin
      const auto ul_i            = ul_fun(t_i);                        // u-lin

      // linearize dynamics and insert new constraint A xi + B ui + E = [x0 ... XNi] di

      const auto & [f_i, df_i] = diff::dr<1, DT>(ocp.f, wrt(t_i, xl_i, ul_i));

      // clang-format off
      block_add(qp.A, dcon_B + (M + i) * Nx, xvar_B + (M + i) * Nx, df_i.template middleCols<Nx>(1), tf);        // A
      block_add(qp.A, dcon_B + (M + i) * Nx, uvar_B + (M + i) * Nu, df_i.template middleCols<Nu>(1 + Nx), tf);   // B
      // clang-format on

      if constexpr (!IsCommutative<X>) {
        block_add(qp.A, dcon_B + (M + i) * Nx, xvar_B + (M + i) * Nx, ad<X>(f_i + dxl_i), -tf / 2);
      }

      for (auto j = 0u; j < Ki + 1; ++j) {
        for (auto diag = 0u; diag < Nx; ++diag) {
          qp.A.coeffRef(dcon_B + (M + i) * Nx + diag, (M + j) * Nx + diag) -= alpha * Dus(j, i);
        }
      }

      qp.l.segment(dcon_B + (M + i) * Nx, Nx) = -tf * (f_i - dxl_i);
      qp.u.segment(dcon_B + (M + i) * Nx, Nx) = qp.l.segment(dcon_B + (M + i) * Nx, Nx);
    }
  }
}

/// @brief ocp_to_qp_update: running constraints part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_cr(
  QuadraticProgramSparse<double> & qp,
  OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;

  const double t0 = 0.;

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N       = mesh.N_colloc();
  const auto xvar_L  = Nx * (N + 1);
  const auto uvar_L  = Nu * N;
  const auto dcon_L  = Nx * N;
  const auto crcon_L = ocp_t::Ncr * N;
  const auto crcon_B = dcon_L;

  //////////////////////////////
  //// LINEARIZATION POINTS ////
  //////////////////////////////

  auto xslin = mesh.all_nodes() | transform([&](double t) { return xl_fun(t0 + (tf - t0) * t); });
  auto uslin = mesh.all_nodes() | transform([&](double t) { return ul_fun(t0 + (tf - t0) * t); });

  /////////////////////////////
  //// RUNNING CONSTRAINTS ////
  /////////////////////////////

  mesh_eval<1, DT>(work.cr_out, mesh, ocp.cr, 0, tf, xslin, uslin);

  block_write(qp.A, crcon_B, 0, work.cr_out.dF.middleCols(2, xvar_L + uvar_L));
  qp.l.segment(crcon_B, crcon_L) = ocp.crl.replicate(N, 1) - work.cr_out.F;
  qp.u.segment(crcon_B, crcon_L) = ocp.cru.replicate(N, 1) - work.cr_out.F;
}

/// @brief ocp_to_qp_update: end constraints part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_ce(
  QuadraticProgramSparse<double> & qp,
  [[maybe_unused]] OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  [[maybe_unused]] auto && ul_fun)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;

  static constexpr auto Nx = ocp_t::Nx;

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N       = mesh.N_colloc();
  const auto xvar_L  = Nx * (N + 1);
  const auto xvar_B  = 0u;
  const auto dcon_L  = Nx * N;
  const auto crcon_L = ocp_t::Ncr * N;
  const auto cecon_L = ocp_t::Nce;
  const auto crcon_B = dcon_L;
  const auto cecon_B = crcon_B + crcon_L;

  //////////////////////////////
  //// LINEARIZATION POINTS ////
  //////////////////////////////

  const X xl0 = xl_fun(0.);
  const X xlf = xl_fun(tf);

  //////////////////////////////
  //// ENDPOINT CONSTRAINTS ////
  //////////////////////////////

  const Eigen::Vector<double, 1> ql{1.};
  const auto & [ceval, dceval] = diff::dr<1, DT>(ocp.ce, wrt(tf, xl0, xlf, ql));

  block_write(qp.A, cecon_B, xvar_B, dceval.middleCols(1, Nx));                     // dce / dx0
  block_write(qp.A, cecon_B, xvar_B + xvar_L - Nx, dceval.middleCols(1 + Nx, Nx));  // dce / dxf

  qp.l.segment(cecon_B, cecon_L) = ocp.cel - ceval;
  qp.u.segment(cecon_B, cecon_L) = ocp.ceu - ceval;
}

/**
 * @brief Update a qp for ocp_to_qp_update()
 *
 * @param[out] qp quadratic program
 * @param[in, out] work
 * @param[in] ocp input problem
 * @param[in] mesh time discretization
 * @param[in] tf time horizon
 * @param[in] xl_fun state linearization (must be differentiable w.r.t. time)
 * @param[in] ul_fun input linearization
 */
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update(
  QuadraticProgramSparse<double> & qp,
  [[maybe_unused]] OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  const auto & xl_fun,
  [[maybe_unused]] const auto & ul_fun)
{
  ocp_to_qp_update_cost<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
  ocp_to_qp_update_dyn<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
  ocp_to_qp_update_cr<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
  ocp_to_qp_update_ce<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
}

}  // namespace detail
// \endcond

/**
 * @brief Formulate an optimal control problem as a quadratic program via linearization.
 *
 * @param ocp input problem
 * @param mesh time discretization
 * @param tf time horizon
 * @param xl_fun state linearization (must be differentiable w.r.t. time)
 * @param ul_fun input linearization
 *
 * @return sparse quadratic program for a flattened formulation of ocp.
 *
 * @note allocates memory for each call. To reduce memory allocation for repeated calls, see
 * ocp_to_qp_allocate() and ocp_to_qp_update().
 *
 * @see qpsol_to_ocpsol()
 */
template<diff::Type DT = diff::Type::Default>
QuadraticProgramSparse<double> ocp_to_qp(
  const OCPType auto & ocp, const MeshType auto & mesh, double tf, auto && xl_fun, auto && ul_fun)
{
  QuadraticProgramSparse<double> qp;
  detail::OcpToQpWorkmemory work;

  detail::ocp_to_qp_allocate<DT>(qp, work, ocp, mesh);
  detail::ocp_to_qp_update<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);

  qp.A.makeCompressed();
  qp.P.makeCompressed();

  return qp;
}

/**
 * @brief Convert QP solution to OCP solution
 *
 * If qp_sol solves a QP obtained via ocp_to_qp(), then qpsol_to_ocpsol(qp_sol)
 * is the corrensponding OCP solution.
 *
 * @param ocp optimal control problem
 * @param mesh discretization mesh
 * @param qpsol solution to quadratic program obtained via ocp_to_qp()
 * @param tf final time used in ocp_to_qp()
 * @param xl_fun state linearization trajectory used in ocp_to_qp()
 * @param ul_fun input linearization trajectory used in ocp_to_qp()
 *
 * @see ocp_to_qp()
 */
auto qpsol_to_ocpsol(
  const OCPType auto & ocp,
  const MeshType auto & mesh,
  const QPSolution<-1, -1, double> & qpsol,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using ocp_t = std::decay_t<decltype(ocp)>;

  using X = typename ocp_t::X;
  using U = typename ocp_t::U;

  static constexpr int Nx  = ocp_t::Nx;
  static constexpr int Nu  = ocp_t::Nu;
  static constexpr int Nq  = ocp_t::Nq;
  static constexpr int Ncr = ocp_t::Ncr;
  static constexpr int Nce = ocp_t::Nce;

  const auto N = mesh.N_colloc();

  const auto xvar_L = Nx * (N + 1);
  const auto uvar_L = Nu * N;

  const auto xvar_B    = 0u;
  const auto uvar_B    = xvar_L;
  Eigen::MatrixXd Xmat = qpsol.primal.segment(xvar_B, xvar_L).reshaped(Nx, N + 1);
  Eigen::MatrixXd Umat = qpsol.primal.segment(uvar_B, uvar_L).reshaped(Nu, N);

  auto xfun = [t0     = 0.,
               tf     = tf,
               mesh   = mesh,
               Xmat   = std::move(Xmat),
               xl_fun = std::forward<decltype(xl_fun)>(xl_fun)](double t) -> X {
    const auto tngnt =
      mesh.template eval<Eigen::Vector<double, Nx>>((t - t0) / (tf - t0), Xmat.colwise(), 0, true);
    return rplus(xl_fun(t), tngnt);
  };

  auto ufun = [t0     = 0.,
               tf     = tf,
               mesh   = mesh,
               Umat   = std::move(Umat),
               ul_fun = std::forward<decltype(ul_fun)>(ul_fun)](double t) -> U {
    const auto tngnt =
      mesh.template eval<Eigen::Vector<double, Nu>>((t - t0) / (tf - t0), Umat.colwise(), 0, false);
    return rplus(ul_fun(t), tngnt);
  };

  return OCPSolution<X, U, Nq, Nce, Ncr>{
    .t0 = 0.,
    .tf = tf,
    .u  = std::move(ufun),
    .x  = std::move(xfun),
  };
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__OCP_TO_QP_HPP_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_FACTORIZATIONS_CHOL_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_FACTORIZATIONS_CHOL_HPP_INCLUDED

#include <nt2/linalg/functions/lu.hpp>
#include <nt2/include/functions/qr.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/potrf.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/tril.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  //CHOL Scalar
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      BOOST_ASSERT_MSG(is_gtz(a0), "Matrix must be positive definite");
      return  nt2::sqrt(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef typename nt2::meta::as_real<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      BOOST_ASSERT_MSG(is_gtz(a0), "Matrix must be positive definite");
      return nt2::sqrt(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                              (unspecified_<A2>)
                            )
  {
    typedef typename nt2::meta::as_real<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&, const A2&) const
    {
      BOOST_ASSERT_MSG(is_gtz(a0), "Matrix must be positive definite");
      return nt2::sqrt(a0);
    }
  };
  //============================================================================
  //Cholesky factorization
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::chol_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type child0;
    typedef typename child0::value_type                                  type_t;
    typedef nt2::memory::container<tag::table_,  type_t, nt2::_2D>   o_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - R = chol(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0 ,  A1& a1
              , boost::mpl::long_<1> const& , boost::mpl::long_<1> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY(nt2::potrf(boost::proto::value(a),'U'));
      boost::proto::child_c<0>(a1) = nt2::triu(a);
    }

    //==========================================================================
    /// INTERNAL ONLY - L = chol(A,lower_/upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<1> const&
              ) const
    {
      eval( a0, a1, N0(), N1()
          , boost::proto::value(boost::proto::child_c<1>(a0))
          );
    }

    //==========================================================================
    /// INTERNAL ONLY - L = chol(A,lower_)
    BOOST_FORCEINLINE
    void eval( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<1> const&
              , nt2::policy<ext::lower_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY(nt2::potrf(boost::proto::value(a),'L'));
      boost::proto::child_c<0>(a1) = nt2::tril(a);
    }

    //==========================================================================
    /// INTERNAL ONLY - R = chol(A,upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<1> const&
              , nt2::policy<ext::upper_> const&
              ) const
    {
      eval(a0,a1,boost::mpl::long_<1>(),N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - [R,P] = chol(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<1> const& , boost::mpl::long_<2> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'U');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      if (info == 0)
        boost::proto::child_c<0>(a1) = nt2::triu(a);
      else
        boost::proto::child_c<0>(a1) = nt2::triu(a(nt2::_(1, info-1), nt2::_(1, info-1)));
      boost::proto::child_c<1>(a1) = info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [R,P] = chol(A,lower_/upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              ) const
    {
      eval( a0, a1, N0(), N1()
          , boost::proto::value(boost::proto::child_c<1>(a0))
          );
    }

    //==========================================================================
    /// INTERNAL ONLY - L = chol(A, raw_,lower_/upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              ) const
    {
      eval( a0, a1, N0(), N1()
          , boost::proto::value(boost::proto::child_c<1>(a0))
          , boost::proto::value(boost::proto::child_c<2>(a0))
          );
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,lower_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::lower_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'L');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      if (info == 0)
        boost::proto::child_c<0>(a1) = nt2::tril(a);
      else
        boost::proto::child_c<0>(a1) = nt2::tril(a(nt2::_(1, info-1), nt2::_(1, info-1)));
      boost::proto::child_c<1>(a1) = info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [R,P] = chol(A,upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::upper_> const&
              ) const
    {
      eval(a0,a1,
           boost::mpl::long_<1>(),N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,raw_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::raw_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'U');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      assign_swap( boost::proto::child_c<0>(a1), a);
      boost::proto::child_c<1>(a1) = info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,raw_,lower_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::raw_> const&
              , nt2::policy<ext::lower_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'L');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      assign_swap( boost::proto::child_c<0>(a1), a);
      boost::proto::child_c<1>(a1) =  info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,raw_,upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::raw_> const&
              , nt2::policy<ext::upper_> const&
              ) const
    {
      eval(a0, a1
          , boost::mpl::long_<2>(), N1(), nt2::policy<ext::raw_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,upper_,raw_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::upper_> const&
              , nt2::policy<ext::raw_> const&
              ) const
    {
      eval(a0, a1
          , boost::mpl::long_<2>(), N1(), nt2::policy<ext::raw_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,lower_,raw_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::lower_> const&
              , nt2::policy<ext::raw_> const&
              ) const
    {
      eval(a0, a1
          , boost::mpl::long_<3>(),  boost::mpl::long_<2>()
          , nt2::policy<ext::raw_>(), nt2::policy<ext::lower_>());
    }

  };
} }

#endif


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 Rice University 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.
*********************************************************************/

/* Author: Mark Moll */

#include "ompl/base/spaces/DubinsStateSpace.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/Exception.h"
#include <queue>
#include <boost/math/constants/constants.hpp>

using namespace ompl::base;

namespace
{
    const double twopi = 2. * boost::math::constants::pi<double>();
    const double DUBINS_EPS = 1e-6;
    const double DUBINS_ZERO = -1e-7;

    inline double mod2pi(double x)
    {
        if (x < 0 && x > DUBINS_ZERO)
            return 0;
        double xm = x - twopi * floor(x / twopi);
        if (twopi - xm < .5 * DUBINS_EPS) xm = 0.;
        return xm;
    }

    DubinsStateSpace::DubinsPath dubinsLSL(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
        if (tmp >= DUBINS_ZERO)
        {
            double theta = atan2(cb - ca, d + sa - sb);
            double t = mod2pi(-alpha + theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha + t) - sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsRSR(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
        if (tmp >= DUBINS_ZERO)
        {
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha - t) + sa - sb - d) < 2* DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[1], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsRSL(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
        if (tmp >= DUBINS_ZERO)
        {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
            double t = mod2pi(alpha - theta);
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[2], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsLSR(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
        if (tmp >= DUBINS_ZERO)
        {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
            double t = mod2pi(-alpha + theta);
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[3], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsRLR(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
        if (fabs(tmp) < 1.)
        {
            double p = twopi - acos(tmp);
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta + .5 * p);
            double q = mod2pi(alpha - beta - t + p);
            assert(fabs(2. * sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < 2 * DUBINS_EPS);
            assert(fabs(-2. * cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[4], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsLRL(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
        if (fabs(tmp) < 1.)
        {
            double p = twopi - acos(tmp);
            double theta = atan2(-ca + cb, d + sa - sb);
            double t = mod2pi(-alpha + theta + .5 * p);
            double q = mod2pi(beta - alpha - t + p);
            assert(fabs(-2. * sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < 2 * DUBINS_EPS);
            assert(fabs(2. * cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[5], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubins(double d, double alpha, double beta)
    {
        if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
            return {DubinsStateSpace::dubinsPathType[0], 0, d, 0};

        DubinsStateSpace::DubinsPath path(dubinsLSL(d, alpha, beta)), tmp(dubinsRSR(d, alpha, beta));
        double len, minLength = path.length();

        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRSL(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLSR(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRLR(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLRL(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
            path = tmp;
        return path;
    }
}

const ompl::base::DubinsStateSpace::DubinsPathSegmentType ompl::base::DubinsStateSpace::dubinsPathType[6][3] = {
    {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT},
    {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT},
    {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT},
    {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT},
    {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT},
    {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}};

double ompl::base::DubinsStateSpace::distance(const State *state1, const State *state2) const
{
    if (isSymmetric_)
        return rho_ * std::min(dubins(state1, state2).length(), dubins(state2, state1).length());
    return rho_ * dubins(state1, state2).length();
}

void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
{
    bool firstTime = true;
    DubinsPath path;
    interpolate(from, to, t, firstTime, path, state);
}

void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
                                               DubinsPath &path, State *state) const
{
    if (firstTime)
    {
        if (t >= 1.)
        {
            if (to != state)
                copyState(state, to);
            return;
        }
        if (t <= 0.)
        {
            if (from != state)
                copyState(state, from);
            return;
        }

        path = dubins(from, to);
        if (isSymmetric_)
        {
            DubinsPath path2(dubins(to, from));
            if (path2.length() < path.length())
            {
                path2.reverse_ = true;
                path = path2;
            }
        }
        firstTime = false;
    }
    interpolate(from, path, t, state);
}

void ompl::base::DubinsStateSpace::interpolate(const State *from, const DubinsPath &path, double t, State *state) const
{
    auto *s = allocState()->as<StateType>();
    double seg = t * path.length(), phi, v;

    s->setXY(0., 0.);
    s->setYaw(from->as<StateType>()->getYaw());
    if (!path.reverse_)
    {
        for (unsigned int i = 0; i < 3 && seg > 0; ++i)
        {
            v = std::min(seg, path.length_[i]);
            phi = s->getYaw();
            seg -= v;
            switch (path.type_[i])
            {
                case DUBINS_LEFT:
                    s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
                    s->setYaw(phi + v);
                    break;
                case DUBINS_RIGHT:
                    s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
                    s->setYaw(phi - v);
                    break;
                case DUBINS_STRAIGHT:
                    s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
                    break;
            }
        }
    }
    else
    {
        for (unsigned int i = 0; i < 3 && seg > 0; ++i)
        {
            v = std::min(seg, path.length_[2 - i]);
            phi = s->getYaw();
            seg -= v;
            switch (path.type_[2 - i])
            {
                case DUBINS_LEFT:
                    s->setXY(s->getX() + sin(phi - v) - sin(phi), s->getY() - cos(phi - v) + cos(phi));
                    s->setYaw(phi - v);
                    break;
                case DUBINS_RIGHT:
                    s->setXY(s->getX() - sin(phi + v) + sin(phi), s->getY() + cos(phi + v) - cos(phi));
                    s->setYaw(phi + v);
                    break;
                case DUBINS_STRAIGHT:
                    s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
                    break;
            }
        }
    }
    state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
    state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
    getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
    state->as<StateType>()->setYaw(s->getYaw());
    freeState(s);
}

ompl::base::DubinsStateSpace::DubinsPath ompl::base::DubinsStateSpace::dubins(const State *state1,
                                                                              const State *state2) const
{
    const auto *s1 = static_cast<const StateType *>(state1);
    const auto *s2 = static_cast<const StateType *>(state2);
    double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
    double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
    double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx);
    double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
    return ::dubins(d, alpha, beta);
}

void ompl::base::DubinsMotionValidator::defaultSettings()
{
    stateSpace_ = dynamic_cast<DubinsStateSpace *>(si_->getStateSpace().get());
    if (stateSpace_ == nullptr)
        throw Exception("No state space for motion validator");
}

bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2,
                                                    std::pair<State *, double> &lastValid) const
{
    /* assume motion starts in a valid configuration so s1 is valid */

    bool result = true, firstTime = true;
    DubinsStateSpace::DubinsPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    if (nd > 1)
    {
        /* temporary storage for the checked state */
        State *test = si_->allocState();

        for (int j = 1; j < nd; ++j)
        {
            stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
            if (!si_->isValid(test))
            {
                lastValid.second = (double)(j - 1) / (double)nd;
                if (lastValid.first != nullptr)
                    stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
                result = false;
                break;
            }
        }
        si_->freeState(test);
    }

    if (result)
        if (!si_->isValid(s2))
        {
            lastValid.second = (double)(nd - 1) / (double)nd;
            if (lastValid.first != nullptr)
                stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
            result = false;
        }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}

bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2) const
{
    /* assume motion starts in a valid configuration so s1 is valid */
    if (!si_->isValid(s2))
        return false;

    bool result = true, firstTime = true;
    DubinsStateSpace::DubinsPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    /* initialize the queue of test positions */
    std::queue<std::pair<int, int>> pos;
    if (nd >= 2)
    {
        pos.emplace(1, nd - 1);

        /* temporary storage for the checked state */
        State *test = si_->allocState();

        /* repeatedly subdivide the path segment in the middle (and check the middle) */
        while (!pos.empty())
        {
            std::pair<int, int> x = pos.front();

            int mid = (x.first + x.second) / 2;
            stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);

            if (!si_->isValid(test))
            {
                result = false;
                break;
            }

            pos.pop();

            if (x.first < mid)
                pos.emplace(x.first, mid - 1);
            if (x.second > mid)
                pos.emplace(mid + 1, x.second);
        }

        si_->freeState(test);
    }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_CEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_CEA_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_qsfn.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_auth.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace cea{
            static const double EPS = 1e-10;

            struct par_cea
            {
                double qp;
                double apa[APA_SIZE];
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_cea_ellipsoid : public base_t_fi<base_cea_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_cea m_proj_parm;

                inline base_cea_ellipsoid(const Parameters& par)
                    : base_t_fi<base_cea_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    xy_x = this->m_par.k0 * lp_lon;
                    xy_y = .5 * pj_qsfn(sin(lp_lat), this->m_par.e, this->m_par.one_es) / this->m_par.k0;
                }

                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    lp_lat = pj_authlat(asin( 2. * xy_y * this->m_par.k0 / this->m_proj_parm.qp), this->m_proj_parm.apa);
                    lp_lon = xy_x / this->m_par.k0;
                }
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_cea_spheroid : public base_t_fi<base_cea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_cea m_proj_parm;

                inline base_cea_spheroid(const Parameters& par)
                    : base_t_fi<base_cea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    xy_x = this->m_par.k0 * lp_lon;
                    xy_y = sin(lp_lat) / this->m_par.k0;
                }

                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double t;

                    if ((t = fabs(xy_y *= this->m_par.k0)) - EPS <= 1.) {
                        if (t >= 1.)
                            lp_lat = xy_y < 0. ? -HALFPI : HALFPI;
                        else
                            lp_lat = asin(xy_y);
                        lp_lon = xy_x / this->m_par.k0;
                    } else throw proj_exception();;
                }
            };

            // Equal Area Cylindrical
            template <typename Parameters>
            void setup_cea(Parameters& par, par_cea& proj_parm)
            {
                double t = 0;
                if (pj_param(par.params, "tlat_ts").i &&
                    (par.k0 = cos(t = pj_param(par.params, "rlat_ts").f)) < 0.)
                  throw proj_exception(-24);
                if (par.es) {
                    t = sin(t);
                    par.k0 /= sqrt(1. - par.es * t * t);
                    par.e = sqrt(par.es);
                    pj_authset(par.es, proj_parm.apa);
                    proj_parm.qp = pj_qsfn(1., par.e, par.one_es);
                // par.inv = e_inverse;
                // par.fwd = e_forward;
                } else {
                // par.inv = s_inverse;
                // par.fwd = s_forward;
                }
            }

        }} // namespace detail::cea
    #endif // doxygen

    /*!
        \brief Equal Area Cylindrical projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
         - lat_ts=
        \par Example
        \image html ex_cea.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct cea_ellipsoid : public detail::cea::base_cea_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline cea_ellipsoid(const Parameters& par) : detail::cea::base_cea_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::cea::setup_cea(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Equal Area Cylindrical projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
         - lat_ts=
        \par Example
        \image html ex_cea.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct cea_spheroid : public detail::cea::base_cea_spheroid<Geographic, Cartesian, Parameters>
    {
        inline cea_spheroid(const Parameters& par) : detail::cea::base_cea_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::cea::setup_cea(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class cea_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<cea_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                    else
                        return new base_v_fi<cea_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void cea_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("cea", new cea_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_CEA_HPP