// Boost.Polygon library detail/skeleton_predicates.hpp header file

//          Copyright Andrii Sydorchuk 2012.
//          Copyright Lucanus Simonson 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_POLYGON_DETAIL_SKELETON_PREDICATES
#define BOOST_POLYGON_DETAIL_SKELETON_PREDICATES

#ifndef BOOST_POLYGON_NO_DEPS
#include <boost/cstdint.hpp>
#endif

#include <cmath>

#include "voronoi_ctypes.hpp"
#include "voronoi_robust_fpt.hpp"

namespace boost {
namespace polygon {
namespace detail {

template <typename T>
struct skeleton_ctype_traits;

template <>
struct skeleton_ctype_traits<int32> {
  typedef int32 int_type;
  typedef int64 int_x2_type;
  typedef extended_int<16> big_int_type;
  typedef fpt64 fpt_type;
  typedef type_converter_fpt to_fpt_converter_type;
};

template <typename T>
class intersection_event {
 public:
  typedef T coordinate_type;

  intersection_event() {}

  coordinate_type x() const { return x_; }
  void x(const coordinate_type& val) { x_ = val; }

  coordinate_type y() const { return y_; }
  void y(const coordinate_type& val) { y_ = val; }

  coordinate_type r() const { return r_; }
  void r(const coordinate_type& val) { r_ = val; }

 private:
  coordinate_type x_;
  coordinate_type y_;
  coordinate_type r_;
};

template <typename CTYPE_TRAITS>
class skeleton_predicates {
 public:
  typedef typename CTYPE_TRAITS::int_type int_type;
  typedef typename CTYPE_TRAITS::int_x2_type int_x2_type;
  typedef typename CTYPE_TRAITS::big_int_type big_int_type;
  typedef typename CTYPE_TRAITS::fpt_type fpt_type;
  typedef typename CTYPE_TRAITS::to_fpt_converter_type to_fpt_converter;

  template <typename Segment>
  class event_existence_predicate {
   public:
    typedef Segment segment_type;

    bool operator()(const segment_type& segment1,
                    const segment_type& segment2,
                    const segment_type& segment3) {
      // TODO(asydorchuk): For now don't do any checks and just return true.
      // Should be enough to solve convex polygon case.
      return true;
    }
  };

  template <typename Segment, typename Event>
  class mp_event_formation_functor {
   public:
    typedef Segment segment_type;
    typedef Event event_type;
    typedef robust_sqrt_expr<big_int_type, fpt_type, to_fpt_converter>
        robust_sqrt_expr_type;

    void operator()(
        const segment_type& segment1,
        const segment_type& segment2,
        const segment_type& segment3,
        event_type* event,
        bool recompute_x = true,
        bool recompute_y = true,
        bool recompute_r = true) {
      a[0] = static_cast<int_x2_type>(segment1.high().x()) -
             static_cast<int_x2_type>(segment1.low().x());
      a[1] = static_cast<int_x2_type>(segment2.high().x()) -
             static_cast<int_x2_type>(segment2.low().x());
      a[2] = static_cast<int_x2_type>(segment3.high().x()) -
             static_cast<int_x2_type>(segment3.low().x());

      b[0] = static_cast<int_x2_type>(segment1.high().y()) -
             static_cast<int_x2_type>(segment1.low().y());
      b[1] = static_cast<int_x2_type>(segment2.high().y()) -
             static_cast<int_x2_type>(segment2.low().y());
      b[2] = static_cast<int_x2_type>(segment3.high().y()) -
             static_cast<int_x2_type>(segment3.low().y());

      c[0] = static_cast<int_x2_type>(segment1.low().x()) *
             static_cast<int_x2_type>(segment1.high().y()) -
             static_cast<int_x2_type>(segment1.low().y()) *
             static_cast<int_x2_type>(segment1.high().x());
      c[1] = static_cast<int_x2_type>(segment2.low().x()) *
             static_cast<int_x2_type>(segment2.high().y()) -
             static_cast<int_x2_type>(segment2.low().y()) *
             static_cast<int_x2_type>(segment2.high().x());
      c[2] = static_cast<int_x2_type>(segment3.low().x()) *
             static_cast<int_x2_type>(segment3.high().y()) -
             static_cast<int_x2_type>(segment3.low().y()) *
             static_cast<int_x2_type>(segment3.high().x());

      for (int i = 0; i < 3; ++i) {
        int j = (i+1) % 3;
        int k = (i+2) % 3;
        cA[i] = a[j] * b[k] - a[k] * b[j];
        cB[i] = a[i] * a[i] + b[i] * b[i];
      }
      fpt_type inv_denom = 1.0 / to_fpt(sqrt_expr_.eval3(cA, cB));

      if (recompute_r) {
        for (int i = 0; i < 3; ++i) {
          cA[i] = cA[i] * c[i];
        }
        fpt_type r = to_fpt(cA[0] + cA[1] + cA[2]);
        event->r(r * inv_denom);
        if (is_neg(event->r())) {
          event->r(-event->r());
	}
      }

      if (recompute_x) {
        for (int i = 0; i < 3; ++i) {
          int j = (i+1) % 3;
          int k = (i+2) % 3;
          cA[i] = a[j] * c[k] - a[k] * c[j];
        }
        fpt_type x = to_fpt(sqrt_expr_.eval3(cA, cB));
        event->x(x * inv_denom);
      }

      if (recompute_y) {
        for (int i = 0; i < 3; ++i) {
          int j = (i+1) % 3;
          int k = (i+2) % 3;
          cA[i] = b[j] * c[k] - b[k] * c[j];
        }
        fpt_type y = to_fpt(sqrt_expr_.eval3(cA, cB));
        event->y(y * inv_denom);
      }
    }

   private:
    big_int_type a[3];
    big_int_type b[3];
    big_int_type c[3];
    big_int_type cA[3];
    big_int_type cB[3];
    to_fpt_converter to_fpt;
    robust_sqrt_expr_type sqrt_expr_;
  };

  template <typename Segment,
            typename Event,
            typename EEP = event_existence_predicate<Segment>,
            typename EFF = mp_event_formation_functor<Segment, Event> >
  class event_formation_predicate {
   public:
    typedef Segment segment_type;
    typedef Event event_type;
    typedef EEP event_existence_predicate_type;
    typedef EFF event_formation_functor_type;

    bool operator()(const segment_type& segment1, const segment_type& segment2,
                    const segment_type& segment3, event_type* event) {
      if (!event_existence_predicate_(segment1, segment2, segment3)) {
        return false;
      }
      event_formation_functor_(segment1, segment2, segment3, event);
      return true;
    }

   private:
    event_existence_predicate_type event_existence_predicate_;
    event_formation_functor_type event_formation_functor_;
  };
};
}  // detail
}  // polygon
}  // boost

#endif  // BOOST_POLYGON_DETAIL_SKELETON_PREDICATES


#include "CompliantStaticSolver.h"

#include <sofa/core/ObjectFactory.h>

#include <boost/math/tools/minima.hpp>
#include <tuple>

#include "../utils/nr.h"
#include "../utils/scoped.h"

#include "../constraint/Constraint.h"

namespace sofa {
namespace component {
namespace odesolver {

// TODO use std::numeric_limits

CompliantStaticSolver::CompliantStaticSolver()
    : epsilon(initData(&epsilon, (SReal)1e-16, "epsilon", "division by zero threshold")),
      line_search(initData(&line_search, unsigned(LS_SECANT), "line_search",
                           "line search method, 0: none (use dt), 1: brent, 2: secant (default). (warning: brent does not work with constraints.")),
      conjugate(initData(&conjugate, true, "conjugate", "conjugate descent directions")),
      ls_precision(initData(&ls_precision, (SReal)1e-8, "ls_precision", "line search precision")),
      ls_iterations(initData(&ls_iterations, unsigned(10), "ls_iterations",
                             "line search iterations")),
      ls_step(initData(&ls_step, (SReal)1e-8, "ls_step",
                       "line search bracketing step (should not cross any extrema from current position"))
{

}

struct CompliantStaticSolver::helper {

    simulation::common::VectorOperations vec;
    simulation::common::MechanicalOperations mec;

    core::behavior::MultiVecDeriv dx;
    core::behavior::MultiVecDeriv f;

    helper(const core::ExecParams* params,
           core::objectmodel::BaseContext* ctx) : vec(params, ctx),
                                                  mec(params, ctx),
                                                  dx( &vec, core::VecDerivId::dx() ),
                                                  f( &vec, core::VecDerivId::force() )    {
    }

    // TODO const args ?
    SReal dot(core::MultiVecDerivId lhs, core::MultiVecDerivId rhs)  {
        vec.v_dot(lhs, rhs);
        return vec.finish();
    }

    // arg can't be dx !
    void K(core::MultiVecDerivId res, core::MultiVecDerivId arg) {
        vec.v_eq(dx, arg);
        vec.v_clear( res );

        mec.addMBKdx( res , 0, 0, -1, true, true);
        mec.projectResponse( res );
    }

    void forces(core::MultiVecDerivId res) {
        mec.computeForce( res );
        mec.projectResponse( res );
    }

    template<class Vec>
    void realloc(Vec& res) {
        res.realloc( &vec, false, true );
    }

    template<class Res, class A, class B>
    void set(Res& res,
             A& a,
             B& b,
             SReal lambda) {
        vec.v_op(res, a, b, lambda);
    }

