// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_IS_ACCEPTABLE_TURN_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_IS_ACCEPTABLE_TURN_HPP

#include <boost/range/value_type.hpp>

#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/detail/overlay/turn_info.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace is_valid
{

template
<
    typename Geometry,
    order_selector Order = geometry::point_order<Geometry>::value,
    typename Tag = typename tag<Geometry>::type
>
struct acceptable_operation
{};

template <typename Polygon>
struct acceptable_operation<Polygon, counterclockwise, polygon_tag>
{
    static const detail::overlay::operation_type value =
        detail::overlay::operation_union;
};

template <typename Polygon>
struct acceptable_operation<Polygon, clockwise, polygon_tag>
{
    static const detail::overlay::operation_type value =
        detail::overlay::operation_intersection;
};

template <typename MultiPolygon>
struct acceptable_operation<MultiPolygon, counterclockwise, multi_polygon_tag>
{
    static const detail::overlay::operation_type value =
        detail::overlay::operation_intersection;
};

template <typename MultiPolygon>
struct acceptable_operation<MultiPolygon, clockwise, multi_polygon_tag>
{
    static const detail::overlay::operation_type value =
        detail::overlay::operation_union;
};

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct is_acceptable_turn
{};

template <typename Ring>
struct is_acceptable_turn<Ring, ring_tag>
{
    template <typename Turn>
    static inline bool apply(Turn const&)
    {
        return false;
    }
};

template <typename Polygon>
class is_acceptable_turn<Polygon, polygon_tag>
{
protected:
    template <typename Turn, typename Method, typename Operation>
    static inline bool check_turn(Turn const& turn,
                                  Method method,
                                  Operation operation)
    {
        return turn.method == method
            && turn.operations[0].operation == operation
            && turn.operations[1].operation == operation;
    }

public:
    template <typename Turn>
    static inline bool apply(Turn const& turn)
    {
        using namespace detail::overlay;

        if ( turn.operations[0].seg_id.ring_index
             == turn.operations[1].seg_id.ring_index )
        {
            return false;
        }

        operation_type const op = acceptable_operation<Polygon>::value;

        return check_turn(turn, method_touch_interior, op)
            || check_turn(turn, method_touch, op)
            ;
    }
};

template <typename MultiPolygon>
class is_acceptable_turn<MultiPolygon, multi_polygon_tag>
    : is_acceptable_turn<typename boost::range_value<MultiPolygon>::type>
{
private:
    typedef typename boost::range_value<MultiPolygon>::type polygon;
    typedef is_acceptable_turn<polygon> base;

public:
    template <typename Turn>
    static inline bool apply(Turn const& turn)
    {
        using namespace detail::overlay;

        if ( turn.operations[0].seg_id.multi_index
             == turn.operations[1].seg_id.multi_index )
        {
            return base::apply(turn);
        }

        operation_type const op = acceptable_operation<MultiPolygon>::value;
        if ( base::check_turn(turn, method_touch_interior, op)
          || base::check_turn(turn, method_touch, op))
        {
            return true;
        }

        // Turn is acceptable only in case of a touch(interior) and both lines
        // (polygons) do not cross
        return (turn.method == method_touch
                || turn.method == method_touch_interior)
                && turn.touch_only;
    }
};

}} // namespace detail::is_valid
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_IS_ACCEPTABLE_TURN_HPP


#include "catch.hpp"

// test utils
#include "test_utils.hpp"

// mapnik
#include <mapnik/agg_renderer.hpp>
#include <mapnik/datasource_cache.hpp>
#include <mapnik/feature_factory.hpp>
#include <mapnik/geometry.hpp>
#include <mapnik/version.hpp>
#if MAPNIK_VERSION >= 300100
#include <mapnik/geometry/is_empty.hpp>
#include <mapnik/geometry/reprojection.hpp>
#else
#include <mapnik/geometry_is_empty.hpp>
#include <mapnik/geometry_reprojection.hpp>
#endif
#include <mapnik/image_util.hpp>
#include <mapnik/load_map.hpp>
#include <mapnik/memory_datasource.hpp>
#include <mapnik/projection.hpp>
#include <mapnik/proj_transform.hpp>
#include <mapnik/util/file_io.hpp>
#include <mapnik/util/fs.hpp>
#include <mapnik/util/geometry_to_geojson.hpp>
#include <mapnik/vertex_adapters.hpp>

// boost
#include <boost/optional/optional_io.hpp>

// mapnik-vector-tile
#include "vector_tile_processor.hpp"
#include "vector_tile_geometry_decoder.hpp"
#include "vector_tile_datasource_pbf.hpp"

// libprotobuf
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wsign-conversion"
#include "vector_tile.pb.h"
#pragma GCC diagnostic pop

// std
#include <fstream>

TEST_CASE("vector tile from simplified geojson")
{
    unsigned tile_size = 256 * 100;
    mapnik::Map map(256,256,"+init=epsg:3857");
    mapnik::layer lyr("layer","+init=epsg:4326");
    std::shared_ptr<mapnik::memory_datasource> ds = testing::build_geojson_ds("./test/data/poly.geojson");
    ds->set_envelope(mapnik::box2d<double>(160.147311,11.047284,160.662858,11.423830));
    lyr.set_datasource(ds);
    map.add_layer(lyr);
    mapnik::vector_tile_impl::processor ren(map);
    mapnik::vector_tile_impl::tile out_tile = ren.create_tile(0,0,0,tile_size);
    CHECK(out_tile.is_painted() == true);
    CHECK(out_tile.is_empty() == false);

    vector_tile::Tile tile;
    tile.ParseFromString(out_tile.get_buffer());
    REQUIRE(1 == tile.layers_size());
    vector_tile::Tile_Layer const& layer = tile.layers(0);
    CHECK(std::string("layer") == layer.name());
    REQUIRE(1 == layer.features_size());
    vector_tile::Tile_Feature const& f = layer.features(0);
    unsigned z = 0;
    unsigned x = 0;
    unsigned y = 0;
    double resolution = mapnik::EARTH_CIRCUMFERENCE/(1 << z);
    double tile_x = -0.5 * mapnik::EARTH_CIRCUMFERENCE + x * resolution;
    double tile_y =  0.5 * mapnik::EARTH_CIRCUMFERENCE - y * resolution;
    double scale = static_cast<double>(layer.extent())/resolution;
    protozero::pbf_reader layer_reader;
    out_tile.layer_reader(0, layer_reader);
    REQUIRE(layer_reader.next(mapnik::vector_tile_impl::Layer_Encoding::FEATURES));
    protozero::pbf_reader feature_reader = layer_reader.get_message();
    int32_t geometry_type = mapnik::vector_tile_impl::Geometry_Type::UNKNOWN;
    mapnik::vector_tile_impl::GeometryPBF::pbf_itr geom_itr;
    while (feature_reader.next())
    {
        if (feature_reader.tag() == mapnik::vector_tile_impl::Feature_Encoding::GEOMETRY)
        {
            geom_itr = feature_reader.get_packed_uint32();
        }
        else if (feature_reader.tag() == mapnik::vector_tile_impl::Feature_Encoding::TYPE)
        {
            geometry_type = feature_reader.get_enum();
        }
        else
        {
            feature_reader.skip();
        }
    }
    mapnik::vector_tile_impl::GeometryPBF geoms(geom_itr);
    auto geom = mapnik::vector_tile_impl::decode_geometry<double>(geoms, geometry_type, 2, tile_x, tile_y, scale, -1.0 * scale);

    unsigned int n_err = 0;
    mapnik::projection wgs84("+init=epsg:4326",true);
    mapnik::projection merc("+init=epsg:3857",true);
    mapnik::proj_transform prj_trans(merc,wgs84);
    mapnik::geometry::geometry<double> projected_geom = mapnik::geometry::reproject_copy(geom,prj_trans,n_err);
    CHECK( n_err == 0 );
    std::string geojson_string;
    CHECK( mapnik::util::to_geojson(geojson_string,projected_geom) );
    std::string geojson_file = "./test/data/simplified_geometry.geojson";
    mapnik::util::file input(geojson_file);
    if (input.is_open())
    {
        std::string json_string(input.data().get(), input.size());
        CHECK (geojson_string == json_string);
    }
    if (!mapnik::util::exists(geojson_file) || (std::getenv("UPDATE") != nullptr))
    {
        std::ofstream out(geojson_file);
        out << geojson_string;
    }
}

