#ifndef TVTML_DATA2D_HPP
#define TVTML_DATA2D_HPP

// system includes
#include <cassert>
#include <limits>
#include <cmath>
#include <random>

#include <string>
#include <iostream>
#include <sstream>
#include <fstream>

// OpenCV includes
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#ifdef TV_DATA_DEBUG
    #include <opencv2/highgui/highgui.hpp>
#endif

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

// video++ includes
#include <vpp/vpp.hh>
#include <vpp/utils/opencv_bridge.hh>

// own includes
#include "manifold.hpp"

namespace tvmtl{

// Specialization 2D Data
template < typename MANIFOLD >
class Data< MANIFOLD, 2>{

    public:
	static const int img_dim;
	// Manifold typedefs
	typedef typename MANIFOLD::value_type value_type;
	typedef typename MANIFOLD::scalar_type scalar_type;

	// Storage typedefs
	typedef vpp::image2d<value_type> storage_type;

	typedef double weights_type;
	typedef vpp::image2d<weights_type> weights_mat;

	typedef bool inp_type;
	typedef vpp::image2d<inp_type> inp_mat;

	inline bool doInpaint() const { return inpaint_; }

	// Data Init functions
	inline void initEdgeweights();
	inline void initInp();

	// Input functions
	void rgb_imread(std::string filename);
	void rgb_readBrightness(std::string filename);
	void rgb_readChromaticity(std::string filename);

	void readMatrixDataFromCSV(std::string filename, const int nx, const int ny);

	// Noise functions
	void add_gaussian_noise(double stdev);
	//void add_gaussian_noise_spd(double stdev);

	// Random Input functions
	// TODO: - Paramaterize for manifold type
	//       - Use user-defined functor as parameter
	void create_nonsmooth_son(const int ny, const int nx);
	void create_nonsmooth_spd(const int ny, const int nx);

	// EdgeFunctions
	void findEdgeWeights();
	void setEdgeWeights(const weights_mat&);

	// Inpainting Functions
	void findInpWeights(const int channel=2);
	void createRandInpWeights(const double threshold);

	// OutputFunctions
	void rgb_saveimage(std::string fname);
	void rgb_show();

	void output_weights(const weights_mat& mat, std::string filename) const;

	void output_img(std::string filename) const;
	void output_matval_img(std::string filename) const;
	void output_nimg(std::string filename) const;
//    private:
	// Data members
	// TODO Don't forget to initialize with 1px border
	// alignment defaults to 16byte for SSE/SSE2, 32 Byte for AVX
	storage_type img_;
	storage_type noise_img_;
	weights_mat edge_weights_;

	bool inpaint_;
	inp_mat inp_;
};

/*----- Implementation 2D Data ------*/
template < typename MANIFOLD >
const int Data<MANIFOLD, 2>::img_dim = 2;

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::setEdgeWeights(const weights_mat& w){
    edge_weights_= vpp::clone(w);
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::initInp(){
    inp_ = inp_mat(noise_img_.domain());
    vpp::fill(inp_, false);
    inpaint_ = false;
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::initEdgeweights(){
    edge_weights_ = weights_mat(noise_img_.domain());
    vpp::fill(edge_weights_, 1.0);
}

// FIXME: This is problematic for greyscale pictures since vuchar3 and vu[i] is hardcoded, works only due to range check of Eigen for []
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::findEdgeWeights(){
    #ifdef TV_DATA_DEBUG
	std::cout << "Find Edges..." << std::endl;
    #endif
    cv::Mat gray, edge;

    { // Local Scope to save memory
	vpp::image2d<vpp::vuchar3> ucharimg(noise_img_.domain());

	// Convert double to uchar function
	auto double2uchar = [] (auto& i, const auto& n) {
	    value_type v = n * static_cast<typename MANIFOLD::scalar_type>(std::numeric_limits<unsigned char>::max());
	    vpp::vuchar3 vu = vpp::vuchar3::Zero();
	    vu[0]=static_cast<unsigned char>(v[2]);
	    vu[1]=static_cast<unsigned char>(v[1]);
	    vu[2]=static_cast<unsigned char>(v[0]);
	    i = vu;
	};

	vpp::pixel_wise(ucharimg, noise_img_) | double2uchar;
	cv::Mat src = vpp::to_opencv(ucharimg);
	cv::cvtColor(src, gray, CV_BGR2GRAY);
    }

    cv::blur(gray, edge, cv::Size(3,3));
    cv::Canny(edge, edge, 50, 150, 3);

    #ifdef TV_DATA_DEBUG
	cv::namedWindow( "Detected Edges", cv::WINDOW_NORMAL );
	cv::imshow("Detected Edges", edge);
	cv::waitKey(0);
    #endif

    vpp::image2d<unsigned char> ucharweights = vpp::from_opencv<unsigned char>(edge);
    vpp::pixel_wise(ucharweights, edge_weights_)() | [] (const unsigned char &uw, weights_type& ew) {
	ew = 1.0 - 0.99 * ( static_cast<weights_type>(uw) / static_cast<weights_type>(std::numeric_limits<unsigned char>::max()) );
    };

    #ifdef TV_DATA_DEBUG
	output_weights(edge_weights_, "wedge.csv");
    #endif
}

//TODO: static assert to avoid data that has not exactly 3 channels
// static assert that channel is element {1,2,3}
// make enum RGB out
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::findInpWeights(const int channel){
    #ifdef TV_DATA_DEBUG
	std::cout << "Find Inpainting Area..." << std::endl;
    #endif
    inp_ = inp_mat(noise_img_.domain());
    vpp::pixel_wise(noise_img_, inp_) | [&] (const value_type& img, inp_type& inp) { inp = static_cast<inp_type>(img[channel-1] > 0.95); };
    //vpp::pixel_wise(noise_img_, inp_) | [&] (const value_type& img, inp_type& inp) { inp = static_cast<inp_type>(img[channel-1] > 0.95); };
    #ifdef TV_DATA_DEBUG
	int nr = inp_.nrows();
	int nc = inp_.ncols();

	std::fstream f;
	f.open("inp.csv", std::fstream::out);

	for (int r=0; r<nr; r++){
	    const inp_type* cur = &inp_(r,0);
	    for (int c=0; c<nc; c++){
		f << cur[c];
		if(c != nc-1) f << ",";
	    }
	    f <<  std::endl;
	}
	f.close();
    #endif
}

//TODO: static assert to avoid data that has not exactly 3 channels
// static assert that channel is element {1,2,3}
// make enum RGB out
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::createRandInpWeights(const double threshold){

    #ifdef TV_DATA_DEBUG
	std::cout << "Create Random Inp Weights..." << std::endl;
    #endif

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> rand(0.0, 1.0);

    inp_ = inp_mat(noise_img_.domain());
    vpp::pixel_wise(noise_img_, inp_) | [&] (const value_type& img, inp_type& inp) { inp = static_cast<inp_type>(rand(gen) > threshold); };

    #ifdef TV_DATA_DEBUG
	int nr = inp_.nrows();
	int nc = inp_.ncols();

	std::fstream f;
	f.open("inp.csv", std::fstream::out);

	for (int r=0; r<nr; r++){
	    const inp_type* cur = &inp_(r,0);
	    for (int c=0; c<nc; c++){
		f << cur[c];
		if(c != nc-1) f << ",";
	    }
	    f <<  std::endl;
	}
	f.close();
    #endif
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_imread(std::string filename){
	static_assert(MANIFOLD::value_dim == 3,"ERROR: RGB Input requires a Manifold with embedding dimension N=3!");
	vpp::image2d<vpp::vuchar3> input_image;
	input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(filename)));
	noise_img_ = storage_type(input_image.domain());
	// Convert Picture of uchar to double
	    vpp::pixel_wise(input_image, noise_img_) | [] (auto& i, auto& n) {
	    value_type v = value_type::Zero();
	    vpp::vuchar3 vu = i;
	    // TODO: insert manifold scalar type
	    v[0]=static_cast<double>(vu[2]); //opencv saves as BGR
	    v[1]=static_cast<double>(vu[1]);
	    v[2]=static_cast<double>(vu[0]);
	    n = v / static_cast<double>(std::numeric_limits<unsigned char>::max());
	};
    img_ = vpp::clone(noise_img_, vpp::_border = 1);

    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_saveimage(std::string fname){
    static_assert(MANIFOLD::value_dim == 3,"ERROR: RGB Ouput requires a Manifold with embedding dimension N=3!");
	// Convert Picture of double to uchar
	vpp::image2d<vpp::vuchar3> vucharimg(img_.domain());
	vpp::pixel_wise(vucharimg, img_) | [] (auto& i, auto& n) {
	    value_type v = n * (double) std::numeric_limits<unsigned char>::max();
	    vpp::vuchar3 vu = vpp::vuchar3::Zero();
	    vu[0]=(unsigned char) v[2];
	    vu[1]=(unsigned char) v[1];
	    vu[2]=(unsigned char) v[0];
	    i = vu;
	};

	cv::imwrite(fname, to_opencv(vucharimg));
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_show(){
    static_assert(MANIFOLD::value_dim == 3,"ERROR: RGB Ouput requires a Manifold with embedding dimension N=3!");
	// Convert Picture of double to uchar
	vpp::image2d<vpp::vuchar3> vucharimg(img_.domain());
	vpp::pixel_wise(vucharimg, img_) | [] (auto& i, auto& n) {
	    value_type v = n * (double) std::numeric_limits<unsigned char>::max();
	    vpp::vuchar3 vu = vpp::vuchar3::Zero();
	    vu[0]=(unsigned char) v[2];
	    vu[1]=(unsigned char) v[1];
	    vu[2]=(unsigned char) v[0];
	    i = vu;
	};
	cv::namedWindow("Image Viewer", cv::WINDOW_NORMAL );
	cv::imshow( "Image Viewer", vpp::to_opencv(vucharimg));
	cv::waitKey(0);

}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_readBrightness(std::string filename){
	static_assert(MANIFOLD::value_dim == 1,"ERROR: Brightness Input requires a Manifold with embedding dimension N=1!");
	vpp::image2d<vpp::vuchar3> input_image;
	input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(filename)));
	noise_img_ = storage_type(input_image.domain());
	// Convert Picture of uchar to double
	    vpp::pixel_wise(input_image, noise_img_)(vpp::_no_threads) | [] (auto& i, auto& n) {
	    vpp::vdouble3 v;
	    vpp::vuchar3 vu = i;
	    // TODO: insert manifold scalar type
	    v[0]=static_cast<double>(vu[2]); //opencv saves as BGR
	    v[1]=static_cast<double>(vu[1]);
	    v[2]=static_cast<double>(vu[0]);
	    v = v / static_cast<double>(std::numeric_limits<unsigned char>::max());
	    n.setConstant(v.norm()/std::sqrt(3));
	};
    img_ = vpp::clone(noise_img_, vpp::_border = 1);

    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_readChromaticity(std::string filename){
	static_assert(MANIFOLD::value_dim == 3,"ERROR: Chromaticity Input requires a Manifold with embedding dimension N=3!");
	vpp::image2d<vpp::vuchar3> input_image;
	input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(filename)));
	noise_img_ = storage_type(input_image.domain());
	// Convert Picture of uchar to double
	    vpp::pixel_wise(input_image, noise_img_) | [] (auto& i, auto& n) {
	    value_type v;
	    // TODO: insert manifold scalar type
	    v[0]=static_cast<double>(i[2])+1.0; //opencv saves as BGR
	    v[1]=static_cast<double>(i[1])+1.0;
	    v[2]=static_cast<double>(i[0])+1.0;
	    v = v / (static_cast<double>(std::numeric_limits<unsigned char>::max())+1.0);

	    double norm = v.norm();
	    if(norm!=0)
		n = v / norm;
	    else
		n = v;

	    #ifdef TV_DATA_DEBUG
		if(!std::isfinite(n(0))){
		    std::cout << "\nvuchar pixel " << i << std::endl;
		    std::cout << "double pixel " << v << std::endl;
		    std::cout << "normalized pixel " << n << std::endl;
		}
	    #endif
	};
    img_ = vpp::clone(noise_img_, vpp::_border = 1);

    initInp();
    initEdgeweights();
}

template <typename MANIFOLD>
void Data<MANIFOLD, 2>::readMatrixDataFromCSV(std::string filename, const int nx, const int ny){
    #ifdef TV_DATA_DEBUG
	std::cout << "ReadMatrixData from CSV File..." << std::endl;
    #endif
    noise_img_ = storage_type(ny, nx);
    vpp::fill(noise_img_, MANIFOLD::value_type::Zero());

    const int N = MANIFOLD::value_type::RowsAtCompileTime;
    const int N2 = MANIFOLD::value_dim;
    int cols, rows = 0;

    std::ifstream infile(filename, std::ifstream::in);

    std::string line = "";
    while (std::getline(infile, line)){
	std::stringstream strstr(line);
	std::string word = "";

	if(cols == 0)
	    while (std::getline(strstr, word, ','))
		cols++;

	rows++;
    }

    assert(N2==cols);
    assert(nx*ny==rows);

    infile.clear();
    infile.seekg(0, std::ios_base::beg);

    Eigen::Matrix<typename MANIFOLD::scalar_type, N2, 1> vectorizedMat;
    vectorizedMat.setZero();

    auto it = noise_img_.begin();
    while (std::getline(infile, line)){
	std::stringstream strstr(line);
	std::string word = "";
	int j=0;
	while (std::getline(strstr, word,',')){
	    typename MANIFOLD::scalar_type entry= static_cast<typename MANIFOLD::scalar_type>(std::stod(word));
	    vectorizedMat(j) = entry;
	    ++j;
	}
	*it = Eigen::Map<typename MANIFOLD::value_type>(vectorizedMat.data());
	it.next();
    }
    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);

