#ifndef STAN_MATH_PRIM_PROB_PARETO_RNG_HPP
#define STAN_MATH_PRIM_PROB_PARETO_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a Pareto random variate for the given shape and scale
 * parameters using the specified random number generator.
 *
 * y_min and alpha can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_scale type of scale parameter
 * @tparam T_shape type of shape parameter
 * @tparam RNG type of random number generator
 * @param y_min (Sequence of) positive scale parameter(s)
 * @param alpha (Sequence of) positive shape parameter(s)
 * @param rng random number generator
 * @return (Sequence of) Pareto random variate(s)
 * @throw std::domain_error if y_min or alpha are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_shape, typename T_scale, class RNG>
inline typename VectorBuilder<true, double, T_shape, T_scale>::type pareto_rng(
    const T_scale& y_min, const T_shape& alpha, RNG& rng) {
  using boost::exponential_distribution;
  using boost::variate_generator;
  static const char* function = "pareto_rng";
  check_positive_finite(function, "Scale parameter", y_min);
  check_positive_finite(function, "Shape parameter", alpha);
  check_consistent_sizes(function, "Scale Parameter", y_min, "Shape parameter",
                         alpha);

  scalar_seq_view<T_scale> y_min_vec(y_min);
  scalar_seq_view<T_shape> alpha_vec(alpha);
  size_t N = max_size(y_min, alpha);
  VectorBuilder<true, double, T_scale, T_shape> output(N);

  for (size_t n = 0; n < N; ++n) {
    variate_generator<RNG&, exponential_distribution<> > exp_rng(
        rng, exponential_distribution<>(alpha_vec[n]));
    output[n] = y_min_vec[n] * std::exp(exp_rng());
  }

  return output.data();
}

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


/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, WVU Interactive Robotics Laboratory
*                       https://web.statler.wvu.edu/~irl/
* All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <stdio.h>
#include <math.h>
#include <boost/array.hpp>
#include <ros/ros.h>
#include <ros/time.h>
#include <vicon_bridge/Markers.h>
#include <vicon_bridge/Marker.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer_interface.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <wvu_swarm_std_msgs/vicon_bot.h>
#include <wvu_swarm_std_msgs/vicon_bot_array.h>
#include <wvu_swarm_std_msgs/robot_command.h>
#include <wvu_swarm_std_msgs/robot_command_array.h>
#include <wvu_swarm_std_msgs/vicon_point.h>
#include <wvu_swarm_std_msgs/vicon_points.h>

void msgCallback(const wvu_swarm_std_msgs::vicon_bot_array &msg) {}

// Method to find distance between two points
double getDist(const geometry_msgs::Point first, const geometry_msgs::Point second);

// Method to populate a vector with a Gerono Lemniscate
//   Params: a vector to fill with the points, a value 'a' defining the curve,
//   and a value for an increment in degrees to iterate through 360 degrees
void genGeronoLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr);

int main(int argc, char **argv)
{
    // Initialize a node
    ros::init(argc, argv, "tracker");

    // Generates nodehandle, publisher, subscribers
    ros::NodeHandle n;
    ros::NodeHandle n_priv("~"); // private handle
    ros::Publisher pub;
    ros::Subscriber sub;

    //  Where to get the array of bots, what topic to advertise, where to get transform of target
    std::string viconArrayTopic, advertiseTopic, transformPrefix;

    //  Value of 'a' in formula in cm, value to iterate degrees, distance to "find" points at
    int lemniscateConstant, lemniscateInterval, cutoffRadius;

    // Import parameters from launchfile
    n_priv.param<std::string>("vicon_array_topic", viconArrayTopic, "/viconArray");
    n_priv.param<std::string>("advertise_topic", advertiseTopic, "target");
    n_priv.param<std::string>("transform_prefix", transformPrefix, "cardbot_");
    n_priv.param<int>("lemniscate_constant", lemniscateConstant, 50);
    n_priv.param<int>("lemniscate_interval", lemniscateInterval, 10);
    n_priv.param<int>("cutoff_radius", cutoffRadius, 10);

    // Sets loop rate at 100Hz
    ros::Rate rate(10);

    // Subscribe to tracker's vicon topic, advertise result vector
    sub = n.subscribe(viconArrayTopic, 10, &msgCallback);
    pub = n.advertise<wvu_swarm_std_msgs::vicon_points>(advertiseTopic, 1000);

    // Set up transform buffer
    tf2_ros::Buffer tfBuffer;

    // Set up transform listener
    tf2_ros::TransformListener tfListener(tfBuffer);

    // Generate a Gerono Lemniscate (figure 8) as discrete points
    std::vector<geometry_msgs::Point> path;
    genGeronoLemn(path, lemniscateConstant, lemniscateInterval);

    // Pick one point out of the path
    geometry_msgs::Point currentPoint = path.at(0);
    int currentIndex = 0;
    ROS_INFO("Point initialized at %f, %f\n", currentPoint.x, currentPoint.y);

    // Run while ros functions
    while(ros::ok())
    {
        // Get the information of the vicon array
        wvu_swarm_std_msgs::vicon_bot_array viconArray =
                *(ros::topic::waitForMessage<wvu_swarm_std_msgs::vicon_bot_array>(viconArrayTopic));

        double minimumPointDistance = 10000.0; // Trivially large number, in cm

        // Iterate through all bots
        for(wvu_swarm_std_msgs::vicon_bot iteratorBot : viconArray.poseVect)
        {
            // Find this bot's point
            geometry_msgs::Point botPt;
            botPt.x = iteratorBot.botPose.transform.translation.x;
            botPt.y = iteratorBot.botPose.transform.translation.y;
            botPt.z = 0;

            // Find distance from this bot to the point
            double thisDist = getDist(botPt, currentPoint);

            // Change minimum if necessary
            if(thisDist < minimumPointDistance) minimumPointDistance = thisDist;
        }

        // Move the point if any bot has come too close
        if(minimumPointDistance < cutoffRadius)
        {
            // Rotate through vector if needed
            if(currentIndex + 1 >= path.size())
                currentIndex = 0;
            else
                currentIndex++;

            currentPoint = path.at(currentIndex);

            ROS_INFO("Point moved to %f, %f\n", currentPoint.x, currentPoint.y);
        }

        // Publish target point to be grabbed by the Processor
        wvu_swarm_std_msgs::vicon_points output;
        wvu_swarm_std_msgs::vicon_point outputPt;
        outputPt.x = currentPoint.x;
        outputPt.y = currentPoint.y;
        output.point.push_back(outputPt);
        wvu_swarm_std_msgs::vicon_point dummyPt;
        dummyPt.x = 30.0;
        dummyPt.y = 30.0;
        output.point.push_back(dummyPt);
        pub.publish(output);

        ros::spinOnce();
        rate.sleep();
    }
}

double getDist(const geometry_msgs::Point first, const geometry_msgs::Point second)
{
    double xDiff = first.x - second.x;
    double yDiff = first.y - second.y;
    double zDiff = first.z - second.z;

    return sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff);
}

void genGeronoLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr)
{
    // Iterate across 360 degrees
    for(int t = 0; t < 360; t += incr)
    {
        // Create a point
        geometry_msgs::Point temp;

        double rads = t * 3.14156265 / 180;

        // Do math to find its location
        //   This lemniscate is rotated 90 deg
        temp.x = a * cos(rads) * sin(rads);
        temp.y = a * sin(rads);
        temp.z = 0; //flatworld

        // Add to vector
        target.push_back(temp);

        ROS_INFO("Added point at %d degrees, or %f rads, at %f, %f, 0\n", t, rads, temp.x, temp.y);
    }
}


#pragma once

#include <cstddef>
#include <boost/preprocessor/seq/elem.hpp>
#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>

#include <giecs/bits.hpp>

namespace giecs
{

#define OPERATOR(name, T, op) \
template <typename T2> \
name<N> operator op (T2 const v) const \
{ \
    return name<N>(T(this->getValue()) op T(v)); \
} \
name<N> operator op (name<N> const v) const \
{ \
    return name<N>(T(this->getValue()) op T(v.getValue())); \
}

#define OPERATOR_EQ(name, T, op) \
template <typename T2> \
name<N>& operator op (T2 const v) \
{ \
    this->value op v; \
    return *this; \
} \
name<N>& operator op (name<N> const v) \
{ \
    this->value op T(v.getValue()); \
    return *this; \
}

#define DEF_OPERATOR(r, data, elem) OPERATOR(BOOST_PP_SEQ_ELEM(0,data), BOOST_PP_SEQ_ELEM(1,data), elem)
#define DEF_OPERATOR_EQ(r, data, elem) OPERATOR_EQ(BOOST_PP_SEQ_ELEM(0,data), BOOST_PP_SEQ_ELEM(1,data), elem)

#define TYPE_BITS(name, T, op_seq, eq_op_seq) \
template <std::size_t N> \
class name : public Bits<N> \
{ \
    public: \
        using Bits<N>::Bits; \
        BOOST_PP_SEQ_FOR_EACH(DEF_OPERATOR, (name)(T), op_seq)  \
        BOOST_PP_SEQ_FOR_EACH(DEF_OPERATOR_EQ, (name)(T), eq_op_seq)

#define NUMBER_TYPE_BITS(name, T) \
TYPE_BITS(name, T, \
        (+)(-)(*)(/)(>)(>=)(<)(<=), \
        (+=)(-=)(*=)(/=))

NUMBER_TYPE_BITS(Int, int)
Int<N>& operator ++ ()
{
    ++this->value;
    return *this;
}
Int<N>& operator -- ()
{
    --this->value;
    return *this;
}
};

NUMBER_TYPE_BITS(Float, float)
};

} // namespace giecs


