/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2012 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

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

/*! \file swaptionvolmatrix.hpp
    \brief Swaption matrix whith hull white smiles
*/

#ifndef quantlib_swaption_volatility_hullwhite_hpp
#define quantlib_swaption_volatility_hullwhite_hpp

#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/indexes/SwapIndex.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/termstructures/volatility/swaption/swaptionvoldiscrete.hpp>
#include <ql/termstructures/volatility/swaption/swaptionvolstructure.hpp>
#include <ql/models/shortrate/calibrationhelpers/swaptionhelper.hpp>
#include <ql/models/shortrate/onefactormodels/hullwhite.hpp>
#include <ql/math/interpolations/interpolation2d.hpp>
#include <ql/math/matrix.hpp>
#include <ql/math/solvers1D/brent.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/math/optimization/simplex.hpp>
#include <ql/math/optimization/costfunction.hpp>
#include <ql/math/optimization/endcriteria.hpp>
#include <ql/math/optimization/problem.hpp>
#include <ql/termstructures/volatility/smilesection.hpp>
#include <ql/pricingengines/swaption/jamshidianswaptionengine.hpp>
#include <boost/noncopyable.hpp>
#include <vector>

#include <iostream>

#include <hullwhitesmilesection.hpp>

namespace QuantLib {

    class Quote;

    class SwaptionVolatilityHullWhite : public SwaptionVolatilityDiscrete,
                                     private boost::noncopyable {
      public:
        //! floating reference date, floating market data
        SwaptionVolatilityHullWhite(const Real reversion, const Handle<YieldTermStructure>& yts, const boost::shared_ptr<SwapIndex> indexBase,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const std::vector<std::vector<Handle<Quote> > >& vols,
                    const DayCounter& dayCounter);
        //! fixed reference date, floating market data
        SwaptionVolatilityHullWhite(const Real reversion, const Handle<YieldTermStructure>& yts, const boost::shared_ptr<SwapIndex> indexBase,
                    const Date& referenceDate,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const std::vector<std::vector<Handle<Quote> > >& vols,
                    const DayCounter& dayCounter);
        //! floating reference date, fixed market data
        SwaptionVolatilityHullWhite(const Real reversion, const Handle<YieldTermStructure>& yts, const boost::shared_ptr<SwapIndex> indexBase,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const Matrix& volatilities,
                    const DayCounter& dayCounter);
        //! fixed reference date, fixed market data
        SwaptionVolatilityHullWhite(const Real reversion, const Handle<YieldTermStructure>& yts, const boost::shared_ptr<SwapIndex> indexBase,
                    const Date& referenceDate,
                    const Calendar& calendar,
                    BusinessDayConvention bdc,
                    const std::vector<Period>& optionTenors,
                    const std::vector<Period>& swapTenors,
                    const Matrix& volatilities,
                    const DayCounter& dayCounter);
        // fixed reference date and fixed market data, option dates
        SwaptionVolatilityHullWhite(const Real reversion, const Handle<YieldTermStructure>& yts, const boost::shared_ptr<SwapIndex> indexBase,
								 const Date& referenceDate,
                                 const std::vector<Date>& optionDates,
                                 const std::vector<Period>& swapTenors,
                                 const Matrix& volatilities,
                                 const DayCounter& dayCounter);
        //! \name LazyObject interface
        //@{
        void performCalculations() const;
        //@}
        //! \name TermStructure interface
        //@{
        Date maxDate() const;
        //@}
        //! \name VolatilityTermStructure interface
        //@{
        Rate minStrike() const;
        Rate maxStrike() const;
        //@}
        //! \name SwaptionVolatilityStructure interface
        //@{
        const Period& maxSwapTenor() const;
        //@}
        //! \name Other inspectors
        //@{
        //! returns the lower indexes of surrounding volatility matrix corners
        std::pair<Size,Size> locate(const Date& optionDate,
                                    const Period& swapTenor) const {
            return locate(timeFromReference(optionDate),
                          swapLength(swapTenor));
        }
        //! returns the lower indexes of surrounding volatility matrix corners
        std::pair<Size,Size> locate(Time optionTime,
                                    Time swapLength) const {
            return std::make_pair(interpolation_.locateY(optionTime),
                                  interpolation_.locateX(swapLength));
        }
        //@}
      protected:
        boost::shared_ptr<SmileSection> smileSectionImpl(const Date&,
                                                         const Period&) const;
        boost::shared_ptr<SmileSection> smileSectionImpl(Time,
                                                         Time) const;
        Volatility volatilityImpl(Time optionTime,
                                  Time swapLength,
                                  Rate strike) const;
		Volatility volatilityImpl(const Date& optionDate, const Period& swapTenor, Rate strike) const;

      private:
        void checkInputs(Size volRows,
                         Size volsColumns) const;
        void registerWithMarketData();
        std::vector<std::vector<Handle<Quote> > > volHandles_;
        mutable Matrix volatilities_;
        mutable Interpolation2D interpolation_;
		mutable Matrix hwsigmas_;
		mutable Interpolation2D interpolationSigma_;
		Real reversion_;
		Handle<YieldTermStructure> yts_;
		boost::shared_ptr<SwapIndex> indexBase_;

		struct calibrationFunction : CostFunction {

			calibrationFunction(boost::shared_ptr<CalibratedModel> model, boost::shared_ptr<SwaptionHelper> helper) : model_(model), helper_(helper) {}

			Real value(const Array& params0) const {
				Array params(2);
				params[0]=model_->params()[0];
				params[1]=params0[0]*params0[0];
				model_->setParams(params);
				Real error=helper_->calibrationError();
				return error;
			}

			Disposable<Array> values(const Array& params) const {
			   Array result(1);
			   result[0] = value(params);
			   return result;
			}

			boost::shared_ptr<CalibratedModel> model_;
			boost::shared_ptr<SwaptionHelper> helper_;
		};
	};

    // inline definitions

    inline Date SwaptionVolatilityHullWhite::maxDate() const {
        return optionDates_.back();
    }

    inline Rate SwaptionVolatilityHullWhite::minStrike() const {
        return QL_MIN_REAL;
    }

    inline Rate SwaptionVolatilityHullWhite::maxStrike() const {
        return QL_MAX_REAL;
    }

    inline const Period& SwaptionVolatilityHullWhite::maxSwapTenor() const {
        return swapTenors_.back();
    }

    inline Volatility SwaptionVolatilityHullWhite::volatilityImpl(Time optionTime,
                                                               Time swapLength,
                                                               Rate strike) const {
        calculate();
		return smileSection(optionTime,swapLength,true)->volatility(strike);
    }

	inline Volatility SwaptionVolatilityHullWhite::volatilityImpl(const Date& optionDate,
                                                const Period& swapTenor, Rate strike) const {
        calculate();
		return smileSection(optionDate,swapTenor,true)->volatility(strike);
    }

}

#endif


/*
 * personBHMIP.cpp
 *
 *  Created on: Jul 9, 2013
 *      Author: gferrer
 */
#include "scene_elements/person_bhmip.h"
#include <Eigen/Dense>
#include <iostream>

Cperson_bhmip::Cperson_bhmip(unsigned int id, Cperson_abstract::target_type person_target_type,
		 Cperson_abstract::force_type person_force_type, double _time_window) :
    Cperson_abstract(id,person_target_type, person_force_type),
    time_window_( _time_window ) , phi_var_( 0.8*0.8 )
{
	destinations_.reserve(15);
}

Cperson_bhmip::~Cperson_bhmip()
{

}

