#include "detector_impl.hpp"
#include "util.hpp"
#include "icp.hpp"

#include <cvx/util/geometry/kdtree.hpp>
#include <cvx/util/geometry/octree.hpp>
#include <cvx/util/misc/cv_helpers.hpp>
#include <cvx/util/imgproc/rgbd.hpp>

#include <cvx/viz/scene/scene.hpp>
#include <cvx/viz/scene/camera.hpp>
#include <cvx/viz/renderer/renderer.hpp>

#include <fstream>
#include <queue>

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace cvx::util ;
using namespace cvx::viz ;
using namespace std;
using namespace Eigen ;

//#define DEBUG

void compute_model_bounds(const PointList3f &cloud, ModelData &data) {

    Vector3f c(0, 0, 0) ;
    uint count = 0 ;

    Vector3f minp(  std::numeric_limits<float>::max(),
                    std::numeric_limits<float>::max(),
                    std::numeric_limits<float>::max()),
            maxp( -std::numeric_limits<float>::max(),
                  -std::numeric_limits<float>::max(),
                  -std::numeric_limits<float>::max()) ;

    for ( uint i=0 ; i<cloud.size() ; i++ ) {

        const Vector3f &q = cloud[i] ;
        count ++ ;

        c += q ;

        minp.x() = std::min(minp.x(), q.x()) ;
        minp.y() = std::min(minp.y(), q.y()) ;
        minp.z() = std::min(minp.z(), q.z()) ;

        maxp.x() = std::max(maxp.x(), q.x()) ;
        maxp.y() = std::max(maxp.y(), q.y()) ;
        maxp.z() = std::max(maxp.z(), q.z()) ;
    }

    data.bmin_ = minp ;
    data.bmax_ = maxp ;

    data.center_ = c/count ;

    float max_distance = 0 ;

    for(uint i=0 ; i<cloud.size() ; i++ )
        max_distance = std::max( max_distance, (cloud[i] - data.center_).squaredNorm() ) ;

    data.diameter_ = 2 * sqrt(max_distance) ;

    data.search_.train(cloud) ;

}

std::istream& safe_getline(std::istream& is, std::string& t) ;

bool ObjectDetectorImpl::init(const string &rf_path, const Dataset &ds) {

    forest_.read(rf_path) ;

    labels_ = ds.label_map_ ;
    for( uint i=0 ; i<ds.models_.size() ; i++ ) {
        ModelData data ;

        data.scene_ = ds.models_[i] ;
        data.cloud_ = ds.clouds_[i] ;
        data.renderer_ = std::make_shared<Renderer>(data.scene_) ;
        data.camera_ = ds.world_to_model_[i] ;
        compute_model_bounds(ds.clouds_[i], data) ;

        model_map_[labels_[i]] = data ;
    }

    return true ;
}

void ObjectDetectorImpl::compute_label_probabilities(const vector< vector<RandomForest::Node *> > &leafs, vector< map<string, float> > &probs) {
    uint nt = leafs.size() ;
    uint nsamples = leafs[0].size() ;

    probs.resize(nsamples) ;

    for( uint i=0 ; i<nsamples ; i++ ) {

        float bgprod = 1 ;
        for( uint j=0 ; j<nt ; j++) {
            RandomForest::Node *n = leafs[j][i] ;

            float bg ;
            bg = n->data_.n_background_ / (float) n->data_.sample_count_ ;
            bgprod *= bg ;
        }

        float lsum = 0 ;
        map<string, float> lprods ;

        for ( uint k=0 ; k<labels_.size() ; k++ ) {
            string clabel = labels_[k] ;
            float lprod = 1 ;
            for( uint j=0 ; j<nt ; j++) {
                RandomForest::Node *n = leafs[j][i] ;

                float pl ;
                if ( n->data_.classes_.count(clabel) == 0 ) pl = 0 ;
                else
                    pl =  n->data_.classes_[clabel] / (float) n->data_.sample_count_ ;

                lprod *= pl ;
            }
            lprods[clabel] = lprod ;
            lsum += lprod ;
        }

        for ( uint k=0 ; k<labels_.size() ; k++ ) {
            float p = lprods[labels_[k]]/( lsum + bgprod + 1.0e-8) ;

            if ( p > 0 )
                probs[i][labels_[k]] = p ;
        }
    }
}

void ObjectDetectorImpl::sample_pose_hypotheses(const std::string &clabel,
                                                const PinholeCamera &cam,
                                                const Dataset &ds,
                                                const LeafList &leafs,
                                                const LabelProbList &probs,
                                                vector<Matrix4f> &poses)
{
    //boost::timer::auto_cpu_timer t ;

    vector<float> weights ;
    PointList2f pts ;

    ModelData &md = model_map_[clabel] ;

    float object_diameter = md.diameter_ ;

    for(uint i=0 ; i<probs.size() ; i++ ) {

        float p = 0.0 ;
        LabelProbMapType::const_iterator it = probs[i].find(clabel) ;
        if ( it != probs[i].end() ) p = it->second ;

         weights.push_back(p) ;

        const cv::Point &pt = ds.samples_[i] ;
        pts.push_back(Vector2f(pt.x, pt.y)) ;
    }

    KDTree2 ksearch(pts) ;

    std::discrete_distribution<> pt_sampler(weights.begin(), weights.end()) ;
    std::uniform_int_distribution<> tree_sampler(0, leafs.size() - 1) ;

    const cv::Mat &dmap = ds.depth_images_[0] ;

    uint max_iterations = params_.max_ransac_iterations_ ;

    // iterate until we obtain enough hypotheses or reach the maximum number of trials

#pragma omp parallel for
    for ( uint iterations = 0 ; iterations < max_iterations ; iterations++ ) {

        if ( poses.size() < params_.max_hypotheses_ ) {

            // sample first point
            uint idx0 = pt_sampler(g_.generator()) ;
            uint t0 = tree_sampler(g_.generator()) ;

            const cv::Point &pt = ds.samples_[idx0] ;
            float z = dmap.at<ushort>(pt.y, pt.x)/1000.0 ;
            float search_radius = cam.fx() * object_diameter / z  ;

            // sample second point within radius around first point

            vector<uint> indexes ;
            vector<float> distances ;

            ksearch.withinRadius(pts[idx0], search_radius * search_radius, indexes, distances);

            if ( indexes.empty() ) continue ;

            vector<float> weights1 ;

            for( uint i=1 ; i<indexes.size() ; i++ ) {

                uint idx = indexes[i] ;
                float p = 0.0 ;
                LabelProbMapType::const_iterator it = probs[idx].find(clabel) ;
                if ( it != probs[idx].end() ) p = it->second ;

                weights1.push_back(p) ;
            }

            std::discrete_distribution<> pt_radius_sampler(weights1.begin(), weights1.end()) ;

            uint idx1 = indexes[pt_radius_sampler(g_.generator())+1] ;
            uint t1 = tree_sampler(g_.generator()) ;

            if ( idx0 == idx1 ) continue ;

            uint idx2 = indexes[pt_radius_sampler(g_.generator())+1] ;
            uint t2 = tree_sampler(g_.generator()) ;

            if ( idx0 == idx2 || idx1 == idx2 ) continue ;

            // Now we have 3 points on the image. we obtain the corresponding forest predictions and image back-projected 3D coordinates

            Vector3f o0, o1, o2 ;

            if ( leafs[t0][idx0]->data_.coordinates_.count(clabel) == 0 ) continue ;
            else o0 = leafs[t0][idx0]->data_.coordinates_[clabel] ;

            if ( leafs[t1][idx1]->data_.coordinates_.count(clabel) == 0 ) continue ;
            else o1 = leafs[t1][idx1]->data_.coordinates_[clabel] ;

            if ( leafs[t2][idx2]->data_.coordinates_.count(clabel) == 0 ) continue ;
            else o2 = leafs[t2][idx2]->data_.coordinates_[clabel] ;

            Vector3f p0 = back_project(dmap, cam, ds.samples_[idx0]) ; // these are in camera coordinate frame
            Vector3f p1 = back_project(dmap, cam, ds.samples_[idx1]) ;
            Vector3f p2 = back_project(dmap, cam, ds.samples_[idx2]) ;

            Matrix3Xf src(3, 3), dst(3, 3) ;

            Affine3f m2w(md.camera_.inverse().eval()) ;
            o0 = m2w * o0 ; o1 = m2w * o1 ; o2 = m2w * o2 ; // these are in world coordinate frame

            src.col(0) = o0 ; src.col(1) = o1 ; src.col(2) = o2 ;
            dst.col(0) = p0 ; dst.col(1) = p1 ; dst.col(2) = p2 ;

            // find alignment transformation

            Affine3f H = find_rigid(src, dst); // c2w

            // reject erroneous ones

            if ( ( H * o0 - p0).norm() > 0.05 * object_diameter ) continue ;
            if ( ( H * o1 - p1).norm() > 0.05 * object_diameter ) continue ;
            if ( ( H * o2 - p2).norm() > 0.05 * object_diameter ) continue ;
#ifdef DEBUG
            cv::Mat clr = ds.rgb_images_[0].clone() ;

            cv::circle(clr, ds.samples_[idx0], 3, cv::Scalar(255, 255, 0), 4) ;
            cv::circle(clr, ds.samples_[idx1], 3, cv::Scalar(255, 255, 0), 4) ;
            cv::circle(clr, ds.samples_[idx2], 3, cv::Scalar(255, 255, 0), 4) ;

            cv::imwrite("/tmp/samples.png", clr) ;

            cout << H.matrix() << endl ;
#endif
            // store accepted pose
#pragma omp critical
            poses.push_back(H.matrix()) ; // c2w
        }
    }
}

