/**
 * 3D NDT-UKF Node.
 */

#include <ros/ros.h>
#include <angles/angles.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Pose.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <velodyne_pointcloud/rawdata.h>
#include <velodyne_pointcloud/point_types.h>
#include <pcl_ros/impl/transforms.hpp>

#include "tf/message_filter.h"
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>

#include <sensor_msgs/PointCloud2.h>

#include <ndt_generic/utils.h>
#include <ndt_generic/eigen_utils.h>
#include <ndt_mcl/3d_ndt_ukf.h>
#include <ndt_map/ndt_map.h>

#include <ndt_rviz/ndt_rviz.h>
#include <ndt_generic/pcl_utils.h>

inline void normalizeEulerAngles(Eigen::Vector3d &euler) {
    if (fabs(euler[0]) > M_PI/2) {
        euler[0] += M_PI;
        euler[1] += M_PI;
        euler[2] += M_PI;

        euler[0] = angles::normalize_angle(euler[0]);
        euler[1] = angles::normalize_angle(euler[1]);
        euler[2] = angles::normalize_angle(euler[2]);
    }
}

std::string affine3dToString(const Eigen::Affine3d &T) {
    std::ostringstream stream;
    stream << std::setprecision(std::numeric_limits<double>::digits10);
    Eigen::Vector3d rot = T.rotation().eulerAngles(0,1,2);
    normalizeEulerAngles(rot);

    stream << T.translation().transpose() << " " << rot.transpose();
    return stream.str();
}

class NDTUKF3DNode {

private:
    ros::NodeHandle nh_;
    NDTUKF3D *ndtukf;
    boost::mutex ukf_m,message_m;

    message_filters::Subscriber<sensor_msgs::PointCloud2> *points2_sub_;
    message_filters::Subscriber<nav_msgs::Odometry> *odom_sub_;
    ros::Subscriber scan_sub_;
    ros::Subscriber scan2_sub_;
    ros::Subscriber gt_sub;

    ///Laser sensor offset
    Eigen::Affine3d sensorPoseT; //<<Sensor offset with respect to odometry frame
    ros::Duration sensorTimeOffset_;
    Eigen::Affine3d sensorPoseT2; //<<Sensor offset with respect to odometry frame
    ros::Duration sensorTimeOffset2_;
    Eigen::Affine3d Told,Todo,Todo_old,Tcum; //<<old and current odometry transformations
    Eigen::Affine3d initPoseT; //<<Sensor offset with respect to odometry frame

    pcl::PointCloud<pcl::PointXYZ> scan2_cloud_;
    ros::Time scan2_t0_;

    bool use_dual_scan;
    bool hasSensorPose, hasInitialPose;
    bool isFirstLoad;
    bool forceSIR, do_visualize;
    bool saveMap;						///< indicates if we want to save the map in a regular intervals
    std::string mapName; ///<name and the path to the map
    std::string output_map_name;
    double resolution;
    double subsample_level;
    int pcounter;

    ros::Publisher ukf_pub; ///< The output of UKF is published with this!
    ros::Publisher marker_pub_;
    ros::Publisher markerarray_pub_;
    ros::Publisher pointcloud_pub_;
    ros::Publisher pointcloud2_pub_;

    std::string tf_base_link, tf_sensor_link, tf_gt_link, points_topic, odometry_topic, odometry_frame, scan_topic, scan2_topic;

    ros::Timer heartbeat_slow_visualization_;
    ros::Timer heartbeat_fast_visualization_;
    bool do_pub_ndt_markers_;
    bool do_pub_sigmapoints_markers_;

    std::ofstream gt_file_;
    std::ofstream gt2d_file_;

    std::ofstream est_file_;
    std::ofstream est2d_file_;
    std::string gt_topic;

    bool use_initial_pose_from_gt;

    boost::shared_ptr<velodyne_rawdata::RawData> data_;
    boost::shared_ptr<velodyne_rawdata::RawData> data2_;
    tf::TransformListener tf_listener;

    int skip_nb_rings_;
    int skip_start_;
    double voxel_filter_size_;
    int cloud_min_size_;

    bool draw_ml_lines;

public:
    NDTUKF3DNode(ros::NodeHandle param_nh) : data_(new velodyne_rawdata::RawData()), data2_(new velodyne_rawdata::RawData()) {

        //////////////////////////////////////////////////////////
        // Setup the data parser for the raw packets
        //////////////////////////////////////////////////////////
        {
            data_->setup(param_nh); // To get the calibration file
            double max_range, min_range, view_direction, view_width;
            param_nh.param<double>("max_range", max_range, 130.);
            param_nh.param<double>("min_range", min_range, 2.);
            param_nh.param<double>("view_direction", view_direction, 0.);
            param_nh.param<double>("view_width", view_width, 6.3);
            data_->setParameters(min_range, max_range, view_direction, view_width);

            param_nh.param<int>("skip_nb_rings", skip_nb_rings_, 0);
            param_nh.param<int>("skip_start", skip_start_, 0);
            param_nh.param<double>("voxel_filter_size", voxel_filter_size_, -1.);
            param_nh.param<int>("cloud_min_size", cloud_min_size_, 2000); // minimum amount of points (to avoid some problems with lost packages...).
        }
        // Need to have a separate configuration file for the second scanner...
        std::string calibration2;
        param_nh.param<std::string>("calibration2", calibration2, std::string(""));
        {
            if (calibration2 != std::string("")) {
                ros::NodeHandle nh_tmp("scan2");
                nh_tmp.setParam("calibration", calibration2);
                data2_->setup(nh_tmp);
                double max_range, min_range, view_direction, view_width;
                param_nh.param<double>("max_range2", max_range, 130.);
                param_nh.param<double>("min_range2", min_range, 2.);
                param_nh.param<double>("view_direction2", view_direction, 0.);
                param_nh.param<double>("view_width2", view_width, 6.3);
                data2_->setParameters(min_range, max_range, view_direction, view_width);        }
        }

        //////////////////////////////////////////////////////////
        /// Prepare Pose offsets
        //////////////////////////////////////////////////////////
        bool use_sensor_pose, use_initial_pose;
        double pose_init_x,pose_init_y,pose_init_z,
            pose_init_r,pose_init_p,pose_init_t;
        double sensor_pose_x,sensor_pose_y,sensor_pose_z,
            sensor_pose_r,sensor_pose_p,sensor_pose_t;

        param_nh.param<bool>("set_sensor_pose", use_sensor_pose, true);
        param_nh.param<bool>("set_initial_pose", use_initial_pose, false);
        param_nh.param<bool>("set_initial_pose_from_gt", use_initial_pose_from_gt, false);

        if(use_initial_pose) {
            ///initial pose of the vehicle with respect to the map
            param_nh.param("pose_init_x",pose_init_x,0.);
            param_nh.param("pose_init_y",pose_init_y,0.);
            param_nh.param("pose_init_z",pose_init_z,0.);
            param_nh.param("pose_init_r",pose_init_r,0.);
            param_nh.param("pose_init_p",pose_init_p,0.);
            param_nh.param("pose_init_t",pose_init_t,0.);
            initPoseT =  Eigen::Translation<double,3>(pose_init_x,pose_init_y,pose_init_z)*
                Eigen::AngleAxis<double>(pose_init_r,Eigen::Vector3d::UnitX()) *
                Eigen::AngleAxis<double>(pose_init_p,Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxis<double>(pose_init_t,Eigen::Vector3d::UnitZ()) ;

            hasInitialPose=true;
        } else {
            hasInitialPose=false;
        }

        if(use_sensor_pose) {
            ///pose of the sensor with respect to the vehicle odometry frame
            param_nh.param("sensor_pose_x",sensor_pose_x,0.);
            param_nh.param("sensor_pose_y",sensor_pose_y,0.);
            param_nh.param("sensor_pose_z",sensor_pose_z,0.);
            param_nh.param("sensor_pose_r",sensor_pose_r,0.);
            param_nh.param("sensor_pose_p",sensor_pose_p,0.);
            param_nh.param("sensor_pose_t",sensor_pose_t,0.);
            hasSensorPose = true;
            sensorPoseT =  Eigen::Translation<double,3>(sensor_pose_x,sensor_pose_y,sensor_pose_z)*
                Eigen::AngleAxis<double>(sensor_pose_r,Eigen::Vector3d::UnitX()) *
                Eigen::AngleAxis<double>(sensor_pose_p,Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxis<double>(sensor_pose_t,Eigen::Vector3d::UnitZ()) ;

            param_nh.param("sensor_pose2_x",sensor_pose_x,0.);
            param_nh.param("sensor_pose2_y",sensor_pose_y,0.);
            param_nh.param("sensor_pose2_z",sensor_pose_z,0.);
            param_nh.param("sensor_pose2_r",sensor_pose_r,0.);
            param_nh.param("sensor_pose2_p",sensor_pose_p,0.);
            param_nh.param("sensor_pose2_t",sensor_pose_t,0.);

            sensorPoseT2 =  Eigen::Translation<double,3>(sensor_pose_x,sensor_pose_y,sensor_pose_z)*
                Eigen::AngleAxis<double>(sensor_pose_r,Eigen::Vector3d::UnitX()) *
                Eigen::AngleAxis<double>(sensor_pose_p,Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxis<double>(sensor_pose_t,Eigen::Vector3d::UnitZ()) ;

        } else {
            hasSensorPose = false;
        }

        double sensor_time_offset;
        param_nh.param("sensor_time_offset", sensor_time_offset, 0.);
        sensorTimeOffset_ = ros::Duration(sensor_time_offset);
        param_nh.param("sensor_time_offset2", sensor_time_offset, 0.);
        sensorTimeOffset2_ = ros::Duration(sensor_time_offset);

        //////////////////////////////////////////////////////////
        /// Prepare the map
        //////////////////////////////////////////////////////////
        param_nh.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));
        param_nh.param<bool>("save_output_map", saveMap, true);
        param_nh.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));
        param_nh.param<double>("map_resolution", resolution , 0.2);
        param_nh.param<double>("subsample_level", subsample_level , 1);

        fprintf(stderr,"USING RESOLUTION %lf\n",resolution);

        perception_oru::NDTMap ndmap(new perception_oru::LazyGrid(resolution));
        ndmap.loadFromJFF(mapName.c_str());
        ROS_INFO_STREAM("Loaded map: " << mapName << " containing " << ndmap.getAllCells().size() << " cells");

        //////////////////////////////////////////////////////////
        /// Prepare UKF object
        //////////////////////////////////////////////////////////

        ndtukf = new NDTUKF3D(resolution,ndmap);
        UKF3D::Params ukf_params;
        param_nh.getParam("motion_model", ndtukf->motion_model);
        param_nh.getParam("motion_model_offset", ndtukf->motion_model_offset);
        param_nh.param<double>("resolution_sensor", ndtukf->resolution_sensor, resolution);
        param_nh.param<double>("ukf_range_var", ukf_params.range_var, 1.);
        param_nh.param<double>("ukf_alpha", ukf_params.alpha, 0.1);
        param_nh.param<double>("ukf_beta", ukf_params.beta, 2.);
        param_nh.param<double>("ukf_kappa", ukf_params.kappa, 3.);
        param_nh.param<double>("ukf_min_pos_var", ukf_params.min_pos_var, 0.01);
        param_nh.param<double>("ukf_min_rot_var", ukf_params.min_rot_var, 0.01);
        param_nh.param<double>("ukf_range_filter_max_dist", ukf_params.range_filter_max_dist, 1.);
        param_nh.param<int>("ukf_nb_ranges_in_update", ukf_params.nb_ranges_in_update, 100);

        ndtukf->setParamsUKF(ukf_params);

        ukf_pub = nh_.advertise<nav_msgs::Odometry>("ndt_ukf",10);

        //////////////////////////////////////////////////////////
        /// Prepare the callbacks and message filters
        //////////////////////////////////////////////////////////
        //the name of the TF link associated to the base frame / odometry frame
        param_nh.param<std::string>("tf_base_link", tf_base_link, std::string("/base_link"));
        //the name of the tf link associated to the 3d laser scanner
        param_nh.param<std::string>("tf_laser_link", tf_sensor_link, std::string("/velodyne_link"));
        param_nh.param<std::string>("tf_gt_link", tf_gt_link, std::string(""));