TEST_CASE("vector tile transform -- should not throw on coords outside merc range")
{
    unsigned tile_size = 256 * 64;
    mapnik::Map map(256,256,"+init=epsg:3857");
    // Note: 4269 is key. 4326 will trigger custom mapnik reprojection code
    // that does not hit proj4 and clamps values
    mapnik::layer lyr("layer","+init=epsg:4269");
    mapnik::parameters params;
    params["type"] = "shape";
    params["file"] = "./test/data/poly-lat-invalid-4269.shp";
    std::shared_ptr<mapnik::datasource> ds =
        mapnik::datasource_cache::instance().create(params);
    lyr.set_datasource(ds);
    map.add_layer(lyr);
    mapnik::vector_tile_impl::processor ren(map);
    mapnik::vector_tile_impl::tile out_tile = ren.create_tile(0,0,0,tile_size);
    CHECK(out_tile.is_painted() == true);
    CHECK(out_tile.is_empty() == false);

    vector_tile::Tile tile;
    tile.ParseFromString(out_tile.get_buffer());

    // serialize to message
    std::string buffer;
    CHECK(tile.SerializeToString(&buffer));
    CHECK(71 == buffer.size());
    // now create new objects
    mapnik::Map map2(256,256,"+init=epsg:3857");
    vector_tile::Tile tile2;
    CHECK(tile2.ParseFromString(buffer));
    CHECK(1 == tile2.layers_size());
    vector_tile::Tile_Layer const& layer2 = tile2.layers(0);
    CHECK(std::string("layer") == layer2.name());
    CHECK(1 == layer2.features_size());
    CHECK(1 == layer2.features(0).id());
    CHECK(3 == layer2.features(0).type());

    mapnik::layer lyr2("layer",map.srs());

    protozero::pbf_reader layer_reader;
    out_tile.layer_reader(0, layer_reader);
    std::shared_ptr<mapnik::vector_tile_impl::tile_datasource_pbf> ds2 = std::make_shared<
                                    mapnik::vector_tile_impl::tile_datasource_pbf>(
                                        layer_reader,0,0,0);
    //ds2->set_envelope(bbox);
    CHECK( ds2->type() == mapnik::datasource::Vector );
    CHECK( ds2->get_geometry_type() == mapnik::datasource_geometry_t::Collection );
    mapnik::layer_descriptor lay_desc = ds2->get_descriptor();
    std::vector<std::string> expected_names;
    expected_names.push_back("FID");
    std::vector<std::string> names;
    for (auto const& desc : lay_desc.get_descriptors())
    {
        names.push_back(desc.get_name());
    }
    CHECK(names == expected_names);
    lyr2.set_datasource(ds2);
    lyr2.add_style("style");
    map2.add_layer(lyr2);
    mapnik::load_map(map2,"test/data/polygon-style.xml");
    //std::clog << mapnik::save_map_to_string(map2) << "\n";
    map2.zoom_all();
    mapnik::image_rgba8 im(map2.width(),map2.height());
    mapnik::agg_renderer<mapnik::image_rgba8> ren2(map2,im);
    ren2.apply();
    if (!mapnik::util::exists("test/fixtures/transform-expected-1.png"))
    {
        mapnik::save_to_file(im,"test/fixtures/transform-expected-1.png","png32");
    }
    unsigned diff = testing::compare_images(im,"test/fixtures/transform-expected-1.png");
    CHECK(0 == diff);
    if (diff > 0)
    {
        mapnik::save_to_file(im,"test/fixtures/transform-actual-1.png","png32");
    }
}

TEST_CASE("vector tile transform2 -- should not throw reprojected data from local NZ projection")
{
    unsigned tile_size = 256 * 64;
    mapnik::box2d<double> bbox(-20037508.342789,-20037508.342789,20037508.342789,20037508.342789);
    mapnik::Map map(256,256,"+init=epsg:3857");
    // Note: 4269 is key. 4326 will trigger custom mapnik reprojection code
    // that does not hit proj4 and clamps values
    mapnik::layer lyr("layer","+proj=nzmg +lat_0=-41 +lon_0=173 +x_0=2510000 +y_0=6023150 +ellps=intl +units=m +no_defs");
    mapnik::parameters params;
    params["type"] = "shape";
    params["file"] = "./test/data/NZ_Coastline_NZMG.shp";
    std::shared_ptr<mapnik::datasource> ds =
        mapnik::datasource_cache::instance().create(params);
    lyr.set_datasource(ds);
    map.add_layer(lyr);

    mapnik::vector_tile_impl::processor ren(map);
    mapnik::vector_tile_impl::tile out_tile = ren.create_tile(0,0,0,tile_size);
    CHECK(out_tile.is_painted() == true);
    CHECK(out_tile.is_empty() == false);

    vector_tile::Tile tile;
    tile.ParseFromString(out_tile.get_buffer());

    // serialize to message
    std::string buffer;
    CHECK(tile.SerializeToString(&buffer));
    CHECK(231 == buffer.size());
    // now create new objects
    mapnik::Map map2(256,256,"+init=epsg:3857");
    vector_tile::Tile tile2;
    CHECK(tile2.ParseFromString(buffer));
    CHECK(1 == tile2.layers_size());
    vector_tile::Tile_Layer const& layer2 = tile2.layers(0);
    CHECK(std::string("layer") == layer2.name());
    CHECK(2 == layer2.features_size());

    mapnik::layer lyr2("layer",map.srs());
    protozero::pbf_reader layer_reader;
    out_tile.layer_reader(0, layer_reader);
    std::shared_ptr<mapnik::vector_tile_impl::tile_datasource_pbf> ds2 = std::make_shared<
                                    mapnik::vector_tile_impl::tile_datasource_pbf>(
                                        layer_reader,0,0,0);
    ds2->set_envelope(bbox);
    CHECK( ds2->type() == mapnik::datasource::Vector );
    CHECK( ds2->get_geometry_type() == mapnik::datasource_geometry_t::Collection );
    mapnik::layer_descriptor lay_desc = ds2->get_descriptor();
    std::vector<std::string> expected_names;
    expected_names.push_back("featurecla");
    expected_names.push_back("scalerank");
    std::vector<std::string> names;
    for (auto const& desc : lay_desc.get_descriptors())
    {
        names.push_back(desc.get_name());
    }
    CHECK(names == expected_names);
    lyr2.set_datasource(ds2);
    lyr2.add_style("style");
    map2.add_layer(lyr2);
    mapnik::load_map(map2,"test/data/polygon-style.xml");
    //std::clog << mapnik::save_map_to_string(map2) << "\n";
    map2.zoom_to_box(bbox);
    mapnik::image_rgba8 im(map2.width(),map2.height());
    mapnik::agg_renderer<mapnik::image_rgba8> ren2(map2,im);
    ren2.apply();
    if (!mapnik::util::exists("test/fixtures/transform-expected-2.png")) {
        mapnik::save_to_file(im,"test/fixtures/transform-expected-2.png","png32");
    }
    unsigned diff = testing::compare_images(im,"test/fixtures/transform-expected-2.png");
    CHECK(0 == diff);
    if (diff > 0) {
        mapnik::save_to_file(im,"test/fixtures/transform-actual-2.png","png32");
    }
}