float ObjectDetectorImpl::compute_energy(const std::string &clabel, const Matrix4f &pose, const LeafList &leafs,
                                         const LabelProbList &probs,
                                         const PinholeCamera &cam, const Dataset &ds)
{
    ModelData &data = model_map_[clabel] ;

    cv::Mat_<ushort> depth = ds.depth_images_[0] ;

    cv::Mat_<ushort> zbuffer = render_mask(clabel, cam, pose) ;
    uint w = zbuffer.cols, h = zbuffer.rows ;
#ifdef DEBUG
   cv::imwrite("/tmp/zz.png", zbuffer) ;
#endif
    // depth component

    float ed = 0 ;
    uint ed_count = 0 ;

    for( uint i=0 ; i<h ; i++ )
        for( uint j=0 ; j<w ; j++ ) {
            ushort oval = zbuffer[i][j] ;
            ushort ival = depth[i][j] ;

            if ( oval == 0 || ival == 0 ) continue ;

            Vector3f ip = cam.backProject(j, i, ival/1000.0) ;
            Vector3f op = cam.backProject(j, i, oval/1000.0) ;

            float e = std::min((ip - op).norm(), params_.depth_error_threshold_)/params_.depth_error_threshold_ ;
            ed_count ++ ;
            ed += e ;
        }

    if ( ed_count == 0 ) ed = FLT_MAX ;
    else ed /= ed_count ;

    // object component (this differs a bit from the paper since we iterate over the pixels that are sampled from the image instead over all pixels in the mask)

    float eo = 0 ;
    uint eo_count = 0 ;

    uint n_trees = leafs.size() ;
    uint n_samples = ds.samples_.size() ;

    for( uint i=0 ; i<n_samples ; i++ ) {
        const cv::Point &pt = ds.samples_[i] ;

        ushort val = zbuffer[pt.y][pt.x] ;
        if ( val == 0 ) continue ;

        // here we have only samples that fall inside the mask

        for( uint j=0 ; j<n_trees ; j++ ) {
            RandomForest::Node *n = leafs[j][i] ;

            float p ;

            map<string, uint64_t>::const_iterator it = n->data_.classes_.find(clabel) ;
            if ( it == n->data_.classes_.end() ) p = 1.0e-5 ;
            else p = it->second / (float)n->data_.sample_count_ ;

            eo += -log(p) ;
            eo_count ++ ;
        }

    }

    if ( eo_count == 0 ) eo = FLT_MAX ;
    else eo /= eo_count ;

    // coordinate component (same as above)

    float ec = 0 ;
    uint ec_count = 0 ;

    float cet = params_.coord_error_threshold_ * data.diameter_ ;
    cet = cet * cet ;

    Matrix4f cami(data.camera_.inverse()) ;
    Affine3f m2w(cami) ;

    for( uint i=0 ; i<n_samples ; i++ ) {
        const cv::Point &pt = ds.samples_[i] ;

        ushort ival = depth[pt.y][pt.x] ;
        ushort oval = zbuffer[pt.y][pt.x] ;
        if ( ival == 0 || oval == 0 ) continue ;

        // exclude pixels with low class probability
        LabelProbMapType::const_iterator it = probs[i].find(clabel) ;
        if ( it == probs[i].end() || it->second < params_.class_membership_threshold_ )
            continue ;

        // coordinates in world frame based on predicted pose

        Vector3f ip = cam.backProject(pt.x, pt.y, ival/1000.0) ;
        ip = Isometry3f(pose.inverse().eval()) * ip ;

        for( uint j=0 ; j<n_trees ; j++ ) {
            RandomForest::Node *n = leafs[j][i] ;

            map<string, Vector3f>::const_iterator it = n->data_.coordinates_.find(clabel) ;
            Vector3f op = m2w * it->second ;

            ec += std::min((ip - op).squaredNorm(), cet)/cet ;
            ec_count ++ ;
        }
    }

    if ( ec_count < 100 ) return FLT_MAX ;

    if ( ec_count == 0 ) eo = FLT_MAX ;
    else ec /= ec_count ;

#ifdef DEBUG
    cv::imwrite("/tmp/zbuffer.png", depthViz(zbuffer)) ;
    cout << ed << ' ' << eo << ' ' << ec << endl ;
#endif

    return params_.lambda_ed_ * ed + params_.lambda_eo_ * eo + params_.lambda_ec_ * ec ;
}

typedef pair<float, uint16_t> PoseHypothesis ;

class Comparator {
public:
    bool operator () (const PoseHypothesis &h1, const PoseHypothesis &h2 ) {
        return h1.first >= h2.first ;
    }
};

typedef priority_queue<PoseHypothesis, vector<PoseHypothesis>, Comparator> PQueueType ;

void ObjectDetectorImpl::find_pose_candidates(const string &clabel, const PinholeCamera &cam, const Dataset &ds, const ObjectDetectorImpl::LeafList &leafs,
                                              const ObjectDetectorImpl::LabelProbList &probs, std::vector<Matrix4f> &poses, vector<float> &errors)
{

    std::vector<Matrix4f> candidates ;
    sample_pose_hypotheses(clabel, cam, ds, leafs, probs, candidates) ;

    PQueueType q ;

    for( uint k=0 ; k<candidates.size() ; k++ ) {

        float e = compute_energy(clabel, candidates[k], leafs, probs, cam, ds) ;

#ifdef DEBUG
        cout << e << endl ;
#endif
        q.push(PoseHypothesis(e, k)) ;
    }

    if ( q.empty() ) return ;

    // keep only best hypotheses

    float hthresh = params_.best_hypothesis_threshold_ * q.top().first ;

    while ( !q.empty() ) {
        PoseHypothesis p = q.top() ; q.pop() ;
        if ( p.first < hthresh ) {
            poses.push_back(candidates[p.second]) ;
            errors.push_back(p.first) ;
        }
        if ( poses.size() == params_.max_retain_candidates_ ) break ;
    }

}

cv::Mat ObjectDetectorImpl::render_mask(const string &clabel, const PinholeCamera &cam, const Matrix4f &pose) {

    // transform from source camera frame to object frame, then according to the pose to the target camera frame and finally switch axes

    ModelData &data = model_map_[clabel] ;

    // the opengl camera points along the negative Z axis so we have to switch coordinate systems
    Matrix4f axis_switch = Matrix4f::Identity() ;
    axis_switch(1, 1) = -1 ;
    axis_switch(2, 2) = -1 ;
//                              c2w
    Matrix4f cc = axis_switch * pose  ;

    CameraPtr pcam(new PerspectiveCamera(cam)) ;
    pcam->setViewTransform(cc);
    pcam->setBgColor({0, 0, 0, 1});

    data.renderer_->render(pcam) ;

    return data.renderer_->getDepth() ;
}

void ObjectDetectorImpl::refine_pose_candidates(const string &clabel, const PinholeCamera &cam, const Dataset &ds, const ObjectDetectorImpl::LeafList &leafs,
                                                const ObjectDetectorImpl::LabelProbList &probs, std::vector<Matrix4f> &poses, std::vector<float> &errors)
{

    ModelData &data = model_map_[clabel] ;

    cv::Mat_<ushort> depth = ds.depth_images_[0] ;

    for( uint iter = 0 ; iter < params_.max_refinement_steps_ ; iter ++ ) {

        bool no_more_refinement_needed = true ;

        for( uint k=0 ; k<poses.size() ; k++ ) {

            Isometry3f H(poses[k]) ;

            cv::Mat_<ushort> zbuffer = render_mask(clabel, cam, poses[k]) ;

            uint w = zbuffer.cols, h = zbuffer.rows ;

            uint n_trees = leafs.size() ;
            uint n_samples = ds.samples_.size() ;

            // try to find all sample pixels within the object mask that lead to a small coordinate re-projection error

            vector<Vector3f> ipts, opts ;

            Matrix4f cami(data.camera_.inverse());
             Affine3f m2w(cami) ;

            for( uint i=0 ; i<n_samples ; i++ ) {
                const cv::Point &pt = ds.samples_[i] ;

                ushort val = zbuffer[pt.y][pt.x] ;
                if ( val == 0 ) continue ;

                Vector3f ip = back_project(depth, cam, pt) ;

                // here we have only samples that fall inside the mask

                float min_e = FLT_MAX ;
                uint best_j ;
                Vector3f bop ;

                // find best tree and associated re-projection error

                for( uint j=0 ; j<n_trees ; j++ ) {
                    RandomForest::Node *n = leafs[j][i] ;

                    Vector3f op = m2w * n->data_.coordinates_[clabel] ;

                    float e = ( H * op - ip).norm() ;
                    if ( e < min_e ) {
                        min_e = e ;
                        best_j = j ;
                        bop = op ;
                    }
                }

                if ( min_e < params_.inlier_threshold_ ) {
                    ipts.push_back(ip) ;
                    opts.push_back(bop) ;
                }
            }

            uint n_inliers = ipts.size() ;

            if ( n_inliers < 3 ) continue ;

            Eigen::Map<Matrix3Xf> src((float *)opts.data(), 3, n_inliers)  ;
            Eigen::Map<Matrix3Xf> dst((float *)ipts.data(), 3, n_inliers)  ;

            // find alignment transformation

            Affine3f Hr = find_rigid(src, dst);

            float e = compute_energy(clabel, Hr.matrix(), leafs, probs, cam, ds) ;

            if ( e < errors[k] ) {
                no_more_refinement_needed = false ;
                errors[k] = e ;
                poses[k] = Hr.matrix() ;
            }

        }

        if ( no_more_refinement_needed ) break ;

    }
}

float ObjectDetectorImpl::refine_pose_icp(const string &clabel, const cv::Mat &dim, const PinholeCamera &cam, const Matrix4f &pose)
{
    Isometry3f ipose(pose.inverse().eval()) ;

    cv::Mat_<ushort> zbuffer = render_mask(clabel, cam, pose) ;
    uint w = zbuffer.cols, h = zbuffer.rows ;

    cv::Mat_<ushort> depth(dim) ;

    PointList3f ipts, rpts ;

    for ( uint i=0 ; i<h ; i++ ) {
        for(uint j=0 ; j<w ; j++ ) {

            ushort zval = zbuffer[i][j] ;

            if ( zval == 0 ) continue ;

            ushort ival = depth[i][j] ;

            if ( ival == 0 ) continue ;

            Vector3f ip = cam.backProject(j, i, ival/1000.0) ;

            ipts.push_back(ip) ;
        }
    }

    // resample cloud

    sampleCloudCenters(ipts, 0.01, rpts, {-10, -10, -10}, {10, 10, 10});

    ModelData &data = model_map_[clabel] ;

    ICPAligner::Parameters icp_params ;
    icp_params.inlier_distance_threshold_ = data.diameter_  ;

    ICPAligner icp(icp_params) ;

    uint n_inliers ;
    float error = icp.align(data.search_, data.cloud_, rpts, ipose, n_inliers) ;

    return error ;

}

void ObjectDetectorImpl::detectAll(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const PinholeCamera &cam,
                                   vector<ObjectDetector::Result> &results) {

    // pass image through the forest

    Dataset ds ;

    ds.addSingleImage(rgb, depth, mask, params_.n_samples_);

    LeafList leafs ;

    forest_.apply_parallel(ds, leafs) ;

    // compute label probabilities (eq. 1)

    LabelProbList probs ;

    compute_label_probabilities(leafs, probs);
#ifdef DEBUG
    show_classes(ds, probs) ;
    show_coords(ds, leafs, 0) ;
    show_coords(ds, leafs, 1) ;
    show_coords(ds, leafs, 2) ;
#endif

    for( uint k=0 ; k<labels_.size() ; k++ ) {

        vector<Matrix4f> poses ;
        vector<float> errors ;

        string clabel = labels_[k];

        find_pose_candidates(clabel, cam, ds, leafs, probs, poses, errors) ;
        refine_pose_candidates(clabel, cam, ds, leafs, probs, poses, errors) ;

        cv::Mat cim = ds.rgb_images_[0].clone() ;
#ifdef DEBUG
        for( uint i=0 ; i<poses.size() && i < 4; i++ ) {
            if ( i==0 ) draw_bbox(cim, cam, clabel, poses[i], cv::Vec3b(0, 255, 0)) ;
            else draw_bbox(cim, cam, clabel, poses[i], cv::Vec3b(0, 255, 255)) ;
        }

        imwrite("/tmp/detections.png", cim) ;
#endif
        if ( poses.size() > 0 && errors[0] < params_.max_energy_threshold_ ) {
            Matrix4f best_pose = poses[0] ;
            float error = errors[0] ;

            ObjectDetector::Result res ;
            res.label_ = clabel ;
            res.error_ = error ;
            res.pose_ = best_pose ;
            results.push_back(res) ;
        }
    }

}