void Cperson_bhmip::add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter)
{
	SpointV_cov filtered_pose;
	if( filter != Cperson_abstract::No_filtering )
	{
		SpointV_cov pointV = point;//only positions, position_covariances and time_stamp
		double dx, dy;
		if ( !trajectory_.empty() )
		{
			dx = point.x - trajectory_.back().x;
			dy = point.y - trajectory_.back().y;
			double dt =  point.time_stamp - trajectory_.back().time_stamp ;
			//just to avoid singularities, ideally time stamps are well separated in time
			if ( point.time_stamp - trajectory_.back().time_stamp  < 0.001 ) dt = 0.1;
			pointV.vx = dx / dt;
			pointV.vy = dy / dt;
		}
		else
		{
			pointV.vx = 0;
			pointV.vy = 0;
		}

		//slide temporal window for all trajectories
		trajectory_.push_back(pointV);
		trajectory_windowing();

		//filters velocities
		switch( filter )
		{
		  case Cperson_abstract::Linear_regression_filtering:
			filtered_pose = filter_current_state_linear_regression( );
			break;
		  case Cperson_abstract::Bayes_filtering:
			filtered_pose = filter_current_state_linear_regression_bayes( );
		  	break;
		  case Cperson_abstract::Low_pass_linear_regression_filtering:
		  default :
			filtered_pose = low_pass_filter_current_state_linear_regression( );
			break;
		}

	}
	else
	{
		//no filter is required
		trajectory_.push_back(point);
		//slide temporal window for all trajectories
		trajectory_windowing();
		filtered_pose = trajectory_.back();
	}

	//update current positions and velocities according to filtering results
	diff_pointV_ = filtered_pose - current_pointV_ ;
	current_pointV_ = filtered_pose;
	desired_velocity_ = filtered_pose.v();
	now_ = filtered_pose.time_stamp;

	//precalculates data required for the BHMIP
	intention_precalculation();

	observation_update_ = true;
}

SpointV_cov Cperson_bhmip::filter_current_state_linear_regression( )
{

	//Discard a single and double pose trajectory --------------------------------------------
	if (trajectory_.size() <= 2 )
	{
		return trajectory_.back();
	}
	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
	//vector and matrices initialization
	unsigned int window_elements = trajectory_.size();
	double initial_ts = trajectory_.front().time_stamp;
	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
	Eigen::MatrixXd R (window_elements , 2 ); //1st order regression -> 2 coeffs
	for (unsigned int i = 0 ; i < window_elements ; i++)
	{
		vx ( i ) = trajectory_.at(i).vx;
		vy ( i ) = trajectory_.at(i).vy;
		R.row ( i ) << 1 , trajectory_.at(i).time_stamp - initial_ts;
	}
	//pseudo inverse of the R matrix ---------------------------------------------------------------
	Eigen::MatrixXd invR ( 2 , window_elements ) , sqrR ( 2 , 2 );
	sqrR = R.transpose() * R;
	invR = sqrR.inverse() * R.transpose() ;

	//linear regression ----------------------------------------------------------------------------
	Eigen::Vector2d  r_pred (1 , trajectory_.back().time_stamp - initial_ts ),vx_coef,vy_coef;
	//invR * v = beta -> parameter estimation
	vx_coef = invR * vx;
	vy_coef = invR * vy;
	SpointV_cov filtered_pose(
			trajectory_.back().x, //no filtering at position
			trajectory_.back().y,
			trajectory_.back().time_stamp,
			r_pred.dot ( vx_coef ),
			r_pred.dot ( vy_coef ) );
    filtered_pose.cov[0] = trajectory_.back().cov_xx();
    filtered_pose.cov[1] = trajectory_.back().cov_xy();
    filtered_pose.cov[4] = trajectory_.back().cov_xy();
    filtered_pose.cov[5] = trajectory_.back().cov_yy();

	//covariance of velocity prediction, with respect to regression velocity
	Eigen::VectorXd vx_mean ( window_elements ) , vy_mean ( window_elements );
	vx_mean = R*vx_coef;
	vy_mean = R*vy_coef;
	double cov_vx = (vx - vx_mean).dot( vx - vx_mean );
	double cov_vy = (vy - vy_mean).dot( vy - vy_mean );
	double cov_vxy = (vy - vy_mean).dot( vx - vx_mean );

	//regression covariances + projected uncertainty from positions P_x / dt
	filtered_pose.cov[10] = cov_vx + 0.001;
	filtered_pose.cov[11] = cov_vxy;
	filtered_pose.cov[14] = filtered_pose.cov[11];
	filtered_pose.cov[15] = cov_vy + 0.001;

	return filtered_pose;
}

SpointV_cov Cperson_bhmip::filter_current_state_linear_regression_bayes( )
{

	//Discard a single and double pose trajectory --------------------------------------------
	if (trajectory_.size() <= 2 )
	{
		return trajectory_.back();
	}
	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
	//vector and matrices initialization
	unsigned int window_elements = trajectory_.size();
	double initial_ts = trajectory_.front().time_stamp;
	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
	Eigen::MatrixXd R (window_elements , 2 ); //1st order regression -> 2 coeffs
	for (unsigned int i = 0 ; i < window_elements ; i++)
	{
		vx ( i ) = trajectory_.at(i).vx;
		vy ( i ) = trajectory_.at(i).vy;
		R.row ( i ) << 1 , trajectory_.at(i).time_stamp - initial_ts;
	}
	//pseudo inverse of the R matrix ---------------------------------------------------------------
	Eigen::MatrixXd invR ( 2 , window_elements ) , sqrR ( 2 , 2 );
	sqrR = R.transpose() * R;
	invR = sqrR.inverse() * R.transpose() ;

	//linear regression ----------------------------------------------------------------------------
	Eigen::Vector2d  r_pred (1 , trajectory_.back().time_stamp - initial_ts ),vx_coef,vy_coef;
	//invR * v = beta -> parameter estimation
	vx_coef = invR * vx;
	vy_coef = invR * vy;
	SpointV_cov filtered_pose(
			trajectory_.back().x, //no filtering at position
			trajectory_.back().y,
			trajectory_.back().time_stamp,
			r_pred.dot ( vx_coef ),
			r_pred.dot ( vy_coef ) );
    filtered_pose.cov[0] = trajectory_.back().cov_xx();
    filtered_pose.cov[1] = trajectory_.back().cov_xy();
    filtered_pose.cov[4] = trajectory_.back().cov_xy();
    filtered_pose.cov[5] = trajectory_.back().cov_yy();

	//covariance of velocity prediction, average of uncertainty propagated from estimated positions, equidistribited
	double dt;
	Eigen::Matrix2d v_cov_joint = Eigen::Matrix2d::Zero();
	for( unsigned int i = 0; i < window_elements ; i++)
	{
		if( i == 0 )
			dt = R(1,1)-R(0,1);
		else
			dt = R(i,1)-R(i-1,1);
		Eigen::Matrix2d v_cov;
		v_cov << trajectory_.at(i).cov_xx() , trajectory_.at(i).cov_xy(),
				trajectory_.at(i).cov_xy() , trajectory_.at(i).cov_yy();
		v_cov_joint += v_cov / (dt*dt);
	}

	//regression covariances + projected uncertainty from positions P_x / dt
	double N = 2.0*(double)window_elements*(double)window_elements;
	filtered_pose.cov[10] = v_cov_joint(0,0)/N;
	filtered_pose.cov[11] = v_cov_joint(0,1)/N;
	filtered_pose.cov[14] = filtered_pose.cov[11];
	filtered_pose.cov[15] = v_cov_joint(1,1)/N;

	return filtered_pose;
}