/*
 * Copyright (C) 2019 University of Utah
 * Copyright (c) 2012, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author: Tobias Kunz <tobias@gatech.edu>
 * Date: 05/2012
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 *
 * Algorithm details and publications:
 * http://www.golems.org/node/1570
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <iostream>
#include <cstdio>
#include <Eigen/Core>
#include "Trajectory.h"
#include "Path.h"
#include <trajectory_smoothing/GetSmoothTraj.h>
using namespace std;
using namespace Eigen;

bool srv_call(trajectory_smoothing::GetSmoothTraj::Request &req, trajectory_smoothing::GetSmoothTraj::Response &res)
{
  // store the joint angle waypoints:
  list<VectorXd> waypoints;
  int dof=req.init_traj.points[0].positions.size();
  for(int i=0;i<req.init_traj.points.size();++i)
  {
    VectorXd waypoint(dof);
    for(int j=0;j<dof;++j)
    {
      waypoint[j]=req.init_traj.points[i].positions[j];
    }
    waypoints.push_back(waypoint);
  }

  // store the acceleration bounds:

  VectorXd maxAcceleration(dof);
  VectorXd maxVelocity(dof);
  for(int i=0;i<dof;++i)
  {
    maxAcceleration[i]=req.max_acc[i];
    maxVelocity[i]=req.max_vel[i];
  }

  double max_deviation=req.max_dev;
  Trajectory trajectory(Path(waypoints, max_deviation), maxVelocity, maxAcceleration);
  trajectory.outputPhasePlaneTrajectory();

  // create output trajectory:
  trajectory_msgs::JointTrajectory out_traj;
  out_traj.joint_names=req.init_traj.joint_names;
  double dt=req.dt;
  if(trajectory.isValid())
  {
    double duration = trajectory.getDuration();
    res.duration=duration;
    cout << "Trajectory duration: " << duration << " s" << endl << endl;
    for(double t = 0.0; t < duration; t += dt)
    {
      // storing positions:
      trajectory_msgs::JointTrajectoryPoint pt;
      pt.positions.resize(dof);
      pt.velocities.resize(dof);
      pt.accelerations.resize(dof);
      for(int j=0;j<dof;++j)
      {
        pt.positions[j]=trajectory.getPosition(t)[j];
        pt.velocities[j]=trajectory.getVelocity(t)[j];
        // using finite differencing to get acceleration:
        if(t==0.0)
        {
          pt.accelerations[j]=(trajectory.getVelocity(t+dt)[j]-trajectory.getVelocity(t)[j])/dt;
        }
        else
        {
          pt.accelerations[j]=(trajectory.getVelocity(t)[j]-trajectory.getVelocity(t-dt)[j])/dt;
        }
      }
      out_traj.points.push_back(pt);
    }
    res.success=true;
  }
  else
  {
    cout << "Trajectory generation failed." << endl;
    res.success=false;
  }
  res.smooth_traj=out_traj;

  return true;
}

int main(int argc,char** argv)
{
  ros::init(argc,argv,"trajectory_smoothing_server");
  ros::NodeHandle n;
  ros::ServiceServer service=n.advertiseService("get_smooth_trajectory",srv_call);
  ros::spin();
  return 1;
}


/*
 Copyright 2017 Marius Appel <marius.appel@uni-muenster.de>

 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.
*/

/*
 *
 * eo_reduce  --t_size="10980 10980" --t_srs="EPSG:32634" --t_win="600000 709800 5790240 5900040" -f "mean" -o /home/marius/Desktop/test_reduce.tif /home/marius/github/eo_webtools/sentinel_webview/data/S2A_MSIL1C_20161213T093402_N0204_R136_T34UFD_20161213T093403.vrt /home/marius/github/eo_webtools/sentinel_webview/data/S2A_MSIL1C_20171002T094031_N0205_R036_T34UFD_20171002T094035.vrt
 * eo_reduce -f "mean" --t_win="799500 809500 789000 799000" --t_size="334 334" --t_srs="EPSG:32636" -o /home/marius/Desktop/test_reduce_landsat.tif /home/marius/github/eo_webtools/eo_reduce/data/Landsat/
 *
 *
 */

/*
 * TODO:
 * - add resampling argument
 * - implement band selection
 * - add memorylimit argument
 */

#include <iostream>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <gdal_utils.h>
#include <gdal_priv.h>
#include <ogr_spatialref.h>
#include <thread>
#include <queue>
#include "timer.h"

#define BLOCK_SIZE 256
#define MEM_HARD_LIMIT_MB 512

typedef struct {
    double left;
    double right;
    double top;
    double bottom;
    std::string srs;
    uint32_t width;
    uint32_t height;
} space_view;

struct arg_input {
    std::string output_path; // output file (TIF + VRT?)
    std::vector<std::string> input_image;
    bool verbose;
    std::vector<uint16_t> b; // bands
    uint16_t m;
    space_view sview;
    double (*func)(double*, uint32_t, uint32_t);
} args;

template<typename T>
struct bounds_2d {
    T left, bottom, top, right;
};

template<typename T>
struct coords_2d {
    T x, y;
};

double median(double *buf, uint32_t delta, uint32_t n) {

    if (n==0) return nan("");

    std::vector<double> v;
    for (uint32_t i=1; i<n; ++i)
        v.push_back(buf[i * delta]);

    std::sort(v.begin(), v.end());
    if (v.size() % 2 == 0) {
        return (v[v.size() / 2 - 1] + v[v.size() / 2]) / 2.0;
    }
    else return v[v.size() / 2];

//    std::priority_queue<double, std::priority_queue<double>::container_type, std::less<double> > left;
//    std::priority_queue<double, std::priority_queue<double>::container_type, std::greater<double> > right;
//
//    if (n==0) return nan("");
//    left.push(buf[0]);
//
//    for (uint32_t i=1; i<n; ++i) {
//        double el = buf[i * delta];
//
//        if (left.size() > right.size()) {
//            double l=left.top();
//            if (el < l) {
//                left.pop();
//                right.push(l);
//                left.push(el);
//            }
//            else right.push(el);
//        }
//        else {
//            double r = right.top();
//            if (el > r) {
//                right.pop();
//                left.push(r);
//                right.push(el);
//            }
//            else {
//                left.push(el);
//            }
//        }
//        assert(left.size() + right.size() == i+1);
//    }
//    if (right.size() == left.size()) return (right.top() + left.top()) / 2.0;
//    return left.top();
}

inline double mean(double *buf, uint32_t delta, uint32_t n) {
    if (n==0) return nan("");
    uint32_t count=0;
    double sum = 0.0;
    for (uint32_t i=0; i<n; ++i) {
        if (std::isfinite(buf[i * delta])) {
            ++count;
            sum += buf[i * delta];
        }
    }
    return sum / count;
}

double min(double *buf, uint32_t delta, uint32_t n) {
    if (n==0) return nan("");
    double min = DBL_MAX;
    for (uint32_t i=0; i<n; ++i) {
        if (std::isfinite(buf[i * delta])) {
            if (buf[i * delta] < min) min = buf[i * delta];
        }
    }
    return std::isfinite(min)? min : nan("");
}

double max(double *buf, uint32_t delta, uint32_t n) {
    if (n==0) return nan("");
    double max = -DBL_MAX;
    for (uint32_t i=0; i<n; ++i) {
        if (std::isfinite(buf[i * delta])) {
            if (buf[i * delta] > max) max = buf[i * delta];
        }
    }
    return std::isfinite(max)? max : nan("");
}

bool intersects(bounds_2d<double> a, bounds_2d<double> b) {
    return (
        a.right >= b.left &&
        a.left <= b.right &&
        a.top >= b.bottom &&
        a.bottom <= b.top);
}

inline uint32_t buf_index(uint32_t ii, uint16_t ib, uint32_t ix, uint32_t iy,
                          uint32_t ni, uint16_t nb, uint32_t nx, uint32_t ny) {
    //return ii * (nb * nx * ny) + ib * (nx * ny) + iy * (nx) + ix * 1; // assuming order x fastest, y 2nd, band 3rd, image 4th
    return ii * (nb * nx * ny) + iy * (nx * nb) + ix * (nb) + ib * 1; // assuming order band fastest, x 2nd, y 3rd, image 4th
}

// returns the gap in the buffer between pixel values from different images but equal x, y, and band
inline uint32_t buf_gap_image(uint32_t ni, uint16_t nb, uint32_t nx, uint32_t ny) {
    // this should return ni;
    return buf_index(1,0,0,0,ni,nb,nx,ny) - buf_index(0,0,0,0,ni,nb,nx,ny);
}

// returns the gap in the buffer between pixel values from different bands but equal x, y, and image
inline uint32_t buf_gap_band(uint32_t ni, uint16_t nb, uint32_t nx, uint32_t ny) {
    return buf_index(0,1,0,0,ni,nb,nx,ny) - buf_index(0,0,0,0,ni,nb,nx,ny);
}

inline uint32_t buf_gap_x(uint32_t ni, uint16_t nb, uint32_t nx, uint32_t ny) {
    return buf_index(0,0,1,0,ni,nb,nx,ny) - buf_index(0,0,0,0,ni,nb,nx,ny);
}

inline uint32_t buf_gap_y(uint32_t ni, uint16_t nb, uint32_t nx, uint32_t ny) {
    return buf_index(0,0,0,1,ni,nb,nx,ny) - buf_index(0,0,0,0,ni,nb,nx,ny);
}