bool ObjectDetectorImpl::detect(const std::string &label, const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &mask, const PinholeCamera &cam,
                                Matrix4f &pose, float &error) {

    // pass image through the forest

    Dataset ds ;

    ds.addSingleImage(rgb, depth, mask, params_.n_samples_);

    LeafList leafs ;

    forest_.apply_parallel(ds, leafs) ;

    // compute label probabilities (eq. 1)

    LabelProbList probs ;

    compute_label_probabilities(leafs, probs);

    assert( std::find(labels_.begin(), labels_.end(), label) != labels_.end() ) ;

    vector<Matrix4f> poses ;
    vector<float> errors ;

    find_pose_candidates(label, cam, ds, leafs, probs, poses, errors) ;
    refine_pose_candidates(label, cam, ds, leafs, probs, poses, errors) ;

    if ( poses.size() > 0 && errors[0] > params_.max_energy_threshold_ ) {
        Matrix4f best_pose = poses[0] ;
        float best_error = errors[0] ;
#ifdef DEBUG

        cv::Mat cim = ds.rgb_images_[0].clone() ;
        draw_bbox(cim, cam, label, best_pose, cv::Vec3b(0, 255, 255)) ;
        imwrite("/tmp/detection.png", cim) ;
#endif

        pose = best_pose ;
        error = best_error ;
        return true ;

    }
    return false ;
}

void ObjectDetectorImpl::show_classes(const Dataset &ds, const LabelProbList &probs) {

    cv::Mat_<cv::Vec3b> res(480, 640) ;

    res = cv::Vec3b(0, 0, 0);

    cv::Vec3b def_clrs[6] = { cv::Vec3b( 255, 0, 0), cv::Vec3b( 0, 255, 0 ), cv::Vec3b( 0, 0, 255 ), cv::Vec3b(255, 255, 0), cv::Vec3b(255, 0, 255), cv::Vec3b(0, 255, 255) } ;

    map<string, cv::Vec3b> cmap ;

    for( uint i=0 ; i<labels_.size() ; i++ ) {
        if ( i < 6 ) cmap[labels_[i]] = def_clrs[i] ;
        else cmap[labels_[i]] = cv::Vec3b(rand() % 255, rand() % 255, rand() % 255) ;
    }

    for( uint i=0 ; i<ds.samples_.size() ; i++ ) {

        float max_votes = 0 ;
        string best_label ;

        map<string, float>::const_iterator it = probs[i].begin() ;

        for ( ; it != probs[i].end() ; ++it ) {
            string label = it->first ;
            float p = it->second ;

            if ( p > 0.9 && p > max_votes ) {
                max_votes = p ;
                best_label = label ;
            }
        }

        const cv::Point &p = ds.samples_[i] ;
        cv::Vec3b clr = ( best_label.empty() ) ? cv::Vec3b(0, 0, 0) : cmap[best_label] ;

        res[p.y][p.x] = clr ;

    }

    cv::imwrite("/tmp/labels.png", res) ;
}

void ObjectDetectorImpl::show_coords(const Dataset &ds, const LeafList &leafs, uint t) {

    cv::Mat_<cv::Vec3b> res(480, 640) ;

    res = cv::Vec3b(0, 0, 0);

    for( uint i=0 ; i<ds.samples_.size() ; i++ ) {

        RandomForest::Node *node = leafs[t][i] ;

        float max_votes = node->data_.n_background_ ;
        string best_label ;

        map<string, uint64_t>::const_iterator it = node->data_.classes_.begin() ;
        for( ; it != node->data_.classes_.end() ; ++it ) {
            const string &label = it->first ;
            uint64_t count = it->second ;

            if ( count > max_votes ) {
                max_votes = count ;
                best_label = label ;
            }
        }

        if ( best_label.empty() ) continue ;

        Vector3f coords = node->data_.coordinates_[best_label] ;

        ModelData &md = model_map_[best_label] ;

        Vector3f vm = coords ;
        Vector3f ext = 1.5*(md.bmax_ - md.bmin_) ;

        uint8_t vx = 255*(vm.x()/ext.x() + 0.5);
        uint8_t vy = 255*(vm.y()/ext.y() + 0.5);
        uint8_t vz = 255*(vm.z()/ext.z() + 0.5);

        const cv::Point &p = ds.samples_[i] ;
        res[p.y][p.x] = cv::Vec3b(vx, vy, vz) ;
    }

    imwritef(res, "/tmp/coords_%02d.png", t) ;

}

