/* glz_classifier.cc
   Jeremy Barnes, 6 August 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.
   $Source$

   Implementation of the GLZ classifier.
*/

#include "glz_classifier.h"
#include "classifier_persist_impl.h"
#include "null_feature_space.h"
#include "dense_features.h"
#include "jml/algebra/irls.h"
#include <boost/timer.hpp>
#include "training_index.h"
#include "config_impl.h"
#include <limits>
#include "jml/utils/vector_utils.h"
#include "jml/compiler/compiler.h"

using namespace std;
using namespace ML::DB;

namespace ML {

/*****************************************************************************/
/* GLZ_CLASSIFIER                                                            */
/*****************************************************************************/

GLZ_Classifier::GLZ_Classifier()
    : add_bias(true), link(LOGIT), optimized_(false)
{
}

GLZ_Classifier::
GLZ_Classifier(const std::shared_ptr<const Feature_Space> & fs,
               const Feature & predicted)
    : Classifier_Impl(fs, predicted), add_bias(true), link(LOGIT),
      optimized_(false)
{
}

GLZ_Classifier::
GLZ_Classifier(DB::Store_Reader & store,
               const std::shared_ptr<const Feature_Space> & fs)
{
    reconstitute(store, fs);
}

GLZ_Classifier::
GLZ_Classifier(DB::Store_Reader & store)
{
    reconstitute(store);
    set_feature_space(std::shared_ptr<const Feature_Space>
                      (new Null_Feature_Space()));
}

GLZ_Classifier::~GLZ_Classifier()
{
}

distribution<float>
GLZ_Classifier::
extract(const Feature_Set & feature_set) const
{
    distribution<float> result(features.size());

    float NaN = std::numeric_limits<float>::quiet_NaN();

    Feature_Set::const_iterator
        prev_last = feature_set.begin(),
        fend = feature_set.end();

    for (unsigned i = 0;  i < features.size();  ++i) {
        const Feature & to_find = features[i].feature;

        Feature_Set::const_iterator first, last;

        // Optimization: assume that the features are there in the same order
        // as we wanted to access them, so that we can simply step through
        // rather than having to search for them each time.
        if (prev_last != fend && prev_last.feature() == to_find) {
            last = first = prev_last;
            do {
                ++last;
            } while (last != fend && last.feature() == to_find);
        }
        else {
            boost::tie(first, last) = feature_set.find(features[i].feature);
        }

        prev_last = last;

        switch (features[i].type) {
        case Feature_Spec::VALUE_IF_PRESENT:
            if (first == last || isnan((*first).second)) {
                result[i] = NaN;
                break;
            }
            // fall through

        case Feature_Spec::VALUE:
            if (last - first != 1)
                throw Exception("GLZ_Classifier::decode() feature "
                                + feature_space_->print(features[i].feature)
                                + " occurred " + ostream_format(last - first)
                                + " times; exactly 1 required");
            if (isnan((*first).second))
                throw Exception("GLZ_Classifier::decode() feature "
                                + feature_space_->print(features[i].feature)
                                + " was missing");

            result[i] = (*first).second;
            break;

        case Feature_Spec::PRESENCE:
            if (first == last || isnan((*first).second)) {
                result[i] = NaN;
                break;
            }
            result[i] = (*first).second;
            break;

        default:
            throw Exception("GLZ_Classifier::decode(): invalid type");
        }
    }

    return result;
}

distribution<float>
GLZ_Classifier::
decode(const Feature_Set & feature_set) const
{
    distribution<float> result = extract(feature_set);
    for (unsigned i = 0;  i < result.size();  ++i) {
        result[i] = decode_value(result[i], features[i]);
    }

    return result;
}

Label_Dist
GLZ_Classifier::predict(const Feature_Set & features,
                        PredictionContext * context) const
{
    distribution<float> features_c = extract(features);
    Label_Dist result = predict(features_c);
    return result;
}

Label_Dist
GLZ_Classifier::predict(const distribution<float> & features_c,
                        PredictionContext * context) const
{
    if (features_c.size() != features.size())
        throw Exception("wrong number of features");
    return do_predict_impl(&features_c[0], 0);
}

bool
GLZ_Classifier::
optimization_supported() const
{
    return true;
}

bool
GLZ_Classifier::
predict_is_optimized() const
{
    return optimized_;
}

bool
GLZ_Classifier::
optimize_impl(Optimization_Info & info)
{
    feature_indexes.clear();

    // Fill in the feature order
    for (unsigned i = 0;  i < features.size();  ++i) {
        map<Feature, int>::const_iterator it
            = info.feature_to_optimized_index.find(features[i].feature);
        if (it == info.feature_to_optimized_index.end())
            throw Exception("GLZ_Classifier::optimize(): feature not found");
        feature_indexes.push_back(it->second);
    }

    return optimized_ = true;
}

Label_Dist
GLZ_Classifier::
optimized_predict_impl(const float * features_c,
                       const Optimization_Info & info,
                       PredictionContext * context) const
{
    return do_predict_impl(features_c, &feature_indexes[0]);
}

void
GLZ_Classifier::
optimized_predict_impl(const float * features_c,
                       const Optimization_Info & info,
                       double * accum_out,
                       double weight,
                       PredictionContext * context) const
{
    do_predict_impl(features_c, &feature_indexes[0], accum_out, weight);
}

float
GLZ_Classifier::
optimized_predict_impl(int label,
                       const float * features_c,
                       const Optimization_Info & info,
                       PredictionContext * context) const
{
    return do_predict_impl(label, features_c, &feature_indexes[0]);
}

float
GLZ_Classifier::
decode_value(float feat_val, const Feature_Spec & spec) const
{
    if (JML_UNLIKELY(isnan(feat_val))) {
        switch (spec.type) {
        case Feature_Spec::VALUE:
            feat_val = 0.0;
#if 0
            throw Exception("GLZ_Classifier: feature "
                            + feature_space()->print(spec.feature)
                            + " is missing");
#endif
        case Feature_Spec::VALUE_IF_PRESENT:
        case Feature_Spec::PRESENCE:
            feat_val = 0.0;
            break;
        default:
            throw Exception("invalid feature spec type");
        }
    }
    else if (JML_UNLIKELY(spec.type == Feature_Spec::PRESENCE))
        feat_val = 1.0;
    else if (JML_UNLIKELY(!isfinite(feat_val)))
        throw Exception("GLZ_Classifier: feature "
                        + feature_space()->print(spec.feature)
                        + " is not finite");

    return feat_val;
}

double
GLZ_Classifier::
do_accum(const float * features_c,
         const int * indexes,
         int label) const
{
    double accum = 0.0;

    for (unsigned j = 0;  j < features.size();  ++j) {
        int idx = (indexes ? indexes[j] : j);
        float feat_val = decode_value(features_c[idx], features[j]);
        accum +=  feat_val * weights[label][j];
    }

    if (add_bias) accum += weights[label][features.size()];

    //cerr << "do accum " << label << " = " << accum << endl;

    return apply_link_inverse(accum, link);
}

Label_Dist
GLZ_Classifier::
do_predict_impl(const float * features_c,
                const int * indexes) const
{
    Label_Dist result(label_count());

    for (unsigned i = 0;  i < result.size();  ++i)
        result[i] = do_accum(features_c, indexes, i);

    return result;
}

void
GLZ_Classifier::
do_predict_impl(const float * features_c,
                const int * indexes,
                double * accum,
                double weight) const
{
    int nl = label_count();

    for (unsigned i = 0;  i < nl;  ++i)
        accum[i] += weight * do_accum(features_c, indexes, i);
}

float
GLZ_Classifier::
do_predict_impl(int label,
                const float * features_c,
                const int * indexes) const
{
    return do_accum(features_c, indexes, label);
}

std::vector<ML::Feature>
GLZ_Classifier::
all_features() const
{
    vector<ML::Feature> result;
    for (unsigned i = 0;  i < features.size();  ++i)
        if (i == 0 || features[i].feature != result.back())
            result.push_back(features[i].feature);
    return result;
}

Output_Encoding
GLZ_Classifier::
output_encoding() const
{
    if (label_count() == 1) return OE_PM_INF;
    else return OE_PROB;
}

Explanation
GLZ_Classifier::
explain(const Feature_Set & feature_set,
        int label,
        double weight,
        PredictionContext * context) const
{
    Explanation result(feature_space(), weight);

    for (unsigned j = 0;  j < features.size();  ++j) {
        float feat_val = decode_value(feature_set[features[j].feature],
                                      features[j]);
        result.feature_weights[features[j].feature]
            += weight * weights[label][j] * feat_val;
    }

    if (add_bias) result.bias += weight * weights[label][features.size()];

    return result;
}

std::string GLZ_Classifier::print() const
{
    return "GLZ_Classifier: link " + ML::print(link);
}

std::string GLZ_Classifier::summary() const
{
    return "GLZ_Classifier: link " + ML::print(link);
}

namespace {

static const std::string GLZ_CLASSIFIER_MAGIC = "GLZ_CLASSIFIER";
static const compact_size_t GLZ_CLASSIFIER_VERSION = 3;

} // file scope

std::string GLZ_Classifier::class_id() const
{
    return GLZ_CLASSIFIER_MAGIC;
}

void GLZ_Classifier::serialize(DB::Store_Writer & store) const
{
    store << GLZ_CLASSIFIER_MAGIC << GLZ_CLASSIFIER_VERSION;
    feature_space()->serialize(store, predicted_);
    store << (int)add_bias << weights << link;
    store << compact_size_t(features.size());
    for (unsigned i = 0;  i < features.size();  ++i) {
        feature_space_->serialize(store, features[i].feature);
        store << (char)features[i].type;
    }
    store << compact_size_t(0x12345);
}

void GLZ_Classifier::
reconstitute(DB::Store_Reader & store)
{
    string magic;
    compact_size_t version;
    store >> magic >> version;
    if (magic != GLZ_CLASSIFIER_MAGIC)
        throw Exception("Attempt to reconstitute \"" + magic
                                + "\" with boosted stumps reconstitutor");
    if (version != GLZ_CLASSIFIER_VERSION)
        throw Exception(format("Attemp to reconstitute GLZ classifier "
                               "version %zd, only %zd supported",
                               version.size_,
                               GLZ_CLASSIFIER_VERSION.size_));

    if (!feature_space_)
        throw Exception("GLZ_Classifier::reconstitute(): feature space not "
                        "initialised");

    predicted_ = MISSING_FEATURE;
    if (version >= 2) // added in version 2
        feature_space()->reconstitute(store, predicted_);

    int add_bias_;  store >> add_bias_;  add_bias = add_bias_;
    store >> weights >> link;

    compact_size_t nf(store);
    features.resize(nf);

    if (version < 3) {
        for (unsigned i = 0;  i < features.size();  ++i)
            feature_space_->reconstitute(store, features[i].feature);
    }
    else {
        for (unsigned i = 0;  i < features.size();  ++i) {
            feature_space_->reconstitute(store, features[i].feature);
            char c;  store >> c;
            features[i].type = Feature_Spec::Type(c);
        }
    }
    compact_size_t guard(store);
    if (guard != 0x12345)
        throw Exception("GLZ_Classifier::reconstitute(): bad guard value");

    init(feature_space_, predicted_, weights.size());

    optimized_ = false;
}

void GLZ_Classifier::
reconstitute(DB::Store_Reader & store,
             const std::shared_ptr<const Feature_Space> & fs)
{
    feature_space_ = fs;
    reconstitute(store);
}

GLZ_Classifier * GLZ_Classifier::make_copy() const
{
    return new GLZ_Classifier(*this);
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Impl, GLZ_Classifier>
    GLZC_REGISTER(GLZ_CLASSIFIER_MAGIC);

} // file scope

} // namespace ML


