/*
# Copyright 2018 HyphaROS Workshop.
# Latest Modifier: HaoChih, LIN (hypha.ros@gmail.com)
# Original Author: ChanYuan KUO & YoRu LU
#
# 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 <iostream>
#include <map>
#include <math.h>
#include "ros/ros.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <visualization_msgs/Marker.h>

#include <Eigen/Core>
#include <Eigen/QR>
#include <vector>

#include <string>
#include <fstream>

using namespace std;
using std::string;

/********************/
/* CLASS DEFINITION */
/********************/
class PurePursuit
{
    public:
        PurePursuit();
        void initMarker();
        bool isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose);
        bool isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos);
        double getYawFromPose(const geometry_msgs::Pose& carPose);

        double polyeval(Eigen::VectorXd coeffs, double x);
        Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);

        double getCar2GoalDist();

	double getW(double _alpha);

        double get_alpha(const geometry_msgs::Pose& carPose);

    private:

        ros::NodeHandle n_;
        ros::Subscriber odom_sub, path_sub, goal_sub, amcl_sub;
        ros::Publisher ackermann_pub, cmdvel_pub, marker_pub;
        ros::Timer timer1, timer2;
        tf::TransformListener tf_listener;

        ros::Time tracking_stime;
        ros::Time tracking_etime;
        ros::Time tracking_time;
        int tracking_time_sec;
        int tracking_time_nsec;

        //time flag
        bool start_timef = false;
        bool end_timef = false;

        std::ofstream file;

        double _waypointsDist = -1.0;
        double _pathLength = 8.0;
        bool _path_computed = false;

        string _map_frame, _odom_frame, _car_frame;

        unsigned int idx = 0;

        visualization_msgs::Marker points, line_strip, goal_circle;
        geometry_msgs::Point odom_goal_pos, goal_pos;
        geometry_msgs::Twist cmd_vel;
        ackermann_msgs::AckermannDriveStamped ackermann_cmd;
        nav_msgs::Odometry odom;
        nav_msgs::Path map_path, odom_path, odom_path_w, _odom_path;

        int _downSampling;

        double L, Lfw, Vcmd, lfw, steering, velocity;
        double w;
        double steering_gain, max_w, base_angle, goal_radius, speed_incremental;
        int controller_freq;
        bool foundForwardPt, goal_received, goal_reached, cmd_vel_mode, debug_mode, smooth_accel;

        void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);
        void pathCB(const nav_msgs::Path::ConstPtr& pathMsg);
        void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg);
        void amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg);
        void controlLoopCB(const ros::TimerEvent&);

}; // end of class

PurePursuit::PurePursuit()
{
    //Private parameters handler
    ros::NodeHandle pn("~");

    //Car parameter
    pn.param("L", L, 0.26); // length of car
    pn.param("Vcmd", Vcmd, 1.0);// reference speed (m/s)
    pn.param("Lfw", Lfw, 0.5); // forward look ahead distance (m)
    pn.param("lfw", lfw, 0.13); // distance between front the center of car

    //Controller parameter
    pn.param("controller_freq", controller_freq, 20);
    pn.param("steering_gain", steering_gain, 1.0);
    pn.param("max_w", max_w, 1.0);
    pn.param("goal_radius", goal_radius, 0.5); // goal radius (m)
    pn.param("base_angle", base_angle, 0.0); // neutral point of servo (rad)
    pn.param("cmd_vel_mode", cmd_vel_mode, false); // whether or not publishing cmd_vel
    pn.param("debug_mode", debug_mode, false); // debug mode
    pn.param("smooth_accel", smooth_accel, true); // smooth the acceleration of car
    pn.param("speed_incremental", speed_incremental, 0.5); // speed incremental value (discrete acceleraton), unit: m/s

    //Parameter for topics & Frame name
    pn.param<std::string>("map_frame", _map_frame, "map" ); //*****for mpc, "odom"
    pn.param<std::string>("odom_frame", _odom_frame, "odom");
    pn.param<std::string>("car_frame", _car_frame, "base_footprint" );

    //Publishers and Subscribers
    odom_sub = n_.subscribe("/odom", 1, &PurePursuit::odomCB, this);
    //path_sub = n_.subscribe("/pure_pursuit/global_planner", 1, &PurePursuit::pathCB, this);
    //goal_sub = n_.subscribe("/pure_pursuit/goal", 1, &PurePursuit::goalCB, this);
    path_sub = n_.subscribe("/move_base/TrajectoryPlannerROS/global_plan", 1, &PurePursuit::pathCB, this);
    goal_sub = n_.subscribe("/move_base_simple/goal", 1, &PurePursuit::goalCB, this);
    amcl_sub = n_.subscribe("/amcl_pose", 5, &PurePursuit::amclCB, this);
    marker_pub = n_.advertise<visualization_msgs::Marker>("/pure_pursuit/path_marker", 10);
    ackermann_pub = n_.advertise<ackermann_msgs::AckermannDriveStamped>("/pure_pursuit/ackermann_cmd", 1);
    if(cmd_vel_mode) cmdvel_pub = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

    //Timer
    timer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &PurePursuit::controlLoopCB, this); // Duration(0.05) -> 20Hz

    //Init variables
    foundForwardPt = false;
    goal_received = false;
    goal_reached = false;
    velocity = 0.0;
    w = 0.0;
    steering = base_angle;

    idx = 0;
    file.open("/home/nscl1016/catkin_ws/src/mpc_ros/pure_pursuit.csv");

    //Show info
    ROS_INFO("[param] base_angle: %f", base_angle);
    ROS_INFO("[param] Vcmd: %f", Vcmd);
    ROS_INFO("[param] Lfw: %f", Lfw);

    //Visualization Marker Settings
    initMarker();

    cmd_vel = geometry_msgs::Twist();
    ackermann_cmd = ackermann_msgs::AckermannDriveStamped();

}

void PurePursuit::initMarker()
{
    points.header.frame_id = line_strip.header.frame_id = goal_circle.header.frame_id = "odom";
    points.ns = line_strip.ns = goal_circle.ns = "Markers";
    points.action = line_strip.action = goal_circle.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = line_strip.pose.orientation.w = goal_circle.pose.orientation.w = 1.0;
    points.id = 0;
    line_strip.id = 1;
    goal_circle.id = 2;

    points.type = visualization_msgs::Marker::POINTS;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    goal_circle.type = visualization_msgs::Marker::CYLINDER;
    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    //LINE_STRIP markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;

    goal_circle.scale.x = goal_radius;
    goal_circle.scale.y = goal_radius;
    goal_circle.scale.z = 0.1;

    // Points are green
    points.color.g = 1.0f;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    //goal_circle is yellow
    goal_circle.color.r = 1.0;
    goal_circle.color.g = 1.0;
    goal_circle.color.b = 0.0;
    goal_circle.color.a = 0.5;
}

void PurePursuit::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    this->odom = *odomMsg;
}

void PurePursuit::pathCB(const nav_msgs::Path::ConstPtr& pathMsg) //jaewan
{
    this->map_path = *pathMsg;

    if(goal_received && !goal_reached)
    {
        nav_msgs::Path odom_path = nav_msgs::Path();
        try
        {
            double total_length = 0.0;
            int sampling = _downSampling;

            //find waypoints distance
            if(_waypointsDist <=0.0)
            {
                double dx = pathMsg->poses[1].pose.position.x - pathMsg->poses[0].pose.position.x;
                double dy = pathMsg->poses[1].pose.position.y - pathMsg->poses[0].pose.position.y;
                _waypointsDist = sqrt(dx*dx + dy*dy);
                _downSampling = int(_pathLength/10.0/_waypointsDist);
            }

            // Cut and downsampling the path
            for(int i =0; i< pathMsg->poses.size(); i++)
            {
                if(total_length > _pathLength)
                    break;

                if(sampling == _downSampling)
                {
                    geometry_msgs::PoseStamped tempPose;
                    tf_listener.transformPose(_odom_frame, ros::Time(0) , pathMsg->poses[i], _map_frame, tempPose);
                    odom_path.poses.push_back(tempPose);
                    sampling = 0;
                }
                total_length = total_length + _waypointsDist;
                sampling = sampling + 1;
            }

            if(odom_path.poses.size() >= 6 )
            {
                _odom_path = odom_path; // Path waypoints in odom frame
                _path_computed = true;
                // publish odom path
                odom_path.header.frame_id = _odom_frame;
                odom_path.header.stamp = ros::Time::now();
            }
            else
            {
                cout << "Failed to path generation" << endl;
                _waypointsDist = -1;
            }
            //DEBUG
            //cout << endl << "N: " << odom_path.poses.size() << endl
            //<<  "Car path[0]: " << odom_path.poses[0];
            // << ", path[N]: " << _odom_path.poses[_odom_path.poses.size()-1] << endl;
        }
        catch(tf::TransformException &ex)
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
        }
    }

}

void PurePursuit::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg)
{
    this->goal_pos = goalMsg->pose.position;
    try
    {
        geometry_msgs::PoseStamped odom_goal;
        tf_listener.transformPose("odom", ros::Time(0) , *goalMsg, "map" ,odom_goal);
        odom_goal_pos = odom_goal.pose.position;
        goal_received = true;
        goal_reached = false;

        /*Draw Goal on RVIZ*/
        goal_circle.pose = odom_goal.pose;
        marker_pub.publish(goal_circle);
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
}

double PurePursuit::getYawFromPose(const geometry_msgs::Pose& carPose)
{
    float x = carPose.orientation.x;
    float y = carPose.orientation.y;
    float z = carPose.orientation.z;
    float w = carPose.orientation.w;

    double tmp,yaw;
    tf::Quaternion q(x,y,z,w);
    tf::Matrix3x3 quaternion(q);
    quaternion.getRPY(tmp,tmp, yaw);

    return yaw;
}

bool PurePursuit::isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose)
{
    float car2wayPt_x = wayPt.x - carPose.position.x;
    float car2wayPt_y = wayPt.y - carPose.position.y;
    double car_theta = getYawFromPose(carPose);

    float car_car2wayPt_x = cos(car_theta)*car2wayPt_x + sin(car_theta)*car2wayPt_y;
    float car_car2wayPt_y = -sin(car_theta)*car2wayPt_x + cos(car_theta)*car2wayPt_y;

    if(car_car2wayPt_x >0) /*is Forward WayPt*/
        return true;
    else
        return false;
}

bool PurePursuit::isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos)
{
    double dx = wayPt.x - car_pos.x;
    double dy = wayPt.y - car_pos.y;
    double dist = sqrt(dx*dx + dy*dy);

    if(dist < Lfw)
        return false;
    else if(dist >= Lfw)
        return true;
}

double PurePursuit::get_alpha(const geometry_msgs::Pose& carPose)
{
    geometry_msgs::Point carPose_pos = carPose.position;
    double carPose_yaw = getYawFromPose(carPose);
    geometry_msgs::Point odom_path_wayPt;
    geometry_msgs::Point forwardPt;
    geometry_msgs::Point odom_car2WayPtVec;
    foundForwardPt = false;

    if(!goal_reached){

        for(int i =0; i< _odom_path.poses.size(); i++)
        {
            geometry_msgs::PoseStamped map_path_pose = _odom_path.poses[i];
            geometry_msgs::PoseStamped odom_path_pose;

            try
            {
                tf_listener.transformPose("odom", ros::Time(0) , map_path_pose, "map" ,odom_path_pose);
                odom_path_wayPt = odom_path_pose.pose.position;
                bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt,carPose);

                if(_isForwardWayPt)
                {
                    bool _isWayPtAwayFromLfwDist = isWayPtAwayFromLfwDist(odom_path_wayPt,carPose_pos);
                    if(_isWayPtAwayFromLfwDist)
                    {
                        forwardPt = odom_path_wayPt;
                        foundForwardPt = true;
                        break;
                    }
                }
            }
            catch(tf::TransformException &ex)
            {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }
        }

    }
    else if(goal_reached)
    {
        forwardPt = odom_goal_pos;
        foundForwardPt = false;
        //ROS_INFO("goal REACHED!");
    }

    /*Visualized Target Point on RVIZ*/
    /*Clear former target point Marker*/
    points.points.clear();
    line_strip.points.clear();

    if(foundForwardPt && !goal_reached)
    {
        points.points.push_back(carPose_pos);
        points.points.push_back(odom_path_wayPt);
        line_strip.points.push_back(carPose_pos);
        line_strip.points.push_back(odom_path_wayPt);
    }

    marker_pub.publish(points);
    marker_pub.publish(line_strip);

    double alpha_t = atan2(odom_path_wayPt.y - carPose_pos.y,odom_path_wayPt.x - carPose_pos.x) - carPose_yaw;

    return alpha_t;
}