    initInp();
    initEdgeweights();
}

template <typename MANIFOLD>
void Data<MANIFOLD, 2>::add_gaussian_noise(double stdev){

    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<typename MANIFOLD::scalar_type> rand(0.0, stdev);

    auto generate = [&] (typename MANIFOLD::scalar_type entry){
	return entry + rand(gen);
    };

	if(MANIFOLD::non_isometric_embedding)
	    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_preprocessing(i); };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i) { i = i.unaryExpr(generate); };

	if(MANIFOLD::non_isometric_embedding)
	    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_postprocessing(i); };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i) { MANIFOLD::projector(i); };

    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);
}

/*
//TODO: Generalize implementation to be compatible with add_gaussian_noise
template <typename MANIFOLD>
void Data<MANIFOLD, 2>::add_gaussian_noise_spd(double stdev){

    static_assert(MANIFOLD::MyType==SPD,"add_gaussian_noise_spd is implemented only for SPD(N)!");

    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_preprocessing(i); };

    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<scalar_type> rand(0.0, stdev);

    auto generate = [&] (scalar_type entry){
	return rand(gen);
    };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i) {
	value_type r = value_type::Zero().unaryExpr(generate);
	i = i + (r + r.transpose()) * 0.5;
    };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_postprocessing(i); };

    img_ = vpp::clone(noise_img_, vpp::_border = 1);
}
*/

// TODO: Generalize to general N
template <typename MANIFOLD>
void Data<MANIFOLD, 2>::create_nonsmooth_son(const int ny,const int nx){
    #ifdef TV_DATA_DEBUG
	std::cout << "Create Nonsmooth SO(3) Picture..." << std::endl;
    #endif

    const int N = MANIFOLD::value_type::RowsAtCompileTime;

    static_assert(MANIFOLD::MyType==SO, "ERROR: Only possible for SO(N) manifolds");
    static_assert(N==3, "ERROR: Only possible for SO(3) manifolds");

    noise_img_ = storage_type(ny, nx);

    auto son_inserter = [&] (typename MANIFOLD::value_type& v, const vpp::vint2& coord){

	typename MANIFOLD::scalar_type x = coord(1)*1.0 / nx;
	typename MANIFOLD::scalar_type y = coord(0)*1.0 / ny;
	Eigen::Matrix<typename MANIFOLD::scalar_type, N, 1> rotation_axis;
	if(x > 0.5)
	    rotation_axis << 2.0 * x, y, 0.0;
	else
	    rotation_axis << 0.0, 2.0 * x, 0.5;

	typename MANIFOLD::scalar_type alpha;
	if(x > y)
	    alpha = x + y;
	else
	    alpha = M_PI * 0.5 + x - y;

	//v = MANIFOLD::value_type::Identity();
	v = Eigen::AngleAxis<typename MANIFOLD::scalar_type>(alpha, rotation_axis.normalized());
    };

    vpp::pixel_wise(noise_img_, noise_img_.domain()) | son_inserter;
    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);

    initInp();
    initEdgeweights();
}

// TODO: Generalize to general N
template <typename MANIFOLD>
void Data<MANIFOLD, 2>::create_nonsmooth_spd(const int ny,const int nx){
    #ifdef TV_DATA_DEBUG
	std::cout << "Create Nonsmooth SPD(3) Picture..." << std::endl;
    #endif

    const int N = MANIFOLD::value_type::RowsAtCompileTime;

    static_assert(MANIFOLD::MyType==SPD, "ERROR: Only possible for SPD(N) manifolds");
    static_assert(N==3, "ERROR: Only possible for SPD(3) manifolds");

    noise_img_ = storage_type(ny, nx);

    auto spd_inserter = [&] (typename MANIFOLD::value_type& v, const vpp::vint2& coord){

	typename MANIFOLD::scalar_type x = 1.0 * coord(1) / nx;
	typename MANIFOLD::scalar_type y = 1.0 * coord(0) / ny;

	Eigen::Matrix<typename MANIFOLD::scalar_type, N, N> R;
	Eigen::DiagonalMatrix< typename MANIFOLD::scalar_type, N> D(N);
	Eigen::Matrix<typename MANIFOLD::scalar_type, N, 1> rotation_axis;
	typename MANIFOLD::scalar_type alpha;

	if(x + y < 1.0){
	    rotation_axis << x, y, 2.0;
	    alpha = x + 2.0 * y;
	}
	else{
	    rotation_axis << y, -x, 1.0;
	    alpha = y + 2.0 * x;
	}

	D.diagonal() << x + 0.2, y + 0.2, 0.5;
	R  = Eigen::AngleAxis<typename MANIFOLD::scalar_type>(alpha, rotation_axis.normalized());
	v = R.transpose() * D * R;
    };

    vpp::pixel_wise(noise_img_, noise_img_.domain()) | spd_inserter;
    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);

    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_weights(const weights_mat& weights, std::string filename) const{
    int nr = weights.nrows();
    int nc = weights.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);

    for (int r=0; r<nr; r++){
	const typename Data<MANIFOLD,2>::weights_type* cur = &weights(r,0);
	for (int c=0; c<nc; c++){
	    f << cur[c];
	    if(c != nc-1) f << ",";
	}
	f <<  std::endl;
    }
    f.close();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_matval_img(std::string filename) const{
    int nr = img_.nrows();
    int nc = img_.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "\n");
    for (int r=0; r<nr; r++){
	const auto* cur = &img_(r,0);
	for (int c=0; c<nc; c++)
	    f << cur[c].format(CommaInitFmt);
    }
    f.close();
}
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_img(std::string filename) const{
    int nr = img_.nrows();
    int nc = img_.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
    for (int r=0; r<nr; r++){
	const auto* cur = &img_(r,0);
	for (int c=0; c<nc; c++){
	    f << cur[c].format(CommaInitFmt);
	    if(c != nc-1) f << ",";
	}
	f <<  std::endl;
    }
    f.close();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_nimg(std::string filename) const{
    int nr = noise_img_.nrows();
    int nc = noise_img_.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
    for (int r=0; r<nr; r++){
	const auto* cur = &noise_img_(r,0);
	for (int c=0; c<nc; c++){
	    f << cur[c].format(CommaInitFmt);
	    if(c != nc-1) f << ",";
	}
	f <<  std::endl;
    }
    f.close();
}

}// end namespace tvmtl

#endif