// Copyright Aleksey Gurtovoy 2002-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: inherit_linearly.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-11 02:26:17 -0400 (Sat, 11 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/inherit_linearly.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/list.hpp>

#include <iostream>

namespace mpl = boost::mpl;
using namespace mpl::placeholders;

template< typename Base, typename T >
struct tuple_part
    : Base
{
    typedef tuple_part type; // note the typedef
    typedef typename Base::index::next index;

    friend T& field(tuple_part& t, index) { return t.field_; }
    T field_;
};

struct empty_tuple
{
    typedef mpl::int_<-1> index;
};

typedef mpl::inherit_linearly<
      mpl::list<int,char const*,bool>
    , tuple_part<_,_>
    , empty_tuple
    >::type my_tuple;

int main()
{
    my_tuple t;

    field(t, mpl::int_<0>()) = -1;
    field(t, mpl::int_<1>()) = "text";
    field(t, mpl::int_<2>()) = false;

    std::cout
        << field(t, mpl::int_<0>()) << '\n'
        << field(t, mpl::int_<1>()) << '\n'
        << field(t, mpl::int_<2>()) << '\n'
        ;

    return 0;
}


#include <boost/graph/adjacency_list_io.hpp>


#include <armadillo>
#include <imcmc/imcmc.hpp>
#include <imcmc/parser++.hpp>
#include "ClassEngine.hh"
#include "CosmoTheory.hpp"
#include "CMB.hpp"
#include "class.h"

using namespace std;
using namespace imcmc;
using namespace imcmc::parser;

Data_CMB_Dist::Data_CMB_Dist(){
    // do nothing
    use_Hu_fitting = true;
}

Data_CMB_Dist::~Data_CMB_Dist(){
    // do nothing
}

void Data_CMB_Dist::Init( string& CMB_dist_prior_dataset ){

    data_info.GetInfo(CMB_dist_prior_dataset);

    string dist_prior_file = "";
    string inv_covmat_file = "";
	string dist_prior_std_file = ""; // used only for Planck distance prior
	format = -1;	// 0--WMAP; 1--Planck

	if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"format") ){
		format = Read::Read_Int_from_File(CMB_dist_prior_dataset,"format");
		if( format != 0 && format != 1 ){
			MPI_ClassMC_ERROR("format can only take 0 or 1 !");
		}
	}
	else{
		MPI_ClassMC_ERROR("the key \'format\' can not be found in file: "+CMB_dist_prior_dataset);
	}

    if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"use_Hu_fitting") )
        use_Hu_fitting = Read::Read_Bool_from_File(CMB_dist_prior_dataset,"use_Hu_fitting");
    else{
        MPI_ClassMC_WARNING("use_Hu_fitting is not found, so use default value \'true\'.");
        // exit(0);
    }

    if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"dist_prior_file") )
        dist_prior_file = Read::Read_String_from_File(CMB_dist_prior_dataset,"dist_prior_file");
    else
        MPI_ClassMC_ERROR("dist_prior_file not set in: "+CMB_dist_prior_dataset);

    if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"inv_covmat_file") )
        inv_covmat_file = Read::Read_String_from_File(CMB_dist_prior_dataset,"inv_covmat_file");
    else
        MPI_ClassMC_ERROR("inv_covmat_file not set in: "+CMB_dist_prior_dataset);

	if( format == 0 ){
    	distance_prior  = arma::zeros(1,3);
    	covmat_inv      = arma::zeros(3,3);

    	distance_prior.load(dist_prior_file,arma::auto_detect); // loading CMB distance prior {lA,R,z_rec}
    	covmat_inv.load(inv_covmat_file,arma::auto_detect);     // loading covariance matrix
	}
	else if( format == 1 ){
//		cout << "loading Planck distance prior data ...\n";
		arma::rowvec tmp = arma::zeros(1,3);
		string std_file = Read::Read_String_from_File(CMB_dist_prior_dataset,"dist_prior_std_file");
		tmp.load(std_file,arma::auto_detect);
		std_R 	= tmp(0);
		std_lA	= tmp(1);
		std_Obh2= tmp(2);

		distance_prior_plk = arma::zeros(1,3);
		covmat_inv_plk     = arma::zeros(3,3);

    //  read Planck distance prior data
		distance_prior_plk.load(dist_prior_file,arma::auto_detect);

    //  read the normalized covariance matrix
		covmat_inv_plk.load(inv_covmat_file,arma::auto_detect);

    //  convert to the original covariance matrix
        covmat_inv_plk(0,0) *= std_R*std_R;
        covmat_inv_plk(0,1) *= std_R*std_lA;
        covmat_inv_plk(0,2) *= std_R*std_Obh2;

		covmat_inv_plk(1,0) *= std_lA*std_R;
		covmat_inv_plk(1,1) *= std_lA*std_lA;
		covmat_inv_plk(1,2) *= std_lA*std_Obh2;

		covmat_inv_plk(2,0) *= std_Obh2*std_R;
		covmat_inv_plk(2,1) *= std_Obh2*std_lA;
		covmat_inv_plk(2,2) *= std_Obh2*std_Obh2;

        covmat_inv_plk = covmat_inv_plk.i();
	}

}

double Prior_CMB_Dist(  imcmc_double&   param,
                        double&         lndet,
                        double&         chisq,
                        void*           model,
                        void*           data,
                        istate&         state ){

    chisq = lndet = 0.0;
    CosmoTheory *cosmo = static_cast<CosmoTheory*>(model);
    CMB_Dist    *cmbdist = static_cast<CMB_Dist*>(data);

    double lA, R, z_rec; // this format matches WMAP7/9 distance prior data

    double H0   = cosmo->engine->get_H0();
    double Om0  = cosmo->engine->get_Omega_m();
    double ra_rec, rs_rec;

    if( cmbdist->use_Hu_fitting ){
        z_rec  = cosmo->engine->z_rec_Hu();
        ra_rec = cosmo->engine->get_Da(z_rec)*(1.+z_rec)/cosmo->engine->get_a_today();
        rs_rec = cosmo->engine->get_rs(z_rec);
    }
    else{
        z_rec  = cosmo->engine->z_rec();
        ra_rec = cosmo->engine->ra_rec();
        rs_rec = cosmo->engine->rs_rec();
    }

    lA = _PI_*ra_rec/rs_rec;
    R  = sqrt(Om0)*H0*ra_rec/(_c_*1e-3);

    arma::rowvec residual = arma::zeros(1,3);

    switch (cmbdist->format){
        case 0:
        // for WMAP distance prior
            residual(0) = lA;
            residual(1) = R;
            residual(2)	= z_rec;
            residual = residual - cmbdist->distance_prior;
            chisq = arma::as_scalar( residual * cmbdist->covmat_inv * residual.t() );
            break;
        case 1:
        // for Planck distance prior
            residual(0) = R;
            residual(1) = lA;
            residual(2)	= cosmo->engine->get_Omega_b()*(H0/100)*(H0/100);
            residual = residual - cmbdist->distance_prior_plk;
            chisq = arma::as_scalar( residual * cmbdist->covmat_inv_plk * residual.t() );
            break;
        default:
            break;
    }

    return lndet - 0.5*chisq;
}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Math/LevenbergMarquardt.h>
#include <vw/Math/Matrix.h>
#include <vw/Camera/CameraModel.h>
#include <asp/IsisIO/IsisInterfaceLineScan.h>

#include <algorithm>
#include <vector>

#include <Camera.h>
#include <CameraDetectorMap.h>
#include <CameraDistortionMap.h>
#include <CameraFocalPlaneMap.h>
#include <iTime.h>

#include <boost/smart_ptr/scoped_ptr.hpp>

using namespace vw;
using namespace asp;
using namespace asp::isis;

// Construct
IsisInterfaceLineScan::IsisInterfaceLineScan(std::string const& filename): IsisInterface(filename), m_alphacube(*m_cube) {

  // Gutting Isis::Camera
  m_distortmap = m_camera->DistortionMap();
  m_focalmap   = m_camera->FocalPlaneMap();
  m_detectmap  = m_camera->DetectorMap();
}

