/*******************************************************************************
 * reservoir/ams_select.hpp
 *
 * Approximate selection from sorted sequences (in this case, mostly B-trees)
 *
 * Copyright (C) 2019 Lorenz Hübschle-Schneider <lorenz@4z2.de>
 *
 * All rights reserved. Published under the GNU General Public License 3
 ******************************************************************************/

#pragma once
#ifndef RESERVOIR_AMS_SELECT_HEADER
#define RESERVOIR_AMS_SELECT_HEADER

#include <reservoir/aggregate.hpp>
#include <reservoir/logger.hpp>
#include <reservoir/select_helpers.hpp>
#include <reservoir/timer.hpp>
#include <reservoir/util.hpp>

#include <tlx/die/core.hpp>
#include <tlx/math/aggregate.hpp>

#include <boost/mpi.hpp>

#include <algorithm>
#include <functional>
#include <limits>
#include <ostream>
#include <random>
#include <utility>
#include <vector>

namespace mpi = boost::mpi;

namespace reservoir {

template <typename Seq>
class ams_select {
public:
    static constexpr const char *short_name = "[ams]";
    static const std::string name() {
        return "ams-select";
    }

    using Iterator = typename Seq::const_iterator;
    using Cmp = typename Seq::key_compare;
    using Key = typename Seq::key_type;
    using Elem = typename Seq::value_type;

    // pair of iterator and local rank
    using Result = std::pair<Iterator, ssize_t>;

    static constexpr bool debug = false;
    static constexpr bool check = false;
    static constexpr bool time = true;

    // Construct a selector using a given communicator and seed.  The seed must
    // be different on every PE (member of the communicator)!
    ams_select(mpi::communicator &comm, size_t seed)
        : comm_(comm), rng_(seed) {}

    // wrapper for kmin = kmax
    Result operator()(Seq &seq, size_t k) {
        return operator()(seq, k, k);
    }

    Result operator()(const Seq &seq, const size_t kmin, const size_t kmax) {
        timer total_timer;

        sLOGR << "Selecting between" << kmin << "and" << kmax << "with"
              << comm_.size() << "PEs";
        if (kmin > kmax || kmax == 0) {
            sLOGR << "aborting: kmin =" << kmin << "kmax =" << kmax;
            return std::make_pair(seq.begin(), 0);
        }
        if constexpr (check) {
            seq.verify();
            size_t size = seq.size();
            mpi::all_reduce(comm_, mpi::inplace(size), std::plus<>());
            sLOGR << "Checking size: want at least" << kmin << "have" << size;
            tlx_die_unless(kmin <= size);
        }

        // calculate global size
        size_t size = seq.size();
        mpi::all_reduce(comm_, mpi::inplace(size), std::plus<>());
        LOGR << "global size: " << size;
        // clang-format off
        tlx_die_verbose_unless(kmin <= size,
                               "Cannot select " << kmin << " to " << kmax
                               << " smallest out of " << size << " items; have "
                               << seq.size() << " at PE " << comm_.rank());
        // clang-format on

        auto res = select(seq, kmin, kmax, 0, seq.size(), size);

        if constexpr (check) {
            size_t result_size = res.second;
            mpi::all_reduce(comm_, mpi::inplace(result_size), std::plus<>());
            tlx_die_verbose_unless(kmin <= result_size && kmax >= result_size,
                                   "Expected between " << kmin << " and " << kmax
                                                       << " got " << result_size);
        }

        if constexpr (debug) {
            comm_.barrier();
            auto it = seq.begin();
            std::vector<Elem> vec;
            while (it != res.first) {
                vec.emplace_back(*it++);
            }
            size_t size = vec.size();
            pLOG << "local result " << size << " elements";
            // double-check output size
            mpi::all_reduce(comm_, mpi::inplace(size), std::plus<>());
            tlx_die_unless(size >= kmin && size <= kmax);
        }

        stats_.record_total(total_timer.get());
        stats_.reset_level();
        pLOGC(time && debug) << "stats: " << stats_;
        return res;
    }

    _detail::select_stats<time> &get_stats() {
        return stats_;
    }

protected:
    Result select(const Seq &seq, const ssize_t kmin, const ssize_t kmax,
                  const ssize_t min_idx, const ssize_t max_idx,
                  const ssize_t global_size) {
        stats_.next_level(); // debug timings
        if (comm_.rank() == 0)
            stats_.record_size(global_size);
        timer_.reset();

        tlx_die_verbose_unless(max_idx >= min_idx,
                               "Expected max_idx >= min_idx, got max_idx = "
                                   << max_idx << " min_idx = " << min_idx);
        tlx_die_unless(kmin <= kmax && kmin <= global_size);

        const ssize_t local_size = max_idx - min_idx;
        spLOG << "kmin =" << kmin << "kmax =" << kmax
              << "global_size =" << global_size << "local range:" << min_idx
              << max_idx << "size" << local_size;

        Key pivot;

        if (kmin == 1 || kmax == 1) {
            pivot = std::numeric_limits<Key>::max();
            if (local_size > 0) {
                pivot = get_key(seq.find_rank(min_idx));
            }
            pLOG << "Aborting at level " << stats_.level << " with kmin = " << kmin
                 << " kmax = " << kmax << " local minimum: " << pivot;
            mpi::all_reduce(comm_, mpi::inplace(pivot), mpi::minimum<Key>());

            auto [ub_pos, ub_it] = seq.rank_of_upper_bound(pivot);
            // slightly cheaty, normally we subtract min_idx from ub_pos
            if (static_cast<ssize_t>(ub_pos) < min_idx) {
                ub_pos = min_idx;
                ub_it = seq.find_rank(min_idx);
            }
            pLOG << "pivot = " << pivot << " pos " << ub_pos;

            stats_.record(timer_.get());
            return std::make_pair(ub_it, ub_pos);
        }

        if (kmin < global_size - kmax) {
            stats_.kcase.add(0);
            double p =
                1.0 - std::pow((kmin - 1.0) / kmax, 1.0 / (kmax - kmin + 1));
            sLOGR << "Case 1, p =" << p << "base" << (kmin - 1.0) / kmax
                  << "exponent" << 1.0 / (kmax - kmin + 1);
            tlx_die_unless(0 <= p && p <= 1);

            std::geometric_distribution<ssize_t> pidx_dist(p);
            ssize_t pivot_idx = pidx_dist(rng_);
            tlx_die_unless(pivot_idx >= 0);

            if (pivot_idx < local_size) {
                pivot = get_key(seq.find_rank(min_idx + pivot_idx));
            } else {
                pivot = std::numeric_limits<Key>::max();
                stats_.pidx_oob++;
            }
            spLOG << "chose pivot index" << pivot_idx << "value" << pivot
                  << "for local size" << local_size;
            // use smallest local pivot as global pivot
            mpi::all_reduce(comm_, mpi::inplace(pivot), mpi::minimum<Key>());
        } else {
            stats_.kcase.add(1);
            double p =
                1.0 - std::pow((global_size - kmax) / (global_size - kmin + 1.0),
                               1.0 / (kmax - kmin + 1));
            sLOGR << "Case 2, p =" << p << "base"
                  << (global_size - kmax) / (global_size - kmin + 1.0)
                  << "exponent" << 1.0 / (kmax - kmin + 1);
            tlx_die_unless(0 <= p && p <= 1);

            std::geometric_distribution<ssize_t> pidx_dist(p);
            ssize_t pivot_idx = pidx_dist(rng_);
            tlx_die_unless(pivot_idx >= 0);

            if (pivot_idx < local_size) {
                pivot = get_key(seq.find_rank(max_idx - pivot_idx - 1));
            } else {
                pLOG << "pivot idx " << pivot_idx << " OOB, >= " << local_size;
                pivot = std::numeric_limits<Key>::min();
                stats_.pidx_oob++;
            }
            spLOG << "chose pivot index" << pivot_idx << "value" << pivot
                  << "for local size" << local_size;
            // use largest local pivot as global pivot
            mpi::all_reduce(comm_, mpi::inplace(pivot), mpi::maximum<Key>());
        }
        LOGR << "pivot value = " << pivot;

        auto [ub_pos, lb_pos, ub_it, lb_it] = _detail::get_bounds(
            seq, stats_, pivot, min_idx, max_idx, comm_, short_name, debug);
        auto [global_ub, global_lb] =
            _detail::global_bound(ub_pos, lb_pos, global_size, comm_);

        sLOGRC(debug || global_ub > global_lb + 1)
            << "have" << global_lb << "smaller than," << global_ub
            << "leq to pivot of" << global_size << "want" << kmin << "to" << kmax;

        stats_.record(timer_.get());

        if (global_ub < kmin) {
            // recurse on elements larger than pivot
            stats_.right();
            sLOGR << "recursion: right; global_ub =" << global_ub
                  << "lb =" << global_lb << "; ub <" << kmin
                  << "= kmin for pivot =" << pivot;
            if (global_ub == 0)
                stats_.size_unchanged++;
            else if (global_ub * 50 <= global_size || global_ub <= 5)
                stats_.tinychange++;
            return select(seq, kmin - global_ub, kmax - global_ub,
                          min_idx + ub_pos, max_idx, global_size - global_ub);
        } else if (global_lb > kmax) {
            // recurse on elements smaller than pivot
            stats_.left();
            sLOGR << "recursion: left; global_ub =" << global_ub
                  << "lb = " << global_lb << "; lb > " << kmax
                  << "= kmax for pivot =" << pivot;
            if (global_lb == global_size)
                stats_.size_unchanged++;
            else if ((global_size - global_lb) * 50 <= global_size ||
                     (global_size - global_lb) <= 5)
                stats_.tinychange++;
            return select(seq, kmin, kmax, min_idx, min_idx + lb_pos, global_lb);
        } else {
            // Nearly done; result key is equal to pivot, but exact split is tbd
            return _detail::find_eq_pos(global_ub, ub_pos, ub_it, global_lb,
                                        lb_pos, lb_it, min_idx, kmin - global_lb,
                                        comm_, debug, short_name);
        }
    }

    constexpr Key get_key(const Iterator &it) {
        if constexpr (std::is_same_v<Key, Elem>) {
            return *it;
        } else {
            return it->first;
        }
    }

    mpi::communicator &comm_;
    std::mt19937_64 rng_;
    mutable _detail::select_stats<time> stats_;
    mutable timer timer_;
};

} // namespace reservoir

#endif // RESERVOIR_AMS_SELECT_HEADER


#include "teca_cartesian_mesh_subset.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_cartesian_mesh_util.h"

#include <algorithm>
#include <iostream>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;

//#define TECA_DEBUG