    template<class A, class B>
    void set(A& res,
             B& b) {
        vec.v_eq(res, b);
    }

};

CompliantStaticSolver::ls_info::ls_info()
    : eps(0),
      iterations(15),
      precision(1e-7),
      fixed_step(1e-5),
      bracket_step(1e-8) {

}

// minimizes (implicit) potential function in direction dir) by
// zeroing g = grad^T dir using secant method

// precond:
// - op.f contains forces for current pos
void CompliantStaticSolver::ls_secant(helper& op,
                                      core::MultiVecCoordId pos,
                                      core::MultiVecDerivId dir,
                                      const ls_info& info) {
    scoped::timer step("ls_secant");

    SReal dg = 0;
    SReal dx = 0;

    SReal g_prev = 0;
    SReal total = 0;

    // slightly damp newton iterations TODO hardcode
    static const SReal damping = 1e-14;

    SReal fixed = info.fixed_step;

    for(unsigned k = 0, n = info.iterations; k < n; ++k) {

        if( k ) {
            // update forces
            op.mec.propagateX(pos, true);
            op.forces( op.f );
        }

        const SReal g = op.dot(op.f, dir);

        // std::cout << "line search (secant) " << k << " "  << total << " " << g << std::endl;

        // are we done ?
        if( std::abs(g) <= info.precision ) break;

        dg = g - g_prev;

        const SReal dx_prev = dx;

        // fallback on fixed step
        dx = fixed;

        // (damped) secant method
        if( k && (std::abs(dg) > info.eps)) {
            dx = -(dx_prev / (dg + damping)) * g;
        } else {

            // try to move more to change function
            fixed *= 2;

        }

        total += dx;

        // move dx along dir
        op.set(pos, pos, dir, dx);

        // next
        g_prev = g;
    }

}

struct CompliantStaticSolver::potential_energy {

    // WARNING: backup/restore pos on scope entry/exit
    potential_energy(helper& op,
                     const core::MultiVecCoordId& pos,
                     const core::MultiVecDerivId& dir,
                     const core::MultiVecCoordId& tmp,
                     bool restore = true)
        : op(op),
          pos(pos),
          dir(dir),
          tmp(tmp),
          restore(restore) {

        // backup start position
        op.set(tmp, pos);
    }

    helper& op;

    const core::MultiVecCoordId& pos;
    const core::MultiVecDerivId& dir;
    const core::MultiVecCoordId& tmp;

    // do we restore pos in dtor ?
    bool restore;

    SReal operator()(SReal x) const {

        // move to x along dir
        op.set(pos, tmp, dir, x);

        // update forces/energy
        op.mec.propagateX(pos, true);

        // TODO apparently, this is not needed
        // op.forces(op.f);

        // note: potential energy only works for pos !
        SReal dummy, result;
        op.mec.computeEnergy(dummy, result);

        // std::cout << "potential energy: " << x << ", " << result << std::endl;

        return result;
    }

    ~potential_energy() {
        if( restore ) {
            op.set(pos, tmp);
        }
    }

};

void CompliantStaticSolver::ls_brent(helper& op,
                                     core::MultiVecCoordId pos,
                                     core::MultiVecDerivId dir,
                                     const ls_info& info,
                                     core::MultiVecCoordId tmp) {
    scoped::timer step("ls_brent");
    typedef utils::nr::optimization<SReal> opt;

    opt::func_call a, b, c;

    a.x = 0;
    b.x = info.bracket_step;

    opt::func_call res;

    {
        // we need the scope because f might reset pos on scope exit
        const potential_energy f(op, pos, dir, tmp, false);

        opt::minimum_bracket(a, b, c, f);

        // std::cout << "bracketing: " << a.x << ", " << c.x << std::endl;

        // TODO compute this from precision
        const int bits = 32;
        {
            boost::uintmax_t iter = info.iterations;
            std::tie(res.x, res.f) = boost::math::tools::brent_find_minima(f,
                                                               a.x, c.x,
                                                               bits,
                                                               iter);
            // std::cout << "brent: " << res.x << std::endl;
        }

    }

    // TODO make sure the last function call is the closest to the optimium
    // op.set(pos, pos, dir, res.x);

    // TODO do we want to do this ?
    // op.mec.propagateX(pos, true);
    // op.forces(op.f);

}

// accumulate force in an external vector
class AugmentedLagrangianVisitor : public simulation::MechanicalVisitor {

public:
    AugmentedLagrangianVisitor(const sofa::core::MechanicalParams* mparams,
                               core::MultiVecId id)
        : MechanicalVisitor(mparams),
          id(id) { }

    core::MultiVecId id;

    Result mstate(simulation::Node* node,
                  core::behavior::BaseMechanicalState* mm) {

        linearsolver::Constraint* c = node->get<linearsolver::Constraint>(core::objectmodel::BaseContext::Local);

        if( c ) {
            // TODO project ?

            // add force to external force
            mm->vOp(params, id.getId(mm), core::ConstVecId::force() );
        }

        return RESULT_CONTINUE;
    }

