/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     belief_prop.cpp
* \author   Collin Johnson
*
* Definition of functions for running belief propagation:
*
*   - belief_propagation
*   - loopy_belief_propagation
*/

#include <hssh/local_topological/area_detection/labeling/belief_prop.h>
#include <hssh/local_topological/area_detection/labeling/factor_graph.h>
#include <boost/range/iterator_range.hpp>
#include <cassert>

namespace vulcan
{
namespace hssh
{

int run_to_convergence(FactorGraph::VarIter beginVars,
                       FactorGraph::VarIter endVars,
                       FactorGraph::FactorIter beginFactors,
                       FactorGraph::FactorIter endFactors,
                       UpdateType type,
                       int maxIters = 0);

void belief_propagation(FactorGraph& graph, UpdateType type)
{
    assert(graph.sizeVars() > 0);

    // Use the first variable as the root
    int totalMessages = 0;

    // Initialize with all leaves sending their messages
    for(auto& fact : boost::make_iterator_range(graph.beginFactors(), graph.endFactors()))
    {
        if(fact->isLeaf())
        {
            int numSent = fact->sendMarginals(type, UpdateRule::force);
            assert(numSent == 1);   // only one marginal can be sent by a leaf
            totalMessages += numSent;
        }
    }

    for(auto& var : boost::make_iterator_range(graph.beginVars() + 1, graph.endVars()))
    {
        if(var->isLeaf())
        {
            int numSent = var->sendMarginals(true);
            assert(numSent == 1);   // only one marginal can be sent by a leaf
            totalMessages += numSent;
        }
    }

    // Run a single iteration
    totalMessages += run_to_convergence(graph.beginVars() + 1,
                                        graph.endVars(),
                                        graph.beginFactors(),
                                        graph.endFactors(),
                                        type);

    // Now the root node is ready, so send that message
    assert((*graph.beginVars())->freshCount() == (*graph.beginVars())->degree());
    totalMessages += (*graph.beginVars())->sendMarginals();

    // Run the algorithm to completion
    totalMessages += run_to_convergence(graph.beginVars() + 1,
                                        graph.endVars(),
                                        graph.beginFactors(),
                                        graph.endFactors(),
                                        type);

    std::cout << "Completed belief propagation sum-product using a total of " << totalMessages << " messages.\n";
}

void loopy_belief_propagation(FactorGraph& graph, UpdateType type)
{
    for(auto& e : boost::make_iterator_range(graph.beginEdges(), graph.endEdges()))
    {
        e->setChangeThreshold(1e-4);
        e->setMessagesToUnity();
    }

//     // Initialize with all leaves sending their messages
//     for(auto& fact : boost::make_iterator_range(graph.beginFactors(), graph.endFactors()))
//     {
//         if(fact->isLeaf())
//         {
//             int numSent = fact->sendMarginals(true);
//             assert(numSent == 1);   // only one marginal can be sent by a leaf
//         }
//     }
//
//     for(auto& var : boost::make_iterator_range(graph.beginVars() + 1, graph.endVars()))
//     {
//         if(var->isLeaf())
//         {
//             int numSent = var->sendMarginals(true);
//             assert(numSent == 1);   // only one marginal can be sent by a leaf
//         }
//     }

    int totalMessages = 0;
    totalMessages = run_to_convergence(graph.beginVars(),
                                       graph.endVars(),
                                       graph.beginFactors(),
                                       graph.endFactors(),
                                       type,
                                       20);

    std::cout << "Completed loopy belief propagation sum-product using a total of " << totalMessages << " messages.\n";
}

int run_to_convergence(FactorGraph::VarIter beginVars,
                       FactorGraph::VarIter endVars,
                       FactorGraph::FactorIter beginFactors,
                       FactorGraph::FactorIter endFactors,
                       UpdateType type,
                       int maxIters)
{
    // Iterate until there are no remaining fresh entities
    int iteration = 1;
    int totalMessages = 0;
    int numSent = 1;
    while((numSent > 0) && ((maxIters == 0) || (iteration < maxIters)))
    {
        numSent = 0;

        for(auto& fact : boost::make_iterator_range(beginFactors, endFactors))
        {
            if(fact->freshCount() > 0)
            {
                numSent += fact->sendMarginals(type, UpdateRule::if_needed);
            }
        }

        for(auto& var : boost::make_iterator_range(beginVars, endVars))
        {
            if(var->freshCount() > 0)
            {
                numSent += var->sendMarginals();
            }
        }

        std::cout << "Sent " << numSent << " messages on iteration " << iteration << '\n';
        ++iteration;
        totalMessages += numSent;
    }

    return totalMessages;
}

} // namespace hssh
} // namespace vulcan