// --------------------------------------------------------------------------
teca_cartesian_mesh_subset::teca_cartesian_mesh_subset()
    : bounds({0l,0l,0l,0l,0l,0l}), cover_bounds(false)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_cartesian_mesh_subset::~teca_cartesian_mesh_subset()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_cartesian_mesh_subset::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for " + prefix + "(teca_cartesian_mesh_subset)");

    opts.add_options()
        TECA_POPTS_GET(vector<double>, prefix, bounds, "bounding box given by x0,x1,y0,y1,z0,z1")
        TECA_POPTS_GET(bool, prefix, cover_bounds, "(T)use smallest subset covering or (F)largest subset contained by bounds")
        ;

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_cartesian_mesh_subset::set_properties(
    const string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, vector<double>, prefix, bounds)
    TECA_POPTS_SET(opts, bool, prefix, cover_bounds)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_cartesian_mesh_subset::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_cf_reader::get_output_metadata" << endl;
#endif
    (void)port;

    teca_metadata coords;
    const_p_teca_variant_array x;
    const_p_teca_variant_array y;
    const_p_teca_variant_array z;

    if (input_md[0].get("coordinates", coords)
        || !(x = coords.get("x")) || !(y = coords.get("y"))
        || !(z = coords.get("z")))
    {
        TECA_ERROR("metadata has invalid coordinates")
        return teca_metadata();
    }

    vector<unsigned long> ext(6, 0l);

    TEMPLATE_DISPATCH_FP(
        const teca_variant_array_impl,
        x.get(),

        const NT *p_x = static_cast<TT*>(x.get())->get();
        const NT *p_y = static_cast<TT*>(y.get())->get();
        const NT *p_z = static_cast<TT*>(z.get())->get();

        if (bounds_to_extent(
            static_cast<NT>(this->bounds[0]), static_cast<NT>(this->bounds[1]),
            static_cast<NT>(this->bounds[2]), static_cast<NT>(this->bounds[3]),
            static_cast<NT>(this->bounds[4]), static_cast<NT>(this->bounds[5]),
            p_x, p_y, p_z, x->size()-1, y->size()-1, z->size()-1,
            cover_bounds, ext))
        {
            vector<double> actual(6, 0.0);

            x->get(0, actual[0]);
            x->get(x->size()-1, actual[1]);

            y->get(0, actual[2]);
            y->get(y->size()-1, actual[3]);

            z->get(0, actual[4]);
            z->get(z->size()-1, actual[5]);

            TECA_ERROR("requested bounds ["
                << this->bounds[0] << ", " << this->bounds[1] << ", "
                << this->bounds[2] << ", " << this->bounds[3] << ", "
                << this->bounds[4] << ", " << this->bounds[5]
                << "]  does not fall in the valid range ["
                << actual[0] << ", " << actual[1] << ", "
                << actual[2] << ", " << actual[3] << ", "
                << actual[4] << ", " << actual[5] << "]")

            return teca_metadata();
        }

        this->extent = ext;

        teca_metadata out_md(input_md[0]);
        out_md.insert("whole_extent", ext);
        return out_md;
        )

    TECA_ERROR("get_output_metadata failed")
    return teca_metadata();
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_cartesian_mesh_subset::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    (void)input_md;

    vector<teca_metadata> up_reqs(1, request);

    up_reqs[0].insert("extent", this->extent);

    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_cartesian_mesh_subset::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_cartesian_mesh_subset::execute" << endl;
#endif
    (void)port;
    (void)request;

    p_teca_cartesian_mesh in_target
        = std::dynamic_pointer_cast<teca_cartesian_mesh>(std::const_pointer_cast<teca_dataset>(input_data[0]));

    if (!in_target)
    {
        TECA_ERROR("invalid input dataset")
        return nullptr;
    }

    // pass input through via shallow copy
    p_teca_cartesian_mesh target = teca_cartesian_mesh::New();
    target->shallow_copy(in_target);

    return target;
}


/*
 *   This file is part of WARG's computer-vision
 *
 *  Copyright (c) 2015-2016, Waterloo Aerial Robotics Group (WARG)
 *  All rights reserved.
 *
 *  This software is licensed under a modified version of the BSD 3 clause license
 *  that should have been included with this software in a file called COPYING.txt
 *  Otherwise it is available at:
 *  https://raw.githubusercontent.com/UWARG/computer-vision/master/COPYING.txt
 */

#include <boost/log/trivial.hpp>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/timer.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <iostream>
#include "frame.h"
#include "pictureimport.h"

using namespace cv;
using namespace std;
using namespace boost;
using namespace filesystem;
namespace logging = boost::log;

double matrix[] = {
    2.4052826789763981e+03, 0, 2000,
    0, 2.4052826789763981e+03, 1500,
    0, 0, 1
};

double newmatrix[] = {
    2000, 0, 2000,
    0, 2000, 1500,
    0, 0, 1
};

double distortion[] = {
    -0.392, 0.146, 0, 0, -0.023
};

int scale;

int intDistortion[5];

Camera camera(
    Size(4000, 3000),
    Size2d(120, 90),
    Mat(
        Size(3, 3),
        CV_8UC1,
        matrix
    ),
    Mat(
        Size(5, 1),
        CV_8UC1,
        distortion
    ),
    Mat(
        Size(3, 3),
        CV_64F,
        newmatrix
    )

);

vector<Frame*> frames;
int fileIndex = 0;

void on_file_change(int, void*) {
    distortion[0] = (intDistortion[0] - 500)/1000.0;
    distortion[1] = (intDistortion[1] - 500)/1000.0;
    distortion[2] = (intDistortion[2] - 500)/1000.0;
    distortion[3] = (intDistortion[3] - 500)/1000.0;
    distortion[4] = (intDistortion[4] - 500)/1000.0;
    newmatrix[0] = (scale - 500)*10.0;
    newmatrix[4] = (scale - 500)*10.0;
    Camera camera(
        Size(4000, 3000),
        Size2d(120, 90),
        Mat(
            Size(3, 3),
            CV_64F,
            matrix
        ),
        Mat(
            Size(5, 1),
            CV_64F,
            distortion
        ),
        Mat(
            Size(3, 3),
            CV_64F,
            newmatrix
        )
    );
    Mat* img = frames[fileIndex]->undistort(camera);

    Mat dst;
    Size size(960,640);
    resize(*img, dst, size);

    imshow("image", dst);
    delete img;
}