    virtual Result fwdMappedMechanicalState(simulation::Node* node,
                                            core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

    virtual Result fwdMechanicalState(simulation::Node* node,
                                      core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

};

// somehow setting external force directly does not work
class WriteExternalForceVisitor : public simulation::MechanicalVisitor {
public:
    WriteExternalForceVisitor(const sofa::core::MechanicalParams* mparams,
                              core::MultiVecId id)
        : MechanicalVisitor(mparams),
          id(id) { }

    core::MultiVecId id;

    Result mstate(simulation::Node* /*node*/,
                  core::behavior::BaseMechanicalState* mm) {

        // add force to external force

        // we need to add to externalForce (gravity)
        mm->vOp(params, core::VecId::externalForce(), id.getId(mm));
        // core::VecId::externalForce(),
        // , 1.0 );

        return RESULT_CONTINUE;
    }

    virtual Result fwdMappedMechanicalState(simulation::Node* node,
                                            core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

    virtual Result fwdMechanicalState(simulation::Node* node,
                                      core::behavior::BaseMechanicalState* mm) {
        return mstate(node, mm);
    }

};

SOFA_DECL_CLASS(CompliantStaticSolver)
int CompliantStaticSolverClass = core::RegisterObject("Static solver")
    .add< CompliantStaticSolver >();

    void CompliantStaticSolver::solve(const core::ExecParams* params,
                                SReal dt,
                                core::MultiVecCoordId pos,
                                core::MultiVecDerivId vel) {

        helper op(params, getContext() );

        // mparams setup
        op.mec.mparams.setImplicit(false);
        op.mec.mparams.setEnergy(true);

        // descent direction
        op.realloc(dir);

        // lagrange multipliers
        op.realloc(lambda);

        // some work vector for postions
        op.realloc(tmp);

        // first iteration
        if(!iteration) {
            op.vec.v_clear( lambda );
            previous = 0;

        }

        // why on earth does this dot work ?!

        // core::behavior::MultiVecDeriv ext(&op.vec, core::VecDerivId::externalForce() );
        // op.set(ext, lambda);
        {
            WriteExternalForceVisitor vis(&op.mec.mparams, lambda.id());
            getContext()->executeVisitor(&vis, true);

            // core::behavior::MultiVecDeriv ext(&op.vec, core::VecDerivId::externalForce() );
            // std::cout << ext << std::endl;
        }

        // obtain (projected) gradient
        op.forces( op.f );

        // note: we *could* skip the above when line-search is on
        // after the first iteration, but we would miss any change in
        // the scene (e.g. mouse interaction)

        // polar-ribiere
        const SReal current = op.dot(op.f, op.f);

        if(!iteration) {
            // something large at first ?

            // TODO figure out a reasonable default
            augmented = std::sqrt(current) / 2.0;
        }

        SReal beta = 0;

        const SReal eps = epsilon.getValue();
        if( conjugate.getValue() && std::abs(previous) > eps ) {

            {
                if(iteration > 0) {
                    // polak-ribiere
                    beta = (current - op.dot(vel, op.f) ) / previous;

                    // dai-yuan
                    // beta = current / (op.dot(dir, vel) - op.dot(dir, op.f) );
                }

                // direction reset
                // beta = std::max(0.0, beta);
            }

        }

        // conjugation
        op.set(dir, op.f, dir, beta);

        // polak-ribiere
        {
            // backup previous f to vel
            op.set(vel, op.f);
        }

        // line search
        const unsigned ls = line_search.getValue();
        if( ls ) {
            ls_info info;

            info.eps = eps;
            info.precision = ls_precision.getValue();
            info.iterations = ls_iterations.getValue();
            info.fixed_step = dt;
            info.bracket_step = ls_step.getValue();

            switch( ls ) {

            case LS_SECANT:
                ls_secant(op, pos, dir.id(), info);
                break;

            case LS_BRENT:
                ls_brent(op, pos, dir.id(), info, tmp.id());
                break;

            default:
                throw std::runtime_error("bad line-search");
            }

        } else {
            // fixed step
            op.set(pos, pos, dir.id(), dt);
        }

        const SReal error = std::sqrt( op.dot(op.f, op.f) );

        if( f_printLog.getValue() ) {
            sout << "forces norm: " << error << sendl;
        }

        // augmented lagrangian
        if( error <= augmented ) {

            // TODO don't waste time if we have no constraints
            op.mec.propagateX(pos, true);
            op.forces(op.f);

            AugmentedLagrangianVisitor vis(&op.mec.mparams, lambda.id() );
            getContext()->executeVisitor( &vis, true );

            // TODO should we reset CG ?
            // op.vec.v_clear(dir);

            augmented /= 2;

            if( f_printLog.getValue() ) {
                sout << "augmented lagrangian threshold: " << augmented << sendl;
            }

        }

        // next iteration
        previous = current;

        ++iteration;
    }

    void CompliantStaticSolver::reset() {
        iteration = 0;

    }

    void CompliantStaticSolver::init() {

    }

}
}
}


#ifndef Dh_DDMRM_IceRay_main_interface_python_core_geometry_transform_HPP
#define Dh_DDMRM_IceRay_main_interface_python_core_geometry_transform_HPP

#include <boost/python.hpp>

#include "../../../def_submodule.hpp"

#include "IceRay/type/math/matrix.hpp"
#include "IceRay/type/math/affine.hpp"
#include "IceRay/type/math/coord.hpp"
#include "IceRay/type/basic/basic.hpp"

#include "IceRay/geometry/_pure/_base.hpp"

typedef GS_DDMRM::S_IceRay::S_type::S_matrix::GT_scalar3D  GTs_matrix3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar3D   GTs_coord3D;
typedef GS_DDMRM::S_IceRay::S_type::S_affine::GT_scalar3D  GTs_affine3D;
typedef GS_DDMRM::S_IceRay::S_type::GT_scalar              GTs_scalar;
typedef GS_DDMRM::S_IceRay::S_geometry::S__pure::GC__base  GTs__base;

#endif


#include <opencv2/face.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/objdetect.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/core/ocl.hpp>
#include <fstream>
#include <iostream>
#include <vector>
#include <string>

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

#include "dlib_resnet_face_recognition.h"
#include "opencv_resnet_face_detection.h"
#include "speak.h"
#include "lang.h"

#include "listdir.h"
#include "pathutils.h"

using namespace std;
using namespace cv;
using namespace cv::face;
using namespace dlib;

void alignFace(Mat &imFace, Mat &alignedImFace, std::vector<Point2f> &landmarks, bool alsoLandmarks=false)
{
    float l_x = landmarks[39].x;
    float l_y = landmarks[39].y;
    float r_x = landmarks[42].x;
    float r_y = landmarks[42].y;
    float dx = r_x - l_x;
    float dy = r_y - l_y;
    double angle = atan2(dy, dx) * 180 / 3.1415927;
    Point2f eyesCenter;
    eyesCenter.x = (l_x + r_x) / 2.0;
    eyesCenter.y = (l_y + r_y) / 2.0;
    Mat M = Mat(2, 3, CV_32F);
    M = getRotationMatrix2D(eyesCenter, angle, 1);
    warpAffine(imFace, alignedImFace, M, imFace.size());
    if (alsoLandmarks)
        for (size_t i = 0; i < landmarks.size(); i++) {
            landmarks[i] = Point2f (
                M.at<double>(0, 0) * landmarks[i].x + M.at<double>(0, 1) * landmarks[i].y + M.at<double>(0,2),
                M.at<double>(1, 0) * landmarks[i].x + M.at<double>(1, 1) * landmarks[i].y + M.at<double>(1,2)
            );
        }
}

Mat getCroppedFaceRegion(Mat image, std::vector<Point2f> landmarks)
{
    int ytop = min(landmarks[19].y,landmarks[24].y);
    int ybottom = max(landmarks[6].y,landmarks[10].y);
    int xleft = min(landmarks[17].x,landmarks[4].x);
    int xright = min(landmarks[26].x,landmarks[12].x);
    Rect roi(xleft,ytop,xright-xleft+1,ybottom-ytop+1);
    return image(roi);
}

Mat getCroppedHeadRegion(Mat image, std::vector<Point2f> landmarks)
{
    int x1Limit = landmarks[0].x - (landmarks[36].x - landmarks[0].x);
    int x2Limit = landmarks[16].x + (landmarks[16].x - landmarks[45].x);
    int y1Limit = landmarks[27].y - 3*(landmarks[30].y - landmarks[27].y);
    int y2Limit = landmarks[8].y + (landmarks[30].y - landmarks[29].y);
    int x1 = max(x1Limit,0);
    int x2 = min(x2Limit, image.cols);
    int y1 = max(y1Limit, 0);
    int y2 = min(y2Limit, image.rows);
    cv::Rect selectedRegion = cv::Rect( x1, y1, x2-x1, y2-y1 );
    return image(selectedRegion);
}

std::vector<Point2f> getLandmarks (shape_predictor &landmarkDetector, Mat &img, Rect rect)
{
    std::vector<Point2f> points;
    dlib::cv_image<dlib::bgr_pixel> dlibIm(img);
    dlib::rectangle dlibRect(rect.x,rect.y,rect.x+rect.width,rect.y+rect.height);
    dlib::full_object_detection landmarks = landmarkDetector(dlibIm, dlibRect);
    for (size_t i = 0; i < landmarks.num_parts(); i++) {
        Point2f pt(landmarks.part(i).x(), landmarks.part(i).y());
        points.push_back(pt);
    }
    return points;
}

std::vector<Point2f> getFaceAndLandmarks (shape_predictor &landmarkDetector, Mat &img)
{
    std::vector<Rect> faces;
    std::vector<float> confidences;
    resnet_face_detect(img,faces,confidences);
    int kind = 0;
    if (faces.size() > 0) {
        size_t best = 0;
        for (size_t i=1; i<faces.size(); i++)
            if (abs(faces[best].x+faces[best].width/2-img.cols/2) > abs(faces[i].x+faces[i].width/2-img.cols/2)) best = i;
        Rect faceRect =  faces[best];
        return getLandmarks(landmarkDetector,img,faceRect);
    }
    else return std::vector<Point2f>();
}

int stabilize (int label, int n)
{
    static std::deque<int> labels;
    if (label < 0) {
        if (labels.size() > 0) labels.pop_front();
        return label;
    }
    else {
        labels.push_back(label);
        if (labels.size() > n) labels.pop_front();
        std::deque<int> values;
        std::copy(labels.begin(), labels.end(), std::back_inserter(values));
        std::nth_element(values.begin(), values.begin() + values.size()/2, values.end());
        return values[values.size()/2];
    }
}

int main(int argc,char** argv) {
    bool to_train = true;
    if (argc > 1) to_train = false;

    cout << "...started" << endl;

    resnet_face_init();
    cout << "...resnet face detector loaded" << endl;

    shape_predictor landmarkDetector;
    deserialize("shape_predictor_68_face_landmarks.dat") >> landmarkDetector;
    cout << "...face landmark detector loaded" << endl;

    resnet_descriptor_init();
    cout << "...resnet face recognition loaded" << endl;

    std::vector<int> labels;
    std::vector<std::vector<float>> descriptors;
    std::vector<string> folders;
    string faceDatasetFolder = "faces";
    string descriptorDatasetFolder = "descriptors";
    if (to_train) {
		std::vector<std::vector<string>> filenames;
		listdirs(faceDatasetFolder, folders, filenames);
		printdirs(folders,filenames);

        std::vector<Mat> images;
        std::vector<string> imageFilenames;
		for (size_t i=0; i<folders.size(); i++) {
			for (size_t j=0; j<filenames[i].size(); j++) {
				Mat im = cv::imread(faceDatasetFolder+"/"+folders[i]+"/"+filenames[i][j],IMREAD_COLOR);
				std::vector<Point2f> landmarks = getFaceAndLandmarks(landmarkDetector, im);
				if (landmarks.size() < 68) continue;

				Mat gray, aligned;
				cvtColor(im, gray, COLOR_BGR2GRAY);
				alignFace(gray, aligned, landmarks, true);
				Mat imFace = getCroppedFaceRegion(aligned, landmarks);
				equalizeHist(imFace, imFace);

				printf("...preprocessed %s %s\n",folders[i].c_str(),filenames[i][j].c_str());

				cv::resize(imFace, imFace, Size(150, 150));

				images.push_back(imFace);
				labels.push_back(i);
                imageFilenames.push_back(filenames[i][j].substr(0,filenames[i][j].find_last_of(".")));
			}
		}

		cout << "...faces loaded" << endl;

		for (size_t j = 0; j < images.size(); j++) {
			string path(descriptorDatasetFolder+"/"+folders[labels[j]]);
			createpath(path);
			imwrite(path+"/"+imageFilenames[j]+".png",images[j]);
			std::vector<float> descriptor = resnet_descriptor(images[j]);
			descriptors.push_back(descriptor);
			write_resnet_descriptor(path+"/"+imageFilenames[j]+".txt",labels[j],descriptor);
			printf("...descripted %s %d\n",folders[labels[j]].c_str(),labels[j]);
		}

		cout << "...trained" << endl;
    }
    else {
        std::vector<std::vector<string>> filenames;
        listdirs(descriptorDatasetFolder, folders, filenames, true);
        printdirs(folders,filenames);

        for (size_t i=0; i<folders.size(); i++) {
            for (size_t j=0; j<filenames[i].size(); j++) {
                int label;
                std::vector<float> descriptor;
                read_resnet_descriptor(descriptorDatasetFolder+"/"+folders[i]+"/"+filenames[i][j],label,descriptor);
                descriptors.push_back(descriptor);
                labels.push_back(label);
                cout << "... " << descriptorDatasetFolder+"/"+folders[i]+"/"+filenames[i][j] << " " << label << ". " << descriptor.size() << endl;
            }
        }

        cout << "...descriptors loaded" << endl;
    }

    int rightLabel = -1;
	speak_init(0,"en");
	speak("I am robot");
    int said = -1;
    int lastOptLabel = -1;
    int lastLabel = -1;
    int see = 0;

    VideoCapture camera(0);
    Mat frame;
    while (frame.empty()) camera >> frame;

	int tim=0, lasttim=0;
    for (;;) {

        camera >> frame;
        Mat img = frame.clone();

        std::vector<Rect> faces;
        std::vector<float> confidences;
        resnet_face_detect(frame,faces,confidences);

        int kind = 0;
		Rect2d faceRect;
        if (faces.size() > 0) {
            size_t best = 0;
            for (size_t i=1; i<faces.size(); i++)
                if (abs(faces[best].x+faces[best].width/2-img.cols/2) > abs(faces[i].x+faces[i].width/2-img.cols/2)) best = i;
            faceRect =  faces[best];
            cv::rectangle(img,faceRect,Scalar(255,0,0),2);
            kind = 1;
        }
        //cout << " " << faceRect << " " << kind;

		tim = time(NULL);
        Mat imFace, imHead;
        if (kind > 0) {

            std::vector<Point2f> landmarks = getLandmarks(landmarkDetector, frame, faceRect);
            if (landmarks.size() == 68) {
                for (size_t k=0;k<landmarks.size();k++)
                    cv::circle(img,landmarks[k],2,cv::Scalar(0,0,255),FILLED);

                Mat gray, aligned;
                cvtColor(frame, gray, COLOR_BGR2GRAY);
                alignFace(gray, aligned, landmarks, true);
                imHead = getCroppedHeadRegion(aligned, landmarks);
                imFace = getCroppedFaceRegion(aligned, landmarks);
                equalizeHist(imFace, imFace);
                cv::imshow("imFace",imFace);

                std::vector<float> descriptor = resnet_descriptor(imFace);
                float minDistance = 1e9f;
                float minId = -1;
                float limitDistance = 0.5;
                for (size_t k = 0; k < descriptors.size(); k++) {
                    float distance = norm(descriptor, descriptors[k],NORM_L2);
                    if (distance < limitDistance && distance < minDistance) {
                        minDistance = distance;
                        minId = k;
                    }
                }
                //cout << " predicted: " << minId << " distance " << minDistance;
                if (minId >= 0) {
                    int minLabel = labels[minId];
                    if (minLabel != lastLabel) {
                        if (!resnet_double_check(imHead)) minId = -1;
                    }
                }
                //cout << " confirmed: " << minId;
                if (minId >= 0 || rightLabel != -1) {
                    int minLabel = (minId >=0) ? labels[minId] : rightLabel;
                    //cout << " " << minLabel << " " << folders[minLabel];
                    putText(img,folders[minLabel],faceRect.tl()-Point2d(0,12),0,1.0,Scalar(0,0,255),2);
                    lastLabel = minLabel;
                    if (rightLabel != -1 && (minLabel != rightLabel || minId == -1)) {
                        int id = tim;
                        cv::imwrite(faceDatasetFolder+"/"+folders[rightLabel]+"/"+to_string(id)+".png",frame);
                        cv::imwrite(descriptorDatasetFolder+"/"+folders[rightLabel]+"/"+to_string(id)+".png",imFace);
                        write_resnet_descriptor(descriptorDatasetFolder+"/"+folders[rightLabel]+"/"+to_string(id)+".txt",rightLabel,descriptor);
                        descriptors.push_back(descriptor);
                        labels.push_back(rightLabel);
                    }
                    int optLabel = stabilize(minLabel,5);
                    if (optLabel == lastOptLabel) {
                        see++;
                        if (see >= 5) {
                            if (optLabel != said) {
                                if (exists(descriptorDatasetFolder+"/"+folders[optLabel]+"/voice.txt")) {
									ifstream f(descriptorDatasetFolder+"/"+folders[optLabel]+"/voice.txt");
									string voice;
									if (f >> voice) speakYouAre(voice);
									else speakYouAre(folders[optLabel]);
									f.close();
                                }
                                else speakYouAre(folders[optLabel]);
                                said = optLabel;
                            }
                        }
                        else if (see % 2 == 0) speak("e...");
                    }
                    else {
                        lastOptLabel = optLabel;
                        see=0;
                    }
                }
            }
			lasttim = tim;
        }
        else {
            //cout << " Faces not detected.";
        }

        //cout << " (" << rightLabel << ")" << endl;
        cv::imshow("Detected_shape",img);
        int key = waitKey(1);
        if (key == 27) break;
        else if (key == 's') {
            int id = tim;
            cv::imwrite(to_string(id)+".png",frame);
            if (!imFace.empty()) cv::imwrite(to_string(id)+"_.png",imFace);
        }

		std::string quest;
		if (listen(quest)) {
            string voice;
			if (listenWho(quest)) {
				said = -1;
				//cout << "who?" << endl;
			}
			else if (listenIam(quest,voice)) {
				string folder = usa(voice); //filename cannot contain any characters
				rightLabel = -1;
				for (size_t k=0; k<folders.size(); k++) {
					if (folder == folders[k]) {
						rightLabel = k;
						break;
					}
				}
				if (rightLabel == -1) {
					createpath(faceDatasetFolder+"/"+folder);
					createpath(descriptorDatasetFolder+"/"+folder);
					ofstream out(descriptorDatasetFolder+"/"+folder+"/voice.txt");
					out << voice;
					out.close();
					rightLabel = (int) folders.size();
					folders.push_back(folder);
					cout << "... " << folder << " created and associated with label " << rightLabel << endl;
				}
				cout << "... " << folder << " logged in (" << rightLabel << ")" << endl;
			}
        }
		if (tim >= lasttim+3) {
            if (rightLabel != -1) {
                cout << "... " << folders[rightLabel] << " logged out" << endl;
                rightLabel = -1;
            }
        }
        if (tim >= lasttim + 20) {
            see = 0;
            said = -1;
        }

    }
    return 0;
}


/*
 * Sample application for eos deformable face mesh fitting
 * w/ initial dlib face detection and landmark regression
 * using the hunter package manager.
 * Simple, needs error handling...
 *
 * https://github.com/patrikhuber/eos
 * https://github.com/davisking/dlib
 * https://github.com/ruslo/hunter
 */

#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing.h>
#include <dlib/opencv/cv_image.h>

#include <eos/core/Landmark.hpp>
#include <eos/core/LandmarkMapper.hpp>
#include <eos/morphablemodel/MorphableModel.hpp>
#include <eos/morphablemodel/Blendshape.hpp>
#include <eos/fitting/fitting.hpp>
#include <eos/fitting/nonlinear_camera_estimation.hpp>
#include <eos/render/detail/render_detail.hpp>
#include <eos/render/utils.hpp>

#include <opencv2/highgui.hpp>

#include <cxxopts.hpp>

#include <glm/ext.hpp>

struct Resources
{
    // dlib:
    std::string landmarks;