double PurePursuit::getCar2GoalDist()
{
    geometry_msgs::Point car_pose = this->odom.pose.pose.position;
    double car2goal_x = this->odom_goal_pos.x - car_pose.x;
    double car2goal_y = this->odom_goal_pos.y - car_pose.y;

    return sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
}

double PurePursuit::getW(double _alpha)
{
    return 2*sin(_alpha)/Lfw;
}

double PurePursuit::polyeval(Eigen::VectorXd coeffs, double x)
{
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); i++)
    {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

Eigen::VectorXd PurePursuit::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order)
{
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::MatrixXd A(xvals.size(), order + 1);

    for (int i = 0; i < xvals.size(); i++)
        A(i, 0) = 1.0;

    for (int j = 0; j < xvals.size(); j++)
    {
        for (int i = 0; i < order; i++)
            A(j, i + 1) = A(j, i) * xvals(j);
    }

    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}
void PurePursuit::amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg)
{

    if(this->goal_received)
    {
        double car2goal_x = this->goal_pos.x - amclMsg->pose.pose.position.x;
        double car2goal_y = this->goal_pos.y - amclMsg->pose.pose.position.y;
        double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
        if(dist2goal < this->goal_radius)
        {
            if(start_timef)
            {
                tracking_etime = ros::Time::now();
                tracking_time_sec = tracking_etime.sec - tracking_stime.sec;
                tracking_time_nsec = tracking_etime.nsec - tracking_stime.nsec;

                file << "tracking time"<< "," << tracking_time_sec << "," <<  tracking_time_nsec << "\n";

                file.close();

                start_timef = false;
            }
            this->goal_reached = true;
            this->goal_received = false;
            this->_path_computed = false;

            ROS_INFO("Goal Reached !");
            cout << "tracking time: " << tracking_time_sec << "." << tracking_time_nsec << endl;

        }
    }
}

void PurePursuit::controlLoopCB(const ros::TimerEvent&)
{

    geometry_msgs::Pose carPose = this->odom.pose.pose;
    geometry_msgs::Twist carVel = this->odom.twist.twist;

    tf::Pose pose;
    tf::poseMsgToTF(carPose, pose);
    double alpha = tf::getYaw(pose.getRotation());

    if(this->goal_received && !this->goal_reached && this->_path_computed)
    {
        if(!start_timef)
        {
            tracking_stime == ros::Time::now();

            start_timef = true;
        }

        double alpha = get_alpha(carPose);

	    this->w = this->base_angle + getW(alpha)*this->steering_gain;

        if(this->w > max_w)
        {
            this->w = max_w;
        }
        else if(this->w < -max_w)
        {
            this->w = -max_w;
        }

        nav_msgs::Odometry odom_w = odom;
        nav_msgs::Path odom_path_w = _odom_path;

        // Update system states: X=[x, y, theta, v]
        const double px = odom_w.pose.pose.position.x; //pose: odom frame
        const double py = odom_w.pose.pose.position.y;
        tf::Pose pose;
        tf::poseMsgToTF(odom_w.pose.pose, pose);
        const double theta = tf::getYaw(pose.getRotation());

        // Waypoints related parameters
        const int N = odom_path_w.poses.size(); // Number of waypoints
        const double costheta = cos(theta);
        const double sintheta = sin(theta);

        // Convert to the vehicle coordinate system
        Eigen::VectorXd x_veh(N);
        Eigen::VectorXd y_veh(N);
        for(int i = 0; i < N; i++)
        {
            const double dx = odom_path_w.poses[i].pose.position.x - px;
            const double dy = odom_path_w.poses[i].pose.position.y - py;
            x_veh[i] = dx * costheta + dy * sintheta;
            y_veh[i] = dy * costheta - dx * sintheta;
        }

        // Fit waypoints
        auto coeffs = polyfit(x_veh, y_veh, 3);

        const double cte  = polyeval(coeffs, 0.0);
        const double etheta = atan(coeffs[1]);

        cout << "cte: " << cte << endl;
        cout << "etheta: " << etheta << endl;
        cout << "v : " << cmd_vel.linear.x  << endl;
        cout << "w : " << cmd_vel.angular.z  << endl;

        idx++;
        file << idx<< "," << cte << "," <<  etheta << "," << cmd_vel.linear.x << "," << cmd_vel.angular.z << "\n";

            /*Estimate Gas Input*/
        if(!this->goal_reached)
        {
            if(this->smooth_accel)
                this->velocity = std::min(this->velocity + this->speed_incremental, this->Vcmd);
            else
                this->velocity = this->Vcmd;
            if(debug_mode) ROS_INFO("Velocity = %.2f, w = %.2f", this->velocity, this->w);
        }

    }

    else
    {
        this->velocity = 0.0;
		this->w = 0;
    }

    if(this->cmd_vel_mode)
    {
        this->cmd_vel.linear.x = this->velocity;
        this->cmd_vel.angular.z = this->w;
	    this->cmdvel_pub.publish(this->cmd_vel);
    }
}

/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "PurePursuit");
    PurePursuit controller;
    ros::AsyncSpinner spinner(2); // Use multi threads
    spinner.start();
    ros::waitForShutdown();
    return 0;
}


#include <iostream>
#include <ros/ros.h>
#include <vector>
#include <math.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h> //for sensor_msgs::PointCloud2
#include <geometry_msgs/TransformStamped.h>

#include <pcl/point_types.h> // for pcl::PointXYZ
#include <pcl_conversions/pcl_conversions.h> // for pcl::fromROSMsg()

#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
// #include <pcl_ros/transforms.hpp>

#include <pcl/segmentation/sac_segmentation.h> //for SACSegmentation
#include <pcl/filters/extract_indices.h> //for ExtractIndices
#include <pcl/filters/statistical_outlier_removal.h>

#include <tf2_ros/transform_listener.h>
#include <tf/transform_listener.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <Eigen/Dense>

ros::Publisher org_low_frq_pub;
ros::Publisher processed_pcl_pub;

// with PointCloud2Ptr not working, but with PointCloud2::ConstPtr it works
// void sync_callback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pcl_in, const nav_msgs::Odometry::ConstPtr &pose_in)

void sync_callback(const  sensor_msgs::PointCloud2::ConstPtr &pcl_in, const nav_msgs::Odometry::ConstPtr &pose_in)
{
  // Solve all of perception here...
  ROS_INFO("\n\nrcvd msg ... ");

  ROS_INFO_STREAM("pcl  time stamp: "<< pcl_in->header.stamp);
  ROS_INFO_STREAM("pose time stamp: "<< pose_in->header.stamp);

  //convert the input into a pcl::PointCloud< pcl::PointXYZ> object
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);

  // variables  for filtered/transformed/low_frq  pcl
  pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*pcl_in, *cloudPtr);

  //*********** pcl to std vector ***********
  pcl::PointCloud<pcl::PointXYZI> org_pcl, fr_pcl, sph_pcl, cart_pcl;
  std::copy(cloudPtr->points.begin(), cloudPtr->points.end(),std::back_inserter(org_pcl));

  // ROS_INFO_STREAM("hight: "<< pcl_in->height);
  // ROS_INFO_STREAM("width: "<< pcl_in->width);
  // ROS_INFO_STREAM("point_step: "<< pcl_in->point_step);
  // ROS_INFO_STREAM("row_step: "<< pcl_in->row_step);
  // ROS_INFO_STREAM("cloudPtr [0]: "<< cloudPtr->points.size());
  // ROS_INFO_STREAM("org_pcl: "<< org_pcl.size() );
  // ROS_INFO_STREAM("org_pcl : 1st pt: "<<org_pcl[0].x<<", "<< org_pcl[0].y<<", "<<org_pcl[0].z<<", "<<org_pcl[0].intensity);

  //*********** clustering ***********
  // next step

  //*********** from cartesian to spherical ***********
  // dist = np.sqrt(x**2 + y**2 + z**2)
  // theta = np.arctan2(y, x)
  // alpha = np.arccos(z / dist)

  std::vector<pcl::PointXYZI> sph_pcl_vec, cart_pcl_vec;
  float rds = 5.0, rds1= 5.5; // surface radius
  for(auto pt: org_pcl){
    // ROS_INFO_STREAM("pt: " << pt.x);
    float dst = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z );
    float th = atan2(pt.y , pt.x );
    float al = acos(pt.z / dst );
    pcl::PointXYZI shp_pt;
    shp_pt.x = th;
    shp_pt.y = al;
    shp_pt.z = rds;
    shp_pt.intensity = dst;
    sph_pcl_vec.push_back(shp_pt);

    float x = rds1 * sin(al) * cos(th);
    float y = rds1 * sin(al) * sin(th);
    float z = rds1 * cos(al);
    pcl::PointXYZI cart_pt;
    cart_pt.x = x;
    cart_pt.y = y;
    cart_pt.z = z;
    cart_pt.intensity = dst;
    cart_pcl_vec.push_back(cart_pt);
  }
  ROS_INFO_STREAM("sph_pcl_vec: "<< sph_pcl_vec.size() );
  ROS_INFO_STREAM("cart_pcl_vec: "<< cart_pcl_vec.size() );

  //*********** std vector to pcl ***********
  std::copy(sph_pcl_vec.begin(), sph_pcl_vec.end(),std::back_inserter(sph_pcl));
  sph_pcl.header.frame_id = "velodyne";

  std::copy(cart_pcl_vec.begin(), cart_pcl_vec.end(),std::back_inserter(cart_pcl));
  cart_pcl.header.frame_id = "velodyne";

  ROS_INFO_STREAM("sph_pcl: "<< sph_pcl.size());
  ROS_INFO_STREAM("cart_pcl: "<< cart_pcl.size());

  //*********** from velodyne to world frame ***********
  // Eigen::Quaternionf rotation(pose_in->pose.pose.orientation.w, pose_in->pose.pose.orientation.x, pose_in->pose.pose.orientation.y,
                              // pose_in->pose.pose.orientation.z);
  // Eigen::Vector3f origin(pose_in->pose.pose.position.x, pose_in->pose.pose.position.y, pose_in->pose.pose.position.z+0.322);

  // Eigen::Quaternionf rotation(1,0,0,0);
  // Eigen::Vector3f origin(0,0,0);

  // transformPointCloud(org_pcl, *transformed_cloudPtr, origin, rotation);
  // transformed_cloudPtr->header.frame_id = "world"; //should be after transform otherwise aha
  // transformed_cloudPtr->header.frame_id = "velodyne"; //should be after transform otherwise aha

  // ROS_INFO_STREAM("transformed_cloudPtr: "<< transformed_cloudPtr->points.size());
  // ROS_INFO_STREAM("transformed_cloudPtr: "<< transformed_cloudPtr->header.frame_id);

//*********** from pcl to ros msg ***********
  sensor_msgs::PointCloud2 sph_pcl_msg;
  pcl::toROSMsg(sph_pcl, sph_pcl_msg);
  sph_pcl_msg.header.stamp = pcl_in->header.stamp; //.toSec();
  processed_pcl_pub.publish(sph_pcl_msg);

  // ROS_INFO_STREAM("hight: "<< sph_pcl_msg.height);
  // ROS_INFO_STREAM("width: "<< sph_pcl_msg.width);
  // ROS_INFO_STREAM("point_step: "<< sph_pcl_msg.point_step);
  // ROS_INFO_STREAM("row_step: "<< sph_pcl_msg.row_step);

  sensor_msgs::PointCloud2 cart_pcl_msg;
  pcl::toROSMsg(cart_pcl, cart_pcl_msg);
  sph_pcl_msg.header.stamp = pcl_in->header.stamp;//.toSec();
  org_low_frq_pub.publish(cart_pcl_msg);
  ros::Duration(0.3).sleep();

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pcl_surface_projection");
  ROS_INFO("pcl_surface_projection node ... ");

  ros::NodeHandle nh;

  //create publisher object
  processed_pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/pcl_in_wrld_fr", 1);
  org_low_frq_pub = nh.advertise<sensor_msgs::PointCloud2>("/org_low_frq_pcl", 1);

  // message filter time synchronization
  message_filters::Subscriber<nav_msgs::Odometry> pose_sub(nh, "/ground_truth/state", 1);
  message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh, "/mid/points", 1);
  message_filters::TimeSynchronizer<sensor_msgs::PointCloud2, nav_msgs::Odometry> sync(pcl_sub, pose_sub, 10);

  sync.registerCallback(boost::bind(&sync_callback, _1, _2)); // try std instead boost not working as well
  ros::spin();

  return 0;
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_LINE_SEARCH_H_
#define TURI_LINE_SEARCH_H_