// Custom Function to help avoid over invoking the deeply buried
// functions of Isis::Sensor
void IsisInterfaceLineScan::SetTime(Vector2 const& px, bool calc_pose) const {
  if (px != m_c_location) {
    m_c_location = px;
    m_detectmap->SetParent(m_alphacube.AlphaSample(px[0]),
                           m_alphacube.AlphaLine(px[1]));

    if (calc_pose) {
      // Calculating the spacecraft position and orientation (hence pose)
      m_camera->instrumentPosition(&m_center[0]);
      m_center *= 1000;

      std::vector<double> rot_inst = m_camera->instrumentRotation()->Matrix();
      std::vector<double> rot_body = m_camera->bodyRotation()->Matrix();
      MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
      MatrixProxy<double,3,3> R_body(&(rot_body[0]));
      m_pose = Quat(R_body*transpose(R_inst));
    }
  }
}

class EphemerisLMA : public vw::math::LeastSquaresModelBase<EphemerisLMA> {
  vw::Vector3 m_point;
  Isis::Camera* m_camera;
  Isis::CameraDistortionMap *m_distortmap;
  Isis::CameraFocalPlaneMap *m_focalmap;
public:
  typedef vw::Vector<double> result_type; // Back project result
  typedef vw::Vector<double> domain_type; // Ephemeris time
  typedef vw::Matrix<double> jacobian_type;

  inline EphemerisLMA(vw::Vector3 const& point,
                      Isis::Camera* camera,
                      Isis::CameraDistortionMap* distortmap,
                      Isis::CameraFocalPlaneMap* focalmap) : m_point(point), m_camera(camera), m_distortmap(distortmap), m_focalmap(focalmap) {}

  inline result_type operator()(domain_type const& x) const;
};

// LMA for projecting point to linescan camera
EphemerisLMA::result_type
EphemerisLMA::operator()(EphemerisLMA::domain_type const& x) const {

  // Setting Ephemeris Time
  m_camera->setTime(Isis::iTime(x[0]));

  // Calculating the look direction in camera frame
  Vector3 instru;
  m_camera->instrumentPosition(&instru[0]);
  instru *= 1000;  // Spice gives in km
  Vector3 lookB = normalize(m_point - instru);
  std::vector<double> lookB_copy(3);
  std::copy(lookB.begin(), lookB.end(), lookB_copy.begin());
  std::vector<double> lookJ = m_camera->bodyRotation()->J2000Vector(lookB_copy);
  std::vector<double> lookC = m_camera->instrumentRotation()->ReferenceVector(lookJ);
  Vector3 look;
  std::copy(lookC.begin(), lookC.end(), look.begin());

  // Projecting to mm focal plane
  look = m_camera->FocalLength() * (look / look[2]);
  m_distortmap->SetUndistortedFocalPlane(look[0], look[1]);
  m_focalmap->SetFocalPlane(m_distortmap->FocalPlaneX(),
                            m_distortmap->FocalPlaneY());
  result_type result(1);
  // Not exactly sure about lineoffset .. but ISIS does it
  result[0] = m_focalmap->DetectorLineOffset() - m_focalmap->DetectorLine();

  return result;
}

Vector2
IsisInterfaceLineScan::point_to_pixel(Vector3 const& point) const {

  // First seed LMA with an ephemeris time in the middle of the image
  double middle = lines() / 2;
  m_detectmap->SetParent(1, m_alphacube.AlphaLine(middle));
  double start_e = m_camera->time().Et();

  // Build LMA
  EphemerisLMA model(point, m_camera.get(), m_distortmap, m_focalmap);
  int status;
  Vector<double> objective(1), start(1);
  start[0] = start_e;
  Vector<double> solution_e = math::levenberg_marquardt(model,
                                                        start,
                                                        objective,
                                                        status);

  // Make sure we found ideal time
  VW_ASSERT(status > 0, vw::camera::PointToPixelErr() << " Unable to project point into ISIS linescan camera ");

  // Converting now to pixel
  m_camera->setTime(Isis::iTime(solution_e[0]));

  // Working out pointing
  m_camera->instrumentPosition(&m_center[0]);
  m_center *= 1000;
  Vector3 look = normalize(point-m_center);

  // Calculating rotation to camera frame
  std::vector<double> rot_inst = m_camera->instrumentRotation()->Matrix();
  std::vector<double> rot_body = m_camera->bodyRotation()->Matrix();
  MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
  MatrixProxy<double,3,3> R_body(&(rot_body[0]));
  m_pose = Quat(R_body*transpose(R_inst));

  look = inverse(m_pose).rotate(look);
  look = m_camera->FocalLength() * (look / look[2]);
  m_distortmap->SetUndistortedFocalPlane(look[0], look[1]);
  m_focalmap->SetFocalPlane(m_distortmap->FocalPlaneX(),
                            m_distortmap->FocalPlaneY());
  m_detectmap->SetDetector(m_focalmap->DetectorSample(),
                           m_focalmap->DetectorLine());
  Vector2 pixel(m_detectmap->ParentSample(),
                m_detectmap->ParentLine());
  pixel[0] = m_alphacube.BetaSample(pixel[0]);
  pixel[1] = m_alphacube.BetaLine(pixel[1]);
  SetTime(pixel, false);

  pixel -= Vector2(1,1);
  return pixel;
}

Vector3
IsisInterfaceLineScan::pixel_to_vector(Vector2 const& pix) const {
  Vector2 px = pix + Vector2(1,1);
  SetTime(px, true);

  // Projecting to get look direction
  Vector3 result;
  m_focalmap->SetDetector(m_detectmap->DetectorSample(),
                          m_detectmap->DetectorLine());
  m_distortmap->SetFocalPlane(m_focalmap->FocalPlaneX(),
                              m_focalmap->FocalPlaneY());
  result[0] = m_distortmap->UndistortedFocalPlaneX();
  result[1] = m_distortmap->UndistortedFocalPlaneY();
  result[2] = m_distortmap->UndistortedFocalPlaneZ();
  result = normalize(result);
  result = m_pose.rotate(result);
  return result;
}

Vector3
IsisInterfaceLineScan::camera_center(Vector2 const& pix) const {
  Vector2 px = pix + Vector2(1,1);
  SetTime(px, true);
  return m_center;
}

Quat
IsisInterfaceLineScan::camera_pose(Vector2 const& pix) const {
  Vector2 px = pix + Vector2(1,1);
  SetTime(px, true);
  return m_pose;
}


#include <memory>
#include <random>

#include <Eigen/Core>
#include <aslam/common/memory.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <maplab-common/unique-id.h>
#include <posegraph/edge.h>
#include <posegraph/vertex.h>

#include "vi-map/loopclosure-edge.h"
#include "vi-map/mission.h"
#include "vi-map/test/vi-map-test-helpers.h"
#include "vi-map/vi-map.h"
#include "vi-map/vi_map.pb.h"

namespace vi_map {

class VIMapEdgeRemovalTest : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    vi_map::test::generateMap<TransformationEdge>(kTestDataVertexCount, &map_);
    initial_mission_ = map_.getIdOfFirstMission();
    map_.getAllEdgeIds(&initial_edges_);
    map_.getAllVertexIds(&initial_vertices_);
    map_.getAllLandmarkIds(&initial_landmarks_);
  }

  size_t numVertices() const {
    return kTestDataVertexCount;
  }

  size_t numLoopClosureEdges() const;
  void addRandomLoopClosureEdges(const size_t number_of_edges);

  size_t removeLoopClosureEdges() {
    return map_.removeLoopClosureEdges();
  }

  vi_map::VIMap map_;
  pose_graph::VertexIdList initial_vertices_;
  pose_graph::EdgeIdList initial_edges_;
  vi_map::LandmarkIdSet initial_landmarks_;
  vi_map::MissionId initial_mission_;

 private:
  // Modify if test dataset with different vertex count used.
  static constexpr size_t kTestDataVertexCount = 405u;
  static constexpr double kSwitchVariable = 1.0;
  static constexpr double kSwitchVariableVariance = 1e-3;
};

size_t VIMapEdgeRemovalTest::numLoopClosureEdges() const {
  size_t number_of_loop_edges = 0u;
  pose_graph::EdgeIdList edges;
  map_.getAllEdgeIds(&edges);

  for (const pose_graph::EdgeId& edge : edges) {
    number_of_loop_edges += static_cast<int>(
        map_.getEdgeType(edge) == pose_graph::Edge::EdgeType::kLoopClosure);
  }
  return number_of_loop_edges;
}

void VIMapEdgeRemovalTest::addRandomLoopClosureEdges(
    const size_t number_of_edges_to_add) {
  Eigen::Matrix<double, 6, 6> T_A_B_covariance;
  pose::Transformation T_A_B;

  constexpr size_t kSeed = 85u;
  std::srand(kSeed);

  std::mt19937 integer_generator(kSeed);
  constexpr size_t kLowerBound = 0u;
  constexpr size_t kUpperBound = kTestDataVertexCount - 1u;
  std::uniform_int_distribution<> vertex_index_uniform_distribution(
      kLowerBound, kUpperBound);

  constexpr size_t kVerticesOffset = 10u;
  for (size_t edges_added = 0u; edges_added < number_of_edges_to_add;
       ++edges_added) {
    const size_t vertex_idx_from =
        vertex_index_uniform_distribution(integer_generator);
    const size_t vertex_idx_to =
        (vertex_idx_from + kVerticesOffset) % kTestDataVertexCount;
    ASSERT_NE(vertex_idx_from, vertex_idx_to);

    T_A_B.setRandom();
    T_A_B_covariance.setRandom();

    pose_graph::EdgeId edge_id;
    common::generateId(&edge_id);
    ASSERT_TRUE(edge_id.isValid());

    const pose_graph::VertexId& from = initial_vertices_[vertex_idx_from];
    const pose_graph::VertexId& to = initial_vertices_[vertex_idx_to];

    map_.addEdge(
        vi_map::Edge::UniquePtr(
            new LoopClosureEdge(
                edge_id, from, to, kSwitchVariable, kSwitchVariableVariance,
                T_A_B, T_A_B_covariance)));
  }
}