    // eos:
    std::string model = "share/sfm_shape_3448.bin";
    std::string mappings = "share/ibug_to_sfm.txt";
    std::string blendshapes = "share/expression_blendshapes_3448.bin";
    std::string contour = "share/model_contours.json";
    std::string edgetopology = "share/sfm_3448_edge_topology.json";
};

struct GLMTransform
{
    glm::mat4x4 modelview;
    glm::mat4x4 projection;
    glm::vec4 viewport;
};

#define EOS_OPTIMIZE_PROJECTION 1

static void draw_wireframe(cv::Mat &image, const eos::core::Mesh& mesh, const GLMTransform &transform, const cv::Scalar &colour)
{
    const auto &modelview = transform.modelview;
    const auto &projection = transform.projection;
    const auto &viewport = transform.viewport;

#if EOS_OPTIMIZE_PROJECTION
    // http://glasnost.itcarlow.ie/~powerk/GeneralGraphicsNotes/projection/viewport_transformation.html
    const float x = viewport[0] + (viewport[2]/2.f);
    const float y = viewport[1] + (viewport[3]/2.f);
    glm::mat4 S = glm::scale(glm::mat4(1.0f), {viewport[2]/2.f, viewport[3]/2.f, 0.5});
    glm::mat4 T = glm::translate(glm::mat4(1.0f), {x, y, 0.5f});
    glm::mat4 M = T * S;
    const auto portProjModelView = (M * projection * modelview);
#endif

    std::vector<cv::Point2f> points(mesh.vertices.size());
    for(int i = 0; i < mesh.vertices.size(); i++)
    {
        const auto& v = mesh.vertices[i];
#if EOS_OPTIMIZE_PROJECTION
        // glm::project() expands to: Viewport * Project * Model * View
        //
        // glm::vec4 q1, q0 = modelViewProj * p0;
        // q1 = q0 / q0.w;
        // q1 = (q1 * 0.5f) + 0.5f;
        // q1[0] = q1[0] * viewport[2] + viewport[0];
        // q1[1] = q1[1] * viewport[3] + viewport[1];
        glm::vec4 p = portProjModelView * glm::vec4(v[0], v[1], v[2], 1.0); p /= p.w;
#else
        const auto p = glm::project({ v[0], v[1], v[2] }, modelview, projection, viewport);
#endif

        points[i] = {p.x, p.y};
    }

    for (const auto& triangle : mesh.tvi)
    {
        const auto &p0 = points[triangle[0]];
        const auto &p1 = points[triangle[1]];
        const auto &p2 = points[triangle[2]];

        if (eos::render::detail::are_vertices_ccw_in_screen_space(glm::vec2(p0.x,p0.y), {p1.x,p1.y}, {p2.x, p2.y}))
        {
            cv::line(image, p0, p1, colour);
            cv::line(image, p1, p2, colour);
            cv::line(image, p2, p0, colour);
        }
    }
};

static eos::core::LandmarkCollection<cv::Vec2f> convertLandmarks(const dlib::full_object_detection &shape)
{
    eos::core::LandmarkCollection<cv::Vec2f> landmarks;
    landmarks.reserve(shape.num_parts());
    for(int i = 0; i < shape.num_parts(); i++)
    {
        landmarks.push_back( {std::to_string(i+1), cv::Vec2f(shape.part(i).x(), shape.part(i).y())} );
    }
    return landmarks;
}

int main(int argc, char **argv)
{
    const auto argumentCount = argc;

    Resources assets;
    std::string input, output;

    cxxopts::Options options("eos-demo", "Command line interface for eos 2d->3d face model fitting");

    // clang-format off
    options.add_options()

        // input/output:
        ("i,input", "Input image", cxxopts::value<std::string>(input))
        ("o,output", "Output image", cxxopts::value<std::string>(output))

        // dlib:
        ("landmarks", "Landmark regressor", cxxopts::value<std::string>(assets.landmarks))

        // eos:
        ("model", "3D dephormable model", cxxopts::value<std::string>(assets.model))
        ("mapping", "Landmark mapping", cxxopts::value<std::string>(assets.mappings))
        ("model-contour", "Model contour indices", cxxopts::value<std::string>(assets.contour))
        ("edge-topology", "Model's precomputed edge topology", cxxopts::value<std::string>(assets.edgetopology))
        ("blendshapes", "Blendshapes", cxxopts::value<std::string>(assets.blendshapes));
    // clang-format on

    options.parse(argc, argv);
    if ((argumentCount <= 1) || options.count("help"))
    {
        std::cout << options.help({ "" }) << std::endl;
        return 0;
    }

    // Instantiate dlib detector and shape regressor:
    dlib::frontal_face_detector detector = dlib::get_frontal_face_detector();
    dlib::shape_predictor shape_predictor;
    dlib::deserialize(assets.landmarks) >> shape_predictor;

    // Instantiate eos models:
    auto morphable_model = eos::morphablemodel::load_model(assets.model);
    auto landmark_mapper = eos::core::LandmarkMapper(assets.mappings);
    auto blendshapes = eos::morphablemodel::load_blendshapes(assets.blendshapes);
    auto model_contour = eos::fitting::ModelContour::load(assets.contour);
    auto ibug_contour = eos::fitting::ContourLandmarks::load(assets.mappings);
    auto edge_topology = eos::morphablemodel::load_edge_topology(assets.edgetopology);

    cv::Mat gray, image = cv::imread(input, cv::IMREAD_COLOR);
    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

    const auto boxes = detector(dlib::cv_image<uint8_t>(gray));
    for(const auto & box : boxes)
    {
        // dlib: detection + landmark pose regression
        const auto shape = shape_predictor(dlib::cv_image<uint8_t>(gray), box);
        const auto landmarks = convertLandmarks(shape);

        // eos: deformable face mesh matching
        eos::core::Mesh mesh;
        eos::fitting::RenderingParameters rendering_params;
        std::tie(mesh, rendering_params) = eos::fitting::fit_shape_and_pose(
            morphable_model,
            blendshapes,
            landmarks,
            landmark_mapper,
            image.cols,
            image.rows,
            edge_topology,
            ibug_contour,
            model_contour,
            50, boost::none, 30.0f);

        cv::Mat affine_from_ortho = eos::fitting::get_3x4_affine_camera_matrix(
           rendering_params,
           image.cols,
           image.rows);

        const auto viewport = eos::fitting::get_opencv_viewport(image.cols, image.rows);
        const auto modelView = rendering_params.get_modelview();
        const auto projection = rendering_params.get_projection();
        draw_wireframe(image, mesh, {modelView, projection, viewport}, {0,255,0});

        cv::Rect roi(box.left(), box.top(), box.width(), box.height());
        cv::rectangle(image, roi, {0,255,0}, 2, 8);
        for (const auto &p : landmarks)
        {
            const auto &q = p.coordinates;
            std::cout << q << std::endl;
            cv::circle(image, cv::Point(q[0], q[1]), 4, {0,0,255}, -1, 8);
        }

        //cv::imshow("eos", image);
        //cv::waitKey(0);
    }

    cv::imwrite(output, image);
}


#pragma once

#include "block.hpp"
#include "list.hpp"

#include <boost/multiprecision/cpp_int.hpp>

#include <cstdint>
#include <string>
#include <variant>

namespace gs2 {

class Value {
    public:
        using IntType = boost::multiprecision::cpp_int;