// Types
#include <core/data/flexible_type/flexible_type.hpp>

// Optimization
#include <ml/optimization/optimization_interface.hpp>
#include <ml/optimization/regularizer_interface.hpp>
#include <Eigen/Core>
#include <core/logging/assertions.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
// 1. Feature: Armijo cubic interpolation line search.
// 2. Feature: Wolfe cubic interpolation line search.
// 3. Optimization: Add accepted function value to the ls_return structure
//    (reduces 1 func eval per iteration)
//

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup line_search Line Search
 * \{
 */

/** "Zoom" phase for More and Thuente line seach.
 *
 * \note Applicable for smooth functions only.
 *
 * This code is a C++ port of Jorge Nocedal's implementaiton of More and
 * Thuente [2] line search. This code was availiable at
 * http://www.ece.northwestern.edu/~nocedal/lbfgs.html
 *
 * Nocedal's Condition for Use: This software is freely available for
 * educational or commercial purposes. We expect that all publications
 * describing work using this software quote at least one of the references
 * given below. This software is released under the BSD License
 *
 * The purpose of cstep is to compute a safeguarded step for a linesearch and
 * to update an interval of uncertainty for a minimizer of the function.
 *
 * The parameter stx contains the step with the least function value. The
 * parameter stp contains the current step. It is assumed that the derivative
 * at stx is negative in the direction of the step. If brackt is set true then
 * a minimizer has been bracketed in an interval of uncertainty with endpoints
 * stx and sty.
 *
 *
 * \param[in] stx  Best step obtained so far.
 * \param[in] fx   Function value at the best step so far.
 * \param[in] dx   Directional derivative at the best step so far.
 * \param[in] sty  Step at the end point of the uncertainty interval.
 * \param[in] fy   Function value at end point of uncertainty interval.
 * \param[in] dy   Directional derivative at end point of uncertainty interval.
 * \param[in] stp  Current step.
 * \param[in] fp   Function value at current step.
 * \param[in] dp   Directional derivative at the current step.
 * \param[in] brackt Has the minimizer has been bracketed?
 * \param[in] stpmin Min step size
 * \param[in] stpmax Max step size.
 *
 *
 */
inline bool cstep(double &stx, double &fx, double &dx,
           double &sty, double &fy, double &dy,
           double &stp, double &fp,  double &dp,
           bool &brackt, double stpmin, double stpmax){

  const double p66 = 0.66;
  bool info = false;

  // Check the input parameters for errors.
  // This should not happen.
  if ((brackt && (stp <= std::min(stx,sty) || stp >= std::max(stx,sty)))
      || (dx*(stp-stx) >= LS_ZERO) || (stpmax < stpmin)){
     return info;
  }

  // Determine if the derivatives have opposite sign.
  double sgnd = dp*(dx/std::abs(dx));

  // Local variables
  bool bound;
  double theta, s, gamma, p, q, r, stpc, stpq, stpf;

  // First case.
  // -------------------------------------------------------------------------
  // A higher function value. The minimum is bracketed. If the cubic step is
  // closer to stx than the quadratic step, the cubic step is taken, else the
  // average of the cubic and quadratic steps is taken.
  if (fp > fx){

    info = true;
    bound = true;
    theta = 3*(fx - fp)/(stp - stx) + dx + dp;
    s = std::max(std::abs(theta), std::max(std::abs(dx),std::abs(dp)));
    gamma = s*sqrt(pow(theta/s,2) - (dx/s)*(dp/s));

    if (stp < stx)
     gamma = -gamma;

    p = (gamma - dx) + theta;
    q = ((gamma - dx) + gamma) + dp;
    r = p/q;
    stpc = stx + r*(stp - stx);
    stpq = stx + ((dx/((fx-fp)/(stp-stx)+dx))/2)*(stp - stx);

    if (std::abs(stpc-stx) < std::abs(stpq-stx)){
       stpf = stpc;
    } else{
      stpf = stpc + (stpq - stpc)/2;
    }

    brackt = true;

  }
  // Second case.
  // -------------------------------------------------------------------------
  // A lower function value and derivatives of opposite sign. The
  // minimum is bracketed. If the cubic step is closer to stx than the
  // quadratic (secant) step, the cubic step is taken, else the quadratic step
  // is taken.
  else if(sgnd <= 0){

    info = true;
    bound = false;

    theta = 3*(fx - fp)/(stp - stx) + dx + dp;
    s = std::max(std::abs(theta), std::max(std::abs(dx),std::abs(dp)));
    gamma = s*sqrt(pow(theta/s,2) - (dx/s)*(dp/s));

    if (stp > stx)
       gamma = -gamma;

    p = (gamma - dp) + theta;
    q = ((gamma - dp) + gamma) + dx;
    r = p/q;
    stpc = stp + r*(stx - stp);
    stpq = stp + (dp/(dp-dx))*(stx - stp);
    if (std::abs(stpc-stp) > std::abs(stpq-stp)){
       stpf = stpc;
    } else {
       stpf = stpq;
    }
    brackt = true;
  }
  // Third case.
  // -------------------------------------------------------------------------
  // A lower function value, derivatives of the same sign, and the
  // magnitude of the derivative decreases.  The cubic step is only used if the
  // cubic tends to infinity in the direction of the step or if the minimum of
  // the cubic is beyond stp. Otherwise the cubic step is defined to be either
  // stpmin or stpmax. The quadratic (secant) step is also computed and if the
  // minimum is bracketed then the the step closest to stx is taken, else the
  // step farthest away is taken.
  else if (std::abs(dp) < std::abs(dx)){

    info = true;
    bound = true;
    theta = 3*(fx - fp)/(stp - stx) + dx + dp;
    s = std::max(std::abs(theta), std::max(std::abs(dx),std::abs(dp)));

    // The case gamma = 0 only arises if the cubic does not tend to infinity
    // in the direction of the step.

    gamma = s*sqrt(std::max(0., pow((theta/s),2) - (dx/s)*(dp/s)));
    if (stp > stx){
        gamma = -gamma;
    }

    p = (gamma - dp) + theta;
    q = (gamma + (dx - dp)) + gamma;
    r = p/q;
    if((r < 0.0) && (std::abs(gamma) <= OPTIMIZATION_ZERO)){
       stpc = stp + r*(stx - stp);
    }else if (stp > stx){
       stpc = stpmax;
    }else{
       stpc = stpmin;
    }

    stpq = stp + (dp/(dp-dx))*(stx - stp);
    if (brackt){
       if (std::abs(stp-stpc) < std::abs(stp-stpq)){
          stpf = stpc;
       }else{
          stpf = stpq;
       }
    }else{
       if (std::abs(stp-stpc) > std::abs(stp-stpq)){
          stpf = stpc;
       }else{
          stpf = stpq;
       }
    }

  }
  // Fourth case.
  // -------------------------------------------------------------------------
  // A lower function value, derivatives of the same sign, and the magnitude of
  // the derivative does not decrease. If the minimum is not bracketed, the
  // step is either stpmin or stpmax, else the cubic step is taken.
  else{
     info = true;
     bound = false;
     if (brackt){
        theta = 3*(fp - fy)/(sty - stp) + dy + dp;
        s = std::max(std::abs(theta), std::max(std::abs(dy),std::abs(dp)));
        gamma = s*sqrt(pow(theta/s,2) - (dy/s)*(dp/s));

        if (stp > sty){
            gamma = -gamma;
        }
        p = (gamma - dp) + theta;
        q = ((gamma - dp) + gamma) + dy;
        r = p/q;
        stpc = stp + r*(sty - stp);
        stpf = stpc;
     }else if (stp > stx){
        stpf = stpmax;
     }else {
        stpf = stpmin;
     }
  }

  // Update the interval of uncertainty.
  // This update does not depend on the new step or the case analysis above.
  if (fp > fx){
     sty = stp;
     fy = fp;
     dy = dp;
  }else{

     if (sgnd < 0.0){
        sty = stx;
        fy = fx;
        dy = dx;
     }
     stx = stp;
     fx = fp;
     dx = dp;
  }

  // Compute the new step and safeguard it.
  stpf = std::min(stpmax,stpf);
  stpf = std::max(stpmin,stpf);
  stp = stpf;
  if (brackt && bound){
     if (sty > stx){
        stp = std::min(stx+p66*(sty-stx),stp);
     }else{
        stp = std::max(stx+p66*(sty-stx),stp);
     }
  }

  // cstep-failed
  if (info == false){
     logprogress_stream << "Warning:"
                        << " Unable to interpolate step size intervals."
                        << std::endl;
  }
  return info;

}

/**
 *
 * Compute step sizes for line search methods to satisfy Strong Wolfe conditions.
 *
 * \note Applicable for smooth functions only.
 *
 * This code is a C++ port of Jorge Nocedal's implementaiton of More and
 * Thuente [2] line search. This code was availiable at
 * http://www.ece.northwestern.edu/~nocedal/lbfgs.html
 *
 *   Line search based on More' and Thuente [1] to find a step which satisfies
 *   a Wolfe conditions for sufficient decrease condition and a curvature
 *   condition.
 *
 *   At each stage the function updates an interval of uncertainty with
 *   endpoints. The interval of uncertainty is initially chosen so that it
 *   contains a minimizer of the modified function
 *
 *        f(x+stp*s) - f(x) - ftol*stp*(gradf(x)'s).
 *
 *   If a step is obtained for which the modified function has a nonpositive
 *   function value and nonnegative derivative, then the interval of
 *   uncertainty is chosen so that it contains a minimizer of f(x+stp*s).
 *
 *   The algorithm is designed to find a step which satisfies the sufficient
 *   decrease condition
 *         f(x+stp*s) .le. f(x) + ftol*stp*(gradf(x)'s),          [W1]
 *   and the curvature condition
 *         std::abs(gradf(x+stp*s)'s)) .le. gtol*std::abs(gradf(x)'s).      [W2]
 *
 *  References:
 *
 *  (1) More, J. J. and D. J. Thuente. "Line Search Algorithms with
 *  Guaranteed Sufficient Decrease." ACM Transactions on Mathematical Software
 *  20, no. 3 (1994): 286-307.
 *
 * (2) Wright S.J  and J. Nocedal. Numerical optimization. Vol. 2.
 *                         New York: Springer, 1999.
 * \param[in] model Any model with a first order optimization interface.
 * \param[in] init_step Initial step size
 * \param[in] point Starting point for the solver.
 * \param[in] gradient Gradient value at this point (saves computation)
 * \param[in]    reg   Shared ptr to an interface to a smooth regularizer.
 * \returns stats Line searnorm ch return object
 *
*/
template <typename Vector>
inline ls_return more_thuente(
    first_order_opt_interface& model,
    double init_step,
    double init_func_value,
    DenseVector point,
    Vector gradient,
    DenseVector direction,
    double function_scaling = 1.0,
    const std::shared_ptr<smooth_regularizer_interface> reg=NULL,
    size_t max_function_evaluations = LS_MAX_ITER){

    // Initialize the return object
    ls_return stats;

    // Input checking: Initial step size can't be zero.
    if (init_step <= LS_ZERO){
      logprogress_stream << " Error:"
                         <<" \nInitial step step less than "<< LS_ZERO
                         << "." << std::endl;
      return stats;
    }

    // Check that the initia direction is a descent direction.
    // This can only occur of your gradients were computed incorrectly
    // or the problem is non-convex.
    DenseVector x0 = point;
    double Dphi0 = gradient.dot(direction);
    if ( (init_step <= 0) | (Dphi0 >= OPTIMIZATION_ZERO) ){
      logprogress_stream << " Error: Search direction is not a descent direction."
                          <<" \nDetected numerical difficulties." << std::endl;
    }

    // Initializing local variables
    // stx, fx, dgx: Values of the step, function, and derivative at the best
    //               step.
    //
    // sty, fy, dgy: Values of the step, function, and derivative at the other
    //               endpoint of the interval of uncertainty.
    //
    // st, f, dg   : Values of the step, function, and derivative at current
    //               step
    //
    // g           : Gradient w.r.t x and step (vector) at the current point
    //
    double stx = LS_ZERO;
    double fx = init_func_value;
    double dgx = Dphi0;                   // Derivative of f(x + s d) w.r.t s

    double sty = LS_ZERO;
    double fy = init_func_value;
    double dgy = Dphi0;

    double stp = init_step;
    double f = init_func_value;
    double dg = Dphi0;
    Vector g = gradient;
    DenseVector reg_gradient(gradient.size());

    // Interval [stmax, stmin] of uncertainty
    double stmax = LS_ZERO;
    double stmin = LS_MAX_STEP_SIZE;

    // Flags and local variables
    bool brackt = false;                 // Bracket or Zoon phase?
    bool stage1 = true;

    double wolfe_func_dec  = LS_C1*Dphi0;         // Wolfe condition [W1]
    double wolfe_curvature = LS_C2*Dphi0;         // Wolfe condition [W2]
    double width = stmax - stmin;
    double width2 = 2*width;

    // Constants used in this code.
    // (Based on http://www.ece.northwestern.edu/~nocedal/lbfgs.html)
    const double p5 = 0.5;
    const double p66 = 0.66;
    const double xtrapf = 4;
    bool infoc = true;                            // Zoom status

    // Start searching
    while (true){

      // Set the min and max steps based on the current interval of uncertainty.
      if (brackt == true){
        stmin = std::min(stx,sty);
        stmax = std::max(stx,sty);
      }else{
        stmin = stx;
        stmax = stp + xtrapf*(stp - stx);
      }

      // Force the step to be within the bounds
      stp = std::max(stp,LS_ZERO);
      stp = std::min(stp,LS_MAX_STEP_SIZE);

      // If an unusual termination is to occur then let 'stp' be the lowest point
      // obtained so far.
      if ( (infoc == false)
          || (brackt && (stmax-stmin <= LS_ZERO))){

         logprogress_stream << "Warning:"
            << " Unusual termination criterion reached."
            << "\nReturning the best step found so far."
            << " This typically happens when the number of features is much"
            << " larger than the number of training samples. Consider pruning"
            << " features manually or increasing the regularization value."
            << std::endl;
         stp = stx;
      }

      // Reached func evaluation limit -- return the best one so far.
      if (size_t(stats.func_evals) >= max_function_evaluations){
        stats.step_size = stx;
        stats.status = true;
        return stats;
      }

      // Evaluate the function and gradient at stp and compute the directional
      // derivative.
      point = x0 + stp * direction;
      model.compute_first_order_statistics(point, g, f);
      stats.num_passes++;
      stats.func_evals++;
      stats.gradient_evals++;
      if (reg != NULL){
        reg->compute_gradient(point, reg_gradient);
        f += reg->compute_function_value(point);
        g += reg_gradient;
      }

      if(function_scaling != 1.0) {
        f *= function_scaling;
        g *= function_scaling;
      }

      dg = g.dot(direction);

      double ftest = init_func_value + stp*wolfe_func_dec;

      // Termination checking
      // Note: There are many good checks and balances used in Nocedal's code
      // Some of them are overly defensive and should not happen.

      // Rounding errors
      if ( (brackt && ((stp <= stmin) || (stp >= stmax))) || (infoc == false)){
          logprogress_stream << "Warning: Rounding errors"
            << " prevent further progress. \nThere may not be a step which"
            << " satisfies the sufficient decrease and curvature conditions."
            << " \nTolerances may be too small or dataset may be poorly scaled."
            << " This typically happens when the number of features is much"
            << " larger than the number of training samples. Consider pruning"
            << " features manually or increasing the regularization value."
            << std::endl;
          stats.step_size = stp;
          stats.status = false;
          return stats;
      }

      // Step is more than LS_MAX_STEP_SIZE
      if ((stp >= LS_MAX_STEP_SIZE) && (f <= ftest) && (dg <= wolfe_func_dec)){
        logprogress_stream << "Warning: Reached max step size."
                           << std::endl;
        stats.step_size = stp;
        stats.status = true;
        return stats;
      }

      // Step is smaller than LS_ZERO
      if ((stp <= LS_ZERO) && ((f > ftest) || (dg >= wolfe_func_dec))){
        logprogress_stream << "Error: Reached min step size."
                           << " Cannot proceed anymore."
                           << std::endl;
        stats.step_size = stp;
        stats.status = false;
        return stats;
      }

      // Relative width of the interval of uncertainty is reached.
      if (brackt && (stmax-stmin <= LS_ZERO)){
        logprogress_stream << "Error: \nInterval of uncertainty"
                           << "lower than step size limit." << std::endl;
        stats.status = false;
        return stats;
      }

      // Wolfe conditions W1 and W2 are satisfied! Woo!
      if ((f <= ftest) && (std::abs(dg) <= -wolfe_curvature)){
        stats.step_size = stp;
        stats.status = true;
        return stats;
      }

      // Stage 1 is a search for steps for which the modified function has
      // a nonpositive value and nonnegative derivative.
      if ( stage1 && (f <= ftest) && (dg >= wolfe_curvature)){
             stage1 = false;
      }

      // A modified function is used to predict the step only if we have not
      // obtained a step for which the modified function has a nonpositive
      // function value and nonnegative derivative, and if a lower function
      // value has been  obtained but the decrease is not sufficient.

      if (stage1 && (f <= fx) && (f > ftest)){

         // Define the modified function and derivative values.
         double fm = f - stp*wolfe_func_dec;
         double fxm = fx - stx*wolfe_func_dec;
         double fym = fy - sty*wolfe_func_dec;
         double dgm = dg - wolfe_func_dec;
         double dgxm = dgx - wolfe_func_dec;
         double dgym = dgy - wolfe_func_dec;

         // Call cstep to update the interval of uncertainty and to compute the
         // new step.
         infoc = cstep(stx,fxm, dgxm,
                       sty, fym, dgym,
                       stp, fm, dgm,
                       brackt,
                       stmin,stmax);

         // Reset the function and gradient values for f.
         fx = fxm + stx*wolfe_func_dec;
         fy = fym + sty*wolfe_func_dec;
         dgx = dgxm + wolfe_func_dec;
         dgy = dgym + wolfe_func_dec;

      }else{

         // Call cstep to update the interval of uncertainty and to compute the
         // new step.
         infoc = cstep(stx,fx, dgx,
                       sty, fy, dgy,
                       stp, f, dg,
                       brackt,
                       stmin,stmax);

      }

      // Force a sufficient decrease in the size of the interval of uncertainty.
      if (brackt){
         if (std::abs(sty-stx) >= p66*width2){
           stp = stx + p5*(sty - stx);
         }

         width2 = width;
         width = std::abs(sty-stx);
      }

    } // end-of-while

    return stats;

}

/**
 *
 * Armijo backtracking to compute step sizes for line search methods.
 *
 * \note Applicable for smooth functions only.
 *
 * Line search based on an backtracking strategy to ensure sufficient
 * decrease in function values at each iteration.
 *
 * References:
 * (1) Wright S.J  and J. Nocedal. Numerical optimization. Vol. 2.
 *                         New York: Springer, 1999.
 *
 * \param[in] model Any model with a first order optimization interface.
 * \param[in] init_step Initial step size.
 * \param[in] init_func Initial function value.
 * \param[in] point     Starting point for the solver.
 * \param[in] gradient  Gradient value at this point.
 * \param[in] direction Direction of the next step .
 *
 * \returns stats Line searnorm ch return object
 *
*/
template <typename Vector>
inline ls_return armijo_backtracking(
    first_order_opt_interface& model,
    double init_step,
    double init_func_value,
    DenseVector point,
    Vector gradient,
    DenseVector direction){

    // Step 1: Initialize the function
    // ------------------------------------------------------------------------
    // Min function decrease according to Armijo conditions.
    // The choice of constants are based on Nocedal and Wright [1].
    double sufficient_decrease = LS_C1*(gradient.dot(direction));
    ls_return stats;
    double step_size = init_step;
    DenseVector new_point = point;

    // Step 2: Backtrack
    // -----------------------------------------------------------------------
    while (stats.func_evals <= LS_MAX_ITER && step_size >= LS_ZERO){

      // Check for sufficient decrease
      new_point = point + step_size * direction;
      if (model.compute_function_value(new_point) <= init_func_value
          + step_size * sufficient_decrease){

        stats.step_size = step_size;
        stats.status = true;
        return stats;

      }

      step_size *= 0.5;
      stats.func_evals += 1;

    }

    return stats;
}

/**
 *
 * Backtracking to compute step sizes for line search methods.
 *
 * \note Applicable for non-smooth functions.
 *
 * Line search based on an backtracking strategy to "some" decrease in function
 * values at each iteration.
 *
 * References:
 * (1) Wright S.J  and J. Nocedal. Numerical optimization. Vol. 2.
 *                         New York: Springer, 1999.
 *
 * \param[in] model Any model with a first order optimization interface.
 * \param[in] init_step Initial step size.
 * \param[in] init_func Initial function value.
 * \param[in] point     Starting point for the solver.
 * \param[in] gradient  Gradient value at this point.
 * \param[in] direction Direction of the next step .
 * \param[in] reg       Regularizer interface!
 *
 * \returns stats Line searnorm ch return object
 *
*/
template <typename Vector>
inline ls_return backtracking(
    first_order_opt_interface& model,
    double init_step,
    double init_func_value,
    DenseVector point,
    Vector gradient,
    DenseVector direction,
    const std::shared_ptr<regularizer_interface> reg=NULL){

    // Step 1: Initialize the function
    // ------------------------------------------------------------------------
    ls_return stats;
    double step_size = init_step;
    DenseVector delta_point(point.size());;
    DenseVector new_point(point.size());;

    // Step 2: Backtrack
    // -----------------------------------------------------------------------
    while (stats.func_evals <= LS_MAX_ITER && step_size >= LS_ZERO){

      // Check for sufficient decrease
      new_point = point + step_size * direction;
      if(reg != NULL){
        reg->apply_proximal_operator(new_point, step_size);
      }

      delta_point = new_point - point;
      if (model.compute_function_value(new_point) <=
             init_func_value + gradient.dot(delta_point)
                            + 0.5 * delta_point.squaredNorm()/step_size){

        stats.step_size = step_size;
        stats.status = true;
        return stats;

      }

      step_size *= 0.5;
      stats.func_evals += 1;

    }

    return stats;
}

/** This function estimates a minumum point between
 *  two function values with known gradients.
 *
 *  The minimum value must lie between the two function values, f1 and f2, and the
 *  gradients must indicate the minimum lies between the two values and that the
 *  function is convex.
 *
 *  Under these situations, a third degree polynomial / cubic spline can be fit
 *  to the two function points, and this is gauranteed to have a single minimum
 *  between f1 and f2.  This is what this function returns.
 *
 *  \param[in] dist  The distance between the points f1 and f2.
 *  \param[in] f1    The value of the function at the left point.
 *  \param[in] f2    The value of the function at the right point.
 *  \param[in] g1    The derivative of the function at the left point.
 *  \param[in] g2    The derivative of the function at the right point.
 *
 */
inline double gradient_bracketed_linesearch(double dist, double f1, double f2,
                                            double g1, double g2) {
  // Assume f1 is evaluated at 0, f2 is evaluated at 1.  To make the latter true,
  // we need to multiply the gradients by the appropriate scaling factor.
  g1 *= dist;
  g2 *= dist;

  // We have to have this such that a minimum point is between f1 and f2, which
  DASSERT_LE(g1, 1e-4);   // Condition 1
  DASSERT_GE(g2, -1e-4);  // Condition 2

  // Also make sure the function is convex;
  DASSERT_LE(f2 - g2, f1 + 1e-4);  // Condition 3
  DASSERT_LE(f1 + g1, f2 + 1e-4);  // Condition 4

  // Now, we have 4 known variables, so construct a 3rd order polynomial to
  // approximate the solution between the two points.  Then find the minimum of
  // that.

  // With the convexity coefficients above, a third order polynomial, given as
  //
  //   p(t) = a*t^3 + b*t^2 + c*t + d
  //
  // will have exactly one minima between 0 and 1 by the conditions above.
  //
  // The coefficients can be easily derived by:
  // p(0) = f1
  // p(1) = f2
  // p'(0) = g1
  // p'(1) = g2
  //
  // Some algebra yields:
  //
  const double a = -2*(f2 - f1) + (g2 + g1);
  const double b = 3*(f2 - f1) - g2 - 2*g1;
  const double c = g1;
  const double d = f1;

  // std::cout << "coeff = (" << a << ", " << b << ", " << c << std::endl;

  // Starting iterate
  double left = 0;
  double right = 1;

  double best_value = std::min(f1, f2);
  double best_loc = f1 < f2 ? 0 : 1;

  for(size_t m_iter = 0; m_iter < 32; ++m_iter) {

    double t = 0.5 * (right + left);

    // Make sure that the recent value is indeed better
    double v = d + c*t + b*t*t + a*t*t*t;
    // double vpp = 2*b + 6*a*t;

    if(v < best_value) {
      best_value = v;
      best_loc = t;
    }

    if(right - left < 1e-6) {
      break;
    }

    double vp = c + 2*b*t + 3*a*t*t;

    if(vp > 0) {
      right = t;
    } else {
      left = t;
    }

  }

  return dist * best_loc;
}

/// \}

} // optimizaiton

} // turicreate