/*
  Copyright (c) 2020, Robin Zimmeck <robin@robin-zimmeck.de>

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice, this
     list of conditions and the following disclaimer.

  2. 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.

  3. Neither the name of the copyright holder nor the names of its contributors
     may be used to endorse or promote products derived from this software
     without specific prior written permission.

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

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include "minisi/unit.h"

using namespace minisi;

BOOST_AUTO_TEST_CASE(unit_pow_test)
{
  using unit1_t = unit_t<1, 1, 1, 1, 1, 1, 1, 2, 3, 4, 5, 6, 7, 8>;
  using unit2_t = unit_t<2, 3, 4, 5, 6, 7, 8, 1, 1, 1, 1, 1, 1, 1>;

  using pow_t = unit_pow_t<unit1_t, unit2_t>;

  BOOST_CHECK_EQUAL(pow_t::Time::num, 1);
  BOOST_CHECK_EQUAL(pow_t::Time::den, 1);
  BOOST_CHECK_EQUAL(pow_t::Length::num, 1);
  BOOST_CHECK_EQUAL(pow_t::Length::den, 1);
  BOOST_CHECK_EQUAL(pow_t::Mass::num, 1);
  BOOST_CHECK_EQUAL(pow_t::Mass::den, 1);
  BOOST_CHECK_EQUAL(pow_t::Current::num, 1);
  BOOST_CHECK_EQUAL(pow_t::Current::den, 1);
  BOOST_CHECK_EQUAL(pow_t::Temperature::num, 1);
  BOOST_CHECK_EQUAL(pow_t::Temperature::den, 1);
  BOOST_CHECK_EQUAL(pow_t::Amount::num, 1);
  BOOST_CHECK_EQUAL(pow_t::Amount::den, 1);
  BOOST_CHECK_EQUAL(pow_t::Intensity::num, 1);
  BOOST_CHECK_EQUAL(pow_t::Intensity::den, 1);
}

BOOST_AUTO_TEST_CASE(unit_product_test)
{
  using unit1_t = unit_t<1, 1, 1, 1, 1, 1, 1, 3, 4, 5, 6, 7, 8, 9>;
  using unit2_t = unit_t<2, 3, 4, 5, 6, 7, 8, 3, 4, 5, 6, 7, 8, 9>;

  using product_t = unit_product_t<unit1_t, unit2_t>;

  BOOST_CHECK_EQUAL(product_t::Time::num, 1);
  BOOST_CHECK_EQUAL(product_t::Time::den, 1);
  BOOST_CHECK_EQUAL(product_t::Length::num, 1);
  BOOST_CHECK_EQUAL(product_t::Length::den, 1);
  BOOST_CHECK_EQUAL(product_t::Mass::num, 1);
  BOOST_CHECK_EQUAL(product_t::Mass::den, 1);
  BOOST_CHECK_EQUAL(product_t::Current::num, 1);
  BOOST_CHECK_EQUAL(product_t::Current::den, 1);
  BOOST_CHECK_EQUAL(product_t::Temperature::num, 1);
  BOOST_CHECK_EQUAL(product_t::Temperature::den, 1);
  BOOST_CHECK_EQUAL(product_t::Amount::num, 1);
  BOOST_CHECK_EQUAL(product_t::Amount::den, 1);
  BOOST_CHECK_EQUAL(product_t::Intensity::num, 1);
  BOOST_CHECK_EQUAL(product_t::Intensity::den, 1);
}

BOOST_AUTO_TEST_CASE(unit_quotient_test)
{
  using unit1_t = unit_t<1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1>;

  using unit2_t = unit_t<2, 3, 4, 5, 6, 7, 8, 3, 4, 5, 6, 7, 8, 9>;

  using quotient_t = unit_quotient_t<unit1_t, unit2_t>;

  BOOST_CHECK_EQUAL(quotient_t::Time::num, 1);
  BOOST_CHECK_EQUAL(quotient_t::Time::den, 3);
  BOOST_CHECK_EQUAL(quotient_t::Length::num, 1);
  BOOST_CHECK_EQUAL(quotient_t::Length::den, 4);
  BOOST_CHECK_EQUAL(quotient_t::Mass::num, 1);
  BOOST_CHECK_EQUAL(quotient_t::Mass::den, 5);
  BOOST_CHECK_EQUAL(quotient_t::Current::num, 1);
  BOOST_CHECK_EQUAL(quotient_t::Current::den, 6);
  BOOST_CHECK_EQUAL(quotient_t::Temperature::num, 1);
  BOOST_CHECK_EQUAL(quotient_t::Temperature::den, 7);
  BOOST_CHECK_EQUAL(quotient_t::Amount::num, 1);
  BOOST_CHECK_EQUAL(quotient_t::Amount::den, 8);
  BOOST_CHECK_EQUAL(quotient_t::Intensity::num, 1);
  BOOST_CHECK_EQUAL(quotient_t::Intensity::den, 9);
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.
// 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_STRATEGIES_LINE_INTERPOLATE_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_LINE_INTERPOLATE_GEOGRAPHIC_HPP

#include <boost/geometry/strategies/detail.hpp>

#include <boost/geometry/strategies/distance/detail.hpp>

#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/line_interpolate.hpp>

#include <boost/geometry/strategies/line_interpolate/services.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace line_interpolate
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public strategies::detail::geographic_base<Spheroid>
{
    using base_t = strategies::detail::geographic_base<Spheroid>;

public:
    geographic() = default;

    explicit geographic(Spheroid const& spheroid)
        : base_t(spheroid)
    {}

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  distance::detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic
                <
                    FormulaPolicy, Spheroid, CalculationType
                >(base_t::m_spheroid);
    }

    template <typename Geometry>
    auto line_interpolate(Geometry const&) const
    {
        return strategy::line_interpolate::geographic
                <
                    FormulaPolicy, Spheroid, CalculationType
                >(base_t::m_spheroid);
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, geographic_tag>
{
    using type = strategies::line_interpolate::geographic<>;
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::line_interpolate::geographic<FP, S, CT> >
{
    static auto get(strategy::line_interpolate::geographic<FP, S, CT> const& s)
    {
        return strategies::line_interpolate::geographic<FP, S, CT>(s.model());
    }
};

} // namespace services

}} // namespace strategies::line_interpolate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_LINE_INTERPOLATE_GEOGRAPHIC_HPP


#include <action_executor/action_executor.h>
#include <chrono>
#include <thread>
#include <boost/bind.hpp>
#include <fstream>

#define SHOOT_TYPE_DEFAULT              0
#define EXECUTOR_IDLE                 200
#define K_GIMBAL                      1.0
#define K_DRONE                       1.0
#define K_DRONE_YAW                   0.3
#define GIMBAL_FREQ                   0.1
#define TRAILER_FREQ                  0.1
#define MAIN_FREQ                   0.033
#define RT_FREQ                     0.033
#define TO_DEG                   180/M_PI
#define TO_RAD                   M_PI/180
#define MAX_DRONE_SPEED                 1
#define K1_GIMBAL                     1.0
#define K2_GIMBAL                     0.5
#define K_OFFSET                      2.0

// Constructor
Executor::Executor(int _argc, char** _argv)
{
  ros::init(_argc, _argv, "Executor");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");

  pnh.param<int>("drone_id", drone_id_, 1);
  pnh.param<float>("k1_gimbal", k_1,     K1_GIMBAL);
  pnh.param<float>("k2_gimbal", k_2,     K2_GIMBAL);
  pnh.param<float>("k_offset", k_offset, K_OFFSET);
  pnh.param<int>("image_width", w_im, 640);
  pnh.param<int>("image_height",h_im, 480);
  pnh.param<float>("MAX_YAW_RATE", MAX_YAW_RATE, 0.2);
  pnh.param<double>("trailer", trailer_size, 0.5);
  std::vector<double> p_C_B_aux;
  pnh.getParam("p_C_B",p_C_B_aux);
  p_C_B.x() = p_C_B_aux[0];
  p_C_B.y() = p_C_B_aux[1];
  p_C_B.z() = p_C_B_aux[2];
  pnh.param<bool>("simulation",simulation,true);
  pnh.param<bool>("onboard",onboard, true);
  pnh.param<bool>("offset", offset, false);
  pnh.param<bool>("f_target", f_target, false);

  target_image_offset_x_MAX = w_im/2;
  target_image_offset_x_MIN = -target_image_offset_x_MAX;
  target_image_offset_x_13  = w_im/3-w_im/2;
  target_image_offset_x_23  = w_im*2/3-w_im/2;
  target_image_offset_y_MAX = h_im/2;
  target_image_offset_y_MIN = -target_image_offset_y_MAX;
  target_image_offset_y_13  = h_im/3-h_im/2;
  target_image_offset_y_23  = h_im*2/3-h_im/2;

  ROS_INFO("Setting up Executor %d", drone_id_);

  // Subscribers
  ros::Subscriber target_array_sub_;
  if (onboard)
    target_array_sub_                          = nh.subscribe<multidrone_msgs::TargetStateArray>("/target_3d_state", 1, &Executor::targetarrayCallback, this);
  else
    target_array_sub_                          = nh.subscribe<multidrone_msgs::TargetStateArray>("/targets_pose", 1, &Executor::targetarrayCallback, this);
  ros::Subscriber drone_pose_sub               = nh.subscribe<geometry_msgs::PoseStamped>("ual/pose", 1, &Executor::dronePose, this);
  ros::Subscriber drone_vel_sub                = nh.subscribe<geometry_msgs::TwistStamped>("ual/velocity",1, &Executor::droneVelocity, this);
  ros::Subscriber drone_state_sub              = nh.subscribe<uav_abstraction_layer::State>("ual/state",1, &Executor::droneState, this);
  ros::Subscriber gimbal_status_sub            = nh.subscribe<multidrone_msgs::GimbalStatus>("gimbal/status",1, &Executor::gimbalStatus, this);

  ros::Subscriber focus_sub_;
  if(f_target)
    focus_sub_                                 = nh.subscribe<std_msgs::Int32>("visual_analysis/focus_value_target", 1, &Executor::focusCallback, this);
  else
    focus_sub_                                 = nh.subscribe<std_msgs::Int32>("visual_analysis/focus_value_image", 1, &Executor::focusCallback, this);

  // ###########  Communication with SWAP  ########## //
  ros::Subscriber confl_warning_sub_           = nh.subscribe("collision_warning", 1, &Executor::collisionWarningCallback, this);
  wished_mov_dir_pub_                          = nh.advertise<geometry_msgs::Vector3>("wished_movement_direction",1 , true);  // the final true is required
  ros::Subscriber avoid_mov_dir_sub_           = nh.subscribe("avoid_movement_direction", 1, &Executor::avoidMovementCallback, this);
  // ###########  #######################  ########### //

  // Publishers
  gimbal_cmd_basecam_pub                       = nh.advertise<geometry_msgs::Vector3>("gimbal/cmd",1);
  go_to_waypoint_pub_                          = nh.advertise<geometry_msgs::PoseStamped>("ual/set_pose",1);
  set_velocity_pub_                            = nh.advertise<geometry_msgs::TwistStamped>("ual/set_velocity",1);
  trajectory_                                  = nh.advertise<nav_msgs::Odometry>("desired_trajectory",1);
  lyapunov_                                    = nh.advertise<geometry_msgs::Point32>("gimbal_lyapunov",1);
  pixel_publish                                = nh.advertise<geometry_msgs::Vector3>("pixel_position",1);

  // Service Client
  cmd_long_client_                             = nh.serviceClient<mavros_msgs::CommandLong>("mavros/cmd/command");
  set_param_client_                            = nh.serviceClient<mavros_msgs::ParamSet>("mavros/param/set");
  get_param_client_                            = nh.serviceClient<mavros_msgs::ParamGet>("mavros/param/get");
  take_off_client_                             = nh.serviceClient<uav_abstraction_layer::TakeOff>("ual/take_off");
  go_to_waypoint_client_                       = nh.serviceClient<uav_abstraction_layer::GoToWaypoint>("ual/go_to_waypoint");
  land_client_                                 = nh.serviceClient<uav_abstraction_layer::Land>("ual/land");
  follow_target_client_                        = nh.serviceClient<multidrone_msgs::FollowTarget>("follow_target");
  set_framing_type_client_                     = nh.serviceClient<multidrone_msgs::SetFramingType>("visual_analysis/set_framing_type");
  camera_control_client_                       = nh.serviceClient<multidrone_msgs::CameraControl>("camera_control");

  // Service Server
  ros::ServiceServer manual_controls_service_  = nh.advertiseService("manual_controls", &Executor::ManualControlServiceCallback, this);

  if(simulation) {
    std::vector<double> camera_matrix;
    double cols, rows;
    pnh.getParam("camera_matrix/rows",rows);
    pnh.getParam("camera_matrix/cols",cols);
    camera_matrix.reserve(cols*rows);
    pnh.getParam("camera_matrix/data",camera_matrix);

    K  <<  K_min ,     0.000000,   319.5,    // BMMCC camera_matrix minimum zoom
          0.000000,      K_min,   239.5,
          0.000000,     0.000000, 1.000;

    ROS_INFO("Executor [%d] initialized in simulation mode!", drone_id_);
    // Initialise gimbal cmd
    #ifdef MAVROS_VERSION_BELOW_0_25_0
    gimbal_cmd_.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
    #else
    gimbal_cmd_.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
    #endif
    gimbal_cmd_.request.confirmation = false;
    gimbal_cmd_.request.param7 = 2; // MAV_MOUNT_MODE::MAV_MOUNT_MODE_MAVLINK_TARGETING

    p_C_B << -0.051, 0, 0.162; // gimbal position with respect to drone CG

    // Set MNT_MODE parameters on FCU for gimbal control via Mavlink
    setMountModeParameters();

  }
  else{

    K  <<  K_min ,     0.000000,   319.5,    // BMMCC camera_matrix minimum zoom
          0.000000,      K_min,   239.5,
          0.000000,     0.000000, 1.000;

    ROS_INFO("Executor [%d] initialized in real mode", drone_id_);
  }

  multidrone_msgs::ManualControls manual_control_msg_;
  manual_control_msg_.request.control = multidrone_msgs::ManualControls::Request::CAMERA_RESET;
  ManualControlServiceCallback(manual_control_msg_.request, manual_control_msg_.response);

  w_est_ << 0.0, 0.0, 0.0;
  w_est << 0.0, 0.0, 0.0;

  R_S_I <<    1,  0,  0,
              0, -1,  0,
              0,  0, -1;

  // std::cout << "Gimbal position is \n" << p_C_B << "\n\n";

  // Wait until having initial drone pose
  while (!has_drone_pose_ || !has_drone_vel_ && ros::ok()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
    ros::spinOnce();
  }

  // action service
  server_ = new Server(nh, "action_server", false);
  server_->registerGoalCallback(boost::bind(&Executor::actionCallback, this));
  server_->registerPreemptCallback(boost::bind(&Executor::preemptCallback, this));

  server_->start();

  t_0 = ros::Time::now().toSec();

  // start Timer
  timer_        = nh.createTimer(ros::Duration(MAIN_FREQ), &Executor::timerCallback, this); //33Hz
  timer_trailer = nh.createTimer(ros::Duration(TRAILER_FREQ), &Executor::timerCallbackTrailer, this, false, false); //10Hz
  timer_gimbal  = nh.createTimer(ros::Duration(GIMBAL_FREQ), &Executor::timerCallbackGimbal, this, false, false); //5Hz
  timer_rt      = nh.createTimer(ros::Duration(RT_FREQ), &Executor::referenceTrajectory, this, false, false); //33Hz

  ROS_INFO("Executor [%d] running", drone_id_);

  ros::MultiThreadedSpinner spinner(4); // Use 4 threads
  spinner.spin();
}

// Destructor
Executor::~Executor()
{
  delete server_;
};

// Timer callback
void Executor::timerCallback(const ros::TimerEvent&)
{
  lyapunov_publisher();

  if(confl_warning_){
    setpoint_vel_.twist.linear.x = avoid_mov_direction_.x;
    setpoint_vel_.twist.linear.y = avoid_mov_direction_.y;
    setpoint_vel_.twist.linear.z = 0;
    set_velocity_pub_.publish(setpoint_vel_);
  }
  else
  {
   switch (action_type_){
     case multidrone_msgs::DroneAction::TYPE_TAKEOFF:{
      timer_trailer.stop();
      takeOff();
      multidrone_msgs::ManualControls manual_control_msg_;
      manual_control_msg_.request.control = multidrone_msgs::ManualControls::Request::FOCUS_RESET;
      ManualControlServiceCallback(manual_control_msg_.request, manual_control_msg_.response);
      break;
     }
    case multidrone_msgs::DroneAction::TYPE_LAND:
      running_trailer = false;
      timer_trailer.stop();
      timer_gimbal.stop();
      timer_trajectory.stop();
      gimbalSafeMode();
      land();
      break;
    case multidrone_msgs::DroneAction::TYPE_GOTOWAYPOINT:
      timer_trailer.stop();
      timer_gimbal.stop();
      timer_trajectory.stop();
      gimbalSafeMode();
      running_trailer = false;
      feedback_.status= true;
      server_->publishFeedback(feedback_);
      goToWaypoint();
      break;
    case multidrone_msgs::DroneAction::TYPE_SHOOTING:
      if (!running_trailer) {
        timer_gimbal.start();
        if (parameters_set){
          if (list_wp_.size() == 1 || list_wp_.size() == 0)
            orientation_0 = 0;
          else
            orientation_0 = atan2 (list_wp_[1].point.y - list_wp_[0].point.y, list_wp_[1].point.x - list_wp_[0].point.x);

          gimbalSafeMode();
          running_trailer = true;
          trailer.init(orientation_0,rt_target_pos_);
          timer_trailer.start();
          timer_trajectory.start();
          parameters_set = false;
          feedback_.status= true;
          server_->publishFeedback(feedback_);
          // std::cout << "GOT A SA\n\n";
        }
      }
      break;
    default:
      if (running_trailer) {
        timer_trailer.stop();
        timer_gimbal.stop();
        timer_trajectory.stop();
        running_trailer = false;
        gimbalSafeMode();
      }
    }
  }
}

void Executor::timerCallbackGimbal(const ros::TimerEvent&)
{
  if (has_gimbal_status_ || simulation) {
    if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::NONE)
      gimbalAutomatic(pan_s,tilt_s,pan_e,tilt_e, duration_.data.sec, start_time);
    else
      gimbalController();
  }else
      gimbalSafeMode();

   if(shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL || shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL_GPS)
    if(has_visual_2D_target_status_)
      CameraControl();

  if (count < count_lim/4)
    K(0) = K_min;
  else if (count < count_lim/2)
    K(0) = (3*K_min+K_max)/4;
  else if (count < 3*count_lim/4)
    K(0) = (K_min+3*K_max)/4;
  else
    K(0) = K_max;

  K(4) = K(0);

  double t_now;
  t_now = ros::Time::now().toSec();
  if (t_now - time_of_last_gimbal_status > 1)
    has_gimbal_status_ = false;
  if (t_now - time_of_last_target_status > 1)
    has_shooting_target_status_ = false;
  if (t_now - time_of_last_target_visual_2D_status > 1)
    has_visual_2D_target_status_ = false;
  if (t_now - time_of_last_drone_pose > 1)
    has_drone_pose_ = false;
}

void Executor::timerCallbackTrailer(const ros::TimerEvent&)
{
  if (has_rt_target_status_){
    trailer.run_trailer(rt_target_pos_);
    followVehicle();
    trajectory_publisher();
  }
  has_rt_target_status_ = false;
}

void Executor::trajectory_publisher ()
{
  nav_msgs::Odometry _msg;
  desired_point << trailer.desired_point_, trailer.altitude;
  desired_vel << trailer.v_desired;

  _msg.header.stamp = ros::Time::now();
  _msg.header.frame_id = "map";
  _msg.child_frame_id = "drone_" + std::to_string(drone_id_) + "/base_link";
  _msg.pose.pose.position.x = desired_point[0];
  _msg.pose.pose.position.y = desired_point[1];
  _msg.pose.pose.position.z = trailer.altitude;
  _msg.pose.pose.orientation.x = 0;
  _msg.pose.pose.orientation.y = 0;
  _msg.pose.pose.orientation.z = 0;
  _msg.pose.pose.orientation.w = 1;
  _msg.twist.twist.linear.x  = desired_vel[0];
  _msg.twist.twist.linear.y  = desired_vel[1];
  _msg.twist.twist.linear.z  = 0;
  _msg.twist.twist.angular.x = 0;
  _msg.twist.twist.angular.y = 0;
  _msg.twist.twist.angular.z = setpoint_vel_.twist.angular.z;
  trajectory_.publish(_msg);
}

void Executor::lyapunov_publisher()
{
  geometry_msgs::Point32 _msg;
  _msg.x = lyapunov_visual;
  _msg.y = drone_yaw_;
  lyapunov_.publish(_msg);
}

void Executor::targetarrayCallback(const multidrone_msgs::TargetStateArray::ConstPtr& _msg) // real target callback
{

  //rt target
  if (rt_mode_ != multidrone_msgs::ShootingAction::RT_MODE_VIRTUAL_TRAJ){
    for (auto target:_msg->targets) {
      if (target.target_id == rt_id_) {
        tf::twistMsgToEigen(target.velocity.twist, rt_target_twist_);

        if (rt_mode_ == multidrone_msgs::ShootingAction::RT_MODE_ACTUAL_TARGET)
          tf2::fromMsg(target.pose.pose.position, rt_target_pos_);
        targetStatus(true);
      }
    }
  }
  //gimbal target
  if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::GPS || shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL_GPS){
    for (auto target:_msg->targets) {
      if (target.target_id == shooting_id_) {
        tf2::fromMsg(target.pose.pose.position,shooting_target_pos_);
        tf2::fromMsg(target.pose.pose.orientation,shooting_target_att_);
        tf::twistMsgToEigen(target.velocity.twist, shooting_target_twist_);
        targetStatus(false);
      }
    }
  }
}

void Executor::referenceTrajectory(const ros::TimerEvent&){

  if (rt_mode_ == multidrone_msgs::ShootingAction::RT_MODE_VIRTUAL_TRAJ){
    rt_target_pos_.x()  = rt_target_pos_.x() + total_displ.x()*formation_speed_*RT_FREQ/(total_displ.norm());
    rt_target_pos_.y()  = rt_target_pos_.y() + total_displ.y()*formation_speed_*RT_FREQ/(total_displ.norm());
    rt_target_pos_.z()  = rt_target_pos_.z() + total_displ.z()*formation_speed_*RT_FREQ/(total_displ.norm());
    if (abs(rt_target_pos_.x()-list_wp_[0].point.x) >= abs(total_displ.x()) && abs(rt_target_pos_.y()-list_wp_[0].point.y) >= abs(total_displ.y()) && abs(rt_target_pos_.z()-list_wp_[0].point.z) >= abs(total_displ.z()))
      formation_speed_=0;
    targetStatus(true);
  }

  if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::VIRTUAL){
    shooting_target_pos_.x()   = shooting_target_pos_.x() + total_displ.x()*formation_speed_*RT_FREQ/(total_displ.norm());
    shooting_target_pos_.y()   = shooting_target_pos_.y() + total_displ.y()*formation_speed_*RT_FREQ/(total_displ.norm());
    shooting_target_pos_.z()   = shooting_target_pos_.z() + total_displ.z()*formation_speed_*RT_FREQ/(total_displ.norm());

    desired_yaw = atan2 (total_displ.y() - list_wp_[0].point.y, total_displ.x() - list_wp_[0].point.x);
    shooting_target_att_.z()   = sin(desired_yaw/2);
    shooting_target_att_.y()   = 0;
    shooting_target_att_.x()   = 0;
    shooting_target_att_.w()   = cos(desired_yaw/2);
    shooting_target_twist_.x() = total_displ.x()*formation_speed_/(total_displ.norm());
    shooting_target_twist_.y() = total_displ.y()*formation_speed_/(total_displ.norm());
    shooting_target_twist_.z() = total_displ.z()*formation_speed_/(total_displ.norm());
    if (abs(shooting_target_pos_.x()-list_wp_[0].point.x) >= abs(total_displ.x()) && abs(shooting_target_pos_.y()-list_wp_[0].point.y) >= abs(total_displ.y()) && abs(shooting_target_pos_.z()-list_wp_[0].point.z) >= abs(total_displ.z()))
     formation_speed_=0;
    targetStatus(false);
  }
}

void Executor::targetStatus(bool rt)
{
  if(rt)
    has_rt_target_status_ = true;
  else{
    p_T_I = R_S_I*shooting_target_pos_;
    if(shooting_mode_ == multidrone_msgs::TargetIdentifierType::GPS || shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL_GPS){
      geometry_msgs::Vector3 pixel_msg;
      q_I = p_T_I - p_B_I - R_B_I*p_C_B;
      q_C = R_C_I.transpose()*q_I;

      q_C2.x() = q_C.y()/q_C.z();
      q_C2.y() = -q_C.x()/q_C.z();
      q_C2.z() = q_C.z()/q_C.z();
      pixel = K*q_C2;

      pixel_msg.x = pixel[0];
      pixel_msg.y = pixel[1];
      pixel_msg.z = pixel[2];

      pixel_publish.publish(pixel_msg);
    }
    has_shooting_target_status_ = true;
  }
}

void Executor::focusCallback(const std_msgs::Int32::ConstPtr& _msg)
{
  focus_value = _msg->data;
}

void Executor::run_w_estimate()
{
  Eigen::Vector3d w_est_dot;

  dT_w = GIMBAL_FREQ;
  w_est_dot = k_2*w_e;
  w_est = w_est_+w_est_dot*dT_w;

  w_est_ = w_est;
}

// Drone pose callback
void Executor::dronePose(const geometry_msgs::PoseStamped::ConstPtr& _msg)
{
  tf::pointMsgToEigen(_msg->pose.position, drone_pos_);
  tf::quaternionMsgToEigen(_msg->pose.orientation, drone_att_);
  drone_yaw_ = atan2(2*(drone_att_.w()*drone_att_.z()+drone_att_.x()*drone_att_.y()),1-2*(drone_att_.y()*drone_att_.y()+drone_att_.z()*drone_att_.z()));

  R_B_S = drone_att_.normalized().toRotationMatrix();
  R_B_I = R_S_I*R_B_S;
  p_B_I = R_S_I*drone_pos_;

  time_of_last_drone_pose = ros::Time::now().toSec();
  has_drone_pose_ = true;
}

// Drone velocity callback
void Executor::droneVelocity(const geometry_msgs::TwistStamped::ConstPtr& _msg)
{

  tf::twistMsgToEigen(_msg->twist, drone_twist_);

  drone_vel_ << drone_twist_(0,0), drone_twist_(1,0), drone_twist_(2,0);
  drone_ang_vel_ << drone_twist_(3,0), drone_twist_(4,0), drone_twist_(5,0);

  p_B_I_dot = R_S_I*drone_vel_;
  w_B = drone_ang_vel_;

  has_drone_vel_ = true;
}

// Drone state callback
void Executor::droneState(const uav_abstraction_layer::State::ConstPtr& _msg)
{
  drone_state_.state = _msg->state;
}

// Gimbal status callback
void Executor::gimbalStatus(const multidrone_msgs::GimbalStatus::ConstPtr& _msg)
{

  tf::quaternionMsgToEigen(_msg->orientation, gimbal_att_);
  R_C_I = Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(-M_PI, Eigen::Vector3d::UnitX())*gimbal_att_.normalized().toRotationMatrix();

  time_of_last_gimbal_status = ros::Time::now().toSec();
  has_gimbal_status_ = true;
}

bool Executor::ManualControlServiceCallback(multidrone_msgs::ManualControls::Request& req, multidrone_msgs::ManualControls::Response& res)
{
  multidrone_msgs::CameraControl camera_control_msg_;
  if (req.control == multidrone_msgs::ManualControls::Request::GIMBAL_MOVE_UP)
    target_image_offset_y -= target_image_offset_step;
  else if (req.control == multidrone_msgs::ManualControls::Request::GIMBAL_MOVE_DOWN)
    target_image_offset_y += target_image_offset_step;
  else if (req.control == multidrone_msgs::ManualControls::Request::GIMBAL_MOVE_LEFT)
    target_image_offset_x -= target_image_offset_step;
  else if (req.control == multidrone_msgs::ManualControls::Request::GIMBAL_MOVE_RIGHT)
    target_image_offset_x += target_image_offset_step;
  else if (req.control == multidrone_msgs::ManualControls::Request::CAMERA_ZOOM_IN){
    if (count < 20){
      count++;
      camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_ZOOM;
      camera_control_msg_.request.value = multidrone_msgs::CameraControl::Request::VALUE_INCREASE;
      camera_control_client_.call(camera_control_msg_);
    }
  }
  else if (req.control == multidrone_msgs::ManualControls::Request::CAMERA_ZOOM_OUT){
    if (count > 0){
      count--;
      camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_ZOOM;
      camera_control_msg_.request.value = multidrone_msgs::CameraControl::Request::VALUE_DECREASE;
      camera_control_client_.call(camera_control_msg_);
    }
  }
  else if (req.control == multidrone_msgs::ManualControls::Request::CONTROL_FOCUS_INCREASE) {
    camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_FOCUS;
    if(cmd < 212)
      cmd += inc;
    camera_control_msg_.request.value = cmd;
    camera_control_client_.call(camera_control_msg_);
  }
  else if (req.control == multidrone_msgs::ManualControls::Request::CONTROL_FOCUS_DECREASE) {
    camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_FOCUS;
    if(cmd > 44)
      cmd -= inc;
    camera_control_msg_.request.value = cmd;
    camera_control_client_.call(camera_control_msg_);
  }
  else if (req.control == multidrone_msgs::ManualControls::Request::CONTROL_RECORD_START) {
    if(!recording){
      camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_RECORD;
      camera_control_msg_.request.value = multidrone_msgs::CameraControl::Request::VALUE_INCREASE;
      camera_control_client_.call(camera_control_msg_);
      recording = true;
    } else ROS_WARN("Already recording");
  }
  else if (req.control == multidrone_msgs::ManualControls::Request::CONTROL_RECORD_STOP) {

    if(recording){
      camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_RECORD;
      camera_control_msg_.request.value = multidrone_msgs::CameraControl::Request::VALUE_DECREASE;
      camera_control_client_.call(camera_control_msg_);
      recording = false;
    } else ROS_WARN("Not recording");
  }
  else if (req.control == multidrone_msgs::ManualControls::Request::CAMERA_RESET) {
    camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CAMERA_RESET;
    camera_control_client_.call(camera_control_msg_);
    count = 0;
  } else if (req.control == multidrone_msgs::ManualControls::Request::FOCUS_RESET) {
    focus_reset_tree(54,92);
  } else if (req.control == multidrone_msgs::ManualControls::Request::GIMBAL_RETURN_HOME) {
    gimbal_home = true;
  }

  if (target_image_offset_x > target_image_offset_x_MAX)
    target_image_offset_x = target_image_offset_x_MAX;
  else if (target_image_offset_x < target_image_offset_x_MIN)
    target_image_offset_x = target_image_offset_x_MIN;
  if (target_image_offset_y > target_image_offset_y_MAX)
    target_image_offset_y = target_image_offset_y_MAX;
  else if (target_image_offset_y < target_image_offset_y_MIN)
    target_image_offset_y = target_image_offset_y_MIN;

    return true;
}

void Executor::focus_reset_tree(int beg, int end){
  // std::cout << " Starting Auto Focus" << "\n";
  int BEST, low_left, high_left = -1, low_right = -1, high_right, curr, curr_left, curr_right;
  bool  stop_left, stop_right;
  multidrone_msgs::CameraControl camera_control_msg_;
  camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_FOCUS;
  camera_control_msg_.request.manual_ctrl = false;
  f_reset = false;
  buffer[0] = -1;
  curr = (end+beg)/2 + (end+beg)%4;
  low_left = beg;
  high_right = end;

  camera_control_msg_.request.value = curr;
  camera_control_client_.call(camera_control_msg_);
  std::this_thread::sleep_for(std::chrono::milliseconds(400));

  BEST = curr;
  buffer[0] = focus_value;

  high_left = curr;
  low_right = curr;

  curr_left = (high_left+low_left)/2 - ((high_left+low_left)/2)%4;
  curr_right = (high_right+low_right)/2 + ((high_right+low_right)/2)%4;

  if(curr_left == curr){
    stop_left = true;
    buffer[1] = -1;
  }else stop_left = false;

  if(curr_right == curr){
    stop_right = true;
    buffer[2] = -1;
  }else stop_right = false;

  while(!stop_left || !stop_right){
    if(!stop_left){
      camera_control_msg_.request.value = curr_left;
      camera_control_client_.call(camera_control_msg_);
      std::this_thread::sleep_for(std::chrono::milliseconds(400));

      buffer[1] = focus_value;
      if(buffer[1] > buffer[0]){
        buffer[0] = buffer[1];
        BEST = curr_left;
      }
    }

    if(!stop_right){
      camera_control_msg_.request.value = curr_right;
      camera_control_client_.call(camera_control_msg_);
      std::this_thread::sleep_for(std::chrono::milliseconds(500));

      buffer[2] = focus_value;
      if(buffer[2] > buffer[0]){
        buffer[0] = buffer[2];
        BEST = curr_right;
      }
    }

    if(buffer[1] >= buffer[2]){
      curr = curr_left;
      high_right = curr;
    } else {
      curr = curr_right;
      low_left = curr;
    }

    high_left = curr;
    low_right = curr;

    curr_left = (high_left+low_left)/2 - ((high_left+low_left)/2)%4;
    curr_right = (high_right+low_right)/2 + ((high_right+low_right)/2)%4;

    if(curr_left == curr){
      stop_left = true;
      buffer[1] = -1;
    } else stop_left = false;

    if(curr_right == curr){
      stop_right = true;
      buffer[2] = -1;
    } else stop_right = false;

  }

  camera_control_msg_.request.value = BEST;
  camera_control_client_.call(camera_control_msg_);
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  f_reset = true;
  // std::cout << " Done. Focus value is  " << BEST <<"\n";
}

void Executor::setMountModeParameters()
{
  mavros_msgs::ParamGet get_param_in, get_param_out;
  get_param_in.request.param_id = "MNT_MODE_IN";
  get_param_out.request.param_id = "MNT_MODE_OUT";

  mavros_msgs::ParamSet set_param;

  ros::Time last_request = ros::Time::now();
  while(ros::ok())
  {
    if((ros::Time::now() - last_request > ros::Duration(5.0)))
    {
      if(get_param_client_.call(get_param_in) && get_param_in.response.success && get_param_in.response.value.integer != 3)
      {
        set_param.request.param_id = "MNT_MODE_IN";
        set_param.request.value.integer = 3;
        set_param_client_.call(set_param);
      }

      if(get_param_client_.call(get_param_out) && get_param_out.response.success && get_param_out.response.value.integer != 0)
      {
        set_param.request.param_id = "MNT_MODE_OUT";
        set_param.request.value.integer = 0;
        set_param_client_.call(set_param);
      }

      if(get_param_in.response.value.integer == 3 && get_param_out.response.value.integer == 0)
        break;

      last_request = ros::Time::now();
    }
    ros::spinOnce();
    ros::Duration(0.1).sleep();
  }
}

void Executor::toEulerAngles_YXZ(const Eigen::Quaterniond& _q, Eigen::Vector3d& euler)
{
  Eigen::Quaterniond q(Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(-M_PI, Eigen::Vector3d::UnitX())*_q.normalized().toRotationMatrix());

  double t0 = -2.0 * (q.x() * q.y() - q.w() * q.z());
  double t1 = q.w()*q.w() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z();
  double t2 = 2.0 * (q.y()*q.z() + q.w()*q.x());
  double t3 = -2.0 * (q.x()*q.z() - q.w()*q.y());
  double t4 = q.w()*q.w() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z();

  // 0
  euler.y() = atan2(t3, t4);

  // 1
  t2 = t2 > 1.0 ? 1.0 : t2;
  t2 = t2 < -1.0 ? -1.0 : t2;
  euler.x() = asin(t2);

  // 2
  euler.z() = atan2(t0, t1);
}

void Executor::rotationMatrixToEulerAngles_XYZ(Eigen::Matrix3d& R, Eigen::Vector3d& euler){
  if (R(2,0) < 1) {
    if (R(2,0) > -1) {
      euler.y() = asin(-R(2,0));
      euler.z() = atan2(R(1,0),R(0,0));
      euler.x() = atan2(R(2,1),R(2,2));
    } else { // R(2,0) = -1
      // Not a unique solution: euler.x() - euler.z() = atan2(-R(1,2),R(1,1))
      euler.y() = M_PI/2;
      euler.z() = -atan2(-R(1,2),R(1,1));
      euler.x() = 0;
    }
  } else { // R(2,0) = 1
// Not a unique solution: euler.x() + euler.z() = atan2(-R(1,2),R(1,1))
    euler.y() = -M_PI/2;
    euler.z() = atan2(-R(1,2),R(1,1));
    euler.x() = 0;
  }
}

void Executor::preemptCallback()
{
goal_preempt_ = true;
}

void Executor::toSMatrix(Eigen::Vector3d& w, Eigen::Matrix3d& S)
{
  S <<      0,  -w.z(),   w.y(),
        w.z(),       0,  -w.x(),
       -w.y(),   w.x(),       0;
}

void Executor::toSInvMatrix(Eigen::Matrix3d& R__, Eigen::Vector3d& S_inv__)
{
  S_inv__ << R__(2,1), R__(0,2), R__(1,0);
}

void Executor::toPIMatrix(Eigen::Vector3d& rx, Eigen::Matrix3d& PI)
{
  PI = Eigen::Matrix3d::Identity() - rx*rx.transpose();
}

void Executor::takeOff()
{
  feedback_.status= true;
  server_->publishFeedback(feedback_);
  uav_abstraction_layer::TakeOff take_off_msg;
  take_off_msg.request.height = list_wp_[0].point.z;
  take_off_msg.request.blocking = true;
  result_.goal_achieved = take_off_client_.call(take_off_msg);
  server_->setSucceeded(result_);
  action_type_ = EXECUTOR_IDLE;
}

void Executor::land()
{
  feedback_.status= true;
  server_->publishFeedback(feedback_);
  uav_abstraction_layer::Land land_msg;
  land_msg.request.blocking = true;
  result_.goal_achieved = land_client_.call(land_msg);
  server_->setSucceeded(result_);
  action_type_ = EXECUTOR_IDLE;
}

void Executor::goToWaypoint()
{
  if (cont < list_wp_.size()) {
    result_.goal_achieved=false;

    setpoint_pose_.header.stamp     = ros::Time::now();
    setpoint_pose_.header.frame_id  = "map";
    setpoint_pose_.pose.position.x  = list_wp_[cont].point.x;
    setpoint_pose_.pose.position.y  = list_wp_[cont].point.y;
    setpoint_pose_.pose.position.z  = list_wp_[cont].point.z;

    setpoint_pose_.pose.orientation.z = drone_att_.z();
    setpoint_pose_.pose.orientation.y = drone_att_.y();
    setpoint_pose_.pose.orientation.x = drone_att_.x();
    setpoint_pose_.pose.orientation.w = drone_att_.w();

    go_to_waypoint_pub_.publish(setpoint_pose_);

    if (drone_alarmed_when_doing_gotowaypoint_==true) { // Alarm received while doing the gotowaypoint, don't keep iterating.
      action_type_ = EXECUTOR_IDLE;
      action_received_ = false;
      drone_alarmed_when_doing_gotowaypoint_ = false;
      cont = 0;
      return;
    }
    wp << setpoint_pose_.pose.position.x , setpoint_pose_.pose.position.y , setpoint_pose_.pose.position.z;
    diff = (wp - drone_pos_).norm();

    if(diff<=0.5) { // if wp is reached, command next waypoint
      cont = cont +1;
    }

  } else if (cont == list_wp_.size()) {
    if (final_yaw_if_gotowaypoint_.z==0 && final_yaw_if_gotowaypoint_.y==0 && final_yaw_if_gotowaypoint_.x==0 && final_yaw_if_gotowaypoint_.w==0) {
      result_.goal_achieved=true;
      server_->setSucceeded(result_);
      action_type_ = EXECUTOR_IDLE;
      action_received_ = false;
      drone_alarmed_when_doing_gotowaypoint_ = false;
      cont = 0;
      return;
    }
    // Last go to wp at the final position of the path but with the final yaw:
    setpoint_pose_.header.stamp     = ros::Time::now();
    setpoint_pose_.header.frame_id  = "map";
    setpoint_pose_.pose.position.x  = list_wp_[cont-1].point.x;
    setpoint_pose_.pose.position.y  = list_wp_[cont-1].point.y;
    setpoint_pose_.pose.position.z  = list_wp_[cont-1].point.z;

    setpoint_pose_.pose.orientation.z = final_yaw_if_gotowaypoint_.z;
    setpoint_pose_.pose.orientation.y = final_yaw_if_gotowaypoint_.y;
    setpoint_pose_.pose.orientation.x = final_yaw_if_gotowaypoint_.x;
    setpoint_pose_.pose.orientation.w = final_yaw_if_gotowaypoint_.w;

    go_to_waypoint_pub_.publish(setpoint_pose_);
    if (drone_alarmed_when_doing_gotowaypoint_==true) { // Alarm received while doing the gotowaypoint, don't keep iterating.
      action_type_ = EXECUTOR_IDLE;
      action_received_ = false;
      drone_alarmed_when_doing_gotowaypoint_ = false;
      cont = 0;
      return;
    }

    // Desired yaw (z-axis rotation) in radians:
    double desired_yaw = atan2(2*(final_yaw_if_gotowaypoint_.w*final_yaw_if_gotowaypoint_.z+final_yaw_if_gotowaypoint_.x*final_yaw_if_gotowaypoint_.y),\
    1-2*(final_yaw_if_gotowaypoint_.y*final_yaw_if_gotowaypoint_.y+final_yaw_if_gotowaypoint_.z*final_yaw_if_gotowaypoint_.z));

    diff = abs(desired_yaw - drone_yaw_);

    if(diff<=0.261799388) { // if wp is reached (difference <= 10 sexagesimal degrees), goal achieved
      result_.goal_achieved=true;
      server_->setSucceeded(result_);
      action_type_ = EXECUTOR_IDLE;
      action_received_ = false;
      drone_alarmed_when_doing_gotowaypoint_ = false;
      cont = 0;
    }
  }
}

void Executor::followVehicle()
{
  desired_point << trailer.desired_point_, trailer.altitude;

  double pos_error_tanh_x = MAX_DRONE_SPEED*tanh(K_DRONE*(drone_pos_[0]-desired_point[0])/MAX_DRONE_SPEED);
  double pos_error_tanh_y = MAX_DRONE_SPEED*tanh(K_DRONE*(drone_pos_[1]-desired_point[1])/MAX_DRONE_SPEED);
  double pos_error_tanh_z = MAX_DRONE_SPEED*tanh(K_DRONE*(drone_pos_[2]-desired_point[2])/MAX_DRONE_SPEED);
  Eigen::Vector3d pos_error_tanh(pos_error_tanh_x,pos_error_tanh_y,pos_error_tanh_z);
  Eigen::Vector3d v_;
  v_ << trailer.v_desired, 0;
  cmd_linear_velocity = -pos_error_tanh + v_;

  tf::vectorEigenToMsg(cmd_linear_velocity, cmd_linear_velocity_);

  setpoint_vel_.header.stamp = ros::Time::now();
  setpoint_vel_.header.frame_id = "map";
  if(!std::isnan(cmd_linear_velocity.x()) && !std::isnan(cmd_linear_velocity.y()) && !std::isnan(cmd_linear_velocity.z())
  && !std::isinf(cmd_linear_velocity.x()) && !std::isinf(cmd_linear_velocity.y()) && !std::isinf(cmd_linear_velocity.z()))
    setpoint_vel_.twist.linear = cmd_linear_velocity_;
  else {
    action_type_ = EXECUTOR_IDLE;
    feedback_.status = false;
    feedback_.action_id = action_id_;
    server_->publishFeedback(feedback_);
    ROS_ERROR("Got NAN, shooting action will be discarded. \ndesired_point =  %f, %f, %f  \nv_ = %f, %f, %f \nv_target =  %f, %f \nv_trailer_ = %f, %f \nw_trailer_ = %f \nTs = %f \n", desired_point.x(), desired_point.y(), desired_point.z(), v_.x(),v_.y(),v_.z(), trailer.v_target.x(), trailer.v_target.y(), trailer.v_trailer_.x(), trailer.v_trailer_.y(), trailer.w_trailer, trailer.Ts[trailer.filter-2]);
    return;
  }

  setpoint_vel_.twist.angular.x = 0;
  setpoint_vel_.twist.angular.y = 0;

  if (shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_FLY_THROUGH || shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_LATERAL || shooting_mode_ == multidrone_msgs::TargetIdentifierType::NONE)
  	    drone_heading_error = remainder((drone_yaw_ - trailer.a_), 2*M_PI);
  else if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_ORBIT){
    Eigen::Vector3d heading = (drone_pos_ - rt_target_pos_);
    if (trailer.angular_speed > 0)
      heading = heading.cross(-Eigen::Vector3d::UnitZ());
    else
      heading = heading.cross(Eigen::Vector3d::UnitZ());
    drone_heading_error = remainder((drone_yaw_ - atan2(heading.y(),heading.x())), 2*M_PI);
  }
  else
    drone_heading_error = remainder((drone_yaw_ - atan2 (shooting_target_pos_.y() - drone_pos_.y(), shooting_target_pos_.x() - drone_pos_.x())), 2*M_PI);

  if (drone_heading_error > M_PI)
    drone_heading_error = -2*M_PI + drone_heading_error;
  setpoint_vel_.twist.angular.z = MAX_YAW_RATE*tanh((-K_DRONE_YAW*drone_heading_error + trailer.w_trailer)/MAX_YAW_RATE);

  goal_direction_ = setpoint_vel_.twist.linear;
  wished_mov_dir_pub_.publish(goal_direction_);
  set_velocity_pub_.publish(setpoint_vel_);
}

/**
 *
 */