int main(int argc, char ** argv) {

    logging::core::get()->set_filter(logging::trivial::severity >= logging::trivial::info);
    if (argc < 2) {
        BOOST_LOG_TRIVIAL(error) << "Invalid number of arguments";
    }

    PictureImport importer(argv[1], NULL, camera);
    Frame *current = NULL;
    while ((current = importer.next_frame()) != NULL) {
        frames.push_back(current);
    }

    on_file_change(0, 0);

    namedWindow("image", CV_WINDOW_NORMAL);
    createTrackbar("Image", "image", &fileIndex, frames.size() - 1, on_file_change);
    createTrackbar("k0", "image", &intDistortion[0], 1000, on_file_change);
    createTrackbar("k1", "image", &intDistortion[1], 1000, on_file_change);
    createTrackbar("k2", "image", &intDistortion[2], 1000, on_file_change);
    createTrackbar("k3", "image", &intDistortion[3], 1000, on_file_change);
    createTrackbar("k4", "image", &intDistortion[4], 1000, on_file_change);
    createTrackbar("scale", "image", &scale, 1000, on_file_change);

    while (true) {
        waitKey(0);
    }
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
namespace mp = boost::multiprecision;
using Booint = mp::cpp_int;
using namespace std;
int main() {
    Booint a;
    Booint b;
    Booint c;
    Booint d;
    cin >> a >> b >> c >> d;
    if ((a <= c && c <= b) || (c <= a && a <= d)) {
        cout << "Yes" << endl;
    }
    else cout << "No" << endl;
}


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include "mars_wrapper_pose.h"
#include "mars_msg_conv.h"

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <mars/core_logic.h>
#include <mars/core_state.h>
#include <mars/sensors/imu/imu_measurement_type.h>
#include <mars/sensors/imu/imu_sensor_class.h>
#include <mars/sensors/pose/pose_measurement_type.h>
#include <mars/type_definitions/buffer_data_type.h>
#include <mars/type_definitions/buffer_entry_type.h>
#include <mars_msg_conv.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Dense>
#include <iostream>
#include <string>

using namespace mars;

MarsWrapperPose::MarsWrapperPose(ros::NodeHandle nh)
  : reconfigure_cb_(boost::bind(&MarsWrapperPose::configCallback, this, _1, _2))
  , p_wi_init_(0, 0, 0)
  , q_wi_init_(Eigen::Quaterniond::Identity())
{
  reconfigure_srv_.setCallback(reconfigure_cb_);

  initialization_service_ = nh.advertiseService("init_service", &MarsWrapperPose::initServiceCallback, this);

  // Read parameter
  publish_on_propagation_ = nh.param<bool>("pub_on_prop", publish_on_propagation_);
  use_ros_time_now_ = nh.param<bool>("use_ros_time_now", use_ros_time_now_);
  verbose_output_ = nh.param<bool>("verbose", verbose_output_);
  verbose_ooo_ = nh.param<bool>("verbose_out_of_order", verbose_ooo_);
  discard_ooo_prop_meas_ = nh.param<bool>("discard_ooo_prop_meas", discard_ooo_prop_meas_);

  pub_cb_buffer_size_ = uint32_t(nh.param<int>("pub_cb_buffer_size", int(pub_cb_buffer_size_)));
  sub_imu_cb_buffer_size_ = uint32_t(nh.param<int>("sub_imu_cb_buffer_size", int(sub_imu_cb_buffer_size_)));
  sub_sensor_cb_buffer_size_ = uint32_t(nh.param<int>("sub_sensor_cb_buffer_size", int(sub_sensor_cb_buffer_size_)));

  std::cout << "Setup framework components" << std::endl;

  // Framework components
  imu_sensor_sptr_ = std::make_shared<mars::ImuSensorClass>("IMU");
  core_states_sptr_ = std::make_shared<mars::CoreState>();
  core_states_sptr_.get()->set_propagation_sensor(imu_sensor_sptr_);
  core_logic_ = mars::CoreLogic(core_states_sptr_);
  core_logic_.buffer_.set_max_buffer_size(800);

  core_logic_.verbose_ = verbose_output_;
  core_logic_.verbose_out_of_order_ = verbose_ooo_;
  core_logic_.discard_ooo_prop_meas_ = discard_ooo_prop_meas_;

  core_states_sptr_->set_noise_std(Eigen::Vector3d(0.013, 0.013, 0.013), Eigen::Vector3d(0.0013, 0.0013, 0.0013),
                                   Eigen::Vector3d(0.083, 0.083, 0.083), Eigen::Vector3d(0.0083, 0.0083, 0.0083));

  // Sensors
  pose1_sensor_sptr_ = std::make_shared<mars::PoseSensorClass>("Pose1", core_states_sptr_);
  Eigen::Matrix<double, 6, 1> pose_meas_std;
  pose_meas_std << 0.05, 0.05, 0.05, 3 * (M_PI / 180), 3 * (M_PI / 180), 3 * (M_PI / 180);
  pose1_sensor_sptr_->R_ = pose_meas_std.cwiseProduct(pose_meas_std);

  PoseSensorData pose_calibration;
  pose_calibration.state_.p_ip_ = Eigen::Vector3d(0.0, 0.0, 0.0);
  pose_calibration.state_.q_ip_ = Eigen::Quaterniond::Identity();
  Eigen::Matrix<double, 6, 6> pose_cov;
  pose_cov.setZero();
  pose_cov.diagonal() << 0.0025, 0.0025, 0.0025, 0.0076, 0.0076, 0.0076;  // 5cm, 5deg
  pose_calibration.sensor_cov_ = pose_cov;
  pose1_sensor_sptr_->set_initial_calib(std::make_shared<PoseSensorData>(pose_calibration));

  // TODO is set here for now, but will be managed by core logic in later versions
  pose1_sensor_sptr_->const_ref_to_nav_ = true;

  // Subscriber
  sub_imu_measurement_ =
      nh.subscribe("imu_in", sub_imu_cb_buffer_size_, &MarsWrapperPose::ImuMeasurementCallback, this);
  sub_pose_measurement_ =
      nh.subscribe("pose_in", sub_sensor_cb_buffer_size_, &MarsWrapperPose::PoseMeasurementCallback, this);
  sub_pose_with_cov_measurement_ = nh.subscribe("pose_with_cov_in", sub_sensor_cb_buffer_size_,
                                                &MarsWrapperPose::PoseWithCovMeasurementCallback, this);
  sub_odom_measurement_ =
      nh.subscribe("odom_in", sub_sensor_cb_buffer_size_, &MarsWrapperPose::OdomMeasurementCallback, this);
  sub_transform_measurement_ =
      nh.subscribe("transform_in", sub_sensor_cb_buffer_size_, &MarsWrapperPose::TransformMeasurementCallback, this);

  // Publisher
  pub_ext_core_state_ = nh.advertise<mars_ros::ExtCoreState>("core_ext_state_out", pub_cb_buffer_size_);
  pub_ext_core_state_lite_ = nh.advertise<mars_ros::ExtCoreStateLite>("core_ext_state_lite_out", pub_cb_buffer_size_);
  pub_core_pose_state_ = nh.advertise<geometry_msgs::PoseStamped>("core_pose_state_out", pub_cb_buffer_size_);
  pub_pose1_state_ = nh.advertise<geometry_msgs::PoseStamped>("pose_cal_state_out", pub_cb_buffer_size_);
}

bool MarsWrapperPose::init()
{
  core_logic_.core_is_initialized_ = false;
  core_logic_.buffer_.ResetBufferData();
  pose1_sensor_sptr_->is_initialized_ = false;

  return true;
}

bool MarsWrapperPose::initServiceCallback(std_srvs::SetBool::Request& /*request*/,
                                          std_srvs::SetBool::Response& res)
{
  init();
  res.success = true;
  ROS_INFO_STREAM("Initialized filter trough ROS Service");

  return true;
}

void MarsWrapperPose::configCallback(mars_ros::marsConfig& config, uint32_t /*level*/)
{
  // Config parameter overwrite
  publish_on_propagation_ = config.pub_on_prop;
  core_logic_.verbose_ = config.verbose;
  verbose_output_ = config.verbose;
  use_ros_time_now_ = config.use_ros_time_now;

  if (config.initialize)
  {
    init();
    ROS_INFO_STREAM("Initialized filter trough Reconfigure GUI");
  }

  config.initialize = false;
}

void MarsWrapperPose::ImuMeasurementCallback(const sensor_msgs::ImuConstPtr& meas)
{
  // Map the measutement to the mars type
  Time timestamp;

  if (use_ros_time_now_)
  {
    timestamp = Time(ros::Time::now().toSec());
  }
  else
  {
    timestamp = Time(meas->header.stamp.toSec());
  }

  // Generate a measurement data block
  BufferDataType data;
  data.set_sensor_data(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));

  // Call process measurement
  core_logic_.ProcessMeasurement(imu_sensor_sptr_, timestamp, data);

  // Initialize the first time at which the propagation sensor occures
  if (!core_logic_.core_is_initialized_)
  {
    core_logic_.Initialize(p_wi_init_, q_wi_init_);
  }

  if (publish_on_propagation_)
  {
    this->RunCoreStatePublisher();
  }
}

void MarsWrapperPose::PoseMeasurementCallback(const geometry_msgs::PoseStampedConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PoseMeasurementType pose_meas = MarsMsgConv::PoseMsgToPoseMeas(*meas);
  PoseMeasurementUpdate(pose1_sensor_sptr_, pose_meas, timestamp);
}

void MarsWrapperPose::PoseWithCovMeasurementCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PoseMeasurementType pose_meas = MarsMsgConv::PoseWithCovMsgToPoseMeas(*meas);
  PoseMeasurementUpdate(pose1_sensor_sptr_, pose_meas, timestamp);
}

void MarsWrapperPose::TransformMeasurementCallback(const geometry_msgs::TransformStampedConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PoseMeasurementType pose_meas = MarsMsgConv::TransformMsgToPoseMeas(*meas);
  PoseMeasurementUpdate(pose1_sensor_sptr_, pose_meas, timestamp);
}

void MarsWrapperPose::OdomMeasurementCallback(const nav_msgs::OdometryConstPtr& meas)
{
  // Map the measurement to the mars sensor type
  Time timestamp(meas->header.stamp.toSec());

  PoseMeasurementType pose_meas = MarsMsgConv::OdomMsgToPoseMeas(*meas);
  PoseMeasurementUpdate(pose1_sensor_sptr_, pose_meas, timestamp);
}

void MarsWrapperPose::RunCoreStatePublisher()
{
  mars::BufferEntryType latest_state;
  core_logic_.buffer_.get_latest_state(&latest_state);
  mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;

  pub_ext_core_state_.publish(MarsMsgConv::ExtCoreStateToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
  pub_ext_core_state_lite_.publish(
      MarsMsgConv::ExtCoreStateLiteToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));

  pub_core_pose_state_.publish(
      MarsMsgConv::ExtCoreStateToPoseMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
}

void MarsWrapperPose::PoseMeasurementUpdate(std::shared_ptr<mars::PoseSensorClass> sensor_sptr,
                                            const PoseMeasurementType& pose_meas, const Time& timestamp)
{
  Time timestamp_corr;

  if (use_ros_time_now_)
  {
    timestamp_corr = Time(ros::Time::now().toSec());
  }
  else
  {
    timestamp_corr = timestamp;
  }

  // TMP feedback init pose
  p_wi_init_ = pose_meas.position_;
  q_wi_init_ = pose_meas.orientation_;

  // Generate a measurement data block
  BufferDataType data;
  data.set_sensor_data(std::make_shared<PoseMeasurementType>(pose_meas));

  // Call process measurement
  if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp_corr, data))
  {
    return;
  }

  // Publish the latest core state
  this->RunCoreStatePublisher();

  // Publish the latest sensor state
  mars::BufferEntryType latest_result;
  core_logic_.buffer_.get_latest_sensor_handle_state(sensor_sptr, &latest_result);
  mars::PoseSensorStateType pose_sensor_state = sensor_sptr.get()->get_state(latest_result.data_.sensor_);

  pub_pose1_state_.publish(MarsMsgConv::PoseStateToPoseMsg(latest_result.timestamp_.get_seconds(), pose_sensor_state));
}


/*
 * Modified by MartynP <git@disputedip.com>
 *
 * The original header:
 *
 * This is a stripped-down and partially modified version of Boost's CRC library.
 * I extracted the crc_basic class and removed the template parameter, because I
 * want to specify the original template parameter at run-time.
 *
 * Martin Schobert <schobert@sitsec.net>
 *
 * The original header:
 *
 * Boost CRC library crc.hpp header file  -----------------------------------//
 *
 * Copyright 2001, 2004 Daryle Walker.  Use, modification, and distribution are
 * subject to the Boost Software License, Version 1.0.  (See accompanying file
 * LICENSE_1_0.txt or a copy at <http://www.boost.org/LICENSE_1_0.txt>.)
 *
 *  See <http://www.boost.org/libs/crc/> for the library's home page.
 *
 */

#ifndef CRC_HPP
#define CRC_HPP

#include <boost/crc.hpp>

#define MAX_VALUE(width) ((1 << width) - 1)

//  Simple cyclic redundancy code (CRC) class declaration  -------------------//

class my_crc_basic {

public:

  // The FEED_TYPE deines how bits are fed into the CRC
  enum FEED_TYPE {
    AUTO = 0,
    LINEAR_FORWARD = 1,
    LINEAR_REVERSED = 2,
    BYTEWISE_REVERSED = 3

  };

  typedef uint32_t value_type;

  // Constructor
  explicit my_crc_basic(unsigned int width,
			value_type truncated_polynominal = 0,
			value_type initial_remainder = 0,
			value_type final_xor_value = 0,
			bool reflect_input = false,
			bool reflect_remainder = false );

  void set( value_type truncated_polynominal,
	    value_type initial_remainder = 0,
	    value_type final_xor_value = 0,
	    bool reflect_input = false,
	    bool reflect_remainder = false);

  void reset( value_type new_rem );
  void process_bit( bool bit );
  value_type checksum() const;

  bool calc_crc(value_type const use_initial,
		boost::dynamic_bitset<> const& msg,
		value_type const expected_crc, FEED_TYPE feed_type = AUTO);

  void calc_crc(value_type const use_initia,
		boost::dynamic_bitset<> const& msg);

  bool calc_crc(value_type const use_initial,
		uint8_t msg[], size_t msg_length,
		value_type const expected_crc);

  value_type reflect(value_type  x ) const;

private:
  // Member data
  unsigned int width_;
  value_type rem_;
  value_type poly_, init_, final_;  // non-const to allow assignability
  bool rft_in_, rft_out_;     // non-const to allow assignability

  uint32_t high_bit_mask;;
  uint32_t sig_bits;

};

#endif


#include <ctime>
#include <chrono>
#include <boost/filesystem.hpp>
#include <boost/filesystem.hpp>
#include <boost/range/iterator_range.hpp>
#include "helpers.h"
#include "cylinder_fitting_hough.h"

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::search::KdTree<PointT>::Ptr tree;