SpointV_cov Cperson_bhmip::low_pass_filter_current_state_linear_regression( )
{

	//Discard a single and double pose trajectory --------------------------------------------
	if (trajectory_.size() <= 2 )
	{
		return trajectory_.back();
	}
	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
	//vector and matrices initialization
	unsigned int window_elements = trajectory_.size();
//	double initial_ts = trajectory_.front().time_stamp;
	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
	Eigen::VectorXd R (window_elements ); //0st order regression -> 1 coeffs
	Eigen::VectorXd r_pred (window_elements);
	for (unsigned int i = 0 ; i < window_elements ; i++)
	{
		vx ( i ) = trajectory_.at(i).vx;
		vy ( i ) = trajectory_.at(i).vy;
		R(i)=1;
		r_pred(i)=1;
	}
	//pseudo inverse of the R matrix ---------------------------------------------------------------
	Eigen::VectorXd invR (  window_elements );
	double sqrR =  R.dot( R);
	invR = (1/sqrR) * R.transpose() ;

	//linear regression ----------------------------------------------------------------------------

	//invR * v = beta -> parameter estimation
	double vx_coef =  invR.dot(vx.transpose());
	double vy_coef = invR.dot(vy.transpose());

	SpointV_cov filtered_pose(
			trajectory_.back().x, //no filtering at position
			trajectory_.back().y,
			trajectory_.back().time_stamp,
			vx_coef ,
			vy_coef );

    filtered_pose.cov[0] = trajectory_.back().cov_xx();
    filtered_pose.cov[1] = trajectory_.back().cov_xy();
    filtered_pose.cov[4] = trajectory_.back().cov_xy();
    filtered_pose.cov[5] = trajectory_.back().cov_yy();

	//covariance of velocity prediction, with respect to regression velocity
	Eigen::VectorXd vx_mean ( window_elements ) , vy_mean ( window_elements );
	vx_mean = R*vx_coef;
	vy_mean = R*vy_coef;
	double cov_vx = (vx - vx_mean).dot( vx - vx_mean );
	double cov_vy = (vy - vy_mean).dot( vy - vy_mean );
	double cov_vxy = (vy - vy_mean).dot( vx - vx_mean );

	//regression covariances + projected uncertainty from positions P_x / dt
	filtered_pose.cov[10] = cov_vx;// + 0.001;
	filtered_pose.cov[11] = cov_vxy;
	filtered_pose.cov[14] = filtered_pose.cov[11];
	filtered_pose.cov[15] = cov_vy;// + 0.001;

	return filtered_pose;
}

void  Cperson_bhmip::intention_precalculation()
{
	double phi , phi_prob;
	std::vector<double> s;
	phi_pose2dest_.push_back( s );
	phi_prob_.push_back( s );
	for(unsigned int i = 0; i < destinations_.size() ; ++i)
	{
		double dx = destinations_[i].x - current_pointV_.x;
		double dy = destinations_[i].y - current_pointV_.y;
		phi = diffangle( atan2(  dy , dx ) , current_pointV_.orientation());
		phi_pose2dest_.back().push_back( phi );
		//c * Pr ( phi | destination_i) calculations
		phi_prob = 100*exp( - phi*phi / phi_var_ );
		phi_prob_.back().push_back( phi_prob );
	}

}

void Cperson_bhmip::trajectory_windowing()
{
	while(  trajectory_.back().time_stamp  - trajectory_.front().time_stamp >  time_window_ )
	{
		trajectory_.pop_front();
		if(type_ == Person)//Robot and Virtual_Person do not require prediction methods
		{
			phi_pose2dest_.pop_front();
			phi_prob_.pop_front();
		}
	}
}

void Cperson_bhmip::refresh_person( double now )
{
	//this method is used when manual remove of persons is required and thus, we must
	//guarantee that the trajectory container and other variables are OK

	if (trajectory_.empty() )
	{
		while( trajectory_.back().time_stamp - now > time_window_)
		{
			trajectory_.pop_front();
			if (trajectory_.empty() ) break;
		}
	}
	now_ = now;
}

void Cperson_bhmip::prediction( double min_v_to_predict )
{
	//calculate if it is adequate to predict intentionality:
	//if target is almost stopped and decelerating
	if( current_pointV_.v() <  min_v_to_predict)
	{
		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
				1.0, Sdestination::Stopping);
		desired_velocity_ = 0.0;//to avoid being too reactive to its environment, it remains in place v_d = 0.0
		return;
	}

	//calculate probabilities to destinations
	double norm_term(0.0),max_prob(-0.1);
	unsigned int max_prob_index;
	posterior_destinations_prob_.clear();
	for( unsigned int i = 0; i<destinations_.size() ; ++i)
	{
		posterior_destinations_prob_.push_back( destinations_[i].prob);
		for (unsigned int j = 0; j < phi_prob_.size() ; ++j)
		{
			//phi_probabilities [ time ] [ destination ]
			posterior_destinations_prob_[i] *= phi_prob_[j][i];
		}
		norm_term += posterior_destinations_prob_[i];
		if( max_prob < posterior_destinations_prob_[i])
		{
			max_prob_index = i;
			max_prob = posterior_destinations_prob_[i];
		}
	}

	//uncertain behavior of prediction depending on certain threshold
	if( 0) //max_prob < 1e16 )//hardcoded threshold corresponds aprox to a diff of 45º
	{
		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
				1.0, Sdestination::Uncertain);
		return;
	}
	//normalization of probabilities
	norm_term = 1/norm_term;
	if ( !is_nan( norm_term ) && !destinations_.empty() )
	{
		for (unsigned int i = 0 ; i < destinations_.size() ; ++i)
			posterior_destinations_prob_[i] *= norm_term;
		best_destination_ = destinations_[max_prob_index];
		best_destination_.prob = posterior_destinations_prob_[max_prob_index];
	}
	else
	{
		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
				1.0, Sdestination::Uncertain);
	}
}

void Cperson_bhmip::reset(  )
{
	trajectory_.clear();
	current_pointV_ = SpointV_cov();
	diff_pointV_ = SpointV_cov();
	phi_pose2dest_.clear();
	phi_prob_.clear();
	force_to_goal_ = Sforce() ;
	force_int_person_ = Sforce() ;
	force_obstacle_ = Sforce() ;
	force_int_robot_ = Sforce() ;
}