void Executor::collisionWarningCallback(const std_msgs::Bool::ConstPtr& collision_warning)
{
  confl_warning_ = collision_warning->data;
}

/**
 *
 */
void Executor::avoidMovementCallback(const geometry_msgs::Vector3::ConstPtr& avoidance_direction)
{
    avoid_mov_direction_.x = avoidance_direction->x;
    avoid_mov_direction_.y = avoidance_direction->y;
    avoid_mov_direction_.z = avoidance_direction->z;
}

void Executor::actionCallback()
{
  // If the new drone action is a gotowaypoint and right now is running another gotowaypoint, it means that an alarm has been triggered while doing it.
  // In this case wait until the current gotowaypoint finish to start the next alarm's gotowaypoint.
  if (action_type_==multidrone_msgs::DroneAction::TYPE_GOTOWAYPOINT && goal_.action_type==multidrone_msgs::DroneAction::TYPE_GOTOWAYPOINT) {
    drone_alarmed_when_doing_gotowaypoint_ = true;
    while (drone_alarmed_when_doing_gotowaypoint_ && ros::ok) {
      ros::Rate loop_rate(1); // [Hz]
      loop_rate.sleep();
    }
  }

  timer_rt.stop();
  timer_trailer.stop();
  running_trailer = false;
  action_received_ = true;
  start_time = ros::Time::now().toSec();
  goal_ = server_->acceptNewGoal()->action_goal;

  // saving list of waypoints
  cont = 0;
  list_wp_.clear();
  action_type_= goal_.action_type;
  action_id_ = goal_.action_id;

  // Save final_yaw_if_gotowaypoint from this drone action into its class attribute, so that later a new last waypoint with different yaw can be sent in the navigation actions.
  final_yaw_if_gotowaypoint_.x = goal_.final_yaw_if_gotowaypoint.x;
  final_yaw_if_gotowaypoint_.y = goal_.final_yaw_if_gotowaypoint.y;
  final_yaw_if_gotowaypoint_.z = goal_.final_yaw_if_gotowaypoint.z;
  final_yaw_if_gotowaypoint_.w = goal_.final_yaw_if_gotowaypoint.w;

  if (action_type_ == multidrone_msgs::DroneAction::TYPE_SHOOTING) {
    shooting_parameters_.clear();
    target_parameters_.clear();
    for(int i = 0; i<goal_.shooting_action.shooting_roles[0].shooting_parameters.size(); i++)
      shooting_parameters_[goal_.shooting_action.shooting_roles[0].shooting_parameters[i].param] = goal_.shooting_action.shooting_roles[0].shooting_parameters[i].value;
    for(int i = 0; i<goal_.shooting_action.shooting_roles[0].target_parameters.size(); i++)
      target_parameters_[goal_.shooting_action.shooting_roles[0].target_parameters[i].param] = goal_.shooting_action.shooting_roles[0].target_parameters[i].value;
    for(int i = 0; i<goal_.shooting_action.rt_trajectory.size(); i++)
      list_wp_.push_back(goal_.shooting_action.rt_trajectory[i]);

    shooting_action_                =  goal_.shooting_action.shooting_roles[0].shooting_type.type;
    trailer.originOfFormation_(0)   =  goal_.shooting_action.rt_displacement.x + list_wp_[0].point.x;
    trailer.originOfFormation_(1)   =  goal_.shooting_action.rt_displacement.y + list_wp_[0].point.y;
    duration_                       =  goal_.shooting_action.duration;
    length_                         =  goal_.shooting_action.length;
    rt_mode_                        =  goal_.shooting_action.rt_mode;
    // std::cout << "rt_mode_ is " << (int)rt_mode_ << "\n";
    if (rt_mode_ != multidrone_msgs::ShootingAction::RT_MODE_VIRTUAL_TRAJ){
      rt_id_                        =  goal_.shooting_action.rt_id;
      // std::cout << "rt_id_ is " << (int)rt_id_ << "\n";
    }
    shooting_mode_                  =  goal_.shooting_action.shooting_roles[0].target_identifier_type.type;

    if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::GPS){
      shooting_id_                  =  (uint8_t) target_parameters_["ID"];
      // std::cout << "shooting_id_ is " << (int)shooting_id_ << "\n";
    }
    else if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL){
      multidrone_msgs::FollowTarget follow_target_msg;
      follow_target_msg.request.target_id = 0;
      follow_target_msg.request.target_type.type = 0;
      follow_target_client_.call(follow_target_msg);
    }
    else if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL_GPS){
      multidrone_msgs::FollowTarget follow_target_msg;
      shooting_id_                  =  (uint8_t) target_parameters_["ID"];
      follow_target_msg.request.target_id = 0;
      follow_target_msg.request.target_type.type = 1;
      follow_target_client_.call(follow_target_msg);
      // std::cout << "VISUAL_GPS shooting_id_ is " << (int)shooting_id_ << "\n";
    }
    else if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::NONE){
      pan_s                         =  target_parameters_["pan_s"];
      tilt_s                        =  target_parameters_["tilt_s"];
      pan_e                         =  target_parameters_["pan_e"];
      tilt_e                        =  target_parameters_["tilt_e"];
      }
    else if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::STATIC){
      latitude                      =  target_parameters_["latitude"];
      longitude                     =  target_parameters_["longitude"];
      altitude                      =  target_parameters_["altitude"];
    }

    if (rt_mode_ == multidrone_msgs::ShootingAction::RT_MODE_VIRTUAL_TRAJ || shooting_mode_ == multidrone_msgs::TargetIdentifierType::VIRTUAL){
      total_displ << (list_wp_[list_wp_.size()-1].point.x - list_wp_[0].point.x) , (list_wp_[list_wp_.size()-1].point.y -list_wp_[0].point.y) , (list_wp_[list_wp_.size()-1].point.z -list_wp_[0].point.z);
      formation_speed_ = goal_.shooting_action.formation_speed;
      if (total_displ.norm() == 0){
        total_displ << 1,1,1;
        formation_speed_ = 0;
      }
      if (rt_mode_ == multidrone_msgs::ShootingAction::RT_MODE_VIRTUAL_TRAJ){
        rt_target_pos_.x() = list_wp_[0].point.x;
        rt_target_pos_.y() = list_wp_[0].point.y;
        rt_target_pos_.z() = list_wp_[0].point.z;
      }
      if (shooting_mode_ == multidrone_msgs::TargetIdentifierType::VIRTUAL){
        shooting_target_pos_.x() = list_wp_[0].point.x;
        shooting_target_pos_.y() = list_wp_[0].point.y;
        shooting_target_pos_.z() = list_wp_[0].point.z;
      }
      timer_rt.start();
    }

    //trailer parameters
    trailer.initial_azimuth   = 0;
    trailer.radius            = 0;
    trailer.angular_speed     = 0;
    trailer.rx                = 0;
    trailer.rx_e              = 0;
    trailer.rx_s              = 0;
    trailer.ry                = 0;
    trailer.rx_dot            = 0;
    trailer.ry_dot            = 0;
    trailer.altitude          = 0;
    trailer.z_e               = 0;
    trailer.altitude_dot      = 0;
    trailer.d                 = trailer_size;
    if (rt_mode_ != multidrone_msgs::ShootingAction::RT_MODE_ACTUAL_TARGET)
      trailer.d               = 0;

    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_STATIC) //0
    {
      trailer.altitude          = shooting_parameters_["z_0"];
      trailer.trailer_type      = TRAILER_TYPE_INERTIAL;
    }
    else if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_FLY_THROUGH && rt_mode_ == multidrone_msgs::ShootingAction::RT_MODE_VIRTUAL_TRAJ) //1
    {
      trailer.altitude          = shooting_parameters_["z_0"];
      trailer.trailer_type      = TRAILER_TYPE_VARIABLE;
    }
    else if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_ESTABLISH) //2
    {
      trailer.rx_s              = shooting_parameters_["x_s"];
      trailer.rx_e              = shooting_parameters_["x_e"];
      trailer.rx_dot            = (shooting_parameters_["x_e"] - shooting_parameters_["x_s"])/duration_.data.sec;
      trailer.altitude          = shooting_parameters_["z_s"];
      trailer.z_e               = shooting_parameters_["z_e"];
      trailer.altitude_dot      = (shooting_parameters_["z_e"] - shooting_parameters_["z_s"])/duration_.data.sec;
      trailer.trailer_type      = TRAILER_TYPE_VARIABLE;
    }
    else if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_CHASE || shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_LEAD) //3
    {
      trailer.rx_s              = shooting_parameters_["x_s"];
      trailer.rx_e              = shooting_parameters_["x_e"];
      trailer.rx_dot            = (shooting_parameters_["x_e"] - shooting_parameters_["x_s"])/duration_.data.sec;
      trailer.altitude          = shooting_parameters_["z_0"];
      trailer.trailer_type      = TRAILER_TYPE_VARIABLE;
    }
    else if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_FLYBY) //5
    {
      trailer.rx_s              = shooting_parameters_["x_s"];
      trailer.rx_e              = shooting_parameters_["x_e"];
      trailer.ry                = shooting_parameters_["y_0"];
      trailer.rx_dot            = (shooting_parameters_["x_e"] - shooting_parameters_["x_s"])/duration_.data.sec;
      trailer.altitude          = shooting_parameters_["z_0"];
      trailer.trailer_type      = TRAILER_TYPE_VARIABLE;
    }
    else if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_LATERAL) //6
    {
      trailer.ry                = shooting_parameters_["y_0"];
      trailer.altitude          = shooting_parameters_["z_0"];
      trailer.trailer_type      = TRAILER_TYPE_VARIABLE;
    }
    else if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_ELEVATOR) //7
    {
      trailer.altitude          = shooting_parameters_["z_s"];
      trailer.z_e               = shooting_parameters_["z_e"];
      trailer.altitude_dot      = (shooting_parameters_["z_e"] - shooting_parameters_["z_s"])/duration_.data.sec;
      trailer.trailer_type      = TRAILER_TYPE_INERTIAL;
    }
    else  if(shooting_action_ == multidrone_msgs::ShootingType::SHOOT_TYPE_ORBIT) //8
    {
      trailer.initial_azimuth  = shooting_parameters_["azimuth_s"];
      trailer.radius           = shooting_parameters_["r_0"];
      trailer.angular_speed    = shooting_parameters_["azimuth_speed"];
      trailer.altitude         = shooting_parameters_["z_0"];
      trailer.trailer_type     = TRAILER_TYPE_ORBIT;
    }
    else {
      action_type_ = EXECUTOR_IDLE;
      feedback_.status = false;
      feedback_.action_id = action_id_;
      server_->publishFeedback(feedback_);
      return;
    }
    // std::cout << "shooting_action_ is " << (int)shooting_action_ << "\n";
    // std::cout << "trailer.ry is "       << trailer.ry            << "\n";
    // std::cout << "trailer.altitude is " << trailer.altitude      << "\n";

    trailer.r << trailer.rx, trailer.ry;
    trailer.r_dot << trailer.rx_dot, trailer.ry_dot;

    parameters_set = true;

    multidrone_msgs::SetFramingType set_framing_type_msg;
    set_framing_type_msg.request.target_framing_type.type = goal_.shooting_action.shooting_roles[0].framing_type.type;
    set_framing_type_client_.call(set_framing_type_msg);
  }
  else //Takeoff, land or gotowaypoint
    for(int i = 0; i<goal_.path.size(); i++)
      list_wp_.push_back(goal_.path[i]);
}