        ///topic to wait for packet data
        param_nh.param<std::string>("scan_topic", scan_topic, "");
        param_nh.param<std::string>("scan2_topic", scan2_topic, "");
        use_dual_scan = false;
        if (scan2_topic != std::string("")) {
            use_dual_scan = true;
            if (calibration2 == std::string("")) {
                ROS_ERROR("no calibration2 parameter given for the scan2 scanner!!!");
            }
        }

        ///topic to wait for point clouds
        param_nh.param<std::string>("points_topic",points_topic,"points");
        ///topic to wait for odometry messages
        param_nh.param<std::string>("odometry_topic",odometry_topic,"odometry");
        param_nh.param<std::string>("odometry_frame", odometry_frame, "/world");

        // If a scan_topic is provided, force to use it
        if (scan_topic != std::string("")) {
            ROS_INFO_STREAM("A scan topic is specified [will use that along with tf] : " << scan_topic);
            scan_sub_ = nh_.subscribe<velodyne_msgs::VelodyneScan>(scan_topic,10,&NDTUKF3DNode::scan_callback, this);
            if (scan2_topic != std::string("")) {
                scan2_sub_ = nh_.subscribe<velodyne_msgs::VelodyneScan>(scan2_topic,10,&NDTUKF3DNode::scan2_callback, this);
                ROS_INFO_STREAM("A scan2 topic is specified [will use that along with tf] : " << scan_topic);
            }
        }
        else {
            ROS_ERROR_STREAM("No scan_topic specified... quitting.");
            exit(-1);
        }

        isFirstLoad=true;
        pcounter =0;

        //////////////////////////////////////////////////////////
        /// Visualization
        //////////////////////////////////////////////////////////
        param_nh.param<bool>("do_visualize", do_visualize, true);
        param_nh.param<bool>("do_pub_ndt_markers", do_pub_ndt_markers_, true);
        param_nh.param<bool>("do_pub_sigmapoints_markers", do_pub_sigmapoints_markers_, true);
        param_nh.param<bool>("draw_ml_lines", draw_ml_lines, true);

        marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 3);
        markerarray_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 10);

        pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("interppoints", 15);
        pointcloud2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("ml_points", 15);

        heartbeat_slow_visualization_   = nh_.createTimer(ros::Duration(1.0),&NDTUKF3DNode::publish_visualization_slow,this);
        heartbeat_fast_visualization_   = nh_.createTimer(ros::Duration(0.1),&NDTUKF3DNode::publish_visualization_fast,this);

        //////////////////////////////////////////////////////////
        /// Evaluation
        //////////////////////////////////////////////////////////
        std::string gt_filename, gt2d_filename, est_filename, est2d_filename;
        param_nh.param<std::string>("gt_topic",gt_topic,"");
        param_nh.param<std::string>("output_gt_file",  gt_filename, "loc_gt_pose.txt");
        param_nh.param<std::string>("output_gt2d_file",  gt2d_filename, "loc_gt2d_pose.txt");
        param_nh.param<std::string>("output_est_file", est_filename, "loc_est_pose.txt");
        param_nh.param<std::string>("output_est2d_file", est2d_filename, "loc_est2d_pose.txt");
        if (gt_filename != std::string("")) {
            gt_file_.open(gt_filename.c_str());
            if (tf_gt_link == std::string("")) {
                ROS_WARN("tf_gt_link not set - will not log any gt data");
            }
        }
        if (gt2d_filename != std::string("")) {
            gt2d_file_.open(gt2d_filename.c_str());
            if (tf_gt_link == std::string("")) {
                ROS_WARN("tf_gt_link not set - will not log any gt data");
            }
        }

        if (est_filename != std::string("")) {
            est_file_.open(est_filename.c_str());
        }
        if (est2d_filename != std::string("")) {
            est2d_file_.open(est2d_filename.c_str());
        }

        if (!gt_file_.is_open() || !gt2d_file_.is_open() || !est_file_.is_open() || !est2d_file_.is_open())
        {

            ROS_ERROR_STREAM("Failed to open : " << est_file_.rdbuf() << " | " << est2d_file_.rdbuf() << " | " << gt_file_.rdbuf() << " | " << gt2d_file_.rdbuf());
        }

        if (gt_topic != std::string(""))
        {
            ROS_INFO_STREAM("Subscribing to : " << gt_topic);
            gt_sub = nh_.subscribe<nav_msgs::Odometry>(gt_topic,10,&NDTUKF3DNode::gt_callback, this);
        }

    }
    ~NDTUKF3DNode() {
        if (gt_file_.is_open())
            gt_file_.close();
        if (gt2d_file_.is_open())
            gt2d_file_.close();
        if (est_file_.is_open())
            est_file_.close();
        if (est2d_file_.is_open())
            est2d_file_.close();

        delete points2_sub_;
        delete odom_sub_;
        delete ndtukf;
    }

    void publish_visualization_fast(const ros::TimerEvent &event) {

        if (do_pub_sigmapoints_markers_)
        {
            ukf_m.lock();
            std::vector<Eigen::Affine3d> sigmas = ndtukf->getSigmasAsAffine3d();
            for (int i = 0; i < sigmas.size(); i++) {
                markerarray_pub_.publish(ndt_visualisation::getMarkerFrameAffine3d(sigmas[i], "sigmapoints" + ndt_generic::toString(i), 1., 0.1));
                std::cout << "sigmas[i] : " << ndt_generic::affine3dToStringRPY(sigmas[i]) << std::endl;
            }
            ukf_m.unlock();
        }
    }

    void publish_visualization_slow(const ros::TimerEvent &event) {
        if (do_pub_ndt_markers_)
        {
            // visualization_msgs::Marker markers_ndt;
            // ndt_visualisation::markerNDTCells2(*(graph->getLastFeatureFuser()->map),
            //                                    graph->getT(), 1, "nd_global_map_last", markers_ndt);
            // marker_pub_.publish(markers_ndt);
            ukf_m.lock();
            marker_pub_.publish(ndt_visualisation::markerNDTCells(ndtukf->map, 1, "nd_map"));
            ukf_m.unlock();
        }
    }

    void initialize() {
        //if not, check if initial robot pose has been set
        if(!hasInitialPose) {
            //can't do anything, wait for pose message...
            ROS_INFO("waiting for initial pose");
            return;
        }
        //initialize filter
        Eigen::Vector3d tr = initPoseT.translation();
        Eigen::Vector3d rot = initPoseT.rotation().eulerAngles(0,1,2);

        Todo_old=Todo;
        Tcum = initPoseT;

        //ndtukf->initializeFilter(initPoseT, 50, 50, 10, 100.0*M_PI/180.0, 100.0*M_PI/180.0 ,100.0*M_PI/180.0);
        // ndtukf->initializeFilter(initPoseT, 0.5, 0.5, 0.5, 2.0*M_PI/180.0, 2.0*M_PI/180.0 ,2.0*M_PI/180.0);
        //        ndtukf->initializeFilter(initPoseT, 2, 2, 2, 5.0*M_PI/180.0, 5.0*M_PI/180.0 ,5.0*M_PI/180.0);
        ndtukf->initializeFilter(initPoseT, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01);
        isFirstLoad = false;
    }

    // Simply to get an easier interface to tf.
    bool getTransformationForTime(const ros::Time &t0, const std::string &frame_id, tf::Transform &T) {
        bool success = false;
        try {
            success = tf_listener.waitForTransform(odometry_frame, frame_id, t0, ros::Duration(2.0));
        }
        catch (tf::TransformException &ex) {
            ROS_WARN_THROTTLE(100, "%s", ex.what());
            return false;
        }
        if (!success) {
            return false;
        }
        tf::StampedTransform Ts;
        tf_listener.lookupTransform (odometry_frame, frame_id, t0, Ts);
        T = Ts;
        return true;
    }

    // Simply to get an easier interface to tf.
    bool getRelativeTransformationForTime(const ros::Time &t0,const ros::Time &t1, const std::string &frame_id,tf::Transform &T)  {
        bool success = false;
        try {
            success = tf_listener.waitForTransform(frame_id, t0, frame_id, t1, odometry_frame, ros::Duration(2.0));
        }
        catch (tf::TransformException &ex) {
            ROS_WARN_THROTTLE(100, "%s", ex.what());
            return false;
        }
        if (!success) {
            return false;
        }
        tf::StampedTransform Ts;
        tf_listener.lookupTransform (frame_id, t0, frame_id, t1, odometry_frame, Ts);
        T = Ts;
        return true;
    }

    bool getTransformationAffine3d(const ros::Time &t0, const std::string &frame_id, Eigen::Affine3d &T) {
        tf::Transform tf_T;
        if (!getTransformationForTime(t0, frame_id, tf_T)) {
            ROS_ERROR_STREAM("Failed to get from /tf : " << frame_id);
            return false;
        }
        tf::transformTFToEigen(tf_T, T);
        return true;
    }

    // Access the scans directly. This to better handle the time offsets / perform interpolation. Anyway it is useful to have better access to the points.
    void scan_callback(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg) {
        ukf_m.lock();

        // Get the time stamp from the header, the pose at this time will be seen as the origin.
        ros::Time t0; // Corresponding time stamp which relates the scan to the odometry tf frame.
        t0 = scanMsg->header.stamp + sensorTimeOffset_;

        // If we are using the dual scan setup then we need to have the timestamp of the previous one in order to get syncronized pointclouds
        if (use_dual_scan) {
            if (scan2_cloud_.empty()) {
                ukf_m.unlock();
                return;
            }
            t0 = scan2_t0_ - sensorTimeOffset2_ + sensorTimeOffset_;
        }

        // Get the current odometry position (need to check the incremental odometry meassure since last call).
        {
            tf::Transform T;
            if (!getTransformationForTime(t0, tf_base_link, T)) {
                ROS_INFO_STREAM("Waiting for : " << tf_base_link);
                ukf_m.unlock();
                return;
            }
            tf::transformTFToEigen(T, Todo);
        }

        //check if we have done iterations
        if(isFirstLoad) {
            initialize();
            ukf_m.unlock();
            return;
        }

        Eigen::Affine3d Tm;
        Tm = Todo_old.inverse()*Todo;

        if(Tm.translation().norm()<0.01 && fabs(Tm.rotation().eulerAngles(0,1,2)[2])<(0.5*M_PI/180.0)) {
            ROS_INFO_STREAM("Not enough distance / rot traversed : " << Tm.translation().norm() << " / " << fabs(Tm.rotation().eulerAngles(0,1,2)[2]));
            ukf_m.unlock();
            return;
        }

        // We're interessted in getting the pointcloud in the vehicle frame (that is to transform it using the sensor pose offset).
        pcl::PointCloud<pcl::PointXYZ> cloud, cloud2;

        velodyne_rawdata::VPointCloud pnts,conv_points;
        tf::Transform T_sensorpose;
        tf::transformEigenToTF(sensorPoseT, T_sensorpose);

        for (size_t next = 0; next < scanMsg->packets.size(); ++next) {
            data_->unpack(scanMsg->packets[next], pnts);
            // Get the transformation of this scan packet (the vehicle pose)

            ros::Time t1 = scanMsg->packets[next].stamp + sensorTimeOffset_;
            // Get the relative pose...
            tf::Transform T;
            if (!getRelativeTransformationForTime(t0, t1, tf_base_link, T)) {
                continue;
            }

            tf::Transform Tcloud =  T * T_sensorpose;
            pcl_ros::transformPointCloud(pnts,conv_points,Tcloud);
            for (size_t i = 0; i < conv_points.size(); i++) {
                if (skip_nb_rings_ == 0) {
                    cloud.push_back(pcl::PointXYZ(conv_points.points[i].x,
                                                  conv_points.points[i].y,
                                                  conv_points.points[i].z));
                }
                else if (conv_points.points[i].ring % (skip_nb_rings_+1) == skip_start_ % (skip_nb_rings_+1)) {
                    cloud.push_back(pcl::PointXYZ(conv_points.points[i].x,
                                                  conv_points.points[i].y,
                                                  conv_points.points[i].z));
                }
                else {

                }
            }
            pnts.clear();
            conv_points.clear();
        }

        if (voxel_filter_size_ > 0) {
            // Create the filtering object
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>());
            *cloud2 = cloud;
            pcl::VoxelGrid<pcl::PointXYZ> sor;
            sor.setInputCloud (cloud2);
            sor.setLeafSize (voxel_filter_size_, voxel_filter_size_, voxel_filter_size_);
            pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
            sor.filter (cloud);

            // pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
            // pcl::VoxelGrid<PointType> downSizeFilterCorner;
            // downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);

            // downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);
            // downSizeFilterCorner.filter(*laserCloudCornerStack);

        }

        // Check size of the cloud
        if (cloud.size() < cloud_min_size_) {
            ukf_m.unlock();
            return;
        }

        if (use_dual_scan) {
            cloud += scan2_cloud_;
            scan2_cloud_.clear();
        }

        //
        Tcum = Tcum*Tm;
        Todo_old=Todo;

        //update filter -> + add parameter to subsample ndt map in filter step
        ndtukf->updateAndPredict/*Eff*/(Tm, cloud/*, subsample_level*/, sensorPoseT);
        //ndtukf->predict(Tm);

        {
            sensor_msgs::PointCloud2 pcloud;
            pcl::toROSMsg(ndtukf->getFilterRaw(), pcloud);
            pcloud.header.stamp = t0;
            pcloud.header.frame_id = "interppoints";
            pointcloud_pub_.publish(pcloud);
        }

        {
            sensor_msgs::PointCloud2 pcloud;
            pcl::toROSMsg(ndtukf->getFilterPred(),pcloud);
            pcloud.header.stamp = t0;
            pcloud.header.frame_id = "interppoints";
            pointcloud2_pub_.publish(pcloud);

            if (draw_ml_lines) {

                marker_pub_.publish(ndt_visualisation::getMarkerLineListFromTwoPointClouds(ndtukf->getFilterRaw(), ndtukf->getFilterPred(), 0, "ml_lines", "interppoints", 0.05));
            }
        }

        //publish pose
        sendROSOdoMessage(ndtukf->getMean(),t0);
        ukf_m.unlock();
    }

    // Simply store the point cloud, all the updates are from the scan_callback()
    void scan2_callback(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg) {
        ukf_m.lock();

        // Get the time stamp from the header, the pose at this time will be seen as the origin.
        ros::Time t0;
        t0 = scanMsg->header.stamp + sensorTimeOffset2_;
        scan2_t0_ = t0;
        // Get the current odometry position (need to check the incremental odometry meassure since last call).
        {
            tf::Transform T;
            if (!getTransformationForTime(t0, tf_base_link, T)) {
                ROS_INFO_STREAM("Waiting for : " << tf_base_link);
                ukf_m.unlock();
                return;
            }
            tf::transformTFToEigen(T, Todo);
        }

        //check if we have done iterations
        if(isFirstLoad) {
            initialize();
            ukf_m.unlock();
            return;
        }

        Eigen::Affine3d Tm;
        Tm = Todo_old.inverse()*Todo;

        if(Tm.translation().norm()<0.01 && fabs(Tm.rotation().eulerAngles(0,1,2)[2])<(0.5*M_PI/180.0)) {
            ukf_m.unlock();
            return;
        }

        // We're interessted in getting the pointcloud in the vehicle frame (that is to transform it using the sensor pose offset).
        pcl::PointCloud<pcl::PointXYZ> cloud;

        velodyne_rawdata::VPointCloud pnts,conv_points;
        tf::Transform T_sensorpose;
        tf::transformEigenToTF(sensorPoseT2, T_sensorpose);

        for (size_t next = 0; next < scanMsg->packets.size(); ++next) {
            data2_->unpack(scanMsg->packets[next], pnts);
            // Get the transformation of this scan packet (the vehicle pose)

            ros::Time t1 = scanMsg->packets[next].stamp + sensorTimeOffset_;
            // Get the relative pose...
            tf::Transform T;
            if (!getRelativeTransformationForTime(t0, t1, tf_base_link, T)) {
                continue;
            }

            tf::Transform Tcloud =  T * T_sensorpose;
            pcl_ros::transformPointCloud(pnts,conv_points,Tcloud);
            for (size_t i = 0; i < conv_points.size(); i++) {
                cloud.push_back(pcl::PointXYZ(conv_points.points[i].x,
                                              conv_points.points[i].y,
                                              conv_points.points[i].z));
            }
            pnts.clear();
            conv_points.clear();
        }

        // Check size of the cloud
        if (cloud.size() < 3000) {
            ukf_m.unlock();
            return;
        }

        scan2_cloud_ = cloud;

        ukf_m.unlock();
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    //
    bool sendROSOdoMessage(Eigen::Affine3d mean,ros::Time ts){
        nav_msgs::Odometry O;
        static int seq = 0;
        O.header.stamp = ts;
        O.header.seq = seq;
        O.header.frame_id = odometry_frame;
        O.child_frame_id = "/ukf_pose";

        O.pose.pose.position.x = mean.translation()[0];
        O.pose.pose.position.y = mean.translation()[1];
        O.pose.pose.position.z = mean.translation()[2];
        Eigen::Quaterniond q (mean.rotation());
        tf::Quaternion qtf;
        tf::quaternionEigenToTF (q, qtf);
        O.pose.pose.orientation.x = q.x();
        O.pose.pose.orientation.y = q.y();
        O.pose.pose.orientation.z = q.z();
        O.pose.pose.orientation.w = q.w();

        seq++;
        ukf_pub.publish(O);

        static tf::TransformBroadcaster br;
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(mean.translation()[0],mean.translation()[1], mean.translation()[2]) );

        transform.setRotation( qtf );
        br.sendTransform(tf::StampedTransform(transform, ts, "world", "ukf_pose"));

        if (est_file_.is_open()) {
            est_file_ << ts << " " << perception_oru::transformToEvalString(mean);
        }
        if (est2d_file_.is_open()) {
            est2d_file_ << ts << " " << perception_oru::transformToEval2dString(mean);
        }
        if (gt_file_.is_open() && tf_gt_link != std::string("")) {
            Eigen::Affine3d T_gt;
            if (getTransformationAffine3d(ts, tf_gt_link, T_gt)) {
                gt_file_ << ts << " " << perception_oru::transformToEvalString(T_gt);
            }
            if (gt2d_file_.is_open()) {
                gt2d_file_ << ts << " " << perception_oru::transformToEval2dString(T_gt);
            }
        }

        return true;
    }

    // Callback
    void gt_callback(const nav_msgs::Odometry::ConstPtr& msg_in)
    {
        Eigen::Quaterniond qd;
        Eigen::Affine3d gt_pose;

        qd.x() = msg_in->pose.pose.orientation.x;
        qd.y() = msg_in->pose.pose.orientation.y;
        qd.z() = msg_in->pose.pose.orientation.z;
        qd.w() = msg_in->pose.pose.orientation.w;

        gt_pose = Eigen::Translation3d (msg_in->pose.pose.position.x,
                                        msg_in->pose.pose.position.y,msg_in->pose.pose.position.z) * qd;

        // Query the pose from the /tf instead (see abouve in sendOdoMessage).
        // if (gt_file_.is_open()) {
        //   gt_file_ << msg_in->header.stamp << " " << lslgeneric::transformToEvalString(gt_pose);
        // }

        // m.lock();
        if(use_initial_pose_from_gt && !hasInitialPose) {
            hasInitialPose = true;
            ROS_INFO("Set initial pose from GT track");
            initPoseT = gt_pose;
        }
        // m.unlock();
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////////

};