int run() {

    // 1. Extract spatial extent of all input datasets

    // 2. Create empty output dataset (GTiff?)

    // 3. Over all blocks of the output dataset (e.g. 256x256),

    // 3.1 gather relevant scenes

    // 3.2 Call gdalwarp and read data to MEM datasets

    // 3.3 call RasterIO for all scenes

    // 3.4. Apply reduce function over returned arrays over all bands

    // 3.5. write output block of input image

    // 4. [Create overviews]

    // 5. [Create VRT]

    // further ideas:
    // - cache chunks of input data
    // - hold pool for open input datasets to prevent out of open file limits from OS

    // ASSUMPTIONS
    // a) All images have the same number of bands and bands from different datasets with the same band number have the same data type
    // b) Output datatype is double for all bands

    OGRSpatialReference srs_out;
    srs_out.SetFromUserInput(args.sview.srs.c_str());
    double affine_out[6];
    affine_out[0] = args.sview.left;
    affine_out[3] = args.sview.top;
    affine_out[1] = (args.sview.right - args.sview.left) / (double)args.sview.width;
    affine_out[5] = -(args.sview.top - args.sview.bottom) / (double)args.sview.height;
    affine_out[2] = 0;
    affine_out[4] = 0;

    GDALDriverH mem_driver = GDALGetDriverByName("MEM");
    GDALDriverH gtiff_driver = GDALGetDriverByName("GTiff");

    uint16_t n_bands = 0;
    int* band_list = NULL;

    std::vector<bounds_2d<double>> extent;
    for (uint32_t i=0; i<args.input_image.size(); ++i) {
        GDALDataset *gdal_in = (GDALDataset *) GDALOpenShared(args.input_image[i].c_str(), GA_ReadOnly);
        if (gdal_in == NULL) {
            std::cout << "ERROR: GDALOpen failed for '" << args.input_image[i] << "'" <<  std::endl;
            return 1;
        }

        if (i==0) {
            n_bands = gdal_in->GetRasterCount(); // ASSUMPTION a)
            band_list = (int*)calloc(n_bands, sizeof(int));
            for (uint16_t ib=0; ib<n_bands; ++ib) band_list[ib] = ib+1;
        }

        OGRSpatialReference srs_in(gdal_in->GetProjectionRef()); // Does this work?
        double affine_in[6];
        gdal_in->GetGeoTransform(affine_in);

        OGRCoordinateTransformation *coord_transform = OGRCreateCoordinateTransformation( &srs_in, &srs_out );

        double x[4] = {affine_in[0],
                          affine_in[0],
                          affine_in[0] + affine_in[1] * gdal_in->GetRasterXSize() + affine_in[2] * gdal_in->GetRasterYSize(),
                          affine_in[0] + affine_in[1] * gdal_in->GetRasterXSize() + affine_in[2] * gdal_in->GetRasterYSize()};
        double y[4] = {affine_in[3],
                          affine_in[3] + affine_in[4] * gdal_in->GetRasterXSize() + affine_in[5] * gdal_in->GetRasterYSize(),
                          affine_in[3],
                          affine_in[3] + affine_in[4] * gdal_in->GetRasterXSize() + affine_in[5] * gdal_in->GetRasterYSize()};

        if( coord_transform == NULL || !coord_transform->Transform( 4, x, y )) {
            std::cout << "ERROR: coordinate transformation failed" << std::endl;
            return 1;
        }

        double xmin = DBL_MAX;
        double ymin = DBL_MAX;
        double xmax = -DBL_MAX;
        double ymax = -DBL_MAX;
        for (uint8_t k=0; k<4; ++k) {
            if (x[k] < xmin ) xmin = x[k];
            if (y[k] < ymin ) ymin = y[k];
            if (x[k] > xmax ) xmax = x[k];
            if (y[k] > ymax ) ymax = y[k];
        }

        bounds_2d<double> in_extent;
        in_extent.left = xmin;
        in_extent.right = xmax;
        in_extent.top = ymax;
        in_extent.bottom = ymin;

        extent.push_back(in_extent);

        // TODO: derive spatial index?
        // STRTree envelope -> int (index in vector)

        GDALClose((GDALDatasetH)gdal_in);
    }

    CPLStringList out_co(NULL);
    out_co.AddNameValue("TILED","YES");
    out_co.AddNameValue("BLOCKXSIZE","256");
    out_co.AddNameValue("BLOCKYSIZE","256");
    //out_co.AddNameValue("COMPRESS","DEFLATE");

    GDALDatasetH gdal_out = GDALCreate(gtiff_driver, args.output_path.c_str(), args.sview.width, args.sview.height, n_bands, GDT_Float64, out_co.List());
    char *wkt_out;
    srs_out.exportToWkt(&wkt_out);
    GDALSetProjection(gdal_out, wkt_out);
    GDALSetGeoTransform(gdal_out,affine_out);

    uint32_t n_blocks_x = (uint32_t)std::ceil((double)args.sview.width / (double)BLOCK_SIZE);
    uint32_t n_blocks_y = (uint32_t)std::ceil((double)args.sview.height / (double)BLOCK_SIZE);

    for (uint32_t ix=0; ix < n_blocks_x; ++ix) {
        for (uint32_t iy=0; iy < n_blocks_y; ++iy) {

            // Get extent of block
            bounds_2d<double> block_extent;
            block_extent.bottom = affine_out[3] + affine_out[5] * fmin((iy+1)*BLOCK_SIZE, args.sview.height);
            block_extent.top    = affine_out[3] + affine_out[5] * (iy)*BLOCK_SIZE;
            block_extent.right = affine_out[0] + affine_out[1] * fmin((ix+1)*BLOCK_SIZE, args.sview.width) ;
            block_extent.left = affine_out[0] + affine_out[1] * (ix)*BLOCK_SIZE;

            bounds_2d<uint32_t>block_extent_int; // direction of axes is not important here
            block_extent_int.left =  ix * BLOCK_SIZE;
            block_extent_int.top  =  iy * BLOCK_SIZE;
            block_extent_int.right =  fmin(block_extent_int.left + BLOCK_SIZE, args.sview.width);
            block_extent_int.bottom =  fmin(block_extent_int.top + BLOCK_SIZE, args.sview.height);

            // this is the actual width and height of the current block, which potentially has been cropped to
            // the extent of the output image
            uint32_t block_width =std::abs(block_extent_int.right - block_extent_int.left);
            uint32_t block_height = std::abs(block_extent_int.bottom - block_extent_int.top);

            // Gather all scenes which intersect with the current block
            // TODO: use spatial index to make filtering more efficient
            std::vector<uint32_t> image_ids;
            for (uint32_t ii=0; ii<args.input_image.size(); ++ii) {
                if (intersects(block_extent,extent[ii])) {
                    image_ids.push_back(ii);
                   // std::cout << "Block (" << ix << "," << iy << ") intersects with image " << ii << "."  << std::endl;
                }
               // else std::cout << "Block (" << ix << "," << iy  << ") DOES NOT intersect with image " << ii << "."  << std::endl;
                //print_extent(block_extent);
               // print_extent(extent[ii]);
            }

            // allocate memory to read data from all bands
            uint64_t buf_size_in = block_width*block_height*image_ids.size() * n_bands * sizeof(double);
            if (buf_size_in / (1024 * 1024) > MEM_HARD_LIMIT_MB) {
                std::cout << "ERROR: reached memory limit of " << MEM_HARD_LIMIT_MB << " MiB, please choose smaller block sizes." <<  std::endl;
                return 1;
            }
            double* buf_in = (double*)calloc(block_width*block_height*image_ids.size() * n_bands, sizeof(double));

            // TODO not needed?
            for (uint32_t j=0; j<block_width*block_height*image_ids.size() * n_bands; ++j) {
                buf_in[j] = nan("");
            }

            // Read data for block from all scenes
            for (uint32_t ii=0; ii<image_ids.size(); ++ii) {

                GDALDataset *gdal_in = (GDALDataset *) GDALOpenShared(args.input_image[image_ids[ii]].c_str(), GA_ReadOnly);
                if (gdal_in == NULL) {
                    std::cout << "ERROR: GDALOpen failed for '" << args.input_image[image_ids[ii]] << "'" <<  std::endl;
                    return 1;
                }

                // Call GDALWarp
                GDALDatasetH gdal_warped = GDALCreate(mem_driver, "", block_width, block_height, n_bands, GDT_Float64, NULL);
                //GDALDatasetH gdal_warped = GDALCreate(gtiff_driver, ("/home/marius/Desktop/" + std::to_string(ii) + ".tif" ).c_str(), block_width, block_height, n_bands, GDT_Float64, NULL);
                for (uint16_t ib=0; ib<n_bands; ++ib) {
                    GDALRasterBandH b = GDALGetRasterBand(gdal_warped, ib+1);
                    GDALSetRasterNoDataValue(b, nan(""));
                    //GDALFillRaster(b,nan(""), 0);
                }

                GDALSetProjection(gdal_warped, wkt_out);
                double affine[6];
                affine[0] = block_extent.left;
                affine[3] = block_extent.top;
                affine[1] = (block_extent.right - block_extent.left) / ((double) block_width);
                affine[5] = (block_extent.bottom - block_extent.top) / ((double) block_height);
                affine[2] = 0.0;
                affine[4] = 0.0;
                GDALSetGeoTransform(gdal_warped, affine);

                CPLStringList warp_args(NULL);
                warp_args.AddString("-of");
                warp_args.AddString("MEM");
                warp_args.AddString("-t_srs");
                warp_args.AddString(args.sview.srs.c_str());

                warp_args.AddString("-dstnodata");
                warp_args.AddString("nafn");

                warp_args.AddString("-te"); // xmin ymin xmax ymax
                warp_args.AddString(std::to_string(block_extent.left).c_str());
                warp_args.AddString(std::to_string(block_extent.bottom).c_str());
                warp_args.AddString(std::to_string(block_extent.right).c_str());
                warp_args.AddString(std::to_string(block_extent.top).c_str());

                warp_args.AddString("-te_srs");
                warp_args.AddString(args.sview.srs.c_str());

                warp_args.AddString("-ts");
                warp_args.AddString(std::to_string(block_width).c_str());
                warp_args.AddString(std::to_string(block_height).c_str());

                GDALWarpAppOptions *warp_opts = GDALWarpAppOptionsNew(warp_args.List(), NULL);
                if (warp_opts == NULL) {
                    std::cout << "ERROR: Cannot create gdalwarp options." << std::endl;
                    return 1;
                }
                GDALDatasetH gdal_warpin = (GDALDatasetH) gdal_in;
                GDALWarp(NULL, gdal_warped, 1, &gdal_warpin, warp_opts, NULL);
                GDALWarpAppOptionsFree(warp_opts);

                GDALDatasetRasterIOEx(gdal_warped,GF_Read, 0, 0, block_width, block_height, &buf_in[ buf_index(ii, 0,0,0,image_ids.size(), n_bands, block_width, block_height)],
                                      block_width, block_height, GDT_Float64, n_bands, NULL, 0, 0, 0, NULL);

                GDALClose(gdal_warped);
                GDALClose((GDALDatasetH)gdal_in);
            }

            uint32_t delta_i = buf_gap_image(image_ids.size(), n_bands,block_width, block_height);
            uint32_t delta_b = buf_gap_band(image_ids.size(), n_bands,block_width, block_height);
            uint32_t delta_x = buf_gap_x(image_ids.size(), n_bands,block_width, block_height);
            uint32_t delta_y = buf_gap_y(image_ids.size(), n_bands,block_width, block_height);

            // Do the analysis
            // uint64_t buf_size_out= BLOCK_SIZE*BLOCK_SIZE * n_bands * sizeof(double);
            double* buf_out = (double*)calloc(block_width*block_height * n_bands,sizeof(double));

            // test: fill buf out with nan
            for (uint32_t j=0; j<block_width*block_height * n_bands; ++j) {
                buf_out[j] = nan("");
            }

            if (!image_ids.empty()) {
                for (uint16_t in_ib = 0; in_ib < n_bands; ++in_ib) {
                    for (uint32_t in_ix = 0; in_ix < block_width; ++in_ix) {
                        for (uint32_t in_iy = 0; in_iy < block_height; ++in_iy) {
                            uint32_t i = buf_index(0, in_ib, in_ix, in_iy, image_ids.size(), n_bands, block_width,
                                                   block_height);

                            double val = args.func(&buf_in[i], delta_i, image_ids.size());

                           // std::cout << i << " in [" << 0 << "," << block_width * block_height * image_ids.size() * n_bands << "]" << std::endl;
                            if (!std::isfinite(val)) val = nan("");
                            buf_out[buf_index(0, in_ib, in_ix, in_iy, 1, n_bands, block_width, block_height)] = val;

                        }
                    }
                    // maybe write single band already here
                }
            }

            GDALDatasetRasterIOEx(gdal_out, GF_Write, block_extent_int.left, block_extent_int.top, block_width,block_height, buf_out, block_width, block_height, GDT_Float64, n_bands, NULL, 0, 0, 0, NULL);

            free(buf_in);
            free(buf_out);
           // if (band_list != NULL) free(band_list);
        }
    }

    for (uint16_t ib=0; ib<n_bands; ++ib) {
        GDALRasterBandH b = GDALGetRasterBand(gdal_out, ib+1);
        GDALSetRasterNoDataValue(b,nan(""));
    }

    GDALClose((GDALDatasetH)gdal_out);

}