void Executor::gimbalAutomatic(double pan_s, double tilt_s, double pan_e, double tilt_e, double duration, double start_time)
{
  double t    = ros::Time::now().toSec() - start_time;
  double pan  = pan_s + t*(pan_e - pan_s)/duration;
  double tilt = tilt_s + t*(tilt_e - tilt_s)/duration;

  toEulerAngles_YXZ(gimbal_att_,gimbal_euler);
  geometry_msgs::Vector3 msg_gimbal_cmd;

  msg_gimbal_cmd.x = - gimbal_euler.x()*TO_DEG;
  msg_gimbal_cmd.y = 1.5*(tilt - gimbal_euler.y()*TO_DEG);
  msg_gimbal_cmd.z = 1.5*(pan - (gimbal_euler.z()+drone_yaw_)*TO_DEG);

  if(t>=duration){
    msg_gimbal_cmd.y = 0;
    msg_gimbal_cmd.z = 0;
  }
    gimbal_cmd_basecam_pub.publish(msg_gimbal_cmd);

  if(simulation){
    rotationMatrixToEulerAngles_XYZ(R_x, gimbal_euler);

    if(std::abs(gimbal_euler[0]) >= 3){ //to avoid singularity on simulated gimbal
      gimbal_euler[0] = gimbal_euler[0] - M_PI;
      gimbal_euler[1] = M_PI - gimbal_euler[1];
      gimbal_euler[2] = gimbal_euler[2] - M_PI;
    }
    if (!has_gimbal_status_)
      R_C_I = R_x;

    gimbal_cmd_.request.param1 = tilt -90;  // pitch
		gimbal_cmd_.request.param2 = gimbal_euler.x()*TO_DEG;;// roll
		gimbal_cmd_.request.param3 = pan +drone_yaw_*TO_DEG;  // yaw
		if(t<duration)
      cmd_long_client_.call(gimbal_cmd_);
  }
}