void Cperson_bhmip::rotate_and_translate_trajectory(double R, double thetaZ, double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug_odometry){
	// Function for local tracking. This function change the frame of the Current_pose and the window of poses for the person,
	// tacking into account the actual position of the robot.
	// Also compensate the rotational velocities applied to the person when the robot turns.
		// R -> Robot Translation ; thetaZ -> Robot rotation

	if(debug_odometry){
	std::cout << "!!!!!!!!!!!!!!!!!! (people prediction) rotate_and_translate_trajectory !!!!!!!!!!!!!" << std::endl;
	}
	hom2_ant.clear();

	Eigen::MatrixXd homT1; // homogeneous transfor para pasar los tracks al nuevo frame de la odometria.
	Eigen::MatrixXd homT1vis;
	Eigen::MatrixXd homT2;
	Eigen::MatrixXd J; // necesaria para introducir el aumento de la covarianza por culpa del error en R=traslacion y en thetaZ=rotación.
	Eigen::MatrixXd H_cov;
	Eigen::MatrixXd P;
	homT1.resize(3, 3);
	homT1vis.resize(3, 3);
	homT2.resize(3, 3);
	J.resize(4, 2);
	H_cov.resize(4, 4);
	P.resize(2, 2);

	/*homT1.row(0) << cos(thetaZ), -sin(thetaZ), R * cos(thetaZ/2);
	homT1.row(1) << sin(thetaZ), cos(thetaZ), R * sin(thetaZ/2);
	homT1.row(2) << 0, 0, 1;*/

	if(debug_odometry){
	std::cout << "(people prediction) homT1 tf=" << std::endl;
	std::cout << "["<<vect_odom_eigen_tf[0]<<","<<vect_odom_eigen_tf[1]<<","<< vect_odom_eigen_tf[2]<<"]" << std::endl;
	std::cout << "["<<vect_odom_eigen_tf[3]<<","<<vect_odom_eigen_tf[4]<<","<< vect_odom_eigen_tf[5]<<"]" << std::endl;
	std::cout << "["<<vect_odom_eigen_tf[6]<<","<<vect_odom_eigen_tf[7]<<","<< vect_odom_eigen_tf[8]<<"]"  << std::endl;
	}

	homT1.row(0) << vect_odom_eigen_tf[0], vect_odom_eigen_tf[1], vect_odom_eigen_tf[2];
	homT1.row(1) << vect_odom_eigen_tf[3], vect_odom_eigen_tf[4], vect_odom_eigen_tf[5];
	homT1.row(2) << vect_odom_eigen_tf[6], vect_odom_eigen_tf[7], vect_odom_eigen_tf[8];

	if(debug_odometry){
	homT1vis.row(0) << cos(thetaZ), -sin(thetaZ), R * cos(thetaZ/2);
	homT1vis.row(1) << sin(thetaZ), cos(thetaZ), R * sin(thetaZ/2);
	homT1vis.row(2) << 0, 0, 1;
		std::cout << "homT1vis _ prediction  = [ " << homT1(0, 0) << " , " << homT1(0, 1)
				<< " , " << homT1(0, 2) << std::endl << "               "
				<< homT1(1, 0) << " , " << homT1(1, 1) << " , " << homT1(1, 2)
				<< std::endl << "               " << homT1(2, 0) << " , "
				<< homT1(2, 1) << " , " << homT1(2, 2) << std::endl;
	}

	//homT2 = homT1.inverse();
	homT2 = homT1;//.inverse();

	if(debug_odometry){
		std::cout << "homT2 _ prediction = [ " << homT2(0, 0) << " , " << homT2(0, 1)
				<< " , " << homT2(0, 2) << std::endl << "               "
				<< homT2(1, 0) << " , " << homT2(1, 1) << " , " << homT2(1, 2)
				<< std::endl << "               " << homT2(2, 0) << " , "
				<< homT2(2, 1) << " , " << homT2(2, 2) << std::endl;
	}
		/*hom2_ant.push_back(homT2(0, 0));
		hom2_ant.push_back(homT2(0, 1));
		hom2_ant.push_back(homT2(0, 2));
		hom2_ant.push_back(homT2(1, 0));
		hom2_ant.push_back(homT2(1, 1));
		hom2_ant.push_back(homT2(1, 2));
		hom2_ant.push_back(homT2(2, 0));
		hom2_ant.push_back(homT2(2, 1));
		hom2_ant.push_back(homT2(2, 2));
		hom2_tf.push_front(hom2_ant);*/

	H_cov.row(0) << cos(thetaZ), sin(thetaZ), 0, 0;
	H_cov.row(1) << -sin(thetaZ), cos(thetaZ), 0, 0;
	H_cov.row(2) << 0, 0, cos(thetaZ), sin(thetaZ);
	H_cov.row(3) << 0, 0, -sin(thetaZ), cos(thetaZ);

	P.row(0) << 10 * R / 100, 0; //[G_R^2       0    ]
	P.row(1) << 0, 10 * thetaZ / 100; //[  0    G_thetaZ^2]

	// Robot local velocity
	trajectory_local_velocity_x_.push_back(linear_vx);
	trajectory_local_velocity_y_.push_back(linear_vy);

	// Change tracks trajectories to the actual robot frame.

	for(unsigned int i=0; i<trajectory_.size(); i++){

		/*Eigen::MatrixXd homT2_act;
		for(unsigned int j=0;j=<i;j++){
			homT2_act=homT2_act*hom2_tf
		}*/

		SpointV_cov track_ant=trajectory_[i];
		if(debug_odometry){
			std::cout << " Before (people prediction) trajectory_("<<i<<")"<< std::endl;
				trajectory_[i].print();
		}
		SpointV_cov track_act; //trajectory point transformed.

		Eigen::MatrixXd pos_ant(3,1);
		Eigen::MatrixXd pos_act(3,1);
		Eigen::MatrixXd vel_ant(3,1);
		Eigen::MatrixXd vel_act(3,1);
		Eigen::MatrixXd cov_ant(4,4);
		Eigen::MatrixXd cov_act(4,4);
		Eigen::MatrixXd robot_linear_vel(3,1);

		robot_linear_vel(0, 0) = linear_vx;
		robot_linear_vel(1, 0) = linear_vy;
		robot_linear_vel(2, 0) = 0;

		pos_ant(0, 0) = track_ant.x;
		pos_ant(1, 0) = track_ant.y;
		pos_ant(2, 0) = 1;

		pos_act = homT2 * pos_ant;

		vel_ant.row(0) << track_ant.vx;
		vel_ant.row(1) << track_ant.vy;
		vel_ant.row(2) << 0;

		vel_act = homT2 * vel_ant;

		//vel_act=vel_act-robot_linear_vel- w_robot x pos_act;

		J.row(0) << -cos(thetaZ), -sin(thetaZ) * track_ant.x
						+ cos(thetaZ) * track_ant.y + R * sin(thetaZ );
		J.row(1) << sin(thetaZ), -cos(thetaZ) * track_ant.x
						- sin(thetaZ) * track_ant.y + R * cos(thetaZ );
		J.row(2) << 0, -sin(thetaZ) * track_ant.vx + cos(thetaZ) * track_ant.vy;
		J.row(3) << 0, -cos(thetaZ) * track_ant.vx - sin(thetaZ) * track_ant.vy;

		cov_ant.row(0) << track_ant.cov[0], track_ant.cov[1], track_ant.cov[2], track_ant.cov[3];
		cov_ant.row(1) << track_ant.cov[4], track_ant.cov[5], track_ant.cov[6], track_ant.cov[7];
		cov_ant.row(2) << track_ant.cov[8], track_ant.cov[9], track_ant.cov[10], track_ant.cov[11];
		cov_ant.row(3) << track_ant.cov[12], track_ant.cov[13], track_ant.cov[14], track_ant.cov[15];

		//cov_act = H_cov * cov_ant * H_cov.transpose() + J * P * J.transpose();
		cov_act = H_cov * cov_ant * H_cov.transpose();

		std::vector<double> vect_cov_act;
		vect_cov_act.reserve(16);
		vect_cov_act.resize(16, 0.0);
		vect_cov_act[0] = (double) cov_act(0, 0); // cov_xx
		vect_cov_act[1] = (double) cov_act(0, 1); //cov_xy
		vect_cov_act[2] = (double) cov_act(0, 2); // cov_xvx
		vect_cov_act[3] = (double) cov_act(0, 3); //cov_xvy
		vect_cov_act[4] = (double) cov_act(1, 0); //cov_yx
		vect_cov_act[5] = (double) cov_act(1, 1); //cov_yy
		vect_cov_act[6] = (double) cov_act(1, 2); //cov_yvx
		vect_cov_act[7] = (double) cov_act(1, 3); //cov_yvy
		vect_cov_act[8] = (double) cov_act(2, 0); // cov_vxvx
		vect_cov_act[9] = (double) cov_act(2, 1); //cov_vxvy
		vect_cov_act[10] = (double) cov_act(2, 2); // cov_vxx
		vect_cov_act[11] = (double) cov_act(2, 3); //cov_vxy
		vect_cov_act[12] = (double) cov_act(3, 0); //cov_vyvx
		vect_cov_act[13] = (double) cov_act(3, 1); //cov_vyvy
		vect_cov_act[14] = (double) cov_act(3, 2); //cov_vyx
		vect_cov_act[15] = (double) cov_act(3, 3); //cov_vyy

		track_act = SpointV_cov(pos_act(0, 0), pos_act(1, 0),track_ant.time_stamp, vel_act(0, 0), vel_act(1, 0),vect_cov_act);

		trajectory_[i]=track_act;

		if(debug_odometry){
		std::cout << "(people prediction) track_ant("<<i<<")" << std::endl;
		track_ant.print();
		std::cout << " (people prediction) track_act("<<i<<")"<< std::endl;
		track_act.print();
		std::cout << " After (people prediction) trajectory_("<<i<<")"<< std::endl;
		trajectory_[i].print();
		}
	}

	// change current_pointV_ with odometry
	SpointV_cov ant_current_pointV=current_pointV_;
	if(debug_odometry){
	std::cout << " ANT current_pointV_="<< std::endl;
	current_pointV_.print();
	}
	//SpointV_cov act_current_pointV;
	Eigen::MatrixXd pos_ant_current_pointV(3,1);
	Eigen::MatrixXd pos_act_current_pointV(3,1);
	Eigen::MatrixXd vel_ant_current_pointV(3,1);
	Eigen::MatrixXd vel_act_current_pointV(3,1);
	Eigen::MatrixXd cov_ant_current_pointV(4,4);
	Eigen::MatrixXd cov_act_current_pointV(4,4);

	pos_ant_current_pointV(0, 0) = ant_current_pointV.x;
	pos_ant_current_pointV(1, 0) = ant_current_pointV.y;
	pos_ant_current_pointV(2, 0) = 1;

	pos_act_current_pointV = homT2 * pos_ant_current_pointV;

	vel_ant_current_pointV.row(0) << ant_current_pointV.vx;
	vel_ant_current_pointV.row(1) << ant_current_pointV.vy;
	vel_ant_current_pointV.row(2) << 0;

	vel_act_current_pointV = homT2 * vel_ant_current_pointV;

	J.row(0) << -cos(thetaZ), -sin(thetaZ) * ant_current_pointV.x + cos(thetaZ) * ant_current_pointV.y + R * sin(thetaZ / 2);
	J.row(1) << sin(thetaZ), -cos(thetaZ) * ant_current_pointV.x - sin(thetaZ) * ant_current_pointV.y + R * cos(thetaZ / 2);
	J.row(2) << 0, -sin(thetaZ) * ant_current_pointV.vx + cos(thetaZ) * ant_current_pointV.vy;
	J.row(3) << 0, -cos(thetaZ) * ant_current_pointV.vx - sin(thetaZ) * ant_current_pointV.vy;

	cov_ant_current_pointV.row(0) << ant_current_pointV.cov[0], ant_current_pointV.cov[1], ant_current_pointV.cov[2], ant_current_pointV.cov[3];
	cov_ant_current_pointV.row(1) << ant_current_pointV.cov[4], ant_current_pointV.cov[5], ant_current_pointV.cov[6], ant_current_pointV.cov[7];
	cov_ant_current_pointV.row(2) << ant_current_pointV.cov[8], ant_current_pointV.cov[9], ant_current_pointV.cov[10], ant_current_pointV.cov[11];
	cov_ant_current_pointV.row(3) << ant_current_pointV.cov[12], ant_current_pointV.cov[13], ant_current_pointV.cov[14], ant_current_pointV.cov[15];

	cov_act_current_pointV = H_cov * cov_ant_current_pointV * H_cov.transpose() + J * P * J.transpose();

	std::vector<double> vect_cov_act_current_pointV;
	vect_cov_act_current_pointV.reserve(16);
	vect_cov_act_current_pointV.resize(16, 0.0);
	vect_cov_act_current_pointV[0] = (double) cov_act_current_pointV(0, 0); // cov_xx
	vect_cov_act_current_pointV[1] = (double) cov_act_current_pointV(0, 1); //cov_xy
	vect_cov_act_current_pointV[2] = (double) cov_act_current_pointV(0, 2); // cov_xvx
	vect_cov_act_current_pointV[3] = (double) cov_act_current_pointV(0, 3); //cov_xvy
	vect_cov_act_current_pointV[4] = (double) cov_act_current_pointV(1, 0); //cov_yx
	vect_cov_act_current_pointV[5] = (double) cov_act_current_pointV(1, 1); //cov_yy
	vect_cov_act_current_pointV[6] = (double) cov_act_current_pointV(1, 2); //cov_yvx
	vect_cov_act_current_pointV[7] = (double) cov_act_current_pointV(1, 3); //cov_yvy
	vect_cov_act_current_pointV[8] = (double) cov_act_current_pointV(2, 0); // cov_vxvx
	vect_cov_act_current_pointV[9] = (double) cov_act_current_pointV(2, 1); //cov_vxvy
	vect_cov_act_current_pointV[10] = (double) cov_act_current_pointV(2, 2); // cov_vxx
	vect_cov_act_current_pointV[11] = (double) cov_act_current_pointV(2, 3); //cov_vxy
	vect_cov_act_current_pointV[12] = (double) cov_act_current_pointV(3, 0); //cov_vyvx
	vect_cov_act_current_pointV[13] = (double) cov_act_current_pointV(3, 1); //cov_vyvy
	vect_cov_act_current_pointV[14] = (double) cov_act_current_pointV(3, 2); //cov_vyx
	vect_cov_act_current_pointV[15] = (double) cov_act_current_pointV(3, 3); //cov_vyy

	//double track_vx=vel_act_current_pointV(0,0) - v_rot_x;
	//double track_vy=vel_act_current_pointV(1,0) - v_rot_y;

	//vel_act_current_pointV(0,0)=track_vx;
	//vel_act_current_pointV(1,0)=track_vy;

	current_pointV_ = SpointV_cov(pos_act_current_pointV(0, 0), pos_act_current_pointV(1, 0), ant_current_pointV.time_stamp, vel_act_current_pointV(0,0), vel_act_current_pointV(1,0),vect_cov_act_current_pointV);
	if(debug_odometry){
	std::cout << " AcT current_pointV_="<< std::endl;
	current_pointV_.print();
	}
}