int main (int argc, char** argv)
{
	// Arguments
	std::string pointcloud_clusters_path=std::string(argv[1]);

	// HOUGH PARAMETERS
	unsigned int angle_bins=atoi(argv[2]);
	std::cout << "angle_bins: " << angle_bins<< std::endl;

	unsigned int radius_bins=atoi(argv[3]);
	std::cout << "radius_bins: " << radius_bins<< std::endl;

	unsigned int position_bins=atoi(argv[4]);
	std::cout << "position_bins: " << position_bins<< std::endl;

	// OUR HOUGH IMPLEMENTATION PARAMETERS
	unsigned int orientation_accumulators_num=atoi(argv[5]);
	std::cout << "orientation_accumulators_num: " << orientation_accumulators_num<< std::endl;

	unsigned int gaussian_sphere_points_num=atoi(argv[6]);
	std::cout << "gaussian_sphere_points_num: " << gaussian_sphere_points_num<< std::endl;

	float accumulator_peak_threshold=atof(argv[7]);
	std::cout << "accumulator_peak_threshold: " << accumulator_peak_threshold<< std::endl;

	float min_radius=atof(argv[8]);
	std::cout << "min_radius: " << min_radius << std::endl;

	float max_radius=atof(argv[9]);
	std::cout << "max_radius: " << max_radius << std::endl;

	int mode=atof(argv[10]);
	std::cout << "mode: " << mode << std::endl;

	bool soft_voting=atoi(argv[11]);
	std::cout << "soft_voting: " << soft_voting << std::endl;

	bool visualize=atoi(argv[12]);
	std::cout << "visualize: " << visualize << std::endl;

	std::vector<std::string> image_files,annotation_files,point_cloud_files;

	boost::shared_ptr<VisualizeFittingData> visualizer;

	if(visualize)
		visualizer=boost::shared_ptr<VisualizeFittingData>(new VisualizeFittingData());

	// Gaussian Sphere Uniform
	std::vector<double> weights;
	std::vector<Eigen::Matrix<double, 3 ,1> > means;
	std::vector<Eigen::Matrix<double, 3 ,1> > std_devs;
	weights.push_back(1.0);
	Eigen::Matrix<double, 3 ,1> mean_eigen(0,0,0);
	means.push_back(mean_eigen);
	Eigen::Matrix<double, 3 ,1> std_dev_eigen(0.5,0.5,0.5);
	std_devs.push_back(std_dev_eigen);

	GaussianMixtureModel gmm(weights,means,std_devs);
	GaussianSphere gaussian_sphere(gmm,gaussian_sphere_points_num,orientation_accumulators_num);

	boost::shared_ptr<CylinderFittingHough> cylinder_fitting(new CylinderFittingHough(gaussian_sphere,
	(unsigned int)angle_bins,
	(unsigned int)radius_bins,
	(unsigned int)position_bins,
	(float)min_radius,
	(float)max_radius,
	(float)accumulator_peak_threshold,
	mode,
	false,
	soft_voting));

	unsigned int current_dir=0;
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

	std::fstream cluster_fitting_score;

	// For each directory
	for (boost::filesystem::directory_iterator itr(pointcloud_clusters_path); itr!=boost::filesystem::directory_iterator(); ++itr)
	{
		std::cout << "directory number:" << ++current_dir << std::endl;

		std::string pointcloud_clusters_sub_path	    = pointcloud_clusters_path+itr->path().filename().string()+"/";
		PointClouds point_clouds(pointcloud_clusters_sub_path);
		std::string results_path_current=pointcloud_clusters_sub_path+"results/";

		createDirectory(results_path_current);
		cluster_fitting_score.open(results_path_current+"cluster_fitting_score.txt", std::fstream::in | std::fstream::out | std::fstream::trunc);
		for(unsigned int i=0;i<point_clouds.file_names.size();++i)
		{
			try
			{
				pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
				pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>());

				// Load 3D point cloud information in PCL format
				point_cloud=point_clouds.loadPointCloud(pointcloud_clusters_sub_path+point_clouds.file_names[i]);

				// Normal estimation parameter (not organized)
				ne.setKSearch (6);
				ne.setSearchMethod(tree);
				ne.setInputCloud(point_cloud);
				ne.compute(*cloud_normals);
				FittingData fitting_data=cylinder_fitting->fit(point_cloud,cloud_normals);

				cluster_fitting_score << fitting_data.confidence << std::endl;

				/* VISUALIZE */
				if(visualize)
					fitting_data.visualize(point_cloud,visualizer);
				/* END VISUALIZE */

			}
			catch (std::exception& e)
			{
				std::cout << e.what() << std::endl;
				continue;
			}
		}
		cluster_fitting_score.close();
	}
	std::cout << "end" << std::endl;
	return 0;
}


// Copyright 2014-2015 SDL plc
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//     http://www.apache.org/licenses/LICENSE-2.0
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#define SDL_TRANSFORM_MAIN_LOG_WEIGHT 1
#define SDL_TRANSFORM_MAIN_EXPECTATION_WEIGHT 1

#ifndef NDEBUG
#include <sdl/Hypergraph/HypergraphWriter.hpp>
#endif
#include <sdl/Hypergraph/AllPairsShortestDistance.hpp>
#include <sdl/Hypergraph/Arc.hpp>
#include <sdl/Hypergraph/ArcParserFct.hpp>
#include <sdl/Hypergraph/ExpectationWeight.hpp>
#include <sdl/Hypergraph/FeatureWeight.hpp>
#include <sdl/Hypergraph/HypergraphMain.hpp>
#include <sdl/Hypergraph/IHypergraph.hpp>
#include <sdl/Hypergraph/InsideAlgorithm.hpp>
#include <sdl/Hypergraph/MapHypergraph.hpp>
#include <sdl/Hypergraph/MutableHypergraph.hpp>
#include <sdl/Hypergraph/NgramWeightMapper.hpp>
#include <sdl/Hypergraph/SortStates.hpp>
#include <sdl/Hypergraph/Weight.hpp>
#include <sdl/Vocabulary/HelperFunctions.hpp>
#include <sdl/Util/Input.hpp>
#include <sdl/Util/Matrix.hpp>
#include <sdl/Util/ProgramOptions.hpp>
#include <sdl/IVocabulary-fwd.hpp>
#include <sdl/SharedPtr.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