TEST_F(VIMapEdgeRemovalTest, RemoveLoopClosureEdgesTest) {
  EXPECT_EQ(numLoopClosureEdges(), 0u);

  constexpr size_t kNumNewLoopEdges = 100u;
  addRandomLoopClosureEdges(kNumNewLoopEdges);

  EXPECT_EQ(numLoopClosureEdges(), kNumNewLoopEdges);
  EXPECT_EQ(removeLoopClosureEdges(), kNumNewLoopEdges);
  EXPECT_EQ(numLoopClosureEdges(), 0u);
}

}  // namespace vi_map

MAPLAB_UNITTEST_ENTRYPOINT


#ifndef JHMI_MACROCELL_HPP_NRC20150429
#define JHMI_MACROCELL_HPP_NRC20150429

#include "liver/utility.hpp"
#include "utility/binary_tree.hpp"
#include "utility/bresenham.hpp"
#include "utility/cube.hpp"
#include "utility/units.hpp"
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <range/v3/getlines.hpp>
#include <range/v3/range_for.hpp>
#include <fstream>

namespace jhmi {
  template <typename T> using cidx_to = std::unordered_map<cell_id, T>;
  enum class cell_type { normal, tumor };
  struct macrocell {
    m3 center;
    vessel_id parent_vessel;
    cell_type type;
    m radius;
    cell_id id;
    cubic_meters_per_second flow;
    Pa pressure;
    int3 idx;
  };

  cell_id id(macrocell const& m) {
    return m.id;
  }
  cube<m3> extents(macrocell const& m) {
    return {m.center - m.radius * dbl3{1,1,1}, m.center + m.radius * dbl3{1,1,1}};
  }
  template <typename Convert, typename F>
  inline F traverse(macrocell const& m, Convert c, F f) {
    auto u = extents(m);
    return closed_traverse(cube<decltype(c(u.ul()))>{c(u.ul()), c(u.lr())}, 1, f);
  }

  bool operator==(macrocell const& lhs, macrocell const& rhs) {
    return nearly_equal(lhs.center, rhs.center)
     && lhs.parent_vessel == rhs.parent_vessel
     && lhs.type == rhs.type
     && nearly_equal(lhs.radius, rhs.radius)
     && lhs.id == rhs.id
     && nearly_equal(lhs.flow, rhs.flow)
     && nearly_equal(lhs.pressure, rhs.pressure)
     && lhs.idx == rhs.idx;
  }
}//jhmi

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <algorithm>

#include <boost/make_shared.hpp>

#include "tudat/simulation/estimation_setup/createOneWayRangePartials.h"

namespace tudat
{

namespace observation_partials
{

using namespace ephemerides;

//! Function to generate one-way range partial wrt an initial position of a body.
std::shared_ptr< OneWayRangePartial > createOneWayRangePartialWrtBodyPosition(
        const observation_models::LinkEnds oneWayRangeLinkEnds,
        const simulation_setup::SystemOfBodies& bodies,
        const std::string bodyToEstimate,
        const std::shared_ptr< OneWayRangeScaling > oneWayRangeScaler,
        const std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >&
        lightTimeCorrectionPartialObjects  )
{
    // Create position partials of link ends for current body position
    std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > positionPartials =
            createCartesianStatePartialsWrtBodyState( oneWayRangeLinkEnds, bodies, bodyToEstimate );

    // Create one-range partials if any position partials are created (i.e. if any dependency exists).
    std::shared_ptr< OneWayRangePartial > oneWayRangePartial;
    if( positionPartials.size( ) > 0 )
    {
        oneWayRangePartial = std::make_shared< OneWayRangePartial >(
                    oneWayRangeScaler, positionPartials, std::make_pair(
                        estimatable_parameters::initial_body_state, std::make_pair( bodyToEstimate, "" ) ),
                    lightTimeCorrectionPartialObjects );
    }

    // Return range partial object (nullptr if no dependency exists).
    return oneWayRangePartial;
}

//! Function to generate one-way range partial wrt an initial position of a body.
std::shared_ptr< OneWayRangePartial > createOneWayRangePartialWrtBodyRotationalState(
        const observation_models::LinkEnds oneWayRangeLinkEnds,
        const simulation_setup::SystemOfBodies& bodies,
        const std::string bodyToEstimate,
        const std::shared_ptr< OneWayRangeScaling > oneWayRangeScaler,
        const std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >&
        lightTimeCorrectionPartialObjects  )
{
    // Create position partials of link ends for current body position
    std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > positionPartials =
            createCartesianStatePartialsWrtBodyRotationalState( oneWayRangeLinkEnds, bodies, bodyToEstimate );

    // Create one-range partials if any position partials are created (i.e. if any dependency exists).
    std::shared_ptr< OneWayRangePartial > oneWayRangePartial;
    if( positionPartials.size( ) > 0 )
    {
        oneWayRangePartial = std::make_shared< OneWayRangePartial >(
                    oneWayRangeScaler, positionPartials, std::make_pair(
                        estimatable_parameters::initial_body_state, std::make_pair( bodyToEstimate, "" ) ),
                    lightTimeCorrectionPartialObjects );
    }

    // Return range partial object (nullptr if no dependency exists).
    return oneWayRangePartial;
}

}

}