#include <kv/Heine.hpp>
#include <kv/qAiry.hpp>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <cmath>
typedef kv::interval<double> itv;
typedef kv::complex< kv::interval<double> > cp;
using namespace std;
namespace ub = boost::numeric::ublas;
int main()
{
  cout.precision(17);
  ub::vector< itv > x(100);
  int n=40;
  itv q,xx,xy,omega;
  q="0.9";
  omega="0.3";
  x(0)=1.55;
  xy=mid(x(0));
  for(int i=0;i<=n;i++){
    xx=mid(x(i));
    x(i+1)=xx-kv::Ramanujan_qAiry(itv(q),itv(xx))*(xy-q*xy-omega)
      /(kv::Ramanujan_qAiry(itv(q),itv(xy))-kv::Ramanujan_qAiry(itv(q),itv(q*xy+omega)))
      +(1-(xy-q*xy-omega)*(kv::Ramanujan_qAiry(itv(q),itv(x(i)))-kv::Ramanujan_qAiry(itv(q),itv(q*x(i)+omega)))
	/(x(i)-q*x(i)-omega)/(kv::Ramanujan_qAiry(itv(q),itv(xy))-kv::Ramanujan_qAiry(itv(q),itv(q*xy+omega))))*(x(i)-xx);
    cout<<x(i+1)<<endl;
    cout<<"value of RqA inf"<<kv::Ramanujan_qAiry(itv(q),itv(x(i+1).lower()))<<endl;
    cout<<"value of RqA sup"<<kv::Ramanujan_qAiry(itv(q),itv(x(i+1).upper()))<<endl;
    cout<<"value of RqA mid"<<kv::Ramanujan_qAiry(itv(q),itv(mid(x(i+1))))<<endl;
    cout<<"value of contraction const:"<<abs(1-(xy-q*xy-omega)*(kv::Ramanujan_qAiry(itv(q),itv(x(i)))-kv::Ramanujan_qAiry(itv(q),itv(q*x(i)+omega)))
					     /(x(i)-q*x(i)-omega)/(kv::Ramanujan_qAiry(itv(q),itv(xy))-kv::Ramanujan_qAiry(itv(q),itv(q*xy+omega)))).upper()<<endl;
  }
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file expressions.hpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2014-04-02
 */

#ifndef PAAL_EXPRESSIONS_HPP
#define PAAL_EXPRESSIONS_HPP

#include "paal/lp/ids.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/print_collection.hpp"
#include "paal/utils/pretty_stream.hpp"

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/count_if.hpp>

#include <functional>
#include <unordered_map>

namespace paal {
namespace lp {

namespace {
struct linear_expression_traits {
    static const utils::compare<double> CMP;
};

const utils::compare<double> linear_expression_traits::CMP = utils::compare<double>();
}

/**
 * Expression class.
 */
class linear_expression {
    typedef std::unordered_map<col_id, double> Elements;
    typedef Elements::const_iterator ExprIter;

  public:
    /// Constructor.
    linear_expression() {}

    /// Constructor.
    linear_expression(col_id col, double coef = 1.) {
        m_coefs.emplace(col, coef);
    }

    /// Addition operator.
    linear_expression &operator+=(const linear_expression &expr) {
        join_expression(expr, utils::plus{});
        return *this;
    }

    /// Subtraction operator.
    linear_expression &operator-=(const linear_expression &expr) {
        join_expression(expr, utils::minus{});
        return *this;
    }

    /// Multiplication by a constant operator.
    linear_expression &operator*=(double val) {
        for (auto &elem : m_coefs) {
            elem.second *= val;
        }
        return *this;
    }

    /// Division by a constant operator.
    linear_expression &operator/=(double val) { return operator*=(1. / val); }

    /// Returns the coefficient for a given column.
    double get_coefficient(col_id col) const {
        auto elem = m_coefs.find(col);
        if (elem != m_coefs.end()) {
            return elem->second;
        } else {
            return 0.;
        }
    }

    /// Returns the iterator range of the elements in the expression.
    const Elements &get_elements() const { return m_coefs; }

    /// Returns the size (number of nonzero coefficients) of the expression.
    int non_zeros() const {
        return boost::count_if(m_coefs, [](std::pair<col_id, double> x) {
            return !linear_expression_traits::CMP.e(x.second, 0);
        });
    }

  private:
    template <typename Operation>
    void join_expression(const linear_expression &expr, Operation op) {
        for (auto new_elem : expr.m_coefs) {
            auto elem = m_coefs.find(new_elem.first);
            if (elem == m_coefs.end()) {
                new_elem.second = op(0., new_elem.second);
                m_coefs.insert(new_elem);
            } else {
                elem->second = op(elem->second, new_elem.second);
            }
        }
    }

    Elements m_coefs;
};

namespace detail {
inline std::string col_id_to_string(col_id col) {
    return " x_" + std::to_string(col.get());
}

template <typename Stream, typename PrintCol>
void print_expression(Stream &o, const linear_expression &expr,
                      PrintCol print_col) {
    print_collection(o, expr.get_elements() |
                            boost::adaptors::transformed(
                                [&](std::pair<col_id, double> col_and_val) {
        return pretty_to_string(col_and_val.second) +
               print_col(col_and_val.first);
    }),
                     " + ");
}
};

/// operator<< : printing expression
template <typename Stream>
Stream &operator<<(Stream &o, const linear_expression &expr) {
    detail::print_expression(o, expr, detail::col_id_to_string);
    return o;
}

/// linear_expression + linear_expression operator.
inline linear_expression operator+(linear_expression expr_left,
                                   const linear_expression &expr_right) {
    expr_left += expr_right;
    return expr_left;
}

/// linear_expression - linear_expression operator.
inline linear_expression operator-(linear_expression expr_left,
                                   const linear_expression &expr_right) {
    expr_left -= expr_right;
    return expr_left;
}

/// linear_expression * double operator.
inline linear_expression operator*(linear_expression expr, double val) {
    expr *= val;
    return expr;
}

/// double * linear_expression operator.
inline linear_expression operator*(double val, const linear_expression &expr) {
    return expr * val;
}

/// linear_expression / double operator.
inline linear_expression operator/(linear_expression expr, double val) {
    expr /= val;
    return expr;
}

/// Unary - operator.
inline linear_expression operator-(const linear_expression &expr) {
    return expr * (-1.);
}

} // lp
} // paal

#endif // PAAL_EXPRESSIONS_HPP


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  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 MPI-IS 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: Andreas Orthey */

#include <ompl/base/spaces/SphereStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>
#include <boost/math/constants/constants.hpp>

using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

SphereStateSampler::SphereStateSampler(const StateSpace *space) : StateSampler(space)
{
}

void SphereStateSampler::sampleUniform(State *state)
{
    // see for example http://corysimon.github.io/articles/uniformdistn-on-sphere/
    double theta = 2.0 * pi * rng_.uniformReal(0, 1) - pi; //uniform in [-pi,+pi]
    double phi = acos(1.0 - 2.0 * rng_.uniformReal(0, 1));//in [0,+pi]
    SphereStateSpace::StateType *S = state->as<SphereStateSpace::StateType>();
    S->setThetaPhi(theta, phi);
}

void SphereStateSampler::sampleUniformNear(State *state, const State *near, double distance)
{
    SphereStateSpace::StateType *S = state->as<SphereStateSpace::StateType>();
    const SphereStateSpace::StateType *Snear = near->as<SphereStateSpace::StateType>();
    S->setTheta(rng_.uniformReal(Snear->getTheta() - distance, Snear->getTheta() + distance));
    S->setPhi(rng_.uniformReal(Snear->getPhi() - distance, Snear->getPhi() + distance));
    space_->enforceBounds(state);
}

void SphereStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
{
    SphereStateSpace::StateType *S = state->as<SphereStateSpace::StateType>();
    const SphereStateSpace::StateType *Smean = mean->as<SphereStateSpace::StateType>();
    S->setTheta(rng_.gaussian(Smean->getTheta(), stdDev));
    S->setPhi(rng_.gaussian(Smean->getPhi(), stdDev));
    space_->enforceBounds(state);
}

StateSamplerPtr SphereStateSpace::allocDefaultStateSampler() const
{
    return std::make_shared<SphereStateSampler>(this);
}

SphereStateSpace::SphereStateSpace(double radius):
  radius_(radius)
{
    setName("Sphere" + getName());
    type_ = STATE_SPACE_SPHERE;

    StateSpacePtr SO2(std::make_shared<SO2StateSpace>());
    StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
    R1->as<RealVectorStateSpace>()->setBounds(0, pi);

    addSubspace(SO2, 1.0);
    addSubspace(R1, 1.0);
    lock();
}

double SphereStateSpace::distance(const State *state1, const State *state2) const
{
    // https://en.wikipedia.org/wiki/Great-circle_distance#Formulae

    const SphereStateSpace::StateType *S1 = state1->as<SphereStateSpace::StateType>();
    const SphereStateSpace::StateType *S2 = state2->as<SphereStateSpace::StateType>();

    // Note: Formula assumes phi in [-pi/2,+pi/2]
    float t1 = S1->getTheta();
    float phi1 = S1->getPhi() - pi/2.0;

    float t2 = S2->getTheta();
    float phi2 = S2->getPhi() - pi/2.0;

    // This is the Vincenty formula, but it is less numerically stable
    // double dt = t2 - t1;
    // double d1 = powf(cos(phi2) * sin(dt), 2);
    // double d2 = powf(cos(phi1) * sin(phi2) - sin(phi1) * cos(phi2) * cos(dt), 2);
    // double numerator = sqrtf(d1 + d2);
    // double denumerator = sin(phi1) * sin(phi2) + cos(phi1) * cos(phi2) * cos(dt);
    // return radius_ * atan2(numerator, denumerator);

    // Haversine formula
    float s = 0.5*(phi1 - phi2);
    float t = 0.5*(t1 - t2);
    float d = sqrtf(sin(s)*sin(s) + cos(phi1)*cos(phi2)*sin(t)*sin(t));
    return 2*radius_*asin(d);

}

double SphereStateSpace::getMeasure() const
{
    return 4 * pi * radius_ * radius_;
}

State *SphereStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

Eigen::Vector3f SphereStateSpace::toVector(const State *state) const
{
    Eigen::Vector3f v;

    const SphereStateSpace::StateType *S1 = state->as<SphereStateSpace::StateType>();
    float theta = S1->getTheta();
    float phi = S1->getPhi();

    v[0] = radius_*sin(phi)*cos(theta);
    v[1] = radius_*sin(phi)*sin(theta);
    v[2] = radius_*cos(phi);

    return v;
}


#include "sigma.h"
#include "../wavelet/wavelet.h"
#include <armadillo>
#include <iostream>

rpm::vector<double> SIGMA::analyse(const rpm::vector<double>& lx, double fs)
{
    constexpr double fmin = 50;
    constexpr double fmax = 400;

    constexpr double Tmin = 1.0 / fmax;
    constexpr double Tmax = 1.0 / fmin;

    constexpr double oqmin = 0.1;
    constexpr double oqmax = 0.9;

    constexpr double gwlen = Tmin;
    constexpr double fwlen = 0.000;

    constexpr int nclust = 2;

    // Calculate SWT
    constexpr int nlev = 5;
    Wt::DiscreteWavelet wavelet(Wt::BIOR, 15);
    rpm::vector<double> swc = Analysis::swt(lx, wavelet, nlev, 0, true);

    // Calculate multiscale product
    int dlen = (int) Wt::swt_buffer_length((int) lx.size());
    rpm::vector<double> mp(dlen, 1.0);
    for (int k = 1; k * dlen < (int) swc.size(); ++k) {
        for (int i = 0; i < dlen; ++i) {
            mp[i] *= swc[k * dlen + i];
        }
    }

    // Find third roots
    rpm::vector<double> nmp(mp);
    rpm::vector<double> pmp(mp);
    rpm::vector<double> crnmp(mp.size());
    rpm::vector<double> crpmp(mp.size());
    for (int i = 0; i < (int) mp.size(); ++i) {
        // Half-wave rectify on negative half of mp for GCI
        if (nmp[i] > 0)
            nmp[i] = 0;

        // Half-wave rectify on positive half of mp for GOI
        if (pmp[i] < 0)
            pmp[i] = 0;

        crnmp[i] = cbrt(nmp[i]);
        crpmp[i] = cbrt(pmp[i]);
    }

    // Group delay evaluation on mp
    auto [gcic, nsew, ngrdel, ntoff] = xewgrdel(nmp, fs, gwlen, fwlen);
    ngrdel.insert(ngrdel.begin(), ntoff, 0.0);
    ngrdel.erase(std::prev(ngrdel.end(), ntoff), ngrdel.end());

    auto [goic, psew, pgrdel, ptoff] = xewgrdel(pmp, fs, gwlen, fwlen);
    pgrdel.insert(pgrdel.begin(), ptoff, 0.0);
    pgrdel.erase(std::prev(pgrdel.end(), ptoff), pgrdel.end());

    // Set up other variables
    rpm::vector<double> gci(lx.size(), 0.0);
    rpm::vector<double> goi(lx.size(), 0.0);

    // --- GCI Detection ---

    // Model GD slope
    const int nr = ((int) std::round(gwlen * fs)) / 2 - 1;
    const int mngrdellen = 2 * nr + 1;
    rpm::vector<double> mngrdel(mngrdellen);
    for (int i = 0; i < mngrdellen; ++i) {
        mngrdel[i] = i - nr;
    }
    rpm::vector<double> cmngrdel(ngrdel.size(), 0.0);

    const int snfv = (int) gcic.size();
    arma::mat nfv(3, snfv, arma::fill::zeros);

    for (int i = 0; i < snfv; ++i) {
        int lbnd = (int) std::round(gcic[i] - nr);
        int ubnd = lbnd + mngrdellen - 1;

        if (lbnd >= 0 && ubnd < (int) ngrdel.size()) {
            double sum = 0.0;
            double min = HUGE_VAL;
            double mean = 0.0;

            for (int k = lbnd; k <= ubnd; ++k) {
                sum += crnmp[k];
                if (crnmp[k] < min)
                    min = crnmp[k];

                double v = mngrdel[k - lbnd] - ngrdel[k];
                mean += v * v;

                cmngrdel[k] = mngrdel[k - lbnd];
            }

            nfv(0, i) = sum; // Sum of crnmp over GD window
            nfv(1, i) = min; // Peak value of crnmp
            nfv(2, i) = sqrt(mean / mngrdellen); // Phase slope deviation
        }
    }

    // Determine clusters
    arma::gmm_diag ngmm;
    bool status = ngmm.learn(nfv, 3, arma::eucl_dist, arma::random_subset, 10, 5, 1e-8, false);

    // Find cluster with lowest crnmp sum
    int I = (int) arma::index_min(ngmm.means.row(0));

    // If the candidate belongs to the chosen cluster then keep
    for (int i = 0; i < snfv; ++i) {
        if (ngmm.assign(nfv.col(i), arma::eucl_dist) == (arma::uword) I) {
            int k = std::max(0, std::min((int) gci.size() - 1, (int) std::round(gcic[i])));
            gci[k] = 1.0;
        }
    }

    // --- Post-filter swallows (GCIs only) ---

    if (gci.size() > 2) {
        // If a gci is separated from all others by more than Tmax, delete
        rpm::list<int> fgci;
        for (int i = 0; i < (int) gci.size(); ++i) {
            if (gci[i] > 0) {
                fgci.push_back(i);
            }
        }

        // Check first one
        auto i0 = fgci.begin();
        auto i1 = std::next(i0);
        if (*i1 - *i0 > Tmax * fs) {
            fgci.pop_front();
        }

        // Check the middle
        auto i2 = std::next(fgci.begin(), 2);
        auto iend = std::prev(fgci.end());

        while (i2 != iend) {
            auto i2prev = std::prev(i2);
            auto i2next = std::next(i2);

            if ((*i2 - *i2prev > Tmax * fs)
                    && (*i2next - *i2 > Tmax * fs)) {
                fgci.erase(i2);
            }

            i2 = i2next;
        }

        // Check last one
        auto iendprev = std::prev(iend);
        if (*iend - *iendprev > Tmax * fs) {
            fgci.pop_back();
        }

        // Convert back
        std::fill(gci.begin(), gci.end(), 0.0);
        for (auto t : fgci) {
            gci[t] = 1.0;
        }
    }

    return gci;

    // --- GOI detection ---

}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// 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__C1_HPP_
#define SMOOTH__C1_HPP_

#include <Eigen/Core>

#include <complex>

#include "internal/c1.hpp"
#include "internal/lie_group_base.hpp"
#include "internal/macro.hpp"
#include "lie_group.hpp"
#include "map.hpp"
#include "so2.hpp"

namespace smooth {

// \cond
template<typename Scalar>
class SO2;
// \endcond

/**
 * @brief Base class for C1 Lie group types.
 *
 * Memory layout
 * -------------
 *
 * - Group:    \f$ \mathbf{x} = [a, b] \f$
 * - Tangent:  \f$ \mathbf{a} = [s, \omega_z] \f$
 *
 * Constraints
 * -----------
 *
 * - Group:   \f$ a^2 + b^2 > 0 \f$
 * - Tangent: \f$ -\pi < \omega_z \leq \pi \f$
 *
 * Lie group matrix form
 * ---------------------
 *
 * \f[
 * \mathbf{X} =
 * \begin{bmatrix}
 *  b & -a \\
 *  a &  b
 * \end{bmatrix} \in \mathbb{R}^{2 \times 2}
 * \f]
 *
 *
 * Lie algebra matrix form
 * -----------------------
 *
 * \f[
 * \mathbf{a}^\wedge =
 * \begin{bmatrix}
 *   s & -\omega_z \\
 *  \omega_z &   s \\
 * \end{bmatrix} \in \mathbb{R}^{2 \times 2}
 * \f]
 */
template<typename _Derived>
class C1Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;

protected:
  C1Base() = default;

public:
  SMOOTH_INHERIT_TYPEDEFS;

  /**
   * @brief Rotation angle.
   */
  Scalar angle() const
  {
    using std::atan2;

    return atan2(
      static_cast<const _Derived &>(*this).coeffs().x(),
      static_cast<const _Derived &>(*this).coeffs().y());
  }

  /**
   * @brief Scaling.
   */
  Scalar scaling() const
  {
    using std::sqrt;

    return sqrt(
      static_cast<const _Derived &>(*this).coeffs().x()
        * static_cast<const _Derived &>(*this).coeffs().x()
      + static_cast<const _Derived &>(*this).coeffs().y()
          * static_cast<const _Derived &>(*this).coeffs().y());
  }

  /**
   * @brief Rotation.
   */
  SO2<Scalar> so2() const
  {
    return SO2<Scalar>(c1());  // it's normalized inside SO2
  }

  /**
   * @brief Complex number representation.
   */
  std::complex<Scalar> c1() const
  {
    return std::complex<Scalar>(
      static_cast<const _Derived &>(*this).coeffs().y(),
      static_cast<const _Derived &>(*this).coeffs().x());
  }

  /**
   * @brief Rotation and scaling action on 2D vector.
   */
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 2, 1> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return Base::matrix() * v;
  }
};