int main(int argc, char **argv){
    ros::init(argc, argv, "NDT-UKF");
    ros::NodeHandle paramHandle ("~");
    NDTUKF3DNode ukfnode(paramHandle);
    ros::spin();
    return 0;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, nearbyint_, boost::dispatch::elementwise_<nearbyint_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, nearbyint_)
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::nearbyint_,nearbyint);

} }

#endif


#ifndef STAN_MATH_PRIM_SCAL_PROB_POISSON_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_POISSON_RNG_HPP

#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_less.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/gamma_q.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

template <class RNG>
inline int poisson_rng(double lambda, RNG& rng) {
  using boost::random::poisson_distribution;
  using boost::variate_generator;

  static const char* function = "poisson_rng";

  check_not_nan(function, "Rate parameter", lambda);
  check_nonnegative(function, "Rate parameter", lambda);
  check_less(function, "Rate parameter", lambda, POISSON_MAX_RATE);

  variate_generator<RNG&, poisson_distribution<> > poisson_rng(
      rng, poisson_distribution<>(lambda));
  return poisson_rng();
}

}  // namespace math
}  // namespace stan
#endif


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/Core/Log.h>
#include <vw/Camera/PinholeModel.h>
#include <vw/Math/EulerAngles.h>

#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
#include <vw/Math/LinearAlgebra.h>
#endif

// For std::setprecision
#include <iomanip>

#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
#include <vw/Camera/TsaiFile.pb.h>
using google::protobuf::RepeatedFieldBackInserter;
#endif

#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

using namespace vw;