/*
http://tapenade.inria.fr:8080/tapenade/paste.jsp
  Paste shelxl_wght_ls.f
    top: kwt
    dependent: t
    independent: ic
    Reverse Mode
  unzip TapenadeResults.zip default_b-all.f
  scitbx.apply_tapenade_hints default_b-all.f > kwt_b.f
  diff -u kwt_b.f kwt_b_edited.f > kwt_b_patch
  cat shelxl_wght_ls.f kwt_b_edited.f | grep -v '^C'
  Paste output
    top: kwt_b
    dependent: t icb
    independent: ic
    Tangent Multidirectional Mode
  unzip TapenadeResults.zip default_dv-all.f
  scitbx.apply_tapenade_hints default_dv-all.f > kwt_b_dv.f
  fable.cout shelxl_wght_ls.f kwt_b_dv.f --inline-all --no-fem-do-safe --namespace=cctbx::xray::targets > shelxl_wght_ls.hpp
*/

#include <boost/python.hpp>
#include <cctbx/xray/targets/shelxl_wght_ls.hpp>
#include <cctbx/import_scitbx_af.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/accessors/mat_grid.h>

namespace cctbx { namespace xray { namespace boost_python {

boost::python::tuple
kwt_b_dv_wrapper(
  af::const_ref<double> const& f_obs,
  af::const_ref<double> const& i_obs,
  af::const_ref<double> const& i_sig,
  af::const_ref<double> const& ic,
  double wa,
  double wb)
{
  TBXX_ASSERT(i_obs.size() == f_obs.size());
  TBXX_ASSERT(i_sig.size() == f_obs.size());
  TBXX_ASSERT(ic.size() == f_obs.size());
  std::size_t nh = static_cast<int>(f_obs.size());
  double k = 0;
  af::shared<double> w(nh);
  double t = 0;
  double tb = 1;
  af::shared<double> icb(nh);
  af::shared<double> icbd(nh);
  targets::kwt_b_dv(
    k,
    w.front(),
    t,
    tb,
    nh,
    f_obs.front(),
    i_obs.front(),
    i_sig.front(),
    ic.front(),
    icb.front(),
    icbd.front(),
    wa,
    wb);
  return boost::python::make_tuple(k, w, t, icb, icbd);
}

void
wrap_targets_shelxl_wght_ls()
{
  using namespace boost::python;
  def("targets_shelxl_wght_ls_kwt_b_dv", kwt_b_dv_wrapper, (
    arg("f_obs"), arg("i_obs"), arg("i_sig"),
    arg("ic"),
    arg("wa"), arg("wb")));
}

}}}


#include "y2016/actors/vision_align_actor.h"

#include <chrono>
#include <functional>
#include <numeric>

#include <Eigen/Dense>

#include "aos/commonmath.h"
#include "aos/logging/logging.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
#include "aos/util/trapezoid_profile.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
#include "y2016/vision/vision_generated.h"

namespace y2016 {
namespace actors {

VisionAlignActor::VisionAlignActor(::aos::EventLoop *event_loop)
    : aos::common::actions::ActorBase<vision_align_action::Goal>(
          event_loop, "/vision_align_action"),
      vision_status_fetcher_(
          event_loop->MakeFetcher<::y2016::vision::VisionStatus>("/vision")),
      drivetrain_goal_sender_(
          event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
              "/drivetrain")) {
  event_loop->SetRuntimeRealtimePriority(29);
}

bool VisionAlignActor::RunAction(
    const vision_align_action::VisionAlignActionParams * /*params*/) {
  const double robot_radius =
      control_loops::drivetrain::GetDrivetrainConfig().robot_radius;
  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                      event_loop()->monotonic_now(),
                                      ActorBase::kLoopOffset);
  while (true) {
    const int iterations = phased_loop.SleepUntilNext();
    if (iterations != 1) {
      AOS_LOG(WARNING, "vision align actor skipped %d iterations\n",
              iterations - 1);
    }

    if (ShouldCancel()) {
      return true;
    }
    if (!vision_status_fetcher_.Fetch()) {
      continue;
    }
    if (!vision_status_fetcher_->left_image_valid() ||
        !vision_status_fetcher_->right_image_valid()) {
      continue;
    }

    const double side_distance_change =
        vision_status_fetcher_->horizontal_angle() * robot_radius;
    if (!::std::isfinite(side_distance_change)) {
      continue;
    }

    const double left_current =
        vision_status_fetcher_->drivetrain_left_position();
    const double right_current =
        vision_status_fetcher_->drivetrain_right_position();

    auto builder = drivetrain_goal_sender_.MakeBuilder();

    frc971::control_loops::drivetrain::Goal::Builder goal_builder =
        builder.MakeBuilder<frc971::control_loops::drivetrain::Goal>();
    goal_builder.add_wheel(0.0);
    goal_builder.add_throttle(0.0);
    goal_builder.add_highgear(false);
    goal_builder.add_quickturn(false);
    goal_builder.add_controller_type(
        frc971::control_loops::drivetrain::ControllerType::MOTION_PROFILE);
    goal_builder.add_left_goal(left_current + side_distance_change);
    goal_builder.add_right_goal(right_current - side_distance_change);

    if (builder.Send(goal_builder.Finish()) !=
        aos::RawSender::Error::kOk) {
      AOS_LOG(WARNING, "sending drivetrain goal failed\n");
    }
  }

  AOS_LOG(INFO, "Done moving\n");
  return true;
}

}  // namespace actors
}  // namespace y2016


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_ARCH_COMMON_FUNCTION_SIMD_LOG2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SIMD_LOG2_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/sdk/hierarchy/simd.hpp>
#include <boost/simd/arch/common/detail/simd/logarithm.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/ilog2.hpp>
#include <boost/simd/sdk/is_not_scalar.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0,typename X)
                          , bd::cpu_
                          , bd::pack_< bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return detail::logarithm<A0,is_not_scalar_t<A0>>::log2(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0,typename X)
                          , bd::cpu_
                          , bd::pack_< bd::arithmetic_<A0>, X >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return bs::ilog2(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0, typename A1,typename X)
                          , bd::cpu_
                          , bd::pack_< bd::floating_<A0>, X >
                          , bd::pack_< bd::integer_<A1>, X >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1& a1) const BOOST_NOEXCEPT
    {
      return frexp(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0, typename A1, typename A2)
                          , bd::cpu_
                          , bd::pack_< bd::floating_<A0> >
                          , bd::pack_< bd::floating_<A1> >
                          , bd::pack_< bd::integer_<A2> >
                          )
  {
    BOOST_FORCEINLINE void operator() ( A0 const& a0, A1& a1, A2& a2) const
    {
      return frexp(a0, a1, a2);
    }
  };
} } }

#endif


#include "PreProcessing.h"

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

#include <iostream>

#include <cmath>
#include <cv_bridge/cv_bridge.h>

namespace preprocessing
{

PreProcessing::PreProcessing(const CameraInfo& cameraInfo, const QuadState& state)
    : cameraInfo_(cameraInfo), state_(state)
{
    cv::Mat distortedCamera = (cv::Mat_<float>(3,3) << 422.918640,    0.000000,    350.119451,
            0.000000,  423.121112,    236.380265,
            0.000000,    0.000000,      1.000000);

    cv::Mat cameraDistortion = (cv::Mat_<float>(1,5) << -0.321590, 0.089597, 0.001090, -0.000489, 0.000000);

    cv::Mat undistortedCamera = cv::getOptimalNewCameraMatrix(distortedCamera, cameraDistortion, cv::Size(640, 480), 0);

    cv::initUndistortRectifyMap(distortedCamera, cameraDistortion, cv::Mat(), undistortedCamera,
                                cv::Size(640, 480), CV_32FC1, distortionMap1_, distortionMap2_);
}

void PreProcessing::preProcessImage(cv::Mat& raw, cv::Mat& preProcessed, cv::Mat& perspectiveTransform)
{
    /*
    cv::Mat typeConverted;
    if (raw.type() != CV_8UC1) {
        cv::cvtColor(raw, typeConverted, CV_BGR2GRAY);
    }
    else
    {
        raw.copyTo(typeConverted);
    }
    */

    cv::Mat undistorted = raw;
    if (!undistortType_) {
        cv::remap(raw, undistorted, distortionMap1_, distortionMap2_, CV_INTER_LINEAR);
    }

    cv::Mat perspective;
    removePerspective(undistorted, perspective, perspectiveTransform);

    cv::Mat blured;
    cv::GaussianBlur(perspective, blured, cv::Size(7,7), 8, 8);

    preProcessed = blured;
}

void PreProcessing::removePerspective(cv::Mat& input, cv::Mat& rectified, cv::Mat& toPerspective)
{
    double roll, pitch, yaw = 0.0;

    tf::Quaternion q = tf::Quaternion::getIdentity();
    q.setRPY(CV_PI, 0.0, 0.0);
    tf::Transform z2x(q);

    tf::Transform origin2fcu = tf::Transform(state_.getOrigin2Fcu().getRotation());
    tf::Transform fcu2camera = tf::Transform(state_.getFcu2Camera().getRotation());

    tf::Transform debug = origin2fcu * fcu2camera * z2x;

    tf::Matrix3x3 n(debug.getRotation());
    n.getRPY(roll, pitch, yaw);

    Eigen::Matrix3f r = (Eigen::AngleAxisf(roll,   Eigen::Vector3f::UnitX()) *
                         Eigen::AngleAxisf(-pitch,  Eigen::Vector3f::UnitY()) *
                         Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitZ())).toRotationMatrix();

    Eigen::Matrix4f R = Eigen::Matrix4f::Zero();
    R(3, 3) = 1;
    for (int i = 0; i < 3; ++i)
    {
        for (int j = 0; j < 3; j++)
        {
            R(i, j) = r(i, j);
        }
    }

    double height = input.size().height;
    double width = input.size().width;
    double f = cameraInfo_.focalLength;
    double HFOV = cameraInfo_.hfov;
    double VFOV = cameraInfo_.vfov;

    Eigen::Vector4f src[4] { Eigen::Vector4f( 1.0,  1.0, 0.0, 1.0),
                             Eigen::Vector4f(-1.0,  1.0, 0.0, 1.0),
                             Eigen::Vector4f(-1.0, -1.0, 0.0, 1.0),
                             Eigen::Vector4f( 1.0, -1.0, 0.0, 1.0) };

    Eigen::Vector4f dst[4] { Eigen::Vector4f( 1.0,  1.0, 0.0, 1.0),
                             Eigen::Vector4f(-1.0,  1.0, 0.0, 1.0),
                             Eigen::Vector4f(-1.0, -1.0, 0.0, 1.0),
                             Eigen::Vector4f( 1.0, -1.0, 0.0, 1.0) };

    Eigen::Matrix4f P = getPerspectiveProjectionTransform(f, width, height);
    Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
    T(2, 3) = -1.0;

    Eigen::Matrix4f perspectiveTransform = P * T * R;
    Eigen::Matrix4f orthoTransform =  P * T;

    for (int i = 0; i < 4; ++i)
    {
        dst[i] = orthoTransform * dst[i];
        dst[i] /= dst[i][3];

        src[i] = perspectiveTransform * src[i];
        src[i] /= src[i][3];
    }

    cv::Point2f tSrc[4], tDst[4];
    for (int i = 0; i < 4; ++i)
    {
        tSrc[i] = cv::Point2f(src[i].x() * width / 2.0 + width / 2.0, src[i].y() * height / 2.0 + height / 2.0);
        tDst[i] = cv::Point2f(dst[i].x() * width / 2.0 + width / 2.0, dst[i].y() * height / 2.0 + height / 2.0);
    }

    cv::warpPerspective(input, rectified, cv::getPerspectiveTransform(tSrc, tDst), input.size());
    toPerspective = cv::getPerspectiveTransform(tDst, tSrc);
}