void ObjectDetectorImpl::draw_bbox(cv::Mat &img, const PinholeCamera &cam, const string &clabel, const Matrix4f &pose, const cv::Vec3b &clr)
{
    ModelData &data = model_map_[clabel] ;

    Vector3f box[8] ;

    Isometry3f wc(pose) ;

    box[0] = wc * Vector3f(data.bmin_.x(), data.bmin_.y(), data.bmin_.z()) ;
    box[1] = wc * Vector3f(data.bmin_.x(), data.bmax_.y(), data.bmin_.z()) ;
    box[2] = wc * Vector3f(data.bmax_.x(), data.bmax_.y(), data.bmin_.z()) ;
    box[3] = wc * Vector3f(data.bmax_.x(), data.bmin_.y(), data.bmin_.z()) ;

    box[4] = wc * Vector3f(data.bmin_.x(), data.bmin_.y(), data.bmax_.z()) ;
    box[5] = wc * Vector3f(data.bmin_.x(), data.bmax_.y(), data.bmax_.z()) ;
    box[6] = wc * Vector3f(data.bmax_.x(), data.bmax_.y(), data.bmax_.z()) ;
    box[7] = wc * Vector3f(data.bmax_.x(), data.bmin_.y(), data.bmax_.z()) ;

    cv::Point pts[8] ;

    for( uint i=0 ; i<8 ; i++ ) {
        cv::Point2d p = cam.project(cv::Point3d(box[i].x(), box[i].y(), box[i].z())) ;
        pts[i] = cv::Point(p.x, p.y) ;
    }

    cv::line(img, pts[0], pts[1], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[1], pts[2], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[2], pts[3], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[3], pts[0], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;

    cv::line(img, pts[4], pts[5], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[5], pts[6], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[6], pts[7], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[7], pts[4], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;

    cv::line(img, pts[0], pts[4], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[1], pts[5], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[3], pts[7], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
    cv::line(img, pts[2], pts[6], cv::Scalar(clr[0], clr[1], clr[2]), 2) ;
}

void ObjectDetectorImpl::draw_results(cv::Mat &img, const PinholeCamera &cam, const std::vector<ObjectDetector::Result> &results ) {
    cv::Vec3b def_clrs[6] = { cv::Vec3b( 255, 0, 0), cv::Vec3b( 0, 255, 0 ), cv::Vec3b( 0, 0, 255 ), cv::Vec3b(255, 255, 0), cv::Vec3b(255, 0, 255), cv::Vec3b(0, 255, 255) } ;

    map<string, cv::Vec3b> cmap ;

    for( uint i=0 ; i<labels_.size() ; i++ ) {
        if ( i < 6 ) cmap[labels_[i]] = def_clrs[i] ;
        else cmap[labels_[i]] = cv::Vec3b(rand() % 255, rand() % 255, rand() % 255) ;
    }

    for( uint i=0 ; i<results.size() ; i++ ) {
        const ObjectDetector::Result &res = results[i] ;
        cv::Vec3b clr = cmap[res.label_] ;
        draw_bbox(img, cam, res.label_, res.pose_, clr) ;
    }

}

void ObjectDetectorImpl::draw_result(cv::Mat &img, const PinholeCamera &cam, const std::string &clabel, const Matrix4f &pose, const cv::Vec3b &clr) {
    draw_bbox(img, cam, clabel, pose, clr) ;
}

void ObjectDetectorImpl::refine(const string &label, const cv::Mat &depth, const PinholeCamera &cam, Matrix4f &pose, float &error)
{
    error = refine_pose_icp(label, depth, cam, pose) ;
}


//
// Created by eliane on 05/03/19.
//

#include "DifferentialEvolution.h"

#include <ensmallen.hpp>

#undef BOOST_LOG

#include <boost/log/trivial.hpp>
#include <Engines/GlobalHeuristics/Utilities/MetropolisHastings.h>

namespace SmolDock::Heuristics {

    DifferentialEvolution::DifferentialEvolution(Score::ScoringFunction* scorFunc_, Optimizer::Optimizer* optimizer_,
                                           unsigned int seed_, DifferentialEvolution::Parameters params_) :
            scorFunc(scorFunc_), optimizer(optimizer_), rndGenerator(seed_), params(params_) {
                BOOST_LOG_TRIVIAL(error) << "DifferentialEvolution is not implemented. ";

    }

    bool DifferentialEvolution::search() {

        arma::mat currentState = scorFunc->getStartingConditions();

        unsigned int populationSize = 100;
        unsigned int maxGenerations = 25000;
        double crossoverRate = 0.6;
        double differentialWeight = 0.02;

        ens::DE DiffentialEvolutionHeur(populationSize, maxGenerations, crossoverRate, differentialWeight);

        DiffentialEvolutionHeur.Optimize(*this->scorFunc, currentState);

        double scoreBeforeLocalOptim = this->scorFunc->Evaluate(currentState);

        this->optimizer->optimize(currentState);
        double scoreAfterLocalOptim = this->optimizer->getScore();

        this->result = this->optimizer->getRawResultMatrix();

        BOOST_LOG_TRIVIAL(debug) << "DifferentialEvolution: Score after DE          = " << scoreBeforeLocalOptim;
        BOOST_LOG_TRIVIAL(debug) << "DifferentialEvolution: Score after local optim = " << scoreAfterLocalOptim;

        return true;
    }

    arma::mat DifferentialEvolution::getResultMatrix() {
        return this->result;
    }

}


/***************************************************************************
 * Copyright (C) 2009 by gempa GmbH
 *
 * Author: Jan Becker
 * Email: jabe@gempa.de
 *
 * Modifications 2010 - 2011 by Stefan Heimers <heimers@sed.ethz.ch>
 * Modifications 2014 - 2015 by  JeromeSalichon
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 ***************************************************************************/

#define SEISCOMP_COMPONENT MLR

#include "mlr.h"
#include <seiscomp3/logging/log.h>
#include <seiscomp3/core/strings.h>
#include <seiscomp3/processing/magnitudeprocessor.h>
#include <seiscomp3/math/geo.h>

#include <iostream>

#include <boost/bind.hpp>
#include <unistd.h>

// Only valid within 0-20 degrees
#define DELTA_MIN 0.
#define DELTA_MAX 20

// Only valid for depths up to 800 km
#define DEPTH_MAX 800

#define MAG_TYPE "MLr"

using namespace std;

using namespace Seiscomp;
using namespace Seiscomp::Processing;

ADD_SC_PLUGIN(
	"MLr GNS magnitude, J. Ristau method",
	"Derived from MLh magnitude & Sc3 Tutorial methods, J.Salichon, GNS Science New Zealand, J.Becker, Gempa",
	0, 0, 1
);

class Magnitude_MLR : public Processing::MagnitudeProcessor {
	public:
		struct param_struct {
			double dist;
			double A;
			bool nomag;
		};

		list<param_struct> list_of_parametersets;
		param_struct selected_parameterset;

		Magnitude_MLR() : Processing::MagnitudeProcessor(MAG_TYPE) {}

		//Same amplitude measurement as MLv (ML Sc3 amplitude on vertical)
		std::string amplitudeType() const {
			return "MLv" ;
		}

		// Do not need to compute another amplitude.

		bool setup(const Settings &settings) {
			// Reset the configuration
			list_of_parametersets.clear();
			try {
				// Read the settings variable MLr.params. This can be found in the applications
				// configuration file at:
				// module.trunk.global.MLr.params
				//   or per station (highest priority)
				// module.trunk.NZ.WEL.MLr.params
				if ( !initParameters(list_of_parametersets, settings.getString("MLr.params")) )
					return false;
			}
			catch ( ... ) {}

			return true;
		}

		MagnitudeProcessor::Status computeMagnitude(
		      double amplitude,   // in milimeters
		      double period,      // in seconds
		      double delta,       // in degrees
		      double depth,       // in kilometers
		      double &value)
		{
			if ( delta < DELTA_MIN || delta > DELTA_MAX )
				return DistanceOutOfRange;

			if ( depth > DEPTH_MAX )
				return DepthOutOfRange;

			return compute_ML(amplitude, delta, depth, &value);
		}

	private:
		// create a C++ list of structs containing all configured
		// parameter sets.
		// run this once at program start
		bool initParameters(list<param_struct> &paramlist, const string &params) {
			string paramset, range_str, minrange_str;
			string A_str;

			// for each parameter set
			istringstream iss_params(params);
			while ( getline(iss_params,paramset,';') ) {
				// extract the parameters from this parameter set
				istringstream iss_paramset(paramset);
				iss_paramset >> range_str;
				iss_paramset >> A_str;

				param_struct new_paramset;
				if ( !Core::fromString(new_paramset.dist, range_str) ) {
					SEISCOMP_ERROR("MLr: %s: range is not a numeric value",
					               params.c_str());
					return false;
				}

				if ( A_str == "nomag" ) {
					new_paramset.A     = 0;
					new_paramset.nomag = true;
				}
				else {
					if ( !Core::fromString(new_paramset.A, A_str) ) {
						SEISCOMP_ERROR("MLr: %s: not a numeric value",
						               A_str.c_str());
						return false;
					}

					new_paramset.nomag = false;
				}

				paramlist.push_back(new_paramset);
			}

			return true;
		}

		// select the right parameter set for a given distance. init_parameters() must
		// have been called before using this function.
		param_struct selectParameters(double distance, const list<param_struct> &paramlist) {
			double last_dist = 1000000; // arbitrary number larger than any expected distance;
			param_struct selected_parameters;

			// defaults in case the distance is out of the definded ranges
			selected_parameters.nomag = true;
			selected_parameters.dist = 0;
			selected_parameters.A = 0;

			list<param_struct>::const_iterator paramit;
			for ( paramit = paramlist.begin(); paramit != paramlist.end(); ++paramit ) {
				if ( (paramit->dist < last_dist) && (paramit->dist >= distance) ) {
					selected_parameters = *paramit;
					last_dist = paramit->dist;
				}
			}

			return selected_parameters;
		}

		MagnitudeProcessor::Status compute_ML(
		        double amplitude, // in micrometers
		        double delta,     // in degrees
		        double depth,     // in kilometers
		        double *mag) {
			float epdistkm,hypdistkm;
			float LogAmpREF; //MLr parameters
			float magTemp ; //MLr parameters

			if ( amplitude <= 0. ) {
				*mag = 0;
				return Error;
			}

			// examples:
			// epdistkm <= 60 km =>    mag=log10(waampl) + 0.018 *epdistkm+1.77 ;
			// 60 < epdistkm <= 700 => mag=log10(waampl) + 0.0038*epdistkm+2.62 ;
			// more generic: mag = log10(waampl) + A * epdistkm
			//
			// a list of distance ranges and corresponding values for A
			// is read from a config file.

			// calculate the distance in kilometers from the distance in degrees
			epdistkm = Math::Geo::deg2km(delta);
			hypdistkm = sqrt(epdistkm*epdistkm + depth * depth);

			// read the values for A and epdistkm from the config file and
			// select the right set depending on the distance
			selected_parameterset = selectParameters(hypdistkm, list_of_parametersets);

			SEISCOMP_DEBUG("epdistkm: %f\n",epdistkm);
			SEISCOMP_DEBUG("hypdistkm: %f\n",hypdistkm);

			if ( selected_parameterset.nomag ) {
				SEISCOMP_DEBUG( "epicenter distance out of configured range, no magnitude");
				return DistanceOutOfRange;
			}
			else {
				SEISCOMP_DEBUG("The selected range is: %f", selected_parameterset.dist);
				SEISCOMP_DEBUG("A:     %f", selected_parameterset.A);
				//SEISCOMP_DEBUG("MLr: station %s.%s: %s",settings.networkCode.c_str(), settings.stationCode.c_str(),s.c_str());

				// ORI(sed): *mag = log10(amplitude)  + selected_parameterset.A * hypdistkm + selected_parameterset.B;
				// JRistau: *mag = log10(amplitude) - Log(Amp(hypdistkm)) + Selected_parameterset.A  ;
				//
				// Computing terms of magnitude
				LogAmpREF = 0.2869 - (1.272 * 1e-3) * hypdistkm - (1.493 * log10(hypdistkm) ) ;

				// magTemp = log10(amplitude) - LogAmpREF + StatCorr ;
				// A is  defined as STATION dependent ie. module.trunk.NZ.WEL.MLr.params ;
				// Not corrected stations: Default is 0. Use nomag option to disable;
				magTemp = log10(amplitude) - LogAmpREF + selected_parameterset.A ;
				*mag = magTemp ;

				SEISCOMP_DEBUG("Mlr:     %f", *mag);
				return OK;
			}
		}
};

REGISTER_MAGNITUDEPROCESSOR(Magnitude_MLR, MAG_TYPE);


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

#include <boost/simd/operator/functions/shift_right.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/assert.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( shift_right_, tag::cpu_
                                   , (A0)(A1)(X)
                                   , ((simd_<floating_<A0>,X>))
                                     ((generic_<integer_<A1> >))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type int_type;
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_right: a shift is out of range");

      return bitwise_cast<result_type>( bitwise_cast<int_type>(a0) >> a1 );
    }
  };
} } }

#endif


// ======================================================================
/*!
 * \file
 * \brief Regression tests for class NFmiAreaFactory
 */
// ======================================================================

#include "NFmiAreaFactory.h"
#include "NFmiEquidistArea.h"
#include "NFmiGdalArea.h"
#include "NFmiGnomonicArea.h"
#include "NFmiLatLonArea.h"
#include "NFmiMercatorArea.h"
#include "NFmiOrthographicArea.h"
#include "NFmiRotatedLatLonArea.h"
#include "NFmiStereographicArea.h"
#include "NFmiYKJArea.h"
#include <boost/shared_ptr.hpp>
#include <macgyver/Exception.h>
#include <regression/tframe.h>

#include <stdexcept>
#include <string>

//! Protection against conflicts with global functions
namespace NFmiAreaFactoryTest
{
// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for latlon projections
 */
// ----------------------------------------------------------------------

void create_latlon()
{
  {
    const std::string def = "latlon:10,20,30,40";
    NFmiLatLonArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "latlon:10,20,30,40:2,3";
    NFmiLatLonArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40), NFmiPoint(0, 0), NFmiPoint(2, 3));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "latlon:10,20,30,40:0,1,2,3";
    NFmiLatLonArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40), NFmiPoint(0, 1), NFmiPoint(2, 3));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for rotlatlon projections
 */
// ----------------------------------------------------------------------

void create_rotlatlon()
{
  boost::shared_ptr<NFmiArea> area;

  {
    const std::string def = "rotlatlon:10,20,30,40";
    NFmiRotatedLatLonArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40), NFmiPoint(0, -90));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "rotlatlon,-30:10,20,30,40";
    NFmiRotatedLatLonArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40), NFmiPoint(0, -30));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "rotlatlon,-50,30:10,20,30,40";
    NFmiRotatedLatLonArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40), NFmiPoint(30, -50));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for ykj projections
 */
// ----------------------------------------------------------------------

void create_ykj()
{
  boost::shared_ptr<NFmiArea> area;

  {
    const std::string def = "ykj:10,20,30,40";
    NFmiYKJArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for mercator projections
 */
// ----------------------------------------------------------------------

void create_mercator()
{
  boost::shared_ptr<NFmiArea> area;

  {
    const std::string def = "mercator:10,20,30,40";
    NFmiMercatorArea expected(NFmiPoint(10, 20), NFmiPoint(30, 40));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for stereographic projections
 */
// ----------------------------------------------------------------------

void create_stereographic()
{
  boost::shared_ptr<NFmiArea> area;

  {
    const std::string def = "stereographic:10,20,30,40";
    NFmiStereographicArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 0, NFmiPoint(0, 0), NFmiPoint(1, 1), 90, 60);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "stereographic,20:10,20,30,40";
    NFmiStereographicArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 20, NFmiPoint(0, 0), NFmiPoint(1, 1), 90, 60);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "stereographic,20,90,50:10,20,30,40";
    NFmiStereographicArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 20, NFmiPoint(0, 0), NFmiPoint(1, 1), 90, 50);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for gnomonic projections
 */
// ----------------------------------------------------------------------

void create_gnomonic()
{
  boost::shared_ptr<NFmiArea> area;

  {
    const std::string def = "gnomonic:10,20,30,40";
    NFmiGnomonicArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 0, NFmiPoint(0, 0), NFmiPoint(1, 1), 90, 60);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "gnomonic,20:10,20,30,40";
    NFmiGnomonicArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 20, NFmiPoint(0, 0), NFmiPoint(1, 1), 90, 60);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "gnomonic,20,90,50:10,20,30,40";
    NFmiGnomonicArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 20, NFmiPoint(0, 0), NFmiPoint(1, 1), 90, 50);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for equidist projections
 */
// ----------------------------------------------------------------------

void create_equidist()
{
  boost::shared_ptr<NFmiArea> area;

  {
    const std::string def = "equidist:10,20,30,40";
    NFmiEquidistArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 0, NFmiPoint(0, 0), NFmiPoint(1, 1), 90);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "equidist,20:10,20,30,40";
    NFmiEquidistArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 20, NFmiPoint(0, 0), NFmiPoint(1, 1), 90);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "equidist,20,70:10,20,30,40";
    NFmiEquidistArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), 20, NFmiPoint(0, 0), NFmiPoint(1, 1), 70);
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * \brief Test NFmiAreaFactory::Create() for Gdal projections
 */
// ----------------------------------------------------------------------