// Old deprecated format of Pinhole I/O. Didn't support all distortion options.
// Reads in a file containing parameters of a pinhole model with
// a tsai lens distortion model. An example is provided at the end of this file.
void camera::PinholeModel::read_old_file(std::string const& filename) {

  char line[2048];
  double fu, fv, cu, cv;
  Vector3 u_direction, v_direction, w_direction;
  Vector3 C;
  Matrix3x3 R;
  Vector4 distortion_params(0,0,0,0);

  FILE *cam_file = fopen(filename.c_str(), "r");
  if (cam_file == 0) vw_throw( IOErr() << "PinholeModel::read_file: Could not open file\n" );

  // Read intrinsic parameters
  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"fu = %lf", &fu) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x focal length\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"fv = %lf", &fv) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y focal length\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"cu = %lf", &cu) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x principal point\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"cv = %lf", &cv) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y principal point\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"u_direction = %lf %lf %lf", &u_direction(0), &u_direction(1), &u_direction(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read u direction vector\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"v_direction = %lf %lf %lf", &v_direction(0), &v_direction(1), &v_direction(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read v direction vector\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"w_direction = %lf %lf %lf", &w_direction(0), &w_direction(1), &w_direction(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read w direction vector\n" );
  }

  // Read extrinsic parameters
  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"C = %lf %lf %lf", &C(0), &C(1), &C(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file: Could not read C (camera center) vector\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if ( sscanf(line, "R = %lf %lf %lf %lf %lf %lf %lf %lf %lf",
              &R(0,0), &R(0,1), &R(0,2),
              &R(1,0), &R(1,1), &R(1,2),
              &R(2,0), &R(2,1), &R(2,2)) != 9 ) {
      fclose(cam_file);
      vw_throw( IOErr() << "PinholeModel::read_file(): Could not read rotation matrix\n" );
  }

  // Read distortion parameters.
   fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"k1 = %lf", &distortion_params[0] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter k1\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"k2 = %lf", &distortion_params[1] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter k2\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"p1 = %lf", &distortion_params[2] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter p1\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"p2 = %lf", &distortion_params[3] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter p2\n" );
  }

  fclose(cam_file);

  m_u_direction = u_direction;
  m_v_direction = v_direction;
  m_w_direction = w_direction;
  m_pixel_pitch = 1;

  m_fu = fu;
  m_fv = fv;
  m_cu = cu;
  m_cv = cv;
  m_camera_center = C;

  m_rotation = R;
  this->rebuild_camera_matrix();

  if( distortion_params == Vector4(0,0,0,0))
    m_distortion.reset(new NullLensDistortion());
  else
    m_distortion.reset(new TsaiLensDistortion(distortion_params));
}

// Reads in a file containing parameters of a pinhole model with
// a tsai lens distortion model.
void camera::PinholeModel::read_file(std::string const& filename) {
  this->read(filename);
}

void camera::PinholeModel::read(std::string const& filename) {

  fs::path filename_path( filename );
  if ( filename_path.extension() == ".pinhole" ) {
#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
    std::fstream input( filename.c_str(), std::ios::in | std::ios::binary );
    if ( !input )
      vw_throw( IOErr() << "Pinhole::read_file: Could not open " << filename << "\n" );
    TsaiFile file;
    if ( !file.ParseFromIstream( &input ) )
      vw_throw( IOErr() << "Pinhole::read_file: Protocol buffer failed to parse \"" << filename << "\"\n" );
    input.close();

    // Making sure protobuf seems correct
    VW_ASSERT( file.focal_length_size() == 2,
               IOErr() << "Pinhole::read_file: Unexpected amount of focal lengths." );
    VW_ASSERT( file.center_point_size() == 2,
               IOErr() << "Pinhole::read_file: Unexpected amount of center points." );
    VW_ASSERT( file.u_direction_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of u vector." );
    VW_ASSERT( file.v_direction_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of v vector." );
    VW_ASSERT( file.w_direction_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of w vector." );
    VW_ASSERT( file.camera_center_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of camera vector." );
    VW_ASSERT( file.camera_rotation_size() == 9,
               IOErr() << "Pinhole::read_file: Unexpected size of rotation matrix." );

    typedef VectorProxy<double,3> Vector3P;
    m_u_direction = Vector3P(file.mutable_u_direction()->mutable_data());
    m_v_direction = Vector3P(file.mutable_v_direction()->mutable_data());
    m_w_direction = Vector3P(file.mutable_w_direction()->mutable_data());
    m_camera_center = Vector3P(file.mutable_camera_center()->mutable_data());
    m_fu = file.focal_length(0);
    m_fv = file.focal_length(1);
    m_cu = file.center_point(0);
    m_cv = file.center_point(1);
    m_rotation = MatrixProxy<double,3,3>(file.mutable_camera_rotation()->mutable_data());
    m_pixel_pitch = file.pixel_pitch();

    this->rebuild_camera_matrix();

    if ( file.distortion_name() == "NULL" ) {
      VW_ASSERT( file.distortion_vector_size() == 0,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new NullLensDistortion());
    } else if ( file.distortion_name() == "TSAI" ) {
      VW_ASSERT( file.distortion_vector_size() == 4,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new TsaiLensDistortion(VectorProxy<double,4>(file.mutable_distortion_vector()->mutable_data())));
    } else if ( file.distortion_name() == "BROWNCONRADY" ) {
      VW_ASSERT( file.distortion_vector_size() == 8,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new BrownConradyDistortion(VectorProxy<double,8>(file.mutable_distortion_vector()->mutable_data())));
    } else if ( file.distortion_name() == "AdjustableTSAI" ) {
      VW_ASSERT( file.distortion_vector_size() > 3,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new AdjustableTsaiLensDistortion(VectorProxy<double>(file.distortion_vector_size(),file.mutable_distortion_vector()->mutable_data())));
    }
#else
    // If you hit this point, you need to install Google Protobuffers to
    // be in order to write.
    vw_throw( IOErr() << "Pinhole::write_file: Camera IO not supported without Google Protobuffers" );
#endif
  } else if ( filename_path.extension() == ".tsai" ) {
    this->read_old_file( filename );
  } else {
    vw_throw( IOErr() << "Unknown PinholeModel filename extension \""
              << filename_path.extension() << "\"" );
  }
}

// Write parameters of an exiting PinholeModel into a .tsai file for later use.
void camera::PinholeModel::write_file(std::string const& filename) const {
  write(filename);
}
void camera::PinholeModel::write(std::string const& filename) const {
#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
  std::string output_file =
    fs::path(filename).replace_extension(".pinhole").string();

  TsaiFile file;
  file.add_focal_length( m_fu );
  file.add_focal_length( m_fv );
  file.add_center_point( m_cu );
  file.add_center_point( m_cv );
  file.set_pixel_pitch( m_pixel_pitch );

  std::copy(m_u_direction.begin(), m_u_direction.end(),
            RepeatedFieldBackInserter(file.mutable_u_direction()));
  std::copy(m_v_direction.begin(), m_v_direction.end(),
            RepeatedFieldBackInserter(file.mutable_v_direction()));
  std::copy(m_w_direction.begin(), m_w_direction.end(),
            RepeatedFieldBackInserter(file.mutable_w_direction()));
  std::copy(m_camera_center.begin(), m_camera_center.end(),
            RepeatedFieldBackInserter(file.mutable_camera_center()));

  std::copy(m_rotation.begin(), m_rotation.end(),
            RepeatedFieldBackInserter(file.mutable_camera_rotation()));

  file.set_distortion_name( m_distortion->name() );
  Vector<double> distort_vec = m_distortion->distortion_parameters();
  std::copy(distort_vec.begin(),distort_vec.end(),
            RepeatedFieldBackInserter(file.mutable_distortion_vector()));

  std::ofstream output(output_file.c_str());
  if( !output.is_open() )
    vw_throw( IOErr() << "PinholeModel::write_file: Could not open file\n" );
  file.SerializeToOstream( &output );
  output.close();
#else
  // If you hit this point, you need to install Google Protobuffers to
  // be in order to write.
  vw_throw( IOErr() << "Pinhole::write_file: Camera IO not supported without Google Protobuffers" );
#endif
}

Vector2 camera::PinholeModel::point_to_pixel(Vector3 const& point) const {

  //  Multiply the pixel location by the camera matrix.
  double denominator = m_camera_matrix(2,0)*point(0) + m_camera_matrix(2,1)*point(1) +
    m_camera_matrix(2,2)*point(2) + m_camera_matrix(2,3);
  Vector2 pixel = Vector2( (m_camera_matrix(0,0)*point(0) + m_camera_matrix(0,1)*point(1) +
                            m_camera_matrix(0,2)*point(2) + m_camera_matrix(0,3)) / denominator,
                           (m_camera_matrix(1,0)*point(0) + m_camera_matrix(1,1)*point(1) +
                            m_camera_matrix(1,2)*point(2) + m_camera_matrix(1,3)) / denominator);

  //  Apply the lens distortion model
  return m_distortion->distorted_coordinates(*this, pixel)/m_pixel_pitch;
}

bool camera::PinholeModel::projection_valid(Vector3 const& point) const {
  // z coordinate after extrinsic transformation
  double z = m_extrinsics(2, 0)*point(0) + m_extrinsics(2, 1)*point(1) +
    m_extrinsics(2, 2)*point(2) + m_extrinsics(2,3);
  return z > 0;
}

Vector3 camera::PinholeModel::pixel_to_vector (Vector2 const& pix) const {
  // Apply the inverse lens distortion model
  Vector2 undistorted_pix = m_distortion->undistorted_coordinates(*this, pix*m_pixel_pitch);

  // Compute the direction of the ray emanating from the camera center.
  Vector3 p(0,0,1);
  subvector(p,0,2) = undistorted_pix;
  return normalize( m_inv_camera_transform * p);
}

void camera::PinholeModel::intrinsic_parameters(double& f_u, double& f_v,
                                                double& c_u, double& c_v) const {
  f_u = m_fu;  f_v = m_fv;  c_u = m_cu;  c_v = m_cv;
}

void camera::PinholeModel::set_intrinsic_parameters(double f_u, double f_v,
                                                    double c_u, double c_v) {
  m_fu = f_u;  m_fv = f_v;  m_cu = c_u;  m_cv = c_v;
  rebuild_camera_matrix();
}

void camera::PinholeModel::set_camera_matrix( Matrix<double,3,4> const& p ) {
#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
  // Solving for camera center
  Matrix<double> cam_nullsp = nullspace(p);
  Vector<double> cam_center = select_col(cam_nullsp,0);
  cam_center /= cam_center[3];
  m_camera_center = subvector(cam_center,0,3);

  // Solving for intrinsics with RQ decomposition
  Matrix<double> M = submatrix(p,0,0,3,3);
  Matrix<double> R,Q;
  rqd( M, R, Q );
  Matrix<double> sign_fix(3,3);
  sign_fix.set_identity();
  if ( R(0,0) < 0 )
    sign_fix(0,0) = -1;
  if ( R(1,1) < 0 )
    sign_fix(1,1) = -1;
  if ( R(2,2) < 0 )
    sign_fix(2,2) = -1;
  R = R*sign_fix;
  Q = sign_fix*Q;
  R /= R(2,2);

  // Pulling out intrinsic and last extrinsic
  Matrix<double,3,3> uvwRotation;
  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;
  m_rotation = inverse(uvwRotation*Q);
  m_fu = R(0,0);
  m_fv = R(1,1);
  m_cu = R(0,2);
  m_cv = R(1,2);

  if ( fabs(R(0,1)) >= 1.2 )
    vw_out(WarningMessage,"camera") << "Significant skew not modelled by pinhole camera\n";

  // Rebuild
  rebuild_camera_matrix();
#else
  vw_throw( NoImplErr() << "PinholeModel::set_Camera_Matrix is unavailable without LAPACK" );
#endif
}

void camera::PinholeModel::rebuild_camera_matrix() {

  /// The intrinsic portion of the camera matrix is stored as
  ///
  ///    [  fx   0   cx  ]
  /// K= [  0    fy  cy  ]
  ///    [  0    0   1   ]
  ///
  /// with fx, fy the focal length of the system (in horizontal and
  /// vertical pixels), and (cx, cy) the pixel coordinates of the
  /// central pixel (the principal point on the image plane).

  m_intrinsics(0,0) = m_fu;
  m_intrinsics(0,1) = 0;
  m_intrinsics(0,2) = m_cu;
  m_intrinsics(1,0) = 0;
  m_intrinsics(1,1) = m_fv;
  m_intrinsics(1,2) = m_cv;
  m_intrinsics(2,0) = 0;
  m_intrinsics(2,1) = 0;
  m_intrinsics(2,2) = 1;

  // The extrinsics are normally built as the matrix:  [ R | -R*C ].
  // To allow for user-specified coordinate frames, the
  // extrinsics are now build to include the u,v,w rotation
  //
  //               | u_0  u_1  u_2  |
  //     Extr. =   | v_0  v_1  v_2  | * [ R | -R*C]
  //               | w_0  w_1  w_2  |
  //
  // The vectors u,v, and w must be orthonormal.

  /*   check for orthonormality of u,v,w              */
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_v_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_v_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( fabs( norm_2(m_u_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_v_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_w_direction) - 1 ) < 0.001 );

  Matrix<double,3,3> uvwRotation;

  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;

  Matrix<double,3,3> m_rotation_inverse = transpose(m_rotation);
  submatrix(m_extrinsics,0,0,3,3) = uvwRotation * m_rotation_inverse;
  select_col(m_extrinsics,3) = uvwRotation * -m_rotation_inverse * m_camera_center;

  m_camera_matrix = m_intrinsics * m_extrinsics;
  m_inv_camera_transform = inverse(uvwRotation*m_rotation_inverse) * inverse(m_intrinsics);
}

// scale_camera
//  Used to modify camera in the event to user resizes the image
camera::PinholeModel
camera::scale_camera(camera::PinholeModel const& camera_model,
                     float const& scale) {
  Vector2 focal = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  focal *= scale;
  offset *= scale;
  boost::shared_ptr<LensDistortion> lens = camera_model.lens_distortion()->copy();
  lens->scale( scale );
  return camera::PinholeModel( camera_model.camera_center(),
                               camera_model.camera_pose().rotation_matrix(),
                               focal[0], focal[1], offset[0], offset[1],
                               camera_model.coordinate_frame_u_direction(),
                               camera_model.coordinate_frame_v_direction(),
                               camera_model.coordinate_frame_w_direction(),
                               *lens );
}

//   /// Given two pinhole camera models, this method returns two new camera
//   /// models that have been epipolar rectified.
//   template <>
//   void epipolar(PinholeModel<NoLensDistortion> const& src_camera0,
//                 PinholeModel<NoLensDistortion> const& src_camera1,
//                 PinholeModel<NoLensDistortion> &dst_camera0,
//                 PinholeModel<NoLensDistortion> &dst_camera1);
camera::PinholeModel
camera::linearize_camera(camera::PinholeModel const& camera_model) {
  Vector2 focal = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  camera::NullLensDistortion distortion;
  return camera::PinholeModel(camera_model.camera_center(),
      camera_model.camera_pose().rotation_matrix(),
      focal[0], focal[1], offset[0], offset[1],
      camera_model.coordinate_frame_u_direction(),
      camera_model.coordinate_frame_v_direction(),
      camera_model.coordinate_frame_w_direction(),
      distortion);
}

std::ostream& camera::operator<<(std::ostream& str,
                                 camera::PinholeModel const& model) {
  str << "Pinhole camera: \n";
  str << "\tCamera Center: " << model.camera_center() << "\n";
  str << "\tRotation Matrix: " << model.camera_pose() << "\n";
  str << "\tIntrinsics:\n";
  str << "\t  focal: " << model.focal_length() << "\n";
  str << "\t  offset: " << model.point_offset() << "\n";
  str << "\tDistortion Model: " << model.lens_distortion()->name() << "\n";
  str << "\t  " << *model.lens_distortion() << "\n";

  return str;
}


//
// Tracker time cluster finder
//
//
// Original author D. Brown and G. Tassielli
//
// framework
#include "art/Framework/Principal/Event.h"
#include "fhiclcpp/ParameterSet.h"
#include "art/Framework/Principal/Handle.h"
#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "art_root_io/TFileService.h"
// Mu2e
#include "GeneralUtilities/inc/Angles.hh"
#include "Mu2eUtilities/inc/MVATools.hh"
#include "Mu2eUtilities/inc/polyAtan2.hh"
// data
#include "RecoDataProducts/inc/ComboHit.hh"
#include "RecoDataProducts/inc/StrawHitFlag.hh"
#include "RecoDataProducts/inc/TimeCluster.hh"
#include "RecoDataProducts/inc/CaloCluster.hh"
// tracking
#include "TrkReco/inc/TrkUtilities.hh"
#include "TrkReco/inc/TrkTimeCalculator.hh"
// root
#include "TH1F.h"
// boost
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
// C++
#include <memory>
#include <algorithm>
#include <utility>
using namespace std;
using namespace boost::accumulators;

namespace {

  struct TimeCluMVA
  {
    vector<Float_t> _pars;
    Float_t& _dt;
    Float_t& _dphi;
    Float_t& _rho;
    Float_t& _nsh;
    Float_t& _plane;
    Float_t& _werr;
    Float_t& _wdist;

    TimeCluMVA() : _pars(7,0.0), _dt(_pars[0]), _dphi(_pars[1]), _rho(_pars[2]), _nsh(_pars[3]),
     _plane(_pars[4]), _werr(_pars[5]), _wdist(_pars[6]){}
//    TimeCluMVA() : _pars(5,0.0), _dt(_pars[0]), _nsh(_pars[1]),
//      _plane(_pars[2]), _werr(_pars[3]), _wdist(_pars[4]){}
  };
}

namespace mu2e {

  class TimeClusterFinder : public art::EDProducer
  {
    public:

        struct Config
        {
            using Name    = fhicl::Name;
            using Comment = fhicl::Comment;
            fhicl::Atom<art::InputTag>              comboHitCollection     {Name("ComboHitCollection"),     Comment("ComboHit collection {Name") };
            fhicl::Atom<art::InputTag>              strawHitFlagCollection {Name("StrawHitFlagCollection"), Comment("StrawHitFlag collection {Name") };
            fhicl::Atom<art::InputTag>              caloClusterCollection  {Name("CaloClusterCollection"),  Comment("Calo cluster collection {Name") };
            fhicl::Table<MVATools::Config>          tcMVA                  {Name("ClusterMVA"),             Comment("MVA for time cluster cleaning") };
            fhicl::Table<MVATools::Config>          tcCaloMVA              {Name("ClusterCaloMVA"),         Comment("MVA for time clsuter cleaning with calo") };
            fhicl::Sequence<std::string>            hsel                   {Name("HitSelectionBits"),       Comment("HitSelectionBits") };
            fhicl::Sequence<std::string>            hbkg                   {Name("HitBackgroundBits"),      Comment("HitBackgroundBits") };
            fhicl::Atom<bool>                       usecc                  {Name("UseCaloCluster"),         Comment("Use calorimeter cluster") };
            fhicl::Atom<bool>                       useccpos               {Name("UseCaloClusterPosition"), Comment("Use calorimeter cluster position") };
            fhicl::Atom<float>                      ccmine                 {Name("CaloClusterMinE"),        Comment("Minimum energy for calorimeter cluster") };
            fhicl::Atom<float>                      ccweight               {Name("CaloClusterWeight"),      Comment("Weight of cluster in tracker hits") };
            fhicl::Table<TrkTimeCalculator::Config> ttcalc                 {Name("T0Calculator"),           Comment("TimeTracker calculator config") };
            fhicl::Atom<bool>                       testflag               {Name("TestFlag"),               Comment("Test hit flags") };
            fhicl::Atom<float>                      maxdt                  {Name("DtMax"),                  Comment("Maximum delta time for hit in cluster") };
            fhicl::Atom<unsigned>                   minnhits               {Name("MinNHits"),               Comment("Minimum number of hits for cluster") };
            fhicl::Atom<float>                      minkeepmva             {Name("MinKeepHitMVA"),          Comment("Minimum MVA score to keep in cluster") };
            fhicl::Atom<float>                      minaddmva              {Name("MinAddHitMVA"),           Comment("Minimum MVA score to add in cluster") };
            fhicl::Atom<float>                      maxdPhi                {Name("MaxdPhi"),                Comment("Maximum delta Phi for hit to be in cluster") };
            fhicl::Atom<float>                      tmin                   {Name("Tmin"),                   Comment("Time histogram start") };
            fhicl::Atom<float>                      tmax                   {Name("Tmax"),                   Comment("Time histogram end") };
            fhicl::Atom<float>                      tbin                   {Name("Tbin"),                   Comment("Time histogram bin width") };
            fhicl::Atom<float>                      pitch                  {Name("AveragePitch"),           Comment("Average helix pitch (= dz/dflight, =sin(lambda)") };
            fhicl::Atom<float>                      ymin                   {Name("Ymin"),                   Comment("Minimum hit in time histo bin for peak") };
            fhicl::Atom<bool>                       recover                {Name("RefineClusters"),         Comment("Apply hit refining algorithm") };
            fhicl::Atom<bool>                       refine                 {Name("PrefilterCluster"),       Comment("Apply hit pre-filtering algorithm") };
            fhicl::Atom<bool>                       preFilter              {Name("RecoverHits"),            Comment("Apply hit recovery algorithm") };
            fhicl::Atom<int>                        npeak                  {Name("PeakWidth"),              Comment("Time Peak Width") };
            fhicl::Atom<int>                        printfreq              {Name("printFrequency"),         Comment("Print frequency"), 100 };
            fhicl::Atom<int>                        debugLevel             {Name("debugLevel"),             Comment("Debut Level"), 0 };
        };

        explicit TimeClusterFinder(const art::EDProducer::Table<Config>& config);

        void beginJob() override;
        void produce(art::Event& e) override;

    private:
       typedef std::pair<Float_t,int> BinContent;
       typedef std::vector<StrawHitIndex>::iterator ISH;

       int                                             _iev;
       const art::ProductToken<ComboHitCollection>     _chToken;
       const art::ProductToken<StrawHitFlagCollection> _shfToken;
       const art::ProductToken<CaloClusterCollection>  _ccToken;
       const StrawHitFlagCollection* _shfcol;
       const ComboHitCollection*     _chcol;
       const CaloClusterCollection*  _cccol;
       StrawHitFlag                  _hsel;
       StrawHitFlag                  _hbkg;
       MVATools                      _tcMVA;
       MVATools                      _tcCaloMVA;
       bool                          _usecc, _useccpos;
       float                         _ccmine, _ccwt;
       TrkTimeCalculator             _ttcalc;
       bool                          _testflag;
       float                         _maxdt;
       unsigned                      _minnhits;
       float                         _minkeepmva, _minaddmva;
       float                         _maxdPhi;
       float                         _tmin, _tmax, _tbin;
       float		             _pitch;
       float                         _ymin;
       bool                          _refine;
       bool                          _preFilter;
       bool                          _recover;
       int                           _npeak;
       int                           _printfreq;
       int                           _debug;
       TH1F                          _timespec;
       TimeCluMVA                    _pmva; // input variables to TMVA for cluster cleaning

      void findClusters(TimeClusterCollection& tccol);
      void findCaloSeeds(TimeClusterCollection& tccol, art::Handle<CaloClusterCollection> const& ccH);
      void fillTimeSpectrum();
      void initCluster(TimeCluster& tc);
      void prefilterCluster(TimeCluster& tc);
      void recoverHits(TimeCluster& tc);
      ISH  removeHit(TimeCluster& tc, ISH);
      void addHit(TimeCluster& tc,size_t iadd);
      void clusterMean(TimeCluster& tc);
      void refineCluster(TimeCluster& tc);
      void findPeaks(TimeClusterCollection& seeds);
      void assignHits(TimeClusterCollection& tccol );
      bool goodHit(const StrawHitFlag& flag) const;
  };

  TimeClusterFinder::TimeClusterFinder(const art::EDProducer::Table<Config>& config) :
     art::EDProducer{config},
     _chToken      { consumes<ComboHitCollection>(      config().comboHitCollection()) },
     _shfToken     { mayConsume<StrawHitFlagCollection>(config().strawHitFlagCollection()) },
     _ccToken      { mayConsume<CaloClusterCollection>( config().caloClusterCollection()) },
     _hsel         ( config().hsel()),
     _hbkg         ( config().hbkg()),
     _tcMVA        ( config().tcMVA()),
     _tcCaloMVA    ( config().tcCaloMVA()),
     _usecc        ( config().usecc()),
     _useccpos     ( config().useccpos()),
     _ccmine       ( config().ccmine()),
     _ccwt         ( config().ccweight()),
     _ttcalc       ( config().ttcalc()),
     _testflag     ( config().testflag()),
     _maxdt        ( config().maxdt()),
     _minnhits     ( config().minnhits()),
     _minkeepmva   ( config().minkeepmva()),
     _minaddmva    ( config().minaddmva()),
     _maxdPhi      ( config().maxdPhi()),
     _tmin         ( config().tmin()),
     _tmax         ( config().tmax()),
     _tbin         ( config().tbin()),
     _pitch        ( config().pitch()),
     _ymin         ( config().ymin()),
     _refine       ( config().refine()),
     _preFilter    ( config().preFilter()),
     _recover      ( config().recover()),
     _npeak        ( config().npeak()),
     _printfreq    ( config().printfreq()),
     _debug        ( config().debugLevel())
    {
        unsigned nbins = (unsigned)rint((_tmax-_tmin)/_tbin);
        _timespec = TH1F("timespec","time spectrum",nbins,_tmin,_tmax);
        produces<TimeClusterCollection>();
    }

  void TimeClusterFinder::beginJob() {
    _tcMVA.initMVA();
    _tcCaloMVA.initMVA();
    if (_debug > 0)
    {
      std::cout << "TimeClusterFinder MVA : " << std::endl;
      _tcMVA.showMVA();
      std::cout << "TimeClusterFinder Calo MVA : " << std::endl;
      _tcCaloMVA.showMVA();
    }
  }

  //--------------------------------------------------------------------------------------------------------------
  void TimeClusterFinder::produce(art::Event & event ){
    _iev = event.id().event();

    if (_debug > 0 && (_iev%_printfreq)==0) std::cout<<"TimeClusterFinder: event="<<_iev<<std::endl;

    auto const& chH = event.getValidHandle(_chToken);
    _chcol = chH.product();

    art::Handle<CaloClusterCollection> ccH{}; // need to cache for later Ptr creation
    if(_usecc){
      ccH = event.getHandle<CaloClusterCollection>(_ccToken);
      _cccol = ccH.product();
    }

    if(_testflag){
      auto shfH = event.getValidHandle(_shfToken);
      _shfcol = shfH.product();
      if(_shfcol->size() != _chcol->size())
	throw cet::exception("RECO")<<"TimeClusterFinder: inconsistent flag collection length " << endl;
    }

    std::unique_ptr<TimeClusterCollection> tccol(new TimeClusterCollection);
    // If requested, use calo clusters to for time cluster seeds
    if (_usecc) findCaloSeeds(*tccol,ccH);
    // find all the hit clusters
    findClusters(*tccol);

    if (_debug > 0) std::cout << "Found " << tccol->size() << " Time Clusters " << std::endl;

    if (_debug > 1){
      for(auto const& tc : *tccol) {
	std::cout << "Time Cluster time = " << tc.t0().t0() << " +- " << tc.t0().t0Err()
	  << " position = " << tc._pos << std::endl;
	if(_debug > 3){
	  for (auto shi : tc._strawHitIdxs ) {
	    std::cout << "Time Cluster hit at index " << shi << std::endl;
	  }
	}
      }
    }
    event.put(std::move(tccol));
  }

  //--------------------------------------------------------------------------------------------------------------
  void TimeClusterFinder::findClusters(TimeClusterCollection& tccol) {
    // find seed from hits
    fillTimeSpectrum();
    findPeaks(tccol);
    // associate hits to seeds
    assignHits(tccol);
    // loop over seeds and fill/refine information
    auto itc = tccol.begin();
    while(itc != tccol.end()){
      TimeCluster& tc = *itc;
      initCluster(tc);
      if (_preFilter) prefilterCluster(tc);
      if( tc.nStrawHits() >= _minnhits) {
	clusterMean(tc);
	if (_refine) refineCluster(tc);
	if (_recover) recoverHits(tc);
      }
      if (tc.nStrawHits() < _minnhits) {
	itc = tccol.erase(itc);
      } else
	++itc;
      //std::cout<<"Collection size final"<<tc._strawHitIdxs.size()<<std::endl;
    }
    // debug test of histogram
    if (_debug > 2) {
      art::ServiceHandle<art::TFileService> tfs;
      TH1F* tspec = tfs->make<TH1F>(_timespec);
      char name[40];
      char title[100];
      snprintf(name,40,"tspec_%i",_iev);
      snprintf(title,100,"time spectrum event %i;nsec",_iev);
      tspec->SetNameTitle(name,title);
    }
  }

  void TimeClusterFinder::findCaloSeeds(TimeClusterCollection& tccol, art::Handle<CaloClusterCollection>const& ccH) {
    for(size_t icalo=0; icalo < _cccol->size(); ++icalo){
      auto const& calo = (*_cccol)[icalo];
      if (calo.energyDep() > _ccmine){
	TimeCluster tc;
	tc._t0 = TrkT0(_ttcalc.caloClusterTime(calo,_pitch), _ttcalc.caloClusterTimeErr());
	tc._caloCluster = art::Ptr<CaloCluster>(ccH,icalo);
	tccol.push_back(tc);
      }
    }
  }

  //--------------------------------------------------------------------------------------------------------------
  void TimeClusterFinder::fillTimeSpectrum() {
    _timespec.Reset();
    for (unsigned istr=0; istr<_chcol->size();++istr) {
      if (_testflag && !goodHit((*_shfcol)[istr])) continue;
      ComboHit const& ch = (*_chcol)[istr];
      float time = _ttcalc.comboHitTime((*_chcol)[istr],_pitch);
      _timespec.Fill(time,ch.nStrawHits());
    }
  }

  void TimeClusterFinder::assignHits(TimeClusterCollection& tccol ) {
  // assign hits to the closest time peak
    for(size_t istr=0; istr<_chcol->size(); ++istr) {
      if ((!_testflag) || goodHit((*_shfcol)[istr])) {
	ComboHit const& ch =(*_chcol)[istr];
	float time = _ttcalc.comboHitTime(ch,_pitch);
	float mindt(1e5);
	auto besttc = tccol.end();
	// find the closest seed (if any)
	for (auto itc = tccol.begin(); itc != tccol.end(); ++itc) {
	  float dt = fabs(time - itc->_t0._t0);
	  // make an absolute cut, including error on the cluster t0
	  if (dt < _maxdt+itc->_t0._t0err && dt < mindt){
	    mindt = dt;
	    besttc = itc;
	  }
	}
	if(besttc != tccol.end())
	  besttc->_strawHitIdxs.push_back(istr);
      }
    }
  }

  //--------------------------------------------------------------------------------------------------------------
  void TimeClusterFinder::findPeaks(TimeClusterCollection& tccol) {
    int nbins = _timespec.GetNbinsX()+1;
    std::vector<bool> alreadyUsed(nbins,false);
    // blank out bins around input times (from calo clusters)
    for(auto const& tc : tccol ){
      int ibin = _timespec.FindBin(tc._t0._t0);
      for(int jbin = std::max(1,ibin-_npeak);jbin < std::min(nbins,ibin+_npeak+1); ++jbin)
	alreadyUsed[jbin] = true;
    }
    // loop over spectrum to find peaks
    std::vector<BinContent> bcv;
    for (int ibin=1;ibin < nbins; ++ibin)
      if (_timespec.GetBinContent(ibin) >= _ymin) bcv.push_back(make_pair(_timespec.GetBinContent(ibin),ibin));
    std::sort(bcv.begin(),bcv.end(),[](const BinContent& x, const BinContent& y){return x.first > y.first;});

    for (const auto& bc : bcv) {
      if (alreadyUsed[bc.second]) continue;
      float nsh(0.0);
      float t0(0.0);
      for (int ibin = std::max(1,bc.second-_npeak);ibin < std::min(nbins,bc.second+_npeak+1); ++ibin) {
	nsh += _timespec.GetBinContent(ibin);
	t0 += _timespec.GetBinCenter(ibin)*_timespec.GetBinContent(ibin);
	alreadyUsed[ibin] = true;
      }
      t0 /= nsh;
      // if the count is enough, create a cluster
      if (nsh > _minnhits){
	TimeCluster tc;
	tc._t0 = TrkT0(t0,_tbin*0.5); // bin width
	tc._nsh = nsh;
	tccol.push_back(tc);
      }
    }
  }

  //--------------------------------------------------------------------------------------------------------------
  void TimeClusterFinder::initCluster(TimeCluster& tc) {
    // use medians to initialize robustly
    accumulator_set<float, stats<tag::min > > tmin;
    accumulator_set<float, stats<tag::max > > tmax;
    accumulator_set<float, stats<tag::weighted_median(with_p_square_quantile) >, float > tacc, xacc, yacc, zacc;

    unsigned nstrs = tc._strawHitIdxs.size();
    tc._nsh = 0;
    for(auto ish :tc._strawHitIdxs) {
      if (_testflag && !goodHit((*_shfcol)[ish])) continue;
      ComboHit const& ch = (*_chcol)[ish];
      unsigned nsh = ch.nStrawHits();
      tc._nsh += nsh;
      const XYZVec& pos = ch.pos();
      float htime = _ttcalc.comboHitTime(ch,_pitch);
      float hwt = ch.nStrawHits();
      tmin(htime);
      tmax(htime);
      tacc(htime,weight=hwt);
      xacc(pos.x(),weight=hwt);
      yacc(pos.y(),weight=hwt);
      zacc(pos.z(),weight=hwt);
    }

    if (tc.hasCaloCluster()) {
    // don't update t0 if there's an assigned calo cluster
      if(_useccpos){
	xacc(tc._caloCluster->cog3Vector().x(),weight=_ccwt);
	yacc(tc._caloCluster->cog3Vector().y(),weight=_ccwt);
      }
    } else {
      static float invsqrt12(1.0/sqrt(12.0));
      tc._t0._t0 = extract_result<tag::weighted_median>(tacc);
      tc._t0._t0err = ( boost::accumulators::extract::max(tmax)-boost::accumulators::extract::min(tmin))*invsqrt12/sqrt(nstrs);
    }
    //
    tc._pos = XYZVec(extract_result<tag::weighted_median>(xacc),
        extract_result<tag::weighted_median>(yacc),
        extract_result<tag::weighted_median>(zacc));

    if (_debug > 0) std::cout<<"Init time peak "<<tc._t0._t0<<std::endl;
  }

  // prefilter based on a rough hemisphere cut and the initial robust position
  void TimeClusterFinder::prefilterCluster(TimeCluster& tc){
    bool changed(true);
    while (changed) {
      changed = false;
      float pphi = polyAtan2( tc._pos.y(), tc._pos.x());
      auto iworst = tc._strawHitIdxs.end();
      float maxadPhi(_maxdPhi);
      for( auto ips = tc._strawHitIdxs.begin(); ips != tc._strawHitIdxs.end(); ++ips){
	ComboHit const& ch = (*_chcol)[*ips];
	float phi   = polyAtan2(ch.pos().y(), ch.pos().x());
	float dphi  = Angles::deltaPhi(phi,pphi);
	float adphi = std::abs(dphi);
	if(adphi > maxadPhi ){
	  iworst = ips;
	  maxadPhi = adphi;
	}
      }
      if( iworst != tc._strawHitIdxs.end()){
	changed = true;
	removeHit(tc,iworst);
      }
    }
  }

  void TimeClusterFinder::recoverHits(TimeCluster& tc){
    bool changed(true);
    while (changed) {
      changed = false;
      float pphi = polyAtan2(tc._pos.y(), tc._pos.x());
      for(size_t ich=0;ich < _chcol->size(); ++ich){
	if ((!_testflag) || goodHit((*_shfcol)[ich])) {
	  if(std::find(tc._strawHitIdxs.begin(),tc._strawHitIdxs.end(),ich) == tc._strawHitIdxs.end()){
	    ComboHit const& ch = (*_chcol)[ich];
	    float cht = _ttcalc.comboHitTime(ch,_pitch);
	    _pmva._dt = fabs(cht - tc._t0._t0);
	    if(_pmva._dt < _maxdt+tc._t0._t0err){
	      float phi = polyAtan2(ch.pos().y(), ch.pos().x());//ch.phi();
	      float dphi = fabs(Angles::deltaPhi(phi,pphi));
	      if(dphi < _maxdPhi){
		_pmva._dphi = dphi;
		_pmva._rho = ch.pos().Perp2();
		_pmva._nsh = ch.nStrawHits();
		_pmva._plane = ch.strawId().plane();
		_pmva._werr = ch.wireRes();
		_pmva._wdist = fabs(ch.wireDist());

		float mvaout(-1.0);
		if (tc.hasCaloCluster())
		  mvaout = _tcCaloMVA.evalMVA(_pmva._pars);
		else
		  mvaout = _tcMVA.evalMVA(_pmva._pars);
		if (mvaout > _minaddmva) {
		  addHit(tc,ich);
		  changed = true;
		}
	      }
	    }
	  }
	}
      }
    }
  }

  std::vector<StrawHitIndex>::iterator TimeClusterFinder::removeHit(TimeCluster& tc, ISH iworst) {
    ComboHit const& ch = (*_chcol)[*iworst];
    unsigned nsh = ch.nStrawHits();
    float denom = float(tc._nsh - nsh);
    // update time cluster properties
    if(!tc.hasCaloCluster()){
      float cht = _ttcalc.comboHitTime(ch,_pitch);
      float newt0  = (tc._t0._t0*tc._nsh - cht*nsh)/denom;
      tc._t0._t0err = sqrt((tc._t0._t0err*tc._t0._t0err*tc._nsh - (cht-newt0)*(cht-tc._t0._t0)*nsh )/denom);
      tc._t0._t0 = newt0;
    }
    tc._pos.SetX((tc._pos.x()*tc._nsh - ch.pos().x()*nsh)/denom);
    tc._pos.SetY((tc._pos.y()*tc._nsh - ch.pos().x()*nsh)/denom);
    tc._pos.SetZ((tc._pos.z()*tc._nsh - ch.pos().x()*nsh)/denom);
    tc._nsh -= nsh;
    return tc._strawHitIdxs.erase(iworst);
  }

  void TimeClusterFinder::addHit(TimeCluster& tc,size_t iadd) {
    ComboHit const& ch = (*_chcol)[iadd];
    unsigned nsh = ch.nStrawHits();
    float denom = float(tc._nsh + nsh);
    // update time cluster properties
    if(!tc.hasCaloCluster()){
      float cht = _ttcalc.comboHitTime(ch,_pitch);
      float newt0  = (tc._t0._t0*tc._nsh + cht*nsh)/denom;
      tc._t0._t0err = sqrt((tc._t0._t0err*tc._t0._t0err*tc._nsh + (cht-newt0)*(cht-tc._t0._t0)*nsh )/denom);
      tc._t0._t0 = newt0;
    }
    tc._pos.SetX((tc._pos.x()*tc._nsh + ch.pos().x()*nsh)/denom);
    tc._pos.SetY((tc._pos.y()*tc._nsh + ch.pos().x()*nsh)/denom);
    tc._pos.SetZ((tc._pos.z()*tc._nsh + ch.pos().x()*nsh)/denom);
    tc._nsh += nsh;
    tc._strawHitIdxs.push_back(iadd);
  }

  void TimeClusterFinder::clusterMean(TimeCluster& tc) {
    // compute properties using weighted mean
    accumulator_set<float, stats<tag::weighted_variance(lazy)>, float > terr;
    accumulator_set<float, stats<tag::weighted_mean >,float > xacc, yacc, zacc;
    for(StrawHitIndex ish : tc._strawHitIdxs) {
      ComboHit const& ch = (*_chcol)[ish];
      float hwt = ch.nStrawHits();
      float cht = _ttcalc.comboHitTime(ch,_pitch);
      terr(cht,weight=hwt);
      xacc(ch.pos().x(),weight=hwt);
      yacc(ch.pos().y(),weight=hwt);
      zacc(ch.pos().z(),weight=hwt);
    }
    if (tc.hasCaloCluster()) {
      if(_useccpos){
	xacc(tc._caloCluster->cog3Vector().x(),weight=_ccwt);
	yacc(tc._caloCluster->cog3Vector().y(),weight=_ccwt);
      }
    } else {
      tc._t0._t0 = extract_result<tag::weighted_mean>(terr);
      tc._t0._t0err = sqrtf(std::max(double(1.0),2.0*extract_result<tag::weighted_variance(lazy)>(terr))/extract_result<tag::count>(terr));
    }

    tc._pos = XYZVec(extract_result<tag::weighted_mean>(xacc),
	extract_result<tag::weighted_mean>(yacc),
	extract_result<tag::weighted_mean>(zacc));
  }

  void TimeClusterFinder::refineCluster(TimeCluster& tc) {
    // mva filtering; remove worst hit iteratively
    bool changed = true;
    while (changed) {
      changed = false;
      auto iworst = tc._strawHitIdxs.end();
      float worstmva(100.0);
      float pphi = polyAtan2(tc._pos.y(), tc._pos.x());
      for (auto ips=tc._strawHitIdxs.begin();ips != tc._strawHitIdxs.end();++ips) {
        ComboHit const& ch = (*_chcol)[*ips];
        float cht = _ttcalc.comboHitTime(ch,_pitch);

        _pmva._dt = fabs(cht - tc._t0._t0);
        float phi = polyAtan2(ch.pos().y(), ch.pos().x());//ch.phi();
        float dphi = Angles::deltaPhi(phi,pphi);
        _pmva._dphi = fabs(dphi);
	_pmva._rho = ch.pos().Perp2();
	_pmva._nsh = ch.nStrawHits();
	_pmva._plane = ch.strawId().plane();
	_pmva._werr = ch.wireRes();
	_pmva._wdist = fabs(ch.wireDist());

	float mvaout(-1.0);
	if (tc.hasCaloCluster())
	   mvaout = _tcCaloMVA.evalMVA(_pmva._pars);
	else
	  mvaout = _tcMVA.evalMVA(_pmva._pars);
	if (mvaout < worstmva) {
	  worstmva = mvaout;
	  iworst = ips;
        }
      }

      if (worstmva < _minkeepmva) {
        changed = true;
	removeHit(tc,iworst);
      }
    }
  }

  bool TimeClusterFinder::goodHit(const StrawHitFlag& flag) const
  {
    return flag.hasAllProperties(_hsel) && !flag.hasAnyProperty(_hbkg);
  }

}

using mu2e::TimeClusterFinder;
DEFINE_ART_MODULE(TimeClusterFinder);


#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <boost/format.hpp>
#include <cmath>
#include <cstdio>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/modeling.hpp>
#include <trajopt_sco/optimizers.hpp>
#include <trajopt_sco/sco_common.hpp>
#include <trajopt_sco/solver_interface.hpp>
#include <trajopt_utils/logging.hpp>
#include <trajopt_utils/macros.h>
#include <trajopt_utils/stl_to_string.hpp>

namespace sco
{
std::ostream& operator<<(std::ostream& o, const OptResults& r)
{
  o << "Optimization results:" << std::endl
    << "status: " << statusToString(r.status) << std::endl
    << "cost values: " << util::Str(r.cost_vals) << std::endl
    << "constraint violations: " << util::Str(r.cnt_viols) << std::endl
    << "n func evals: " << r.n_func_evals << std::endl
    << "n qp solves: " << r.n_qp_solves << std::endl;
  return o;
}

//////////////////////////////////////////////////
////////// private utility functions for  sqp /////////
//////////////////////////////////////////////////

static DblVec evaluateCosts(const std::vector<CostPtr>& costs, const DblVec& x)
{
  DblVec out(costs.size());
  for (size_t i = 0; i < costs.size(); ++i)
  {
    out[i] = costs[i]->value(x);
  }
  return out;
}
static DblVec evaluateConstraintViols(const std::vector<ConstraintPtr>& constraints, const DblVec& x)
{
  DblVec out(constraints.size());
  for (size_t i = 0; i < constraints.size(); ++i)
  {
    out[i] = constraints[i]->violation(x);
  }
  return out;
}
static std::vector<ConvexObjectivePtr> convexifyCosts(const std::vector<CostPtr>& costs, const DblVec& x, Model* model)
{
  std::vector<ConvexObjectivePtr> out(costs.size());
  for (size_t i = 0; i < costs.size(); ++i)
  {
    out[i] = costs[i]->convex(x, model);
  }
  return out;
}
static std::vector<ConvexConstraintsPtr> convexifyConstraints(const std::vector<ConstraintPtr>& cnts, const DblVec& x,
                                                              Model* model)
{
  std::vector<ConvexConstraintsPtr> out(cnts.size());
  for (size_t i = 0; i < cnts.size(); ++i)
  {
    out[i] = cnts[i]->convex(x, model);
  }
  return out;
}

DblVec evaluateModelCosts(const std::vector<ConvexObjectivePtr>& costs, const DblVec& x)
{
  DblVec out(costs.size());
  for (size_t i = 0; i < costs.size(); ++i)
  {
    out[i] = costs[i]->value(x);
  }
  return out;
}
DblVec evaluateModelCntViols(const std::vector<ConvexConstraintsPtr>& cnts, const DblVec& x)
{
  DblVec out(cnts.size());
  for (size_t i = 0; i < cnts.size(); ++i)
  {
    out[i] = cnts[i]->violation(x);
  }
  return out;
}

static std::vector<std::string> getCostNames(const std::vector<CostPtr>& costs)
{
  std::vector<std::string> out(costs.size());
  for (size_t i = 0; i < costs.size(); ++i)
    out[i] = costs[i]->name();
  return out;
}
static std::vector<std::string> getCntNames(const std::vector<ConstraintPtr>& cnts)
{
  std::vector<std::string> out(cnts.size());
  for (size_t i = 0; i < cnts.size(); ++i)
    out[i] = cnts[i]->name();
  return out;
}

void printCostInfo(const DblVec& old_cost_vals, const DblVec& model_cost_vals, const DblVec& new_cost_vals,
                   const DblVec& old_cnt_vals, const DblVec& model_cnt_vals, const DblVec& new_cnt_vals,
                   const std::vector<std::string>& cost_names, const std::vector<std::string>& cnt_names,
                   double merit_coeff)
{
  std::printf("%15s | %10s | %10s | %10s | %10s\n", "", "oldexact", "dapprox", "dexact", "ratio");
  std::printf("%15s | %10s---%10s---%10s---%10s\n", "COSTS", "----------", "----------", "----------", "----------");
  for (size_t i = 0; i < old_cost_vals.size(); ++i)
  {
    double approx_improve = old_cost_vals[i] - model_cost_vals[i];
    double exact_improve = old_cost_vals[i] - new_cost_vals[i];
    if (fabs(approx_improve) > 1e-8)
      std::printf("%15s | %10.3e | %10.3e | %10.3e | %10.3e\n", cost_names[i].c_str(), old_cost_vals[i], approx_improve,
                  exact_improve, exact_improve / approx_improve);
    else
      std::printf("%15s | %10.3e | %10.3e | %10.3e | %10s\n", cost_names[i].c_str(), old_cost_vals[i], approx_improve,
                  exact_improve, "  ------  ");
  }
  if (cnt_names.size() == 0)
    return;
  std::printf("%15s | %10s---%10s---%10s---%10s\n", "CONSTRAINTS", "----------", "----------", "----------", "---------"
                                                                                "-");
  for (size_t i = 0; i < old_cnt_vals.size(); ++i)
  {
    double approx_improve = old_cnt_vals[i] - model_cnt_vals[i];
    double exact_improve = old_cnt_vals[i] - new_cnt_vals[i];
    if (fabs(approx_improve) > 1e-8)
      std::printf("%15s | %10.3e | %10.3e | %10.3e | %10.3e\n", cnt_names[i].c_str(), merit_coeff * old_cnt_vals[i],
                  merit_coeff * approx_improve, merit_coeff * exact_improve, exact_improve / approx_improve);
    else
      std::printf("%15s | %10.3e | %10.3e | %10.3e | %10s\n", cnt_names[i].c_str(), merit_coeff * old_cnt_vals[i],
                  merit_coeff * approx_improve, merit_coeff * exact_improve, "  ------  ");
  }
}

// todo: use different coeffs for each constraint
std::vector<ConvexObjectivePtr> cntsToCosts(const std::vector<ConvexConstraintsPtr>& cnts, double err_coeff,
                                            Model* model)
{
  std::vector<ConvexObjectivePtr> out;
  for (const ConvexConstraintsPtr& cnt : cnts)
  {
    ConvexObjectivePtr obj(new ConvexObjective(model));
    for (const AffExpr& aff : cnt->eqs_)
    {
      obj->addAbs(aff, err_coeff);
    }
    for (const AffExpr& aff : cnt->ineqs_)
    {
      obj->addHinge(aff, err_coeff);
    }
    out.push_back(obj);
  }
  return out;
}

void Optimizer::addCallback(const Callback& cb)
{
  callbacks_.push_back(cb);
}
void Optimizer::callCallbacks()
{
  for (unsigned i = 0; i < callbacks_.size(); ++i)
  {
    callbacks_[i](prob_.get(), results_);
  }
}

void Optimizer::initialize(const DblVec& x)
{
  if (!prob_)
    PRINT_AND_THROW("need to set the problem before initializing");
  if (prob_->getVars().size() != x.size())
    PRINT_AND_THROW(boost::format("initialization vector has wrong length. expected %i got %i") %
                    prob_->getVars().size() % x.size());
  results_.clear();
  results_.x = x;
}

BasicTrustRegionSQPParameters::BasicTrustRegionSQPParameters()
{
  improve_ratio_threshold = 0.25;
  min_trust_box_size = 1e-4;
  min_approx_improve = 1e-4;
  min_approx_improve_frac = -INFINITY;
  max_iter = 50;
  trust_shrink_ratio = 0.1;
  trust_expand_ratio = 1.5;
  cnt_tolerance = 1e-4;
  max_merit_coeff_increases = 5;
  merit_coeff_increase_ratio = 10;
  max_time = INFINITY;
  merit_error_coeff = 10;
  trust_box_size = 1e-1;
}

BasicTrustRegionSQP::BasicTrustRegionSQP()
{
}
BasicTrustRegionSQP::BasicTrustRegionSQP(OptProbPtr prob)
{
  setProblem(prob);
}
void BasicTrustRegionSQP::setProblem(OptProbPtr prob)
{
  Optimizer::setProblem(prob);
  model_ = prob->getModel();
}

void BasicTrustRegionSQP::adjustTrustRegion(double ratio)
{
  param_.trust_box_size *= ratio;
}
void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x)
{
  const VarVector& vars = prob_->getVars();
  assert(vars.size() == x.size());
  const DblVec &lb = prob_->getLowerBounds(), ub = prob_->getUpperBounds();
  DblVec lbtrust(x.size()), ubtrust(x.size());
  for (size_t i = 0; i < x.size(); ++i)
  {
    lbtrust[i] = fmax(x[i] - param_.trust_box_size, lb[i]);
    ubtrust[i] = fmin(x[i] + param_.trust_box_size, ub[i]);
  }
  model_->setVarBounds(vars, lbtrust, ubtrust);
}

#if 0
struct MultiCritFilter {
  /**
   * Checks if you're making an improvement on a multidimensional objective
   * Given a set of past error vectors, the improvement is defined as
   * min_{olderrvec in past_err_vecs} | olderrvec - errvec |^+
   */
  vector<DblVec> errvecs;
  double improvement(const DblVec& errvec) {
    double leastImprovement=INFINITY;
    for (const DblVec& olderrvec : errvecs) {
      double improvement=0;
      for (int i=0; i < errvec.size(); ++i) improvement += pospart(olderrvec[i] - errvec[i]);
      leastImprovement = fmin(leastImprovement, improvement);
    }
    return leastImprovement;
  }
  void insert(const DblVec& x) {errvecs.push_back(x);}
  bool empty() {return errvecs.size() > 0;}
};
#endif

OptStatus BasicTrustRegionSQP::optimize()
{
  std::vector<std::string> cost_names = getCostNames(prob_->getCosts());
  std::vector<ConstraintPtr> constraints = prob_->getConstraints();
  std::vector<std::string> cnt_names = getCntNames(constraints);

  if (results_.x.size() == 0)
    PRINT_AND_THROW("you forgot to initialize!");
  if (!prob_)
    PRINT_AND_THROW("you forgot to set the optimization problem");

  results_.x = prob_->getClosestFeasiblePoint(results_.x);

  assert(results_.x.size() == prob_->getVars().size());
  assert(prob_->getCosts().size() > 0 || constraints.size() > 0);

  OptStatus retval = INVALID;

  for (int merit_increases = 0; merit_increases < param_.max_merit_coeff_increases; ++merit_increases)
  { /* merit adjustment loop */
    for (int iter = 1;; ++iter)
    { /* sqp loop */
      callCallbacks();

      LOG_DEBUG("current iterate: %s", CSTR(results_.x));
      LOG_INFO("iteration %i", iter);

      // speedup: if you just evaluated the cost when doing the line search, use
      // that
      if (results_.cost_vals.empty() && results_.cnt_viols.empty())
      {  // only happens on the first iteration
        results_.cnt_viols = evaluateConstraintViols(constraints, results_.x);
        results_.cost_vals = evaluateCosts(prob_->getCosts(), results_.x);
        assert(results_.n_func_evals == 0);
        ++results_.n_func_evals;
      }

      // DblVec new_cnt_viols = evaluateConstraintViols(constraints, results_.x);
      // DblVec new_cost_vals = evaluateCosts(prob_->getCosts(), results_.x);
      // cout << "costs" << endl;
      // for (int i=0; i < new_cnt_viols.size(); ++i) {
      //   cout << cnt_names[i] << " " << new_cnt_viols[i] -
      //   results_.cnt_viols[i] << endl;
      // }
      // for (int i=0; i < new_cost_vals.size(); ++i) {
      //   cout << cost_names[i] << " " << new_cost_vals[i] -
      //   results_.cost_vals[i] << endl;
      // }

      std::vector<ConvexObjectivePtr> cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get());
      std::vector<ConvexConstraintsPtr> cnt_models = convexifyConstraints(constraints, results_.x, model_.get());
      std::vector<ConvexObjectivePtr> cnt_cost_models = cntsToCosts(cnt_models, param_.merit_error_coeff, model_.get());
      model_->update();
      for (ConvexObjectivePtr& cost : cost_models)
        cost->addConstraintsToModel();
      for (ConvexObjectivePtr& cost : cnt_cost_models)
        cost->addConstraintsToModel();
      model_->update();
      QuadExpr objective;
      for (ConvexObjectivePtr& co : cost_models)
        exprInc(objective, co->quad_);
      for (ConvexObjectivePtr& co : cnt_cost_models)
        exprInc(objective, co->quad_);

      //    objective = cleanupExpr(objective);
      model_->setObjective(objective);

      //    if (logging::filter() >= IPI_LEVEL_DEBUG) {
      //      DblVec model_cost_vals;
      //      for (ConvexObjectivePtr& cost : cost_models) {
      //        model_cost_vals.push_back(cost->value(x));
      //      }
      //      LOG_DEBUG("model costs %s should equalcosts  %s",
      //      printer(model_cost_vals), printer(cost_vals));
      //    }

      while (param_.trust_box_size >= param_.min_trust_box_size)
      {
        setTrustBoxConstraints(results_.x);
        CvxOptStatus status = model_->optimize();
        ++results_.n_qp_solves;
        if (status != CVX_SOLVED)
        {
          LOG_ERROR("convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see "
                    "solver output. saving model to /tmp/fail.lp and IIS to "
                    "/tmp/fail.ilp");
          model_->writeToFile("/tmp/fail.lp");
          model_->writeToFile("/tmp/fail.ilp");
          retval = OPT_FAILED;
          goto cleanup;
        }
        DblVec model_var_vals = model_->getVarValues(model_->getVars());

        DblVec model_cost_vals = evaluateModelCosts(cost_models, model_var_vals);
        DblVec model_cnt_viols = evaluateModelCntViols(cnt_models, model_var_vals);

        // the n variables of the OptProb happen to be the first n variables in
        // the Model
        DblVec new_x(model_var_vals.begin(), model_var_vals.begin() + static_cast<long int>(results_.x.size()));

        if (util::GetLogLevel() >= util::LevelDebug)
        {
          DblVec cnt_costs1 = evaluateModelCosts(cnt_cost_models, model_var_vals);
          DblVec cnt_costs2 = model_cnt_viols;
          for (unsigned i = 0; i < cnt_costs2.size(); ++i)
            cnt_costs2[i] *= param_.merit_error_coeff;
          LOG_DEBUG("SHOULD BE ALMOST THE SAME: %s ?= %s", CSTR(cnt_costs1), CSTR(cnt_costs2));
          // not exactly the same because cnt_costs1 is based on aux variables,
          // but they might not be at EXACTLY the right value
        }

        DblVec new_cost_vals = evaluateCosts(prob_->getCosts(), new_x);
        DblVec new_cnt_viols = evaluateConstraintViols(constraints, new_x);
        ++results_.n_func_evals;

        double old_merit = vecSum(results_.cost_vals) + param_.merit_error_coeff * vecSum(results_.cnt_viols);
        double model_merit = vecSum(model_cost_vals) + param_.merit_error_coeff * vecSum(model_cnt_viols);
        double new_merit = vecSum(new_cost_vals) + param_.merit_error_coeff * vecSum(new_cnt_viols);
        double approx_merit_improve = old_merit - model_merit;
        double exact_merit_improve = old_merit - new_merit;
        double merit_improve_ratio = exact_merit_improve / approx_merit_improve;

        if (util::GetLogLevel() >= util::LevelInfo)
        {
          LOG_INFO(" ");
          printCostInfo(results_.cost_vals, model_cost_vals, new_cost_vals, results_.cnt_viols, model_cnt_viols,
                        new_cnt_viols, cost_names, cnt_names, param_.merit_error_coeff);
          std::printf("%15s | %10.3e | %10.3e | %10.3e | %10.3e\n", "TOTAL", old_merit, approx_merit_improve,
                      exact_merit_improve, merit_improve_ratio);
        }

        if (approx_merit_improve < -1e-5)
        {
          LOG_ERROR("approximate merit function got worse (%.3e). "
                    "(convexification is probably wrong to zeroth order)",
                    approx_merit_improve);
        }
        if (approx_merit_improve < param_.min_approx_improve)
        {
          LOG_INFO("converged because improvement was small (%.3e < %.3e)", approx_merit_improve,
                   param_.min_approx_improve);
          retval = OPT_CONVERGED;
          goto penaltyadjustment;
        }
        if (approx_merit_improve / old_merit < param_.min_approx_improve_frac)
        {
          LOG_INFO("converged because improvement ratio was small (%.3e < %.3e)", approx_merit_improve / old_merit,
                   param_.min_approx_improve_frac);
          retval = OPT_CONVERGED;
          goto penaltyadjustment;
        }
        else if (exact_merit_improve < 0 || merit_improve_ratio < param_.improve_ratio_threshold)
        {
          adjustTrustRegion(param_.trust_shrink_ratio);
          LOG_INFO("shrunk trust region. new box size: %.4f", param_.trust_box_size);
        }
        else
        {
          results_.x = new_x;
          results_.cost_vals = new_cost_vals;
          results_.cnt_viols = new_cnt_viols;
          adjustTrustRegion(param_.trust_expand_ratio);
          LOG_INFO("expanded trust region. new box size: %.4f", param_.trust_box_size);
          break;
        }
      }

      if (param_.trust_box_size < param_.min_trust_box_size)
      {
        LOG_INFO("converged because trust region is tiny");
        retval = OPT_CONVERGED;
        goto penaltyadjustment;
      }
      else if (iter >= param_.max_iter)
      {
        LOG_INFO("iteration limit");
        retval = OPT_SCO_ITERATION_LIMIT;
        goto cleanup;
      }
    }

  penaltyadjustment:
    if (results_.cnt_viols.empty() || vecMax(results_.cnt_viols) < param_.cnt_tolerance)
    {
      if (results_.cnt_viols.size() > 0)
        LOG_INFO("woo-hoo! constraints are satisfied (to tolerance %.2e)", param_.cnt_tolerance);
      goto cleanup;
    }
    else
    {
      LOG_INFO("not all constraints are satisfied. increasing penalties");
      param_.merit_error_coeff *= param_.merit_coeff_increase_ratio;
      param_.trust_box_size = fmax(param_.trust_box_size, param_.min_trust_box_size / param_.trust_shrink_ratio * 1.5);
    }
  }
  retval = OPT_PENALTY_ITERATION_LIMIT;
  LOG_INFO("optimization couldn't satisfy all constraints");

cleanup:
  assert(retval != INVALID && "should never happen");
  results_.status = retval;
  results_.total_cost = vecSum(results_.cost_vals);
  LOG_INFO("\n==================\n%s==================", CSTR(results_));
  callCallbacks();

  return retval;
}
}  // namespace sco


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/constant/mcd.hpp>

#include <boost/hana/comparable/laws.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/constant.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

struct UDT {
    struct Type { };
    Type member;
};

template <typename mcd, typename T, typename U>
void test() {
    BOOST_HANA_CONSTANT_ASSERT(
        Comparable::laws::check(
            list(
                detail::minimal::constant<T, 0, mcd>,
                detail::minimal::constant<T, 1, mcd>,

                detail::minimal::constant<U, 1, mcd>,
                detail::minimal::constant<U, 2, mcd>,

                detail::minimal::constant<decltype(&UDT::member), &UDT::member, mcd>
            )
        )
    );
}

int main() {
    test<Constant::mcd, int, int>();
    test<Constant::mcd, int, unsigned long long>();
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2019 Nil Foundation AG
// Copyright (c) 2018-2019 Mikhail Komarov <nemo@nilfoundation.org>
//
// 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
//
// 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE wesolowski_test

#include <nil/crypto3/vdf/wesolowski.hpp>

#include <nil/crypto3/vdf/algorithm/compute.hpp>
#include <nil/crypto3/vdf/algorithm/verify.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/static_assert.hpp>

#include <iostream>
#include <string>
#include <unordered_map>

using namespace nil::crypto3::vdf;

BOOST_AUTO_TEST_SUITE(wesolowski_test_suite)

    BOOST_AUTO_TEST_CASE(wesolowski_vdf) {

    }

BOOST_AUTO_TEST_SUITE_END()