void Executor::gimbalSafeMode()
{
    w_est << 0.0, 0.0, 0.0;
    w_est_ << 0.0, 0.0, 0.0;
    t_w_ = ros::Time::now().toSec();

    geometry_msgs::Vector3 msg_gimbal_cmd;
    msg_gimbal_cmd.x = 0;
    msg_gimbal_cmd.y = 0;
    msg_gimbal_cmd.z = 0;
    gimbal_cmd_basecam_pub.publish(msg_gimbal_cmd);
}

void Executor::gimbalController()
{

  if (has_drone_pose_ && has_shooting_target_status_ ){ //GPS
    o(0) = K.inverse()(1, 1) * -target_image_offset_y;
    o(1) = K.inverse()(0, 0) * target_image_offset_x;
    o(2) = 0.0;
    q_I = p_T_I - p_B_I - R_B_I*p_C_B + R_C_I*o*q_C.z();
    q_I_dot << 0.0, 0.0, 0.0;
    q_norm = R_C_I.transpose()*(q_I/q_I.norm());
    r_x_3 = q_I/q_I.norm();
    a = r_x_3.cross(-Eigen::Vector3d::UnitZ());
    has_visual_2D_target_status_ = false;
  }
  else if (has_drone_pose_ && shooting_mode_ == multidrone_msgs::TargetIdentifierType::STATIC){ // static
    p_T_I << latitude, longitude, altitude;
    q_I = p_T_I - p_B_I - R_B_I*p_C_B;
    q_I_dot << 0.0, 0.0, 0.0;
    q_norm = R_C_I.transpose()*(q_I/q_I.norm());
    r_x_3 = q_I/q_I.norm();
    a = r_x_3.cross(-Eigen::Vector3d::UnitZ());
    has_visual_2D_target_status_ = false;
  }
  else{
    gimbalSafeMode();
    return;
  }

  r_x_2 = a/a.norm();
  b = r_x_2.cross(r_x_3);
  r_x_1 = b/b.norm();
  R_x.col(0) = r_x_1;
  R_x.col(1) = r_x_2;
  R_x.col(2) = r_x_3;

  if (!has_visual_2D_target_status_)
    R_e = R_C_I.transpose()*R_x;
  else
    R_e = R_x;

  R_e_R_e = R_e-R_e.transpose();
  toSInvMatrix(R_e_R_e,w_e);

  // Calculate Lyapunov function
  V_1_ = Eigen::MatrixXd::Identity(3,3)-R_e;
  lyapunov_visual = V_1_(0,0)+V_1_(1,1)+V_1_(2,2);

  w = k_1*w_e;

  if(shooting_action_ != multidrone_msgs::ShootingType::SHOOT_TYPE_LATERAL && shooting_action_ != multidrone_msgs::ShootingType::SHOOT_TYPE_STATIC &&
     shooting_action_ != multidrone_msgs::ShootingType::SHOOT_TYPE_ELEVATOR){
       run_w_estimate();
       w = w_est + k_1*w_e;
  }

  toEulerAngles_YXZ(gimbal_att_,gimbal_euler);

  Q_inv <<                       cos(gimbal_euler.y()),  0,                         sin(gimbal_euler.y()),
           sin(gimbal_euler.x())*sin(gimbal_euler.y()),  1,  -sin(gimbal_euler.x())*cos(gimbal_euler.y()),
                                -sin(gimbal_euler.y()),  0,                         cos(gimbal_euler.y());
  euler_dot_cmd = Q_inv*w;

  geometry_msgs::Vector3 msg_gimbal_cmd;
  msg_gimbal_cmd.x = euler_dot_cmd[0]*TO_DEG;
  msg_gimbal_cmd.y = euler_dot_cmd[1]*TO_DEG;
  msg_gimbal_cmd.z = euler_dot_cmd[2]*TO_DEG;

  gimbal_cmd_basecam_pub.publish(msg_gimbal_cmd);

  if(simulation){
    if ((shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL || shooting_mode_ == multidrone_msgs::TargetIdentifierType::VISUAL_GPS) && has_visual_2D_target_status_) {R_x = R_C_I*R_e;}

    rotationMatrixToEulerAngles_XYZ(R_x, gimbal_euler);

    if(std::abs(gimbal_euler[0]) >= 3){ //to avoid singularity on simulated gimbal
      gimbal_euler[0] = gimbal_euler[0] - M_PI;
      gimbal_euler[1] = M_PI - gimbal_euler[1];
      gimbal_euler[2] = gimbal_euler[2] - M_PI;
    }
    if (!has_gimbal_status_)
      R_C_I = R_x;

    gimbal_cmd_.request.param1 = gimbal_euler[1]*TO_DEG-90;  // pitch
		gimbal_cmd_.request.param2 = gimbal_euler[0]*TO_DEG;     // roll
		gimbal_cmd_.request.param3 = gimbal_euler[2]*TO_DEG+drone_yaw_*TO_DEG;  // yaw

		cmd_long_client_.call(gimbal_cmd_);
  }
}