Eigen::Matrix4f PreProcessing::getPerspectiveProjectionTransform(double focalLength, double width, double height) const
{
    Eigen::Matrix4f m = Eigen::Matrix4f::Zero();
    m(0, 0) = 2 * focalLength / width;
    m(1, 1) = 2 * focalLength / height;
    m(3, 2) = -1;

    return m;
}

}


//尝试显示两个图片
#include <pangolin/pangolin.h>
#include <vector>
#include <iostream>
#include <cmath>
#include <string>
#include <thread>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>

#define PI (3.1415926535897932346f)

using namespace std;
using namespace cv;

static const std::string window_name = "Two images test";

typedef struct __Pose
{
    __Pose(const vector<GLfloat> _Twc,const bool _keyFrame):
        Twc(_Twc),keyFrame(_keyFrame)
        {;}

    bool keyFrame;
    vector<GLfloat> Twc;
}Pose;

void drawFrame(const float w=2.0f);
double deg2rad(const double deg);
Eigen::Matrix3d degEuler2matrix(double pitch,double roll,double yaw);
vector<GLfloat> eigen2glfloat(Eigen::Isometry3d T);
void drawAllFrames(vector<Pose> frames);

//线程配置相关
void run(void);
void setup(void);

//图像显示相关
void setImageData(unsigned char * imageArray, Mat img){

  for(int i=0;i<img.rows;i++) {
      for(int j=0;j<img.cols;j++)
      {

          //Vec3b rgb=img.at<Vec3b>(j,i);
          /*
          imageArray[i*3*img.cols+j]=rgb[0];
          imageArray[i*3*img.cols+j+1]=rgb[1];
          imageArray[i*3*img.cols+j+2]=rgb[2];
          */

        imageArray[i*3*img.cols+j*3]=img.at<unsigned char>(img.rows-1-i,3*j+2);
        imageArray[i*3*img.cols+j*3+1]=img.at<unsigned char>(img.rows-1-i,3*j+1);
        imageArray[i*3*img.cols+j*3+2]=img.at<unsigned char>(img.rows-1-i,3*j);;
      }
  }
}

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

    setup();

    thread render_loop;
    render_loop=thread(run);
    render_loop.join();

    return 0;
}

void drawFrame(const float w)
{
    const float h=w*0.75;
    const float z=w*0.6;

    glLineWidth(2);

    glBegin(GL_LINES);

    glVertex3f(0,0,0);
    glVertex3f(w,h,z);
    glVertex3f(0,0,0);
    glVertex3f(w,-h,z);
    glVertex3f(0,0,0);
    glVertex3f(-w,-h,z);
    glVertex3f(0,0,0);
    glVertex3f(-w,h,z);
    glVertex3f(w,h,z);
    glVertex3f(w,-h,z);
    glVertex3f(-w,h,z);
    glVertex3f(-w,-h,z);
    glVertex3f(-w,h,z);
    glVertex3f(w,h,z);
    glVertex3f(-w,-h,z);
    glVertex3f(w,-h,z);

    glEnd();
}

double deg2rad(const double deg)
{
    return deg/180.0f*PI;
}

Eigen::Matrix3d degEuler2matrix(double pitch,double roll,double yaw)
{
    Eigen::Vector3d rotation_vector(
            deg2rad((double)yaw),
            deg2rad((double)pitch),
            deg2rad((double)roll));
    Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity();
    rotation_matrix=Eigen::AngleAxisd(rotation_vector[0],Eigen::Vector3d::UnitZ())
        *Eigen::AngleAxisd(rotation_vector[1],Eigen::Vector3d::UnitY())
        *Eigen::AngleAxisd(rotation_vector[2],Eigen::Vector3d::UnitX());

    return rotation_matrix;

}

vector<GLfloat> eigen2glfloat(Eigen::Isometry3d T)
{
    //注意是列优先
    vector<GLfloat> res;
    for(int j=0;j<4;j++)
    {
        for(int i=0;i<4;i++)
        {
            res.push_back(T(i,j));
        }
    }
    return res;
}

void drawAllFrames(vector<Pose> frames)
{
    size_t n=frames.size();

    for(int i=0;i<n;i++)
    {
        if(frames[i].keyFrame)
        {
            glColor3f(1.0f,0.0f,0.0f);
        }
        else
        {
            glColor3f(0.0f,1.0f,0.0f);
        }

        glPushMatrix();
        glMultMatrixf(frames[i].Twc.data());
        drawFrame();
        glPopMatrix();

    }
}

//====================== 线程相关 ======================
void setup(void)
{
     //========================= 窗口 ========================
    pangolin::CreateWindowAndBind(
        window_name,     //窗口标题
        640,        //窗口尺寸
        480);       //窗口尺寸
    glEnable(GL_DEPTH_TEST);

    // unset the current context from the main thread
    pangolin::GetBoundWindow()->RemoveCurrent();
}

void run(void)
{

    // fetch the context and bind it to this thread

    Mat img=imread("../img/pic.jpg");
    Mat img2=imread("../img/test_pic.jpg");
    if(img.empty())
    {
        cout<<"Image is empty. Terminated."<<endl;
        return ;
    }
    else
    {
        cout<<"Image loaded complete."<<endl;
    }

    pangolin::BindToContext(window_name);

    vector<Pose> frames;

    //========================== 3D 交互器 =====================
    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(
            640,480,            //相机图像的长和宽
            420,420,320,240,    //相机的内参,fu fv u0 v0
            0.2,500),           //相机所能够看到的最浅和最深的像素
        pangolin::ModelViewLookAt(
            -20,-20,-20,            //相机光心位置,NOTICE z轴不要设置为0
            0,0,0,              //相机要看的位置
            pangolin::AxisY)    //和观察的方向有关
    );

    //======================== 调节面板 ==============================
     const int UI_WIDTH=180;
     pangolin::View& d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -640.0f/480.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

    pangolin::CreatePanel("ui")
      .SetBounds(
          0.0,
          1.0,
          0.0,
          pangolin::Attach::Pix(UI_WIDTH));

    //接下来要开始准备添加控制选项了
    pangolin::Var<int> varTop("ui.Top",0,0,480);
    pangolin::Var<int> varBottom("ui.Bottom",480,0,480);
    pangolin::Var<int> varLeft("ui.Left",0,0,640);
    pangolin::Var<int> varRight("ui.Right",640,0,640);

    pangolin::Var<bool> checkRatio("ui.use_ratio",false,true);

    pangolin::Var<double> vardTop("ui.Top_r",0.0,0.0,1.0);
    pangolin::Var<double> vardBottom("ui.Bottom_r",1.0,0.0,1.0);
    pangolin::Var<double> vardLeft("ui.Left_r",0.0,0.0,1.0);
    pangolin::Var<double> vardRight("ui.Right_r",1.0,0.0,1.0);

    // =====================  图像窗口 ====================

    pangolin::View& d_image = pangolin::Display("image")
      .SetBounds(
          pangolin::Attach::Pix(0),
          pangolin::Attach::Pix(114),
          pangolin::Attach::Pix(396),
          pangolin::Attach::Pix(640),
          640.0/480)
      .SetLock(pangolin::LockCenter , pangolin::LockCenter);

    d_cam.AddDisplay(d_image);

    unsigned char* imageArray= new unsigned char[3*img.cols*img.rows];
    pangolin::GlTexture imageTexture(img.cols,img.rows,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);

    pangolin::View& d_image2 = pangolin::Display("image2")
      .SetBounds(
          0,
          0.2381,
          0.375,
          1.0,
          640.0/480)
      .SetLock(pangolin::LockCenter, pangolin::LockCenter);

    d_cam.AddDisplay(d_image2);

    unsigned char* imageArray2= new unsigned char[3*img2.cols*img2.rows];
    pangolin::GlTexture imageTexture2(img2.cols,img2.rows,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);

    while( !pangolin::ShouldQuit() )
    {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);

        //改变背景颜色
        glClearColor(0.3,0.3,0.3,0.0);

        /*
        if(checkRatio)
        {
            d_image.SetBounds(
                (double)vardTop,
                (double)vardBottom,
                (double)vardLeft,
                (double)vardRight);
        }
        else
        {
            d_image.SetBounds(
                pangolin::Attach::Pix((int)varTop),
                pangolin::Attach::Pix((int)varBottom),
                pangolin::Attach::Pix((int)varLeft),
                pangolin::Attach::Pix((int)varRight));

        }

        d_image2.SetBounds(
                (double)vardTop,
                (double)vardBottom,
                (double)vardLeft,
                (double)vardRight);

        */
        //Set some random image data and upload to GPU
        setImageData(imageArray,img);
        imageTexture.Upload(imageArray,GL_RGB,GL_UNSIGNED_BYTE);

        //Set some random image data and upload to GPU
        setImageData(imageArray2,img2);
        imageTexture2.Upload(imageArray2,GL_RGB,GL_UNSIGNED_BYTE);

        //display the image
        d_image.Activate();
        glColor3f(1.0,1.0,1.0);
        imageTexture.RenderToViewport();

        //display the image
        d_image2.Activate();
        glColor3f(1.0,1.0,1.0);
        imageTexture2.RenderToViewport();

        // Swap frames and Process Events
        pangolin::FinishFrame();
    }

    //return 0;
    // unset the current context from the main thread
    pangolin::GetBoundWindow()->RemoveCurrent();
}


#pragma once

#include <Eigen/Core>

#include <vector>

#include "../stateobject.hh"
#include "../../AgentPosition/agentposition.hh"
#include "../../OcclusionRay/occlusionray.hh"
#include "../../geometry/LineSegment/LineSegment2/LineSegment2i/linesegment2i.hh"
#include "../../geometry/Polygon2.hh"
#include "../../util/Maybe.hh"

namespace bold
{
  class WorldFrameState : public StateObject
  {
  public:
    WorldFrameState(Maybe<Eigen::Vector3d> ballObservation,
                    std::vector<Eigen::Vector3d> goalObservations,
                    std::vector<LineSegment3d> observedLineSegments,
                    Maybe<Polygon2d> visibleFieldPoly,
                    std::vector<OcclusionRay<double>> occlusionRays,
                    AgentPosition position)
    : d_ballObservation(ballObservation),
      d_goalObservations(goalObservations),
      d_observedLineSegments(observedLineSegments),
      d_visibleFieldPoly(visibleFieldPoly),
      d_occlusionRays(occlusionRays),
      d_position(position)
    {}

    Maybe<Eigen::Vector3d> getBallObservation() const { return d_ballObservation; }
    std::vector<Eigen::Vector3d> getGoalObservations() const { return d_goalObservations; }
    std::vector<LineSegment3d> getObservedLineSegments() const { return d_observedLineSegments; }
    Maybe<Polygon2d> getVisibleFieldPoly() const { return d_visibleFieldPoly; }
    std::vector<OcclusionRay<double>> getOcclusionRays() const { return d_occlusionRays; }

    AgentPosition getPosition() const { return d_position; }

    bool isBallVisible() const { return d_ballObservation.hasValue(); }

    void writeJson(rapidjson::Writer<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::Writer<WebSocketBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }

  private:
    template<typename TBuffer>
    void writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const;

    Maybe<Eigen::Vector3d> d_ballObservation;
    std::vector<Eigen::Vector3d> d_goalObservations;
    std::vector<LineSegment3d> d_observedLineSegments;
    Maybe<Polygon2d> d_visibleFieldPoly;
    std::vector<OcclusionRay<double>> d_occlusionRays;
    AgentPosition d_position;
  };