void create_gdal()
{
  boost::shared_ptr<NFmiArea> area;

  {
    const std::string def = "EPSG:2393|10,20,30,40";
    NFmiGdalArea expected("WGS84", "EPSG:2393", NFmiPoint(10, 20), NFmiPoint(30, 40));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "WGS84:EPSG:2393|10,20,30,40";
    NFmiGdalArea expected("WGS84", "EPSG:2393", NFmiPoint(10, 20), NFmiPoint(30, 40));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    const std::string def = "FMI:EPSG:2393|10,20,30,40";
    NFmiGdalArea expected("FMI", "EPSG:2393", NFmiPoint(10, 20), NFmiPoint(30, 40));
    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));
    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------

void create_desired_resolution()
{
  boost::shared_ptr<NFmiArea> area;

  {
    NFmiLatLonArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), NFmiPoint(0, 0), NFmiPoint(222, 222));

    const std::string def = "latlon:10,20,30,40:10,10km";

    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));

    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  {
    NFmiLatLonArea expected(
        NFmiPoint(10, 20), NFmiPoint(30, 40), NFmiPoint(0, 0), NFmiPoint(222, 222));

    const std::string def = "latlon:10,20,30,40:10000x10000m";

    boost::shared_ptr<NFmiArea> area(NFmiAreaFactory::Create(def));

    if (!(expected == *area))
      TEST_FAILED("Failed to create " + def);
  }

  TEST_PASSED();
}

void create_latvia_stereographic_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  // Latvia stereographic projection
  const std::string def = "+lat_0=90 +proj=stere +lat_ts=60 +ellps=bessel +lon_0=14";
  NFmiStereographicArea expected(
      BottomLeftLatLon, TopRightLatLon, 14, TopLeftXY, BottomRightXY, 90, 60);
  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void create_FMI_stereographic_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  // Latvia stereographic projection
  const std::string def =
      "+proj=stere +lat_0=90 +lon_0=20 +lat_ts=60 +a=6371288 +x_0=124657.602609 "
      "+y_0=3252402.178534 +units=m";
  NFmiStereographicArea expected(
      BottomLeftLatLon, TopRightLatLon, 20, TopLeftXY, BottomRightXY, 90, 60);
  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void create_orthographic_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  const std::string def = "+proj=ortho +lat_0=90N +lat_ts=60 +x_0=580886.31 +y_0=3595677.92";
  NFmiOrthographicArea expected(BottomLeftLatLon, TopRightLatLon, 0, TopLeftXY, BottomRightXY);

  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void create_gnomonic_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  const std::string def =
      "+proj=gnom +lon_0=28E +lat_0=90N +lat_ts=60 +x_0=580886.31 +y_0=3595677.92";
  NFmiGnomonicArea expected(BottomLeftLatLon, TopRightLatLon, 28, TopLeftXY, BottomRightXY, 90, 60);

  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void create_latvia_equidist_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  const std::string def = "+proj=aeqd +lon_0=23.9897 +lat_0=56.9143 +ellps=sphere +a=6371000";
  NFmiEquidistArea expected(
      BottomLeftLatLon, TopRightLatLon, 23.9897, TopLeftXY, BottomRightXY, 56.9143);

  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void create_latlon_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  const std::string def = "+proj=latlon  +lon_0=22E";
  NFmiLatLonArea expected(BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);

  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void create_mercator_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  const std::string def = "+proj=merc";
  NFmiMercatorArea expected(BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);

  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void create_ykj_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  const std::string def =
      "+proj=tmerc +lat_0=0 +lon_0=27 +k=1 +x_0=3500000 +y_0=0 +ellps=intl +units=m +no_defs";
  NFmiYKJArea expected(BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);

  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void test_negative_params_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  const std::string def =
      "+proj=stere +lon_0=25w +lat_0=90 +lat_ts=10s +x_0=580886.31 +y_0=3595677.92";
  NFmiStereographicArea expected(
      BottomLeftLatLon, TopRightLatLon, -25, TopLeftXY, BottomRightXY, 90, -10);

  boost::shared_ptr<NFmiArea> area =
      NFmiAreaFactory::CreateProj(def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
  if (!(expected == *area))
    TEST_FAILED("Failed to create " + def);

  TEST_PASSED();
}