int main(int ac, char *av[]) {

    // input args: input file, output directory, output format,  zoom levels, resampling,
    namespace po = boost::program_options;
    namespace fs = boost::filesystem;

    po::options_description desc("Input arguments");
    desc.add_options()
            ("help,h", "print usage")
            ("output-file,o", po::value<std::string>(),
             "path of output file")
            ("bands,b", po::value<std::string>(),
             "Bands of the input dataset, defaults to all available bands")
            ("t_win", po::value<std::string>(), "coordinates of the target window expressed in the target reference system  with order (xmin, xmax, ymin, ymax).")
            ("t_size", po::value<std::string>(), "size of the target image in pixels with order (width, height).")
            ("t_srs", po::value<std::string>(), "target reference system as GDAL readable string.")
            ("func,f", po::value<std::string>(), "reducer function, currently supported are 'mean, 'min', 'max', 'median")
            ("memory,m", po::value<uint16_t>()->default_value(256), "memory in MB that is available for caching")
            ("verbose,v", "verbose output")
            ("input-file", po::value<std::vector<std::string>>());

    po::positional_options_description p;
    p.add("input-file", -1);
    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
            options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 0;
    }

    args.verbose = false;
    if (vm.count("verbose")) {
        args.verbose = true;
    }

    args.m = vm["memory"].as<uint16_t>();

    if (!vm.count("output-file"))
    {
        std::cout << "ERROR: missing argument output file, specify with -o \"/PATH/TO/FILE\""  << std::endl;
        return 1;
    }
    args.output_path = vm["output-file"].as<std::string>();
    if (fs::exists( args.output_path)) {
        std::cout << "INFO: already existing output file will be overwritten." << std::endl;
    }

    if (vm.count("bands"))
    {
        std::istringstream str(vm["bands"].as<std::string>());
        std::vector<std::string> tokens{std::istream_iterator<std::string>{str}, std::istream_iterator<std::string>{}};
        std::vector<uint16_t> vec;
        for (uint8_t i = 0; i < tokens.size(); ++i) {
            vec.push_back((uint16_t) std::stoi(tokens[i]));
        }
        args.b = vec;
    }
    else {
        // TODO fill bands
    }

    if (!vm.count("t_win"))
    {
        std::cout << "ERROR: no spatial target window given." << std::endl;
        return 1;
    }
    else {
        std::istringstream str(vm["t_win"].as<std::string>());
        std::vector<std::string> tokens{std::istream_iterator<std::string>{str}, std::istream_iterator<std::string>{}};
        std::vector<double> vec;
        for (uint8_t i = 0; i < tokens.size(); ++i) {
            vec.push_back((double) std::stod(tokens[i]));
        }
        if (vec.size() != 4) {
            std::cout << "ERROR: invalid --t_win option provided." << std::endl;
            return 1;
        }
        args.sview.left = vec[0];
        args.sview.right = vec[1];
        args.sview.bottom = vec[2];
        args.sview.top = vec[3];
    }

    if (!vm.count("t_size"))
    {
        std::cout << "ERROR: no spatial target size given." << std::endl;
        return 1;
    }
    else {
        std::istringstream str(vm["t_size"].as<std::string>());
        std::vector<std::string> tokens{std::istream_iterator<std::string>{str}, std::istream_iterator<std::string>{}};
        std::vector<uint32_t> vec;
        for (uint8_t i = 0; i < tokens.size(); ++i) {
            vec.push_back((uint32_t) std::stoi(tokens[i]));
        }
        if (vec.size() != 2) {
            std::cout << "ERROR: invalid --t_size option provided." << std::endl;
            return 1;
        }
        args.sview.width = vec[0];
        args.sview.height = vec[1];
    }

    if (!vm.count("t_srs"))
    {
        std::cout << "ERROR: no target spatial reference system given." << std::endl;
        return 1;
    }
    else {
        args.sview.srs = vm["t_srs"].as<std::string>();
    }

    if (!vm.count("func"))
    {
        std::cout << "ERROR: no reducer function provided." << std::endl;
        return 1;
    }
    else {
        std::string func = vm["func"].as<std::string>();
        std::transform(func.begin(),func.end(),func.begin(),::tolower);
        if(func.compare("mean") == 0 || func.compare("avg") == 0) args.func = &mean;
        else if(func.compare("median") == 0) args.func = &median;
        else if(func.compare("min") == 0) args.func = &min;
        else if(func.compare("max") == 0) args.func = &max;
        else {
            std::cout << "ERROR: invalid reducer function given." << std::endl;
            return 1;
        }
    }

    if (!vm.count("input-file")) {
        std::cout << "ERROR: missing input files or input directory" << std::endl;
        return 1;
    }

    args.input_image = vm["input-file"].as<std::vector<std::string>>();

    if (vm.count("input-file") == 1) {
        fs::path p(args.input_image[0]);

        if (fs::is_directory(p)) {
            args.input_image.clear();
            for(auto& entry : boost::make_iterator_range(fs::directory_iterator(p), {})) {
                args.input_image.push_back(entry.path().string());
            }
            std::cout << "INFO: Added " << args.input_image.size() << " images to input collection." << std::endl;
        }
    }

    GDALAllRegister();
    CPLSetConfigOption("GDAL_MAX_DATASET_POOL_SIZE", "400");
    //CPLSetConfigOption("GDAL_RASTERIO_RESAMPLING", "NEAR"); // does this work? if yes, even for RasterIO (non Ex) and gdalwarp?
    GDALSetCacheMax(1024 * 1024 * args.m);

    timer t;
    t.start();

    run();

    std::cout << "DONE (" << t.time() << "s) ." << std::endl;

    return 0;
}