/*
 * PolynomialSplineVectorSpaceCurve.hpp
 *
 *  Created on: Mar 6, 2015
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include <string>
#include <vector>
#include <Eigen/Core>
#include <glog/logging.h>

#include "curves/Curve.hpp"
#include "curves/VectorSpaceCurve.hpp"
#include "curves/PolynomialSplineContainer.hpp"

namespace curves {

template<typename SplineType, int N>
class PolynomialSplineVectorSpaceCurve : public VectorSpaceCurve<N>
{
 public:
  typedef VectorSpaceCurve<N> Parent;
  typedef typename Parent::ValueType ValueType;
  typedef typename Parent::DerivativeType DerivativeType;

  PolynomialSplineVectorSpaceCurve()
      : VectorSpaceCurve<N>(),
        minTime_(0)
  {
    containers_.resize(N);
  }

  virtual ~PolynomialSplineVectorSpaceCurve()
  {
  }

  virtual void print(const std::string& str = "") const
  {
  }

  virtual Time getMinTime() const
  {
    return minTime_;
  }

  virtual Time getMaxTime() const
  {
    return containers_.at(0).getContainerDuration();
  }

  virtual bool evaluate(ValueType& value, Time time) const
  {
    for (size_t i = 0; i < N; ++i) {
      value(i) = containers_.at(i).getPositionAtTime(time);
    }
    return true;
  }

  virtual bool evaluateDerivative(DerivativeType& value, Time time, unsigned derivativeOrder) const
  {

    for (size_t i = 0; i < N; ++i) {
      if (derivativeOrder == 1) {
        value(i) = containers_.at(i).getVelocityAtTime(time);
      }
      else if (derivativeOrder == 2) {
        value(i) = containers_.at(i).getAccelerationAtTime(time);
      }
      else {
        return false;
      }
    }
    return true;
  }

  virtual void extend(const std::vector<Time>& times, const std::vector<ValueType>& values,
                      std::vector<Key>* outKeys)
  {
    throw std::runtime_error("PolynomialSplineVectorSpaceCurve::extend is not yet implemented!");
  }

  virtual void fitCurve(const std::vector<Time>& times, const std::vector<ValueType>& values,
                        std::vector<Key>* outKeys = NULL)
  {
    minTime_ = times.front();
    for (size_t i = 0; i < N; ++i) {
      std::vector<double> scalarValues;
      scalarValues.reserve(times.size());
      for (size_t t = 0; t < times.size(); ++t) scalarValues.push_back(values.at(t)(i));
      containers_.at(i).setData(times, scalarValues, 0.0, 0.0, 0.0, 0.0);
    }
  }

  virtual void fitCurve(const std::vector<Time>& times,
                        const std::vector<ValueType>& values,
                        const DerivativeType& initialVelocity,
                        const DerivativeType& initialAcceleration,
                        const DerivativeType& finalVelocity,
                        const DerivativeType& finalAcceleration)
  {
    minTime_ = times.front();
    for (size_t i = 0; i < N; ++i) {
      std::vector<double> scalarValues;
      scalarValues.reserve(times.size());
      for (size_t t = 0; t < times.size(); ++t) scalarValues.push_back(values.at(t)(i));
      containers_.at(i).setData(times, scalarValues, initialVelocity(i), initialAcceleration(i),
                                finalVelocity(i), finalAcceleration(i));
    }
  }

  virtual void fitCurve(const std::vector<Time>& times, const std::vector<ValueType>& values,
                        const std::vector<DerivativeType>& firstDerivatives,
                        const std::vector<DerivativeType>& secondDerivatives,
                        std::vector<Key>* outKeys = NULL)
  {
    minTime_ = times.front();
    for (size_t i = 0; i < N; ++i) {
      std::vector<double> scalarValues, scalarFirstDerivates, scalarSecondDerivates;
      scalarValues.reserve(times.size());
      for (size_t t = 0; t < times.size(); ++t) {
        scalarValues.push_back(values.at(t)(i));
        scalarFirstDerivates.push_back(firstDerivatives.at(t)(i));
        scalarSecondDerivates.push_back(secondDerivatives.at(t)(i));
      }
      // TODO Copy all derivates, right now only first and last are supported.
      containers_.at(i).setData(times, scalarValues,
                                *(scalarFirstDerivates.begin()), *(scalarSecondDerivates.begin()),
                                *(scalarFirstDerivates.end() - 1), *(scalarSecondDerivates.end() - 1));
    }
  }

  virtual void fitCurve(const std::vector<SplineOptions>& values,
                        std::vector<Key>* outKeys = NULL)
  {
    // TODO
    throw std::runtime_error("PolynomialSplineVectorSpaceCurve::fitCurve is not yet implemented!");
  }

  virtual void clear()
  {
    for (size_t i = 0; i < N; ++i) {
      containers_.at(i).reset();
    }
  }

  virtual void transformCurve(const ValueType T)
  {
    CHECK(false) << "Not implemented";
  }

 private:
  std::vector<PolynomialSplineContainer> containers_;
  Time minTime_;
};

typedef PolynomialSplineVectorSpaceCurve<PolynomialSplineQuintic, 3> PolynomialSplineQuinticVector3Curve;

} // namespace


#pragma once

#include <controllib/uorb/blocks.hpp>
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
#include <lib/geo/geo.h>

#ifdef USE_MATRIX_LIB
#include "matrix/src/Matrix.hpp"
using namespace matrix;
#else
#include <Eigen/Eigen>
using namespace Eigen;
#endif

// uORB Subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>

// uORB Publications
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <uORB/topics/estimator_status.h>

#define CBRK_NO_VISION_KEY	328754

using namespace control;

enum fault_t {
	FAULT_NONE = 0,
	FAULT_MINOR,
	FAULT_SEVERE
};

enum sensor_t {
	SENSOR_BARO = 0,
	SENSOR_GPS,
	SENSOR_LIDAR,
	SENSOR_FLOW,
	SENSOR_SONAR,
	SENSOR_VISION,
	SENSOR_MOCAP
};

class BlockLocalPositionEstimator : public control::SuperBlock
{
//
// The purpose of this estimator is to provide a robust solution for
// indoor flight.
//
// dynamics:
//
//	x(+) = A * x(-) + B * u(+)
//	y_i = C_i*x
//
// kalman filter
//
//	E[xx'] = P
//	E[uu'] = W
//	E[y_iy_i'] = R_i
//
//	prediction
//		x(+|-) = A*x(-|-) + B*u(+)
//		P(+|-) = A*P(-|-)*A' + B*W*B'
//
//	correction
//		x(+|+) =  x(+|-) + K_i * (y_i - H_i * x(+|-) )
//
//
// input:
// 	ax, ay, az (acceleration NED)
//
// states:
// 	px, py, pz , ( position NED)
// 	vx, vy, vz ( vel NED),
// 	bx, by, bz ( TODO accelerometer bias)
// 	tz (TODO terrain altitude)
//
// measurements:
//
// 	sonar: pz (measured d*cos(phi)*cos(theta))
//
// 	baro: pz
//
// 	flow: vx, vy (flow is in body x, y frame)
//
// 	gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// 	lidar: px (actual measured d*cos(phi)*cos(theta))
//
// 	vision: px, py, pz, vx, vy, vz
//
// 	mocap: px, py, pz
//
public:
	BlockLocalPositionEstimator();
	void update();
	virtual ~BlockLocalPositionEstimator();

private:
	// prevent copy and assignment
	BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
	BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);

	// constants
	static const uint8_t n_x = 9;
	static const uint8_t n_u = 3; // 3 accelerations
	static const uint8_t n_y_flow = 2;
	static const uint8_t n_y_sonar = 1;
	static const uint8_t n_y_baro = 1;
	static const uint8_t n_y_lidar = 1;
	static const uint8_t n_y_gps = 6;
	static const uint8_t n_y_vision = 3;
	static const uint8_t n_y_mocap = 3;
	enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz};
	enum {U_ax = 0, U_ay, U_az};
	enum {Y_baro_z = 0};
	enum {Y_lidar_z = 0};
	enum {Y_flow_x = 0, Y_flow_y};
	enum {Y_sonar_z = 0};
	enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz};
	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz};
	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z};
	enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM};

	// methods
	// ----------------------------
	void initP();

	// predict the next state
	void predict();

	// correct the state prediction wtih a measurement
	void correctBaro();
	void correctGps();
	void correctLidar();
	void correctFlow();
	void correctSonar();
	void correctVision();
	void correctmocap();

	// sensor initialization
	void updateHome();
	void initBaro();
	void initGps();
	void initLidar();
	void initSonar();
	void initFlow();
	void initVision();
	void initmocap();

	// publications
	void publishLocalPos();
	void publishGlobalPos();
	void publishFilteredFlow();
	void publishEstimatorStatus();

	// attributes
	// ----------------------------

	// subscriptions
	uORB::Subscription<vehicle_status_s> _sub_status;
	uORB::Subscription<actuator_armed_s> _sub_armed;
	uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
	uORB::Subscription<vehicle_attitude_s> _sub_att;
	uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
	uORB::Subscription<optical_flow_s> _sub_flow;
	uORB::Subscription<sensor_combined_s> _sub_sensor;
	uORB::Subscription<distance_sensor_s> _sub_distance;
	uORB::Subscription<parameter_update_s> _sub_param_update;
	uORB::Subscription<manual_control_setpoint_s> _sub_manual;
	uORB::Subscription<home_position_s> _sub_home;
	uORB::Subscription<vehicle_gps_position_s> _sub_gps;
	uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
	uORB::Subscription<att_pos_mocap_s> _sub_mocap;

	// publications
	uORB::Publication<vehicle_local_position_s> _pub_lpos;
	uORB::Publication<vehicle_global_position_s> _pub_gpos;
	uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
	uORB::Publication<estimator_status_s> _pub_est_status;

	// map projection
	struct map_projection_reference_s _map_ref;

	// parameters
	BlockParamInt  _integrate;

	BlockParamFloat  _flow_xy_stddev;
	BlockParamFloat  _sonar_z_stddev;

	BlockParamFloat  _lidar_z_stddev;

	BlockParamFloat  _accel_xy_stddev;
	BlockParamFloat  _accel_z_stddev;

	BlockParamFloat  _baro_stddev;

	BlockParamFloat  _gps_xy_stddev;
	BlockParamFloat  _gps_z_stddev;

	BlockParamFloat  _gps_vxy_stddev;
	BlockParamFloat  _gps_vz_stddev;

	BlockParamFloat  _gps_eph_max;

	BlockParamFloat  _vision_xy_stddev;
	BlockParamFloat  _vision_z_stddev;
	BlockParamInt    _no_vision;
	BlockParamFloat  _beta_max;

	BlockParamFloat  _mocap_p_stddev;

	// process noise
	BlockParamFloat  _pn_p_noise_power;
	BlockParamFloat  _pn_v_noise_power;
	BlockParamFloat  _pn_b_noise_power;

	// misc
	struct pollfd _polls[3];
	uint64_t _timeStamp;
	uint64_t _time_last_xy;
	uint64_t _time_last_flow;
	uint64_t _time_last_baro;
	uint64_t _time_last_gps;
	uint64_t _time_last_lidar;
	uint64_t _time_last_sonar;
	uint64_t _time_last_vision_p;
	uint64_t _time_last_mocap;
	int 	 _mavlink_fd;

	// initialization flags
	bool _baroInitialized;
	bool _gpsInitialized;
	bool _lidarInitialized;
	bool _sonarInitialized;
	bool _flowInitialized;
	bool _visionInitialized;
	bool _mocapInitialized;

	// init counts
	int _baroInitCount;
	int _gpsInitCount;
	int _lidarInitCount;
	int _sonarInitCount;
	int _flowInitCount;
	int _visionInitCount;
	int _mocapInitCount;

	// reference altitudes
	float _altHome;
	bool _altHomeInitialized;
	float _baroAltHome;
	float _gpsAltHome;
	float _lidarAltHome;
	float _sonarAltHome;
	float _flowAltHome;
	Vector3f _visionHome;
	Vector3f _mocapHome;

	// flow integration
	float _flowX;
	float _flowY;
	float _flowMeanQual;

	// referene lat/lon
	double _gpsLatHome;
	double _gpsLonHome;

	// status
	bool _canEstimateXY;
	bool _canEstimateZ;
	bool _xyTimeout;

	// sensor faults
	fault_t _baroFault;
	fault_t _gpsFault;
	fault_t _lidarFault;
	fault_t _flowFault;
	fault_t _sonarFault;
	fault_t _visionFault;
	fault_t _mocapFault;

	bool _visionTimeout;
	bool _mocapTimeout;

	perf_counter_t _loop_perf;
	perf_counter_t _interval_perf;
	perf_counter_t _err_perf;

	// state space
	Matrix<float, n_x, 1>  _x; // state vector
	Matrix<float, n_u, 1>  _u; // input vector
	Matrix<float, n_x, n_x>  _P; // state covariance matrix
};


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_POW2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_POW2_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/is_finite.hpp>
#endif
#include <boost/simd/function/fast.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( pow2_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1 const& a1) const
    {
      return fast_(ldexp)(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
    #ifndef BOOST_SIMD_NO_INVALIDS
      BOOST_ASSERT_MSG(boost::simd::assert_all(is_finite(a1)),
                       "pow2 is not defined for an invalid second parameter");
    #endif
      return fast_(ldexp)(a0, bs::toint(a1));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow2_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::integer_<A0> >
                          , bd::generic_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return fast_(ldexp)(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return fast_(ldexp)(One<A0>(), a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( pow2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const  BOOST_NOEXCEPT
    {
    #ifndef BOOST_SIMD_NO_INVALIDS
      BOOST_ASSERT_MSG(boost::simd::assert_all(is_finite(a0)),
                       "pow2 with one parameter is not defined for an invalid entry");
    #endif
      return fast_(ldexp)(One<A0>(), toint(a0));
    }
  };
} } }

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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_TOOLBOX_EXPONENTIAL_FUNCTION_SIMD_COMMON_CBRT_HPP_INCLUDED
#define NT2_TOOLBOX_EXPONENTIAL_FUNCTION_SIMD_COMMON_CBRT_HPP_INCLUDED
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/sdk/simd/meta/is_real_convertible.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/is_gez.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/is_inf.hpp>
#include <nt2/include/functions/frexp.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/select.hpp>
#include <nt2/include/functions/fast_ldexp.hpp>
#include <nt2/include/functions/bitofsign.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/negate.hpp>
#include <nt2/include/functions/remquo.hpp>
#include <nt2/include/functions/tofloat.hpp>
#include <nt2/toolbox/polynomials/function/scalar/impl/horner.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::cbrt_, tag::cpu_,
                       (A0)(X),
                       ((simd_<arithmetic_<A0>,X>))
                      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::cbrt_(tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :  meta::as_real<A0>{};

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::cbrt(tofloat(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::cbrt_, tag::cpu_,
                       (A0)(X),
                       ((simd_<double_<A0>,X>))
                      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::cbrt_(tag::simd_<tag::double_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :  meta::as_real<A0>{};

    NT2_FUNCTOR_CALL(1)
    {
      const A0 z =  nt2::abs(a0);
      const A0 CBRT2  = double_constant< A0, 0x3ff428a2f98d728bll> ();
      const A0 CBRT4  = double_constant< A0, 0x3ff965fea53d6e3dll> ();
      const A0 CBRT2I = double_constant< A0, 0x3fe965fea53d6e3dll> ();
      const A0 CBRT4I = double_constant< A0, 0x3fe428a2f98d728bll> ();
      typedef typename meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<A0>::type stype;
      int_type e;
      A0  x;
      boost::fusion::tie(x, e) = frexp(z);
      x = horner < NT2_HORNER_COEFF_T(stype, 5,
                            (0xbfc13c93386fdff6ll,
                             0x3fe17e1fc7e59d58ll,
                             0xbfee8a4ca3ba37b8ll,
                             0x3ff23d6ee505873all,
                             0x3fd9c0c12122a4fell)
                            ) > (x);
      const int_type flag = is_gez(e);
      int_type e1 =  nt2::abs(e);
      int_type rem = e1;
      e1 = rdiv(e1, Three<int_type>());           //TO DO remquo
      rem = sub(rem, mul(e1, Three<int_type>()));
      e =  negate(e1, e);

      const A0 cbrt2 = sel(flag, CBRT2, CBRT2I);
      const A0 cbrt4 = sel(flag, CBRT4, CBRT4I);
      A0 fact = sel(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
      fact = sel(is_equal(rem, Two<int_type>()), cbrt4, fact);
      x = fast_ldexp(x*fact, e);
      x = x-(x-z/sqr(x))*Third<A0>();
      x = x-(x-z/sqr(x))*Third<A0>(); //two newton passes
      return sel(b_or(is_eqz(a0),is_inf(a0)), a0, b_or(x, bitofsign(a0)));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::cbrt_, tag::cpu_,
                       (A0)(X),
                       ((simd_<float_<A0>,X>))
                      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::cbrt_(tag::simd_<tag::float_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :  meta::as_real<A0>{};

    NT2_FUNCTOR_CALL(1)
    {
      const A0 z =  nt2::abs(a0);
      const A0 CBRT2  = single_constant< A0, 0x3fa14518> ();
      const A0 CBRT4  = single_constant< A0, 0x3fcb2ff5> ();
      const A0 CBRT2I = single_constant< A0, 0x3f4b2ff5> ();
      const A0 CBRT4I = single_constant< A0, 0x3f214518> ();
      typedef typename meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<A0>::type stype;
      int_type e;
      A0  x;
      boost::fusion::tie(x, e) = frexp(z);
      x = horner < NT2_HORNER_COEFF_T(stype, 5,
                            (0xbe09e49a,
                             0x3f0bf0fe,
                             0xbf745265,
                             0x3f91eb77,
                             0x3ece0609)
                            ) > (x);
      const int_type flag = is_gez(e);
      int_type e1 =  nt2::abs(e);
      int_type rem = e1;
       e1 = e1/Three<int_type>();           //TO DO remquo
       rem = rem-e1*Three<int_type>();
//       int_type rem;

//       boost::fusion::tie(rem, e1) = remquo(e1,Three<int_type>());
//       std::cout << " e1 " << e1 << std::endl;
//       std::cout << " rem " << rem << std::endl;
      e =  negate(e1, e);
//       std::cout << " e " << &e << std::endl;

      const A0 cbrt2 = sel(flag, CBRT2, CBRT2I);
      const A0 cbrt4 = sel(flag, CBRT4, CBRT4I);
      A0 fact = sel(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
//       std::cout << " fact " << fact << std::endl;
      fact = sel(is_equal(rem, Two<int_type>()), cbrt4, fact);
//       std::cout << " fact " << fact << std::endl;
      x = fast_ldexp(x*fact, e);
//       std::cout << " 2--x " << x << std::endl;

      x = x-(x-z/sqr(x))*Third<A0>();
      return sel( b_or(is_eqz(a0),is_inf(a0))
                  , a0
                  , b_or(x, bitofsign(a0))
                  );
    }
  };
} }

#endif
// modified by jt the 05/01/2011


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FAST_LDEXP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FAST_LDEXP_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-ieee
    Function object implementing fast_ldexp capabilities

    Computes fast_ldexp value of its parameter.

  **/
  const boost::dispatch::functor<tag::fast_ldexp_> fast_ldexp = {};
} }
#endif

#include <boost/simd/function/scalar/fast_ldexp.hpp>
#include <boost/simd/function/simd/fast_ldexp.hpp>

#endif