void test_invalid_input_proj()
{
  const NFmiPoint BottomLeftLatLon(10, 20), TopRightLatLon(30, 40), TopLeftXY(0, 0),
      BottomRightXY(1, 1);
  {
    // Missing lon_0 parameter
    const std::string def = "+proj=stere +lat_0=90 +lat_ts=10s +x_0=580886.31 +y_0=3595677.92";
    try
    {
      boost::shared_ptr<NFmiArea> area = NFmiAreaFactory::CreateProj(
          def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
      TEST_FAILED("No exception thrown for missing lon_0");
    }

    catch (Fmi::Exception& e)
    {
    }
    catch (...)
    {
      TEST_FAILED("Wrong kind of exception thrown");
    }
  }

  {
    // Missing proj definition
    const std::string def = "+lat_0=90 +lat_ts=10s +x_0=580886.31 +y_0=3595677.92";
    try
    {
      boost::shared_ptr<NFmiArea> area = NFmiAreaFactory::CreateProj(
          def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
      TEST_FAILED("No exception thrown for missing proj definition");
    }

    catch (Fmi::Exception& e)
    {
    }
    catch (...)
    {
      TEST_FAILED("Wrong kind of exception thrown");
    }
  }

  {
    // Missing missing +-prefixes in parameters
    const std::string def = "+lat_0=90 lat_ts=10s x_0=580886.31 +y_0=3595677.92";
    try
    {
      boost::shared_ptr<NFmiArea> area = NFmiAreaFactory::CreateProj(
          def, BottomLeftLatLon, TopRightLatLon, TopLeftXY, BottomRightXY);
      TEST_FAILED("No exception thrown for missing + prefix");
    }

    catch (Fmi::Exception& e)
    {
    }
    catch (...)
    {
      TEST_FAILED("Wrong kind of exception thrown");
    }
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * The actual test suite
 */
// ----------------------------------------------------------------------

class tests : public tframe::tests
{
  virtual const char* error_message_prefix() const { return "\n\t"; }
  void test(void)
  {
    TEST(create_latlon);
    TEST(create_rotlatlon);
    TEST(create_ykj);
    TEST(create_mercator);
    TEST(create_stereographic);
    TEST(create_gnomonic);
    TEST(create_equidist);
    TEST(create_latvia_stereographic_proj);
    TEST(create_FMI_stereographic_proj);
    TEST(create_orthographic_proj);
    TEST(create_gnomonic_proj);
    TEST(create_latvia_equidist_proj);
    TEST(create_latlon_proj);
    TEST(create_mercator_proj);
    TEST(create_ykj_proj);
    TEST(create_desired_resolution);
    TEST(create_gdal);
    TEST(test_negative_params_proj);
    TEST(test_invalid_input_proj);
  }
};

}  // namespace NFmiAreaFactoryTest

//! The main program
int main(void)
{
  using namespace std;
  cout << endl << "NFmiAreaFactory tester" << endl << "======================" << endl;
  NFmiAreaFactoryTest::tests t;
  return t.run();
}

// ======================================================================


/*
 * 	robot_server_dvrk.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2016-11-07
 *
 *  Base class for dVRK robot arms, itself usable for
 *  MTMs and ECM.
 *
 */

#ifndef ROBOT_SERVER_DVRK_HPP_
#define ROBOT_SERVER_DVRK_HPP_

#include <iostream>
#include <sstream>
#include <vector>
#include <cmath>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float32.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <irob_dvrk/arm_types.hpp>
#include <irob_general_robot/robot_server.hpp>

namespace saf {

class RobotServerDVRK: public RobotServer {

public:

  // Constants
  static const std::string READY;

protected:
  const ArmTypes arm_typ;

  // States
  std_msgs::String current_state;
  sensor_msgs::JointState position_joint;
  geometry_msgs::PoseStamped position_cartesian_current;
  std_msgs::String error;
  std_msgs::String warning;

  // Subscribers
  ros::Subscriber current_state_sub;
  ros::Subscriber state_joint_current_sub;
  ros::Subscriber position_cartesian_current_sub;
  ros::Subscriber error_sub;
  ros::Subscriber warning_sub;

  // Publishers
  ros::Publisher position_joint_pub;
  ros::Publisher position_cartesian_pub;

  void subscribeLowLevelTopics();
  void advertiseLowLevelTopics();

public:
  RobotServerDVRK(ros::NodeHandle, ros::NodeHandle, ArmTypes, std::string, bool);
  ~RobotServerDVRK();

  // Callbacks
  void initArm();
  void resetPose(bool);
  void stop();
  void followTrajectory(Trajectory<Pose>);

  void robotStateCB(const std_msgs::String);
  void errorCB(const std_msgs::String);
  void warningCB(const std_msgs::String);
  void stateJointCurrentCB(const sensor_msgs::JointStateConstPtr&);
  virtual void positionCartesianCurrentCB(
      const geometry_msgs::PoseStampedConstPtr&);

  void loadRegistration(std::string);

  double getJointStateCurrent(int);
  std::vector<double> getJointStateCurrent();
  Eigen::Vector3d getPositionCartesianCurrent();
  Eigen::Quaternion<double> getOrientationCartesianCurrent();
  Pose getPoseCurrent();

  //DVRK actions
  std::string getCurrentState();
  void moveJointRelative(int, double, double = 0.01 );
  void moveJointAbsolute(int, double, double = 0.01 );
  void moveCartesianRelative(Eigen::Vector3d, double = 0.01);
  void moveCartesianAbsolute(Eigen::Vector3d, double = 0.01);
  void moveCartesianAbsolute(Eigen::Quaternion<double>, double = 0.01);
  virtual void moveCartesianAbsolute(Pose, double = 0.01);

  void recordTrajectory(Trajectory<Eigen::Vector3d>&);
  void recordTrajectory(Trajectory<Pose>&);

  void checkErrors();
  void checkVelCartesian(const Pose&, const Pose&, double);
  void checkNaNCartesian(const Pose&);
  void checkVelJoint(const sensor_msgs::JointState&,
                     const std::vector<double>&, double);
  void checkNaNJoint(const sensor_msgs::JointState&);

};

}
#endif /* ROBOT_SERVER_DVRK_HPP_ */


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE nn_mlp

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cmath>
#include <algorithm>
#include "mlp.hpp"

BOOST_AUTO_TEST_CASE(nn_elman)
{
  using namespace nn;
  Mlp<Neuron<PfWSum<>, AfSigmoidNoBias<> >, Connection<> > nn(2, 3, 2);
  nn.init();
  BOOST_CHECK_EQUAL(nn.get_nb_neurons(), 3 + 3 + 2);
  BOOST_CHECK_EQUAL(nn.get_nb_connections(), 9 + 6 + 2);
  std::vector<float> in(2);
  in[0] = 1.0f;
  in[1] = 0.2f;
  for (unsigned i = 0; i < 1000; ++i)
    nn.step(in);
  float out1 = nn.get_outf(0);
  float out2 = nn.get_outf(1);

}


#include "PauliOperator.hpp"
#include "IRProvider.hpp"
#include <boost/math/constants/constants.hpp>

namespace xacc {
namespace vqe {
const std::map<std::string, std::pair<c, std::string>> Term:: pauliProducts =  Term::create_map();

PauliOperator::PauliOperator() {
}

PauliOperator::PauliOperator(std::complex<double> c) {
	terms.emplace(std::make_pair("I",c));
}

PauliOperator::PauliOperator(double c) {
	terms.emplace(std::make_pair("I",c));
}

PauliOperator::PauliOperator(std::string var) {
	terms.emplace(std::make_pair("I", var));
}

PauliOperator::PauliOperator(std::complex<double> c, std::string var) {
	terms.emplace(std::piecewise_construct,
            std::forward_as_tuple("I"),
            std::forward_as_tuple(c, var));
}

PauliOperator::PauliOperator(const PauliOperator& i) : terms(i.terms) {
}

/**
 * The Constructor, takes a vector of
 * qubit-gatename pairs. Initializes coefficient to 1
 *
 * @param operators The pauli operators making up this SpinInstruction
 */
PauliOperator::PauliOperator(std::map<int, std::string> operators) {
	terms.emplace(std::make_pair(Term::id(operators), operators));
}

PauliOperator::PauliOperator(std::map<int, std::string> operators, std::string var) {
	terms.emplace(std::piecewise_construct,
            std::forward_as_tuple(Term::id(operators, var)),
            std::forward_as_tuple(var, operators));
}

/**
 * The Constructor, takes a vector of
 * qubit-gatename pairs and this instruction's coefficient
 *
 * @param operators
 * @param coeff
 */
PauliOperator::PauliOperator(std::map<int, std::string> operators,
		std::complex<double> coeff) {
	terms.emplace(std::piecewise_construct,
            std::forward_as_tuple(Term::id(operators)),
            std::forward_as_tuple(coeff, operators));

}

PauliOperator::PauliOperator(std::map<int, std::string> operators,
		double coeff) : PauliOperator(operators, std::complex<double>(coeff,0)) {
}

PauliOperator::PauliOperator(std::map<int, std::string> operators,
		std::complex<double> coeff, std::string var) {
	terms.emplace(std::piecewise_construct,
            std::forward_as_tuple(Term::id(operators, var)),
            std::forward_as_tuple(coeff, var, operators));

}

std::vector<Triplet> PauliOperator::getSparseMatrixElements() {

	// Get number of qubits
	std::set<int> distinctSites;
	for (auto& kv : terms) {
		for (auto& kv2 : kv.second.ops()) {
			distinctSites.insert(kv2.first);
		}
	}

	auto nQubits = distinctSites.size();

	std::vector<Triplet> triplets;

	for (auto& kv : terms) {
		auto termTrips = kv.second.getSparseMatrixElements(nQubits);
		triplets.insert(std::end(triplets), std::begin(termTrips), std::end(termTrips));
	}

	return triplets;
}

ActionResult Term::action(const std::string& bitString, ActionType type) {

	auto _coeff = coeff();
	auto newBits = bitString;
	c i(0,1);

	for (auto& t : ops()) {
		auto idx = t.first;
		auto gate = t.second;

		if (gate == "Z") {
			_coeff *= newBits[idx] == '1' ? -1 : 1;
		} else if (gate == "X") {
			newBits[idx] = (newBits[idx] == '1' ? '0' : '1');
		} else if (gate == "Y") {
			if (type == ActionType::Bra) {
				_coeff *= newBits[idx] == '1' ? i : -i;
			} else {
				_coeff *= newBits[idx] == '1' ? -i : i;
			}
			newBits[idx] = (newBits[idx] == '1' ? '0' : '1');
		}
	}

	return {newBits, _coeff};
}

const std::vector<std::pair<std::string, std::complex<double>>> PauliOperator::computeActionOnKet(
		const std::string& bitString) {

	std::vector<std::pair<std::string, std::complex<double>>> ret;
	std::string newBits = bitString;
	std::complex<double> newCoeff(1,0), i(0,1);

	for (auto& kv : terms) {
		ret.push_back(kv.second.action(bitString, ActionType::Ket));
	}
	return ret;
}

const std::vector<std::pair<std::string, std::complex<double>>> PauliOperator::computeActionOnBra(
		const std::string& bitString) {
	std::vector<std::pair<std::string, std::complex<double>>> ret;

	for (auto& kv : terms) {
		ret.push_back(kv.second.action(bitString, ActionType::Bra));
	}

	return ret;
}

const int PauliOperator::nTerms() {
	return terms.size();
}

bool PauliOperator::isClose(PauliOperator& other) {
	if (!operator==(other)) {
		return false;
	}

	// These are equal, check their coeffs
	for (auto& kv : terms) {
		Term otherTerm = other.terms[kv.first];
		if (std::abs(kv.second.coeff() - otherTerm.coeff()) > 1e-6) {
			return false;
		}
	}

	return true;
}

/**
 * Persist this Instruction to an assembly-like
 * string.
 *
 * @param bufferVarName The name of the AcceleratorBuffer
 * @return str The assembly-like string.
 */
const std::string PauliOperator::toString() {
	std::stringstream s;
	for (auto& kv : terms) {
		std::complex<double> c = std::get<0>(kv.second);
		std::string v = std::get<1>(kv.second);
		std::map<int, std::string> ops = std::get<2>(kv.second);

		s << c << " ";
		if (!v.empty()) {
			s << v << " ";
		}

		for (auto& kv2 : ops) {
			if (kv2.second == "I") {
				s << "I ";
			} else {
				s << kv2.second << kv2.first << " ";
			}
		}

		s << "+ ";
	}

	auto r = s.str().substr(0, s.str().size() - 2);
	boost::trim(r);
	return r;
}

void PauliOperator::clear() {
	terms.clear();
}

PauliOperator& PauliOperator::operator+=( const PauliOperator& v ) noexcept {
	for (auto& kv : v.terms) {

		auto termId = kv.first;
		auto otherTerm = kv.second;

		if (terms.count(termId)) {
			terms.at(termId).coeff() += otherTerm.coeff();
		} else {
			terms.insert({termId, otherTerm});
		}

		if (std::abs(terms[termId].coeff()) < 1e-12) {
			terms.erase(termId);
		}
	}

	return *this;
}

PauliOperator& PauliOperator::operator-=( const PauliOperator& v ) noexcept {
	return operator+=(-1.0 * v);
}

PauliOperator& PauliOperator::operator*=( const PauliOperator& v ) noexcept {

	std::unordered_map<std::string, Term> newTerms;
	for (auto& kv : terms) {
		for (auto& vkv : v.terms) {
			auto multTerm = kv.second * vkv.second;
			auto id = multTerm.id();

			if (!newTerms.insert({id, multTerm}).second) {
				newTerms.at(id).coeff() += multTerm.coeff();
			}

			if (std::abs(newTerms.at(id).coeff()) < 1e-12) {
				newTerms.erase(id);
			}
		}
	}
	terms = newTerms;
	return *this;
}

bool PauliOperator::operator==( const PauliOperator& v ) noexcept {
	if (terms.size() != v.terms.size()) {
		return false;
	}

	for (auto& kv : terms) {
		bool found = false;
		for (auto& vkv : v.terms) {

			if (kv.second.operator==(vkv.second)) {
				found = true;
				break;
			}
		}

		if (!found) {
			return false;
		}
	}

	return true;
}

PauliOperator& PauliOperator::operator*=( const double v ) noexcept {
	return operator*=(std::complex<double>(v,0));
}

PauliOperator& PauliOperator::operator*=( const std::complex<double> v ) noexcept {
	for (auto& kv : terms) {
		std::get<0>(kv.second) *= v;
	}
	return *this;
}

std::vector<Triplet> Term::getSparseMatrixElements(const int nQubits) {

	// X = |1><0| + |0><1|
	// Y = -i|1><0| + i|0><1|
	// Z = |0><0| - |1><1|

	auto comb = [](int N, int K) -> std::vector<std::vector<int>>
	{
		std::string bitmask(K, 1); // K leading 1's
			bitmask.resize(N, 0);// N-K trailing 0's

			std::vector<std::vector<int>> allCombinations;
			// print integers and permute bitmask
			do {
				std::vector<int> bits;
				for (int i = 0; i < N; ++i) {
					if (bitmask[i]) {
						bits.push_back(i);
					}

//					if (bitmask[i]) std::cout << " " << i;
				}

				bool add = true;
				for (int i = 1; i < bits.size(); i++) {
					if(bits[i] % 2 != 0 && std::abs(bits[i] - bits[i-1]) == 1) {
						add = false;
						break;
					}
				}

				if (add) {
					allCombinations.push_back(bits);
				}
			}while (std::prev_permutation(bitmask.begin(), bitmask.end()));

			for (auto a : allCombinations) {
				for (auto b : a) {
					std::cout << b << " ";
				}
				std::cout << "\n";
			}

			std::cout << (3/2) << "\n";
			return allCombinations;
		};

	auto nSites = ops().size();
	auto termCombinations = comb(2*nSites,nSites);

	std::map<std::string, std::vector<std::pair<int, int>>> subterms;
	subterms["X"] = {{1,0}, {0,1}};
	subterms["Y"] = {{1,0}, {0,1}};
	subterms["Z"] = {{0,0}, {1,1}};

	std::string zeroStr = "";
	for (int i = 0; i < nQubits; i++) zeroStr += "0";

	auto ket = zeroStr;
	auto bra = zeroStr;

	std::vector<Triplet> triplets;
	for (auto& combo : termCombinations) {

		std::complex<double> coeff(1,0), i(0,1);
		for (auto& c : combo) {

			auto iter = ops().begin();
			std::advance(iter, c/2);
			auto ithOp = iter->second;
			auto ithOpSite = iter->first;
			auto x = (c % 2);

//			Now I know the Operator and whether this is hte 0 or 1st term for that op

			auto term = subterms[ithOp][x];

			ket[ithOpSite] = term.first ? '1' : '0';
			bra[ithOpSite] = term.second ? '1' : '0';

			if (ithOp == "Y") {
				if (x) {
					coeff *= i;
				} else {
					coeff *= -i;
				}
			} else if (ithOp == "Z") {
				if (x) {
					coeff *= -1;
				}
			}
		}

		std::cout << "HEY WORLD: " << bra << ", " << ket << "\n";
		// now convert bra and ket into integers
		auto row = std::stol(bra, nullptr, 2);
		auto col = std::stol(ket, nullptr, 2);

		triplets.emplace_back(row, col, coeff);
	}

	return triplets;
}

Term& Term::operator*=( const Term& v ) noexcept {

	coeff() *= std::get<0>(v);

	std::string ss;
	std::string myVar = std::get<1>(*this);
	std::string otherVar = std::get<1>(v);
	if (!myVar.empty()) {
		if (!otherVar.empty()) {
			ss = myVar + " " + otherVar;
		} else {
			ss = myVar;
		}
	} else {
		if(!otherVar.empty()) {
			ss = otherVar;
		}
	}

	std::get<1>(*this) = ss;

	auto otherOps = std::get<2>(v);
	for (auto& kv : otherOps) {
		auto qubit = kv.first;
		auto gate = kv.second;
		if (ops().count(qubit)) {
			// This means, we have a op on same qubit in both
			// so we need to check its product
			auto myGate = ops().at(qubit);
			auto gate_coeff = pauliProducts.at(myGate + gate);
			if (gate_coeff.second != "I") {
				ops().at(kv.first) = gate_coeff.second;
			} else {
				ops().erase(kv.first);
			}
			coeff() *= gate_coeff.first;
		} else if (gate != "I") {
			ops().emplace(std::make_pair(qubit, gate));
		}
	}

	return *this;
}

PauliOperator PauliOperator::eval(const std::map<std::string, std::complex<double>> varToValMap) {
	PauliOperator ret;

	for (auto& kv : terms) {

		auto id = kv.first;
		auto term = kv.second;

		for (auto& varVal : varToValMap) {
			if (varVal.first == term.var()) {
				term.var() = "";
				term.coeff() *= varVal.second;
			}
		}

		ret.terms.insert({id, term});
	}

	return ret;
}

std::shared_ptr<IR> PauliOperator::toXACCIR() {
// Create a new GateQIR to hold the spin based terms
	auto gateRegistry = xacc::getService<IRProvider>("gate");
	auto newIr = gateRegistry->createIR();
	int counter = 0;
	auto pi = boost::math::constants::pi<double>();

	// Populate GateQIR now...
	for (auto& inst : terms) {

		Term spinInst = inst.second;

		// Create a GateFunction and specify that it has
		// a parameter that is the Spin Instruction coefficient
		// that will help us get it to the user for their purposes.

		auto gateFunction = gateRegistry->createFunction(
				inst.first, {},
				std::vector<InstructionParameter> { InstructionParameter(
						spinInst.coeff()), InstructionParameter(
						spinInst.isIdentity() ? 1 : 0) });

		// Loop over all terms in the Spin Instruction
		// and create instructions to run on the Gate QPU.
		std::vector<std::shared_ptr<xacc::Instruction>> measurements;
		auto termsMap = spinInst.ops();

		std::vector<std::pair<int, std::string>> terms;
		for (auto& kv : termsMap) {
			if (kv.second != "I" && !kv.second.empty()) {
				terms.push_back( { kv.first, kv.second });
			}
		}

		for (int i = terms.size() - 1; i >= 0; i--) {
			auto qbit = terms[i].first;
			auto gateName = terms[i].second;
			auto meas = gateRegistry->createInstruction("Measure",
					std::vector<int> { qbit });
			xacc::InstructionParameter classicalIdx(qbit);
			meas->setParameter(0, classicalIdx);
			measurements.push_back(meas);

			if (gateName == "X") {
				auto hadamard = gateRegistry->createInstruction("H", std::vector<int> {
						qbit });
				gateFunction->addInstruction(hadamard);
			} else if (gateName == "Y") {
				auto rx = gateRegistry->createInstruction("Rx", std::vector<int> { qbit });
				InstructionParameter p(pi / 2.0);
				rx->setParameter(0, p);
				gateFunction->addInstruction(rx);
			}

		}

		if (!spinInst.isIdentity()) {
			for (auto m : measurements) {
				gateFunction->addInstruction(m);
			}
		}

		newIr->addKernel(gateFunction);
		counter++;
	}
	return newIr;
}

void PauliOperator::fromXACCIR(std::shared_ptr<IR> ir) {

	terms.clear();

	for (auto& kernel : ir->getKernels()) {
		std::map<int, std::string> pauliTerm;
		for (auto inst : kernel->getInstructions()) {
			bool seen = false;
			if (inst->name() == "H") {
				pauliTerm.insert({inst->bits()[0], "X"});
			} else if (inst->name() == "Rx") {
				pauliTerm.insert({inst->bits()[0], "Y"});
			}

			if (pauliTerm.count(inst->bits()[0]) == 0) {
				pauliTerm.insert({inst->bits()[0], "Z"});
			}
		}

		std::complex<double> c(1,0);
		if (kernel->nParameters() > 0) {
			c = boost::get<std::complex<double>>(kernel->getParameter(0));
		}

		Term t(c, pauliTerm);
		terms.insert({t.id(), t});

	}
}

}
}

bool operator==(const xacc::vqe::PauliOperator& lhs,
		const xacc::vqe::PauliOperator& rhs) {
	if (lhs.getTerms().size() != rhs.getTerms().size()) {
		return false;
	}
	for (auto& kv : lhs.getTerms()) {
		bool found = false;
		for (auto& vkv : rhs.getTerms()) {

			if (kv.second.operator==(vkv.second)) {
				found = true;
				break;
			}
		}

		if (!found) {
			return false;
		}
	}

	return true;
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * 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.
*/
/*! \file BlockCSRMatrix.hpp
Definition of a compressed row sparse block matrix of SiconosMatrix*
*/

#ifndef BLOCKCSRMATRIX_H
#define BLOCKCSRMATRIX_H

#include "NumericsFwd.h"  // Numerics forward declarations
#include "SimulationTypeDef.hpp"
#include "SiconosSerialization.hpp" // for ACCEPT_SERIALIZATION
#include <boost/numeric/ublas/fwd.hpp> // Boost forward declarations

/* with signed int typedef  boost::numeric::ublas::compressed_matrix<double*> CompressedRowMat; */
/* cf http://boost.2283326.n4.nabble.com/LU-decomposition-of-compressed-matrix-td3417929.html */
typedef boost::numeric::ublas::compressed_matrix <
double*, boost::numeric::ublas::basic_row_major<unsigned int>, 0, boost::numeric::ublas::unbounded_array<std::size_t> >  CompressedRowMat;
TYPEDEF_SPTR(CompressedRowMat)
TYPEDEF_SPTR(SparseBlockStructuredMatrix)

/** Definition of a compressed sparse row matrix of SiconosMatrix,
 * used in OneStepNSProblem to store the M matrix.
 *
 * This class defines a specific compressed row sparse storage for
 * blocks matrices, each block being a SiconosMatrix*.
 *
 * It handles:
 * - a SparseMat (boost-ublas) of SiconosMatrix*
 * - a vector<SiconosMatrix*> which handles the non-null blocks

 * - three vector<int> (IndexInt) to save non-null blocks position in
     row, columns and the list of the sizes of diagonal blocks.

 * - two int, the number of blocks in a row and the number of non null blocks.
 *
 * Each block of the current object represents the connection between
 * two coupled Interactions, \n (for example for Lagrangian
 * systems, a single \f$ H W^{-1} H^t \f$ block or for first order
 * systems \f$ hCW^{-1}B \f$ ...) \n
 *
 * This objects is built using an index set of SP::Interaction,
 * that represents the "active" constraints in the OSNS problem and a
 * map<SP::Interaction u1, <SP::Interaction u2, SP::SiconosMatrix block> >, block being the link
 * between u1 and u2. Only Interaction present in the index set are picked out
 * in the map.
 *
 *  A convert method is also implemented to create a
 *  SparseBlockStructuredMatrix which is Numerics-readable.
 *
 * As an example, consider the index set I={u1, u3, u5, u8} and the
 * map where non null blocks are (ui,ui), (u1,u3), (u1,u8), (u3,u1),
 * (u8,u1).\n Each block being a pointer to a 3x3 matrix.\n Then the
 * resulting matrix has 4 X 4 blocks, with 8 non-null blocks and looks
 * like:
 *
 \rst

 .. math::
    :nowrap:

    M=\left\lbrace\begin{array}{cccc}
    b11 & b13 & 0 & b18 \\
    b31 & b22 & 0 & 0 \\
    0   & 0   & b33&0 \\
    b81 & 0   & 0 & b44
    \end{array}\right.

 \endrst
 *
 * with nc = 4, nbNonNullBlocks = 8, RowPos = [0 0 0 1 1 2 3 3],
 * RowCol = [0 1 3 0 1 2 0 3]\n and _diagsize0 = [3 6 9 12].
 *
 * We use stl::vector (which may seems redundent with the double* of
 * the numerics SparseBlockStructuredMatrix) because memory can be
 * reserved during construction or initialized and then vectors are
 * resized when the object is filled in. This avoid some call to
 * malloc/free at each iteration.
 *
 */
class BlockCSRMatrix
{
private:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(BlockCSRMatrix);

  /** Number of blocks rows (first dimension of the block matrix)*/
  unsigned int _nr;

  /** Number of blocks columns (second dimension of the block matrix)*/
  unsigned int _nc;

  /** Sparse-Block Boost Matrix. Each block is a SiconosMatrix**/
  SP::CompressedRowMat _blockCSR;

  /** Specific structure required when a (Numerics) solver block is used */
  SP::SparseBlockStructuredMatrix _sparseBlockStructuredMatrix;

  /** Vector used to save the sum of rows of diagonal blocks of M:
      _diagsize0[i] = _diagsize0[i-1] + ni, ni being the size of the
      diagonal block at row(block) i */
  SP::IndexInt _diagsize0;

  /** Vector used to save the sum of dim of diagonal blocks of M:
      _diagsize0[i] = _diagsize0[i-1] + ni, ni being the size of the
      diagonal block at row(block) i */
  SP::IndexInt _diagsize1;

  /** List of non null blocks positions (in row) */
  SP::IndexInt rowPos;

  /** List of non null blocks positions (in col) */
  SP::IndexInt colPos;

  /** Private copy constructor => no copy nor pass by value */
  BlockCSRMatrix(const BlockCSRMatrix&);

  /** Private assignment -> forbidden
   * \return  BlockCSRMatrix&
   */
  BlockCSRMatrix& operator=(const BlockCSRMatrix&);

public:

  /** Default constructor -> empty matrix
   */
  BlockCSRMatrix();

  /** Constructor with dimension (number of blocks)
      \param n number of blocks in a row/column (only square matrices allowed)
  */
  BlockCSRMatrix(unsigned int n);

  /** Constructor from index set
      \param indexSet the index set of the active constraints
  */
  BlockCSRMatrix(InteractionsGraph& indexSet);

  /** destructor
   */
  ~BlockCSRMatrix();

  /** get size (in block-components)
   * \return unsigned int NumberOfBlocksInARow
   */
  inline unsigned int numberOfBlocksInARow() const
  {
    return _nr;
  };

  /** get total number of non-null blocks
   * \return unsigned int
   */
  unsigned int getNbNonNullBlocks() const;

  /** get the numerics-readable structure
   * \return SP::SparseBlockStructuredMatrix
   */
  inline SP::SparseBlockStructuredMatrix getNumericsMatSparse()
  {
    return _sparseBlockStructuredMatrix;
  };

  /** get the ublas sparse mat
   * \return SP::CompressedRowMat
   */
  inline SP::CompressedRowMat getMSparse()
  {
    return _blockCSR;
  };

  /** get the dimension of the square-diagonal block number num
   * \param i block position
   * \return unsigned int
  */
  IndexInt::value_type getSizeOfDiagonalBlock(int i) const
  {
    if (i == 0) return _diagsize0->at(0);
    else return (_diagsize0->at(i) - _diagsize0->at(i - 1));
  };

  /** get the index of blocks position (i=0 -> rows, i=1 -> columns)
   * \param i unsigned int, 0 for rows, 1 for columns
   * \return SP::IndexInt
   */
  inline SP::IndexInt getPositionsIndex(bool i)
  {
    if (i) return rowPos;
    else return colPos;
  };

  /** fill the current class using an index set
   *  \param indexSet set of the active constraints
   */
  void fill(InteractionsGraph& indexSet);

  /** fill the matrix with the Mass matrix
   * \warning only for NewtonEulerDS
   * \param indexSet of the active constraints
   */
  void fillM(InteractionsGraph& indexSet);

  /** fill the matrix with the H matrix
   * \warning only for NewtonEuler3DR
   * \param indexSet of the active constraints
   */
  void fillH(InteractionsGraph& indexSet);

  /** fill the numerics structure _sparseBlockStructuredMatrix using _blockCSR */
  void convert();

  /** display the current matrix
   */
  void display() const;
};

#endif


#include <boost/algorithm/searching/knuth_morris_pratt.hpp>


#include <boost/geometry/strategies/cartesian/area_surveyor.hpp>


#include <boost/filesystem.hpp>

#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <visualization_msgs/Marker.h>

// Include opencv2
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/PointCloud.h>

#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/eigen.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/range_image/range_image_planar.h>
#include <pcl/io/pcd_io.h>

#include <stop_watch.h>

using namespace cv;
using namespace std;
namespace fs = boost::filesystem;
// namespace plt = matplotlibcpp;

class Cloud2Depth
{
public:
    // typedef pcl::PointXYZRGB Point;
    typedef pcl::PointXYZ Point;
    typedef pcl::PointCloud<Point> PointCloud;

    /*
   * constructor
   * setup which topics are passed out (advertise) and to which topics are listend (subscribe)
   */
    Cloud2Depth() : it_(nh_)
    {
        pub_ = it_.advertise("/aditof_roscpp/aditof_cloud_2_depth_static", 1);

        // sub_ = nh_.subscribe("/cloud_pcd", 1, &Cloud2Depth::cloudCallback, this);

        double dist_thr;
        int max_its;

        // "~" means, that the node hand is opened within the private namespace (to get the "own" parameters)
        ros::NodeHandle private_nh("~");
        // printf("%s",param);
        if (private_nh.getParam("pcd_folder", _param))
        {
            ROS_INFO("Got param: %s", _param.c_str());
            for (fs::directory_entry & entry : fs::directory_iterator(_param))
            {
                // cout << entry.path() << '\n';

            }

        }else
        {
            ROS_ERROR("Failed to get param 'pcd_folder' ");
        }
    }

    ~Cloud2Depth() {}

    void pcdProc(const PointCloud::ConstPtr &cloud_in_msg)
    {
        if (!cloud_in_msg || cloud_in_msg->size() <= 0)
        {
            ROS_WARN("got empty or invalid pointcloud --> ignoring");
            return;
        }
        cv_image = Mat(height_, width_, CV_32FC1, 0.0); //Scalar(std::numeric_limits<float>::max()));

        int minRange = 0; // this is the smallest distance at which the camera can measure depending on the mode (near, far, etc.)

        for (int i = 0; i < cloud_in_msg->points.size(); i++)
        {
            if (cloud_in_msg->points[i].z == cloud_in_msg->points[i].z)
            {
                if (cloud_in_msg->points[i].z != 0.0)
                {
                    z = cloud_in_msg->points[i].z * 1000.0;
                    u = (cloud_in_msg->points[i].x * 1000.0 * focalX_) / z;
                    v = (cloud_in_msg->points[i].y * 1000.0 * focalY_) / z;
                    pixel_pos_x = (int)(u + centerX_);
                    pixel_pos_y = (int)(v + centerY_);

                    if (pixel_pos_x > (width_ - 1))
                    {
                        pixel_pos_x = width_ - 1;
                    }
                    else if (pixel_pos_x < 0)
                    {
                        pixel_pos_x = -pixel_pos_x;
                    }
                    if (pixel_pos_y > (height_ - 1))
                    {
                        pixel_pos_y = height_ - 1;
                    }
                    else if (pixel_pos_y < 0)
                    {
                        pixel_pos_y = -pixel_pos_y;
                    }
                }
                else
                {
                    pixel_pos_x = 0;
                    pixel_pos_y = 0;
                    z = 0.0;
                }

                cv_image.at<float>(pixel_pos_y, pixel_pos_x) = z - minRange;
            }
        }

        cv_image.convertTo(cv_image, CV_16UC1);

        // imshow("Display depth from point cloud", cv_image);
        waitKey(3);

        imwrite("depth_from_pcd.png", cv_image);
        sensor_msgs::ImagePtr output_image = cv_bridge::CvImage(std_msgs::Header(), "16UC1", cv_image).toImageMsg();
        pub_.publish(output_image);
    }

private:
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    // ros::Subscriber sub_;
    image_transport::Publisher pub_;
    // calibration parameters
    // double K[9] = {385.8655956930966, 0.0, 342.3593021849471,
    //                0.0, 387.13463636528166, 233.38372018194542,
    //                0.0, 0.0, 1.0};

    // EEPROM parameters
    double K[9] = {386.804, 0.0, 341.675,
                   0.0, 384, 238.973,
                   0.0, 0.0, 1.0};

    double centerX_ = K[2];
    double centerY_ = K[5];
    double focalX_ = K[0];
    double focalY_ = K[4];
    int height_ = 480;
    int width_ = 640;
    int pixel_pos_x, pixel_pos_y;
    float z, u, v;
    Mat cv_image;
    string _param;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "cloud_2_depth_node");

    Cloud2Depth c2d;

    ros::spin();

    return 0;
}


#include <kr_mav_msgs/PositionCommand.h>
#include <kr_mav_msgs/TRPYCommand.h>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <tf/transform_datatypes.h>

#include <Eigen/Geometry>
#include <kr_pid_control/PIDControl.hpp>

#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x)

class PIDControlNodelet : public nodelet::Nodelet
{
 public:
  PIDControlNodelet()
      : position_cmd_updated_(false),
        position_cmd_init_(false),
        des_yaw_(0),
        current_yaw_(0),
        enable_motors_(false),
        use_external_yaw_(false)
  {
  }

  void onInit(void);

 private:
  void publishTRPYCommand(void);
  void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd);
  void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);
  void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg);

  PIDControl controller_;
  ros::Publisher trpy_command_pub_;
  ros::Subscriber odom_sub_;
  ros::Subscriber position_cmd_sub_;
  ros::Subscriber enable_motors_sub_;

  bool position_cmd_updated_, position_cmd_init_;
  std::string frame_id_;

  Eigen::Vector3f des_pos_, des_vel_, des_acc_, kx_, kv_, ki_;
  float des_yaw_;
  float current_yaw_;
  float ki_yaw_;
  float max_roll_pitch_;
  bool enable_motors_;
  bool use_external_yaw_;
};