    private:
        std::variant<IntType, List, Block> _data;

    public:
        Value(int64_t num);
        Value(IntType num);
        Value(List list);
        Value(Block block);

        bool operator!=(const Value &rhs) const;

        bool isNumber() const;
        const IntType& getNumber() const;
        IntType& getNumber();

        bool isList() const;
        const List& getList() const;
        List& getList();

        bool isBlock() const;
        const Block& getBlock() const;
        Block& getBlock();

        std::string str(bool nested=false) const;
};

} // namespace gs2


#include <tagloc/tracking.h>
#include <bearing_measurement/GetMeasurement.h>
#include <tagloc/algorithms.h>
#include <iostream>
#include <Eigen/Dense>
#include <heartbeat/delimiters.h>
#include <lavant/WaypointNavigation.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/ros.h>
#include <sstream>
#include <std_msgs/String.h>
#include <helpers/geometry_msgs.h>
#include <map>
#include <unistd.h>
#include <fstream>

using namespace std;
bool haveboth;
bool update;
bool theyreok;
bool ignore_failures;
map<string,string> data;

ros::ServiceClient wpclient;
ros::ServiceClient bearingclient;
ros::Subscriber stateIn;
ros::Subscriber hbin;
ros::Publisher hbout;

vector<double> px;
vector<double> py;
vector<double> R;
vector<double> Z;

int FREQ;
int their_count;
double sensor_var=.29;

void publish_z(double x, double y, double bearing, double sensor_variance){
	std::stringstream ss;
	std_msgs::String omsg;
	ss<<"Xn="<<x<<",";
	ss<<"Yn="<<y<<",";
	ss<<"Zn="<<bearing<<",";
	ss<<"Rn="<<sensor_variance<<",";
	ss<<"Zc="<<Z.size();
	ROS_INFO("Publishing the %d meas",Z.size());
	omsg.data = ss.str();
	hbout.publish(omsg);
}
bool measure(int f,double &bearing){
	ROS_INFO("Taking Bearing Measurement");
	bearing_measurement::GetMeasurement::Request breq;
	bearing_measurement::GetMeasurement::Response bres;
	breq.freq = f;
	if (bearingclient.exists()){
		bearingclient.call(breq,bres);

	} else {
		ROS_WARN("Unable to measure.");
	}
	bearing = bres.bearing;
	ROS_INFO("Got: %0.3f", bres.bearing);
	return bres.valid.data;
}
void clear(){
	std_msgs::String omsg;
	data.clear();
	std::stringstream ss;
	ss.clear();
	ROS_WARN("DEFAULT VALUES");
	ROS_INFO("Clearing old measurements.");
	ss<<"Zc="<<0;
	omsg.data = ss.str();
	hbout.publish(omsg);
}
void sync_robots(int timeout_sec=-1/*wait forever*/);
void move(double x, double y);
//int onestep(Eigen::MatrixXd &t,Eigen::MatrixXd &P);

//Maintenance functions:
void setupRos(ros::NodeHandle nh);
void nwin(const std_msgs::String::ConstPtr &msg);
void mapstate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);

void mapstate(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg){
		std::stringstream ss;
		ss<<msg->pose.pose.position.x;
		data["_x"]=ss.str();
		ss.str("");
		ss<<msg->pose.pose.position.y;
		data["_y"]=ss.str();
		ss.str("");
		ss<<RSN::getYaw(msg->pose.pose.orientation);
		data["_th"]=ss.str();
		ss.str("");
}

void nwin(const std_msgs::String::ConstPtr &msg){
	//if (!update) return;

	char *state,*inner_state,*token;

	ROS_INFO("NWIN: %s",msg->data.c_str());
	std::string s = msg->data;
	int msgstart = s.find("[")+1;
	int msgend = s.find("]");
	char *cs_s = (char*)calloc(s.size()+1,sizeof(char));
	strcpy(cs_s,s.c_str());

	std::string sdata = s.substr(msgstart,msgend-msgstart);
	char *cs_data = (char*)calloc(sdata.size()+1,sizeof(char));
	strcpy(cs_data,sdata.c_str());

	//first get all meta-data from the string
	token = strtok_r(cs_s,p_delim.c_str(),&state);
	while(token!=NULL){
		std::string key;
		std::string val;
		std::stringstream ss;
		ss<<strtok_r(token,kv_delim.c_str(),&inner_state);
		ss>>key;ss.clear();
		ss<<strtok_r(NULL,kv_delim.c_str(),&inner_state);
		ss>>val;ss.clear();
		data[key]=val;
		token = strtok_r(NULL,p_delim.c_str(),&state);
	}
	//now get all msg data from the string
	token = strtok_r(cs_data,p_delim.c_str(),&state);
	while(token!=NULL){
		std::string key;
		std::string val;
		std::stringstream ss;
		ss<<strtok_r(token,kv_delim.c_str(),&inner_state);
		ss>>key;ss.clear();
		ss<<strtok_r(NULL,kv_delim.c_str(),&inner_state);
		ss>>val;ss.clear();
		data[key]=val;
		token = strtok_r(NULL,p_delim.c_str(),&state);
	}
//	map<string,string>::iterator kvi = data.begin();
//	for(;kvi!=data.end();kvi++){
//		cout<<"data["<<kvi->first<<"]="<<kvi->second<<endl;
//	}
	//theyreok = data["OK"].size()>0 && strcmp(data["OK"].c_str(),"true")==0;
	if (data["x"].length()>0 &&
			data["y"].length()>0 &&
			data["th"].length()>0 &&
			data["_x"].length()>0 &&
			data["_y"].length()>0 &&
			data["_th"].length()>0){
		haveboth=true;
	}
	if(data["Zc"].size()>0){
		their_count=atoi(data["Zc"].c_str());
	}

	free(cs_s);
	free(cs_data);
}

void stop(){
	lavant::WaypointNavigation::Response wpres;
	lavant::WaypointNavigation::Request wpreq;
	ROS_INFO("Stopping navigation, if any");
	wpreq.desiredState=2;
	wpreq.blockState=wpreq.BLOCKING;
	if (wpclient.exists()){
		wpclient.call(wpreq,wpres);
	} else {
		cerr<<"WP Service doesn't exist"<<endl;
		ROS_WARN("Skipping waypoint service");
	}
}
void move(double x, double y){
	lavant::WaypointNavigation::Response wpres;
	lavant::WaypointNavigation::Request wpreq;
	ROS_INFO("moving to:%0.3f %0.3f",x,y);
	wpreq.desiredState=0;
	wpreq.destination.x=x;
	wpreq.destination.y=y;
	wpreq.blockState=wpreq.BLOCKING;

	if (wpclient.exists()){
		wpclient.call(wpreq,wpres);
	} else {
		cerr<<"WP Service doesn't exist"<<endl;
		ROS_WARN("Skipping waypoint service");
	}
	ROS_INFO("Move Complete");
}
void sync_robots(int s){
	std_msgs::String omsg;
	data.clear();
	ROS_INFO("Waiting for joint state");
	haveboth=false;
	update = true;
	theyreok=false;

	//let them know we need their status
	omsg.data="OK=false";
	hbout.publish(omsg);

	ROS_WARN("Add duration wait");
	ROS_INFO("Waiting for both robot states");
	//ros::Time::now()+
	//wait until we got their status
	ros::Duration(5).sleep();
	while(!haveboth && ros::ok()){
		//while we wait, the local state is updated,
		//published over network, and the other robot is assumed to do the same
		ros::spinOnce();
		ros::Duration(1).sleep();
	}
	ros::Duration(5).sleep();
	//assume we have both our state and other guy's state,
	//as well as any extra data he sent along

	if (!ros::ok()){
		cerr<<"ROS down. Exiting"<<endl;
		return;
	}
	omsg.data="OK=true";
	hbout.publish(omsg);
	ros::Duration(20).sleep();

	ROS_INFO("Got joint state.");
}
void setupRos(ros::NodeHandle nh){
	wpclient=nh.serviceClient<lavant::WaypointNavigation>("/control/command");
	bearingclient=nh.serviceClient<bearing_measurement::GetMeasurement>("bearing_measurement/get_measurement");
	stateIn = nh.subscribe("/robot/state",1,mapstate);
	hbin = nh.subscribe("/network/received",1,nwin);
	hbout = nh.advertise<std_msgs::String>("/network/out",1);
}
int onestep(Eigen::MatrixXd &t,Eigen::MatrixXd &P){
	Eigen::MatrixXd r1(2,1);
	Eigen::MatrixXd r2(2,1);
	Eigen::MatrixXd rend(2,2);
	std::stringstream ss;

	cout<<t<<endl;
	cout<<P<<endl;

	clear();
	sync_robots();
	if (!ros::ok()){
		return EXIT_FAILURE;
	}

	r1<<atof(data["_x"].c_str()),atof(data["_y"].c_str());
	r2<<atof(data["x"].c_str()),atof(data["y"].c_str());

	cout<<"________________"<<endl;
	cout<<"Robot positions:"<<endl;
	cout<<"Me:"<<r1.transpose()<<endl;
	cout<<"Other guy:"<<r2.transpose()<<endl;
	cout<<"________________"<<endl;
	RSN::onestep(r1,r2,t,P,.1,3);
	rend = (r1+r2)/2;

	ROS_INFO("Got destination: %0.3f,%0.3f",r1(0),r1(1));
	cout<<r1<<endl;

	move(r1(0),r1(1));

	double bearing;
	bool zok = measure(FREQ,bearing);

	if (zok || ignore_failures){
		r1<<atof(data["_x"].c_str()),atof(data["_y"].c_str());
		px.push_back(r1(0));
		py.push_back(r1(1));
		Z.push_back(bearing);
		R.push_back(sensor_var);
		ROS_INFO("Publishing Measurement value over network");
		publish_z(r1(0),r1(1),bearing,sensor_var);
	}
	ROS_INFO("Going to rendesvous point");

	move(rend(0),rend(1));
//	clear();
	sync_robots();
	if (!ros::ok()){
		return EXIT_FAILURE;
	}

	ROS_INFO("Examining their data");

	int theirs = atoi(data["Zc"].c_str());
	if ((size_t)theirs == 0){
		ROS_INFO("They have no measurements");
	} else if ((size_t)theirs >= Z.size()){
		ROS_INFO("%Zu (mine) %d (theirs)",Z.size(),theirs);
		ROS_WARN("Not requesting other measurements, just using most current");
		px.push_back(atof(data["Xn"].c_str()));
		py.push_back(atof(data["Yn"].c_str()));
		Z.push_back(atof(data["Zn"].c_str()));
		R.push_back(atof(data["Rn"].c_str()));
	} else if ((size_t) theirs < Z.size()){
		ROS_INFO("They missed a measurement!");
	}

	ROS_INFO("Got Joint State. Updating hypothesis");
	ROS_INFO("Measurement Sequence is:");
	for (unsigned int i=0;i<Z.size();i++){
		cout<<px[i]<<","<<py[i]<<":"<<Z[i]<<endl;
	}

	double tz[2];
	double c[4];
	RSN::lineIntersection(Z.size(),px,py,Z,tz[0],tz[1]);
	RSN::BOT::IWLS_2D(Z.size(),px,py,Z,R,tz,c,true);
	t<<tz[0],tz[1];
	P<<c[0],c[1],c[2],c[3];

	cout<<t<<endl;
	cout<<P<<endl;

	return EXIT_SUCCESS;
}

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