#endif


//////////////////////////////////////////////////////////////////////////////////
// MIT License
//
// Copyright (c) 2017 Vicon Motion Systems Ltd
//
// 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 "RetimerUtils.h"

#include <ViconDataStreamSDKCoreUtils/ClientUtils.h>

#include <boost/math/constants/constants.hpp>

namespace bmc = boost::math::constants;

namespace ClientUtils
{
  typedef std::array< double, 4 > Quaternion;
  typedef std::array< double, 3 > Axis;
  typedef std::array< double, 3 > Displacement;

  Quaternion Conjugate( const Quaternion & i_rInput )
  {
    Quaternion conj = { -i_rInput[0], -i_rInput[1], -i_rInput[2], i_rInput[3] };
    return conj;
  }

  Quaternion Inverse( const Quaternion & i_rInput )
  {
    Quaternion inv = Conjugate( i_rInput );
    double DotProd = 0;
    for( unsigned i = 0; i < 4; ++i ) DotProd += ( i_rInput[i] * i_rInput[i] );
    for( unsigned i = 0; i < 4; ++i ) inv[i] /= DotProd;
    return inv;
  }

  Axis Imaginary( const Quaternion & i_rInput )
  {
    Axis axis = { i_rInput[0], i_rInput[1], i_rInput[2] };
    return axis;
  }