//==============================================================================
//         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_FAST_DIVIDES_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_FAST_DIVIDES_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief fast_divides generic tag

     Represents the fast_divides function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct fast_divides_ : ext::elementwise_<fast_divides_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<fast_divides_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_fast_divides_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::fast_divides_, Site> dispatching_fast_divides_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::fast_divides_, Site>();
   }
   template<class... Args>
   struct impl_fast_divides_;
  }
  /*!

    @par Semantic:

    For every parameters of types respectively T0, T1:

    @code
    T0 r = fast_divides(a0,a1);
    @endcode

    is similar to:

    @code
    T0 r = a0*fast_rec(a1);
    @endcode

    @par Alias:
    @c fast_div, @c fast_rdiv

    @see  @funcref{divides}, @funcref{rec}, @funcref{fast_rec}, @funcref{divs}, @funcref{divfloor},
    @funcref{divceil}, @funcref{divround}, @funcref{divround2even}, @funcref{divfix}
    @param a0

    @param a1

    @return a value of the same type as the second parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fast_divides_ , fast_divides  , 2)

  /// INTERNAL ONLY
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fast_divides_ , fast_div      , 2)

  /// INTERNAL ONLY
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fast_divides_ , fast_rdiv     , 2)
} }

#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2004, 2005 Trustees of Indiana University
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek,
//          Doug Gregor, D. Kevin McGrath
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================//
#ifndef BOOST_GRAPH_KING_HPP
#define BOOST_GRAPH_KING_HPP

#include <boost/config.hpp>
#include <boost/graph/detail/sparse_ordering.hpp>
#include <boost/graph/graph_utility.hpp>

/*
  King Algorithm for matrix reordering
*/

namespace boost {
  namespace detail {
    template<typename OutputIterator, typename Buffer, typename Compare,
             typename PseudoDegreeMap, typename VecMap, typename VertexIndexMap>
    class bfs_king_visitor:public default_bfs_visitor
    {
    public:
      bfs_king_visitor(OutputIterator *iter, Buffer *b, Compare compare,
                       PseudoDegreeMap deg, std::vector<int> loc, VecMap color,
                       VertexIndexMap vertices):
        permutation(iter), Qptr(b), degree(deg), comp(compare),
        Qlocation(loc), colors(color), vertex_map(vertices) { }

      template <typename Vertex, typename Graph>
      void finish_vertex(Vertex, Graph& g) {
        typename graph_traits<Graph>::out_edge_iterator ei, ei_end;
        Vertex v, w;

        typedef typename std::deque<Vertex>::reverse_iterator reverse_iterator;

        reverse_iterator rend = Qptr->rend()-index_begin;
        reverse_iterator rbegin = Qptr->rbegin();

        //heap the vertices already there
        std::make_heap(rbegin, rend, boost::bind<bool>(comp, _2, _1));

        unsigned i = 0;

        for(i = index_begin; i != Qptr->size(); ++i){
          colors[get(vertex_map, (*Qptr)[i])] = 1;
          Qlocation[get(vertex_map, (*Qptr)[i])] = i;
        }

        i = 0;

        for( ; rbegin != rend; rend--){
          percolate_down<Vertex>(i);
          w = (*Qptr)[index_begin+i];
          for (boost::tie(ei, ei_end) = out_edges(w, g); ei != ei_end; ++ei) {
            v = target(*ei, g);
            put(degree, v, get(degree, v) - 1);

            if (colors[get(vertex_map, v)] == 1) {
              percolate_up<Vertex>(get(vertex_map, v), i);
            }
          }

          colors[get(vertex_map, w)] = 0;
          i++;
        }
      }

      template <typename Vertex, typename Graph>
      void examine_vertex(Vertex u, const Graph&) {

        *(*permutation)++ = u;
        index_begin = Qptr->size();

      }
    protected:

      //this function replaces pop_heap, and tracks state information
      template <typename Vertex>
      void percolate_down(int offset){
        int heap_last = index_begin + offset;
        int heap_first = Qptr->size() - 1;

        //pop_heap functionality:
        //swap first, last
        std::swap((*Qptr)[heap_last], (*Qptr)[heap_first]);

        //swap in the location queue
        std::swap(Qlocation[heap_first], Qlocation[heap_last]);

        //set drifter, children
        int drifter = heap_first;
        int drifter_heap = Qptr->size() - drifter;

        int right_child_heap = drifter_heap * 2 + 1;
        int right_child = Qptr->size() - right_child_heap;

        int left_child_heap = drifter_heap * 2;
        int left_child = Qptr->size() - left_child_heap;

        //check that we are staying in the heap
        bool valid = (right_child < heap_last) ? false : true;

        //pick smallest child of drifter, and keep in mind there might only be left child
        int smallest_child = (valid && get(degree, (*Qptr)[left_child]) > get(degree,(*Qptr)[right_child])) ?
          right_child : left_child;

        while(valid && smallest_child < heap_last && comp((*Qptr)[drifter], (*Qptr)[smallest_child])){

          //if smallest child smaller than drifter, swap them
          std::swap((*Qptr)[smallest_child], (*Qptr)[drifter]);
          std::swap(Qlocation[drifter], Qlocation[smallest_child]);

          //update the values, run again, as necessary
          drifter = smallest_child;
          drifter_heap = Qptr->size() - drifter;

          right_child_heap = drifter_heap * 2 + 1;
          right_child = Qptr->size() - right_child_heap;

          left_child_heap = drifter_heap * 2;
          left_child = Qptr->size() - left_child_heap;

          valid = (right_child < heap_last) ? false : true;

          smallest_child = (valid && get(degree, (*Qptr)[left_child]) > get(degree,(*Qptr)[right_child])) ?
            right_child : left_child;
        }

      }

      // this is like percolate down, but we always compare against the
      // parent, as there is only a single choice
      template <typename Vertex>
      void percolate_up(int vertex, int offset){

        int child_location = Qlocation[vertex];
        int heap_child_location = Qptr->size() - child_location;
        int heap_parent_location = (int)(heap_child_location/2);
        unsigned parent_location = Qptr->size() - heap_parent_location;

        bool valid = (heap_parent_location != 0 && child_location > index_begin + offset &&
                      parent_location < Qptr->size());

        while(valid && comp((*Qptr)[child_location], (*Qptr)[parent_location])){

          //swap in the heap
          std::swap((*Qptr)[child_location], (*Qptr)[parent_location]);

          //swap in the location queue
          std::swap(Qlocation[child_location], Qlocation[parent_location]);

          child_location = parent_location;
          heap_child_location = heap_parent_location;
          heap_parent_location = (int)(heap_child_location/2);
          parent_location = Qptr->size() - heap_parent_location;
          valid = (heap_parent_location != 0 && child_location > index_begin + offset);
        }
      }