	Eigen::MatrixXd r1(2,1);
	Eigen::MatrixXd r2(2,1);
	FREQ = 49711;

	string fname="";
	ros::init(argc,argv,"tl");
	ros::NodeHandle nh("/");

	setupRos(nh);

	char HOSTNAME[HOST_NAME_MAX];
	gethostname(HOSTNAME,HOST_NAME_MAX);
	ROS_INFO("Multi-step. I am %s",HOSTNAME);

	clear();
	Eigen::MatrixXd t(2,1);
	Eigen::MatrixXd P(2,2);
	t<<-30,-120;
	P<<900,0,0,900;

	r1<<-10,-60;
	r2<<-40,-60;
	Eigen::MatrixXd rend(2,1);
	rend<<-25,-60;
	ROS_INFO("Starting, going to -30,0");
	move(-30.0,0);

	std::stringstream ss;

	cout<<t<<endl;
	cout<<P<<endl;

	ROS_INFO("Got destination: %0.3f,%0.3f",r1(0),r1(1));
	cout<<r1<<endl;

	move(r1(0),r1(1));

	double bearing;
	bool zok = measure(FREQ,bearing);

	if (zok || ignore_failures){
		r1<<atof(data["_x"].c_str()),atof(data["_y"].c_str());
		px.push_back(r1(0));
		py.push_back(r1(1));
		Z.push_back(bearing);
		R.push_back(sensor_var);
		ROS_INFO("Publishing Measurement value over network");
		publish_z(r1(0),r1(1),bearing,sensor_var);
	}
	ROS_INFO("Going to rendesvous point");

	move(rend(0),rend(1));
	sync_robots();
	if (!ros::ok()){
		return EXIT_FAILURE;
	}

	ROS_INFO("Examining their data");

	int theirs = atoi(data["Zc"].c_str());
	if ((size_t)theirs == 0){
		ROS_INFO("They have no measurements");
	} else if ((size_t)theirs >= Z.size()){
		ROS_INFO("%Zu (mine) %d (theirs)",Z.size(),theirs);
		ROS_WARN("Not requesting other measurements, just using most current");
		px.push_back(atof(data["Xn"].c_str()));
		py.push_back(atof(data["Yn"].c_str()));
		Z.push_back(atof(data["Zn"].c_str()));
		R.push_back(atof(data["Rn"].c_str()));
	} else if ((size_t) theirs < Z.size()){
		ROS_INFO("They missed a measurement!");
	}

	ROS_INFO("Got Joint State. Updating hypothesis");
	ROS_INFO("Measurement Sequence is:");
	for (unsigned int i=0;i<Z.size();i++){
		cout<<px[i]<<","<<py[i]<<":"<<Z[i]<<endl;
	}

	double tz[2];
	double c[4];
	RSN::lineIntersection(Z.size(),px,py,Z,tz[0],tz[1]);
	RSN::BOT::IWLS_2D(Z.size(),px,py,Z,R,tz,c,true);
	Eigen::MatrixXd tt = t;
	t<<tz[0],tz[1];
	P<<c[0],c[1],c[2],c[3];

	t = (t + tt)/2.0;

	r1 = rend;
	r2 = rend;
	RSN::onestep(r1,r2,t,P,.1,3);
	rend = (r1+r2)/2;

	ROS_INFO("Got destination: %0.3f,%0.3f",r1(0),r1(1));
	cout<<r1<<endl;

	move(r1(0),r1(1));

	zok = measure(FREQ,bearing);

	if (zok || ignore_failures){
		r1<<atof(data["_x"].c_str()),atof(data["_y"].c_str());
		px.push_back(r1(0));
		py.push_back(r1(1));
		Z.push_back(bearing);
		R.push_back(sensor_var);
		ROS_INFO("Publishing Measurement value over network");
		publish_z(r1(0),r1(1),bearing,sensor_var);
	}
	ROS_INFO("Going to rendesvous point");

	move(rend(0),rend(1));
	sync_robots();
	if (!ros::ok()){
		return EXIT_FAILURE;
	}

	ROS_INFO("Examining their data");

	theirs = atoi(data["Zc"].c_str());
	if ((size_t)theirs == 0){
		ROS_INFO("They have no measurements");
	} else if ((size_t)theirs >= Z.size()){
		ROS_INFO("%Zu (mine) %d (theirs)",Z.size(),theirs);
		ROS_WARN("Not requesting other measurements, just using most current");
		px.push_back(atof(data["Xn"].c_str()));
		py.push_back(atof(data["Yn"].c_str()));
		Z.push_back(atof(data["Zn"].c_str()));
		R.push_back(atof(data["Rn"].c_str()));
	} else if ((size_t) theirs < Z.size()){
		ROS_INFO("They missed a measurement!");
	}

	ROS_INFO("Got Joint State. Updating hypothesis");
	ROS_INFO("Measurement Sequence is:");
	for (unsigned int i=0;i<Z.size();i++){
		cout<<px[i]<<","<<py[i]<<":"<<Z[i]<<endl;
	}

	RSN::lineIntersection(Z.size(),px,py,Z,tz[0],tz[1]);
	RSN::BOT::IWLS_2D(Z.size(),px,py,Z,R,tz,c,true);
	t<<tz[0],tz[1];
	P<<c[0],c[1],c[2],c[3];