namespace sdl {
namespace Hypergraph {

template <class Arc>
void printDistances(IHypergraph<Arc> const& hg, bool allPairs, bool dag, StateIdTranslation& stateRemap,
                    StateId partBoundary) {
  typedef typename Arc::Weight Weight;
  if (allPairs) {
    // TODO: test
    assert(partBoundary != kNoState);
    StateId n = hg.size();
    assert(partBoundary <= n);
    Util::Matrix<Weight> D(partBoundary, partBoundary, Weight::zero());
    if (dag) {
      AllPairsSortedDag<IHypergraph<Arc>> compute(hg, D);
      for (StateId i = 0; i < n; ++i) {  // state in input
        StateId topi = stateRemap.existingState(i);  // actual state in hg, modified by topo sort
        if (topi == kNoState || topi >= partBoundary) continue;
        for (StateId j = 0; j < n; ++j) {
          if (i == j) continue;
          StateId topj = stateRemap.existingState(j);
          if (topj == kNoState || topj >= partBoundary) continue;
          Weight const& wij = D(topi, topj);
          if (!isZero(wij)) {
            std::cout << i << " -> " << j << " = " << wij << "\n";
          }
        }
      }
    } else {
      floydWarshall(hg, &D);
      for (StateId i = 0; i < n; ++i)
        for (StateId j = 0; j < n; ++j) {
          if (i == j) continue;
          Weight const& wij = D(i, j);
          if (!isZero(wij)) std::cout << i << " -> " << j << " = " << wij << "\n";
        }
    }
  } else {
    boost::ptr_vector<Weight> weights;
    insideAlgorithm(hg, &weights);
    std::size_t i = 0;
    for (Weight w : weights) {
      std::cout << i++ << '\t' << w << '\n';
    }
  }
}

template <class Arc>
void process(std::string const& file, unsigned ngramMax = 0, bool allPairs = false, bool dag = false) {
  std::cerr << "file=" << file << " ngramMax=" << ngramMax << "\n";
  Util::Input in(file);
  MutableHypergraph<Arc> hg(kDefaultProperties
                            | (allPairs ? (kStoreFirstTailOutArcs | (dag ? kStoreInArcs : 0)) : kStoreInArcs));

  IVocabularyPtr pVoc = Vocabulary::createDefaultVocab();
  hg.setVocabulary(pVoc);

  parseText(*in, file, &hg);

  if (allPairs)
    if (!hg.isFsm()) throw std::runtime_error("hg is not an fsm - can't show all-pairs");

  SortStates<Arc> sort;
  sort.partBoundary = hg.size();
  if (allPairs && dag) {
    sort.clearRemap = false;
    sort.inplace(hg);
  }

  typedef typename Arc::Weight Weight;
  if (ngramMax) {
    // TODO: test
    typedef NgramWeightMapper<Arc> Mapper;
    typedef typename Mapper::TargetArc TargetArc;
    MapHypergraph<Arc, TargetArc, Mapper> ngramhg(hg, Mapper(hg, ngramMax));
    printDistances(ngramhg, allPairs, dag, sort.stateRemap, sort.partBoundary);
  } else
    printDistances(hg, allPairs, dag, sort.stateRemap, sort.partBoundary);
}

struct HypInside {
  static int run_main(int argc, char** argv) {
    namespace po = boost::program_options;
    using namespace std;
    try {
      bool help = false;
      unsigned ngramMax = 0;
      bool allPairs = false, dag = false;
      string file = "-", arcType = "log";
      po::options_description generic("Allowed options");
      sdl::AddOption opt(generic);
      opt("arc-type,a", po::value(&arcType)->default_value("log"),
          "arc type (e.g., log, viterbi, expectation, feature)");
      opt("config-file,c", po::value<string>(), "config file name");
      opt("ngram-max,n", po::value(&ngramMax)->default_value(0),
          "if >0, show NgramWeight<arc-type> up to this #of words, starting from arc symbol unigrams");
      opt("help,h", po::bool_switch(&help), "produce help message");
      opt("input-file,i", po::value(&file)->default_value("-"), "input file; - is stdin");
      opt("all-pairs,p", po::bool_switch(&allPairs),
          "output all-pairs shortest paths (s -> d = w) - fsm only");
      opt("dag,d", po::bool_switch(&dag), "for all-pairs, assume input is a DAG (no cycles)");

      po::options_description cmdline_options;
      cmdline_options.add(generic);

      po::options_description config_file_options;
      config_file_options.add(generic);

      po::positional_options_description p;
      p.add("input-file", -1);

      po::variables_map vm;
      store(po::command_line_parser(argc, argv).options(cmdline_options).positional(p).run(), vm);

      if (vm.count("config-file")) {
        Util::Input ifs(vm["config-file"].as<std::string>());
        store(parse_config_file(*ifs, config_file_options), vm);
      }
      notify(vm);

      if (help) {
        std::cout
            << "Run inside algorithm on hypergraph and output inside weight for each "
            << "state reachable from the FINAL state. if all-pairs, output source -> destination = weight"
            << '\n';
        std::cout << generic << "\n";
        return EXIT_FAILURE;
      }

      typedef ViterbiWeightTpl<float> Viterbi;
      if (arcType == "log") {
        typedef ArcTpl<LogWeightTpl<float>> Arc;
        process<Arc>(file, ngramMax, allPairs, dag);
        // TODO: test all but log
      } else if (arcType == "viterbi") {
        typedef ArcTpl<Viterbi> Arc;
        process<Arc>(file, ngramMax, allPairs, dag);
      } else if (arcType == "expectation") {
        typedef ArcTpl<ExpectationWeight> Arc;
        process<Arc>(file, ngramMax, allPairs, dag);
      } else if (arcType == "feature") {
        typedef ArcTpl<FeatureWeight> Arc;
        process<Arc>(file, ngramMax, allPairs, dag);
      } else {
        std::cerr << "unknown arc-type " << arcType << "\n";
        return EXIT_FAILURE;
      }
    } catch (std::exception& e) {
      std::cerr << e.what() << '\n';
      return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
  }
};
}
}

HYPERGRAPH_NAMED_MAIN(Inside)


/*
 * Author: ignacio.martin@imdea.org
 *
 * Copyright 2014 IMDEA Materials Institute, Getafe, Madrid, Spain
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "SaveCmd.h"
#include "kernel/Domain.h"
#include "kernel/Mesh.h"
#include "kernel/Coordinates.h"
#include "domains/SimData.h"
#include "okmc/Particle.h"
#include "lkmc/LatticeSite.h"
#include "lkmc/LatticeParam.h"
#include "domains/MCClient.h"
#include "io/ParameterManager.h"
#include "io/VtkWriter.h"
#include <string>
#include <fstream>
#include <sstream>
#include <map>

#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

using std::string;
using std::endl;
using std::vector;
using std::pair;
using Kernel::Coordinates;

namespace IO {

namespace ublas = boost::numeric::ublas;

SaveCmd::SaveCmd(Tcl_Interp *p, int argc, const char *argv[]) : Command(p, argc, argv)
{
}

int SaveCmd::operator()()
{
	bool bDefects = specified("lkmc.defect");
	vector<string> defects;
	if(specified("defects"))
		defects = getStrings("defects", 1);

	if(specified("xyz"))
	{
		string filename = getString("xyz");
		filename+=".xyz";
		save(filename, XYZ_TYPE, bDefects, defects);
	}
	if(specified("lammps")) {
		string filename = getString("lammps");
		filename+=".dump";
		save(filename, LAMMPS_TYPE, bDefects,defects);
	}
	if(specified("atomeye")) {
		string filename = getString("atomeye");
		filename+=".cfg";
		save(filename, ATOMEYE_TYPE, bDefects, defects);
	}
	if(specified("csv"))
	{
		string filename = getString("csv");
		filename+=".csv";
		save(filename, CSV_TYPE, bDefects, defects);
	}
    if(specified("vtk"))
    {
        string filename = getString("vtk");
        filename+=".vti";
        save(filename, VTK_TYPE, bDefects, defects);
    }
	return TCL_OK;
}

// Be careful, experimental feature!
void SaveCmd::writeAtomeye(std::ofstream &ofs, std::vector<Domains::ParticleData> &part) const {
	Coordinates m, M;
	Domains::global()->client()->getCell(m, M);

	// Note about scale: there is no reason to use the parameter issued by the user
	double scale = 10.; // nm -> A

	ublas::matrix<double> H    = ublas::zero_matrix<double>(3,3);
	ublas::matrix<double> Hinv = ublas::zero_matrix<double>(3,3);

	H(0,0) = (M._y - m._y) * scale;
	H(0,1) = 0.;
	H(0,2) = 0.;

	H(1,0) = 0.;
	H(1,1) = (M._x - m._x) * scale;
	H(1,2) = 0.;

	H(2,0) = 0.;
	H(2,1) = 0.;
	H(2,2) = (M._z - m._z) * scale;

	// Compute invert matrix
	ublas::permutation_matrix<std::size_t> pm(H.size1());

	int res = lu_factorize(H,pm);
	if( res != 0 ) {
		WARNINGMSG("SaveCmd::writeAtomeye: aborting save command because of a problem during LU factorization!");
		return ;
	}
	Hinv.assign(ublas::identity_matrix<double>(H.size1()));
	lu_substitute(H, pm, Hinv);

	ofs << "Number of particles = " << part.size() << std::endl;
	ofs << "A = 1. Angstrom (basic length-scale)" << std::endl;

	for (unsigned i = 0; i < 3; ++i) {
		for (unsigned j = 0; j < 3; ++j) {
		  ofs << "H0(" << i + 1 << "," << j + 1 << ") = " <<  H(i,j) << " A" << std::endl;
		}
	}

	ofs << "Transform(1,1) = 1" << std::endl;
	ofs << "Transform(1,2) = 0" << std::endl;
	ofs << "Transform(1,3) = 0" << std::endl;
	ofs << "Transform(2,1) = 0" << std::endl;
	ofs << "Transform(2,2) = 1" << std::endl;
	ofs << "Transform(2,3) = 0" << std::endl;
	ofs << "Transform(3,1) = 0" << std::endl;
	ofs << "Transform(3,2) = 0" << std::endl;
	ofs << "Transform(3,3) = 1" << std::endl;

	for(vector<Domains::ParticleData>::iterator it = part.begin(); it != part.end(); ++it) {
		ublas::vector<double> x = ublas::zero_vector<double>(3);
		ublas::vector<double> s;

		x(0) = it->get<1>()._y * scale;
		x(1) = it->get<1>()._x * scale;
		x(2) = it->get<1>()._z * scale;

		s = ublas::prod(Hinv, x);

		// Only Si export supported for the moment
		// need to store atomic mass (in a.m.u) in config to support other species
		ofs << "28.0855 Si " << s(0) << " " << s(1) << " " << s(2) <<  " 0 0 0 " << std::endl;
	}
}

void SaveCmd::save(const string &filename, FORMAT_TYPES ft, bool bDefects, const vector<string> &defects) const
{
	Coordinates m, M;
	Domains::global()->client()->getCell(m, M);

	vector<Domains::ParticleData> data;
    if(specified("lattice"))
		for(Domains::MeshElementIterator it= Domains::global()->beginMEI(); it!=Domains::global()->endMEI(); ++it)
		{
			LKMC::LatticeSite *part = it->getFirstLS();
			if(part)
			{
				while(part)
				{
					std::string name = Domains::global()->PM()->getParticleName(it->getMaterial(), part->getPType());
					data.push_back(Domains::ParticleData(it->getMaterial(), part->getCoordinates(), part->getPType(), Kernel::Event::LATTICEATOM, name));
					part = part->getNext();
				}
			}
			else // OKMC Case (build the whole lattice)
			{
				Kernel::M_TYPE mt = it->getMaterial();
				if(Domains::global()->PM()->getMaterialName(mt) == "Gas")
					continue;
				Coordinates m, M;
				it->getCorners(m, M);
				vector<LKMC::Lattice::LatticeInformation> atoms;
				it->getDomain()->_pLat[mt]->fill(m, M, atoms, false);
				float x = it->getAlloyFraction();
				for(vector<LKMC::Lattice::LatticeInformation>::iterator it = atoms.begin(); it!=atoms.end();++it)
				{
					if(Domains::global()->client()->rand() < x)
					{
						std::string name = Domains::global()->PM()->getAlloyName(mt);
						data.push_back(Domains::ParticleData(mt, it->_coords, 1, Kernel::Event::LATTICEATOM, name));
					}
					else
					{
						std::string name = Domains::global()->PM()->getParticleName(mt, it->_type);
						data.push_back(Domains::ParticleData(mt, it->_coords, 0, Kernel::Event::LATTICEATOM, name));
					}
				}
			}
		}
	else
	{
		Domains::global()->data()->getLKMCInterface(data, bDefects);
    	Domains::global()->data()->getOKMCParticles(data, defects);
	}

	std::stringstream ff;
	ff << filename;
	if(!specified("no.print"))
		LOWMSG2("Writing " << data.size() << " atoms in "<< ff.str());
	std::ios_base::openmode mode = std::ios_base::out;
	if(specified("append"))
		mode |= std::ios_base::app;
	std::ofstream ofs(ff.str().c_str(), mode);

	float scale = specified("scale") ? getFloat("scale") : 1.;

	if (ft == LAMMPS_TYPE) {
		m *= scale;
		M *= scale;
		ofs << "ITEM: TIMESTEP\n";
		ofs << Domains::global()->getTime() << std::endl;
		ofs << "ITEM: NUMBER OF ATOMS\n";
		ofs << data.size() << std::endl;
		ofs << "ITEM: BOX BOUNDS\n";
		ofs << m._x << "\t" << M._x << std::endl;
		ofs << m._y << "\t" << M._y << std::endl;
		ofs << m._z << "\t" << M._z << std::endl;
		ofs << "ITEM: ATOMS id type x y z defect iddef\n";
    }
    else if (ft == ATOMEYE_TYPE) {
		writeAtomeye(ofs, data);
	}
    else if (ft == XYZ_TYPE) {
		if(data.size())
		{
			ofs << data.size() << endl;
			ofs << "type x y z def ID defectID" << endl;
		}
		else
			WARNINGMSG("Not writing 0 atoms in " << ff.str());
	}
    else if (ft == VTK_TYPE) {
        VtkWriter::VtkFormat format = VtkWriter::BINARY;
        if (specified("ascii")) format = VtkWriter::ASCII;

        Kernel::Mesh *pme = Domains::global()->getDomain(0)->_pMesh;

        const std::vector<float> &xl = pme->getLines(0);
        const std::vector<float> &yl = pme->getLines(1);
        const std::vector<float> &zl = pme->getLines(2);

        double dx = xl[1]-xl[0];
        double dy = yl[1]-yl[0];
        double dz = zl[1]-zl[0];

        int nx = xl.size()-1;
        int ny = yl.size()-1;
        int nz = zl.size()-1;

        std::vector<int> mat;
        std::vector<double> pot;
        std::vector<double> strainxx, strainyy, strainzz, strainxz, strainxy, strainyz;
        std::vector<double> stressxx, stressyy, stresszz, stressxz, stressxy, stressyz;

        for (int k=0; k<nz; ++k) {
            for (int j=0; j<ny; ++j) {
                for (int i=0; i<nx; ++i) {
                    int idx=pme->getIndexFromIndices(i,j,k);
                    mat.push_back((int)pme->getElement(idx)->getMaterial());
                    pot.push_back(pme->getElement(idx)->electrostaticPotential());
                    strainxx.push_back(pme->getElement(idx)->strain_xx());
                    strainyy.push_back(pme->getElement(idx)->strain_yy());
                    strainzz.push_back(pme->getElement(idx)->strain_zz());
                    strainxy.push_back(pme->getElement(idx)->strain_xy());
                    strainxz.push_back(pme->getElement(idx)->strain_xz());
                    strainyz.push_back(pme->getElement(idx)->strain_yz());
                    stressxx.push_back(pme->getElement(idx)->stress_xx());
                    stressyy.push_back(pme->getElement(idx)->stress_yy());
                    stresszz.push_back(pme->getElement(idx)->stress_zz());
                    stressxy.push_back(pme->getElement(idx)->stress_xy());
                    stressxz.push_back(pme->getElement(idx)->stress_xz());
                    stressyz.push_back(pme->getElement(idx)->stress_yz());
                }
            }
        }

        VtkWriterImageData vti;

        vti.setOrigin(m._x,m._y,m._z);
        vti.setSpacing(dx,dy,dz);
        vti.setSize(nx, ny, nz);

        vti.addCellData("Materials", mat, format);
        vti.addCellData("ElectrostaticPotential", pot, format);
        vti.addCellData("StrainXX", strainxx, format);
        vti.addCellData("StrainYY", strainyy, format);
        vti.addCellData("StrainZZ", strainzz, format);
        vti.addCellData("StrainXY", strainxy, format);
        vti.addCellData("StrainXZ", strainxz, format);
        vti.addCellData("StrainYZ", strainyz, format);
        vti.addCellData("StressXX", stressxx, format);
        vti.addCellData("StressYY", stressyy, format);
        vti.addCellData("StressZZ", stresszz, format);
        vti.addCellData("StressXY", stressxy, format);
        vti.addCellData("StressXZ", stressxz, format);
        vti.addCellData("StressYZ", stressyz, format);
        vti.write(ofs);
    }
    else {
		ofs << " x, y, z, particle, defect, name\n";
	}

	const unsigned nParticles = Domains::global()->PM()->getNParticles();
	for(vector<Domains::ParticleData>::iterator it=data.begin(); it!=data.end(); ++it) {
		switch (ft)
		{
		case XYZ_TYPE:
			ofs << Domains::global()->PM()->getParticleName(it->get<0>(), it->get<2>() % nParticles) << " " <<
			it->get<1>()._x*scale << " " << it->get<1>()._y*scale << " " << it->get<1>()._z*scale << " " <<
			it->get<3>() << " " << it->get<4>() << " " << it->get<5>() << endl;
			break;
		case LAMMPS_TYPE:
			ofs << it-data.begin() +1 << "\t" << it->get<2>() << "\t" <<
			it->get<1>()._x*scale << "\t" << it->get<1>()._y*scale << "\t" << it->get<1>()._z*scale << "\t" <<
			it->get<3>() << "\t" << it->get<5>() << std::endl;
			break;
		case ATOMEYE_TYPE:
			break;
        case VTK_TYPE:
            break;
		case CSV_TYPE:
			ofs << it->get<1>()._x*scale << ", " << it->get<1>()._y*scale << ", " << it->get<1>()._z*scale << ", " <<
			it->get<2>() << ", " <<
			it->get<3>() << ", " <<
			it->get<4>() << ", " <<
			it->get<5>() << std::endl;
			break;
		}
	}
	ofs.close();
	if(!specified("no.print"))
		LOWMSG(" done!.");
}

}


/*
 * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy
 * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy
 * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thread>
#include <chrono>

#include <fstream>
#include <iostream>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

class WaypointWithTime {
 public:
  WaypointWithTime()
      : waiting_time(0),
        position(0, 0, 0),
        yaw(0) {
  }

  WaypointWithTime(double t, float x, float y, float z, float _yaw)
      : waiting_time(t),
        position(x, y, z),
        yaw(_yaw)  {
  }

  double waiting_time;
  Eigen::Vector3d position;
  double yaw;

};

int main(int argc, char** argv){
  ros::init(argc, argv, "waypoint_example");
  ros::NodeHandle nh;
  ros::Publisher trajectory_pub =
      nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
      mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
  ROS_INFO("Started waypoint example.");

  ros::V_string args;
  ros::removeROSArgs(argc, argv, args);

  if (args.size() != 2 && args.size() != 3) {
    ROS_ERROR("Usage: waypoint_example <waypoint_file>"
        "\nThe waypoint file should be structured as: space separated: wait_time [s] x[m] y[m] z[m] yaw[deg])");
    return -1;
  }

  std::vector<WaypointWithTime> waypoints;
  const float DEG_2_RAD = M_PI / 180.0;

  std::ifstream wp_file(args.at(1).c_str());

  if (wp_file.is_open()) {
    double t, x, y, z, yaw;
    // Only read complete waypoints.
    while (wp_file >> t >> x >> y >> z >> yaw) {
      waypoints.push_back(WaypointWithTime(t, x, y, z, yaw * DEG_2_RAD));
    }
    wp_file.close();
    ROS_INFO("Read %d waypoints.", (int )waypoints.size());
  }

  else {
    ROS_ERROR_STREAM("Unable to open poses file: " << args.at(1));
    return -1;
  }

  std_srvs::Empty srv;
  bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
  unsigned int i = 0;

  // Trying to unpause Gazebo for 10 seconds.
  while (i <= 10 && !unpaused) {
    ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
    std::this_thread::sleep_for(std::chrono::seconds(1));
    unpaused = ros::service::call("/gazebo/unpause_physics", srv);
    ++i;
  }

  if (!unpaused) {
    ROS_FATAL("Could not wake up Gazebo.");
    return -1;
  }
  else {
    ROS_INFO("Unpaused the Gazebo simulation.");
  }

  // Wait for t seconds to let the Gazebo GUI show up.
  double t = 6.0;
  ros::Duration(t).sleep();

  ROS_INFO("Start publishing waypoints.");

  for (size_t i = 0; i < waypoints.size(); ++i) {

        trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
        trajectory_msg.header.stamp = ros::Time::now();
        Eigen::Vector3d desired_position(waypoints[i].position.x(), waypoints[i].position.y(), waypoints[i].position.z());
        double desired_yaw = waypoints[i].yaw;
        mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg);

        ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
                 nh.getNamespace().c_str(),
                 desired_position.x(),
                 desired_position.y(),
                 desired_position.z());
        trajectory_pub.publish(trajectory_msg);

         // Wait for t seconds to let the Gazebo GUI show up.
         double t = waypoints[i].waiting_time;
         ros::Duration(t).sleep();

  }

  ros::spin();
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <memory>
#include <iostream>
#include <iomanip>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "Eigen/LU"
#include "metro/regression/Design.hpp"
#include "metro/regression/LogLikelihood.hpp"
#include "metro/regression/IndependentNormalWeightedLogLikelihood.hpp"

// #define DEBUG_NORMALWEIGHTEDLOGLIKELIHOOD 1

namespace metro {
	namespace regression {
		IndependentNormalWeightedLogLikelihood::UniquePtr IndependentNormalWeightedLogLikelihood::create(
			LogLikelihood::UniquePtr ll,
			std::vector< int > parameter_indices,
			std::vector< double > means,
			std::vector< double > variances
		) {
			return UniquePtr( new IndependentNormalWeightedLogLikelihood( ll, parameter_indices, means, variances ) ) ;
		}

		IndependentNormalWeightedLogLikelihood::IndependentNormalWeightedLogLikelihood(
			LogLikelihood::UniquePtr ll,
			std::vector< int > parameter_indices,
			std::vector< double > means,
			std::vector< double > variances
		):
			m_ll( ll ),
			m_parameter_indices( parameter_indices ),
			m_means( means ),
			m_variances( variances ),
			m_normalisation( eZeroAtMean ),
			m_constant( 0.0 ) // ll when at mean
		{
			if( m_normalisation == eZeroAtMean ) {
				m_constant = 0.0 ;
			} else if( m_normalisation == ePDF ) {
				assert(0) ; // This may not be correct yet
				m_constant = 0.0 ;
				for( std::size_t i = 0; i < m_parameter_indices.size(); ++i ) {
					m_constant -= std::log( std::sqrt( 2.0 * 3.14159265358979323846 * m_variances[i] )) ;
				}
			} else {
				assert(0) ;
			}
		}

		std::string IndependentNormalWeightedLogLikelihood::get_parameter_name( std::size_t i ) const {
			return m_ll->get_parameter_name(i) ;
		}
		IndependentNormalWeightedLogLikelihood::IntegerMatrix IndependentNormalWeightedLogLikelihood::identify_parameters() const {
			return m_ll->identify_parameters() ;
		}

		int IndependentNormalWeightedLogLikelihood::number_of_parameters() const {
			return m_ll->number_of_parameters() ;
		}

		int IndependentNormalWeightedLogLikelihood::number_of_outcomes() const {
			return m_ll->number_of_outcomes() ;
		}

		void IndependentNormalWeightedLogLikelihood::evaluate_at( Point const& parameters, int const numberOfDerivatives ) {
			m_ll->evaluate_at( parameters, numberOfDerivatives ) ;
			evaluate( numberOfDerivatives ) ;
		}

		void IndependentNormalWeightedLogLikelihood::evaluate( int const numberOfDerivatives ) {
			m_ll->evaluate( numberOfDerivatives ) ;
			Vector const& parameters = m_ll->parameters() ;

			m_value_of_function = 0.0 ;
			m_value_of_first_derivative.setZero( parameters.size() ) ;
			m_value_of_second_derivative.setZero( parameters.size(), parameters.size() ) ;
			for( std::size_t i = 0; i < m_parameter_indices.size(); ++i ) {
				int const& parameter_index = m_parameter_indices[i] ;
				assert( parameter_index >= 0 && parameter_index < parameters.size() ) ;
				double const centralised = (parameters(parameter_index)-m_means[i]) ;

				m_value_of_function += -0.5 * (centralised * centralised) / m_variances[i] ;
				m_value_of_first_derivative( parameter_index ) = -centralised / m_variances[i] ;
				m_value_of_second_derivative( parameter_index, parameter_index )
					= -1.0 / m_variances[i] ;
			}
			m_value_of_function += m_constant ;
		}

		IndependentNormalWeightedLogLikelihood::Vector IndependentNormalWeightedLogLikelihood::get_prior_mode() const {
			Vector result = Vector::Zero( m_value_of_first_derivative.size() ) ;
			for( std::size_t i = 0; i < m_parameter_indices.size(); ++i ) {
				result(i) = m_means[i] ;
			}
			return result ;
		}

		std::string IndependentNormalWeightedLogLikelihood::get_summary() const {
			std::ostringstream ostr ;
			ostr << "independently-normal-weighted:" << m_ll->get_summary() << "\n" ;
			ostr << "independently-normal-weighted: using the following prior parameters:\n" ;
			std::size_t max_label_length = 0 ;
			int const numberOfParameters = m_ll->identify_parameters().rows() ;
			for( int i = 0; i < numberOfParameters; ++i ) {
				max_label_length = std::max( max_label_length, m_ll->get_parameter_name(i).size() ) ;
			}
			ostr
				<< std::setw(3) << "" << "  "
				<< std::setw( max_label_length + 2 ) << ""
				<< ":"
				<< " " << std::setw(5) << "mean"
				<< " " << std::setw(8) << "variance"
				<< "\n" ;
			for( std::size_t i = 0; i < m_parameter_indices.size(); ++i ) {
				int const parameter_index = m_parameter_indices[i] ;
				ostr << std::setw(3) << parameter_index
					<< ": " << std::setw( max_label_length + 2 ) << m_ll->get_parameter_name(parameter_index) << ":" ;
					ostr
						<< " " << std::setw(5) << m_means[i]
						<< " " << std::setw(8) << m_variances[i]
						<< "\n" ;
			}
			return ostr.str() ;
		}
	}
}


// Copyright (c) 2013-2019 The Elements Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <arith_uint256.h>
#include <blind.h>
#include <coins.h>
#include <uint256.h>
#include <validation.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

#include <secp256k1.h>

// For elements serialization rules
struct ElementsSetup : public TestingSetup {
        ElementsSetup() : TestingSetup("custom") {}
};

BOOST_FIXTURE_TEST_SUITE(blind_tests, ElementsSetup)

// TODO: Make deterministic blinding wrapper function, test caching more exactly

BOOST_AUTO_TEST_CASE(naive_blinding_test)
{
    CKey key1;
    CKey key2;
    CKey keyDummy;

    // Any asset id will do
    CAsset bitcoinID(GetRandHash());
    CAsset otherID(GetRandHash());
    CAsset unblinded_id;
    uint256 asset_blind;
    CScript op_true(OP_TRUE);
    std::vector<CKey> vDummy;

    unsigned char k1[32] = {1,2,3};
    unsigned char k2[32] = {22,33,44};
    unsigned char kDummy[32] = {133,144,155};
    key1.Set(&k1[0], &k1[32], true);
    key2.Set(&k2[0], &k2[32], true);
    keyDummy.Set(&kDummy[0], &kDummy[32], true);
    CPubKey pubkey1 = key1.GetPubKey();
    CPubKey pubkey2 = key2.GetPubKey();
    CPubKey pubkeyDummy = keyDummy.GetPubKey();

    uint256 blind3, blind4, blindDummy;

    std::vector<CTxOut> inputs;
    CTxOut btc_oo(bitcoinID, 11, CScript());
    CTxOut btc_ooo(bitcoinID, 111, CScript());
    CTxOut other_fzz(otherID, 500, CScript());
    CTxOut blind_ozz; // Will be computed later

    {
        inputs.clear();
        inputs.push_back(btc_oo);
        inputs.push_back(btc_ooo);

        // Build a transaction that spends 2 unblinded coins (11, 111), and produces a single blinded one (100) and fee (22).
        CMutableTransaction tx3;
        tx3.vin.resize(2);
        tx3.vin[0].prevout.hash = ArithToUint256(1);

        tx3.vin[0].prevout.n = 0;
        tx3.vin[1].prevout.hash = ArithToUint256(2);
        tx3.vin[1].prevout.n = 0;
        tx3.vout.resize(0);
        tx3.vout.push_back(CTxOut(bitcoinID, 100, CScript() << OP_TRUE));
        // Fee outputs are blank scriptpubkeys, and unblinded value/asset
        tx3.vout.push_back(CTxOut(bitcoinID, 22, CScript()));
        BOOST_CHECK(VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));

        // Malleate the output and check for correct handling of bad commitments
        // These will fail IsValid checks
        std::vector<unsigned char> asset_copy(tx3.vout[0].nAsset.vchCommitment);
        std::vector<unsigned char> value_copy(tx3.vout[0].nValue.vchCommitment);
        tx3.vout[0].nAsset.vchCommitment[0] = 122;
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));
        tx3.vout[0].nAsset.vchCommitment = asset_copy;
        tx3.vout[0].nValue.vchCommitment[0] = 122;
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));
        tx3.vout[0].nValue.vchCommitment = value_copy;

        // Make sure null values are handled correctly
        tx3.vout[0].nAsset.SetNull();
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));
        tx3.vout[0].nAsset.vchCommitment = asset_copy;
        tx3.vout[0].nValue.SetNull();
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));
        tx3.vout[0].nValue.vchCommitment = value_copy;

        // Bad nonce values will result in failure to deserialize
        tx3.vout[0].nNonce.SetNull();
        BOOST_CHECK(VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));
        tx3.vout[0].nNonce.vchCommitment = tx3.vout[0].nValue.vchCommitment;
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));

        // Try to blind with a single non-fee output, which fails as its blinding factor ends up being zero.
        std::vector<uint256> input_blinds;
        std::vector<uint256> input_asset_blinds;
        std::vector<CAsset> input_assets;
        std::vector<CAmount> input_amounts;
        std::vector<uint256> output_blinds;
        std::vector<uint256> output_asset_blinds;
        std::vector<CPubKey> output_pubkeys;
        input_blinds.push_back(uint256());
        input_blinds.push_back(uint256());
        input_asset_blinds.push_back(uint256());
        input_asset_blinds.push_back(uint256());
        input_assets.push_back(bitcoinID);
        input_assets.push_back(bitcoinID);
        input_amounts.push_back(11);
        input_amounts.push_back(111);
        output_pubkeys.push_back(pubkey1);
        output_pubkeys.push_back(CPubKey());
        BOOST_CHECK(BlindTransaction(input_blinds, input_asset_blinds, input_assets, input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, tx3) == 0);

        // Add a dummy output. Must be unspendable since it's 0-valued.
        tx3.vout.push_back(CTxOut(bitcoinID, 0, CScript() << OP_RETURN));
        output_pubkeys.push_back(pubkeyDummy);
        BOOST_CHECK(BlindTransaction(input_blinds, input_asset_blinds, input_assets, input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, tx3) == 2);
        BOOST_CHECK(!tx3.vout[0].nValue.IsExplicit());
        BOOST_CHECK(!tx3.vout[2].nValue.IsExplicit());
        BOOST_CHECK(VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));

        CAmount unblinded_amount;
        BOOST_CHECK(UnblindConfidentialPair(key2, tx3.vout[0].nValue, tx3.vout[0].nAsset, tx3.vout[0].nNonce, op_true, tx3.witness.vtxoutwit[0].vchRangeproof, unblinded_amount, blind3, unblinded_id, asset_blind) == 0);
        // Saving unblinded_id and asset_blind for later since we need for input
        BOOST_CHECK(UnblindConfidentialPair(key1, tx3.vout[0].nValue, tx3.vout[0].nAsset, tx3.vout[0].nNonce, op_true, tx3.witness.vtxoutwit[0].vchRangeproof, unblinded_amount, blind3, unblinded_id, asset_blind) == 1);
        BOOST_CHECK(unblinded_amount == 100);
        BOOST_CHECK(unblinded_id == bitcoinID);
        CAsset temp_asset;
        uint256 temp_asset_blinder;
        BOOST_CHECK(UnblindConfidentialPair(keyDummy, tx3.vout[2].nValue, tx3.vout[2].nAsset, tx3.vout[2].nNonce, CScript() << OP_RETURN, tx3.witness.vtxoutwit[2].vchRangeproof, unblinded_amount, blindDummy, temp_asset, temp_asset_blinder) == 1);
        BOOST_CHECK(unblinded_amount == 0);

        // Storing for next section
        BOOST_CHECK(tx3.vout[0].nValue.IsCommitment());
        BOOST_CHECK(tx3.vout[0].nAsset.IsCommitment());
        blind_ozz = tx3.vout[0];

        tx3.vout[1].nValue = CConfidentialValue(tx3.vout[1].nValue.GetAmount() - 1);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx3), nullptr, false));
    }

    {
        inputs.clear();
        inputs.push_back(btc_ooo);
        inputs.push_back(blind_ozz);

        // Build a transactions that spends an unblinded (111) and blinded (100) coin, and produces only unblinded coins (impossible)
        CMutableTransaction tx4;
        tx4.vin.resize(2);
        tx4.vin[0].prevout.hash = ArithToUint256(2);
        tx4.vin[0].prevout.n = 0;
        tx4.vin[1].prevout.hash = ArithToUint256(3);
        tx4.vin[1].prevout.n = 0;
        tx4.vout.push_back(CTxOut(bitcoinID, 30, CScript() << OP_TRUE));
        tx4.vout.push_back(CTxOut(bitcoinID, 40, CScript() << OP_TRUE));
        tx4.vout.push_back(CTxOut(bitcoinID, 111+100-30-40, CScript()));
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx4), nullptr, false)); // Spends a blinded coin with no blinded outputs to compensate.

        std::vector<uint256> input_blinds;
        std::vector<uint256> input_asset_blinds;
        std::vector<CAsset> input_assets;
        std::vector<CAmount> input_amounts;
        std::vector<uint256> output_blinds;
        std::vector<uint256> output_asset_blinds;
        std::vector<CPubKey> output_pubkeys;
        input_blinds.push_back(uint256());
        input_blinds.push_back(blind3);
        input_asset_blinds.push_back(uint256());
        input_asset_blinds.push_back(asset_blind);
        input_amounts.push_back(111);
        input_amounts.push_back(100);
        input_assets.push_back(unblinded_id);
        input_assets.push_back(unblinded_id);
        output_pubkeys.push_back(CPubKey());
        output_pubkeys.push_back(CPubKey());
        output_pubkeys.push_back(CPubKey());
        BOOST_CHECK(BlindTransaction(input_blinds, input_asset_blinds, input_assets, input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, tx4) == 0); // Blinds nothing
    }

    {
        inputs.clear();
        inputs.push_back(btc_ooo);
        inputs.push_back(blind_ozz);

        // Build a transactions that spends an unblinded (111) and blinded (100) coin, and produces a blinded (30), unblinded (40), and blinded (50) coin and fee (91)
        CMutableTransaction tx4;
        tx4.vin.resize(2);
        tx4.vin[0].prevout.hash = ArithToUint256(2);
        tx4.vin[0].prevout.n = 0;
        tx4.vin[1].prevout.hash = ArithToUint256(3);
        tx4.vin[1].prevout.n = 0;
        tx4.vout.push_back(CTxOut(bitcoinID, 30, CScript() << OP_TRUE));
        tx4.vout.push_back(CTxOut(bitcoinID, 40, CScript() << OP_TRUE));
        tx4.vout.push_back(CTxOut(bitcoinID, 50, CScript() << OP_TRUE));
        // Fee
        tx4.vout.push_back(CTxOut(bitcoinID, 111+100-30-40-50, CScript()));
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx4), nullptr, false)); // Spends a blinded coin with no blinded outputs to compensate.

        std::vector<uint256> input_blinds;
        std::vector<uint256> input_asset_blinds;
        std::vector<CAsset> input_assets;
        std::vector<CAmount> input_amounts;
        std::vector<uint256> output_blinds;
        std::vector<uint256> output_asset_blinds;
        std::vector<CPubKey> output_pubkeys;

        input_blinds.push_back(uint256());
        input_blinds.push_back(blind3);
        input_asset_blinds.push_back(uint256());
        input_asset_blinds.push_back(asset_blind);
        input_amounts.push_back(111);
        input_amounts.push_back(100);
        input_assets.push_back(unblinded_id);
        input_assets.push_back(unblinded_id);

        output_pubkeys.push_back(pubkey2);
        output_pubkeys.push_back(CPubKey());
        output_pubkeys.push_back(pubkey2);
        output_pubkeys.push_back(CPubKey());

        BOOST_CHECK(BlindTransaction(input_blinds, input_asset_blinds, input_assets, input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, tx4) == 2);
        BOOST_CHECK(!tx4.vout[0].nValue.IsExplicit());
        BOOST_CHECK(tx4.vout[1].nValue.IsExplicit());
        BOOST_CHECK(!tx4.vout[2].nValue.IsExplicit());
        // This one broken
        BOOST_CHECK(VerifyAmounts(inputs, CTransaction(tx4), nullptr, false));

        CAmount unblinded_amount;
        CAsset asset_out;
        uint256 asset_blinder_out;
        BOOST_CHECK(UnblindConfidentialPair(key1, tx4.vout[0].nValue, tx4.vout[0].nAsset, tx4.vout[0].nNonce, op_true, tx4.witness.vtxoutwit[0].vchRangeproof, unblinded_amount, blind4, asset_out, asset_blinder_out) == 0);
        BOOST_CHECK(UnblindConfidentialPair(key2, tx4.vout[0].nValue, tx4.vout[0].nAsset, tx4.vout[0].nNonce, op_true, tx4.witness.vtxoutwit[0].vchRangeproof, unblinded_amount, blind4, asset_out, asset_blinder_out) == 1);
        BOOST_CHECK(unblinded_amount == 30);
        BOOST_CHECK(asset_out == unblinded_id);
        BOOST_CHECK(UnblindConfidentialPair(key2, tx4.vout[2].nValue, tx4.vout[2].nAsset, tx4.vout[2].nNonce, op_true, tx4.witness.vtxoutwit[2].vchRangeproof, unblinded_amount, blind4, asset_out, asset_blinder_out) == 1);
        BOOST_CHECK(asset_out == unblinded_id);
        BOOST_CHECK(unblinded_amount == 50);

        // Commit to the wrong script in the rangeproof
        BOOST_CHECK(UnblindConfidentialPair(key2, tx4.vout[2].nValue, tx4.vout[2].nAsset, tx4.vout[2].nNonce, CScript() << OP_FALSE, tx4.witness.vtxoutwit[2].vchRangeproof, unblinded_amount, blind4, asset_out, asset_blinder_out) == 0);

        // Make invalid public keys in nonce commitment, first of right size
        tx4.vout[2].nNonce.vchCommitment = std::vector<unsigned char>(33, 0);
        tx4.vout[2].nNonce.vchCommitment[0] = 0x03;
        BOOST_CHECK(UnblindConfidentialPair(key2, tx4.vout[2].nValue, tx4.vout[2].nAsset, tx4.vout[2].nNonce, op_true, tx4.witness.vtxoutwit[2].vchRangeproof, unblinded_amount, blind4, asset_out, asset_blinder_out) == 0);

        // Next, leading byte claiming to be 33 bytes in size
        tx4.vout[2].nNonce.vchCommitment.resize(1);
        BOOST_CHECK(UnblindConfidentialPair(key2, tx4.vout[2].nValue, tx4.vout[2].nAsset, tx4.vout[2].nNonce, op_true, tx4.witness.vtxoutwit[2].vchRangeproof, unblinded_amount, blind4, asset_out, asset_blinder_out) == 0);

        // Last, blank nonce commitment
        tx4.vout[2].nNonce.vchCommitment.clear();
        BOOST_CHECK(UnblindConfidentialPair(key2, tx4.vout[2].nValue, tx4.vout[2].nAsset, tx4.vout[2].nNonce, op_true, tx4.witness.vtxoutwit[2].vchRangeproof, unblinded_amount, blind4, asset_out, asset_blinder_out) == 0);

        tx4.vout[3].nValue = CConfidentialValue(tx4.vout[3].nValue.GetAmount() - 1);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx4), nullptr, false));

        // Check wallet borromean-based rangeproof results against expected args
        size_t proof_size = DEFAULT_RANGEPROOF_SIZE;
        BOOST_CHECK_EQUAL(tx4.witness.vtxoutwit[2].vchRangeproof.size(), proof_size);
        secp256k1_context *ctx = secp256k1_context_create(SECP256K1_CONTEXT_VERIFY);
        int exp = 0;
        int mantissa = 0;
        uint64_t min_value = 0;
        uint64_t max_value = 0;
        BOOST_CHECK(secp256k1_rangeproof_info(ctx, &exp, &mantissa, &min_value, &max_value, tx4.witness.vtxoutwit[2].vchRangeproof.data(), proof_size) == 1);
        BOOST_CHECK_EQUAL(exp, 0);
        BOOST_CHECK_EQUAL(mantissa, 52); // 52 bit default
        BOOST_CHECK_EQUAL(min_value, 1ULL);
        BOOST_CHECK_EQUAL(max_value, 4503599627370496ULL);
        secp256k1_context_destroy(ctx);
    }
    {
        inputs.clear();
        inputs.push_back(blind_ozz);
        inputs.push_back(other_fzz);

        // Spends 100 blinded bitcoin, 500 of unblinded "other"
        CMutableTransaction tx5;
        tx5.vin.resize(0);
        tx5.vout.resize(0);
        tx5.vin.push_back(CTxIn(COutPoint(ArithToUint256(3), 0)));
        tx5.vin.push_back(CTxIn(COutPoint(ArithToUint256(5), 0)));
        tx5.vout.push_back(CTxOut(bitcoinID, 29, CScript() << OP_TRUE));
        tx5.vout.push_back(CTxOut(bitcoinID, 70, CScript() << OP_TRUE));
        tx5.vout.push_back(CTxOut(otherID, 250, CScript() << OP_TRUE));
        tx5.vout.push_back(CTxOut(otherID, 249, CScript() << OP_TRUE));
        // Fees
        tx5.vout.push_back(CTxOut(bitcoinID, 1, CScript()));
        tx5.vout.push_back(CTxOut(otherID, 1, CScript()));

        // Blinds don't balance
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(tx5), nullptr, false));

        // Blinding setup stuff
        std::vector<uint256> input_blinds;
        std::vector<uint256> input_asset_blinds;
        std::vector<CAsset> input_assets;
        std::vector<CAmount> input_amounts;
        std::vector<uint256> output_blinds;
        std::vector<uint256> output_asset_blinds;
        std::vector<CPubKey> output_pubkeys;
        input_blinds.push_back(blind3);
        input_blinds.push_back(uint256()); //
        input_asset_blinds.push_back(asset_blind);
        input_asset_blinds.push_back(uint256());
        input_amounts.push_back(100);
        input_amounts.push_back(500);
        input_assets.push_back(bitcoinID);
        input_assets.push_back(otherID);
        for (unsigned int i = 0; i < 6; i++) {
            output_pubkeys.push_back(pubkey2);
        }

        CMutableTransaction txtemp(tx5);

        // No blinding keys for fees, bails out blinding nothing, still invalid due to imbalance
        BOOST_CHECK(BlindTransaction(input_blinds, input_asset_blinds, input_assets, input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, txtemp) == -1);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));
        // Last will be implied blank keys
        output_pubkeys.resize(4);

        // Blind transaction, verify amounts
        txtemp = tx5;
        BOOST_CHECK(BlindTransaction(input_blinds, input_asset_blinds, input_assets, input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, txtemp) == 4);
        BOOST_CHECK(VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));

        // Transaction may not have spendable 0-value output
        txtemp.vout.push_back(CTxOut(CAsset(), 0, CScript() << OP_TRUE));
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));

        // Create imbalance by removing fees, should still be able to blind
        txtemp = tx5;
        txtemp.vout.resize(5);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));
        txtemp.vout.resize(4);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));
        BOOST_CHECK(BlindTransaction(input_blinds, input_asset_blinds, input_assets, input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, txtemp) == 4);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));

        txtemp = tx5;
        // Remove other input, make surjection proof impossible for 2 "otherID" outputs
        std::vector<uint256> t_input_blinds;
        std::vector<uint256> t_input_asset_blinds;
        std::vector<CAsset> t_input_assets;
        std::vector<CAmount> t_input_amounts;

        t_input_blinds = input_blinds;
        t_input_asset_blinds = input_asset_blinds;
        t_input_assets = input_assets;
        t_input_amounts = input_amounts;
        txtemp.vin.resize(1);
        inputs.resize(1);
        t_input_blinds.resize(1);
        t_input_asset_blinds.resize(1);
        t_input_assets.resize(1);
        t_input_amounts.resize(1);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));
        BOOST_CHECK(BlindTransaction(t_input_blinds, t_input_asset_blinds, t_input_assets, t_input_amounts, output_blinds, output_asset_blinds, output_pubkeys, vDummy, vDummy, txtemp) == 2);
        BOOST_CHECK(!VerifyAmounts(inputs, CTransaction(txtemp), nullptr, false));
    }
}
BOOST_AUTO_TEST_SUITE_END()