  double Magnitude( const Axis & i_rInput )
  {
    double mag = 0;
    for( unsigned int i = 0; i < 3; ++i )
    {
      mag += i_rInput[i] * i_rInput[i];
    }
    return sqrt( mag );
  }

  void Normalize( Axis & io_rInput )
  {
    double Mag = Magnitude( io_rInput );
    io_rInput /= Mag;
  }

  bool ToAxisAngle( const Quaternion & i_rInput, Axis & o_rAxis, double & o_rAngle )
  {
    bool success = true;

    // angle is always positive
    Axis imag = Imaginary( i_rInput );
    double mag = Magnitude( imag );
    o_rAngle = 2 * std::atan2( mag, i_rInput[3] );

    // direc parallel to imag. part
    o_rAxis = imag;
    if( mag == 0.0 )
    {
      // or signal exception here.
      o_rAxis[2] = 1.0;
      success = false;
    }
    else
    {
      // normalize direction vector
      for( unsigned int i = 0; i < 3; ++i ) o_rAxis[i] /= mag;
    }
    return success;
  }

  Quaternion FromAxisAngle( const Axis & i_rAxis, const double & i_rAngle )
  {
    // This should take a normalized vector
    Axis AxisCopy = i_rAxis;
    Normalize( AxisCopy );

    Quaternion output;

    // half angle
    double a = i_rAngle * 0.5;
    double s = std::sin( a );

    for( int i = 0; i < 3; i++ )
    {
      // imaginary vector is sine of half angle multiplied with axis
      output[i] = s * AxisCopy[i];
    }

    // real part is cosine of half angle
    output[3] = std::cos( a );

    return output;
  }

  RotationMatrix ToRotationMatrix( const Quaternion & i_rInput )
  {
    double x2 = i_rInput[0] * i_rInput[0];
    double xy = i_rInput[0] * i_rInput[1];
    double rx = i_rInput[3] * i_rInput[0];
    double y2 = i_rInput[1] * i_rInput[1];
    double yz = i_rInput[1] * i_rInput[2];
    double ry = i_rInput[3] * i_rInput[1];
    double z2 = i_rInput[2] * i_rInput[2];
    double zx = i_rInput[2] * i_rInput[0];
    double rz = i_rInput[3] * i_rInput[2];
    double r2 = i_rInput[3] * i_rInput[3];

    RotationMatrix Output;
    // fill diagonal terms
    Output[ 0 ] = r2 + x2 - y2 - z2;    //(0,0)
    Output[ 4 ] = r2 - x2 + y2 - z2; // (1,1)
    Output[ 8 ] = r2 - x2 - y2 + z2; // (2,2)

    // fill off diagonal terms (output not transposed; differing from vnl_quaternion::rotation_matrix_transposed
    Output[ 3 ] = 2 * ( xy + rz ); // (1,0)
    Output[ 6 ] = 2 * ( zx - ry ); // (2,0)
    Output[ 7 ] = 2 * ( yz + rx ); // (2,1)
    Output[ 1 ] = 2 * ( xy - rz ); // (0,1)
    Output[ 2 ] = 2 * ( zx + ry ); // (0,2)
    Output[ 5 ] = 2 * ( yz - rx ); // (1,2)

    return Output;
  }

  Axis operator*( const Axis & i_rLeft, double i_rVal )
  {
    Axis Result;
    std::transform( i_rLeft.begin(), i_rLeft.end(), Result.begin(), [&]( double x ){ return x * i_rVal; } );
    return Result;
  }

  Axis operator+=( Axis & i_rLeft, const Axis & i_rRight )
  {
    std::transform( i_rLeft.begin(), i_rLeft.end(), i_rRight.begin(), i_rLeft.begin(), []( double x, double y ){ return x + y; } );
    return i_rLeft;
  }

  Quaternion operator*( const Quaternion & i_rLeft, const Quaternion & i_rRight )
  {
    Quaternion output;
    double r1 = i_rLeft[3];                  // real and img parts of args
    double r2 = i_rRight[3];
    Axis i1 = Imaginary( i_rLeft );
    Axis i2 = Imaginary( i_rRight );
    double real_v = ( r1 * r2 ) - DotProduct( i1, i2 ); // real&img of product q1*q2
    Axis img = CrossProduct( i1, i2 );
    Axis i2r1 = i2*r1;
    Axis i1r2 = i1*r2;
    Axis i2r1i1r2 = i2r1 + i1r2;
    img += i2r1i1r2;
    output = { img[0], img[1], img[2], real_v };
    return output;
  }

  // Simple interpolation using linear interpolation and prediction
  /// Returns a rotation prediction at time t3 from 2 rotation r1 at time t1 and r2 and time t2
  /// where t3 > t2 > t1
  // From http://answers.unity3d.com/questions/168779/extrapolating-quaternion-rotation.html
  Quaternion PredictRotation( Quaternion r1, double t1, Quaternion r2, double t2, double t3 )
  {
    Quaternion rot = r2 * Inverse( r1 ); // rot is rotation from t1 to t2

    double dt = ( t3 - t1 ) / ( t2 - t1 ); // dt = extrapolation factor

    // Convert to Axis/Angle
    Axis axis;
    double angle;
    if( !ToAxisAngle( rot, axis, angle ) )
    {
      // Check the magnitude of the imaginary part is not zero, as then
      // the axis of the quaternion will not be well defined.
      return r1;
    }

    // Assume shortest path
    if( angle > bmc::pi< double >() ) angle -= ( 2 * bmc::pi< double >() );

    // Multiple angle by extrapolation factor
    angle = fmod( angle * dt, ( 2 * bmc::pi< double >() ) );

    // Combine with first rotation
    Quaternion r3 = FromAxisAngle( axis, angle ) * r1;

    return r3;
  }

  /// Returns a translation prediction at time t3 from 2 translations at time d1, t1; d2, t2 where t3 > t2 > t1
  /// Can also be used for linear interpolation where t2 > t3 > t1
  Displacement PredictDisplacement( const Displacement & d1, double t1, const Displacement & d2, double t2, double t3 )
  {

    // disp is displacement from t1 to t2
    Displacement disp( d2 - d1 );

    // extrapolation factor
    double dt = ( t3 - t1 ) / ( t2 - t1 );

    // multiply displacement by extrapolation factor
    disp *= dt;

    // Combine with first displacement
    Displacement d3 = d1 + disp;

    return d3;
  }

  double PredictVal(const double d1, const double t1, const double d2, const double t2, double t3)
  {
    // disp is displacement from t1 to t2
    double disp( d2 - d1 );

    // extrapolation factor
    double dt = ( t3 - t1 ) / ( t2 - t1 );

    // multiply displacement by extrapolation factor
    disp *= dt;

    // Combine with first displacement
    double d3 = d1 + disp;

    return d3;

  }
}


#include <boost/test/unit_test.hpp>
#include "inputs/interp_tests.h"
#include "slice4missing.hpp"
#include "utils/algorithms.hpp"
#include "utils/cubic_iterpolation.h"
//#include "utils/math_utils.h"
#include "cubic_fix.hpp"
void remove_ambiguity_from_data(std::vector<double> &yd,
        std::vector<double> &xd,
        const std::vector<double> &xi)
{
    for (size_t y=0;y<xi.size();y++) {
        auto it = std::find(xd.begin(), xd.end(), xi[y]);

        if(it!=xd.end()){
            auto index = std::distance(xd.begin(), it);
            xd.erase(xd.begin()+index);
            yd.erase(yd.begin()+index);

        }

    }

}

BOOST_AUTO_TEST_CASE(test_cubic)
{
    /*
    auto dataset = from_double_array(std::begin(values), std::end(values), "rand_name");
    auto missing_begin = std::begin(xi_interp);
    auto missing_end = std::end(xi_interp);
    BOOST_TEST_REQUIRE(utils::size(expected_nearest_interp) == utils::size(xi_interp));  // make sure input is valid!!
    std::size_t count = 0u;
    while (missing_begin != missing_end) {
        auto expected = expected_cubic_interp[count];
        auto subset = slice(dataset, missing_begin, 2u);    // the the subset that we need to fix
        BOOST_TEST_REQUIRE(utils::size(subset) == 4u);        // win size * 2
        const auto vals = transform_slice(std::begin(subset), std::end(subset));
        const auto cords = row_nums_transform(std::begin(subset), std::end(subset));
        BOOST_TEST_REQUIRE(vals.size() == cords.size());
        BOOST_TEST_REQUIRE(vals.size() == subset.size());
        auto calc = utils::cubic_interpolation(vals, cords,
                static_cast<utils::fx_list_type::value_type>(*missing_begin));
        if (!round_cmp(expected, calc, 0.0001)) {
            std::cout<<"input is [";
            out_range(std::cout, subset, " ");
            std::cout<<"] at location [";
            out_range(std::cout, cords, " ");
            std::cout<<"] ";
            BOOST_TEST(false, "- fix value "<<calc<<" != expected "<<expected<<" index "<<*missing_begin<<" for value "<<values[*missing_begin]);
        }
        ++missing_begin;
        ++count;
    }
    */
    std::vector<double> data_set(std::begin(values), std::end(values));
    std::vector<double> coordinates;
    std::transform(std::begin(xd_interp), std::end(xd_interp), std::back_inserter(coordinates), [](auto val) {
                            return static_cast<double>(val);
                        }
    );
    std::vector<double> missing_values;
    std::transform(std::begin(xi_interp), std::end(xi_interp), std::back_inserter(missing_values), [](auto val) {
                            return static_cast<double>(val);
                        }
    );
    remove_ambiguity_from_data(data_set, coordinates, missing_values);
    BOOST_TEST_REQUIRE(utils::size(missing_values) == utils::size(expected_cubic_interp));
    auto corrections = interpolate(coordinates, data_set, missing_values);
    BOOST_TEST_REQUIRE(corrections.size() == utils::size(expected_cubic_interp));
    auto start = std::begin(expected_cubic_interp);
    for (auto i = std::begin(corrections); i != std::end(corrections); ++i, ++start) {
        BOOST_TEST_REQUIRE(round_cmp(*i, *start, 0.0001), "the calcualted value "<<*i<<" != "<<*start);
    }
}