      OutputIterator *permutation;
      int index_begin;
      Buffer *Qptr;
      PseudoDegreeMap degree;
      Compare comp;
      std::vector<int> Qlocation;
      VecMap colors;
      VertexIndexMap vertex_map;
    };

  } // namespace detail

  template<class Graph, class OutputIterator, class ColorMap, class DegreeMap,
           typename VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& g,
                std::deque< typename graph_traits<Graph>::vertex_descriptor >
                  vertex_queue,
                OutputIterator permutation,
                ColorMap color, DegreeMap degree,
                VertexIndexMap index_map)
  {
    typedef typename property_traits<DegreeMap>::value_type ds_type;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef iterator_property_map<typename std::vector<ds_type>::iterator, VertexIndexMap, ds_type, ds_type&> PseudoDegreeMap;
    typedef indirect_cmp<PseudoDegreeMap, std::less<ds_type> > Compare;
    typedef typename boost::sparse::sparse_ordering_queue<Vertex> queue;
    typedef typename detail::bfs_king_visitor<OutputIterator, queue, Compare,
      PseudoDegreeMap, std::vector<int>, VertexIndexMap > Visitor;
    typedef typename graph_traits<Graph>::vertices_size_type
      vertices_size_type;
    std::vector<ds_type> pseudo_degree_vec(num_vertices(g));
    PseudoDegreeMap pseudo_degree(pseudo_degree_vec.begin(), index_map);

    typename graph_traits<Graph>::vertex_iterator ui, ui_end;
    queue Q;
    // Copy degree to pseudo_degree
    // initialize the color map
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui){
      put(pseudo_degree, *ui, get(degree, *ui));
      put(color, *ui, Color::white());
    }

    Compare comp(pseudo_degree);
    std::vector<int> colors(num_vertices(g));

    for(vertices_size_type i = 0; i < num_vertices(g); i++)
      colors[i] = 0;

    std::vector<int> loc(num_vertices(g));

    //create the visitor
    Visitor vis(&permutation, &Q, comp, pseudo_degree, loc, colors, index_map);

    while( !vertex_queue.empty() ) {
      Vertex s = vertex_queue.front();
      vertex_queue.pop_front();

      //call BFS with visitor
      breadth_first_visit(g, s, Q, vis, color);
    }

    return permutation;
  }

  // This is the case where only a single starting vertex is supplied.
  template <class Graph, class OutputIterator,
            class ColorMap, class DegreeMap, typename VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& g,
                typename graph_traits<Graph>::vertex_descriptor s,
                OutputIterator permutation,
                ColorMap color, DegreeMap degree, VertexIndexMap index_map)
  {

    std::deque< typename graph_traits<Graph>::vertex_descriptor > vertex_queue;
    vertex_queue.push_front( s );
    return king_ordering(g, vertex_queue, permutation, color, degree,
                         index_map);
  }

  template < class Graph, class OutputIterator,
             class ColorMap, class DegreeMap, class VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& G, OutputIterator permutation,
                ColorMap color, DegreeMap degree, VertexIndexMap index_map)
  {
    if (has_no_vertices(G))
      return permutation;

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;

    std::deque<Vertex>      vertex_queue;

    // Mark everything white
    BGL_FORALL_VERTICES_T(v, G, Graph) put(color, v, Color::white());

    // Find one vertex from each connected component
    BGL_FORALL_VERTICES_T(v, G, Graph) {
      if (get(color, v) == Color::white()) {
        depth_first_visit(G, v, dfs_visitor<>(), color);
        vertex_queue.push_back(v);
      }
    }

    // Find starting nodes for all vertices
    // TBD: How to do this with a directed graph?
    for (typename std::deque<Vertex>::iterator i = vertex_queue.begin();
         i != vertex_queue.end(); ++i)
      *i = find_starting_node(G, *i, color, degree);

    return king_ordering(G, vertex_queue, permutation, color, degree,
                         index_map);
  }

  template<typename Graph, typename OutputIterator, typename VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& G, OutputIterator permutation,
                VertexIndexMap index_map)
  {
    if (has_no_vertices(G))
      return permutation;

    std::vector<default_color_type> colors(num_vertices(G));
    return king_ordering(G, permutation,
                         make_iterator_property_map(&colors[0], index_map,
                                                    colors[0]),
                         make_out_degree_map(G), index_map);
  }

  template<typename Graph, typename OutputIterator>
  inline OutputIterator
  king_ordering(const Graph& G, OutputIterator permutation)
  { return king_ordering(G, permutation, get(vertex_index, G)); }

} // namespace boost

#endif // BOOST_GRAPH_KING_HPP


#pragma once

#include <memory>
#include <functional>
#include <optional>

#include <boost/container_hash/hash.hpp>

namespace miplib {

struct Solver;

namespace detail {
struct IVar;
}  // namespace detail

struct Var
{
  enum class Type { Continuous, Binary, Integer };

  Var(
    Solver const& solver,
    Var::Type const& type,
    std::optional<double> const& lb = std::nullopt,
    std::optional<double> const& ub = std::nullopt,
    std::optional<std::string> const& name = std::nullopt
  );

  Var(Solver const& solver, Var::Type const& type, std::string const& name);

  Var(Var const& v) = default;

  bool is_same(Var const& v1) const;

  bool is_lex_less(Var const& v1) const;

  std::string id() const;

  std::optional<std::string> name() const;
  void set_name(std::string const& new_name);

  double value() const;

  template<class T> T value_as() const;

  Type type() const;

  Solver const& solver() const;

  double lb() const;
  double ub() const;

  void set_lb(double new_lb);
  void set_ub(double new_ub);

  void set_hint(double v);

  std::shared_ptr<detail::IVar> p_impl;

  private:
  friend struct std::hash<Var>;
  friend struct boost::hash<Var>;
  friend struct std::less<Var>;
  friend struct GurobiSolver;
  friend struct ScipSolver;
  friend struct LpsolveSolver;
  friend struct ScipCurrentStateHandle;
  friend struct ScipConstraintHandler;
  friend struct GurobiCurrentStateHandle;
};

template<class T>
T Var::value_as() const
{
  if (std::is_integral<T>::value)
  {
    return T(std::round(value()));
  }
  else
  return T(value());
}

std::ostream& operator<<(std::ostream& os, Var const& v);

namespace detail {

struct IVar
{
  virtual double value() const = 0;

  virtual Var::Type type() const = 0;

  virtual std::optional<std::string> name() const = 0;
  virtual void set_name(std::string const& new_name) = 0;

  virtual Solver const& solver() const = 0;

  virtual double lb() const = 0;
  virtual double ub() const = 0;

  virtual void set_lb(double new_lb) = 0;
  virtual void set_ub(double new_ub) = 0;

  virtual void set_hint(double v) = 0;
};

}  // namespace detail

}  // namespace miplib

// Default hash and comparison functions for Var.
namespace std {
template<>
struct hash<miplib::Var>
{
  std::size_t operator()(miplib::Var const& t) const noexcept
  {
    return std::hash<std::shared_ptr<miplib::detail::IVar>>()(t.p_impl);
  }
};

template<>
struct equal_to<miplib::Var>
{
  bool operator()(miplib::Var const& v1, miplib::Var const& v2) const noexcept
  {
    return v1.is_same(v2);
  }
};

template<>
struct less<miplib::Var>
{
  bool operator()(miplib::Var const& v1, miplib::Var const& v2) const noexcept
  {
    return v1.p_impl < v2.p_impl;
  }
};

}  // namespace std

// Default hash function for Var.
namespace boost {
template<class>
struct hash;
template<>
struct hash<miplib::Var>
{
  std::size_t operator()(miplib::Var const& t) const noexcept
  {
    return std::hash<std::shared_ptr<miplib::detail::IVar>>()(t.p_impl);
  }
};
}  // namespace boost


#include <boost/graph/distributed/dehne_gotz_min_spanning_tree.hpp>


#include <vector>
#include <string>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/net.hpp"
#include "caffe/FRCNN/util/frcnn_param.hpp"
#include "caffe/FRCNN/util/frcnn_helper.hpp"

namespace FRCNN_API{

using std::vector;
using caffe::Blob;
using caffe::Net;
using caffe::Frcnn::FrcnnParam;
using caffe::Frcnn::Point4f;
using caffe::Frcnn::BBox;

class Rpn_Det {
public:
  Rpn_Det(std::string &proto_file, std::string &model_file){
    Set_Model(proto_file, model_file);
  }
  void Set_Model(std::string &proto_file, std::string &model_file);
  void predict(const cv::Mat &img_in, vector<BBox<float> > &results);
private:
  void preprocess(const cv::Mat &img_in, const int blob_idx);
  void preprocess(const vector<float> &data, const int blob_idx);
  vector<boost::shared_ptr<Blob<float> > > predict(const vector<std::string> blob_names);
  boost::shared_ptr<Net<float> > net_;
  float mean_[3];
};

}