	cout<<t<<endl;
	cout<<P<<endl;
	move(10,-120);
	return 0;
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Locus Robotics
 *  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 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.
 */
#include <fuse_constraints/relative_orientation_3d_stamped_constraint.h>

#include <fuse_constraints/normal_delta_orientation_3d_cost_functor.h>
#include <pluginlib/class_list_macros.h>

#include <boost/serialization/export.hpp>
#include <ceres/autodiff_cost_function.h>
#include <Eigen/Geometry>

#include <string>

namespace fuse_constraints
{

RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint(
  const std::string& source,
  const fuse_variables::Orientation3DStamped& orientation1,
  const fuse_variables::Orientation3DStamped& orientation2,
  const fuse_core::Vector4d& delta,
  const fuse_core::Matrix3d& covariance) :
    fuse_core::Constraint(source, {orientation1.uuid(), orientation2.uuid()}),  // NOLINT(whitespace/braces)
    delta_(delta),
    sqrt_information_(covariance.inverse().llt().matrixU())
{
}

RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint(
  const std::string& source,
  const fuse_variables::Orientation3DStamped& orientation1,
  const fuse_variables::Orientation3DStamped& orientation2,
  const Eigen::Quaterniond& delta,
  const fuse_core::Matrix3d& covariance) :
    RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), covariance)
{
}

RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint(
  const std::string& source,
  const fuse_variables::Orientation3DStamped& orientation1,
  const fuse_variables::Orientation3DStamped& orientation2,
  const geometry_msgs::Quaternion& delta,
  const std::array<double, 9>& covariance) :
    RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), toEigen(covariance))
{
}

fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::covariance() const
{
  return (sqrt_information_.transpose() * sqrt_information_).inverse();
}

void RelativeOrientation3DStampedConstraint::print(std::ostream& stream) const
{
  stream << type() << "\n"
         << "  source: " << source() << "\n"
         << "  uuid: " << uuid() << "\n"
         << "  orientation variable1: " << variables().at(0) << "\n"
         << "  orientation variable2: " << variables().at(1) << "\n"
         << "  delta: " << delta().transpose() << "\n"
         << "  sqrt_info: " << sqrtInformation() << "\n";

  if (loss())
  {
    stream << "  loss: ";
    loss()->print(stream);
  }
}

ceres::CostFunction* RelativeOrientation3DStampedConstraint::costFunction() const
{
  return new ceres::AutoDiffCostFunction<NormalDeltaOrientation3DCostFunctor, 3, 4, 4>(
    new NormalDeltaOrientation3DCostFunctor(sqrt_information_, delta_));
}

fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const Eigen::Quaterniond& quaternion)
{
  fuse_core::Vector4d eigen_quaternion_vector;
  eigen_quaternion_vector << quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z();
  return eigen_quaternion_vector;
}

fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const geometry_msgs::Quaternion& quaternion)
{
  fuse_core::Vector4d eigen_quaternion_vector;
  eigen_quaternion_vector << quaternion.w, quaternion.x, quaternion.y, quaternion.z;
  return eigen_quaternion_vector;
}

fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen(const std::array<double, 9>& covariance)
{
  return fuse_core::Matrix3d(covariance.data());
}

}  // namespace fuse_constraints

BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeOrientation3DStampedConstraint);
PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeOrientation3DStampedConstraint, fuse_core::Constraint);


// OpenVDB
#include <openvdb/openvdb.h>

// pybind11
#include <pybind11/eigen.h>
#include <pybind11/functional.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/stl_bind.h>

// std stuff
#include <Eigen/Core>
#include <memory>
#include <vector>

#ifdef PYOPENVDB_SUPPORT
#include "pyopenvdb.h"
#endif

#include "stl_vector_eigen.h"
#include "vdbfusion/VDBVolume.h"

PYBIND11_MAKE_OPAQUE(std::vector<Eigen::Vector3d>);
PYBIND11_MAKE_OPAQUE(std::vector<Eigen::Vector3i>);

namespace py = pybind11;
using namespace py::literals;

namespace vdbfusion {

PYBIND11_MODULE(vdbfusion_pybind, m) {
    auto vector3dvector = pybind_eigen_vector_of_vector<Eigen::Vector3d>(
        m, "_VectorEigen3d", "std::vector<Eigen::Vector3d>",
        py::py_array_to_vectors_double<Eigen::Vector3d>);

    auto vector3ivector = pybind_eigen_vector_of_vector<Eigen::Vector3i>(
        m, "_VectorEigen3i", "std::vector<Eigen::Vector3i>",
        py::py_array_to_vectors_int<Eigen::Vector3i>);
    py::class_<VDBVolume, std::shared_ptr<VDBVolume>> vdb_volume(
        m, "_VDBVolume",
        "This is the low level C++ bindings, all the methods and "
        "constructor defined within this module (starting with a ``_`` "
        "should not be used. Please reffer to the python Procesor class to "
        "check how to use the API");
    vdb_volume
        .def(py::init<float, float, bool>(), "voxel_size"_a, "sdf_trunc"_a,
             "space_carving"_a = false)
        // TODO: add support for this
        .def("_integrate",
             py::overload_cast<const std::vector<Eigen::Vector3d>&, const Eigen::Matrix4d&,
                               const std::function<float(float)>&>(&VDBVolume::Integrate),
             "points"_a, "extrinsic"_a, "weighting_function"_a)
        .def("_integrate",
             py::overload_cast<const std::vector<Eigen::Vector3d>&, const Eigen::Vector3d&,
                               const std::function<float(float)>&>(&VDBVolume::Integrate),
             "points"_a, "origin"_a, "weighting_function"_a)
        .def(
            "_integrate",
            [](VDBVolume& self, const std::vector<Eigen::Vector3d>& points,
               const Eigen::Vector3d& origin) {
                self.Integrate(points, origin, [](float /*sdf*/) { return 1.0f; });
            },
            "points"_a, "origin"_a)
        .def(
            "_integrate",
            [](VDBVolume& self, const std::vector<Eigen::Vector3d>& points,
               const Eigen::Vector3d& origin, float weight) {
                self.Integrate(points, origin, [=](float /*sdf*/) { return weight; });
            },
            "points"_a, "origin"_a, "weight"_a)
        .def(
            "_integrate",
            [](VDBVolume& self, const std::vector<Eigen::Vector3d>& points,
               const Eigen::Matrix4d& extrinsics) {
                self.Integrate(points, extrinsics, [](float /*sdf*/) { return 1.0f; });
            },
            "points"_a, "extrinsic"_a)
        .def(
            "_integrate",
            [](VDBVolume& self, const std::vector<Eigen::Vector3d>& points,
               const Eigen::Matrix4d& extrinsics, float weight) {
                self.Integrate(points, extrinsics, [=](float /*sdf*/) { return weight; });
            },
            "points"_a, "origin"_a, "weight"_a)
#ifdef PYOPENVDB_SUPPORT
        .def("_integrate",
             py::overload_cast<openvdb::FloatGrid::Ptr, const std::function<float(float)>&>(
                 &VDBVolume::Integrate),
             "grid"_a, "weighting_function"_a)
        .def(
            "_integrate",
            [](VDBVolume& self, openvdb::FloatGrid::Ptr grid) {
                self.Integrate(grid, [](float /*sdf*/) { return 1.0f; });
            },
            "grid"_a)
        .def(
            "_integrate",
            [](VDBVolume& self, openvdb::FloatGrid::Ptr grid, float weight) {
                self.Integrate(grid, [=](float /*sdf*/) { return weight; });
            },
            "grid"_a, "weight"_a)
#endif
        .def(
            "_update_tsdf",
            [](VDBVolume& self, const float& sdf, std::vector<int>& ijk,
               const std::function<float(float)>& weighting_function) {
                self.UpdateTSDF(sdf, openvdb::Coord(ijk[0], ijk[1], ijk[2]), weighting_function);
            },
            "sdf"_a, "weighting_function"_a, "ijk"_a)
        .def(
            "_update_tsdf",
            // Same story here, we overload to skip the lambda passing between Python and C++
            [](VDBVolume& self, const float& sdf, std::vector<int>& ijk) {
                self.UpdateTSDF(sdf, openvdb::Coord(ijk[0], ijk[1], ijk[2]),
                                [](float /*sdf*/) { return 1.0f; });
            },
            "sdf"_a, "ijk"_a)
        .def("_extract_triangle_mesh", &VDBVolume::ExtractTriangleMesh, "fill_holes"_a,
             "min_weight"_a)
        .def(
            "_extract_vdb_grids",
            [](const VDBVolume& self, const std::string& filename) {
                openvdb::io::File(filename).write({self.tsdf_, self.weights_});
            },
            "filename"_a)
#ifndef PYOPENVDB_SUPPORT
        .def_property_readonly_static("PYOPENVDB_SUPPORT_ENABLED", [](py::object) { return false; })
#else
        .def_property_readonly_static("PYOPENVDB_SUPPORT_ENABLED", [](py::object) { return true; })
        .def("_prune", &VDBVolume::Prune, "min_weight"_a)
        .def_readwrite("_tsdf", &VDBVolume::tsdf_)
        .def_readwrite("_weights", &VDBVolume::weights_)
#endif
        .def_readwrite("_voxel_size", &VDBVolume::voxel_size_)
        .def_readwrite("_sdf_trunc", &VDBVolume::sdf_trunc_)
        .def_readwrite("_space_carving", &VDBVolume::space_carving_);
}

}  // namespace vdbfusion


/**
 * @file   Keypoint.hpp
 * @author Paul Furgale <paul.furgale@gmail.com>
 * @date   Sat Nov 24 19:47:51 2012
 *
 * @brief  A class template for a single image point
 *
 *
 */

#ifndef ASLAM_KEYPOINT_HPP
#define ASLAM_KEYPOINT_HPP

#include <boost/serialization/access.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/boost/serialization.hpp>
#include <eigen3/Eigen/Core>
#include <aslam/KeypointBase.hpp>
#include <sm/eigen/static_assert.hpp>

namespace aslam {

template<int KEYPOINT_DIMENSION>
class Keypoint : public KeypointBase {
 public:
  SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  enum {
    KeypointDimension = KEYPOINT_DIMENSION /*!< The dimension of the keypoint associated with this geometry policy */
  };

  typedef Eigen::Matrix<double, KeypointDimension, 1> measurement_t;
  typedef Eigen::Matrix<double, KeypointDimension, KeypointDimension> inverse_covariance_t;

  /// \brief default constructor
  Keypoint();

  /// \brief the copy constructor makes a deep copy
  Keypoint(const Keypoint & rhs);