// \cond
template<typename _Scalar>
class C1;
// \endcond

// \cond
template<typename _Scalar>
struct liebase_info<C1<_Scalar>>
{
  static constexpr bool is_mutable = true;

  using Impl   = C1Impl<_Scalar>;
  using Scalar = _Scalar;

  template<typename NewScalar>
  using PlainObject = C1<NewScalar>;
};
// \endcond

/**
 * @brief Storage implementation of C1 Lie group.
 *
 * @see C1Base for memory layout.
 */
template<typename _Scalar>
class C1 : public C1Base<C1<_Scalar>>
{
  using Base = C1Base<C1<_Scalar>>;
  SMOOTH_GROUP_API(C1);

public:
  /**
   * @brief Construct from scaling and angle.
   *
   * @param scaling strictly greater than zero.
   * @param angle angle of rotation (radians).
   */
  C1(const Scalar & scaling, const Scalar & angle)
  {
    using std::cos, std::sin;

    coeffs_.x() = scaling * sin(angle);
    coeffs_.y() = scaling * cos(angle);
  }

  /**
   * @brief Construct from complex number.
   *
   * @param c complex number.
   */
  C1(const std::complex<Scalar> & c)
  {
    coeffs_.x() = c.imag();
    coeffs_.y() = c.real();
  }
};