void PIDControlNodelet::publishTRPYCommand(void)
{
  float ki_yaw = 0;
  Eigen::Vector3f ki = Eigen::Vector3f::Zero();
  // Only enable integral terms when motors are on
  if(enable_motors_)
  {
    ki_yaw = ki_yaw_;
    ki = ki_;
  }
  controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, kx_, kv_, ki, ki_yaw);

  const Eigen::Vector4f &trpy = controller_.getControls();

  kr_mav_msgs::TRPYCommand::Ptr trpy_command(new kr_mav_msgs::TRPYCommand);
  trpy_command->header.stamp = ros::Time::now();
  trpy_command->header.frame_id = frame_id_;
  if(enable_motors_)
  {
    trpy_command->thrust = trpy(0);
    trpy_command->roll = CLAMP(trpy(1), -max_roll_pitch_, max_roll_pitch_);
    trpy_command->pitch = CLAMP(trpy(2), -max_roll_pitch_, max_roll_pitch_);
    trpy_command->yaw = trpy(3);
  }
  trpy_command->aux.current_yaw = current_yaw_;
  trpy_command->aux.enable_motors = enable_motors_;
  trpy_command->aux.use_external_yaw = use_external_yaw_;
  trpy_command_pub_.publish(trpy_command);
}

void PIDControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd)
{
  des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z);
  des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z);
  kx_ = Eigen::Vector3f(cmd->kx[0], cmd->kx[1], cmd->kx[2]);
  kv_ = Eigen::Vector3f(cmd->kv[0], cmd->kv[1], cmd->kv[2]);

  des_yaw_ = cmd->yaw;
  position_cmd_updated_ = true;
  // position_cmd_init_ = true;

  publishTRPYCommand();
}

void PIDControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
{
  const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);
  const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z);

  current_yaw_ = tf::getYaw(odom->pose.pose.orientation);

  controller_.setPosition(position);
  controller_.setVelocity(velocity);
  controller_.setYaw(current_yaw_);

  if(position_cmd_init_)
  {
    // We set position_cmd_updated_ = false and expect that the
    // position_cmd_callback would set it to true since typically a position_cmd
    // message would follow an odom message. If not, the position_cmd_callback
    // hasn't been called and we publish the so3 command ourselves
    // TODO: Fallback to hover if position_cmd hasn't been received for some time
    if(!position_cmd_updated_)
      publishTRPYCommand();
    position_cmd_updated_ = false;
  }
}

void PIDControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg)
{
  if(msg->data)
    ROS_INFO("Enabling motors");
  else
    ROS_INFO("Disabling motors");

  enable_motors_ = msg->data;

  // Reset integrals when toggling motor state
  controller_.resetIntegrals();
}

void PIDControlNodelet::onInit(void)
{
  ros::NodeHandle n(getPrivateNodeHandle());

  std::string quadrotor_name;
  n.param("quadrotor_name", quadrotor_name, std::string("quadrotor"));
  frame_id_ = "/" + quadrotor_name;

  double mass;
  n.param("mass", mass, 0.235);
  controller_.setMass(mass);
  controller_.setMaxIntegral(mass * 3);

  n.param("use_external_yaw", use_external_yaw_, true);

  double max_roll_pitch;
  n.param("max_roll_pitch", max_roll_pitch, 30.0);
  max_roll_pitch_ = max_roll_pitch;

  double ki_x, ki_y, ki_z, ki_yaw;
  n.param("gains/ki/x", ki_x, 0.0);
  n.param("gains/ki/y", ki_y, 0.0);
  n.param("gains/ki/z", ki_z, 0.0);
  n.param("gains/ki/yaw", ki_yaw, 0.01);
  ki_[0] = ki_x, ki_[1] = ki_y, ki_[2] = ki_z;
  ki_yaw_ = ki_yaw;

  trpy_command_pub_ = n.advertise<kr_mav_msgs::TRPYCommand>("trpy_cmd", 10);

  odom_sub_ = n.subscribe("odom", 10, &PIDControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay());
  position_cmd_sub_ = n.subscribe("position_cmd", 10, &PIDControlNodelet::position_cmd_callback, this,
                                  ros::TransportHints().tcpNoDelay());

  enable_motors_sub_ =
      n.subscribe("motors", 2, &PIDControlNodelet::enable_motors_callback, this, ros::TransportHints().tcpNoDelay());
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(PIDControlNodelet, nodelet::Nodelet);