BOOST_AUTO_TEST_CASE(test_cubic2)
{
    std::vector<double> data_set(std::begin(values), std::end(values));
    std::vector<double> coordinates;
    std::transform(std::begin(xd_interp), std::end(xd_interp), std::back_inserter(coordinates), [](auto val) {
                            return static_cast<double>(val);
                        }
    );
    std::vector<double> missing_values;
    std::transform(std::begin(xi_interp), std::end(xi_interp), std::back_inserter(missing_values), [](auto val) {
                            return static_cast<double>(val);
                        }
    );
    remove_ambiguity_from_data(data_set, coordinates, missing_values);
    BOOST_TEST_REQUIRE(utils::size(missing_values) == utils::size(expected_cubic_interp));
    const auto slist = spline(coordinates, data_set);
    BOOST_TEST_REQUIRE(slist.size() == data_set.size());

    auto start = std::begin(expected_cubic_interp);
    for (auto i = std::begin(missing_values); i != std::end(missing_values); ++i, ++start) {
        auto correction = splint(coordinates, data_set, slist, *i);
        BOOST_TEST_REQUIRE(round_cmp(correction, *start, 0.0001), "calcuated value "<<correction<<" != expected value "<<*start);
    }
}

BOOST_AUTO_TEST_CASE(test_cubic_code)
{
    auto dataset = from_double_array(std::begin(values), std::end(values), "rand_name");
    auto size_before = size(dataset);
    auto new_end = filter_out(begin(dataset), end(dataset), std::begin(xi_interp), std::end(xi_interp));
    auto size_after_filter = std::distance(begin(dataset), new_end);
    BOOST_TEST_REQUIRE(size_before > size_after_filter);
    try {
        cubic::spline_data inrep_data{row_nums_transform(begin(dataset), new_end),
                    transform_slice(begin(dataset), new_end)
                };
        for (auto i = 0u; i < utils::size(xi_interp); ++i) {
            auto expected = expected_cubic_interp[i];
            auto before = values[xi_interp[i]];
            BOOST_TEST(expected != before);
            auto fixed = cubic::interpole(inrep_data, xi_interp[i]);
            BOOST_TEST(fixed != before);
            BOOST_TEST(round_cmp(fixed, expected, 0.0001), "the fixed value "<<fixed<<" != "<<expected<<" for value at "<<xi_interp[i]);
        }
    } catch (const std::exception& e) {
        BOOST_TEST(false, "failed to run test: "<<e.what());
    }

}


/*
* The MIT License (MIT)
*
* Copyright 2020 Barbara Barros Carlos, Tommaso Sartor
*
* This file is part of crazyflie_nmpc.
*
* 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.
*/

// standard
#include <iostream>
#include <sstream>
#include <fstream>
#include <ios>

// linear algebra
//
// matrices and vectors
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

// ros
//
#include <ros/ros.h>
// srv
#include <std_srvs/Empty.h>
// msg
#include <std_msgs/String.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <crazyflie_controller/GenericLogData.h>
#include <crazyflie_controller/EulerAnglesStamped.h>
#include <crazyflie_controller/CrazyflieStateStamped.h>
#include <crazyflie_controller/PropellerSpeedsStamped.h>

// dynamic reconfirgure
#include <dynamic_reconfigure/server.h>
#include <crazyflie_controller/crazyflie_estimatorConfig.h>
#include <boost/thread.hpp>
#include "boost/thread/mutex.hpp"

// acados
#include "acados/utils/print.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
// blasfeo
#include "blasfeo/include/blasfeo_d_aux.h"
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"

// crazyflie_model
#include "crazyflie_model/crazyflie_model.h"
#include "acados_solver_crazyflie.h"
#include "acados_sim_solver_crazyflie.h"

// Sampling time
#define TS 0.015
// Number of states
#define NX 13
// Number of control inputs
#define NU 4
// Number of measurements/references on nodes 0..N-1
#define NY 17
// Number of predictions
#define NPU 2
// Number of measurements/references on node N
#define NYN 13

// Constants
#define pi 3.14159265358979323846
#define g0 9.80665

#define RECONFIGURE 0

external_function_param_casadi * forw_vde_casadi;
using namespace Eigen;
using std::ofstream;
using std::cout;
using std::endl;
using std::fixed;
using std::showpos;

// acados sim dims