void Executor::CameraControl(){
  multidrone_msgs::CameraControl camera_control_msg_;
  camera_control_msg_.request.control_function = multidrone_msgs::CameraControl::Request::CONTROL_FUNCTION_ZOOM;
  camera_control_msg_.request.manual_ctrl = false;

  if (zoom_error > 0.05){
    if (count > 0){
      camera_control_msg_.request.value = uint8_t(127 - 88 * (zoom_error-0.05));
      camera_control_client_.call(camera_control_msg_);
      count--;
    }
  }
  else if (zoom_error < -0.05){
    if (count < count_lim){
       camera_control_msg_.request.value = uint8_t(129 - 88 * (zoom_error+0.05));
       camera_control_client_.call(camera_control_msg_);
       count++;
  }
}
}


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 Copyright (C) 2017 Aareal Bank AG
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/date_time.hpp>
#include <boost/make_shared.hpp>
#include <ql/cashflows/cashflows.hpp>
#include <ql/cashflows/coupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/yield/zerospreadedtermstructure.hpp>
#include <qle/pricingengines/discountingriskybondengine.hpp>

using namespace std;
using namespace QuantLib;

namespace QuantExt {

DiscountingRiskyBondEngine::DiscountingRiskyBondEngine(const Handle<YieldTermStructure>& discountCurve,
                                                       const Handle<DefaultProbabilityTermStructure>& defaultCurve,
                                                       const Handle<Quote>& recoveryRate,
                                                       const Handle<Quote>& securitySpread, Period timestepPeriod,
                                                       boost::optional<bool> includeSettlementDateFlows)
    : defaultCurve_(defaultCurve), recoveryRate_(recoveryRate), securitySpread_(securitySpread),
      timestepPeriod_(timestepPeriod), includeSettlementDateFlows_(includeSettlementDateFlows) {
    discountCurve_ =
        securitySpread_.empty()
            ? discountCurve
            : Handle<YieldTermStructure>(boost::make_shared<ZeroSpreadedTermStructure>(discountCurve, securitySpread));
    registerWith(discountCurve_);
    registerWith(defaultCurve_);
    registerWith(recoveryRate_);
    registerWith(securitySpread_);
}

DiscountingRiskyBondEngine::DiscountingRiskyBondEngine(const Handle<YieldTermStructure>& discountCurve,
                                                       const Handle<Quote>& securitySpread, Period timestepPeriod,
                                                       boost::optional<bool> includeSettlementDateFlows)
    : securitySpread_(securitySpread), timestepPeriod_(timestepPeriod),
      includeSettlementDateFlows_(includeSettlementDateFlows) {
    discountCurve_ =
        securitySpread_.empty()
            ? discountCurve
            : Handle<YieldTermStructure>(boost::make_shared<ZeroSpreadedTermStructure>(discountCurve, securitySpread));
    registerWith(discountCurve_);
    registerWith(securitySpread_);
}

void DiscountingRiskyBondEngine::calculate() const {
    QL_REQUIRE(!discountCurve_.empty(), "discounting term structure handle is empty");

    results_.valuationDate = (*discountCurve_)->referenceDate();
    results_.value = calculateNpv(results_.valuationDate, arguments_.cashflows);

    bool includeRefDateFlows =
        includeSettlementDateFlows_ ? *includeSettlementDateFlows_ : Settings::instance().includeReferenceDateEvents();
    // a bond's cashflow on settlement date is never taken into
    // account, so we might have to play it safe and recalculate
    // same parameters as above, we can avoid another call
    if (!includeRefDateFlows && results_.valuationDate == arguments_.settlementDate) {
        results_.settlementValue = results_.value;
    } else {
        // no such luck
        results_.settlementValue = calculateNpv(arguments_.settlementDate, arguments_.cashflows);
    }
}

Real DiscountingRiskyBondEngine::calculateNpv(Date npvDate, const Leg& cashflows,
                                              const Handle<YieldTermStructure>& incomeCurve,
                                              const bool conditionalOnSurvival) const {
    Real npvValue = 0;

    // handle case where we wish to price simply with benchmark curve and scalar security spread
    // i.e. credit curve term structure (and recovery) have not been specified
    // we set the default probability and recovery rate to zero in this instance (issuer credit worthiness already
    // captured within security spread)
    boost::shared_ptr<DefaultProbabilityTermStructure> creditCurvePtr =
        defaultCurve_.empty()
            ? boost::make_shared<QuantLib::FlatHazardRate>(results_.valuationDate, 0.0, discountCurve_->dayCounter())
            : defaultCurve_.currentLink();
    Rate recoveryVal = recoveryRate_.empty() ? 0.0 : recoveryRate_->value();

    // compounding factors for npv date
    Real dfSettl = incomeCurve.empty() ? discountCurve_->discount(npvDate) : incomeCurve->discount(npvDate);
    Real spSettl = conditionalOnSurvival ? creditCurvePtr->survivalProbability(npvDate) : 1.0;

    Size numCoupons = 0;
    bool hasLiveCashFlow = false;
    for (Size i = 0; i < cashflows.size(); i++) {
        boost::shared_ptr<CashFlow> cf = cashflows[i];
        if (cf->hasOccurred(npvDate, includeSettlementDateFlows_))
            continue;
        hasLiveCashFlow = true;

        // Coupon value is discounted future payment times the survival probability
        Probability S = creditCurvePtr->survivalProbability(cf->date()) / spSettl;
        npvValue += cf->amount() * S * discountCurve_->discount(cf->date()) / dfSettl;

        /* The amount recovered in the case of default is the recoveryrate*Notional*Probability of
           Default; this is added to the NPV value. For coupon bonds the coupon periods are taken
           as the timesteps for integrating over the probability of default.
        */
        boost::shared_ptr<Coupon> coupon = boost::dynamic_pointer_cast<Coupon>(cf);
        if (coupon) {
            numCoupons++;
            Date startDate = coupon->accrualStartDate();
            Date endDate = coupon->accrualEndDate();
            Date effectiveStartDate = (startDate <= npvDate && npvDate <= endDate) ? npvDate : startDate;
            Date defaultDate = effectiveStartDate + (endDate - effectiveStartDate) / 2;
            Probability P = creditCurvePtr->defaultProbability(effectiveStartDate, endDate) / spSettl;

            npvValue += coupon->nominal() * recoveryVal * P * discountCurve_->discount(defaultDate) / dfSettl;
        }
    }

    // the ql instrument might not yet be expired and still have not anything to value if
    // the npvDate > evaluation date
    if (!hasLiveCashFlow)
        return 0.0;

    if (cashflows.size() > 1 && numCoupons == 0) {
        QL_FAIL("DiscountingRiskyBondEngine does not support bonds with multiple cashflows but no coupons");
    }

    /* If there are no coupon, as in a Zero Bond, we must integrate over the entire period from npv date to
       maturity. The timestepPeriod specified is used as provide the steps for the integration. This only applies
       to bonds with 1 cashflow, identified as a final redemption payment.
    */
    if (cashflows.size() == 1) {
        boost::shared_ptr<Redemption> redemption = boost::dynamic_pointer_cast<Redemption>(cashflows[0]);
        if (redemption) {
            Date startDate = npvDate;
            while (startDate < redemption->date()) {
                Date stepDate = startDate + timestepPeriod_;
                Date endDate = (stepDate > redemption->date()) ? redemption->date() : stepDate;
                Date defaultDate = startDate + (endDate - startDate) / 2;
                Probability P = creditCurvePtr->defaultProbability(startDate, endDate) / spSettl;

                npvValue += redemption->amount() * recoveryVal * P * discountCurve_->discount(defaultDate) / dfSettl;
                startDate = stepDate;
            }
        }
    }

    return npvValue;
}
} // namespace QuantExt