#include <geometry_msgs/PoseStamped.h>
#include <kr_mav_msgs/SO3Command.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>
#include <tf/transform_datatypes.h>

#include <Eigen/Geometry>

class SO3CmdToMavros : public nodelet::Nodelet
{
 public:
  void onInit(void);

 private:
  void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg);
  void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);
  void imu_callback(const sensor_msgs::Imu::ConstPtr &pose);

  bool odom_set_, imu_set_, so3_cmd_set_;
  Eigen::Quaterniond odom_q_, imu_q_;
  double kf_, lin_cof_a_, lin_int_b_;
  int num_props_;

  ros::Publisher attitude_raw_pub_;
  ros::Publisher odom_pose_pub_;  // For sending PoseStamped to firmware

  ros::Subscriber so3_cmd_sub_;
  ros::Subscriber odom_sub_;
  ros::Subscriber imu_sub_;

  double so3_cmd_timeout_;
  ros::Time last_so3_cmd_time_;
  kr_mav_msgs::SO3Command last_so3_cmd_;
};

void SO3CmdToMavros::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
{
  odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
                               odom->pose.pose.orientation.y, odom->pose.pose.orientation.z);
  odom_set_ = true;

  // Publish PoseStamped for mavros vision_pose plugin
  auto odom_pose_msg = boost::make_shared<geometry_msgs::PoseStamped>();
  odom_pose_msg->header = odom->header;
  odom_pose_msg->pose = odom->pose.pose;
  odom_pose_pub_.publish(odom_pose_msg);
}

void SO3CmdToMavros::imu_callback(const sensor_msgs::Imu::ConstPtr &pose)
{
  imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z);
  imu_set_ = true;

  if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_))
  {
    ROS_INFO("so3_cmd timeout. %f seconds since last command", (ros::Time::now() - last_so3_cmd_time_).toSec());
    const auto last_so3_cmd_ptr = boost::make_shared<kr_mav_msgs::SO3Command>(last_so3_cmd_);

    so3_cmd_callback(last_so3_cmd_ptr);
  }
}

void SO3CmdToMavros::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg)
{
  // both imu_q_ and odom_q_ would be uninitialized if not set
  if(!imu_set_)
  {
    ROS_WARN("Did not receive any imu messages from %s", imu_sub_.getTopic().c_str());
    return;
  }

  if(!odom_set_)
  {
    ROS_WARN("Did not receive any odom messages from %s", odom_sub_.getTopic().c_str());
    return;
  }

  // grab desired forces and rotation from so3
  const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z);

  const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);

  // convert to tf::Quaternion
  tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w());
  tf::Quaternion odom_tf = tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w());

  // extract RPY's
  double imu_roll, imu_pitch, imu_yaw;
  double odom_roll, odom_pitch, odom_yaw;
  tf::Matrix3x3(imu_tf).getRPY(imu_roll, imu_pitch, imu_yaw);
  tf::Matrix3x3(odom_tf).getRPY(odom_roll, odom_pitch, odom_yaw);

  // create only yaw tf:Quaternions
  tf::Quaternion imu_tf_yaw;
  tf::Quaternion odom_tf_yaw;
  imu_tf_yaw.setRPY(0.0, 0.0, imu_yaw);
  odom_tf_yaw.setRPY(0.0, 0.0, odom_yaw);
  const tf::Quaternion tf_imu_odom_yaw = imu_tf_yaw * odom_tf_yaw.inverse();

  // transform!
  const Eigen::Quaterniond q_des_transformed =
      Eigen::Quaterniond(tf_imu_odom_yaw.w(), tf_imu_odom_yaw.x(), tf_imu_odom_yaw.y(), tf_imu_odom_yaw.z()) * q_des;

  // check psi for stability
  const Eigen::Matrix3d R_des(q_des);
  const Eigen::Matrix3d R_cur(odom_q_);

  const float Psi = 0.5f * (3.0f - (R_des(0, 0) * R_cur(0, 0) + R_des(1, 0) * R_cur(1, 0) + R_des(2, 0) * R_cur(2, 0) +
                                    R_des(0, 1) * R_cur(0, 1) + R_des(1, 1) * R_cur(1, 1) + R_des(2, 1) * R_cur(2, 1) +
                                    R_des(0, 2) * R_cur(0, 2) + R_des(1, 2) * R_cur(1, 2) + R_des(2, 2) * R_cur(2, 2)));

  if(Psi > 1.0f)  // Position control stability guaranteed only when Psi < 1
  {
    ROS_WARN_THROTTLE(1, "Psi > 1.0, orientation error is too large!");
  }

  double throttle = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2);

  // Scale force to individual rotor velocities (rad/s).
  throttle = std::sqrt(throttle / num_props_ / kf_);

  // Scaling from rotor velocity (rad/s) to att_throttle for pixhawk
  throttle = lin_cof_a_ * throttle + lin_int_b_;

  // failsafe for the error in traj_gen that can lead to nan values
  //prevents throttle from being sent to 1 if it is nan.
  if (isnan(throttle))
  {
    throttle = 0.0;
  }

  // clamp from 0.0 to 1.0
  throttle = std::min(1.0, throttle);
  throttle = std::max(0.0, throttle);

  if(!msg->aux.enable_motors)
    throttle = 0;

  // publish messages
  auto setpoint_msg = boost::make_shared<mavros_msgs::AttitudeTarget>();
  setpoint_msg->header = msg->header;
  setpoint_msg->type_mask = 0;
  setpoint_msg->orientation.w = q_des_transformed.w();
  setpoint_msg->orientation.x = q_des_transformed.x();
  setpoint_msg->orientation.y = q_des_transformed.y();
  setpoint_msg->orientation.z = q_des_transformed.z();
  setpoint_msg->body_rate.x = msg->angular_velocity.x;
  setpoint_msg->body_rate.y = msg->angular_velocity.y;
  setpoint_msg->body_rate.z = msg->angular_velocity.z;
  setpoint_msg->thrust = throttle;

  attitude_raw_pub_.publish(setpoint_msg);

  // save last so3_cmd
  last_so3_cmd_ = *msg;
  last_so3_cmd_time_ = ros::Time::now();
  so3_cmd_set_ = true;
}

void SO3CmdToMavros::onInit(void)
{
  ros::NodeHandle priv_nh(getPrivateNodeHandle());

  // get thrust scaling parameters
  if(priv_nh.getParam("num_props", num_props_))
    ROS_INFO("Got number of props: %d", num_props_);
  else
    ROS_ERROR("Must set num_props param");

  if(priv_nh.getParam("kf", kf_))
    ROS_INFO("Using kf=%g so that prop speed = sqrt(f / num_props / kf) to scale force to speed.", kf_);
  else
    ROS_ERROR("Must set kf param for thrust scaling. Motor speed = sqrt(thrust / num_props / kf)");

  ROS_ASSERT_MSG(kf_ > 0, "kf must be positive. kf = %g", kf_);

  // get thrust scaling parameters
  if(priv_nh.getParam("lin_cof_a", lin_cof_a_) && priv_nh.getParam("lin_int_b", lin_int_b_))
    ROS_INFO("Using %g*x + %g to scale prop speed to att_throttle.", lin_cof_a_, lin_int_b_);
  else
    ROS_ERROR("Must set coefficients for thrust scaling (scaling from rotor "
              "velocity (rad/s) to att_throttle for pixhawk)");

  // get param for so3 command timeout duration
  priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.25);

  odom_set_ = false;
  imu_set_ = false;
  so3_cmd_set_ = false;

  attitude_raw_pub_ = priv_nh.advertise<mavros_msgs::AttitudeTarget>("attitude_raw", 10);

  odom_pose_pub_ = priv_nh.advertise<geometry_msgs::PoseStamped>("odom_pose", 10);

  so3_cmd_sub_ =
      priv_nh.subscribe("so3_cmd", 10, &SO3CmdToMavros::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay());

  odom_sub_ = priv_nh.subscribe("odom", 10, &SO3CmdToMavros::odom_callback, this, ros::TransportHints().tcpNoDelay());

  imu_sub_ = priv_nh.subscribe("imu", 10, &SO3CmdToMavros::imu_callback, this, ros::TransportHints().tcpNoDelay());
}

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