class ESTIMATOR
	{
	enum systemStates{
		xq = 0,
		yq = 1,
		zq = 2,
		qw = 3,
		qx = 4,
		qy = 5,
		qz = 6,
		vbx = 7,
		vby = 8,
		vbz = 9,
		wx = 10,
		wy = 11,
		wz = 12
	};

	enum controlInputs{
		w1 = 0,
		w2 = 1,
		w3 = 2,
		w4 = 3
	};

	struct euler{
		double phi;
		double theta;
		double psi;
	};

	struct sim_solver_input{
		double x0[NX];
		double Td;
		double u0[NU];
		double u1[NU];
	};

	struct sim_solver_output{
		double status, cpu_time;
		double xn[NX];
	};

	// control
	ros::Publisher p_cf_state;
	ros::Publisher p_cf_euler;
	ros::Publisher p_cf_quaternion;
	// log
	// ros::Publisher p_cf_quat;
	// ros::Publisher p_cf_lvb;
	// ros::Publisher p_cf_avb;
	// ros::Publisher p_cf_lpg;
	//
	ros::Subscriber s_sensorfusion6;
	ros::Subscriber s_imu;
	ros::Subscriber s_eRaptor;
	ros::Subscriber s_euler;
	ros::Subscriber s_motors;
	// ros::Subscriber s_actual_motors;

	// parameters
	double frequency, delay;

	// Variables for joy callback
	double
		joy_roll,
		joy_pitch,
		joy_yaw,
		joy_thrust;

	unsigned int k,i,j,ii;

	// Variables for linear velocity LPF
	std::vector<double> x_samples;
	std::vector<double> y_samples;
	std::vector<double> z_samples;
	std::vector<double> vx_filter_samples;
	std::vector<double> vy_filter_samples;
	std::vector<double> vz_filter_samples;

	// Elapsed time
	double t0;

	// acados struct
	sim_solver_input sim_acados_in;
	sim_solver_output sim_acados_out;
	int sim_acados_status;

	// Variables for reading the IMU data
	float actual_wx;
	float actual_wy;
	float actual_wz;
	float actual_roll;
	float actual_pitch;
	float actual_yaw;

	// Variables for eRaptor data
	float actual_x;
	float actual_y;
	float actual_z;

	// Variables for sensor fusion 6
	float actual_qw;
	float actual_qx;
	float actual_qy;
	float actual_qz;

	int8_t MAX_ORDER = 20;
	std::vector<double> y;

	int32_t
		actual_m1,
		actual_m2,
		actual_m3,
		actual_m4,
		acados_w1_latest,
		acados_w2_latest,
		acados_w3_latest,
		acados_w4_latest,
		acados_w1_prelatest,
		acados_w2_prelatest,
		acados_w3_prelatest,
		acados_w4_prelatest;

	public:

	ESTIMATOR(ros::NodeHandle& n, double delay)
		{

		int status = 0;
		status = crazyflie_acados_sim_create();

		if (status){
			ROS_INFO_STREAM("acados_sim_create() returned status " << status << ". Exiting." << endl);
			exit(1);
		}

		// publish crazyflie state
		p_cf_state = n.advertise<crazyflie_controller::CrazyflieStateStamped>(
			"/cf_estimator/state_estimate",1);
		p_cf_euler = n.advertise<crazyflie_controller::EulerAnglesStamped>(
			"/cf_estimator/euler_angles", 5);

		// subscriber for the motion capture system position
		s_eRaptor = n.subscribe("/crazyflie/external_position", 5, &ESTIMATOR::eRaptorCallback, this);
		// subscriber for the IMU linear acceleration and angular velocities from acc and gyro
		s_imu = n.subscribe("/crazyflie/imu", 5, &ESTIMATOR::imuCallback, this);
		// subscriber for the IMU stabilizer euler angles
		s_euler = n.subscribe("/crazyflie/euler_angles", 5, &ESTIMATOR::eulerCallback, this);
		// subscriber for the motor speeds applied
		s_motors = n.subscribe("/crazyflie/acados_motvel",5, &ESTIMATOR::motorsCallback, this);
		// subscriber for sensor fusion returning the quaternion
		//s_sensorfusion6 = n.subscribe("/crazyflie/sf6",5, &ESTIMATOR::sensorfusion6Callback, this);

		// logs
		// subscriber for the motors rpm
		// s_actual_motors = n.subscribe("/crazyflie/log1", 5, &ESTIMATOR::actual_motorsCallback, this);

		// initialize sim_acados_in.x0
		for (int i=0; i++; i<NX) sim_acados_in.x0[i] = 0;
		sim_acados_in.x0[qw] = 1;

		// initialize control inputs
		for(unsigned int i=0; i < NU; i++) sim_acados_in.u0[i] = 0.0;
		for(unsigned int i=0; i < NU; i++) sim_acados_in.u1[i] = 0.0;

		// Set size of buffer  of the estimated linear velocities to zero
		x_samples.resize(5);
		y_samples.resize(5);
		z_samples.resize(5);

		// Set initial value of the buffer to zero
		for (unsigned int i = 0; i <= 4; i++) x_samples[i] = 0.0;
		for (unsigned int i = 0; i <= 4; i++) y_samples[i] = 0.0;
		for (unsigned int i = 0; i <= 4; i++) z_samples[i] = 0.0;

		// Set size of buffer
		vx_filter_samples.resize(5);
		vy_filter_samples.resize(5);
		vz_filter_samples.resize(5);

		for (unsigned int i = 0; i <= 4; i++) vx_filter_samples[i] = 0.0;
		for (unsigned int i = 0; i <= 4; i++) vy_filter_samples[i] = 0.0;
		for (unsigned int i = 0; i <= 4; i++) vz_filter_samples[i] = 0.0;

		y.resize(MAX_ORDER);
		for (unsigned int i = 0; i <= MAX_ORDER; i++) y[i] = 0.0;

		// Set elapsed time to zero initially
		t0 = 0.0;
		}

	void run(double frequency)
		{
		ros::NodeHandle node;
		ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &ESTIMATOR::predictor, this);

		#if RECONFIGURE
			ROS_DEBUG("Setting up the dynamic reconfigure panel and server");
			dynamic_reconfigure::Server<crazyflie_controller::crazyflie_estimatorConfig> server;
			dynamic_reconfigure::Server<crazyflie_controller::crazyflie_estimatorConfig>::CallbackType f;
			f = boost::bind(&ESTIMATOR::callback_dynamic_reconfigure, this, _1, _2);
			server.setCallback(f);
		#endif

		ros::spin();
		}

	void callback_dynamic_reconfigure(crazyflie_controller::crazyflie_estimatorConfig &config, uint32_t level)
		{

		if (level)
			{
			delay = config.delay;
			ROS_INFO_STREAM("Delay: " << delay << endl);
			ROS_INFO_STREAM("Delay config: " << config.delay << endl);
			}
		}

	Quaterniond euler2quatern(euler angle)
		{

		Quaterniond q;

		double cosPhi = cos(angle.phi*0.5);
		double sinPhi = sin(angle.phi*0.5);

		double cosTheta = cos(angle.theta*0.5);
		double sinTheta = sin(angle.theta*0.5);

		double cosPsi = cos(angle.psi*0.5);
		double sinPsi = sin(angle.psi*0.5);

		q.w() = cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi;
		q.x() = -(cosPsi*cosTheta*sinPhi - sinPsi*sinTheta*cosPhi);
		q.y() = -(cosPsi*sinTheta*cosPhi + sinPsi*cosTheta*sinPhi);
		q.z() = -(sinPsi*cosTheta*cosPhi - cosPsi*sinTheta*sinPhi);

		if(q.w() < 0)
		  {
		  q.w() = -q.w();
		  q.vec() = -q.vec();
		  }

		return q;
		}

	double linearVelocityLPF(
		std::vector <double> q_samples,
		std::vector <double> dq_samples,
		double Ts, double elapsed_time)
		{
		// digital low-pass filter considering Ts = 15 ms
		double dq = 0;
		if (elapsed_time > 1.0)
			dq = 0.3306*dq_samples[4] - 0.02732*dq_samples[3] + 35.7*q_samples[4] - 35.7*q_samples[3];
		else
			dq = (q_samples[4] - q_samples[3]) / Ts;
		return dq;
		}

	Vector3d estimateWordLinearVelocities(float dt, float delta)
		{

		Vector3d vearth;
		//estimte the velocity
		x_samples[0] = x_samples[1];
		x_samples[1] = x_samples[2];
		x_samples[2] = x_samples[3];
		x_samples[3] = x_samples[4];
		x_samples[4] = actual_x;
		y_samples[0] = y_samples[1];
		y_samples[1] = y_samples[2];
		y_samples[2] = y_samples[3];
		y_samples[3] = y_samples[4];
		y_samples[4] = actual_y;
		z_samples[0] = z_samples[1];
		z_samples[1] = z_samples[2];
		z_samples[2] = z_samples[3];
		z_samples[3] = z_samples[4];
		z_samples[4] = actual_z;

		vearth[0] = linearVelocityLPF(x_samples, vx_filter_samples, dt, delta);
		vearth[1] = linearVelocityLPF(y_samples, vy_filter_samples, dt, delta);
		vearth[2] = linearVelocityLPF(z_samples, vz_filter_samples, dt, delta);

		vx_filter_samples[0] = vx_filter_samples[1];
		vx_filter_samples[1] = vx_filter_samples[2];
		vx_filter_samples[2] = vx_filter_samples[3];
		vx_filter_samples[3] = vx_filter_samples[4];
		vx_filter_samples[4] = vearth[0];
		vy_filter_samples[0] = vy_filter_samples[1];
		vy_filter_samples[1] = vy_filter_samples[2];
		vy_filter_samples[2] = vy_filter_samples[3];
		vy_filter_samples[3] = vy_filter_samples[4];
		vy_filter_samples[4] = vearth[1];
		vz_filter_samples[0] = vz_filter_samples[1];
		vz_filter_samples[1] = vz_filter_samples[2];
		vz_filter_samples[2] = vz_filter_samples[3];
		vz_filter_samples[3] = vz_filter_samples[4];
		vz_filter_samples[4] = vearth[2];

		return vearth;
		}

	Vector3d rotateLinearVeloE2B(Quaterniond* q, Vector3d v_inertial)
		{
		 // This is the convertion between
		 // quaternion orientation to rotation matrix
		 // from EARTH to BODY (considering ZYX euler sequence)
		 Matrix3d Sq;
		 Vector3d vb;

	 	 double S11 = 2*(q->w()*q->w()+q->x()*q->x())-1;
	 	 double S12 = 2*(q->x()*q->y()+q->w()*q->z());
	 	 double S13 = 2*(q->x()*q->z()-q->w()*q->y());
	 	 double S21 = 2*(q->x()*q->y()-q->w()*q->z());
	 	 double S22 = 2*(q->w()*q->w()+q->y()*q->y())-1;
	 	 double S23 = 2*(q->y()*q->z()+q->w()*q->x());
	 	 double S31 = 2*(q->x()*q->z()+q->w()*q->y());
	 	 double S32 = 2*(q->y()*q->z()-q->w()*q->x());
	 	 double S33 = 2*(q->w()*q->w()+q->z()*q->z())-1;

		Sq << S11,S12,S13,
		      S21,S22,S23,
		      S31,S32,S33;

		vb = Sq*v_inertial;

		return vb;
		}

	double deg2Rad(double deg)
		{
		return deg / 180.0 * pi;
		}

	double rad2Deg(double rad)
		{
		return rad * 180.0 / pi;
		}

	void sensorfusion6Callback(const crazyflie_controller::GenericLogDataConstPtr& msg)
		{
			actual_qw = msg->values[0];
			actual_qx = msg->values[1];
			actual_qy = msg->values[2];
			actual_qz = msg->values[3];
		}

	void motorsCallback(const crazyflie_controller::PropellerSpeedsStampedPtr& msg)
		{
		// motors rpm from mpc solution
		acados_w1_prelatest = acados_w1_latest;
		acados_w2_prelatest = acados_w2_latest;
		acados_w3_prelatest = acados_w3_latest;
		acados_w4_prelatest = acados_w4_latest;

		acados_w1_latest = msg->w1;
		acados_w2_latest = msg->w2;
		acados_w3_latest = msg->w3;
		acados_w4_latest = msg->w4;

		}

	void actual_motorsCallback(const crazyflie_controller::GenericLogDataConstPtr& msg)
		{
		actual_m1 = msg->values[0];
		actual_m2 = msg->values[1];
		actual_m3 = msg->values[2];
		actual_m4 = msg->values[3];
		}

	void eRaptorCallback(const geometry_msgs::PointStampedConstPtr& msg)
		{
		// Position of crazyflie marker
		actual_x = msg->point.x;
		actual_y = msg->point.y;
		actual_z = msg->point.z;
		}

	void eulerCallback(const geometry_msgs::Vector3StampedPtr& msg)
		{
		// Euler angles
		actual_roll  =  msg->vector.x;
		actual_pitch =  -msg->vector.y;
		actual_yaw   =  msg->vector.z;

		crazyflie_controller::EulerAnglesStamped euler_msg;
		euler_msg.header 	= msg->header;
		euler_msg.roll 		= actual_roll;
		euler_msg.pitch 	= actual_pitch;
		euler_msg.yaw 		= actual_yaw;

		p_cf_euler.publish(euler_msg);
		}

	void imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
		{
		// Angular rates w.r.t. body frame in rad/s
		actual_wx = msg->angular_velocity.x;
		actual_wy = msg->angular_velocity.y;
		actual_wz = msg->angular_velocity.z;
		}

	int krpm2pwm(double Krpm)
		{
		int pwm = ((Krpm*1000)-4070.3)/0.2685;
		return pwm;
		}

	void predictor(const ros::TimerEvent& e)
		{

		if(e.last_real.isZero())
		{
		t0 = e.current_real.toSec();
		}

		double dt = e.current_real.toSec() - e.last_real.toSec();

		// --- Position
		sim_acados_in.x0[xq] = actual_x;
		sim_acados_in.x0[yq] = actual_y;
		sim_acados_in.x0[zq] = actual_z;

		// --- Quaternions
		//
		// Get the euler angles from the onboard stabilizer
		euler eu;
		eu.phi 		= deg2Rad(actual_roll);
		eu.theta 	= deg2Rad(actual_pitch);
		eu.psi 		= deg2Rad(actual_yaw);

		// Convert IMU euler angles to quaternion
		Quaterniond q_imu = euler2quatern(eu);
		q_imu.normalize();

		// storing converted quaternions
		sim_acados_in.x0[qw] = q_imu.w();
		sim_acados_in.x0[qx] = q_imu.x();
		sim_acados_in.x0[qy] = q_imu.y();
		sim_acados_in.x0[qz] = q_imu.z();

		// -- inertial linear velocities
		Vector3d vi_mat;
		vi_mat = estimateWordLinearVelocities(dt,t0);

		// --- Body linear velocities
		Vector3d vb_mat;
		vb_mat = rotateLinearVeloE2B(&q_imu,vi_mat);

		// overwriting linear velocities in the body frame in state vector
		sim_acados_in.x0[vbx] = vb_mat[0];
		sim_acados_in.x0[vby] = vb_mat[1];
		sim_acados_in.x0[vbz] = vb_mat[2];

		// --- Body angular velocities
		sim_acados_in.x0[wx] = actual_wx;
		sim_acados_in.x0[wy] = actual_wy;
		sim_acados_in.x0[wz] = actual_wz;

		// set discretization time
		sim_in_set(crazyflie_sim_config, crazyflie_sim_dims, crazyflie_sim_in, "T", &delay);

		// ROS_INFO_STREAM("Delay: " << delay << endl);

		// set initial state
		sim_in_set(crazyflie_sim_config, crazyflie_sim_dims, crazyflie_sim_in, "x", sim_acados_in.x0);

		// set control
		sim_acados_in.u0[w1] = acados_w1_latest;
		sim_acados_in.u0[w2] = acados_w2_latest;
		sim_acados_in.u0[w3] = acados_w3_latest;
		sim_acados_in.u0[w4] = acados_w4_latest;

		sim_in_set(crazyflie_sim_config, crazyflie_sim_dims, crazyflie_sim_in, "u", sim_acados_in.u0);

		// solve
		sim_acados_status = crazyflie_acados_sim_solve();
		// ROS_INFO_STREAM("Sim solver : " << sim_acados_status << endl);

		// get and print output
		sim_out_get(crazyflie_sim_config, crazyflie_sim_dims, crazyflie_sim_out, "xn", sim_acados_out.xn);

		// assign output signals
		sim_acados_out.status = sim_acados_status;
		// sim_acados_out.cpu_time = (double)crazyflie_sim_out->total_time;

		// publish state
		crazyflie_controller::CrazyflieStateStamped crazyflie_state;

		crazyflie_state.header.stamp = ros::Time::now();

		/*crazyflie_state.pos.x    = sim_acados_in.x0[xq];
		crazyflie_state.pos.y    = sim_acados_in.x0[yq];
		crazyflie_state.pos.z    = sim_acados_in.x0[zq];
		crazyflie_state.vel.x    = sim_acados_in.x0[vbx];
		crazyflie_state.vel.y    = sim_acados_in.x0[vby];
		crazyflie_state.vel.z    = sim_acados_in.x0[vbz];
		crazyflie_state.quat.w   = sim_acados_in.x0[qw];
		crazyflie_state.quat.x   = sim_acados_in.x0[qx];
		crazyflie_state.quat.y   = sim_acados_in.x0[qy];
		crazyflie_state.quat.z   = sim_acados_in.x0[qz];
		crazyflie_state.rates.x  = sim_acados_in.x0[wx];
		crazyflie_state.rates.y  = sim_acados_in.x0[wy];
		crazyflie_state.rates.z  = sim_acados_in.x0[wz];*/

		crazyflie_state.pos.x    = sim_acados_out.xn[xq];
		crazyflie_state.pos.y    = sim_acados_out.xn[yq];
		crazyflie_state.pos.z    = sim_acados_out.xn[zq];
		crazyflie_state.vel.x    = sim_acados_out.xn[vbx];
		crazyflie_state.vel.y    = sim_acados_out.xn[vby];
		crazyflie_state.vel.z    = sim_acados_out.xn[vbz];
		crazyflie_state.quat.w   = sim_acados_out.xn[qw];
		crazyflie_state.quat.x   = sim_acados_out.xn[qx];
		crazyflie_state.quat.y   = sim_acados_out.xn[qy];
		crazyflie_state.quat.z   = sim_acados_out.xn[qz];
		crazyflie_state.rates.x  = sim_acados_out.xn[wx];
		crazyflie_state.rates.y  = sim_acados_out.xn[wy];
		crazyflie_state.rates.z  = sim_acados_out.xn[wz];

		p_cf_state.publish(crazyflie_state);

		}
	};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "cf_estimator");
	ros::NodeHandle n("~");

	double frequency = 66.6;
	double delay;
	n.getParam("delay", delay);

	ESTIMATOR est(n, delay);
	est.run(frequency);

	return 0;
}