// \cond
template<typename _Scalar>
struct liebase_info<Map<C1<_Scalar>>> : public liebase_info<C1<_Scalar>>
{};
// \endcond

/**
 * @brief Memory mapping of C1 Lie group.
 *
 * @see C1Base for memory layout.
 */
template<typename _Scalar>
class Map<C1<_Scalar>> : public C1Base<Map<C1<_Scalar>>>
{
  using Base = C1Base<Map<C1<_Scalar>>>;

  SMOOTH_MAP_API();
};

// \cond
template<typename _Scalar>
struct liebase_info<Map<const C1<_Scalar>>> : public liebase_info<C1<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond

/**
 * @brief Const memory mapping of C1 Lie group.
 *
 * @see C1Base for memory layout.
 */
template<typename _Scalar>
class Map<const C1<_Scalar>> : public C1Base<Map<const C1<_Scalar>>>
{
  using Base = C1Base<Map<const C1<_Scalar>>>;

  SMOOTH_CONST_MAP_API();
};

using C1f = C1<float>;   ///< C1 with float scalar representation
using C1d = C1<double>;  ///< C1 with double scalar representation

}  // namespace smooth

#endif  // SMOOTH__C1_HPP_


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2009 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

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

/*! \file ranluxuniformrng.hpp
    \brief "Luxury" random number generator.
*/