  template<typename TBuffer>
  inline void WorldFrameState::writeJsonInternal(rapidjson::Writer<TBuffer> &writer) const
  {
    writer.StartObject();
    {
      writer.String("pos");
      writer.StartArray();
      {
        writer.Double(d_position.x(), "%.3f");
        writer.Double(d_position.y(), "%.3f");
        writer.Double(d_position.theta(), "%.4f");
      }
      writer.EndArray();

      writer.String("ball");
      if (d_ballObservation.hasValue())
      {
        writer.StartArray();
        writer.Double(d_ballObservation->x(), "%.3f");
        writer.Double(d_ballObservation->y(), "%.3f");
        writer.Double(d_ballObservation->z(), "%.3f");
        writer.EndArray();
      }
      else
      {
        writer.Null();
      }

      writer.String("goals");
      writer.StartArray();
      {
        for (auto const& goalPos : d_goalObservations)
        {
          writer.StartArray();
          writer.Double(goalPos.x(), "%.3f");
          writer.Double(goalPos.y(), "%.3f");
          writer.Double(goalPos.z(), "%.3f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("lines");
      writer.StartArray();
      {
        for (LineSegment3d const& lineSeg : d_observedLineSegments)
        {
          writer.StartArray();
          writer.Double(lineSeg.p1().x(), "%.3f");
          writer.Double(lineSeg.p1().y(), "%.3f");
          writer.Double(lineSeg.p1().z(), "%.3f");
          writer.Double(lineSeg.p2().x(), "%.3f");
          writer.Double(lineSeg.p2().y(), "%.3f");
          writer.Double(lineSeg.p2().z(), "%.3f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("visibleFieldPoly");
      writer.StartArray();
      {
        if (d_visibleFieldPoly.hasValue())
        {
          for (auto const& vertex : d_visibleFieldPoly.value())
          {
            writer.StartArray();
            writer.Double(vertex.x(), "%.3f");
            writer.Double(vertex.y(), "%.3f");
            writer.EndArray();
          }
        }
      }
      writer.EndArray();

      writer.String("occlusionRays");
      writer.StartArray();
      {
        for (auto const& ray : d_occlusionRays)
        {
          // Should be enough to check a single value for NaN
          if (std::isnan(ray.far().x()))
            continue;

          writer.StartArray();
          writer.Double(ray.near().x(), "%.3f");
          writer.Double(ray.near().y(), "%.3f");
          writer.Double(ray.far().x(), "%.3f");
          writer.Double(ray.far().y(), "%.3f");
          writer.EndArray();
        }
      }
      writer.EndArray();
    }
    writer.EndObject();
  }
}


#include "DelaunayTriangulation.h"
#include "DelaunayMemoryManager.h"
#include "math/mathutils.h"
#include <float.h>
#include <cassert>
#include <set>
#include <list>
#include <boost/bind.hpp>

namespace math
{
   //--------------------------------------------------------------------------

   DelaunayTriangulation::DelaunayTriangulation(double xmin, double ymin, double xmax, double ymax, EDelaunayLocationAlgorithms eAlgorithm)
      : _xmin(xmin), _ymin(ymin), _xmax(xmax), _ymax(ymax), _pStartTriangle(0), _eLocationAlgorithm(eAlgorithm)
   {
      assert(_xmin<_xmax);
      assert(_ymin<_ymax);

      _Init();
   }

   //--------------------------------------------------------------------------

   DelaunayTriangulation::~DelaunayTriangulation()
   {
      Clear();
      if (_pStartTriangle)
      {
         //std::cout << "<b>Removing Triangle</b>\n";
         _qLocationStructure->DeleteMemory(_pStartTriangle);

         // This assertion is only valid when one instance of DelaunayTriangulation exists!
         //assert(DelaunayMemoryManager::GetNumTriangles() == 0);
         //assert(DelaunayMemoryManager::GetNumVertices() == 0);

      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_Init()
   {
      // calculate Triangle that surrounds area defined in _xmin, _ymin, _xmax, _ymax
      double Mx = _xmin + (_xmax-_xmin)/2.0;
      double My = _ymin + (_ymax-_ymin)/2.0;
      double r = sqrt((_xmax-Mx)*(_xmax-Mx)+(_ymax-My)*(_ymax-My));
      double Cy = _ymax + (_xmax-Mx)*(_xmax-Mx) / (_ymax-My);
      double Cx = Mx;
      double By = My -r;
      double Bx = Cx-(Cy-My+r)*(_xmax-Cx)/(_ymax-Cy);
      double Ay = My - r;
      double Ax = Cx-(Cy-My+r)*(_xmin-Cx)/(_ymax-Cy);

      DelaunayVertex* A = DelaunayMemoryManager::AllocVertex(Ax,Ay,0,-1);
      DelaunayVertex* B = DelaunayMemoryManager::AllocVertex(Bx,By,0,-1);
      DelaunayVertex* C = DelaunayMemoryManager::AllocVertex(Cx,Cy,0,-1);

      _pStartTriangle = DelaunayMemoryManager::AllocTriangle();

      _pStartTriangle->SetVertex(0, A);
      _pStartTriangle->SetVertex(1, B);
      _pStartTriangle->SetVertex(2, C);

      double xmax, ymax, xmin, ymin;
      xmax = ymax = -1e20;
      xmin = ymin = 1e20;

      xmin = math::Min<double>(Ax, xmin);
      xmin = math::Min<double>(Bx, xmin);
      xmin = math::Min<double>(Cx, xmin);
      ymin = math::Min<double>(Ay, ymin);
      ymin = math::Min<double>(By, ymin);
      ymin = math::Min<double>(Cy, ymin);
      xmax = math::Max<double>(Ax, xmax);
      xmax = math::Max<double>(Bx, xmax);
      xmax = math::Max<double>(Cx, xmax);
      ymax = math::Max<double>(Ay, ymax);
      ymax = math::Max<double>(By, ymax);
      ymax = math::Max<double>(Cy, ymax);

      _qLocationStructure = IDelaunayLocationStructure::CreateLocationStructure(xmin, ymin, xmax, ymax, _eLocationAlgorithm);
      if (_qLocationStructure)
      {
         _qLocationStructure->AddTriangle(_pStartTriangle);
      }

      if (!_pStartTriangle->IsCCW())
      {
         std::cout << "Error: Start-triangle is not ccw!\n";
         assert(false);
      }

      _bError = false;
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_CollectTriangle(DelaunayTriangle* pTri)
   {
      _vecTriangles.push_back(pTri);
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::Clear()
   {
      if (_pStartTriangle)
      {
         _vecTriangles.clear();
         _qLocationStructure->Traverse(boost::bind(&DelaunayTriangulation::_CollectTriangle, this, _1));

         for (size_t i=0;i<_vecTriangles.size();i++)
         {
            _qLocationStructure->DeleteMemory(_vecTriangles[i]);
         }

         _pStartTriangle = 0;
         _Init();
      }
   }

   //--------------------------------------------------------------------------

   //--------------------------------------------------------------------------

   namespace internal
   {
      inline double _CalcTriArea(const ElevationPoint& A, const ElevationPoint& B, const ElevationPoint& C)
      {
         return math::ccw(A.x, A.y, B.x, B.y, C.x, C.y);
         //return 0.5*((B.x - A.x)*(C.y - A.y) - (B.y - A.y)*(C.x - A.x));
      }
   }

   double DelaunayTriangulation::GetElevationAt(double x, double y, ElevationQuery& query_result)
   {
      if (x<=_xmax && x>=_xmin &&
         y<=_ymax && y>=_ymin)
      {
         query_result = EQ_INTERIOR;
         ePointTriangleRelation relation;
         DelaunayTriangle* pTri = _qLocationStructure->GetTriangleAt(x,y,relation);

         if (pTri && !pTri->IsSuperSimplex())
         {
            ElevationPoint P;
            P.x = x; P.y = y; P.elevation = 0.0;

            ElevationPoint A = pTri->GetVertex(0)->GetElevationPointCopy();
            ElevationPoint B = pTri->GetVertex(1)->GetElevationPointCopy();
            ElevationPoint C = pTri->GetVertex(2)->GetElevationPointCopy();

            double elva = A.elevation;
            double elvb = B.elevation;
            double elvc = C.elevation;

            switch(relation)
            {
            case PointTriangle_Edge0:  // Point on Edge 0
            case PointTriangle_Edge1:  // Point on Edge 1
            case PointTriangle_Edge2:  // Point on Edge 2
            case PointTriangle_Inside: // Point inside triangle
               {
                  A.elevation = 0.0;
                  B.elevation = 0.0;
                  C.elevation = 0.0;

                  double F_abc = internal::_CalcTriArea(A,B,C);
                  double F_pbc = internal::_CalcTriArea(P,B,C);
                  double F_apc = internal::_CalcTriArea(A,P,C);

                  double r = F_pbc/F_abc;
                  double s = F_apc/F_abc;
                  double t = 1.0-r-s;

                  A.elevation = elva;
                  B.elevation = elvb;
                  C.elevation = elvc;

                  double Pelv = r*A.elevation + s*B.elevation + t*C.elevation;
                  return Pelv;
               }
               break;
            case PointTriangle_Vertex0:  // Point lies on vertex 0
               return pTri->GetVertex(0)->elevation();
            case PointTriangle_Vertex1:  // Point lies on vertex 1
               return pTri->GetVertex(1)->elevation();
            case PointTriangle_Vertex2:  // Point lies on vertex 2
               return pTri->GetVertex(2)->elevation();
            default: // outside triangle / invalid triangle
               query_result = EQ_EXTERIOR;
               return 0.0;
            }
         }
         else
         {
            query_result = EQ_EXTERIOR;
            return 0.0;
         }
      }
      else
      {
         query_result = EQ_UNDEFINED; // outside valid triangulation area
         return 0.0;
      }

   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::InsertPoint(const ElevationPoint& pt)
   {
      if (pt.x<=_xmax && pt.x>=_xmin &&
         pt.y<=_ymax && pt.y>=_ymin)
      {
         DelaunayVertex* pNewVertex = DelaunayMemoryManager::AllocVertex(pt);
         _pStartTriangle = _qLocationStructure->InsertVertex(pNewVertex, _pStartTriangle);
         _bError = false; // inserting a point invalidates errors!
      }
   }

   void DelaunayTriangulation::_InsertPointSetId(const ElevationPoint& pt, int id)
   {
      if (pt.x<=_xmax && pt.x>=_xmin &&
         pt.y<=_ymax && pt.y>=_ymin)
      {
         DelaunayVertex* pNewVertex = DelaunayMemoryManager::AllocVertex(pt);
         pNewVertex->SetId(id);
         _pStartTriangle = _qLocationStructure->InsertVertex(pNewVertex, _pStartTriangle);
         _bError = false; // inserting a point invalidates errors!
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_ResetVertexId(DelaunayTriangle* pTri)
   {
      assert(pTri);
      assert(pTri->GetVertex(0));
      assert(pTri->GetVertex(1));
      assert(pTri->GetVertex(2));

      pTri->GetVertex(0)->SetId(-1);
      pTri->GetVertex(1)->SetId(-1);
      pTri->GetVertex(2)->SetId(-1);
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_CollectElevationPoints(DelaunayTriangle* pTri)
   {
      assert(pTri);
      assert(pTri->GetVertex(0));
      assert(pTri->GetVertex(1));
      assert(pTri->GetVertex(2));

      // ignore supersimplex triangles.
      if (!pTri->IsSuperSimplex())
      {
         for (int v=0;v<3;v++)
         {
            if (pTri->GetVertex(v)->GetId() == -1)
            {
               pTri->GetVertex(v)->SetId(_idcnt);
               _vPts.push_back(pTri->GetVertex(v)->GetElevationPoint());
               _idcnt++;
            }

            _vIndex.push_back(pTri->GetVertex(v)->GetId());
         }
      }
   }

   //--------------------------------------------------------------------------

   // before calling this make sure to clear _vCutEdge, _vIndex and _vPts
   void DelaunayTriangulation::_CollectTriangulationStructure(DelaunayTriangle* pTri)
   {
      assert(pTri);
      assert(pTri->GetVertex(0));
      assert(pTri->GetVertex(1));
      assert(pTri->GetVertex(2));

      // ignore supersimplex triangles.
      if (!pTri->IsSuperSimplex())
      {
         for (int v=0;v<3;v++)
         {
            if (pTri->GetVertex(v)->GetId() == -1)
            {
               pTri->GetVertex(v)->SetId(_idcnt);
               _vPts.push_back(pTri->GetVertex(v)->GetElevationPoint());
               _idcnt++;
            }

            _vIndex.push_back(pTri->GetVertex(v)->GetId());
         }

         if (pTri->GetTriangle(0) == 0)
         {
            _vCutEdge.push_back(std::pair<int, int>(pTri->GetVertex(0)->GetId(), pTri->GetVertex(1)->GetId()));
         }

         if (pTri->GetTriangle(1) == 0)
         {
            _vCutEdge.push_back(std::pair<int, int>(pTri->GetVertex(1)->GetId(), pTri->GetVertex(2)->GetId()));
         }

         if (pTri->GetTriangle(2) == 0)
         {
            _vCutEdge.push_back(std::pair<int, int>(pTri->GetVertex(2)->GetId(), pTri->GetVertex(0)->GetId()));
         }

      }

   }

   //--------------------------------------------------------------------------

   // Retrieve Triangulation as Point / Index List
   void DelaunayTriangulation::GetPointVec(std::vector<ElevationPoint>& vPoints)
   {

      // Reset All Vertices to 0
      _qLocationStructure->Traverse(boost::bind(&DelaunayTriangulation::_ResetVertexId, this, _1));

      _idcnt = 0; _vPts.clear(); _vIndex.clear(); _vCutEdge.clear();
      _qLocationStructure->Traverse(boost::bind(&DelaunayTriangulation::_CollectTriangulationStructure, this, _1));

      vPoints = _vPts;
      _vPts.clear();

   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::GetTriangleIndices(std::vector<int>& vIndices)
   {
      vIndices = _vIndex;
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::GetCutEdges(std::vector<std::pair<int, int> >& vCutEdges)
   {
      vCutEdges = _vCutEdge;
   }
   //--------------------------------------------------------------------------

   std::vector<DelaunayTriangle*>& DelaunayTriangulation::GetAllTriangles()
   {
      _vecTriangles.clear();
      _qLocationStructure->Traverse(boost::bind(&DelaunayTriangulation::_CollectTriangle, this, _1));

      return _vecTriangles;
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::DeleteMemory(DelaunayTriangle* pTri)
   {
      _qLocationStructure->DeleteMemory(pTri);
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_CutEdges(std::vector< std::pair<int,int> >& vCut)
   {
      // Traverse triangulation structure and delete corresponding triangles

      std::vector<DelaunayTriangle*>& vTri = DelaunayTriangulation::GetAllTriangles();

      std::set<DelaunayTriangle*> setDelete;

      for (size_t i=0;i<vTri.size();i++)
      {
         DelaunayTriangle* pTri = vTri[i];
         DelaunayTriangle* pTri0 = pTri->GetTriangle(0);
         DelaunayTriangle* pTri1 = pTri->GetTriangle(1);
         DelaunayTriangle* pTri2 = pTri->GetTriangle(2);

         int A,B,C;
         A = pTri->GetVertex(0)->GetId();
         B = pTri->GetVertex(1)->GetId();
         C = pTri->GetVertex(2)->GetId();

         // does this triangle contain a cut edge ?

         for (size_t c=0;c<vCut.size();c++)
         {
            int start = vCut[c].first;
            int end = vCut[c].second;

            if (A == start && B == end)
            {
               setDelete.insert(pTri0);
            }
            else if (B == start && C == end)
            {
               setDelete.insert(pTri1);
            }
            else if (C == start && A == end)
            {
               setDelete.insert(pTri2);
            }
         }
      }

      std::set<DelaunayTriangle*>::iterator it = setDelete.begin();

      while (it != setDelete.end())
      {
         DeleteMemory(*it);
         it++;
      }

   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::SetEpsilon(double epsilon)
   {
      assert(_qLocationStructure);

      if (_qLocationStructure)
      {
         _qLocationStructure->SetEpsilon(epsilon);
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_ResetVertexErrors(DelaunayTriangle* pTri)
   {
      pTri->GetVertex(0)->GetElevationPoint().error = -1.0;
      pTri->GetVertex(1)->GetElevationPoint().error = -1.0;
      pTri->GetVertex(2)->GetElevationPoint().error = -1.0;
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_GetCCWVertices(DelaunayTriangle* pTri, int vertex_index, std::vector<DelaunayVertex*>& outputVertices)
   {
      outputVertices.clear();
      std::list<DelaunayVertex*> lst;

      const DelaunayTriangle* pStartTriangle = pTri;

      int vtx = vertex_index;
      int triangle_index = (vtx+2)%3;
      lst.push_front(pTri->GetVertex((vtx+2)%3));

      DelaunayTriangle* A = pTri;
      DelaunayTriangle* C = pTri->GetTriangle(triangle_index);

      while(C && C != pStartTriangle && !C->IsSuperSimplex() && !A->IsSuperSimplex())
      {
         if (A->GetVertex(vtx) == C->GetVertex(0))
         {
            vtx = 0;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(1))
         {
            vtx = 1;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(2))
         {
            vtx = 2;
         }
         else
         {
            assert(false);
            break;
         }

         lst.push_front(C->GetVertex((vtx+2)%3));

         triangle_index = (vtx+2)%3;
         A = C;
         C = C->GetTriangle(triangle_index);
      }

      lst.push_back(pTri->GetVertex((vertex_index+1)%3));

      // ignore non ccw direction!
      // copy result to output
      std::list<DelaunayVertex*>::iterator it = lst.begin();
      while (it!=lst.end())
      {
         outputVertices.push_back(*it);
         it++;
      }

   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_CreateSurroundingPolygon(DelaunayTriangle* pTri, int vertex_index, std::vector<ElevationPoint>& outputPolygon)
   {
      outputPolygon.clear();
      std::list<ElevationPoint> lst;

      const DelaunayTriangle* pStartTriangle = pTri;

      int vtx = vertex_index;
      int triangle_index = (vtx+2)%3;
      lst.push_front(pTri->GetVertex((vtx+2)%3)->GetElevationPoint());

      DelaunayTriangle* A = pTri;
      DelaunayTriangle* B;
      DelaunayTriangle* C = pTri->GetTriangle(triangle_index);

      while(C && C != pStartTriangle && !C->IsSuperSimplex() && !A->IsSuperSimplex())
      {
         if (A->GetVertex(vtx) == C->GetVertex(0))
         {
            vtx = 0;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(1))
         {
            vtx = 1;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(2))
         {
            vtx = 2;
         }
         else
         {
            assert(false);
            break;
         }

         lst.push_front(C->GetVertex((vtx+2)%3)->GetElevationPoint());

         triangle_index = (vtx+2)%3;
         A = C;
         C = C->GetTriangle(triangle_index);
      }

      // other direction and only if pTri->GetTriangle(vtx) != pStartTriangle
      lst.push_back(pTri->GetVertex((vertex_index+1)%3)->GetElevationPoint());

      if (C != pStartTriangle)
      {
         vtx = vertex_index;
         triangle_index = vtx;

         A = pTri;
         B = pTri->GetTriangle(triangle_index);

         while(B && B != pStartTriangle && !B->IsSuperSimplex() && !A->IsSuperSimplex())
         {
            if (A->GetVertex(vtx) == B->GetVertex(0))
            {
               vtx = 0;
            }
            else if (A->GetVertex(vtx) == B->GetVertex(1))
            {
               vtx = 1;
            }
            else if (A->GetVertex(vtx) == B->GetVertex(2))
            {
               vtx = 2;
            }

            lst.push_back(B->GetVertex((vtx+1)%3)->GetElevationPoint());
            triangle_index = vtx;

            A = B;
            B = B->GetTriangle(triangle_index);
         }

      }

      // Convert list to vector (for returning values)
      if (lst.size()>2)
      {
         std::list<ElevationPoint>::iterator it = lst.begin();
         while (it!=lst.end())
         {
            outputPolygon.push_back(*it);
            it++;
         }
      }

   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_CalcVertexErrorsVtx(DelaunayTriangle* pTri, int vtx)
   {
      assert(vtx>=0 && vtx<=3);
      assert(pTri);

      ElevationPoint pt = pTri->GetVertex(vtx)->GetElevationPoint();

      // corner points always have max error
      if (pt.weight < -2
         || pTri->GetVertex((vtx+1)%3)->GetElevationPoint().weight < -2
         || pTri->GetVertex((vtx+2)%3)->GetElevationPoint().weight < -2)
      {
         pTri->GetVertex(vtx)->GetElevationPoint().error = DBL_MAX;
      }
      else
      {
         boost::shared_ptr<DelaunayTriangulation> qTriangulation;

         if (pTri->GetVertex(vtx)->GetElevationPoint().error <= -1.0)
         {
            std::vector<ElevationPoint> outputPolygon;
            _CreateSurroundingPolygon(pTri, vtx, outputPolygon);

            qTriangulation = boost::shared_ptr<DelaunayTriangulation>(new DelaunayTriangulation(_xmin, _ymin, _xmax, _ymax, _eLocationAlgorithm));
            qTriangulation->SetEpsilon(DBL_EPSILON); // can be removed later

            for (size_t i=0;i<outputPolygon.size();i++)
            {
               qTriangulation->InsertPoint(outputPolygon[i]);
            }

            math::ElevationQuery query;
            double elv = qTriangulation->GetElevationAt(pt.x, pt.y, query);

            if (query == EQ_INTERIOR)
            {
               double dError = fabs(elv - pt.elevation);
               pTri->GetVertex(vtx)->GetElevationPoint().error = dError;

               if (dError < _minError)
               {
                  _oVertexMinError.pTri = pTri;
                  _oVertexMinError.idx0 = vtx;
                  _minError = dError;
               }

            }
            else
            {
               pTri->GetVertex(vtx)->GetElevationPoint().error = DBL_MAX;
            }

         }

      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_CalcVertexErrors(DelaunayTriangle* pTri)
   {
      assert(pTri);

      if (!pTri->IsSuperSimplex()) // ignore supersimplex triangles!
      {
         _CalcVertexErrorsVtx(pTri,0);
         _CalcVertexErrorsVtx(pTri,1);
         _CalcVertexErrorsVtx(pTri,2);
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::CalculateVertexErrors()
   {
      _oVertexMinError.pTri = 0;
      _oVertexMinError.idx0 = -1;

      _minError = DBL_MAX;
      // Reset Errors:
      _qLocationStructure->Traverse(boost::bind(&DelaunayTriangulation::_ResetVertexErrors, this, _1));
      _qLocationStructure->Traverse(boost::bind(&DelaunayTriangulation::_CalcVertexErrors, this, _1));

      _bError = true;
      //std::cout << "MinError: " << _minError << "\n";
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::UpdateVertexErrors(std::vector<DelaunayVertex*>& vVertex)
   {

      ePointTriangleRelation e;
      DelaunayTriangle* pTri;
      int idx;

      for (size_t i=0;i<vVertex.size();i++)
      {
         idx = -1;
         pTri = _qLocationStructure->GetTriangleAt(vVertex[i]->x(),vVertex[i]->y(),e);
         if (pTri && !pTri->IsSuperSimplex())
         {
            if (pTri->GetVertex(0) == vVertex[i])
               idx = 0;
            else if (pTri->GetVertex(1) == vVertex[i])
               idx = 1;
            else if (pTri->GetVertex(2) == vVertex[i])
               idx = 2;

            if (idx != -1)
            {
               _CalcVertexErrorsVtx(pTri,idx);
            }
            else
            {
               assert(false); // vertex not found!! How can this be ??!??
            }
         }

      }

      _UpdateMinError();
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_UpdateVertexErrors(DelaunayTriangle* pTri)
   {
      double dError;

      if (!pTri->IsSuperSimplex())
      {
         for (int i=0;i<3;i++)
         {
            dError = pTri->GetVertex(i)->GetElevationPoint().error;
            if (dError < _minError)
            {
               _oVertexMinError.pTri = pTri;
               _oVertexMinError.idx0 = i;
               _minError = dError;
            }
         }
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_UpdateMinError()
   {
      _minError = DBL_MAX;
      _qLocationStructure->Traverse(boost::bind(&DelaunayTriangulation::_UpdateVertexErrors, this, _1));
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::RemoveLeastErrorVertex()
   {
      if (_minError == DBL_MAX)
         return;

      if (_oVertexMinError.pTri)
      {
         // GetNeighbourVertices and save in list
         if (_oVertexMinError.pTri->GetVertex(_oVertexMinError.idx0)->weight() == -3)
         {
            std::cout << "**** FATAL ERROR ****: Removing corner vertex!!\n";
         }

         _RemoveVertex(_oVertexMinError.pTri, _oVertexMinError.idx0);
         _oVertexMinError.pTri = 0;
         _oVertexMinError.idx0 = -1;
      }

   }

   //--------------------------------------------------------------------------

   int DelaunayTriangulation::Simplify(double epsilon, int maxiterations)
   {
      std::vector<DelaunayVertex*> vVertex;
      int rmvsteps = 0;
      if (!_bError)
         CalculateVertexErrors();

       while (_minError <= epsilon && _minError != DBL_MAX && rmvsteps < maxiterations)
       {
          GetCCWVertices(_oVertexMinError.pTri, _oVertexMinError.idx0, vVertex);
          RemoveLeastErrorVertex(); rmvsteps++;
          UpdateVertexErrors(vVertex);
       }

       /*if (rmvsteps >= maxiterations)
       {
          std::cout << "Simplify-Condition: Max Iterations reached!\n";
       }

       if (_minError > epsilon)
       {
          std::cout << "Simplify-Condition: Epsilon condition reached!\n";
       }*/

       return rmvsteps;

   }

   //--------------------------------------------------------------------------
   void DelaunayTriangulation::Reduce(int nPoints)
   {
      std::vector<DelaunayVertex*> vVertex;
      int rmvsteps = 0;
      if (!_bError)
         CalculateVertexErrors();

      while (_minError != DBL_MAX && rmvsteps < nPoints)
      {
         GetCCWVertices(_oVertexMinError.pTri, _oVertexMinError.idx0, vVertex);
         RemoveLeastErrorVertex(); rmvsteps++;
         UpdateVertexErrors(vVertex);
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_GetVertexAt(double x, double y, DelaunayTriangle*& pTri, int& idx)
   {
      ePointTriangleRelation e;
      pTri = _qLocationStructure->GetTriangleAt(x,y,e);
      if (pTri)
      {
         const ElevationPoint& P0 = pTri->GetVertex(0)->GetElevationPoint();
         const ElevationPoint& P1 = pTri->GetVertex(1)->GetElevationPoint();
         const ElevationPoint& P2 = pTri->GetVertex(2)->GetElevationPoint();

         double dist0, dist1, dist2;

         dist0 = sqrt((P0.x-x)*(P0.x-x)+(P0.y-y)*(P0.y-y));
         dist1 = sqrt((P1.x-x)*(P1.x-x)+(P1.y-y)*(P1.y-y));
         dist2 = sqrt((P2.x-x)*(P2.x-x)+(P2.y-y)*(P2.y-y));

         if (dist0 <= dist1 && dist0 <= dist2)
         {
            idx = 0;
         }
         else if (dist1 <= dist0 && dist1 <= dist2)
         {
            idx = 1;
         }
         else //if (dist2 <= dist0 && dist2 <=dist1)
         {
            idx = 2;
         }
      }
      else
      {
         // no triangle found!!
         idx = -1;
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::GetCCWTriangles(DelaunayTriangle* pTri, int vertex_index, std::vector<STriangleVertex>& outputVertices)
   {
      outputVertices.clear();
      std::list<STriangleVertex> lst;

      const DelaunayTriangle* pStartTriangle = pTri;

      STriangleVertex oElement;
      int vtx = vertex_index;
      int triangle_index = (vtx+2)%3;

      oElement.pTri = pTri;
      oElement.idx0 = (vertex_index+1)%3;
      lst.push_front(oElement);

      DelaunayTriangle* A = pTri;
      DelaunayTriangle* B;
      DelaunayTriangle* C = pTri->GetTriangle(triangle_index);

      while(C && C != pStartTriangle)
      {
         if (A->GetVertex(vtx) == C->GetVertex(0))
         {
            vtx = 0;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(1))
         {
            vtx = 1;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(2))
         {
            vtx = 2;
         }
         else
         {
            assert(false);
            break;
         }

         oElement.pTri = C;
         oElement.idx0 = (vtx+1)%3;
         lst.push_front(oElement);

         triangle_index = (vtx+2)%3;
         A = C;
         C = C->GetTriangle(triangle_index);
      }

      // move around other side:

      if (C != pStartTriangle)
      {
         vtx = vertex_index;
         triangle_index = vtx;

         A = pTri;
         B = pTri->GetTriangle(triangle_index);

         while(B && B != pStartTriangle)
         {
            if (A->GetVertex(vtx) == B->GetVertex(0))
            {
               vtx = 0;
            }
            else if (A->GetVertex(vtx) == B->GetVertex(1))
            {
               vtx = 1;
            }
            else if (A->GetVertex(vtx) == B->GetVertex(2))
            {
               vtx = 2;
            }

            oElement.pTri = B;
            oElement.idx0 = (vtx+1)%3;
            lst.push_back(oElement);
            triangle_index = vtx;

            A = B;
            B = B->GetTriangle(triangle_index);
         }

      }

      std::reverse(lst.begin(), lst.end());

      // copy result to output
      std::list<STriangleVertex>::iterator it = lst.begin();
      while (it!=lst.end())
      {
         outputVertices.push_back(*it);
         it++;
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::GetCCWVertices(DelaunayTriangle* pTri, int vertex_index, std::vector<DelaunayVertex*>& outputVertices)
   {
      outputVertices.clear();
      std::list<DelaunayVertex*> lst;

      const DelaunayTriangle* pStartTriangle = pTri;

      int vtx = vertex_index;
      int triangle_index = (vtx+2)%3;

      lst.push_front(pTri->GetVertex((vertex_index+1)%3));

      DelaunayTriangle* A = pTri;
      DelaunayTriangle* B;
      DelaunayTriangle* C = pTri->GetTriangle(triangle_index);

      while(C && C != pStartTriangle)
      {
         if (A->GetVertex(vtx) == C->GetVertex(0))
         {
            vtx = 0;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(1))
         {
            vtx = 1;
         }
         else if (A->GetVertex(vtx) == C->GetVertex(2))
         {
            vtx = 2;
         }
         else
         {
            assert(false);
            break;
         }

         lst.push_front(C->GetVertex((vtx+1)%3));

         triangle_index = (vtx+2)%3;
         A = C;
         C = C->GetTriangle(triangle_index);
      }

      // move around other side:

      if (C != pStartTriangle)
      {
         vtx = vertex_index;
         triangle_index = vtx;

         A = pTri;
         B = pTri->GetTriangle(triangle_index);

         while(B && B != pStartTriangle)
         {
            if (A->GetVertex(vtx) == B->GetVertex(0))
            {
               vtx = 0;
            }
            else if (A->GetVertex(vtx) == B->GetVertex(1))
            {
               vtx = 1;
            }
            else if (A->GetVertex(vtx) == B->GetVertex(2))
            {
               vtx = 2;
            }

            lst.push_back(B->GetVertex((vtx+1)%3));
            triangle_index = vtx;

            A = B;
            B = B->GetTriangle(triangle_index);
         }

      }

      std::reverse(lst.begin(), lst.end());

      // copy result to output
      std::list<DelaunayVertex*>::iterator it = lst.begin();
      while (it!=lst.end())
      {
         outputVertices.push_back(*it);
         it++;
      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::_RemoveVertex(DelaunayTriangle* pTri, int idx)
   {
      if (pTri && idx>=0 && idx<=3)
      {
         bool DebugOutput = false;
         DelaunayVertex* pVertex = pTri->GetVertex(idx); // this is the point P to be removed.

         std::vector<STriangleVertex> outputTriangles;
         GetCCWTriangles(pTri, idx, outputTriangles);

         if (outputTriangles.size() < 3 )
         {
            // this can't happen in a "normal" triangulation
            // unless you try to remove supersimplex corner.
            // however this can happen in a triangulation with holes. This
            // case is currently not supported!
            std::cout<< "*Error* Can't remove specified vertex! (Triangulation would be broken.)\n";
            return;
         }
         else
         {
            //std::cout << "-----------------------------\n";
            //std::cout << "Removing Point\n";
            do
            {
               size_t num_changes = 0;

               //std::cout << "outputTriangles: " << outputTriangles.size() << "\n";

               if (outputTriangles.size() > 3)
               {

                  std::vector<STriangleVertex>::iterator it = outputTriangles.begin();

                  while (it != outputTriangles.end() && outputTriangles.size()>3)
                  {

                     DelaunayTriangle* pCurrentTriangle = (*it).pTri;
                     int idx0 = (*it).idx0;
                     int idx1 = (idx0+1)%3;

                     DelaunayVertex* s0 = pCurrentTriangle->GetVertex(idx0);
                     DelaunayVertex* s1 = pCurrentTriangle->GetVertex(idx1);
                     DelaunayVertex* s2 = pCurrentTriangle->GetOppositeVertex(idx1);

                     // for debugging reasons only:
                     ElevationPoint s0p = s0->GetElevationPoint();
                     ElevationPoint s1p = s1->GetElevationPoint();
                     ElevationPoint s2p = s2->GetElevationPoint();
                     ElevationPoint sP = pVertex->GetElevationPoint();

                     double ccwpredicate = math::ccw(s0,s1,s2);

                     if (DebugOutput)
                        std::cout << "ccwpredicate0 = " << ccwpredicate << "\n";

                     if (ccwpredicate<=0)
                     {
                        // this triangle-combination is ignored!
                     }
                     else
                     {
                        ccwpredicate = math::ccw(s0,s2,pVertex);
                        if (DebugOutput)
                           std::cout << "ccwpredicate1 = " << ccwpredicate << "\n";
                        if (ccwpredicate<0) // P encloses Triangle ?
                        {

                           // this triangle-combination is ignored!
                        }
                        else
                        {
                           // Check if other points are inside cirumcircle!
                           bool bCircleTest = true;

                           std::vector<STriangleVertex>::iterator it2 = outputTriangles.begin();
                           while (it2 != outputTriangles.end())
                           {
                              if ((*it2).pTri != (*it).pTri)
                              {
                                 int tidx0 = (*it2).idx0;
                                 int tidx1 = (tidx0+1)%3;

                                 DelaunayVertex* p0 = (*it2).pTri->GetVertex(tidx0);
                                 DelaunayVertex* p1 = (*it2).pTri->GetVertex(tidx1);

                                 assert(p0 != pVertex);
                                 assert(p1 != pVertex);

                                 if (p0 != s0 && p0 != s1 && p0 != s2 &&
                                     p1 != s0 && p1 != s1 && p1 != s2)
                                 {
                                    double circ = math::InCircleValue(s0,s1,s2,p0);
                                    if (DebugOutput)
                                       std::cout << "circ0 = " << circ << "\n";
                                    if (circ >= DBL_EPSILON)
                                    {
                                       bCircleTest = false;
                                    }
                                    circ = math::InCircleValue(s0,s1,s2,p1);
                                    if (DebugOutput)
                                       std::cout << "circ1 = " << circ << "\n";
                                    if (circ >= DBL_EPSILON)
                                    {
                                       bCircleTest = false;
                                    }
                                 }
                              }
                              it2++;
                           }

                           if (bCircleTest)
                           {
                              // this triangle and (next) are swapped and removed from list.
                              // new (swapped) triangle containing P will be added to list.

                              DelaunayTriangle* C;
                              DelaunayTriangle* D;

                              if (pCurrentTriangle->FlipEdge(idx1, &C, &D))
                              {
                                 it = outputTriangles.erase(it);
                                 if (it==outputTriangles.end())
                                 {
                                    it = outputTriangles.begin();
                                 }
                                 it = outputTriangles.erase(it);

                                 STriangleVertex newElement;

                                 if (C->GetVertex(0) == pVertex)
                                 {
                                    newElement.pTri = C; newElement.idx0 = 1;
                                 }
                                 else if (C->GetVertex(1) == pVertex)
                                 {
                                    newElement.pTri = C; newElement.idx0 = 2;
                                 }
                                 else if (C->GetVertex(2) == pVertex)
                                 {
                                    newElement.pTri = C; newElement.idx0 = 0;
                                 }
                                 else if (D->GetVertex(0) == pVertex)
                                 {
                                    newElement.pTri = D; newElement.idx0 = 1;
                                 }
                                 else if (D->GetVertex(1) == pVertex)
                                 {
                                    newElement.pTri = D; newElement.idx0 = 2;
                                 }
                                 else if (D->GetVertex(2) == pVertex)
                                 {
                                    newElement.pTri = D; newElement.idx0 = 0;
                                 }
                                 else
                                 {
                                    assert(false);
                                 }

                                 it = outputTriangles.insert(it, newElement);
                                 num_changes++;
                              }
                              else
                              {
                                 // can't flip
                              }
                           }

                        }
                     }

                     it++;
                  }
               }

               if (outputTriangles.size()>3 && num_changes == 0)
               {
                  std::cout << "<b>*WARNING* Detected infinite loop!</b>\n";
                  pVertex->GetElevationPoint().error = -0.5;

                  if (DebugOutput)
                     break;
                  else
                     DebugOutput = true;
               }

               // 3 Remaining pairs: Remove 3 Triangles!!
               if (outputTriangles.size() == 3)
               {
                  STriangleVertex* st0 =  &outputTriangles[0];
                  STriangleVertex* st1 =  &outputTriangles[1];
                  STriangleVertex* st2 =  &outputTriangles[2];

                  // Create New Triangle
                  DelaunayTriangle* pNewTriangle = DelaunayMemoryManager::AllocTriangle();
                  pNewTriangle->SetVertex(0, st0->pTri->GetVertex(st0->idx0));
                  pNewTriangle->SetVertex(1, st1->pTri->GetVertex(st1->idx0));
                  pNewTriangle->SetVertex(2, st2->pTri->GetVertex(st2->idx0));

                  DelaunayTriangle* pNeighbour0 = st0->pTri->GetTriangle(st0->idx0);
                  int nr0 = st0->pTri->NeighbourReference(st0->idx0);

                  DelaunayTriangle* pNeighbour1 = st1->pTri->GetTriangle(st1->idx0);
                  int nr1 = st1->pTri->NeighbourReference(st1->idx0);

                  DelaunayTriangle* pNeighbour2 = st2->pTri->GetTriangle(st2->idx0);
                  int nr2 = st2->pTri->NeighbourReference(st2->idx0);

                  pNewTriangle->SetTriangle(0, pNeighbour0);
                  pNewTriangle->SetTriangle(1, pNeighbour1);
                  pNewTriangle->SetTriangle(2, pNeighbour2);

                  _qLocationStructure->DeleteMemory(st0->pTri);
                  _qLocationStructure->DeleteMemory(st1->pTri);
                  _qLocationStructure->DeleteMemory(st2->pTri);

                  if (pNeighbour0)
                     pNeighbour0->SetTriangle(nr0, pNewTriangle);

                  if (pNeighbour1)
                     pNeighbour1->SetTriangle(nr1, pNewTriangle);

                  if (pNeighbour2)
                     pNeighbour2->SetTriangle(nr2, pNewTriangle);

                  _qLocationStructure->AddTriangle(pNewTriangle);

                  //assert(pNewTriangle->IsCCW()); // the mosted hated assertion
               }

            }
            while (outputTriangles.size()>3);
         }

      }
   }

   //--------------------------------------------------------------------------

   void DelaunayTriangulation::RemoveVertex(double x, double y)
   {
      int idx;
      DelaunayTriangle* pTri;
      _GetVertexAt(x,y,pTri,idx);
      _RemoveVertex(pTri,idx);
   }

   //--------------------------------------------------------------------------

} // namespace


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

// Copyright (c) 2015-2017, Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_UTIL_NORMALIZE_SPHEROIDAL_BOX_COORDINATES_HPP
#define BOOST_GEOMETRY_UTIL_NORMALIZE_SPHEROIDAL_BOX_COORDINATES_HPP

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

namespace boost { namespace geometry
{

namespace math
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename Units, typename CoordinateType, bool IsEquatorial = true>
class normalize_spheroidal_box_coordinates
{
private:
    typedef normalize_spheroidal_coordinates<Units, CoordinateType> normalize;
    typedef constants_on_spheroid<CoordinateType, Units> constants;

    static inline bool is_band(CoordinateType const& longitude1,
                               CoordinateType const& longitude2)
    {
        return ! math::smaller(math::abs(longitude1 - longitude2),
                               constants::period());
    }

public:
    static inline void apply(CoordinateType& longitude1,
                             CoordinateType& latitude1,
                             CoordinateType& longitude2,
                             CoordinateType& latitude2,
                             bool band)
    {
        normalize::apply(longitude1, latitude1, false);
        normalize::apply(longitude2, latitude2, false);

        latitude_convert_if_polar<Units, IsEquatorial>::apply(latitude1);
        latitude_convert_if_polar<Units, IsEquatorial>::apply(latitude2);

        if (math::equals(latitude1, constants::min_latitude())
            && math::equals(latitude2, constants::min_latitude()))
        {
            // box degenerates to the south pole
            longitude1 = longitude2 = CoordinateType(0);
        }
        else if (math::equals(latitude1, constants::max_latitude())
                 && math::equals(latitude2, constants::max_latitude()))
        {
            // box degenerates to the north pole
            longitude1 = longitude2 = CoordinateType(0);
        }
        else if (band)
        {
            // the box is a band between two small circles (parallel
            // to the equator) on the spheroid
            longitude1 = constants::min_longitude();
            longitude2 = constants::max_longitude();
        }
        else if (longitude1 > longitude2)
        {
            // the box crosses the antimeridian, so we need to adjust
            // the longitudes
            longitude2 += constants::period();
        }

        latitude_convert_if_polar<Units, IsEquatorial>::apply(latitude1);
        latitude_convert_if_polar<Units, IsEquatorial>::apply(latitude2);

#ifdef BOOST_GEOMETRY_NORMALIZE_LATITUDE
        BOOST_GEOMETRY_ASSERT(! math::larger(latitude1, latitude2));
        BOOST_GEOMETRY_ASSERT(! math::smaller(latitude1, constants::min_latitude()));
        BOOST_GEOMETRY_ASSERT(! math::larger(latitude2, constants::max_latitude()));
#endif

        BOOST_GEOMETRY_ASSERT(! math::larger(longitude1, longitude2));
        BOOST_GEOMETRY_ASSERT(! math::smaller(longitude1, constants::min_longitude()));
        BOOST_GEOMETRY_ASSERT
            (! math::larger(longitude2 - longitude1, constants::period()));
    }

    static inline void apply(CoordinateType& longitude1,
                             CoordinateType& latitude1,
                             CoordinateType& longitude2,
                             CoordinateType& latitude2)
    {
        bool const band = is_band(longitude1, longitude2);

        apply(longitude1, latitude1, longitude2, latitude2, band);
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Short utility to normalize the coordinates of a box on a spheroid
\tparam Units The units of the coordindate system in the spheroid
\tparam CoordinateType The type of the coordinates
\param longitude1 Minimum longitude of the box
\param latitude1 Minimum latitude of the box
\param longitude2 Maximum longitude of the box
\param latitude2 Maximum latitude of the box
\ingroup utility
*/
template <typename Units, typename CoordinateType>
inline void normalize_spheroidal_box_coordinates(CoordinateType& longitude1,
                                                 CoordinateType& latitude1,
                                                 CoordinateType& longitude2,
                                                 CoordinateType& latitude2)
{
    detail::normalize_spheroidal_box_coordinates
        <
            Units, CoordinateType
        >::apply(longitude1, latitude1, longitude2, latitude2);
}

template <typename Units, bool IsEquatorial, typename CoordinateType>
inline void normalize_spheroidal_box_coordinates(CoordinateType& longitude1,
                                                 CoordinateType& latitude1,
                                                 CoordinateType& longitude2,
                                                 CoordinateType& latitude2)
{
    detail::normalize_spheroidal_box_coordinates
        <
            Units, CoordinateType, IsEquatorial
        >::apply(longitude1, latitude1, longitude2, latitude2);
}

/*!
\brief Short utility to normalize the coordinates of a box on a spheroid
\tparam Units The units of the coordindate system in the spheroid
\tparam CoordinateType The type of the coordinates
\param longitude1 Minimum longitude of the box
\param latitude1 Minimum latitude of the box
\param longitude2 Maximum longitude of the box
\param latitude2 Maximum latitude of the box
\param band Indicates whether the box should be treated as a band or
       not and avoid the computation done in the other version of the function
\ingroup utility
*/
template <typename Units, typename CoordinateType>
inline void normalize_spheroidal_box_coordinates(CoordinateType& longitude1,
                                                 CoordinateType& latitude1,
                                                 CoordinateType& longitude2,
                                                 CoordinateType& latitude2,
                                                 bool band)
{
    detail::normalize_spheroidal_box_coordinates
        <
            Units, CoordinateType
        >::apply(longitude1, latitude1, longitude2, latitude2, band);
}

template <typename Units, bool IsEquatorial, typename CoordinateType>
inline void normalize_spheroidal_box_coordinates(CoordinateType& longitude1,
                                                 CoordinateType& latitude1,
                                                 CoordinateType& longitude2,
                                                 CoordinateType& latitude2,
                                                 bool band)
{
    detail::normalize_spheroidal_box_coordinates
        <
            Units, CoordinateType, IsEquatorial
        >::apply(longitude1, latitude1, longitude2, latitude2, band);
}

} // namespace math

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_NORMALIZE_SPHEROIDAL_BOX_COORDINATES_HPP


/***************************************************************************************************************************
 * circular.cpp
 *
 * Author: Ydm
 *
 * Update Time: 2020.8.14
 *
 * descripation: demostration of circular flying using mavros
 *      1.
 *      2.
 *      3.
***************************************************************************************************************************/

#include <ros/ros.h>
#include <fstream>
#include <math.h>
#include <cmath>
#include <string>
#include <time.h>
#include <queue>
#include <vector>
#include <cstdlib>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <iostream>
#include <math_utils.h>
#include <Eigen/Eigen>

#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/Imu.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <nlink_parser/LinktrackNode2.h>		//target information from Ground station

//#include <circular.h>

using namespace std;

float desire_z = 10.0;				//desired altitude
float desire_Radius = 10.0;		//desired radius of circle
float MoveTimeCnt = 0.0;
float priod = 2000.0;			//to change velocity of flying using it

float DESIRE_V = 10;

Eigen::Vector3d P_T0 = {200, 100, 50};		//initial position of Target
Eigen::Vector3d P_M0 = {0, 0, 5};			//initial position of Missile
Eigen::Vector3d V_T = {-3, 0, 0};			//velocity of Target
Eigen::Vector3d V_M = {0, 0, 0};
float VM = 1;						//velocity of Missile

float K1 = 4;						//proportion of guidance
float K2 = 4;

double sita = 0.0;
double psi_v = 0.0;
double phi = 0;
double psi = 0;
double gamma_vehicle = 0;

mavros_msgs::State current_state;
mavros_msgs::State target_state;
Eigen::Vector3d position_get;
Eigen::Vector3d attitude_get;
Eigen::Vector3d velocity_get;
Eigen::Vector3d att_rate_get;

Eigen::Vector3d pos_target; 	//desired value in offboard mode
Eigen::Vector3d pos_drone;
Eigen::Vector3d temp_pos_drone;
Eigen::Vector3d temp_pos_target;
Eigen::Vector3d velocity_sp;
Eigen::Vector2d Actuator_sp;

mavros_msgs::SetMode mode_cmd;
ros::Publisher setpoint_raw_local_pub;
ros::Publisher actuator_setpoint_pub;
ros::ServiceClient set_mode_client;

enum
{
	WATTING,
	CHECKING,
	PREPARE,
	REST,
	FLY,
	FLYOVER,
}FlyState = WATTING;		//the initial state

Eigen::Matrix3d L;
Eigen::Vector3d P_r;
Eigen::Vector3d P_T = P_T0;
Eigen::Vector3d P_M = P_M0;
double x_r = 1;
double y_r = 1;
double z_r = 1;
double x_r_last = 0;
double y_r_last = 0;
double z_r_last = 0;
double q_lambda = 0;
double q_gamma = 0;
double q_lambda_last = 0;
double q_gamma_last= 0;
double q_lambda_delta = 0;
double q_gamma_delta = 0;
double delta_sita = 0.0;
double delta_psi_v = 0.0;
double yaw_target = 0.0;

//receive the current position of drone from controller

void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
	current_state = *msg;
}

float str2float(string str)
{
  float result;
  stringstream stream(str);
  stream >> result;
  return result;
}

void target_cb(const std_msgs::String::ConstPtr& msg)
{
  int num = msg->data.size();
  P_T[0] = str2float(msg->data.substr(8,15));
  P_T[1] = str2float(msg->data.substr(18,25));
  P_T[2] = str2float(msg->data.substr(28,35));

  P_M[0] = str2float(msg->data.substr(43,50));
  P_M[1] = str2float(msg->data.substr(53,60));
  P_M[2] = str2float(msg->data.substr(63,70));
  //东北天坐标系
  //printf("the size of string is %d\n",num);
  //printf("%f\n", P_M[0]);
  //printf("%f\n", P_M[1]);
  //printf("%f\n", P_M[2]);
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	//read the drone postion from the Mavros Package [Frame: ENU]
	Eigen::Vector3d pos_drone_fcu_enu(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);

	pos_drone = pos_drone_fcu_enu;

}

void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
{

    velocity_get[0] = msg->twist.linear.x;
    velocity_get[1] = msg->twist.linear.y;
	velocity_get[2] = msg->twist.linear.z;
}

void att_cb(const sensor_msgs::Imu::ConstPtr& msg)
{
	Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
      //Transform the Quaternion to euler Angles
      Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu);

      attitude_get = euler_fcu;

	att_rate_get[0] = msg->angular_velocity.x;
    att_rate_get[1] = msg->angular_velocity.y;
    att_rate_get[2] = msg->angular_velocity.z;

 }

//send the desired position to controller (vx, vy, vz)
void send_vel_setpoint(const Eigen::Vector3d& vel_sp)
{
	mavros_msgs::PositionTarget pos_setpoint;
	pos_setpoint.type_mask = 0b110111000111;	//0b 110 111 000 111  velocity

	//Bitmask to indicate which dimensions should be ignored (1 means ignore, 0 means not ignore; Bit 10 must set to 0)
	//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7: ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11: yaw, bit 12:yaw_rate
	//Bit 10 should set to 0, means no force sp

	pos_setpoint.coordinate_frame = 1;

	pos_setpoint.velocity.x = vel_sp[0];
	pos_setpoint.velocity.y = vel_sp[1];
	pos_setpoint.velocity.z = vel_sp[2];

	pos_setpoint.yaw = yaw_target;

	setpoint_raw_local_pub.publish(pos_setpoint);
}

//send the desired position to controller (xyz, yaw)
void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
{
	mavros_msgs::PositionTarget pos_setpoint;
	pos_setpoint.type_mask = 0b100111111000;	//0b 100 111 111 000 xyz + yaw

	//Bitmask to indicate which dimensions should be ignored (1 means ignore, 0 means not ignore; Bit 10 must set to 0)
	//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7: ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11: yaw, bit 12:yaw_rate
	//Bit 10 should set to 0, means no force sp

	pos_setpoint.coordinate_frame = 1;

	pos_setpoint.position.x = pos_sp[0];
	pos_setpoint.position.y = pos_sp[1];
	pos_setpoint.position.z = pos_sp[2];

	pos_setpoint.yaw = yaw_sp;

	setpoint_raw_local_pub.publish(pos_setpoint);
}

// 【发布】底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力 注意 这里是NED系的！！
void send_actuator_setpoint(const Eigen::Vector2d& actuator_sp)
{
    mavros_msgs::ActuatorControl actuator_setpoint;

    actuator_setpoint.group_mix = 0;
    actuator_setpoint.controls[0] = 0.0;
    actuator_setpoint.controls[1] = 0.0;
    actuator_setpoint.controls[2] = actuator_sp(1);		//偏航
    actuator_setpoint.controls[3] = actuator_sp(0);		//油门
    actuator_setpoint.controls[4] = 0.0;
    actuator_setpoint.controls[5] = 0.0;
    actuator_setpoint.controls[6] = 0.0;
    actuator_setpoint.controls[7] = 0.0;

    actuator_setpoint_pub.publish(actuator_setpoint);

    // // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // //ned to enu
    // cout << "actuator_target [0 1 2 3] : " << actuator_target.controls[0] << " [ ] "<< -actuator_target.controls[1] <<" [ ] "<<-actuator_target.controls[2]<<" [ ] "<<actuator_target.controls[3] <<" [ ] "<<endl;

    // cout << "actuator_target [4 5 6 7] : " << actuator_target.controls[4] << " [ ] "<< actuator_target.controls[5] <<" [ ] "<<actuator_target.controls[6]<<" [ ] "<<actuator_target.controls[7] <<" [ ] "<<endl;

}
void Guidance_Update(void)
{

	P_r = P_T - P_M;
	x_r = P_r[0];
	y_r = P_r[1];
	z_r = P_r[2];

	psi = atan2(y_r , x_r);			//psi is defined to the angle of T-M line and x axis(East orientation)
	double yaw_fcu = attitude_get[2];		//实际偏航角 东北天坐标系 【-pi - pi】
	//yaw_target = psi + M_PI * 3 / 2;
	//if (yaw_target > 2 * M_PI)
	//{
	//	yaw_target = yaw_target - 2 * M_PI;
	//}
	yaw_target = psi;
	//double yaw_control = -10 * (yaw_target - yaw_fcu) / 2 / M_PI;
	//Actuator_sp[0] = 0.8;				//throttle
	//Actuator_sp[1] = yaw_control;		//yaw
	cout << "psi = " << psi << endl;
	cout << "yaw_fcu = " << yaw_fcu << endl;
	cout << "yaw_target = " << yaw_target << endl;
	//cout << "yaw_control = " << yaw_control << endl;
	V_M << VM * cos(psi), VM * sin(psi), 0;

}

void FlyState_update(void)
{
	switch(FlyState)
	{
		case WATTING:
			if(current_state.mode != "OFFBOARD")	//waitting for offboard mode
			{
				pos_target[0] = pos_drone[0];
				pos_target[1] = pos_drone[1];
				pos_target[2] = pos_drone[2];
				temp_pos_drone[0] = pos_drone[0];
				temp_pos_drone[1] = pos_drone[1];
				temp_pos_drone[2] = pos_drone[2];
				send_pos_setpoint(pos_target, 0);
			}
			else
			{
				pos_target[0] = temp_pos_drone[0];
				pos_target[1] = temp_pos_drone[1];
				pos_target[2] = temp_pos_drone[2];
				send_pos_setpoint(pos_target, 0);
				FlyState = CHECKING;
			}
			//cout << "WATTING" << endl;
			break;

		case CHECKING:
			if(pos_drone[0] == 0 && pos_drone[1] == 0)	//no position information, land
			{
				cout << "Check error, make sure have local location" <<endl;
				mode_cmd.request.custom_mode = "AUTO.LAND";
				set_mode_client.call(mode_cmd);
				FlyState = WATTING;
			}
			else
			{
				FlyState = PREPARE;
				MoveTimeCnt = 0;
			}
			//cout << "CHECKING" << endl;
			break;
		case PREPARE:									//fly to the first point located in x negative axis

			FlyState = REST;
			//cout << "PREPARE" << endl;
			break;
		case REST:
			pos_target[0] = pos_drone[0];				//fly to 10 m
			pos_target[1] = pos_drone[1];
			pos_target[2] = desire_z;
			send_pos_setpoint(pos_target, 0);
			MoveTimeCnt += 1;
			if(MoveTimeCnt >= 100)
			{
				MoveTimeCnt = 0;
				FlyState = FLY;
			}
			if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
			{
				FlyState = WATTING;
			}
			//cout << "REST" << endl;
			break;
		case FLY:
			{

				//Guidance_Update();
				//velocity_sp = V_M;
				velocity_sp = {0.4,0.4,0};
				//send_actuator_setpoint(Actuator_sp);
				send_vel_setpoint(velocity_sp);
				//cout << "I am here!!! " << endl;
				if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
				{
					FlyState = WATTING;
				}
				if(abs(P_r[0]*P_r[0] + P_r[1]*P_r[1] + P_r[2]*P_r[2]) < 0.5)
				{
					cout << "I hit it !!!" << endl;
					FlyState = FLYOVER;
				}
			}
			//cout << "FLY" << endl;
			break;
		case FLYOVER:
			{
				mode_cmd.request.custom_mode = "AUTO.LAND";
				set_mode_client.call(mode_cmd);
				FlyState = WATTING;
			}
			//cout << "FLYOVER" << endl;
			break;
		default:
			cout << "ERROR!!!" << endl;
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "circular_offboard");
	ros::NodeHandle nh;

   	ros::Subscriber target_sub = nh.subscribe<std_msgs::String>("/nlink_linktrack_data_transmission", 100, target_cb);
	ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, state_cb);			//handle return function
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, pos_cb);
	ros::Subscriber attitude_sub = nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data", 10, att_cb);

	setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
	actuator_setpoint_pub = nh.advertise<mavros_msgs::ActuatorControl>("/mavros/actuator_control", 10);

	set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");			//little question
		nh.param<float>("desire_z", desire_z, 10.0);
		nh.param<float>("desire_Radius", desire_Radius, 10.0);

	cout << "guidance node started!!!" << endl;
	ros::Rate rate(20.0);
	while(ros::ok())
	{

		FlyState_update();
		Guidance_Update();
		ros::spinOnce();
		rate.sleep();

		cout << "P_T=" << "\t" << "\t" << "P_M="<< "\t"<< endl;
		cout << "\t" << P_T[0] << "\t" << P_M[0] << endl;
		cout << "\t" << P_T[1] << "\t" << P_M[1] << endl;
		cout << "\t" << P_T[2] << "\t" << P_M[2] << endl;

		cout << "vx = " << V_M[0] << endl;
		cout << "vy = " << V_M[1] << endl;
		cout << "vz = " << V_M[2] << endl;

	}
	return 0;
}