#ifndef BOOST_GEOMETRY_PROJECTIONS_NATEARTH_HPP
#define BOOST_GEOMETRY_PROJECTIONS_NATEARTH_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// The Natural Earth projection was designed by Tom Patterson, US National Park
// Service, in 2007, using Flex Projector. The shape of the original projection
// was defined at every 5 degrees and piece-wise cubic spline interpolation was
// used to compute the complete graticule.
// The code here uses polynomial functions instead of cubic splines and
// is therefore much simpler to program. The polynomial approximation was
// developed by Bojan Savric, in collaboration with Tom Patterson and Bernhard
// Jenny, Institute of Cartography, ETH Zurich. It slightly deviates from
// Patterson's original projection by adding additional curvature to meridians
// where they meet the horizontal pole line. This improvement is by intention
// and designed in collaboration with Tom Patterson.
// Port to PROJ.4 by Bernhard Jenny, 6 June 2011

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct natearth {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace natearth
    {

            static const double A0 = 0.8707;
            static const double A1 = -0.131979;
            static const double A2 = -0.013791;
            static const double A3 = 0.003971;
            static const double A4 = -0.001529;
            static const double B0 = 1.007226;
            static const double B1 = 0.015085;
            static const double B2 = -0.044475;
            static const double B3 = 0.028874;
            static const double B4 = -0.005916;
            static const double C0 = B0;
            static const double C1 = (3 * B1);
            static const double C2 = (7 * B2);
            static const double C3 = (9 * B3);
            static const double C4 = (11 * B4);
            static const double EPS = 1e-11;
            //static const double MAX_Y = (0.8707 * 0.52 * geometry::math::pi<double>());

            template <typename T>
            inline T MAX_Y() { return (0.8707 * 0.52 * detail::ONEPI<T>()); }

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_natearth_spheroid : public base_t_fi<base_natearth_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                inline base_natearth_spheroid(const Parameters& par)
                    : base_t_fi<base_natearth_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    CalculationType phi2, phi4;

                    phi2 = lp_lat * lp_lat;
                    phi4 = phi2 * phi2;
                    xy_x = lp_lon * (A0 + phi2 * (A1 + phi2 * (A2 + phi4 * phi2 * (A3 + phi2 * A4))));
                    xy_y = lp_lat * (B0 + phi2 * (B1 + phi4 * (B2 + B3 * phi2 + B4 * phi4)));
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    static const CalculationType MAX_Y = natearth::MAX_Y<CalculationType>();

                    CalculationType yc, tol, y2, y4, f, fder;

                    /* make sure y is inside valid range */
                    if (xy_y > MAX_Y) {
                        xy_y = MAX_Y;
                    } else if (xy_y < -MAX_Y) {
                        xy_y = -MAX_Y;
                    }

                    /* latitude */
                    yc = xy_y;
                        for (;;) { /* Newton-Raphson */
                        y2 = yc * yc;
                        y4 = y2 * y2;
                        f = (yc * (B0 + y2 * (B1 + y4 * (B2 + B3 * y2 + B4 * y4)))) - xy_y;
                        fder = C0 + y2 * (C1 + y4 * (C2 + C3 * y2 + C4 * y4));
                        yc -= tol = f / fder;
                        if (fabs(tol) < EPS) {
                            break;
                        }
                    }
                    lp_lat = yc;

                    /* longitude */
                    y2 = yc * yc;
                    lp_lon = xy_x / (A0 + y2 * (A1 + y2 * (A2 + y2 * y2 * y2 * (A3 + y2 * A4))));
                }

                static inline std::string get_name()
                {
                    return "natearth_spheroid";
                }

            };

            // Natural Earth
            template <typename Parameters>
            inline void setup_natearth(Parameters& par)
            {
                par.es = 0;
            }

    }} // namespace detail::natearth
    #endif // doxygen

    /*!
        \brief Natural Earth projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_natearth.gif
    */
    template <typename CalculationType, typename Parameters>
    struct natearth_spheroid : public detail::natearth::base_natearth_spheroid<CalculationType, Parameters>
    {
        inline natearth_spheroid(const Parameters& par) : detail::natearth::base_natearth_spheroid<CalculationType, Parameters>(par)
        {
            detail::natearth::setup_natearth(this->m_par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::natearth, natearth_spheroid, natearth_spheroid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class natearth_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<natearth_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void natearth_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("natearth", new natearth_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_NATEARTH_HPP


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#include <boost/simd/function/scalar/fnma.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/generic/function/fnma.hpp>

#if defined(BOOST_HW_SIMD_X86)

#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_FMA3_VERSION
// #    include <boost/simd/arch/x86/fma3/simd/function/fnma.hpp>
#  endif
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_FMA4_VERSION
// #    include <boost/simd/arch/x86/fma4/simd/function/fnma.hpp>
#  endif

#endif

#endif


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

#pragma once

#include "definitions.hpp"
#include "enhance/incremental_number_gen.hpp"
#include <memory>
#include <string>
#include <type_traits>
#if __has_include(<Eigen/Core>)
#include <Eigen/Core>
#include <Eigen/Geometry>
#elif __has_include(<eigen3/Eigen/Core>)
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#endif

#include <tbb/spin_mutex.h>

class ParticleIDGenerator : public enhance::IncrementalNumberGenerator<ParticleIDGenerator> {};

enum PARTICLETYPE { UNDEFINED, MOBILE, FRAME, OSMOTIC };

/*
CONCEPT

- all particle classes inherit from Particle
- simulation routine classes are written for containers of type
  container<std::unique_ptr<Particle>> and must work woth them
  as the overriden interface of derived permits
- members are implemented as pointers since they are smaller than
  Eigen::Vector3 classes without performance loss
- member verctors are initialized zero, mass is one
- set functions are pure virtual and have to be derived

*/

class Particle
{
public:
    // inheritance reuired
    virtual ~Particle() {};

    friend std::ostream& operator<<(std::ostream& os, const Particle&);

    // setting the floating point precision
    typedef float real;

    // setting the vector type
    typedef Eigen::Matrix<real,3,1,0,3,1> cartesian;

    // pure virtual setter functions
    // derived overrides must check pointers berfore
    // accessing underlying pointer
    virtual void setCoords(const cartesian&) = 0;
    virtual void setVelocity(const cartesian&) = 0;
    virtual void setForce(const cartesian&) = 0;
    virtual void setOrientation(const cartesian&) = 0;

    // derived must set name for VMD
    virtual std::string name() const = 0;

    // compare by std::addressof comparison
    bool operator==(const Particle &);

    // will set all old member versions to the actual ones
    void save();

    // reset members to 0
    void clearCoords();
    void clearVelocity();
    void clearForce();
    void clearOrientation();

    // adding to velocity and force for verlet purposes
    void addVelocity(const cartesian&);
    void addForce(const cartesian&);

    // return vectors
    // in DEBUG mode:
    // - check pointers beforehand
    // - check for NaN's
    const cartesian& coords() const;
    const cartesian& coordsOld() const;
    const cartesian& force() const;
    const cartesian& forceOld() const;
    const cartesian& velocity() const;
    const cartesian& velocityOld() const;
    const cartesian& orientation() const;
    const cartesian& orientationOld() const;

    // setter and getter of mass member
    void setMass(float);
    real getMass() const;

    // virtual function for guiding elements to overwrite constraints
    virtual void setOffset(float, float, float);

    // virtual function for particle types to use
    void setBoundingBox(Eigen::AlignedBox<Particle::real,3>);

    // id will be set automatically
    // id is only necessary to generate useful energy matrices
    // for MonteCarlo simulation routine
    const unsigned int ID = ParticleIDGenerator()();

    // a simple type getter, for easier usage in ParticleSimple
    virtual PARTICLETYPE getType() const = 0;

protected:
    // derive or dont use
    Particle() = default;

    // members
    real mass = {1.0};
    std::unique_ptr<cartesian> currentCoords {std::make_unique<cartesian>(cartesian::Zero())};
    std::unique_ptr<cartesian> oldCoords {std::make_unique<cartesian>(cartesian::Zero())};
    std::unique_ptr<cartesian> currentOrientation {std::make_unique<cartesian>(cartesian::Zero())};
    std::unique_ptr<cartesian> oldOrientation {std::make_unique<cartesian>(cartesian::Zero())};

    std::unique_ptr<Eigen::AlignedBox<Particle::real,3>> bounding_box {nullptr};
    bool boundingBox_was_set = false;

    std::unique_ptr<cartesian> currentForce {std::make_unique<cartesian>(cartesian::Zero())};
    std::unique_ptr<cartesian> oldForce {std::make_unique<cartesian>(cartesian::Zero())};
    std::unique_ptr<cartesian> currentVelocity {std::make_unique<cartesian>(cartesian::Zero())};
    std::unique_ptr<cartesian> oldVelocity {std::make_unique<cartesian>(cartesian::Zero())};

    // lock non const member access.
    // TO BE QUESTIONED
    tbb::spin_mutex mutex {};

private:
};


#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <dlib/opencv.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>

#include <opencv2/opencv.hpp>
#include "render_face.hpp"
#define OPENCV_FACE_RENDER

using namespace dlib;
using namespace std;

#include <iostream>
#include <opencv2/core/core.hpp>

#include <geometry_msgs/Twist.h>

#define FACE_DOWNSAMPLE_RATIO 4
#define SKIP_FRAMES 2
#define OPENCV_FACE_RENDER

std::vector<cv::Mat>XYZ_split;
std::vector<double>dataXYZ;

double average_count=0;
int average_frames=6;
//int average_frames=4;

int median_limit=average_frames/2;
int lower_limit=average_frames/3;
int higher_limit=(2*average_frames)/3;

std::deque<double>discrete_deque_x;
std::deque<double>discrete_deque_y;
std::deque<double>discrete_deque_z;

std::deque<double>median_x;
std::deque<double>median_y;
std::deque<double>median_z;

std::deque<double>median_sort_x;
std::deque<double>median_sort_y;
std::deque<double>median_sort_z;

std::deque<double>Med_difference_x;
std::deque<double>Med_difference_y;
std::deque<double>Med_difference_z;

std::deque<double>Med_difference_sort_x;
std::deque<double>Med_difference_sort_y;
std::deque<double>Med_difference_sort_z;

double MAD_x=0;
double MAD_y=0;
double MAD_z=0;

double MADN_x=0;
double MADN_y=0;
double MADN_z=0;

std::deque<double>deviation_value_x;
std::deque<double>deviation_value_y;
std::deque<double>deviation_value_z;

double deviation_count_x=0;
double deviation_count_y=0;
double deviation_count_z=0;

double deviation_limit_x=10;
double deviation_limit_y=10;
double deviation_limit_z=10;

std::deque<double>average_angle_x_deque;
std::deque<double>average_angle_y_deque;
std::deque<double>average_angle_z_deque;

double average_angle_x;
double average_angle_y;
double average_angle_z;

double average_angle_count=0;

double numeric_rotation_x=0;
double numeric_rotation_y=0;
double numeric_rotation_z=0;

std::vector<double>rotation_x;
std::vector<double>rotation_y;
std::vector<double>rotation_z;

std::vector<double>drone_rotation_y;

std::vector<cv::Point2d> image_points;
std::vector<cv::Point2d> face_points;
std::vector<cv::Point3d> nose_end_point3D;
std::vector<cv::Point2d> nose_end_point2D;

/*
std::vector<cv::Point3d> get_3d_model_points()
{
	std::vector<cv::Point3d> modelPoints;

	modelPoints.push_back(cv::Point3d(0.0f, 0.0f, 0.0f));
	modelPoints.push_back(cv::Point3d(0.0f, -330.0f, -65.0f));
	modelPoints.push_back(cv::Point3d(-225.0f, 170.0f, -135.0f));
	modelPoints.push_back(cv::Point3d(225.0f, 170.0f, -135.0f));
	modelPoints.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f));
	modelPoints.push_back(cv::Point3d(150.0f, -150.0f, -125.0f));

	return modelPoints;
}
*/

std::vector<cv::Point3d> get_3d_model_points()
{
  std::vector<cv::Point3d> modelPoints;

  modelPoints.push_back(cv::Point3d(-228.93,92.14,-325.8)); //0,1,16,15
  modelPoints.push_back(cv::Point3d(-224.79,41.24,-318.69));
  modelPoints.push_back(cv::Point3d(228.93,92.14,-325.8));
  modelPoints.push_back(cv::Point3d(224.79,41.24,-318.69));

  modelPoints.push_back(cv::Point3d(-66.9,99.94,-85.72)); //39,40,42,47
  modelPoints.push_back(cv::Point3d(-89.65,96.74,-75.57));
  modelPoints.push_back(cv::Point3d(66.9,99.94,-85.72));
  modelPoints.push_back(cv::Point3d(89.65,96.74,-75.57));

  modelPoints.push_back(cv::Point3d(0,107.43,-41.93)); //27,28,29,30
  modelPoints.push_back(cv::Point3d(0,71.79,-34.67));
  modelPoints.push_back(cv::Point3d(0,37.77,-15.83));
  modelPoints.push_back(cv::Point3d(0,0,0));

  modelPoints.push_back(cv::Point3d(-40.61,-29.94,-52.14)); //31,32,33,34,35
  modelPoints.push_back(cv::Point3d(-22.95,-32.59,-46.07));
  modelPoints.push_back(cv::Point3d(0,-38.54,-42.36));
  modelPoints.push_back(cv::Point3d(22.95,-32.59,-46.07));
  modelPoints.push_back(cv::Point3d(40.61,-29.94,-52.14));

  return modelPoints;
}

/*
std::vector<cv::Point2d> get_2d_image_points(full_object_detection &d)
{
  std::vector<cv::Point2d> image_points;

  image_points.push_back( cv::Point2d( d.part(30).x(), d.part(30).y() ) );　// Nose tip
  image_points.push_back( cv::Point2d( d.part(8).x(), d.part(8).y() ) );   // Chin
  image_points.push_back( cv::Point2d( d.part(36).x(), d.part(36).y() ) ); // Left eye left corner
  image_points.push_back( cv::Point2d( d.part(45).x(), d.part(45).y() ) ); // Right eye right corner
  image_points.push_back( cv::Point2d( d.part(48).x(), d.part(48).y() ) ); // Left Mouth corner
  image_points.push_back( cv::Point2d( d.part(54).x(), d.part(54).y() ) ); // Right mouth corner

  return image_points;
}
*/

std::vector<cv::Point2d> get_2d_image_points(full_object_detection &d)
{
  std::vector<cv::Point2d> image_points;

  image_points.push_back( cv::Point2d( d.part(0).x(), d.part(0).y() ) );
  image_points.push_back( cv::Point2d( d.part(1).x(), d.part(1).y() ) );
  image_points.push_back( cv::Point2d( d.part(16).x(), d.part(16).y() ) );
  image_points.push_back( cv::Point2d( d.part(15).x(), d.part(15).y() ) );

  image_points.push_back( cv::Point2d( d.part(39).x(), d.part(39).y() ) );
  image_points.push_back( cv::Point2d( d.part(40).x(), d.part(40).y() ) );
  image_points.push_back( cv::Point2d( d.part(42).x(), d.part(42).y() ) );
  image_points.push_back( cv::Point2d( d.part(47).x(), d.part(47).y() ) );

  image_points.push_back( cv::Point2d( d.part(27).x(), d.part(27).y() ) );
  image_points.push_back( cv::Point2d( d.part(28).x(), d.part(28).y() ) );
  image_points.push_back( cv::Point2d( d.part(29).x(), d.part(29).y() ) );
  image_points.push_back( cv::Point2d( d.part(30).x(), d.part(30).y() ) );

  image_points.push_back( cv::Point2d( d.part(31).x(), d.part(31).y() ) );
  image_points.push_back( cv::Point2d( d.part(32).x(), d.part(32).y() ) );
  image_points.push_back( cv::Point2d( d.part(33).x(), d.part(33).y() ) );
  image_points.push_back( cv::Point2d( d.part(34).x(), d.part(34).y() ) );
  image_points.push_back( cv::Point2d( d.part(35).x(), d.part(35).y() ) );

  return image_points;
}

std::vector<cv::Point2d> get_2d_face_points(full_object_detection &d)
{
	std::vector<cv::Point2d> face_points;
	for(int i=0;i<68;i++)
	{
		face_points.push_back( cv::Point2d( d.part(i).x(), d.part(i).y() ) );
	}
	return face_points;
}

cv::Mat get_camera_matrix(float focal_length,cv::Point2d center)
{
	cv::Mat camera_matrix=(cv::Mat_<double>(3,3)<<focal_length,0,center.x,0,focal_length,center.y,0,0,1);
	return camera_matrix;
}

void printData(const std::vector<double> &data)
{
	std::ofstream ofs("print.txt");
	for (int i=0;i<data.size();i++)
	{
		ofs<<data[i]<<" ";
	}
}

const double Kcp=-0.5;
//const double Kci=-0.000004;
const double Kci=-0.000005;
//const double Kcd=-0.0;
const double Kcd=-25;
//const double Kc_gain=38.8051;
//const double Kc_gain=40;
const double Kc_gain=40*1.0;

double PID_camera_controller(double _input_value,double _target)
{

	//誤差
	double error=2*(_input_value/2-_target)/_input_value;

	//時間
	static int last_t=0.0;
	double dt=(cv::getTickCount()-last_t)/cv::getTickFrequency();
	last_t=cv::getTickCount();

	//積分項
	static double integral=0.0;
	if(dt>0.1)
	{
		//リセット
		integral=0.0;
	}

	integral+=(error*dt);

	//微分項
	static double previous_error=0.0;
	if(dt>0.1)
	{
	//リセット
	previous_error=0.0;
	}

	double derivative=(error-previous_error)/dt;

	previous_error=error;

	//操作量
	double _output_value=Kcp*error+Kci*integral+Kcd*derivative;
	//double _output_value=-Kcp*error;

	return _output_value;

}

double camera_pan_error=0;
double pre_camera_pan_error=0;

const double Kdp=1.0;
const double Kdi=0.0;
const double Kdd=0.0;
const double Kd_gain=0.1;

double PID_drone_controller(double _input_value)
{

	//誤差
	double error=_input_value/M_PI;

	//時間
	static int last_t=0.0;
	double dt=(cv::getTickCount()-last_t)/cv::getTickFrequency();
	last_t=cv::getTickCount();

	//積分項
	static double integral=0.0;
	if(dt>0.1)
	{
		//リセット
		integral=0.0;
	}

	integral+=(error*dt);

	//微分項
	static double previous_error=0.0;
	if(dt>0.1)
	{
	//リセット
	previous_error=0.0;
	}

	double derivative=(error-previous_error)/dt;

	previous_error=error;

	//操作量
	double _output_value=Kdp*error+Kdi*integral+Kdd*derivative;

	return _output_value;

}

double drone_vy;

geometry_msgs::Twist camera_control;
geometry_msgs::Twist cmd_vel;

int pre_count=0;

double count_checker()
{
  pre_count++;
  return pre_count;
}

shape_predictor pose_model;
image_window win;

void head_estimator(cv_bridge::CvImagePtr _image_mat)
{
	try
	{

		if(pre_count<1)
		{
    	deserialize("shape_predictor_68_face_landmarks.dat") >> pose_model;
		}

		// Load face detection and pose estimation models.
		frontal_face_detector detector = get_frontal_face_detector();
		// Detect faces
		cv_image<bgr_pixel> cimg(_image_mat->image);
		std::vector<rectangle> faces = detector(cimg);

		// Find the pose of each face.
		std::vector<full_object_detection> shapes;
		for (unsigned long i = 0; i < faces.size(); ++i)
			shapes.push_back(pose_model(cimg, faces[i]));

		// Pose estimation
		std::vector<cv::Point3d> model_points = get_3d_model_points();

		for (unsigned long i = 0; i < faces.size(); i++)
		{
			rectangle r(
				(long)(faces[i].left()),
				(long)(faces[i].top()),
				(long)(faces[i].right()),
				(long)(faces[i].bottom())
			);

			full_object_detection shape = pose_model(cimg, r);
			shapes.push_back(shape);

			image_points = get_2d_image_points(shape);
			face_points = get_2d_face_points(shape);

			pre_camera_pan_error=camera_pan_error;
			camera_pan_error=Kc_gain*PID_camera_controller(_image_mat->image.cols,face_points[27].x);
			//cout << camera_pan_error << endl;

			double focal_length = _image_mat->image.cols;
			cv::Mat camera_matrix = get_camera_matrix(focal_length, cv::Point2d(_image_mat->image.cols/2, _image_mat->image.rows/2));
			cv::Mat rotation_vector;
			cv::Mat rotation_matrix;
			cv::Mat translation_vector;

			cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, cv::DataType<double>::type);

			//cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);
			cv::solvePnPRansac(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);

			//cv::Rodrigues(rotation_vector, rotation_matrix);

			nose_end_point3D.push_back(cv::Point3d(0, 0, 1000));

			cv::projectPoints(nose_end_point3D,rotation_vector,translation_vector,camera_matrix,dist_coeffs,nose_end_point2D);

			cv::line(_image_mat->image, image_points[11], nose_end_point2D[0], cv::Scalar(255, 0, 0), 2);

			cv::split(rotation_vector, XYZ_split);
			XYZ_split[0].reshape(0, 1).copyTo(dataXYZ);

			if (average_count > average_frames)
			{

				discrete_deque_x.push_back(dataXYZ[0]);
				discrete_deque_y.push_back(dataXYZ[1]);
				discrete_deque_z.push_back(dataXYZ[2]);

				discrete_deque_x.pop_front();
				discrete_deque_y.pop_front();
				discrete_deque_z.pop_front();

				median_x = discrete_deque_x;
				median_y = discrete_deque_y;
				median_z = discrete_deque_z;

				median_sort_x = discrete_deque_x;
				median_sort_y = discrete_deque_y;
				median_sort_z = discrete_deque_z;

				std::sort(median_sort_x.begin(), median_sort_x.end());
				std::sort(median_sort_y.begin(), median_sort_y.end());
				std::sort(median_sort_z.begin(), median_sort_z.end());

				for (int i=0; i<=average_frames; i++)
				{

					Med_difference_x.push_back(std::abs(median_sort_x[i]-median_sort_x[median_limit]));
					Med_difference_y.push_back(std::abs(median_sort_y[i]-median_sort_y[median_limit]));
					Med_difference_z.push_back(std::abs(median_sort_z[i]-median_sort_z[median_limit]));
				}

				Med_difference_sort_x=Med_difference_x;
				Med_difference_sort_y=Med_difference_y;
				Med_difference_sort_z=Med_difference_z;

				std::sort(Med_difference_sort_x.begin(),Med_difference_sort_x.end());
				std::sort(Med_difference_sort_y.begin(),Med_difference_sort_y.end());
				std::sort(Med_difference_sort_z.begin(),Med_difference_sort_z.end());

				MAD_x=Med_difference_sort_x[median_limit];
				MAD_y=Med_difference_sort_y[median_limit];
				MAD_z=Med_difference_sort_z[median_limit];

				MADN_x=MAD_x/0.675;
				MADN_y=MAD_y/0.675;
				MADN_z=MAD_z/0.675;

				for(int i=0;i<=average_frames;i++)
				{
					deviation_value_x.push_back((10*(median_x[i]-median_sort_x[median_limit]))/MADN_x + 50);
					deviation_value_y.push_back((10*(median_y[i]-median_sort_y[median_limit]))/MADN_x + 50);
					deviation_value_z.push_back((10*(median_z[i]-median_sort_z[median_limit]))/MADN_x + 50);
				}

				average_angle_x=0;
				average_angle_y=0;
				average_angle_z=0;

				deviation_count_x=0;
				deviation_count_y=0;
				deviation_count_z=0;

				for(int i=0;i<=average_frames;i++)
				{
					if(std::abs(deviation_value_x[i]-50)<deviation_limit_x)
					{
						average_angle_x+=median_x[i];
						deviation_count_x++;
					}

					if(std::abs(deviation_value_y[i]-50)<deviation_limit_y)
					{
						average_angle_y+=median_y[i];
						deviation_count_y++;;
					}

					if(std::abs(deviation_value_z[i]-50)<deviation_limit_z)
					{
						average_angle_z+=median_z[i];
						deviation_count_z++;
					}

				}

				average_angle_x/=deviation_count_x;
				average_angle_y/=deviation_count_y;
				average_angle_z/=deviation_count_z;

				median_x.clear();
				median_y.clear();
				median_z.clear();

				median_sort_x.clear();
				median_sort_y.clear();
				median_sort_z.clear();

				Med_difference_x.clear();
				Med_difference_y.clear();
				Med_difference_z.clear();

				Med_difference_sort_x.clear();
				Med_difference_sort_y.clear();
				Med_difference_sort_z.clear();

				deviation_value_x.clear();
				deviation_value_y.clear();
				deviation_value_z.clear();

				if(average_angle_count>2)
				{

					average_angle_x_deque.push_back(average_angle_x);
					average_angle_x_deque.pop_front();

					average_angle_y_deque.push_back(average_angle_y);
					average_angle_y_deque.pop_front();

					average_angle_z_deque.push_back(average_angle_z);
					average_angle_z_deque.pop_front();

					numeric_rotation_x=0;
					numeric_rotation_y=0;
					numeric_rotation_z=0;

					for(int i=0;i<average_angle_count;i++)
					{

						numeric_rotation_x+=average_angle_x_deque[i];
						numeric_rotation_y+=average_angle_y_deque[i];
						numeric_rotation_z+=average_angle_z_deque[i];

					}

					numeric_rotation_x/=average_angle_count;
					numeric_rotation_y/=average_angle_count;
					numeric_rotation_z/=average_angle_count;

					drone_vy=Kd_gain*PID_drone_controller(numeric_rotation_y);

					rotation_x.push_back(numeric_rotation_x);
					rotation_y.push_back(numeric_rotation_y);
					rotation_z.push_back(numeric_rotation_z);

					drone_rotation_y.push_back(numeric_rotation_y);

				}else{

					average_angle_x_deque.push_back(average_angle_x);
					average_angle_y_deque.push_back(average_angle_y);
					average_angle_z_deque.push_back(average_angle_z);

					average_angle_count++;

				}

			}else{

				discrete_deque_x.push_back(dataXYZ[0]);
				discrete_deque_y.push_back(dataXYZ[1]);
				discrete_deque_z.push_back(dataXYZ[2]);

				average_count++;

			}

/*
				double mouth_rx = face_points[54].x - face_points[66].x;
				double mouth_ry = face_points[54].y - face_points[66].y;
				double mouth_lx = face_points[48].x - face_points[66].x;
				double mouth_ly = face_points[48].y - face_points[66].y;

				double cos_smile = (mouth_rx*mouth_lx + mouth_ry*mouth_ly) / (sqrt(mouth_rx*mouth_rx + mouth_ry*mouth_ry)*sqrt(mouth_lx*mouth_lx + mouth_ly*mouth_ly));

				double open_hx = face_points[54].x - face_points[48].x;
				double open_hy = face_points[54].y - face_points[48].y;
				double open_vx = face_points[62].x - face_points[66].x;
				double open_vy = face_points[62].y - face_points[66].y;

				double surprised = sqrt(open_vx*open_vx + open_vy*open_vy) / (sqrt(open_hx*open_hx + open_hy*open_hy));

				if (cos_smile > -0.7&&surprised < 0.4&&-0.5 < average_test_y&&average_test_y < 0.5)
				{
					cv::putText(im, cv::format("smile"), cv::Point(face_points[19].x, (face_points[21].y + face_points[22].y) / 2), cv::FONT_HERSHEY_COMPLEX, 1.5, cv::Scalar(0, 0, 255), 3);
				}

				if (surprised > 0.4&&-0.5 < average_test_y&&average_test_y < 0.5)
				{
					cv::putText(im, cv::format("surprised"), cv::Point(face_points[19].x, (face_points[21].y + face_points[22].y) / 2), cv::FONT_HERSHEY_COMPLEX, 1.5, cv::Scalar(0, 0, 255), 3);
				}

				double eyebrows_x = face_points[21].x - face_points[22].x;
				double eyebrows_y = face_points[21].y - face_points[22].y;
				double eyebrows = sqrt(eyebrows_x*eyebrows_x + eyebrows_y*eyebrows_y);

				double nose_length_x = face_points[27].x - face_points[30].x;
				double nose_length_y = face_points[27].y - face_points[30].y;
				double nose_length = sqrt(nose_length_x*nose_length_x + nose_length_y*nose_length_y);

				double angry = eyebrows / nose_length;

				if (angry < 0.4&&-0.5 < average_test_y&&average_test_y < 0.5)
				{
					cv::putText(im, cv::format("angry"), cv::Point(face_points[19].x, (face_points[21].y + face_points[22].y) / 2), cv::FONT_HERSHEY_COMPLEX, 1.5, cv::Scalar(0, 0, 255), 3);
				}
*/

		}

		if (pre_count % 3 == 0)
		{
			int k = cv::waitKey(3);
			// Quit if 'q' or ESC is pressed
			if (k == 'q' || k == 27)
			{
				printData(drone_rotation_y);
				ros::shutdown();
				return;
			}
		}

		win.clear_overlay();
		win.set_image(cimg);
		win.add_overlay(render_face_detections(shapes));

	}

	catch (serialization_error& e)
	{
		cout << "You need dlib's default face landmarking model file to run this example." << endl;
		cout << "You can get it from the following URL: " << endl;
		cout << "http://dlib.net/files/shape_predictor_68_face_landmarks.dat.bz2" << endl;
		cout << endl << e.what() << endl;
	}

	catch (exception& e)
	{
		cout << e.what() << endl;
	}

}

ros::Publisher camera_control_pub_;
ros::Publisher cmd_vel_pub_;

cv_bridge::CvImagePtr cv_ptr;

void imageCb(const sensor_msgs::ImageConstPtr& msg)
{

	try
	{
		cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
		head_estimator(cv_ptr);

		camera_control.angular.z=camera_pan_error+pre_camera_pan_error;
		camera_control_pub_.publish(camera_control);

		cout<<camera_control.angular.z<<endl;

		cmd_vel.linear.x=0;
		cmd_vel.linear.y=-drone_vy;
		cmd_vel.linear.z=0;

		cmd_vel_pub_.publish(cmd_vel);

		int count=count_checker();

	}

	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
		return;
	}

/*
	if(pre_count%2==0){
		camera_control.angular.z=0;
	}else{
		camera_control.angular.z=0;
	}

	cout<<camera_control.angular.z<<endl;
	cout<<pre_count<<endl;

	camera_control_pub_.publish(camera_control);
*/

	cout<<pre_count<<endl;

	cv::imshow("OPENCV_WINDOW", cv_ptr->image);
	cv::waitKey(3);

}

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

	ros::init(argc, argv, "image_converter");
	ros::NodeHandle nh_;

	image_transport::ImageTransport it_(nh_);
	image_transport::Subscriber image_sub_;

	//ros::Publisher camera_control_pub_;
	camera_control_pub_=nh_.advertise<geometry_msgs::Twist>("/bebop/camera_control",1);

	//ros::Publisher cmd_vel_pub_;
	cmd_vel_pub_=nh_.advertise<geometry_msgs::Twist>("/bebop/pre_cmd_vel",1);

	image_sub_ = it_.subscribe("/usb_cam/image_raw",1,&imageCb);
	//image_sub_ = it_.subscribe("/bebop/image_raw",1,&imageCb);
	ros::Rate loop_rate(30);

	while(ros::ok())
	{
		//ros::spin();
	ros::spinOnce();
	loop_rate.sleep();
	cv::waitKey(3);
	}

	return 0;

}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file testSignature
 * @brief Tests focusing on the details of Signatures to evaluate boost
 * compliance
 * @author Alex Cunningham
 * @date Sept 19th 2011
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/Signature.h>

#include <boost/assign/std/vector.hpp>
#include <vector>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

DiscreteKey X(0, 2), Y(1, 3), Z(2, 2);

/* ************************************************************************* */
TEST(testSignature, simple_conditional) {
  Signature sig(X | Y = "1/1 2/3 1/4");
  Signature::Table table = *sig.table();
  vector<double> row[3]{{0.5, 0.5}, {0.4, 0.6}, {0.2, 0.8}};
  CHECK(row[0] == table[0]);
  CHECK(row[1] == table[1]);
  CHECK(row[2] == table[2]);
  DiscreteKey actKey = sig.key();
  LONGS_EQUAL(X.first, actKey.first);

  DiscreteKeys actKeys = sig.discreteKeys();
  LONGS_EQUAL(2, actKeys.size());
  LONGS_EQUAL(X.first, actKeys.front().first);
  LONGS_EQUAL(Y.first, actKeys.back().first);

  vector<double> actCpt = sig.cpt();
  EXPECT_LONGS_EQUAL(6, actCpt.size());
}

/* ************************************************************************* */
TEST(testSignature, simple_conditional_nonparser) {
  Signature::Table table;
  Signature::Row row1, row2, row3;
  row1 += 1.0, 1.0;
  row2 += 2.0, 3.0;
  row3 += 1.0, 4.0;
  table += row1, row2, row3;

  Signature sig(X | Y = table);
  DiscreteKey actKey = sig.key();
  EXPECT_LONGS_EQUAL(X.first, actKey.first);

  DiscreteKeys actKeys = sig.discreteKeys();
  LONGS_EQUAL(2, actKeys.size());
  LONGS_EQUAL(X.first, actKeys.front().first);
  LONGS_EQUAL(Y.first, actKeys.back().first);

  vector<double> actCpt = sig.cpt();
  EXPECT_LONGS_EQUAL(6, actCpt.size());
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


/*
==== Author:

Relja Arandjelovic (relja@robots.ox.ac.uk)
Visual Geometry Group,
Department of Engineering Science
University of Oxford

==== Copyright:

The library belongs to Relja Arandjelovic and the University of Oxford.
No usage or redistribution is allowed without explicit permission.
*/

#include "colour_sift.h"

#include <string>
#include <stdio.h>

#include <boost/format.hpp>
#include <boost/filesystem.hpp>

#include "image_util.h"
#include "macros.h"

void readRegsAndDescs( const char fileName[], uint32_t &numFeats, std::vector<ellipse> &regions, float *&descs, uint32_t const expectDim ){

    int ret;
    uint32_t numDims, elementsPerPoint, bytesPerElement;

    FILE *fin= fopen(fileName, "rb");

    if (fin==NULL){
        regions.clear();
        numFeats= 0;
        descs= new float[0];
        return;
    }

    {
        uint8_t temp[16];
        ret= fread(temp, 1, 16, fin); // ignore headers
    }

    ret= fread(&elementsPerPoint, sizeof(elementsPerPoint), 1, fin);
    ASSERT( elementsPerPoint == 5); // x y scale orientation cornerness
    ret= fread(&numDims, sizeof(numDims), 1, fin);
    ret= fread(&numFeats, sizeof(numFeats), 1, fin);
    if (numFeats==0){
        regions.clear();
        descs= new float[0];
        return;
    }
    ASSERT(numDims == expectDim);
    ret= fread(&bytesPerElement, sizeof(numFeats), 1, fin);
    ASSERT(bytesPerElement == 8);

    double *regsRaw= new double[elementsPerPoint*numFeats];
    ret= fread(regsRaw, sizeof(double), elementsPerPoint*numFeats, fin);

    double *itRegs= regsRaw;
    double r, a;
    regions.resize( numFeats );
    for (uint32_t i=0; i<numFeats; ++i, itRegs+= 5){
        r= *(itRegs+2);
        a= 1.0/(r*r);
        regions[i].set( *itRegs, *(itRegs+1), a, 0.0, a);
    }

    delete []regsRaw;

    double *descsRaw= new double[numDims*numFeats];
    ret= fread(descsRaw, sizeof(double), numDims*numFeats, fin);
    fclose(fin);
    REMOVE_UNUSED_WARNING(ret);

    descs= new float[numDims*numFeats];
    float *descsEnd= descs + numDims*numFeats;
    double *descsRawIt= descsRaw;
    for (float *descsIt= descs; descsIt != descsEnd; ++descsIt, ++descsRawIt)
        *descsIt= *descsRawIt;
    delete []descsRaw;
}

vanDeSande::vanDeSande(std::string descriptor, double harrisK, double laplaceThreshold)
        : descriptor_(descriptor),
          harrisK_(harrisK),
          laplaceThreshold_(laplaceThreshold) {
    if (descriptor=="opponentsift")
        numDims_= 384;
    else if (descriptor=="sift")
        numDims_= 128;
    else
        throw std::runtime_error( "Unknown descriptor" );
}

void vanDeSande::getFeats( const char fileName[], uint32_t &numFeats, std::vector<ellipse> &regions, float *&descs ) const {
    boost::filesystem::path tmpdir = boost::filesystem::temp_directory_path();
    boost::filesystem::path tmpfn = tmpdir / boost::filesystem::unique_path("vise_relja_retrival_%%%%%%%%%%%%%%%%.jpg");
    std::string tempDescsFn = tmpfn.string();

    // convert to jpg if it isn't jpg already
    std::string fileName_jpeg;
    bool doDelJpeg= false;
    if (!imageUtil::checkAndConvertToJpegTemp(fileName, fileName_jpeg, doDelJpeg)){
        numFeats= 0;
        regions.clear();
        descs= new float[0];
        return;
    }

    // execute
    int ret= system(
        ( boost::format("colorDescriptor \"%s\" --detector harrislaplace --harrisK %.3f --laplaceThreshold %.3f --descriptor %s --noErrorLog --outputFormat binary --output \"%s\" > /dev/null") % fileName % harrisK_ % laplaceThreshold_ % descriptor_ % tempDescsFn ).str().c_str() );
    REMOVE_UNUSED_WARNING(ret);

    // image cleanup
    if (doDelJpeg)
        boost::filesystem::remove( fileName_jpeg );

    // read
    readRegsAndDescs( tempDescsFn.c_str(), numFeats, regions, descs, numDims_ );

    // cleanup
    boost::filesystem::remove( tempDescsFn );

}

std::string
vanDeSande::getRawDescs(float const *descs, uint32_t numFeats) const {
    std::string res(numFeats*numDims_, '\0');
    uint8_t *resIter= reinterpret_cast<uint8_t*>(&res[0]);
    float const *inIter= descs;
    float const *inIterEnd= descs + numFeats*numDims_;
    for (; inIter!=inIterEnd; ++inIter, ++resIter)
        *resIter= static_cast<uint8_t>(*inIter + 0.1); // +0.1 is to counter numerical issues because cast does floor()
    return res;
}