/**
 * @copyright   Copyright (c) 2021, Swift Engineering Inc.
 * @license     Licensed under the MIT license. See LICENSE for details.
 */
#include "TestUtils.hpp"

#include <iostream>

#include <string>
#include <sstream>
#include <boost/array.hpp>
#include <stdio.h>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>

#include "IWindModel.hpp"
#include "GaussianWindModel.hpp"

#define PERCENT_DIFF_100(a, b) ((a == 0) && (b == 0)) ? 0 : (2.0 * abs(a - b) / (a + b) * 100.0)
#define EXPECT_PERCENT_DIFF_LT(a, b, tolerance_100) EXPECT_LT(PERCENT_DIFF_100(a, b), tolerance_100)

namespace avionics_sim {

struct WorldFrameCaseParams {
    std::string case_name;
    ignition::math::Pose3d pose_world_m;
    ignition::math::Vector3d velocity_world_m_per_s;
    ignition::math::Vector3d expectedRates_body_m_per_s;
};

class IWindModelTest : public ::testing::TestWithParam<WorldFrameCaseParams> {
  protected:
    avionics_sim::IWindModel *wind_model_;
    double tolerance_100 = 0.5; /**< Allowable percent tolerance, note this is 0.5%, not 50% */

    static void SetUpTestSuite() {
    }

    static void TearDownTestSuite() {
        // delete shared_resource_;
    }

    virtual void SetUp() {
        // Its not optimal to use a non-mocked RNG, but with a defined seed, there is a defined output
        std::default_random_engine str_generator(1);
        std::default_random_engine dir_generator(2);

        wind_model_ = new GaussianWindModel(str_generator, dir_generator);
    }

    virtual void TearDown() {
    }
};

TEST_P(IWindModelTest, AerodynamicForcesFromVelocityAndOrientation) {
    WorldFrameCaseParams params = GetParam();

    // Given:
    // Initial Position, Rotation and Velocity from params
    double altitude = 0;
    ignition::math::Vector3d velocity = params.velocity_world_m_per_s;
    ignition::math::Pose3d orientation = params.pose_world_m;

    // When:
    WindRate wind_rates = wind_model_->get_rates();

    // Then:
    // The Resultant Forces should equal the expected forces
    v3 expectedLinearRates_m_per_s = params.expectedRates_body_m_per_s;
    std::cout << "Rates: " << wind_rates.linear_rate << "\n";

    EXPECT_NEAR(wind_rates.linear_rate.X(), expectedLinearRates_m_per_s.X(), tolerance_100);
    EXPECT_NEAR(wind_rates.linear_rate.Y(), expectedLinearRates_m_per_s.Y(), tolerance_100);
    EXPECT_NEAR(wind_rates.linear_rate.Z(), expectedLinearRates_m_per_s.Z(), tolerance_100);
}

const std::vector<WorldFrameCaseParams> params{
    {
        "Home Work Problem alpha = 4 deg, beta = -5.1587 deg",
        ignition::math::Pose3d(
            ignition::math::Vector3d(-0.09, 1.48, 0.1),
            ignition::math::Quaterniond(1, 0, 0, 0)),
        ignition::math::Vector3d(0, 0, 0),
        ignition::math::Vector3d(0, 0, 0)
    }
};

INSTANTIATE_TEST_CASE_P(WindModelTests,
                        IWindModelTest,
                        ::testing::ValuesIn(params));

}  // namespace avionics_sim


#include <fstream>
#include <iostream>
#include <math.h>
#define png_infopp_NULL (png_infopp)NULL
#define int_p_NULL (int*)NULL
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <boost/shared_ptr.hpp>
#include <sdf/sdf.hh>
#include <ignition/math/Vector3.hh>

#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "collision_map_request.pb.h"

namespace gazebo
{
typedef const boost::shared_ptr<
  const collision_map_creator_msgs::msgs::CollisionMapRequest>
    CollisionMapRequestPtr;

class CollisionMapCreator : public WorldPlugin
{
  transport::NodePtr node;
  transport::PublisherPtr imagePub;
  transport::SubscriberPtr commandSubscriber;
  physics::WorldPtr world;

  public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
  {
    node = transport::NodePtr(new transport::Node());
    world = _parent;
    // Initialize the node with the world name
    node->Init(world->Name());
    std::cout << "Subscribing to: " << "~/collision_map/command" << std::endl;
    commandSubscriber = node->Subscribe("~/collision_map/command",
      &CollisionMapCreator::create, this);
    imagePub = node->Advertise<msgs::Image>("~/collision_map/image");
  }

  public: void create(CollisionMapRequestPtr &msg)
  {
    std::cout << "Received message" << std::endl;

    std::cout << "Creating collision map with corners at (" <<
      msg->upperleft().x() << ", " << msg->upperleft().y() << "), (" <<
      msg->upperright().x() << ", " << msg->upperright().y() << "), (" <<
      msg->lowerright().x() << ", " << msg->lowerright().y() << "), (" <<
      msg->lowerleft().x() << ", " << msg->lowerleft().y() <<
        ") with collision projected from z = " <<
      msg->height() << "\nResolution = " << msg->resolution() << " m\n" <<
        "Occupied spaces will be filled with: " << msg->threshold() <<
        std::endl;

    double dX_vertical = msg->upperleft().x() - msg->lowerleft().x();
    double dY_vertical = msg->upperleft().y() - msg->lowerleft().y();
    double mag_vertical =
      sqrt(dX_vertical * dX_vertical + dY_vertical * dY_vertical);
    dX_vertical = msg->resolution() * dX_vertical / mag_vertical;
    dY_vertical = msg->resolution() * dY_vertical / mag_vertical;

    double dX_horizontal = msg->upperright().x() - msg->upperleft().x();
    double dY_horizontal = msg->upperright().y() - msg->upperleft().y();
    double mag_horizontal =
      sqrt(dX_horizontal * dX_horizontal + dY_horizontal * dY_horizontal);
    dX_horizontal = msg->resolution() * dX_horizontal / mag_horizontal;
    dY_horizontal = msg->resolution() * dY_horizontal / mag_horizontal;

    int count_vertical = mag_vertical / msg->resolution();
    int count_horizontal = mag_horizontal / msg->resolution();

    if (count_vertical == 0 || count_horizontal == 0)
    {
      std::cout << "Image has a zero dimensions, check coordinates"
                << std::endl;
      return;
    }
    double x,y;

    boost::gil::gray8_pixel_t fill(255-msg->threshold());
    boost::gil::gray8_pixel_t blank(255);
    boost::gil::gray8_image_t image(count_horizontal, count_vertical);

    double dist;
    std::string entityName;
    ignition::math::Vector3d start, end;
    start.Z(msg->height());
    end.Z(0.001);

    gazebo::physics::PhysicsEnginePtr engine = world->Physics();
    engine->InitForThread();
    gazebo::physics::RayShapePtr ray =
      boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
        engine->CreateShape("ray", gazebo::physics::CollisionPtr()));

    std::cout << "Rasterizing model and checking collisions" << std::endl;
    boost::gil::fill_pixels(image._view, blank);

    for (int i = 0; i < count_vertical; ++i)
    {
      std::cout << "Percent complete: " << i * 100.0 / count_vertical
                << std::endl;
      x = i * dX_vertical + msg->lowerleft().x();
      y = i * dY_vertical + msg->lowerleft().y();
      for (int j = 0; j < count_horizontal; ++j)
      {
        x += dX_horizontal;
        y += dY_horizontal;

        start.X(x);
        end.X(x);
        start.Y(y);
        end.Y(y);
        ray->SetPoints(start, end);
        ray->GetIntersection(dist, entityName);
        if (!entityName.empty())
        {
          image._view(i,j) = fill;
        }
      }
    }

    std::cout << "Completed calculations, writing to image" << std::endl;
    if (!msg->filename().empty())
    {
      boost::gil::gray8_view_t view = image._view;

      // Write to png
      // boost::gil::png_write_view(msg->filename(), view);
      // Write to pgm (pnm p2)
      pgm_write_view(msg->filename(), view);
    }
    std::cout << "Output location: " << msg->filename() << std::endl;
  }

  public: void pgm_write_view(const std::string& filename, boost::gil::gray8_view_t& view)
  {
    // Write image to pgm file
    std::cout << "running" << std::endl;
    int h = view.height();
    int w = view.width();

    std::ofstream ofs;
    ofs.open(filename+".pgm");
    ofs << "P2" << '\n';          // grayscale
    ofs << w << ' ' << h << '\n'; // width and height
    ofs << 255 <<  '\n';          // max value
    for (int y = 0; y < h; ++y){
      for (int x = 0; x < w; ++x){
        // std::cout << (int)view(x, y)[0];
        ofs << (int)view(x, y)[0] << ' ';
      }
      ofs << '\n';
    }
    ofs.close();
  }
};

// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(CollisionMapCreator)
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Dense>

#include "camera_help.h"
#include "kontiki/sensors/atan_camera.h"

namespace py = pybind11;

namespace C = kontiki::sensors;

PYBIND11_MODULE(_atan_camera, m) {
//  py::module::import("kontiki.sensors._pinhole_camera");

  using Class = C::AtanCamera;
//  using BaseClass = C::PinholeCamera;
  auto cls = py::class_<Class, std::shared_ptr<Class>>(m, "AtanCamera");
  cls.doc() = R"pbdoc( Arc-tan camera model for wide-angle cameras

  Implementation of the 3-parameter FOV model of [Devernay2001]_.

  Subclass of :class:`PinholeCamera`.

  References
  -------------------------
  .. [Devernay2001]
     Devernay, F. and Faugeras, O.
     *Straight lines have to be straight: Automatic calibration and removal of distortion
     from scenes of structured environments*
     Machie Vision and Applications, Vol 13., 2001

)pbdoc";

  cls.def(py::init<size_t, size_t, double, const Class::CameraMatrix&, const Eigen::Vector2d&, double >());
  cls.def_property("wc", &Class::wc, &Class::set_wc, "Distortion center");
  cls.def_property("gamma", &Class::gamma, &Class::set_gamma, "Distortion parameter");

  // All templated common functions must be explicitly created since they are not inherited
  // from the Pinhole base class.
  cls.def_property("camera_matrix", &Class::camera_matrix, &Class::set_camera_matrix);

  // Common functions
  declare_camera_common<Class>(cls);
}