#ifndef quantlib_ranlux_uniform_rng_h
#define quantlib_ranlux_uniform_rng_h

#include <ql/methods/montecarlo/sample.hpp>
#include <boost/random/ranlux.hpp>

namespace QuantLib {

    //! Uniform random number generator
    /*! M. Luescher's "luxury" random number generator

        Implementation is a proxy for the corresponding boost random
        number generator. For more detail see the boost documentation and:
          M.Luescher, A portable high-quality random number generator for
          lattice field theory simulations, Comp. Phys. Comm. 79 (1994) 100

        Available luxury levels:
        Ranlux3: Any theoretically possible correlations have very small change
                 of being observed.
        Ranlux4: highest possible luxury.
    */
    class Ranlux3UniformRng {
      public:
        typedef Sample<Real> sample_type;

        explicit Ranlux3UniformRng(Size seed = 19780503U)
        : ranlux3_(boost::random::ranlux64_base_01(seed)) {}

        sample_type next() const { return {ranlux3_(), 1.0}; }

      private:
        mutable boost::ranlux64_3_01 ranlux3_;
    };

    class Ranlux4UniformRng {
      public:
        typedef Sample<Real> sample_type;

        explicit Ranlux4UniformRng(Size seed = 19780503U)
        : ranlux4_(boost::random::ranlux64_base_01(seed)) {}

        sample_type next() const { return {ranlux4_(), 1.0}; }

      private:
        mutable boost::ranlux64_4_01 ranlux4_;
    };
}

#endif