  /// \brief the assignment operator makes a deep copy
  Keypoint & operator=(const Keypoint & rhs);

  virtual ~Keypoint();

  /// \brief swap this keypoint with the rhs
  void swap(Keypoint & rhs);

  virtual size_t dimension() const {
    return KeypointDimension;
  }

  ///////////////////////////////////////////////////
  // The measurement and uncertainty
  ///////////////////////////////////////////////////

  /// \brief the image measurement
  const measurement_t & measurement() const;

  /// \brief the image measurement
  const measurement_t & y() const;

  /// \brief set the image measurement
  void setMeasurement(const measurement_t & m);

  /// \brief the octave this keypoint was detected in
  int octave() const;

  /// \brief set the octave this keypoint was detected in
  void setOctave(const int octave);

  /// \breif get/set the trace id
  void setTraceId(const size_t traceId);
  size_t getTraceId();

  /// \brief compute the mahalonibis distance between y and this keypoint
  template<typename DERIVED>
  double mahalonibisDistance(const Eigen::MatrixBase<DERIVED> & y) const;

  /// \brief get the image measurement inverse covariance
  const inverse_covariance_t & inverseMeasurementCovariance() const;

  /// \brief get the image measurement inverse covariance
  const inverse_covariance_t & invR() const;

  /// \brief set the image measurement inverse covariance
  void setInverseMeasurementCovariance(const inverse_covariance_t & invR);
  void setInverseCovariance(const inverse_covariance_t & invR) {
    setInverseMeasurementCovariance(invR);
  }

  /// \brief get the measurement
  virtual Eigen::VectorXd vsMeasurement() const;
  /// \brief set the measurement
  virtual void vsSetMeasurement(const Eigen::VectorXd & m);

  /// \brief get the inverse measurement covariance
  virtual Eigen::MatrixXd vsInverseMeasurementCovariance() const;
  /// \brief set the inverse measurement covariance.
  virtual void vsSetInverseMeasurementCovariance(const Eigen::MatrixXd & invR);

  virtual double vsMahalonibisDistance(const Eigen::VectorXd & y) const;

  ///////////////////////////////////////////////////
  // The descriptor
  ///////////////////////////////////////////////////

  /// \brief get the keypoint descriptor
  virtual const DescriptorBase * descriptor() const;

  /// \brief get the keypoint descriptor
  virtual boost::shared_ptr<DescriptorBase> descriptorPtr() const;

  /// \brief set the keypoint descriptor
  virtual void setDescriptor(const DescriptorBase & descriptor);

  /// \brief set the keypoint descriptor
  ///        the keypoint takes ownership of the pointer
  virtual void setDescriptorRawPtr(DescriptorBase * descriptor);

  /// \brief set the keypoint descriptor
  ///        the keypoint takes ownership of the pointer
  virtual void setDescriptorPtr(boost::shared_ptr<DescriptorBase> descriptor);

  ///////////////////////////////////////////////////
  // The landmark
  ///////////////////////////////////////////////////

  /// \brief get the landmark
  virtual const sm::kinematics::UncertainHomogeneousPoint & landmark() const;
  virtual const sm::kinematics::UncertainHomogeneousPoint * landmarkPtr() const;

  /// \brief set the landmark
  virtual void setLandmark(
      const sm::kinematics::UncertainHomogeneousPoint & landmark);

  /// \brief get the landmark id
  virtual const LandmarkId & landmarkId() const;

  /// \brief set the landmark id
  virtual void setLandmarkId(const LandmarkId & landmarkId);

  /// \brief is the landmark initialized
  virtual bool isLandmarkInitialized() const;

  /// \brief reset and clear the landmark.
  virtual void clearLandmark();

  ///////////////////////////////////////////////////
  // The back projection
  ///////////////////////////////////////////////////

  /// \brief set the back projection
  virtual void setBackProjection(const sm::kinematics::UncertainVector3 & v);

  /// \brief set the back projection
  virtual void setBackProjectionVector(const Eigen::Vector3d & v);

  /// \brief get the back projection pointer
  virtual const sm::kinematics::UncertainVector3 * backProjection() const;

  /// \brief is the back projection initialized?
  virtual bool isBackProjectionSet() const;

  /// \brief clear the back projection
  virtual void clearBackProjection();

  ///////////////////////////////////////////////////
  // Serialization
  ///////////////////////////////////////////////////

  enum {
    CLASS_SERIALIZATION_VERSION = 1
  };BOOST_SERIALIZATION_SPLIT_MEMBER();

  template<class Archive>
  void load(Archive & ar, const unsigned int version) {
    SM_ASSERT_LE(std::runtime_error, version,
                 (unsigned int) CLASS_SERIALIZATION_VERSION,
                 "Unsupported serialization version");

    ar >> BOOST_SERIALIZATION_NVP(_measurement);
    ar >> BOOST_SERIALIZATION_NVP(_inverseMeasurementCovariance);
    ar >> BOOST_SERIALIZATION_NVP(_landmarkId);
    ar >> BOOST_SERIALIZATION_NVP(_landmark);
    ar >> BOOST_SERIALIZATION_NVP(_descriptor);
    ar >> BOOST_SERIALIZATION_NVP(_backProjection);
    if (version > 0) {
      ar >> BOOST_SERIALIZATION_NVP(_octave);
    }
  }

  template<class Archive>
  void save(Archive & ar, const unsigned int /* version */) const {
    ar << BOOST_SERIALIZATION_NVP(_measurement);
    ar << BOOST_SERIALIZATION_NVP(_inverseMeasurementCovariance);
    ar << BOOST_SERIALIZATION_NVP(_landmarkId);
    ar << BOOST_SERIALIZATION_NVP(_landmark);
    ar << BOOST_SERIALIZATION_NVP(_descriptor);
    ar << BOOST_SERIALIZATION_NVP(_backProjection);
    ar << BOOST_SERIALIZATION_NVP(_octave);
  }

  /// \brief Support for unit testing. Is this keypoint binary equal to the other?
  bool isBinaryEqual(const Keypoint<KEYPOINT_DIMENSION> & rhs) const;

  /// \brief Support for unit testing. Set this to a random keypoint.
  void setRandom();
 private:

  /// \brief Frees all memory owned by the keypoint.
  void freeAll();

  /// \brief The keypoint measurement
  measurement_t _measurement;

  /// \brief The inverse covariance measurement
  inverse_covariance_t _inverseMeasurementCovariance;

  /// \brief the landmark id.
  LandmarkId _landmarkId;

  /// \brief the landmark.
  sm::kinematics::UncertainHomogeneousPoint * _landmark;

  /// \brief the keypoint descriptor
  boost::shared_ptr<DescriptorBase> _descriptor;

  /// \brief the back projection, a ray in space originating from the view origin.
  sm::kinematics::UncertainVector3 * _backProjection;

  /// \brief the image octave this keypoint was observed in
  unsigned int _octave;

  /// \brief unique ground truth ID for a keypoint trace (i.e. to easily verifiy 2D2D matching with ground truth simulation)
  size_t _traceId;
};

}  // namespace aslam

#include <aslam/implementation/Keypoint.hpp>

// this is a fancy templated version of
// BOOST_CLASS_VERSION() for serialization versioning
SM_BOOST_CLASS_VERSION_I1 (aslam::Keypoint);

#endif /* ASLAM_KEYPOINT_HPP */


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  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_ARCH_X86_AVX_SIMD_FUNCTION_DEINTERLEAVE_SECOND_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_DEINTERLEAVE_SECOND_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/slice.hpp>
#include <boost/simd/function/combine.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( deinterleave_second_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<A0>;
      auto b0 = bitwise_cast<f_t>(a0);
      auto b1 = bitwise_cast<f_t>(a1);

      return  bitwise_cast<A0>(f_t( _mm256_unpackhi_pd( _mm256_permute2f128_pd(b0,b1,0x20)
                                                      , _mm256_permute2f128_pd(b0,b1,0x31)
                                                      )
                                  )
                              );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( deinterleave_second_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<A0>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const b1 = bitwise_cast<f_t>(a1);

      auto const  x = _mm256_permute2f128_ps(b0,b1,0x20)
                , y = _mm256_permute2f128_ps(b0,b1,0x31);

      return  bitwise_cast<A0>(f_t( _mm256_unpackhi_ps( _mm256_unpacklo_ps(x,y)
                                                  , _mm256_unpackhi_ps(x,y)
                                                  )
                                  )
                              );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( deinterleave_second_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      auto s0 = slice(a0);
      auto s1 = slice(a1);
      return combine( deinterleave_second(std::get<0>(s0),std::get<1>(s0))
                    , deinterleave_second(std::get<0>(s1),std::get<1>(s1))
                    );
    }
  };
} } }

#endif


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

// 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)

#ifndef BOOST_GEOMETRY_INDEX_TEST_MARGIN_HPP
#define BOOST_GEOMETRY_INDEX_TEST_MARGIN_HPP

#include <geometry_index_test_common.hpp>

#include <boost/geometry/index/detail/algorithms/margin.hpp>

//#include <boost/geometry/io/wkt/read.hpp>

template <typename Geometry>
void test_margin(Geometry const& geometry,
            typename bgi::detail::default_margin_result<Geometry>::type expected_value)
{
    typename bgi::detail::default_margin_result<Geometry>::type value = bgi::detail::comparable_margin(geometry);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << " "
        << typeid(typename bgi::detail::default_margin_result<Geometry>::type).name()
        << " "
        << "content : " << value
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(value, expected_value, 0.0001);
}

template <typename Geometry>
void test_geometry(std::string const& wkt,
            typename bgi::detail::default_margin_result<Geometry>::type expected_value)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    test_margin(geometry, expected_value);
}

#endif


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int n; cin >> n;
    if (n % 6 == 0) cout << "yES" << endl;
    else cout << "nO" << endl;
    if (n % 11 == 0) cout << "yES" << endl;
    else cout << "nO" << endl;
}