/* boost random/nierderreiter_base2.hpp header file
 *
 * Copyright Justinas Vygintas Daugmaudis 2010-2018
 * 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)
 */

#ifndef BOOST_RANDOM_NIEDERREITER_BASE2_HPP
#define BOOST_RANDOM_NIEDERREITER_BASE2_HPP

#include <boost/random/detail/niederreiter_base2_table.hpp>
#include <boost/random/detail/gray_coded_qrng.hpp>

#include <boost/dynamic_bitset.hpp>

namespace boost {
namespace random {

/** @cond */
namespace qrng_detail {
namespace nb2 {

// Return the base 2 logarithm for a given bitset v
template <typename DynamicBitset>
inline typename DynamicBitset::size_type
bitset_log2(const DynamicBitset& v)
{
  if (v.none())
    boost::throw_exception( std::invalid_argument("bitset_log2") );

  typename DynamicBitset::size_type hibit = v.size() - 1;
  while (!v.test(hibit))
    --hibit;
  return hibit;
}

// Multiply polynomials over Z_2.
template <typename PolynomialT, typename DynamicBitset>
inline void modulo2_multiply(PolynomialT P, DynamicBitset& v, DynamicBitset& pt)
{
  pt.reset(); // pt == 0
  for (; P; P >>= 1, v <<= 1)
    if (P & 1) pt ^= v;
  pt.swap(v);
}

// Calculate the values of the constants V(J,R) as
// described in BFN section 3.3.
//
// pb = polynomial defined in section 2.3 of BFN.
template <typename DynamicBitset>
inline void calculate_v(const DynamicBitset& pb,
  typename DynamicBitset::size_type kj,
  typename DynamicBitset::size_type pb_degree,
  DynamicBitset& v)
{
  typedef typename DynamicBitset::size_type size_type;

  // Now choose values of V in accordance with
  // the conditions in section 3.3.
  size_type r = 0;
  for ( ; r != kj; ++r)
    v.reset(r);

  // Quoting from BFN: "Our program currently sets each K_q
  // equal to eq. This has the effect of setting all unrestricted
  // values of v to 1."
  for ( ; r < pb_degree; ++r)
    v.set(r);

  // Calculate the remaining V's using the recursion of section 2.3,
  // remembering that the B's have the opposite sign.
  for ( ; r != v.size(); ++r)
  {
    bool term = false;
    for (typename DynamicBitset::size_type k = 0; k < pb_degree; ++k)
    {
      term ^= pb.test(k) & v[r + k - pb_degree];
    }
    v[r] = term;
  }
}

} // namespace nb2

template<typename UIntType, unsigned w, typename Nb2Table>
struct niederreiter_base2_lattice
{
  typedef UIntType value_type;

  BOOST_STATIC_ASSERT(w > 0u);
  BOOST_STATIC_CONSTANT(unsigned, bit_count = w);

private:
  typedef std::vector<value_type> container_type;

public:
  explicit niederreiter_base2_lattice(std::size_t dimension)
  {
    resize(dimension);
  }

  void resize(std::size_t dimension)
  {
    typedef boost::dynamic_bitset<> bitset_type;

    dimension_assert("Niederreiter base 2", dimension, Nb2Table::max_dimension);

    // Initialize the bit array
    container_type cj(bit_count * dimension);

    // Reserve temporary space for lattice computation
    bitset_type v, pb, tmp;

    // Compute Niedderreiter base 2 lattice
    for (std::size_t dim = 0; dim != dimension; ++dim)
    {
      const typename Nb2Table::value_type poly = Nb2Table::polynomial(dim);
      if (poly > std::numeric_limits<value_type>::max()) {
        boost::throw_exception( std::range_error("niederreiter_base2: polynomial value outside the given value type range") );
      }

      const unsigned degree = multiprecision::msb(poly); // integer log2(poly)
      const unsigned space_required = degree * ((bit_count / degree) + 1); // ~ degree + bit_count

      v.resize(degree + bit_count - 1);

      // For each dimension, we need to calculate powers of an
      // appropriate irreducible polynomial, see Niederreiter
      // page 65, just below equation (19).
      // Copy the appropriate irreducible polynomial into PX,
      // and its degree into E. Set polynomial B = PX ** 0 = 1.
      // M is the degree of B. Subsequently B will hold higher
      // powers of PX.
      pb.resize(space_required); tmp.resize(space_required);

      typename bitset_type::size_type kj, pb_degree = 0;
      pb.reset(); // pb == 0
      pb.set(pb_degree); // set the proper bit for the pb_degree

      value_type j = high_bit_mask_t<bit_count - 1>::high_bit;
      do
      {
        // Now choose a value of Kj as defined in section 3.3.
        // We must have 0 <= Kj < E*J = M.
        // The limit condition on Kj does not seem to be very relevant
        // in this program.
        kj = pb_degree;

        // Now multiply B by PX so B becomes PX**J.
        // In section 2.3, the values of Bi are defined with a minus sign :
        // don't forget this if you use them later!
        nb2::modulo2_multiply(poly, pb, tmp);
        pb_degree += degree;
        if (pb_degree >= pb.size()) {
          // Note that it is quite possible for kj to become bigger than
          // the new computed value of pb_degree.
          pb_degree = nb2::bitset_log2(pb);
        }

        // If U = 0, we need to set B to the next power of PX
        // and recalculate V.
        nb2::calculate_v(pb, kj, pb_degree, v);

        // Niederreiter (page 56, after equation (7), defines two
        // variables Q and U.  We do not need Q explicitly, but we
        // do need U.

        // Advance Niederreiter's state variables.
        for (unsigned u = 0; j && u != degree; ++u, j >>= 1)
        {
          // Now C is obtained from V. Niederreiter
          // obtains A from V (page 65, near the bottom), and then gets
          // C from A (page 56, equation (7)).  However this can be done
          // in one step.  Here CI(J,R) corresponds to
          // Niederreiter's C(I,J,R), whose values we pack into array
          // CJ so that CJ(I,R) holds all the values of C(I,J,R) for J from 1 to NBITS.
          for (unsigned r = 0; r != bit_count; ++r) {
            value_type& num = cj[dimension * r + dim];
            // set the jth bit in num
            num = (num & ~j) | (-v[r + u] & j);
          }
        }
      } while (j != 0);
    }

    bits.swap(cj);
  }

  typename container_type::const_iterator iter_at(std::size_t n) const
  {
    BOOST_ASSERT(!(n > bits.size()));
    return bits.begin() + n;
  }

private:
  container_type bits;
};

} // namespace qrng_detail

typedef detail::qrng_tables::niederreiter_base2 default_niederreiter_base2_table;

/** @endcond */

//!Instantiations of class template niederreiter_base2_engine model a \quasi_random_number_generator.
//!The niederreiter_base2_engine uses the algorithm described in
//! \blockquote
//!Bratley, Fox, Niederreiter, ACM Trans. Model. Comp. Sim. 2, 195 (1992).
//! \endblockquote
//!
//!\attention niederreiter_base2_engine skips trivial zeroes at the start of the sequence. For example,
//!the beginning of the 2-dimensional Niederreiter base 2 sequence in @c uniform_01 distribution will look
//!like this:
//!\code{.cpp}
//!0.5, 0.5,
//!0.75, 0.25,
//!0.25, 0.75,
//!0.375, 0.375,
//!0.875, 0.875,
//!...
//!\endcode
//!
//!In the following documentation @c X denotes the concrete class of the template
//!niederreiter_base2_engine returning objects of type @c UIntType, u and v are the values of @c X.
//!
//!Some member functions may throw exceptions of type std::range_error. This
//!happens when the quasi-random domain is exhausted and the generator cannot produce
//!any more values. The length of the low discrepancy sequence is given by
//! \f$L=Dimension \times (2^{w} - 1)\f$.
template<typename UIntType, unsigned w, typename Nb2Table = default_niederreiter_base2_table>
class niederreiter_base2_engine
  : public qrng_detail::gray_coded_qrng<
      qrng_detail::niederreiter_base2_lattice<UIntType, w, Nb2Table>
    >
{
  typedef qrng_detail::niederreiter_base2_lattice<UIntType, w, Nb2Table> lattice_t;
  typedef qrng_detail::gray_coded_qrng<lattice_t> base_t;

public:
  //!Effects: Constructs the default `s`-dimensional Niederreiter base 2 quasi-random number generator.
  //!
  //!Throws: bad_alloc, invalid_argument, range_error.
  explicit niederreiter_base2_engine(std::size_t s)
    : base_t(s) // initialize lattice here
  {}

#ifdef BOOST_RANDOM_DOXYGEN
  //=========================Doxygen needs this!==============================
  typedef UIntType result_type;

  //!Returns: Tight lower bound on the set of values returned by operator().
  //!
  //!Throws: nothing.
  static BOOST_CONSTEXPR result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::min)(); }

  //!Returns: Tight upper bound on the set of values returned by operator().
  //!
  //!Throws: nothing.
  static BOOST_CONSTEXPR result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::max)(); }

  //!Returns: The dimension of of the quasi-random domain.
  //!
  //!Throws: nothing.
  std::size_t dimension() const { return base_t::dimension(); }

  //!Effects: Resets the quasi-random number generator state to
  //!the one given by the default construction. Equivalent to u.seed(0).
  //!
  //!\brief Throws: nothing.
  void seed()
  {
    base_t::seed();
  }

  //!Effects: Effectively sets the quasi-random number generator state to the `init`-th
  //!vector in the `s`-dimensional quasi-random domain, where `s` == X::dimension().
  //!\code
  //!X u, v;
  //!for(int i = 0; i < N; ++i)
  //!    for( std::size_t j = 0; j < u.dimension(); ++j )
  //!        u();
  //!v.seed(N);
  //!assert(u() == v());
  //!\endcode
  //!
  //!\brief Throws: range_error.
  void seed(UIntType init)
  {
    base_t::seed(init);
  }

  //!Returns: Returns a successive element of an `s`-dimensional
  //!(s = X::dimension()) vector at each invocation. When all elements are
  //!exhausted, X::operator() begins anew with the starting element of a
  //!subsequent `s`-dimensional vector.
  //!
  //!Throws: range_error.
  result_type operator()()
  {
    return base_t::operator()();
  }

  //!Effects: Advances *this state as if `z` consecutive
  //!X::operator() invocations were executed.
  //!\code
  //!X u = v;
  //!for(int i = 0; i < N; ++i)
  //!    u();
  //!v.discard(N);
  //!assert(u() == v());
  //!\endcode
  //!
  //!Throws: range_error.
  void discard(boost::uintmax_t z)
  {
    base_t::discard(z);
  }

  //!Returns true if the two generators will produce identical sequences of outputs.
  BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(niederreiter_base2_engine, x, y)
  { return static_cast<const base_t&>(x) == y; }

  //!Returns true if the two generators will produce different sequences of outputs.
  BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(niederreiter_base2_engine)

  //!Writes the textual representation of the generator to a @c std::ostream.
  BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, niederreiter_base2_engine, s)
  { return os << static_cast<const base_t&>(s); }

  //!Reads the textual representation of the generator from a @c std::istream.
  BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, niederreiter_base2_engine, s)
  { return is >> static_cast<base_t&>(s); }

#endif // BOOST_RANDOM_DOXYGEN
};

/**
 * @attention This specialization of \niederreiter_base2_engine supports up to 4720 dimensions.
 *
 * Binary irreducible polynomials (primes in the ring `GF(2)[X]`, evaluated at `X=2`) were generated
 * while condition `max(prime)` < 216 was satisfied.
 *
 * There are exactly 4720 such primes, which yields a Niederreiter base 2 table for 4720 dimensions.
 *
 * However, it is possible to provide your own table to \niederreiter_base2_engine should the default one be insufficient.
 */
typedef niederreiter_base2_engine<boost::uint_least64_t, 64u, default_niederreiter_base2_table> niederreiter_base2;

} // namespace random

} // namespace boost

#endif // BOOST_RANDOM_NIEDERREITER_BASE2_HPP