//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_IS_NEGATIVE_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_IS_NEGATIVE_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

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

     Represents the is_negative function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct is_negative_ : ext::elementwise_<is_negative_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<is_negative_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_is_negative_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };}
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::is_negative_, Site> dispatching_is_negative_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::is_negative_, Site>();
   }
   template<class... Args>
   struct impl_is_negative_;
  }
  /*!
    Returns True if a0 is negative else False.

    This function differs from is_ltz from floating point argument,
    because Mzero is negative but not less than zero.  and Mzero is
    not positive and not greater than zero, It's probably is_ltz that
    you want.

    @par Semantic:

    @code
    logical<T> r = is_negative(a0);
    @endcode

    is similar to:

    @code
    if T is signed
      logical<T> r = bitofsign(a0) == 1;
    else
      logical<T> r = False;
    @endcode

    @par Note:

    Mzero is the floating point 'minus zero',
    i.e. all bits are zero but the sign bit.
    Such a value is treated as zero by ieee standards.

    @param a0

    @return a logical value
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::is_negative_, is_negative, 1)
} }

#endif


/**
 * @brief  ねじれヒープ
 */

#ifndef SKEW_HEAP_HPP
#define SKEW_HEAP_HPP

#include <algorithm>
#include <boost/assert.hpp>
#include <boost/container/pmr/polymorphic_allocator.hpp>
#include <cassert>
#include <memory>
#include <optional>

namespace container {

template <class Key> struct skew_heap_node {
  skew_heap_node *l = nullptr; /**< 左の子 */
  skew_heap_node *r = nullptr; /**< 右の子 */
  Key key;                     /**< キー */
  template <class... Args>
  constexpr explicit skew_heap_node(Args &&... args) noexcept
      : key(std::forward<Args>(args)...) {}
};

/**
 * @brief  ねじれヒープ
 * @tparam Key     キーの型
 * @tparam Compare 比較述語の型
 */
template <class Key, class Compare = std::less<Key>,
          class Allocator =
              boost::container::pmr::polymorphic_allocator<skew_heap_node<Key>>>
struct skew_heap {
public:
  static_assert(std::is_nothrow_constructible_v<Key>);
  using alloc = std::allocator_traits<Allocator>;
  using node = skew_heap_node<Key>;

  explicit skew_heap(std::size_t n = 32) { allocate_pool(n); }
  ~skew_heap() noexcept { free_pool(); }

  /** @brief ねじれヒープHに要素xを挿入する @param const Key& key 要素xのキー */
  template <class... Args> void push(Args &&... args) {
    node *x = create_node(std::forward<Args>(args)...);
    root_ = merge(root_, x);
  }

  /**< @brief ねじれヒープHから先頭のキーを取り出し、要素を削除する */
  std::optional<Key> pop() noexcept {
    if (empty()) {
      return std::nullopt;
    }
    decltype(auto) k = std::move(root_->key);
    node *x = root_;
    root_ = merge(x->l, x->r);
    destroy_node(x);
    return std::make_optional(k);
  }

  /**< @brief ねじれヒープHが空かどうか返す */
  constexpr bool empty() const noexcept { return root_ == nullptr; }

  /**< @brief ねじれヒープHが満杯かどうか返す */
  constexpr bool full() const noexcept { return size_ >= cap_; }

private:
  /**
   * @brief  節点xとyをマージする
   * @note   計算量はならし時間でΟ(lgn)
   * @param  node* x  節点x
   * @param  node* y  節点y
   * @return 新しい部分木の根
   */
  node *merge(node *x, node *y) {
    if (x == nullptr) {
      return y;
    } // xがNILならば、yを返す
    if (y == nullptr) {
      return x;
    } // yがNILならば、xを返す
    if (!cmp_(x->key, y->key)) {
      std::swap(x, y);
    } // x.key > y.keyならば、x.key < y.keyになるよう交換
    x->r = merge(x->r, y); // xの右の子とyをマージし、
    std::swap(x->l, x->r); // xの左の子と右の子を交換する(leftist)
    return x;              // 新しい部分木の根xを返す
  }

  /**< @brief 節点xの記憶領域の確保を行う */
  template <class... Args> node *create_node(Args &&... args) {
    BOOST_ASSERT_MSG(!full(), "Skew heap capcity over.");
    node *x = pool_ + size_;
    alloc::construct(alloc_, x, std::forward<Args>(args)...);
    size_++;
    return x;
  }

  /**< @brief 節点xの記憶領域の解放を行う */
  void destroy_node(node *x) noexcept {
    alloc::destroy(alloc_, x);
    size_--;
  }

  /**< @brief 節点xを根とした部分木を再帰的に解放する */
  void postorder_destroy_nodes(node *x) noexcept {
    if (x == nullptr) {
      return;
    }
    postorder_destroy_nodes(x->l);
    postorder_destroy_nodes(x->r);
    destroy_node(x);
  }

  /**< @brief メモリプールの解放 */
  void free_pool() noexcept {
    postorder_destroy_nodes(root_);
    alloc::deallocate(alloc_, pool_, cap_);
    root_ = pool_ = nullptr;
    size_ = cap_ = 0;
  }

  /**< @brief メモリプールの確保 */
  void allocate_pool(std::size_t n) {
    pool_ = alloc::allocate(alloc_, n);
    cap_ = n;
  }

private:
  node *root_ = nullptr; /**< 木の根   */
  std::size_t cap_ = 0;  /**< ねじれヒープのバッファサイズ */
  std::size_t size_ = 0; /**< ねじれヒープのサイズ */
  node *pool_ = nullptr; /**< メモリプールへのポインタ */
  Compare cmp_;          /**< 比較述語 */
  Allocator alloc_;      /**< アロケータ */
};

} // namespace container

#endif // end if SKEW_HEAP_HPP


/* ----------------------------------------------------------------------------
 * Copyright 2017, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Luca Carlone, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file   Camera.h
 * @brief  Class describing a monocular camera
 * @author Antoni Rosinol
 */

#include "kimera-vio/frontend/Camera.h"

#include <Eigen/Core>

#include <opencv2/core.hpp>

#include <gtsam/geometry/Point2.h>

#include "kimera-vio/frontend/CameraParams.h"
#include "kimera-vio/utils/Macros.h"

namespace VIO {

Camera::Camera(const CameraParams& cam_params)
    : camera_impl_(),
      calibration_(cam_params.intrinsics_.at(0),
                   cam_params.intrinsics_.at(1),
                   0.0,  // No skew
                   cam_params.intrinsics_.at(2),
                   cam_params.intrinsics_.at(3)),
      cam_params_(cam_params) {}

void Camera::project(const LandmarksCV& lmks, KeypointsCV* kpts) const {
  CHECK_NOTNULL(kpts)->clear();
  const auto& n_lmks = lmks.size();
  kpts->reserve(n_lmks);
  for (const auto& lmk : lmks) {
    const gtsam::Point2& kp =
        camera_impl_.project2(gtsam::Point3(lmk.x, lmk.y, lmk.z));
    kpts->push_back(KeypointCV(kp.x(), kp.y()));
  }
}

void Camera::backProject(const KeypointsCV& kps,
                         const double& depth,
                         LandmarksCV* lmks) const {
  CHECK_NOTNULL(lmks)->clear();
  lmks->reserve(kps.size());
  for (const KeypointCV& kp : kps) {
    gtsam::Point2 z(kp.x, kp.y);
    gtsam::Point3 lmk = camera_impl_.backproject(z, depth);
    lmks->push_back(LandmarkCV(lmk.x(), lmk.y(), lmk.z()));
  }
}

}  // namespace VIO


//=========================================================================
//  Copyright (c) Kitware, Inc.
//  All rights reserved.
//  See LICENSE.txt for details.
//
//  This software is distributed WITHOUT ANY WARRANTY; without even
//  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
//  PURPOSE.  See the above copyright notice for more information.
//=========================================================================
#include "vtkArcDepressFilter.h"

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkExecutive.h"
#include "vtkFloatArray.h"
#include "vtkGenericCell.h"
#include "vtkImplicitFunction.h"
#include "vtkIncrementalPointLocator.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkIntersectionPolyDataFilter.h"
#include "vtkKochanekSpline.h"
#include "vtkLine.h"
#include "vtkMergePoints.h"
#include "vtkObjectFactory.h"
#include "vtkPiecewiseFunction.h"
#include "vtkPlaneSource.h"
#include "vtkPointData.h"
#include "vtkPolyData.h"
#include "vtkSmartPointer.h"
#include "vtkTetra.h"
#include "vtkTransform.h"
#include "vtkTriangle.h"
#include <vtkPolyDataNormals.h>

#include "vtkCMBApplyBathymetryFilter.h"

#include <vtkTriangleFilter.h>

#include <algorithm>
#if defined(__GNUC__) && (__GNUC__ > 4) && (__cplusplus)
#include <cmath>
using std::isnan;
#else
#include <math.h>
#endif
#include <cassert>
#include <limits>

#include <boost/shared_ptr.hpp>

vtkStandardNewMacro(vtkArcDepressFilter);

class ArcDepressFunction
{
public:
  ArcDepressFunction() {}
  virtual ~ArcDepressFunction() {}
  virtual double evaluate(double d, double minF, double maxF) = 0;
  virtual double evaluate(double d) = 0;
  virtual void addPoint(double w, double v, double s, double m) = 0;
  virtual void clearPoints() = 0;
  virtual ArcDepressFunction* clone() const = 0;
};

class MixArcDepressFunction : public ArcDepressFunction
{
public:
  MixArcDepressFunction(ArcDepressFunction* f0, ArcDepressFunction* f1, double m)
    : fun0(f0)
    , fun1(f1)
    , mix(m)
  {
    assert(mix >= 0 && mix <= 1);
  }

  ~MixArcDepressFunction() override
  {
    delete fun0;
    delete fun1;
  }

  double evaluate(double d, double minF, double maxF) override
  {
    return (1 - mix) * fun0->evaluate(d, minF, maxF) + (mix)*fun1->evaluate(d, minF, maxF);
  }

  double evaluate(double d) override
  {
    return (1 - mix) * fun0->evaluate(d) + (mix)*fun1->evaluate(d);
  }

  void addPoint(
    double vtkNotUsed(w), double vtkNotUsed(v), double vtkNotUsed(s), double vtkNotUsed(m)) override
  {
    assert(false && "Do not add point on mixing function");
  }

  void clearPoints() override { assert(false && "Do not clear point on mixing function"); }

  ArcDepressFunction* clone() const override
  {
    return new MixArcDepressFunction(fun0->clone(), fun1->clone(), mix);
  }

  ArcDepressFunction *fun0, *fun1;
  double mix;
};

class ArcDepressPiecewiseFun : public ArcDepressFunction
{
public:
  ArcDepressPiecewiseFun()
  {
    fun = vtkPiecewiseFunction::New();
    fun->SetAllowDuplicateScalars(1);
  }
  ~ArcDepressPiecewiseFun() override { fun->Delete(); }
  double evaluate(double d, double minF, double maxF) override
  {
    double wd = this->evaluate(d);
    return (1 - wd) * minF + (wd)*maxF;
  }
  double evaluate(double d) override { return fun->GetValue(d); }
  void addPoint(double w, double v, double s, double m) override { fun->AddPoint(w, v, s, m); }
  void clearPoints() override { fun->RemoveAllPoints(); }

  ArcDepressFunction* clone() const override
  {
    ArcDepressPiecewiseFun* result = new ArcDepressPiecewiseFun();
    result->fun->DeepCopy(fun);
    return result;
  }

  vtkPiecewiseFunction* fun;
};

class ArcDepressSplineFun : public ArcDepressFunction
{
public:
  ArcDepressSplineFun()
  {
    fun = vtkKochanekSpline::New();
    computed = false;
  }
  ~ArcDepressSplineFun() override { fun->Delete(); }
  double evaluate(double d, double minF, double maxF) override
  {
    if (!computed)
    {
      fun->Compute();
      computed = true;
    }

    double wd = fun->Evaluate(d);
    double dist = maxF - minF;
    return minF + (wd)*dist;
  }

  double evaluate(double d) override
  {
    if (!computed)
    {
      fun->Compute();
      computed = true;
    }
    double tmp = fun->Evaluate(d);
    if (tmp < 0)
      tmp = 0;
    else if (tmp > 1)
      tmp = 1;
    return tmp;
  }

  void addPoint(double w, double v, double /*s*/, double /*m*/) override
  {
    if (fun->GetNumberOfPoints() != 0 && prev == w)
    {
      w += 0.000001;
    }
    fun->AddPoint(w, v);
    prev = w;
  }

  ArcDepressFunction* clone() const override
  {
    ArcDepressSplineFun* result = new ArcDepressSplineFun();
    result->fun->DeepCopy(fun);
    return result;
  }

  void clearPoints() override
  {
    fun->RemoveAllPoints();
    computed = false;
  }
  double prev;
  bool computed;
  vtkKochanekSpline* fun;
};

class DepArcMixProfileFunction;
class DepArcWedgeProfileFunction;

class DepArcProfileFunction
{
public:
  enum FunctionType
  {
    Piecewise = 1,
    Spline = 0
  };
  enum Type
  {
    Manual,
    Mix,
    Wedge
  };
  virtual ~DepArcProfileFunction() {}
  virtual Type getType() const = 0;
  virtual double apply(double d, double pt, double& dir) const = 0;
  virtual double apply(double d, double* pt, double* n) const = 0;
  virtual void addWeightPoint(double w, double v, double s, double m) = 0;
  virtual bool inside(double d) const = 0;
  virtual void setFunctionRange(double /*minZ*/, double /*maxZ*/) {}
};

class DepArcMixProfileFunction : public DepArcProfileFunction
{
public:
  DepArcMixProfileFunction(boost::shared_ptr<DepArcProfileFunction> fun0,
    boost::shared_ptr<DepArcProfileFunction> fun1, double m)
    : mix(m)
  {
    assert(fun0 != NULL);
    assert(fun1 != NULL);
    fun[0] = fun0;
    fun[1] = fun1;
    assert(mix > 0 && mix < 1.0);
  }

  DepArcProfileFunction::Type getType() const override { return DepArcProfileFunction::Mix; }

  double apply(double d, double pt, double& dir) const override
  {
    double d1, d2;
    double p1 = fun[0]->apply(d, pt, d1);
    double p2 = fun[1]->apply(d, pt, d2);
    dir = d1 * (1 - mix) + d2 * mix;
    return p1 * (1 - mix) + p2 * mix;
  }

  double apply(double d, double* pt, double* n) const override
  {
    double pta[3], ptb[3];
    double dra = fun[0]->apply(d, pta, n);
    double drb = fun[1]->apply(d, ptb, n);
    double t = mix;
    double tm1 = 1 - t;
    pt[0] = tm1 * pta[0] + t * ptb[0];
    pt[1] = tm1 * pta[1] + t * ptb[1];
    pt[2] = tm1 * pta[2] + t * ptb[2];
    return tm1 * dra + t * drb;
  }

  bool inside(double d) const override { return fun[0]->inside(d) || fun[1]->inside(d); }
  void setFunctionRange(double minZ, double maxZ) override
  {
    fun[0]->setFunctionRange(minZ, maxZ);
    fun[1]->setFunctionRange(minZ, maxZ);
  }

  void addWeightPoint(
    double vtkNotUsed(w), double vtkNotUsed(v), double vtkNotUsed(s), double vtkNotUsed(m)) override
  {
    assert(false && " do not use for the mixing function");
  }

  boost::shared_ptr<DepArcProfileFunction> fun[2];
  double mix;
};

class DepArcWedgeProfileFunction : public DepArcProfileFunction
{
public:
  enum
  {
    Left = 0,
    Right = 1
  };
  enum DisplacementMode
  {
    Dig = 1,
    Raise = 2,
    Level = 3
  };
  DepArcWedgeProfileFunction(DepArcProfileFunction::FunctionType weightFun, bool r,
    DisplacementMode d, bool c, double bw, double disp, double sl, double sr)
    : baseWidth(bw)
    , displacement(disp)
    , relative((r) ? 1 : 0)
    , clamp(c)
    , dispMode(d)
  {
    if (relative)
    {
      maxWidth[Right] = (sr == 0) ? bw : std::abs(bw) + std::abs(disp / sr);
      maxWidth[Left] = (sl == 0) ? bw : std::abs(bw) + std::abs(disp / sl);
    }
    slope[Right] = sr; //:-sr;
    slope[Left] = sl;  //:-sl;
    //maxWidth[Right] = maxwR;
    //maxWidth[Left] = maxwL;
    weightFuntion = (weightFun == DepArcProfileFunction::Piecewise)
      ? dynamic_cast<ArcDepressFunction*>(new ArcDepressPiecewiseFun())
      : dynamic_cast<ArcDepressFunction*>(new ArcDepressSplineFun());
    assert(!relative ||
      (relative && ((dispMode == Dig && disp < 0) || (dispMode == Raise && disp > 0))));
  }
  ~DepArcWedgeProfileFunction() override { delete weightFuntion; }
  DepArcProfileFunction::Type getType() const override { return DepArcProfileFunction::Wedge; }

  bool evalFun(double d, double& weight, double& despDig, double& despRaise) const
  {
    double sign = 1.0;
    unsigned int side = static_cast<unsigned int>(Right);
    if (d < 0)
    {
      sign = -1;
      side = static_cast<unsigned int>(Left);
    }
    if (sign * d > maxWidth[side])
      return false;
    weight = weightFuntion->evaluate(d / maxWidth[side]);
    despDig = despRaise = displacement;

    if (sign * d >= 0.5 * baseWidth)
    {
      double p1 = (sign * d - 0.5 * baseWidth) * slope[side];
      despDig += p1;
      despRaise += -p1;
    }
    return true;
  }

  double apply(double d, double pt, double& dirOut) const override
  {
    dirOut = 0;
    double w, tmpD, tmpR;
    if (!evalFun(d, w, tmpD, tmpR))
      return pt;
    double absolute = (1 - relative); // 1 or 0
    double dir[] = { tmpD - pt * absolute, tmpR - pt * absolute };
    double ptW = (1 - w * absolute);

    if (dispMode & Dig && !(clamp && dir[0] > 0))
    {
      dirOut += dir[0];
      pt = ptW * pt + w * tmpD;
    }
    if (dispMode & Raise && !(clamp && dir[1] < 0))
    {
      dirOut += dir[1];
      pt = ptW * pt + w * tmpR;
    }
    return pt;
  }

  double apply(double d, double* pt, double* n) const override
  {
    assert(relative && "Normal direction only makes sense in relative");
    double w, tmpD, tmpR;
    if (!evalFun(d, w, tmpD, tmpR))
      return 0;
    double wT[] = { (clamp && tmpD > 0) ? 0 : w * tmpD, (clamp && tmpR < 0) ? 0 : w * tmpR };
    double r = 0;
    if (dispMode & Dig)
    {
      pt[0] += wT[0] * n[0];
      pt[1] += wT[0] * n[1];
      pt[2] += wT[0] * n[2];
      r += wT[0];
    }
    if (dispMode & Raise)
    {
      pt[0] += wT[1] * n[0];
      pt[1] += wT[1] * n[1];
      pt[2] += wT[1] * n[2];
      r += wT[1];
    }
    return r;
  }

  void addWeightPoint(double w, double v, double s, double m) override
  {
    weightFuntion->addPoint(w, v, s, m);
  }

  bool inside(double d) const override
  {
    if (d < 0)
      return -d <= maxWidth[Left];
    return d <= maxWidth[Right];
  }

  DepArcProfileFunction* interpolate(DepArcWedgeProfileFunction* other, double t)
  {
    assert(other->weightFuntion != NULL);
    assert(this->weightFuntion != NULL);
    assert(t <= 1.0 && t >= 0);
    assert(this->relative == other->relative);
    DepArcWedgeProfileFunction* result = new DepArcWedgeProfileFunction();
    result->baseWidth = (1 - t) * this->baseWidth + t * other->baseWidth;
    result->displacement = (1 - t) * this->displacement + t * other->displacement;
    double thisS[] = { (this->slope[0] == 0) ? 0 : 1 / this->slope[0],
      (this->slope[1] == 0) ? 0 : 1 / this->slope[1] };
    double otherS[] = { (other->slope[0] == 0) ? 0 : 1 / other->slope[0],
      (other->slope[1] == 0) ? 0 : 1 / other->slope[1] };
    result->slope[0] = ((1 - t) * std::abs(thisS[0]) + t * std::abs(otherS[0]));
    result->slope[1] = ((1 - t) * std::abs(thisS[1]) + t * std::abs(otherS[1]));
    if (result->slope[0] != 0)
      result->slope[0] = 1.0 / result->slope[0];
    if (result->slope[1] != 0)
      result->slope[1] = 1.0 / result->slope[1];
    if (this->relative && other->relative)
    {
      result->maxWidth[Left] = result->baseWidth * 0.5;
      if (result->slope[Left] != 0)
      {
        result->maxWidth[Left] =
          std::abs(result->maxWidth[Left]) + std::abs(result->displacement / result->slope[Left]);
      }
      result->maxWidth[Right] = result->baseWidth * 0.5;
      if (result->slope[Right] != 0)
      {
        result->maxWidth[Right] =
          std::abs(result->maxWidth[Right]) + std::abs(result->displacement / result->slope[Right]);
      }
      result->dispMode = (result->displacement < 0) ? Dig : Raise;
    }
    else
    {
      result->maxWidth[0] = (1 - t) * this->maxWidth[0] + t * other->maxWidth[0];
      result->maxWidth[1] = (1 - t) * this->maxWidth[1] + t * other->maxWidth[1];
      result->dispMode = (t < 0.5) ? this->dispMode : other->dispMode;
    }
    result->weightFuntion =
      new MixArcDepressFunction(this->weightFuntion->clone(), other->weightFuntion->clone(), t);
    result->relative = this->relative;
    result->clamp = this->clamp;
    if (relative == other->relative && clamp == other->clamp && dispMode == other->dispMode)
    {
      assert(result->weightFuntion != NULL);
      return result;
    }
    else if (relative == other->relative && clamp == other->clamp && dispMode != other->dispMode)
    {
      if (t >= 0.5)
      {
        result->clamp = other->clamp;
      }
      assert(result->weightFuntion != NULL);
      return result;
    }
    DepArcWedgeProfileFunction* tmpO = new DepArcWedgeProfileFunction(result);
    tmpO->relative = other->relative;
    tmpO->clamp = other->clamp;
    tmpO->dispMode = other->dispMode;
    assert(result->weightFuntion != NULL);
    assert(tmpO->weightFuntion != NULL);
    return new DepArcMixProfileFunction(boost::shared_ptr<DepArcProfileFunction>(result),
      boost::shared_ptr<DepArcProfileFunction>(tmpO), t);
  }
  void setFunctionRange(double minZ, double maxZ) override
  {
    if (!relative)
    {
      double hBW = baseWidth * 0.5;
      for (int i = 0; i < 2; ++i)
      {
        if (this->slope[i] != 0)
        {
          this->maxWidth[i] =
            std::max(std::abs(hBW) + std::abs((minZ - displacement) / this->slope[i]),
              std::abs(hBW) + std::abs((maxZ - displacement) / this->slope[i]));
        }
      }
    }
  }

private:
  DepArcWedgeProfileFunction() {}
  DepArcWedgeProfileFunction(DepArcWedgeProfileFunction* other)
  {
    this->baseWidth = other->baseWidth;
    this->displacement = other->displacement;
    this->slope[0] = other->slope[0];
    this->slope[1] = other->slope[1];
    this->maxWidth[0] = other->maxWidth[0];
    this->maxWidth[1] = other->maxWidth[1];
    this->relative = other->relative;
    this->clamp = other->relative != 0;
    this->dispMode = other->dispMode;
    weightFuntion = other->weightFuntion->clone();
    assert(weightFuntion != NULL);
  }
  double baseWidth, displacement, slope[2], maxWidth[2];
  int relative;
  bool clamp;
  DisplacementMode dispMode;
  ArcDepressFunction* weightFuntion;
};

class DepArcManualProfileFunction : public DepArcProfileFunction
{
public:
  enum FunctionEnum
  {
    WeightFun = 0,
    DispFun = 1
  };
  enum range
  {
    MIN = 0,
    MAX = 1
  };

  DepArcManualProfileFunction(DepArcProfileFunction::FunctionType despType,
    DepArcProfileFunction::FunctionType weightFun, bool symmetric, bool relative)
    : IsSymmetric(symmetric)
    , IsRelative(relative)
  {
    SelectedFunction[DispFun] = (despType == DepArcProfileFunction::Piecewise)
      ? dynamic_cast<ArcDepressFunction*>(new ArcDepressPiecewiseFun())
      : dynamic_cast<ArcDepressFunction*>(new ArcDepressSplineFun());
    SelectedFunction[WeightFun] = (weightFun == DepArcProfileFunction::Piecewise)
      ? dynamic_cast<ArcDepressFunction*>(new ArcDepressPiecewiseFun())
      : dynamic_cast<ArcDepressFunction*>(new ArcDepressSplineFun());

    this->setMinMaxDistance(0, 0);
    this->setMinMaxDesplacementDepth(0, 0);
  }

  ~DepArcManualProfileFunction() override
  {
    delete SelectedFunction[0];
    delete SelectedFunction[1];
  }

  DepArcProfileFunction::Type getType() const override { return DepArcProfileFunction::Manual; }

  void addWeightPoint(double w, double v, double s, double m) override
  {
    addPoint(WeightFun, w, v, s, m);
  }

  void addPoint(FunctionEnum e, double w, double v, double s, double m)
  {
    SelectedFunction[e]->addPoint(w, v, s, m);
  }

  bool isSymmetric() const { return IsSymmetric; }

  bool evalFun(double d, double& weight, double& desp) const
  {
    if (IsSymmetric)
      d = std::abs(d);
    if (d < getMinDistance() || d > getMaxDistance())
    {
      return false;
    }
    if (d < 0)
    {
      d = d / -getMinDistance();
    }
    else //if( d >= 0)
    {
      d = d / getMaxDistance();
    }
    weight = SelectedFunction[WeightFun]->evaluate(d);
    desp = SelectedFunction[DispFun]->evaluate(d, getMinDepth(), getMaxDepth());
    return true;
  }

  double apply(double d, double pt, double& dir) const override
  {
    double w, tmp;
    dir = 0;
    if (!evalFun(d, w, tmp))
      return pt;
    double wTmp = w * tmp;
    int absolute = (IsRelative) ? 0 : 1;
    dir = wTmp - (1 - w) * pt * absolute;
    return (1 - w * absolute) * pt + wTmp;
    /*if(IsRelative)
    {
      return pt + wTmp;
    }
    else
    {
      return wTmp + w1*pt;
    }*/
  }

  double apply(double d, double* pt, double* n) const override
  {
    double w, tmp;
    assert(IsRelative && "Normal direction only makes sense in relative");
    if (!evalFun(d, w, tmp))
      return 0;
    pt[0] = pt[0] + w * tmp * n[0];
    pt[1] = pt[1] + w * tmp * n[1];
    pt[2] = pt[2] + w * tmp * n[2];
    return w * tmp;
  }

  double getMaxDistance() const { return MinMaxDist[MAX]; }

  double getMinDistance() const { return MinMaxDist[MIN]; }

  double getMaxDepth() const { return MinMaxDespDepth[MAX]; }

  double getMinDepth() const { return MinMaxDespDepth[MIN]; }

  void setMinMaxDesplacementDepth(double min, double max)
  {
    MinMaxDespDepth[MIN] = min;
    MinMaxDespDepth[MAX] = max;
  }

  void setMinMaxDistance(double left, double right)
  {
    MinMaxDist[MIN] = left;
    MinMaxDist[MAX] = right;
  }

  bool inside(double d) const override
  {
    assert(getMaxDistance() != 0);
    if (isSymmetric())
    {
      return std::abs(d) <= getMaxDistance();
    }
    return getMinDistance() <= d && d <= getMaxDistance();
  }

protected:
  bool IsSymmetric;
  bool IsRelative;

  ArcDepressFunction* SelectedFunction[2];
  double MinMaxDespDepth[2];
  double MinMaxDist[2];
};

namespace
{

boost::shared_ptr<DepArcProfileFunction> interpolate_functions(
  boost::shared_ptr<DepArcProfileFunction> fun0, boost::shared_ptr<DepArcProfileFunction> fun1,
  double weight)
{
  if (fun0 == fun1)
    return fun0;
  assert(fun0 != boost::shared_ptr<DepArcProfileFunction>());
  assert(fun1 != boost::shared_ptr<DepArcProfileFunction>());
  if (fun0->getType() != fun1->getType() || fun0->getType() == DepArcProfileFunction::Manual ||
    fun0->getType() == DepArcProfileFunction::Mix)
  {
    return boost::shared_ptr<DepArcProfileFunction>(
      new DepArcMixProfileFunction(fun0, fun1, weight));
  }
  DepArcWedgeProfileFunction* w0 = dynamic_cast<DepArcWedgeProfileFunction*>(fun0.get());
  DepArcWedgeProfileFunction* w1 = dynamic_cast<DepArcWedgeProfileFunction*>(fun1.get());
  return boost::shared_ptr<DepArcProfileFunction>(w0->interpolate(w1, weight));
}
}

class DepArcData
{
  friend class vtkArcDepressFilter;

private:
  DepArcData()
    : IsEnabled(true)
  {
  }

  ~DepArcData() { this->clear(); }

  struct line_seg;

  struct point
  {
    double pt[2];
    line_seg const* ls[2];

    void clearPointFunction() { pointFunction = boost::shared_ptr<DepArcProfileFunction>(); }

    void setFunction(boost::shared_ptr<DepArcProfileFunction> fun) { pointFunction = fun; }

    boost::shared_ptr<DepArcProfileFunction> getFunction() { return pointFunction; }

    point(point const& pin)
    {
      pt[0] = pin.pt[0];
      pt[1] = pin.pt[1];
      pointFunction = pin.pointFunction;
      ls[0] = pin.ls[0];
      ls[1] = pin.ls[1];
    }
    explicit point(double x = 0, double y = 0)
    {
      pt[0] = x;
      pt[1] = y;
      ls[0] = ls[1] = NULL;
    }
    ~point() { clearPointFunction(); }
    void operator=(point const& pin)
    {
      clearPointFunction();
      pt[0] = pin.pt[0];
      pt[1] = pin.pt[1];
      ls[0] = pin.ls[0];
      ls[1] = pin.ls[1];
      pointFunction = pin.pointFunction;
    }
    point operator-(point const& pin) const
    {
      point result(pt[0] - pin.pt[0], pt[1] - pin.pt[1]);

      return result;
    }
    point operator+(point const& pin) const
    {
      point result(pt[0] + pin.pt[0], pt[1] + pin.pt[1]);
      return result;
    }
    point operator*(double t) const
    {
      point result(t * pt[0], t * pt[1]);
      return result;
    }
    double operator[](size_t i) const { return pt[i]; }
    double normSquared() const { return this->dot(*this); }
    double distSquared(point const& other) const
    {
      point tmp = *this - other;
      return tmp.dot(tmp);
    }
    double dot(point const& other) const
    {
      return this->pt[0] * other.pt[0] + this->pt[1] * other.pt[1];
    }
    bool inside(double d) const { return pointFunction->inside(d); }

    double apply(double d, double ptNew, double& dir) const
    {
      assert(pointFunction.get() != NULL);
      return pointFunction->apply(d, ptNew, dir);
    }

    double apply(double d, double* ptNew, double* n) const
    {
      assert(pointFunction.get() != NULL);
      return pointFunction->apply(d, ptNew, n);
    }

  private:
    boost::shared_ptr<DepArcProfileFunction> pointFunction;
  };

  struct line_seg
  {
  public:
    line_seg()
      : pt1(NULL)
      , pt2(NULL)
    {
    }
    line_seg(point* p1, point* p2)
      : pt1(p1)
      , pt2(p2)
    {
      assert(p1 != NULL);
      assert(p2 != NULL);
      dr = *p2 - *p1;
      pt1->ls[0] = this;
      pt2->ls[1] = this;
    }

    ~line_seg()
    {
      pt1->ls[0] = NULL;
      pt2->ls[1] = NULL;
    }

    point getPoint(double t) const
    {
      assert(t <= 1.0);
      if (t == 1)
        return *pt2;
      else if (t == 0)
        return *pt1;
      point result = *(pt1) + dr * t;
      result.setFunction(interpolate_functions(pt1->getFunction(), pt2->getFunction(), t));
      result.ls[0] = this;
      result.ls[1] = NULL;
      return result;
    }

    point findClosestPoint(point& pt) const
    {
      double l2 = dr.normSquared();
      if (l2 == 0)
      {
        return *pt1;
      }
      point tpt1 = pt - *pt1;
      point tpt2 = this->dr;
      double t = tpt1.dot(tpt2) / l2;
      if (t <= 0.0)
      {
        return *pt1;
      }
      else if (t >= 1.0)
      {
        return *pt2;
      }
      else
      {
        return getPoint(t);
      }
    }

    bool side(point& pt, point& closePt) const
    {
      point cpDir = pt - closePt;
      double sign = dr[1] * cpDir[0] - dr[0] * cpDir[1];
      return sign < 0;
    }

    point const& operator[](size_t i) const
    {
      if (i == 0)
        return *pt1;
      return *pt2;
    }
    double length() { return std::sqrt(dr.normSquared()); }

    static bool side(line_seg const* l1, line_seg const* l2, point& pt, point& closePt)
    {
      point cpDir = pt - closePt;
      point testDir = (l1->dr - l2->dr);
      double t = cpDir.dot(testDir);
      point tmp = closePt + testDir * t;
      return l1->side(tmp, closePt);
    }

  private:
    point* pt1;
    point* pt2;
    point dr;
  };

  bool IsEnabled;

  void addPoint(double x, double y)
  {
    size_t at = points.size();
    points.push_back(new point(x, y));
    if (at >= 1)
    {
      size_t prev = at - 1;
      DepArcData::line_seg* l = new DepArcData::line_seg(points[prev], points[at]);
      lines.push_back(l);
    }
  }

  void clear()
  {
    for (unsigned int i = 0; i < lines.size(); ++i)
    {
      delete lines[i];
    }
    lines.clear();
    for (unsigned int i = 0; i < points.size(); ++i)
    {
      delete points[i];
    }
    points.clear();
    functions.clear();
    assert(lines.empty() && points.empty() && functions.empty());
  }

  bool getDistance(double pin[2], double& result, size_t& lsId, point& resultPt) const
  {
    if (points.empty())
    {
      return false;
    }
    point pt(pin[0], pin[1]);
    point closePt = *(points[0]);
    result = pt.distSquared(closePt);
    lsId = 0;
    for (unsigned int i = 0; i < lines.size(); ++i)
    {
      line_seg const* ls = lines[i];
      point tmppt = ls->findClosestPoint(pt);
      double tmp = pt.distSquared(tmppt);
      if (tmp < result)
      {
        result = tmp;
        closePt = tmppt;
        lsId = i;
      }
    }
    result = sqrt(result);
    resultPt = closePt;
    if (closePt.getFunction() == NULL)
    {
      return false;
    }
    if (closePt.ls[0] != NULL && closePt.ls[1] != NULL)
    {
      if (line_seg::side(closePt.ls[0], closePt.ls[1], pt, closePt))
        result = -result;
      //bool s1 = closePt.ls[0]->side(pt, closePt);
      //bool s2 = closePt.ls[1]->side(pt, closePt);
      //if(s1 == s2) if(s1) result = -result;
      //else return false;
    }
    else
    {
      line_seg const* cls = (closePt.ls[0] != NULL) ? closePt.ls[0] : closePt.ls[1];

      assert(cls != NULL);

      if (cls->side(pt, closePt))
        result = -result;
    }
    return closePt.inside(result);
  }

  void closeArc()
  {
    if (lines.empty())
      return;

    size_t prev = points.size() - 1;
    DepArcData::line_seg* l = new DepArcData::line_seg(points[prev], points[0]);
    lines.push_back(l);
  }

  void setFunction(size_t b, size_t e, boost::shared_ptr<DepArcProfileFunction> fun)
  {
    for (; b < e; ++b)
    {
      points[b]->setFunction(fun);
    }
  }

  void setInterpolateFunction(size_t b, size_t e)
  {
    assert(points[b]->getFunction() != NULL);
    assert(points[e]->getFunction() != NULL);
    double total_distance = 0;
    if (points[b]->getFunction() == points[e]->getFunction())
    {
      //optimize
      setFunction(b + 1, e, points[b]->getFunction());
      return;
    }
    boost::shared_ptr<DepArcProfileFunction> fun0 = points[b]->getFunction();
    boost::shared_ptr<DepArcProfileFunction> fun1 = points[e]->getFunction();

    for (size_t i = b; i < e; ++i)
    {
      total_distance += lines[i]->length();
    }
    double currentLength = lines[b]->length();
    for (size_t i = b + 1; i < e; ++i)
    {
      double t = currentLength / total_distance;
      points[i]->setFunction(interpolate_functions(fun0, fun1, t));
      currentLength += lines[i]->length();
    }
  }

  void setUpFunctions()
  {
    size_t b = 0;
    while (b < points.size() && points[b]->getFunction() == NULL)
    {
      b++;
    }
    if (b == points.size())
      return;
    if (b != 0)
    {
      setFunction(0, b, points[b]->getFunction());
    }
    while (b != points.size())
    {
      assert(points[b]->getFunction() != NULL);
      size_t e = b + 1;
      while (e < points.size() && points[e]->getFunction() == NULL)
      {
        e++;
      }
      if (e == points.size())
      {
        setFunction(b + 1, e, points[b]->getFunction());
      }
      else if (e != b + 1)
      {
        setInterpolateFunction(b, e);
      }
      b = e;
    }
  }

  void setFunctionToPoint(int ptId, int fId)
  {
    if (static_cast<size_t>(ptId) < points.size() && points[ptId])
    {
      assert(fId < functions.size());
      points[ptId]->setFunction(functions[fId]);
    }
  }

  void updateBound(double minZ, double maxZ)
  {
    for (unsigned int i = 0; i < functions.size(); ++i)
    {
      if (functions[i])
        functions[i]->setFunctionRange(minZ, maxZ);
    }
  }

  std::vector<line_seg*> lines;
  std::vector<point*> points;
  std::vector<boost::shared_ptr<DepArcProfileFunction> > functions;
};

void vtkArcDepressFilter::PrintSelf(ostream& /*os*/, vtkIndent /*indent*/)
{
}

void vtkArcDepressFilter::ClearActiveArcPoints(int arc_ind)
{
  if (arc_ind < 0 || static_cast<size_t>(arc_ind) >= Arcs.size() || Arcs[arc_ind] == NULL)
    return;
  Arcs[arc_ind]->clear();
  this->Modified();
}

void vtkArcDepressFilter::SetArcAsClosed(int arc_ind)
{
  if (arc_ind < 0 || static_cast<size_t>(arc_ind) >= Arcs.size() || Arcs[arc_ind] == NULL)
    return;
  Arcs[arc_ind]->closeArc();
}

void vtkArcDepressFilter::SetAxis(int axis)
{
  Axis = axis;
}

void vtkArcDepressFilter::AddPointToArc(double in_ind, double v1, double v2)
{
  int ind = static_cast<int>(in_ind);
  if (ind < 0)
    return;
  if (static_cast<size_t>(ind) >= Arcs.size() || Arcs[ind] == NULL)
    return;
  Arcs[ind]->addPoint(v1, v2);
  this->Modified();
}

void vtkArcDepressFilter::SetManualControlRanges(double arc_ind, double funId, double minDispDepth,
  double maxDispDepth, double minDist, double maxDist)
{
  int ind = static_cast<int>(arc_ind);
  int fId = static_cast<int>(funId);
  if (ind < 0)
    return;
  if (static_cast<size_t>(ind) >= Arcs.size() || Arcs[ind] == NULL)
    return;
  DepArcData* td = Arcs[ind];
  assert(fId < td->functions.size());
  boost::shared_ptr<DepArcProfileFunction> fun = td->functions[fId];
  DepArcManualProfileFunction* mfun = dynamic_cast<DepArcManualProfileFunction*>(fun.get());
  assert(mfun != NULL);
  mfun->setMinMaxDesplacementDepth(minDispDepth, maxDispDepth);
  mfun->setMinMaxDistance(minDist, maxDist);
  this->Modified();
}

void vtkArcDepressFilter::AddArc(int arc_ind)
{
  if (arc_ind < 0)
    return;
  size_t st_arc_ind = static_cast<size_t>(arc_ind);
  if (st_arc_ind >= this->Arcs.size())
  {
    this->Arcs.resize(st_arc_ind + 1, NULL);
  }
  if (st_arc_ind < this->Arcs.size() && this->Arcs[st_arc_ind] == NULL)
  {
    this->Arcs[st_arc_ind] = new DepArcData();
    this->Modified();
  }
}

void vtkArcDepressFilter::RemoveArc(int arc_ind)
{
  if (static_cast<size_t>(arc_ind) >= Arcs.size() || Arcs[arc_ind] == NULL)
    return;
  delete this->Arcs[arc_ind];
  this->Arcs[arc_ind] = NULL;
  this->Modified();
}

void vtkArcDepressFilter::SetArcEnable(int arc_ind, int isEnabled)
{
  if (static_cast<size_t>(arc_ind) >= Arcs.size() || arc_ind < 0 || Arcs[arc_ind] == NULL)
    return;
  Arcs[arc_ind]->IsEnabled = isEnabled != 0;
  this->Modified();
}

void vtkArcDepressFilter::SetFunctionToPoint(int arc_ind, int ptId, int funId)
{
  if (arc_ind < 0 || static_cast<size_t>(arc_ind) >= Arcs.size() || Arcs[arc_ind] == NULL)
    return;
  Arcs[arc_ind]->setFunctionToPoint(ptId, funId);
}

void vtkArcDepressFilter::AddWeightingFunPoint(
  double arc_ind, double funId, double x, double y, double m, double s)
{
  int ind = static_cast<int>(arc_ind);
  int fid = static_cast<int>(funId);
  if (ind < 0)
    return;
  if (static_cast<size_t>(ind) >= Arcs.size() || Arcs[ind] == NULL)
    return;
  DepArcData* td = Arcs[ind];
  assert(fid < td->functions.size());
  td->functions[fid]->addWeightPoint(x, y, m, s);
}

void vtkArcDepressFilter::AddManualDispFunPoint(
  double arc_ind, double funId, double x, double y, double m, double s)
{
  int ind = static_cast<int>(arc_ind);
  int fid = static_cast<int>(funId);
  if (ind < 0)
    return;
  if (static_cast<size_t>(ind) >= Arcs.size() || Arcs[ind] == NULL)
    return;
  DepArcData* td = Arcs[ind];
  assert(fid < td->functions.size());

  boost::shared_ptr<DepArcProfileFunction> fun = td->functions[fid];
  DepArcManualProfileFunction* mfun = dynamic_cast<DepArcManualProfileFunction*>(fun.get());
  assert(mfun != NULL);

  mfun->addPoint(DepArcManualProfileFunction::DispFun, x, y, m, s);
}

vtkArcDepressFilter::vtkArcDepressFilter()
  : Axis(2)
{
  currentData = NULL;
  this->UseNormalDirection = false;
}

vtkArcDepressFilter::~vtkArcDepressFilter()
{
  for (unsigned int i = 0; i < this->Arcs.size(); ++i)
  {
    delete this->Arcs[i];
  }
  this->Arcs.clear();
}

void vtkArcDepressFilter::ResizeOrder(int size)
{
  if (size < 0)
    return;
  ApplyOrder.resize(size, -1);
  this->Modified();
}

void vtkArcDepressFilter::SetOrderValue(int loc, int arc_ind)
{
  if (static_cast<size_t>(loc) >= ApplyOrder.size() || loc < 0 ||
    (arc_ind != -1 && static_cast<size_t>(arc_ind) >= Arcs.size()))
    return;
  ApplyOrder[loc] = arc_ind;
  this->Modified();
}

namespace
{
double wedgeVolume(double pts[6][3])
{
  static int tetra[][4] = { { 0, 2, 1, 3 }, { 1, 3, 5, 4 }, { 1, 2, 5, 3 } };
  double r = 0;
  for (unsigned int j = 0; j < 3; ++j)
  {
    r += std::abs(vtkTetra::ComputeVolume(
      pts[tetra[j][0]], pts[tetra[j][1]], pts[tetra[j][2]], pts[tetra[j][3]]));
  }
  return r;
}
double pyramidVolume(double pts[6][3])
{
  double r = 0;
  r += std::abs(vtkTetra::ComputeVolume(pts[0], pts[1], pts[2], pts[4]));
  r += std::abs(vtkTetra::ComputeVolume(pts[1], pts[2], pts[3], pts[4]));
  return r;
}
}

void vtkArcDepressFilter::computeDisplacement(
  vtkPolyData* input, vtkPolyData* output, std::vector<int>& pointChanged)
{
  vtkDataArray* normals = input->GetPointData()->GetNormals();

  vtkIdType i;
  vtkCellArray *newVerts, *newLines, *newPolys;
  vtkPoints* newPoints;
  vtkIdType estimatedSize, numCells = input->GetNumberOfCells();
  vtkIdType numPts = input->GetNumberOfPoints();
  vtkPoints* inPts = input->GetPoints();

  pointChanged.clear();
  pointChanged.resize(numPts, 0);

  estimatedSize = numCells;
  estimatedSize = estimatedSize / 1024 * 1024; //multiple of 1024
  if (estimatedSize < 1024)
  {
    estimatedSize = 1024;
  }

  newPoints = vtkPoints::New();
  newPoints->SetDataType(inPts->GetDataType());
  newPoints->Allocate(numPts, numPts / 2);
  newPoints->DeepCopy(inPts);
  newVerts = vtkCellArray::New();
  newVerts->Allocate(estimatedSize, estimatedSize / 2);
  newLines = vtkCellArray::New();
  newLines->Allocate(estimatedSize, estimatedSize / 2);
  newPolys = vtkCellArray::New();
  newPolys->Allocate(estimatedSize, estimatedSize / 2);

  //Transform points
  double point[3], original[3];
  double normal[3];
  for (size_t t = 0; t < Arcs.size(); ++t)
  {
    if (Arcs[t] != NULL)
      Arcs[t]->setUpFunctions();
  }
  bool useNorm = UseNormalDirection && normals != NULL;
  double currentBounds[2];
  {
    double bounds[6];
    newPoints->GetBounds(bounds);
    currentBounds[0] = bounds[4];
    currentBounds[1] = bounds[5];
  }

  for (unsigned int j = 0; j < ApplyOrder.size() && numPts != 0; ++j)
  {
    int id = ApplyOrder[j];
    if (id < 0 || Arcs[id] == NULL)
      continue;
    DepArcData& dad = *Arcs[id];
    dad.updateBound(currentBounds[0], currentBounds[1]);
    double d;
    size_t lsId;
    DepArcData::point closestPt;
    newPoints->GetPoint(0, point);
    double updateBounds[] = { point[2], point[2] };
    for (i = 0; i < numPts; i++)
    {
      newPoints->GetPoint(i, point);
      inPts->GetPoint(i, original);
      double pt2d[] = { point[0], point[1] };

      if (dad.getDistance(pt2d, d, lsId, closestPt))
      {
        double dir = 0;
        if (useNorm)
        {
          normals->GetTuple(i, normal);
          dir = closestPt.apply(d, point, normal);
        }
        else
        {
          point[2] = closestPt.apply(d, point[2], dir);
        }
        if (point[2] < updateBounds[0])
          updateBounds[0] = point[2];
        if (point[2] > updateBounds[1])
          updateBounds[1] = point[2];
        if (dir != 0)
        {
          pointChanged[i] = (dir < 0) ? -1 : 1;
        }
      }
      assert(useNorm || (point[0] == original[0] && point[1] == original[1]));
      newPoints->SetPoint(i, point);
    }
    currentBounds[0] = updateBounds[0];
    currentBounds[1] = updateBounds[1];
  }
  newVerts->DeepCopy(input->GetVerts());
  newLines->DeepCopy(input->GetLines());
  newPolys->DeepCopy(input->GetPolys());

  if (newVerts->GetNumberOfCells())
  {
    output->SetVerts(newVerts);
  }
  newVerts->Delete();

  if (newLines->GetNumberOfCells())
  {
    output->SetLines(newLines);
  }
  newLines->Delete();

  if (newPolys->GetNumberOfCells())
  {
    output->SetPolys(newPolys);
  }

  newPolys->Delete();

  output->SetPoints(newPoints);
  newPoints->Delete();
}

void vtkArcDepressFilter::computeChange(
  vtkPolyData* input, vtkPoints* originalPts, vtkPoints* newPoints, std::vector<int>& pointChanged)
{
  double* mod[3] = { NULL, &amountAdded, &amountRemoved };
  amountRemoved = 0;
  amountAdded = 0;
#ifndef NDEBUG
  bool useNorm = UseNormalDirection && NULL != input->GetPointData()->GetNormals();
#endif
  for (unsigned int i = 0; i < input->GetNumberOfCells(); ++i)
  {
    vtkCell* cell = input->GetCell(i);
    bool changed = false;
    bool allChanged = true;
    int changeScore = 0;
    int digRaiseCheck[2] = { 0, 0 };
    int notChanged = -1;
    for (unsigned int j = 0; j < 3; ++j)
    {
      int tmpPC = pointChanged[cell->GetPointId(j)];
      changeScore += tmpPC;
      allChanged = allChanged && tmpPC;
      changed = changed || tmpPC;
      if (tmpPC)
        ++digRaiseCheck[(tmpPC + 1) / 2];
      else
        notChanged = static_cast<int>(j);
    }
    if (changed)
    {
      double pts[6][3];
      double* modifier = (digRaiseCheck[0]) ? &amountRemoved : &amountAdded;
      vtkIdType ptIds[] = { cell->GetPointId(0), cell->GetPointId(1), cell->GetPointId(2) };
      if (digRaiseCheck[0] == 3 || digRaiseCheck[1] == 3)
      {
        for (unsigned int j = 0; j < 3; ++j)
        {
          newPoints->GetPoint(cell->GetPointId(j), pts[j + digRaiseCheck[0]]);
          originalPts->GetPoint(cell->GetPointId(j), pts[j + digRaiseCheck[1]]);
          assert(useNorm || (pts[j + digRaiseCheck[0]][0] == pts[j + digRaiseCheck[1]][0] &&
                              pts[j + digRaiseCheck[0]][1] == pts[j + digRaiseCheck[1]][1]));
        }
        *modifier += wedgeVolume(pts);
      }
      else if ((digRaiseCheck[0] == 1 && digRaiseCheck[1] == 0) ||
        (digRaiseCheck[1] == 1 && digRaiseCheck[0] == 0))
      {
        for (unsigned int j = 0; j < cell->GetNumberOfPoints(); ++j)
        {
          vtkIdType id = cell->GetPointId(j);
          if (pointChanged[id])
          {
            if (digRaiseCheck[0])
            {
              originalPts->GetPoint(id, pts[3]);
              newPoints->GetPoint(id, pts[j]);
            }
            else
            {
              originalPts->GetPoint(id, pts[j]);
              newPoints->GetPoint(id, pts[3]);
            }
          }
          else
          {
            newPoints->GetPoint(id, pts[j]);
          }
        }
        *modifier += std::abs(vtkTetra::ComputeVolume(pts[0], pts[1], pts[2], pts[3]));
      }
      else if ((digRaiseCheck[0] == 2 && digRaiseCheck[1] == 0) ||
        (digRaiseCheck[1] == 2 && digRaiseCheck[0] == 0))
      {
        int index = digRaiseCheck[1] / 2;
        int i1Inx[][5] = { { 0, 1, 2, 3, 4 }, { 3, 2, 1, 0, 4 } };
        int i2 = (!pointChanged[ptIds[0]]) ? 0 : (!pointChanged[ptIds[1]]) ? 1 : 2;
        int i2Inx[][5] = { { 2, 1, 1, 2, 0 }, { 0, 2, 2, 0, 1 }, { 1, 0, 0, 1, 2 } };
        newPoints->GetPoint(ptIds[i2Inx[i2][0]], pts[i1Inx[index][0]]);
        newPoints->GetPoint(ptIds[i2Inx[i2][1]], pts[i1Inx[index][1]]);
        originalPts->GetPoint(ptIds[i2Inx[i2][2]], pts[i1Inx[index][2]]);
        originalPts->GetPoint(ptIds[i2Inx[i2][3]], pts[i1Inx[index][3]]);
        newPoints->GetPoint(ptIds[i2Inx[i2][4]], pts[i1Inx[index][4]]);

        *modifier += pyramidVolume(pts);
      }
      else
      {
        for (unsigned int j = 0; j < 3; ++j)
        {
          newPoints->GetPoint(cell->GetPointId(j), pts[j]);
          originalPts->GetPoint(cell->GetPointId(j), pts[j + 3]);
        }
        int coplanar;
        double intersection[2][3];
        double surfaceid[2];
        double tolerance = 0.;
        int r = vtkIntersectionPolyDataFilter::TriangleTriangleIntersection(pts[0], pts[1], pts[2],
          pts[3], pts[4], pts[5], coplanar, intersection[0], intersection[1], surfaceid, tolerance);

        if ((digRaiseCheck[0] == 2 && digRaiseCheck[1] == 1) ||
          (digRaiseCheck[0] == 1 && digRaiseCheck[1] == 2))
        {
          assert(r != 0);
          static const int at[][2] = { { 1, 2 }, { 0, 2 }, { 0, 1 } };
          static const int oatA[][3] = { { 3, 3, 0 }, { 0, 0, 3 }, { 3, 3, 0 } };
          static const int oatB[][3] = { { 0, 0, 3 }, { 3, 3, 0 }, { 0, 0, 3 } };
          static const int oatC[][3] = { { 0, 0, 1 }, { 0, 1, 0 }, { 0, 0, 1 } };

          static const int sign[] = { 0, -1, 1 };
          int t1 = 0;
          for (int j = 0; j < 3; ++j)
          {
            if (sign[digRaiseCheck[0]] * pointChanged[ptIds[j]] > 0)
            {
              t1 = j;
            }
          }

          double tmp = 0;
          tmp = std::abs(
            vtkTetra::ComputeVolume(pts[t1], intersection[0], intersection[1], pts[t1 + 3]));
          *(mod[digRaiseCheck[1]]) += tmp;
          int const* tat = at[t1];
          int z = tat[0], o = tat[1];
          int ota = oatA[t1][digRaiseCheck[0]];
          int otb = oatB[t1][digRaiseCheck[0]];
          int otc = oatC[t1][digRaiseCheck[0]];
          int otd = (otc + 1) % 2;

          assert(pointChanged[ptIds[z]] == pointChanged[ptIds[o]]);
          double pts2[6][3] = { { pts[z + ota][0], pts[z + ota][1], pts[z + ota][2] },
            { pts[z + otb][0], pts[z + otb][1], pts[z + otb][2] },
            { intersection[otc][0], intersection[otc][1], intersection[otc][2] },
            { pts[o + ota][0], pts[o + ota][1], pts[o + ota][2] },
            { pts[o + otb][0], pts[o + otb][1], pts[o + otb][2] },
            { intersection[otd][0], intersection[otd][1], intersection[otd][2] } };
          assert((pointChanged[ptIds[z]] < 0 && mod[digRaiseCheck[0]] == &amountRemoved) ||
            (pointChanged[ptIds[z]] > 0 && mod[digRaiseCheck[0]] == &amountAdded));
          *(mod[digRaiseCheck[0]]) += wedgeVolume(pts2);
        }
        else
        {
#define dist(X, Y) (std::pow(X[0] - Y[0], 2) + std::pow(X[1] - Y[1], 2) + std::pow(X[2] - Y[2], 2))
          static const int at[][2] = { { 1, 2 }, { 2, 0 }, { 0, 1 } };
          if (!r) //must intersect by design
          {
            //move point a small amount on z of the shared point to handel the numeric instablity
            double tmpInter[4][3];
            double surfaceid2[2];
            double tolerance2 = 0.;
            double z = pts[notChanged][2];
            pts[notChanged][2] = z + 1e-8;
            r = vtkIntersectionPolyDataFilter::TriangleTriangleIntersection(pts[3], pts[4], pts[5],
              pts[0], pts[1], pts[2], coplanar, tmpInter[0], tmpInter[1], surfaceid2, tolerance2);
            assert(r);
            pts[notChanged][2] = z - 1e-8;
            r = vtkIntersectionPolyDataFilter::TriangleTriangleIntersection(pts[3], pts[4], pts[5],
              pts[0], pts[1], pts[2], coplanar, tmpInter[2], tmpInter[3], surfaceid2, tolerance2);
            assert(r);
            pts[notChanged][2] = z;
            for (unsigned int j = 0; j < 3; ++j)
            {
              intersection[0][j] = (tmpInter[0][j] + tmpInter[2][j]) * 0.5;
              intersection[1][j] = (tmpInter[1][j] + tmpInter[3][j]) * 0.5;
            }
          }
          double d1 = dist(pts[notChanged], intersection[0]);
          double d2 = dist(pts[notChanged], intersection[1]);
          int otherPt = (d2 < d1) ? 0 : 1;
          int addA[] = { 0, 3 };
          double* mN = &amountAdded;
          double* mO = &amountRemoved;
          if (pointChanged[ptIds[at[notChanged][0]]] < 0)
          {
            addA[0] = 3;
            addA[1] = 0;
            mN = &amountRemoved;
            mO = &amountAdded;
          }
          *mN += std::abs(vtkTetra::ComputeVolume(pts[at[notChanged][0] + addA[0]], pts[notChanged],
            intersection[otherPt], pts[at[notChanged][0] + addA[1]]));

          *mO += std::abs(vtkTetra::ComputeVolume(pts[at[notChanged][1] + addA[0]], pts[notChanged],
            intersection[otherPt], pts[at[notChanged][1] + addA[1]]));
          assert((pointChanged[ptIds[at[notChanged][1]]] < 0 && mO == &amountRemoved) ||
            (pointChanged[ptIds[at[notChanged][1]]] > 0 && mO == &amountAdded));
        }
      }
    }
  }
}

int vtkArcDepressFilter::RequestData(vtkInformation* vtkNotUsed(request),
  vtkInformationVector** inputVector, vtkInformationVector* outputVector)
{
  // get the info objects
  vtkInformation* inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation* outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkPolyData* input = vtkPolyData::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkPolyData* output = vtkPolyData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));
  currentData = input;
  vtkSmartPointer<vtkPolyDataNormals> normalGenerator;
  vtkSmartPointer<vtkTriangleFilter> triangleFilter;

  if (input == NULL)
    return 1;

  if (input->GetNumberOfPoints() < 1)
    return 1;

  this->IsProcessing = true;

  bool noValidFunctions = true;
  for (unsigned int j = 0; j < ApplyOrder.size(); ++j)
  {
    int id = ApplyOrder[j];
    if (id < 0 || Arcs[id] == NULL)
      continue;
    DepArcData& dad = *Arcs[id];
    noValidFunctions &= dad.points.empty();
  }

  if (ApplyOrder.empty() || noValidFunctions)
  {
    //Do nothing for now.
    output->ShallowCopy(input);
    this->IsProcessing = false;
    return 1;
  }

  if (input->GetNumberOfCells() != 0)
  {
    triangleFilter = vtkSmartPointer<vtkTriangleFilter>::New();
    triangleFilter->SetInputData(input);
    triangleFilter->Update();
    input = triangleFilter->GetOutput();
  }

  bool inputHasNormals = input->GetPointData()->GetNormals() != NULL;
  if (UseNormalDirection && !inputHasNormals)
  {
    normalGenerator = vtkSmartPointer<vtkPolyDataNormals>::New();
    normalGenerator->SetInputData(input);
    normalGenerator->ComputePointNormalsOn();
    normalGenerator->ComputeCellNormalsOff();
    normalGenerator->SplittingOff();
    normalGenerator->Update();
    input = normalGenerator->GetOutput();
  }

  std::vector<int> pointChanged; //0 no change, 1 raise, -1 dig
  this->computeDisplacement(input, output, pointChanged);

  if (inputHasNormals)
  {
    normalGenerator = vtkSmartPointer<vtkPolyDataNormals>::New();
    normalGenerator->SetInputData(output);
    normalGenerator->ComputePointNormalsOn();
    normalGenerator->ComputeCellNormalsOff();
    normalGenerator->SplittingOff();
    normalGenerator->Update();
    output->GetPointData()->SetNormals(normalGenerator->GetOutput()->GetPointData()->GetNormals());
  }

  output->Squeeze();

  amountAdded = -1;
  amountRemoved = -1;
  if (input->GetNumberOfCells() != 0 && input->GetCell(0)->GetNumberOfPoints() == 3)
  {
    this->computeChange(input, input->GetPoints(), output->GetPoints(), pointChanged);
  }

  //copy reset of the data
  this->IsProcessing = false;
  return 1;
}

void vtkArcDepressFilter::computeDisplacementChangeOnPointsViaBathymetry(
  double stepSize, double radius)
{
  if (stepSize <= 0 || radius <= 0)
    return;
  vtkSmartPointer<vtkPlaneSource> planeSource = vtkSmartPointer<vtkPlaneSource>::New();
  double bounds[6];
  vtkPoints* points = this->currentData->GetPoints();
  points->GetBounds(bounds);
  double w = bounds[1] - bounds[0];
  double h = bounds[3] - bounds[2];
  int ws = static_cast<int>(std::ceil(w / stepSize));
  int hs = static_cast<int>(std::ceil(h / stepSize));
  planeSource->SetPoint1(bounds[1], bounds[2], 0);
  planeSource->SetPoint2(bounds[0], bounds[3], 0);
  planeSource->SetOrigin(bounds[0], bounds[2], 0);
  planeSource->SetResolution(ws, hs);
  planeSource->SetNormal(0, 0, 1);
  planeSource->Update();
  vtkPolyData* plane = planeSource->GetOutput();
  vtkSmartPointer<vtkCMBApplyBathymetryFilter> bathyFilter =
    vtkSmartPointer<vtkCMBApplyBathymetryFilter>::New();
  double invalidVal = NAN; //TODO This
  bathyFilter->SetElevationRadius(radius);
  bathyFilter->SetInvalidValue(invalidVal); //TODO This
  bathyFilter->SetInputData(0, plane);
  bathyFilter->SetInputData(1, this->currentData);
  bathyFilter->Update();
  vtkPolyData* bethPlane = static_cast<vtkPolyData*>(bathyFilter->GetOutput());
  vtkSmartPointer<vtkTriangleFilter> triangleFilter;
  triangleFilter = vtkSmartPointer<vtkTriangleFilter>::New();
  triangleFilter->SetInputData(bethPlane);
  triangleFilter->Update();
  bethPlane = triangleFilter->GetOutput();
  bethPlane->BuildLinks();
  vtkSmartPointer<vtkIdList> cellIds = vtkSmartPointer<vtkIdList>::New();

  //filter out invalid points
  for (unsigned int i = 0; i < bethPlane->GetNumberOfPoints(); i++)
  {
    double* pt = bethPlane->GetPoint(i);
    if (isnan(pt[2])) // == invalidVal)
    {
      bethPlane->GetPointCells(i, cellIds);
      for (unsigned int j = 0; j < cellIds->GetNumberOfIds(); ++j)
      {
        bethPlane->DeleteCell(cellIds->GetId(j));
      }
    }
  }
  bethPlane->RemoveDeletedCells();
  vtkPolyData* out = bethPlane->NewInstance();
  //Apply the displacment
  std::vector<int> pointChanged; //0 no change, 1 raise, -1 dig
  this->computeDisplacement(bethPlane, out, pointChanged);
  //calculate displacment vol change
  this->computeChange(bethPlane, bethPlane->GetPoints(), out->GetPoints(), pointChanged);
  out->Delete();
}

void vtkArcDepressFilter::setUseNormalDirection(int in)
{
  if (in == -1)
    return;
  if (in == 1)
  {
    this->UseNormalDirection = true;
  }
  if (in == 0)
  {
    this->UseNormalDirection = false;
  }
}

void vtkArcDepressFilter::CreateManualFunction(
  int arc_ind, int funId, int desptFunctionType, int weightFunType, int isRelative, int isSymmetric)
{
  if (arc_ind < 0 || static_cast<size_t>(arc_ind) >= Arcs.size() || Arcs[arc_ind] == NULL)
    return;
  assert(funId >= 0);
  DepArcData* td = Arcs[arc_ind];
  if (funId >= static_cast<int>(td->functions.size()))
  {
    td->functions.resize(funId + 1);
  }
  td->functions[funId] = boost::shared_ptr<DepArcProfileFunction>(new DepArcManualProfileFunction(
    static_cast<DepArcProfileFunction::FunctionType>(desptFunctionType),
    static_cast<DepArcProfileFunction::FunctionType>(weightFunType), (isSymmetric != 0),
    (isRelative != 0)));
  this->Modified();
}

void vtkArcDepressFilter::CreateWedgeFunction(double arc_ind_in, double funId_in,
  double weightFunType_in, double relative_in, double mode_in, double clamp_in, double basewidth,
  double displacement, double slopeLeft, double slopeRight)
{
  int arc_ind = static_cast<int>(arc_ind_in);
  int funId = static_cast<int>(funId_in);
  DepArcProfileFunction::FunctionType weightFunType =
    static_cast<DepArcProfileFunction::FunctionType>(static_cast<int>(weightFunType_in));
  bool relative = relative_in != 0;
  DepArcWedgeProfileFunction::DisplacementMode disMode =
    static_cast<DepArcWedgeProfileFunction::DisplacementMode>(static_cast<int>(mode_in + 1));
  bool clamp = clamp_in != 0;

  if (arc_ind < 0 || static_cast<size_t>(arc_ind) >= Arcs.size() || Arcs[arc_ind] == NULL)
    return;

  assert(funId >= 0);
  DepArcData* td = Arcs[arc_ind];
  if (funId >= static_cast<int>(td->functions.size()))
  {
    td->functions.resize(funId + 1);
  }

  td->functions[funId] = boost::shared_ptr<DepArcProfileFunction>(new DepArcWedgeProfileFunction(
    weightFunType, relative, disMode, clamp, basewidth, displacement, slopeLeft, slopeRight));
  this->Modified();
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_TWO_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_TWO_SPLIT_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
  @file
  @brief Definition of the two_split function
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /// @brief Hierarchy tag for two_split function
    struct two_split_ : ext::elementwise_<two_split_>
    {
      typedef ext::elementwise_<two_split_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_two_split_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::two_split_, Site> dispatching_two_split_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::two_split_, Site>();
    }
    template<class... Args>
    struct impl_two_split_;
  }

  /*!
    @brief

    For any real @c a0, two_split computes two reals @c r0 and @c r1 so that:

    @code
    a0 = r0+r1
    @endcode

    and where @c r0 and @c r1 bit pattern don't overlap.

    @param a0 Value to split

    @return A Fusion Sequence containing @c a0+a1 and the residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::two_split_, two_split, 1)

  /*!
    @brief

    For any real @c a0, two_split computes two reals @c r0 and @c r1 so that:

    @code
    a0 = r0+r1
    @endcode

    and where @c r0 and @c r1 bit pattern don't overlap.

    @param a0 Value to split
    @param a1 L-Value that will receive @c a0+a1

    @return The sum residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::two_split_, two_split
                                            , (A0 const&)(A1&)
                                            , 2
                                            )

  /*!
    @brief

    For any real @c a0, two_split computes two reals @c r0 and @c r1 so that:

    @code
    a0 = r0+r1
    @endcode

    and where @c r0 and @c r1 bit pattern don't overlap.

    @param a0 Value to split
    @param a1 L-Value that will receive @c a0+a1.
    @param a2 L-Value that will receive @c a0+a1 residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::two_split_, two_split
                                            , (A0 const&)(A1&)(A2&)
                                            , 3
                                            )
} }

#endif


/*
 *  div_result.h
 *  MTL
 *
 *  Created by Hui Li (huil@Princeton.EDU)
 *
 */

#ifndef MTL_DIV_RESULT_INCLUDE
#define MTL_DIV_RESULT_INCLUDE

#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>

namespace mtl { namespace traits {

template < typename Op1, typename Op2, typename DivOp > struct div_result_aux {};

/// Result type for dividing Op1 by Op2
/** Can be used in enable-if-style as type is only defined when appropriate **/
template < typename Op1, typename Op2 >
struct div_result
  : public div_result_aux < Op1, Op2, typename ashape::div_op<typename ashape::ashape<Op1>::type,
							      typename ashape::ashape<Op2>::type >::type >
{};

/// Divide column vector by scalar
template < typename Op1, typename Op2 >
struct div_result_aux < Op1, Op2, ::mtl::ashape::cvec_scal_div >
{
    typedef typename vector::divide_by_view<Op1,Op2> type;
};

/// Divide row vector by scalar
template < typename Op1, typename Op2 >
struct div_result_aux < Op1, Op2, ::mtl::ashape::rvec_scal_div >
{
    typedef typename vector::divide_by_view<Op1,Op2> type;
};

/// Divide matrix by scalar
template < typename Op1, typename Op2 >
struct div_result_aux < Op1, Op2, ::mtl::ashape::mat_scal_div >
{
    typedef typename matrix::divide_by_view<Op1,Op2> type;
};

}} // namespace mtl::traits

#endif


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

  @copyright 2015 NumScale SAS

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

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/is_nan.hpp>
#endif
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/detail/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 ( arg_
                             , (typename A0)
                             , bd::cpu_
                             , bs::pedantic_tag
                             , bd::scalar_< bd::floating_<A0> >
                             )
     {
       BOOST_FORCEINLINE A0 operator() (const pedantic_tag &,
                                        A0 a0) const BOOST_NOEXCEPT
       {
         A0 r = if_else_zero(is_negative(a0),Pi<A0>());
       #ifndef BOOST_SIMD_NO_NANS
         return if_allbits_else(is_nan(a0),r);
       #else
         return r;
       #endif
       }
     };

     BOOST_DISPATCH_OVERLOAD ( arg_
                             , (typename A0)
                             , bd::cpu_
                             , bd::scalar_< bd::floating_<A0> >
                             )
     {
       BOOST_FORCEINLINE A0 operator() ( A0 a0 ) const BOOST_NOEXCEPT
       {
         return if_else_zero(is_negative(a0),Pi<A0>());
       }
     };

} } }

#endif


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

#include <bitset>
#include <boost/multiprecision/cpp_int.hpp>
#include <ipmid/utility.hpp>
#include <tuple>

// unsigned fixed-bit sizes
template <unsigned N>
using fixed_uint_t =
    boost::multiprecision::number<boost::multiprecision::cpp_int_backend<
        N, N, boost::multiprecision::unsigned_magnitude,
        boost::multiprecision::unchecked, void>>;
// signed fixed-bit sizes
template <unsigned N>
using fixed_int_t =
    boost::multiprecision::number<boost::multiprecision::cpp_int_backend<
        N, N, boost::multiprecision::signed_magnitude,
        boost::multiprecision::unchecked, void>>;

using uint1_t = fixed_uint_t<1>;
using uint2_t = fixed_uint_t<2>;
using uint3_t = fixed_uint_t<3>;
using uint4_t = fixed_uint_t<4>;
using uint5_t = fixed_uint_t<5>;
using uint6_t = fixed_uint_t<6>;
using uint7_t = fixed_uint_t<7>;
// native uint8_t
using uint9_t = fixed_uint_t<9>;
using uint10_t = fixed_uint_t<10>;
using uint11_t = fixed_uint_t<11>;
using uint12_t = fixed_uint_t<12>;
using uint13_t = fixed_uint_t<13>;
using uint14_t = fixed_uint_t<14>;
using uint15_t = fixed_uint_t<15>;
// native uint16_t
using uint24_t = fixed_uint_t<24>;

// signed fixed-bit sizes
using int2_t = fixed_int_t<2>;
using int3_t = fixed_int_t<3>;
using int4_t = fixed_int_t<4>;
using int5_t = fixed_int_t<5>;
using int6_t = fixed_int_t<6>;
using int7_t = fixed_int_t<7>;
// native int8_t
using int9_t = fixed_int_t<9>;
using int10_t = fixed_int_t<10>;
using int11_t = fixed_int_t<11>;
using int12_t = fixed_int_t<12>;
using int13_t = fixed_int_t<13>;
using int14_t = fixed_int_t<14>;
using int15_t = fixed_int_t<15>;
// native int16_t
using int24_t = fixed_int_t<24>;

// bool is more efficient than a uint1_t
using bit = bool;

// Mechanism for going from uint7_t, int7_t, or std::bitset<7> to 7 bits
// use nrFixedBits<uint7_t> or nrFixedBits<decltype(u7)>
namespace types
{
namespace details
{

template <size_t N>
struct Size
{
    static constexpr size_t value = N;
};

template <unsigned Bits>
constexpr auto getNrBits(const fixed_int_t<Bits>&) -> Size<Bits>;
template <unsigned Bits>
constexpr auto getNrBits(const fixed_uint_t<Bits>&) -> Size<Bits>;
template <size_t Bits>
constexpr auto getNrBits(const std::bitset<Bits>&) -> Size<Bits>;

} // namespace details

/**
 * @brief mechanism to get N from a type like fixed_int_t<N>
 *
 * helper template to extract N from a fixed_(u)int_t variable
 *
 * @tparam T - a type of fixed_int_t<N> or fixed_unint_t<N>
 *
 * @return size_t - evaluates to a constexpr size_t of N
 */
template <typename T>
constexpr auto nrFixedBits =
    decltype(details::getNrBits(std::declval<T>()))::value;

} // namespace types


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_FREXP_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_FREXP_HPP_INCLUDED
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <nt2/sdk/constant/properties.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/bitwise_notand.hpp>
#include <nt2/include/functions/shri.hpp>
#include <nt2/include/functions/seladd.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/functions/seladd.hpp>
#include <boost/fusion/include/vector.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::frexp_, tag::cpu_,
                        (A0)(X),
                        ((simd_<arithmetic_<A0>,X>))
                       );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::frexp_(tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::strip<A0>::type                     A00;
      typedef typename meta::as_integer<A00, signed>::type  exponent;
      typedef boost::fusion::vector<A00,exponent>                type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      eval( a0
          , boost::fusion::at_c<0>(res)
          , boost::fusion::at_c<1>(res)
          );
      return res;
    }
  private:
    template<class A0,class R0,class R1> inline void
    eval(A0 const& a0,R0& r0, R1& r1)const
    {
      typedef typename meta::as_integer<A0, signed>::type      int_type;
      typedef typename meta::scalar_of<int_type>::type        sint_type;
      typedef typename meta::scalar_of<A0>::type                 s_type;
      const sint_type me = Maxexponent<s_type>()-1;
      const sint_type nmb= Nbmantissabits<s_type>();
      const sint_type n1 = ((2*me+3)<<nmb);
      const sint_type n2 = me<<nmb;
      const  int_type vme = splat<int_type>(me);
      A0 ci_exp=simd::native_cast<A0>(splat< int_type>(n1));
      r1 = simd::native_cast<int_type>(b_and(ci_exp, a0));// extract exponent
      A0 x = b_notand(ci_exp, a0);                        // clear exponent in a0
      r1 = sub(shri(r1,nmb), vme);                        // compute exponent
      r0 = b_or(x,splat<int_type>(n2));                   // insert exponent+1 in x
      A0 test0 = is_nez(a0);
      int_type test1 = gt(r1,vme);
      r1 = b_and(r1, b_notand(test1, test0));
      //      r1 = seladd((b_notand(test1, test0)), Zero<int_type>(), r1);
      //      r0 = seladd(test0,Zero<A0>(),seladd(test1,r0,a0));
      r0 = b_and(seladd(test1,r0,a0), test0);
    }
  };
} }

#endif
// modified by jt the 04/01/2011


#include "vtkTemporalTransforms.h"
#include "vtkEigenTools.h"
#include "vtkConversions.h"

#include <vtkCellData.h>
#include <vtkCell.h>
#include <vtkPolyLine.h>
#include <vtkLine.h>
#include <vtkTransform.h>

#include <cmath>

// Eigen
#include <Eigen/Dense>

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkTemporalTransforms)

//-----------------------------------------------------------------------------
vtkTemporalTransforms::vtkTemporalTransforms()
{
  auto points = vtkSmartPointer<vtkPoints>::New();
  points->SetDataTypeToDouble();

  auto timeArray = vtkSmartPointer<vtkDoubleArray>::New();
  timeArray->SetName(this->TimeArrayName);

  auto orientationArray = vtkSmartPointer<vtkDoubleArray>::New();
  orientationArray->SetName(this->OrientationArrayName);
  orientationArray->SetNumberOfComponents(4);

  this->SetPoints(points);
  this->GetPointData()->AddArray(timeArray);
  this->GetPointData()->AddArray(orientationArray);

  // create the cell in the same time for visualization
  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(0);
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  this->SetLines(cell);
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::CreateFromPolyData(vtkPolyData *poly)
{
  if (!poly)
  {
    return nullptr;
  }
  // copy the data into a vtkTemporalTransforms before checking that it is well-formed
  auto temporalTransforms = vtkSmartPointer<vtkTemporalTransforms>::New();
  temporalTransforms->ShallowCopy(poly);

  bool isWellFormed = temporalTransforms->GetTimeArray() &&
                      temporalTransforms->GetTranslationArray() &&
                      temporalTransforms->GetOrientationArray();
  if(!isWellFormed)
  {
    return nullptr;
  }

  // create polyline if needed
  if (temporalTransforms->GetLines()->GetNumberOfCells() == 0)
  {
    // create the cell in the same time for visualization
    auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
    polyLine->GetPointIds()->SetNumberOfIds(temporalTransforms->GetNumberOfPoints());
    for (vtkIdType i = 0; i < temporalTransforms->GetNumberOfPoints(); i++)
    {
      polyLine->GetPointIds()->SetId(i,i);
    }
    auto cell = vtkSmartPointer<vtkCellArray>::New();
    cell->InsertNextCell(polyLine);
    temporalTransforms->SetLines(cell);
  }

  return temporalTransforms;
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTransform> vtkTemporalTransforms::GetTransform(unsigned int transformNumber)
{
  auto axisAngle = this->GetOrientationArray();
  auto transform = vtkSmartPointer<vtkTransform>::New();
  transform->PostMultiply();
  double x = axisAngle->GetTuple4(transformNumber)[0];
  double y = axisAngle->GetTuple4(transformNumber)[1];
  double z = axisAngle->GetTuple4(transformNumber)[2];
  double w = axisAngle->GetTuple4(transformNumber)[3]
      * 180 / vtkMath::Pi(); // vtk need degrees not radians
  transform->RotateWXYZ(w, x, y, z);
  transform->Translate(this->GetTranslationArray()->GetTuple(transformNumber));

  return transform;
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkCustomTransformInterpolator> vtkTemporalTransforms::CreateInterpolator()
{
  auto interpolator = vtkSmartPointer<vtkCustomTransformInterpolator>::New();

  auto timestamp = this->GetTimeArray();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    // get timestamp
    double currentTimestamp = timestamp->GetTuple1(i);

    // create the tranform
    auto transform = this->GetTransform(i);

    // add the tranform to the interpolator
    if (!vtkMath::IsNan(currentTimestamp))
    {
      interpolator->AddTransform(currentTimestamp, transform);
    }
    else
    {
      vtkErrorMacro("Timestamp " << i << "is not a number")
    }
  }
  interpolator->Modified();
  return interpolator;
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::SetOrientationArray(vtkDoubleArray *array)
{
  if (array->GetNumberOfComponents() != 4)
  {
    vtkErrorMacro(<< "The orientation array has " << array->GetNumberOfComponents()
                  << ". 4 components are expected")
  }
  array->SetName(this->OrientationArrayName);
  this->GetPointData()->AddArray(array);
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::SetTranslationArray(vtkDataArray *array)
{
  if (array->GetNumberOfComponents() != 3)
  {
    vtkErrorMacro(<< "The translation array has " << array->GetNumberOfComponents()
                  << ". 3 components are expected")
  }
  auto points =  vtkSmartPointer<vtkPoints>::New();
  this->SetPoints(points);
  this->GetPoints()->SetData(array);

  // create the cell in the same time for visualization
  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(this->GetNumberOfPoints());
  for (vtkIdType i = 0; i < array->GetNumberOfTuples(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  this->SetLines(cell);
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::SetTimeArray(vtkDoubleArray *array)
{
  if (array->GetNumberOfComponents() != 1)
  {
    vtkErrorMacro(<< "The time array has " << array->GetNumberOfComponents()
                  << ". 1 component is expected")
  }
  array->SetName(this->TimeArrayName);
  this->GetPointData()->AddArray(array);
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::IsometricTransform(vtkSmartPointer<vtkTransform> H)
{
  vtkSmartPointer<vtkTemporalTransforms> outputPoses = vtkSmartPointer<vtkTemporalTransforms>::New();
  outputPoses->DeepCopy(this);

  // First, compute the rotation and translation from H 4x4 matrix
  std::pair<Eigen::Vector3d, Eigen::Vector3d> transParams = GetPoseParamsFromTransform(H);
  Eigen::Matrix3d R0 = RollPitchYawToMatrix(transParams.first);
  Eigen::Vector3d T0 = transParams.second;

  // We need to change the angle axis and position arrays
  vtkDoubleArray* xyzwArray = vtkDoubleArray::SafeDownCast(outputPoses->GetOrientationArray());
  vtkDoubleArray* xyzArray = vtkDoubleArray::SafeDownCast(outputPoses->GetTranslationArray());

  // Loop over the transforms points
  for (unsigned int transformIndex = 0; transformIndex < outputPoses->GetNumberOfPoints(); transformIndex++)
  {
    // Get the angle-axis representation
    double* xyzw = xyzwArray->GetTuple4(transformIndex);
    double* xyz = xyzArray->GetTuple3(transformIndex);

    // Compute the corresponding rotation matrix that
    // correspond to the angle axis representation
    Eigen::Vector3d T(xyz[0], xyz[1], xyz[2]);
    Eigen::AngleAxisd angleAxis(xyzw[3], Eigen::Vector3d(xyzw[0], xyzw[1], xyzw[2]));
    Eigen::Matrix3d R = angleAxis.toRotationMatrix();

    // Compute the new rotation and translation
    R = R0 * R;
    T = R0 * T + T0;

    // Get the axis angle representation of this new rotation
    Eigen::AngleAxisd newAngleAxis(R);
    double newXyzw[4] = {newAngleAxis.axis()(0), newAngleAxis.axis()(1),
                         newAngleAxis.axis()(2), newAngleAxis.angle()};

    // Replace it
    xyzwArray->SetTuple4(transformIndex, newXyzw[0], newXyzw[1], newXyzw[2], newXyzw[3]);
    xyzArray->SetTuple3(transformIndex, T(0), T(1), T(2));
  }
  return outputPoses;
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::CycloidicTransform(vtkSmartPointer<vtkTransform> H)
{
  vtkSmartPointer<vtkTemporalTransforms> outputPoses = vtkSmartPointer<vtkTemporalTransforms>::New();
  outputPoses->DeepCopy(this);

  // First, compute the rotation and translation from H 4x4 matrix
  std::pair<Eigen::Vector3d, Eigen::Vector3d> transParams = GetPoseParamsFromTransform(H);
  Eigen::Matrix3d R0 = RollPitchYawToMatrix(transParams.first);
  Eigen::Vector3d T0 = transParams.second;

  // We need to change the angle axis and position arrays
  vtkDoubleArray* xyzwArray = vtkDoubleArray::SafeDownCast(this->GetOrientationArray());
  vtkDoubleArray* xyzArray = vtkDoubleArray::SafeDownCast(this->GetTranslationArray());

  // Loop over the transforms points
  for (unsigned int transformIndex = 0; transformIndex < this->GetNumberOfPoints(); transformIndex++)
  {
    // Get the angle-axis representation
    double* xyzw = xyzwArray->GetTuple4(transformIndex);
    double* xyz = xyzArray->GetTuple3(transformIndex);

    // Compute the corresponding rotation matrix that
    // correspond to the angle axis representation
    Eigen::Vector3d T(xyz[0], xyz[1], xyz[2]);
    Eigen::AngleAxisd angleAxis(xyzw[3], Eigen::Vector3d(xyzw[0], xyzw[1], xyzw[2]));
    Eigen::Matrix3d R = angleAxis.toRotationMatrix();

    // Compute the new rotation and translation
    T = R * T0 + T;
    R = R * R0;

    // Get the axis angle representation of this new rotation
    Eigen::AngleAxisd newAngleAxis(R);
    double newXyzw[4] = {newAngleAxis.axis()(0), newAngleAxis.axis()(1),
                         newAngleAxis.axis()(2), newAngleAxis.angle()};

    // Replace it
    xyzwArray->SetTuple4(transformIndex, newXyzw[0], newXyzw[1], newXyzw[2], newXyzw[3]);
    xyzArray->SetTuple3(transformIndex, T(0), T(1), T(2));
  }
  return outputPoses;
}

//-----------------------------------------------------------------------------
vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::MLSSmoothing(int polDeg, int kernelRadius)
{
  auto outputPoses = vtkSmartPointer<vtkTemporalTransforms>::New();
  outputPoses->DeepCopy(this);

  // We need to change the angle axis and position arrays
  vtkDoubleArray* anglesAxisArray = vtkDoubleArray::SafeDownCast(this->GetOrientationArray());
  vtkDoubleArray* xyzArray = vtkDoubleArray::SafeDownCast(this->GetTranslationArray());

  // First, convert the positions coordinates and the orientation
  // using Eigen container
  std::vector<Eigen::VectorXd> X, Y;
  std::vector<Eigen::VectorXd> Qin, Qout;
  for (int k = 0; k < outputPoses->GetNumberOfPoints(); ++k)
  {
    Eigen::VectorXd x(3, 1);
    Eigen::VectorXd q(4, 1);
    double currPos[3];
    // Positions
    outputPoses->GetPoint(k, currPos);
    x << currPos[0], currPos[1], currPos[2];
    X.push_back(x);

    // Orientations
    double* xyzw = anglesAxisArray->GetTuple4(k);
    Eigen::AngleAxisd angleAxis(xyzw[3], Eigen::Vector3d(xyzw[0], xyzw[1], xyzw[2]));
    Eigen::Quaterniond quat(angleAxis);
    q << quat.w(), quat.x(), quat.y(), quat.z();
    Qin.push_back(q);
  }

  // Smooth the trajectory
  EuclideanMLSSmoothing(X, Y, polDeg, kernelRadius);

  // Smooth the orientations using an euclidean
  // MLS algorithm and then reprojecting the points
  // onto the unit quaternion sphere
  EuclideanMLSSmoothing(Qin, Qout, polDeg, kernelRadius);

  // Set the points
  for (int k = 0; k < outputPoses->GetNumberOfPoints(); ++k)
  {
    // Positions
    double pos[3] = {Y[k](0), Y[k](1), Y[k](2)};
    outputPoses->GetPoints()->SetPoint(k, pos);
    xyzArray->SetTuple3(k, pos[0], pos[1], pos[2]);

    // Orientations
    Eigen::Quaterniond quat(Qout[k](0), Qout[k](1), Qout[k](2), Qout[k](3));
    quat.normalize();
    Eigen::AngleAxisd axisAngle(quat);
    double xyzw[4] = {axisAngle.axis()(0), axisAngle.axis()(1), axisAngle.axis()(2), axisAngle.angle()};
    anglesAxisArray->SetTuple4(k, xyzw[0], xyzw[1], xyzw[2], xyzw[3]);
  }
  return outputPoses;
}

//-----------------------------------------------------------------------------
void vtkTemporalTransforms::PushBack(double time, const Eigen::AngleAxisd& orientation , const Eigen::Vector3d translation)
{
  this->GetTimeArray()->InsertNextTuple1(time);
  this->GetOrientationArray()->InsertNextTuple4(orientation.axis()[0],
                                                orientation.axis()[1],
                                                orientation.axis()[2],
                                                orientation.angle());
  this->GetTranslationArray()->InsertNextTuple(static_cast<const double*>(translation.data()));

  if (this->GetNumberOfPoints() == 1)
  {
    vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
    this->SetLines(lines);
    return; // not enough points (1) to create a new line
  }

  // if control reaches this line, we have at least two points,
  // we append one small line between the new point and the previous one
  vtkSmartPointer<vtkCellArray> lines = this->GetLines();
  vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
  line->GetPointIds()->SetId(0, this->GetNumberOfPoints() - 2);
  line->GetPointIds()->SetId(1, this->GetNumberOfPoints() - 1);
  lines->InsertNextCell(line);
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::ExtractTimes(double tstart, double tend)
{
  auto extract = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    // get timestamp
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    if (currentTimestamp < tstart || currentTimestamp > tend) {
      continue;
    }

    extract->GetTimeArray()->InsertNextTuple1(currentTimestamp);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    extract->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    extract->GetTranslationArray()->InsertNextTuple(xyz);
  }

  return extract;
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::Subsample(int N)
{

  auto extract = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    if (i % N != 0)
    {
      continue;
    }
    // get timestamp
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    extract->GetTimeArray()->InsertNextTuple1(currentTimestamp);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    extract->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    extract->GetTranslationArray()->InsertNextTuple(xyz);
  }

  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(extract->GetNumberOfPoints());
  for (vtkIdType i = 0; i < extract->GetNumberOfPoints(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  extract->SetLines(cell);

  return extract;
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::ApplyTimeshift(double shift)
{
  auto timeshifted = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    timeshifted->GetTimeArray()->InsertNextTuple1(currentTimestamp + shift);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    timeshifted->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    timeshifted->GetTranslationArray()->InsertNextTuple(xyz);
  }

  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(timeshifted->GetNumberOfPoints());
  for (vtkIdType i = 0; i < timeshifted->GetNumberOfPoints(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  timeshifted->SetLines(cell);

  bool isWellFormed = timeshifted->GetTimeArray() &&
                      timeshifted->GetTranslationArray() &&
                      timeshifted->GetOrientationArray();
  if(!isWellFormed)
  {
    std::cout << "warning ! timeshifted not well formed" << std::endl;
  }

  return timeshifted;
}

vtkSmartPointer<vtkTemporalTransforms> vtkTemporalTransforms::ApplyScale(double scale)
{
  auto scaled = vtkSmartPointer<vtkTemporalTransforms>::New();

  for (unsigned int i = 0; i < this->GetNumberOfPoints(); i++)
  {
    double currentTimestamp = this->GetTimeArray()->GetTuple1(i);
    scaled->GetTimeArray()->InsertNextTuple1(currentTimestamp);
    double* aa = this->GetOrientationArray()->GetTuple4(i);
    scaled->GetOrientationArray()->InsertNextTuple(aa);
    double* xyz = this->GetTranslationArray()->GetTuple3(i);
    xyz[0] *= scale;
    xyz[1] *= scale;
    xyz[2] *= scale;
    scaled->GetTranslationArray()->InsertNextTuple(xyz);
  }

  auto polyLine = vtkSmartPointer<vtkPolyLine>::New();
  polyLine->GetPointIds()->SetNumberOfIds(scaled->GetNumberOfPoints());
  for (vtkIdType i = 0; i < scaled->GetNumberOfPoints(); i++)
  {
    polyLine->GetPointIds()->SetId(i,i);
  }
  auto cell = vtkSmartPointer<vtkCellArray>::New();
  cell->InsertNextCell(polyLine);
  scaled->SetLines(cell);

  bool isWellFormed = scaled->GetTimeArray() && scaled->GetTranslationArray() &&
                      scaled->GetOrientationArray();
  if(!isWellFormed)
  {
    std::cout << "warning ! scaled not well formed" << std::endl;
  }

  return scaled;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief test_absolute.cpp

\details
Test absolute units.

Output:
@verbatim
@endverbatim
**/

#include <boost/units/quantity.hpp>
#include <boost/units/absolute.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/make_system.hpp>
#include <boost/units/physical_dimensions.hpp>
#include <boost/units/base_units/si/kelvin.hpp>
#include <boost/units/base_units/temperature/celsius.hpp>
#include <boost/units/base_units/temperature/fahrenheit.hpp>

#include <iostream>

#include <boost/test/minimal.hpp>

#define BOOST_UNITS_CHECK_CLOSE(a, b) (BOOST_CHECK((std::abs((a) - (b)) < .0000001)))

namespace bu = boost::units;
using bu::si::kelvin_base_unit;
using bu::temperature::celsius_base_unit;
using bu::temperature::fahrenheit_base_unit;

typedef bu::unit<bu::temperature_dimension,bu::make_system<kelvin_base_unit>::type> kelvin_type;

typedef bu::unit<bu::temperature_dimension,bu::make_system<celsius_base_unit>::type> celsius_type;

typedef bu::unit<bu::temperature_dimension,bu::make_system<fahrenheit_base_unit>::type> fahrenheit_type;

int test_main(int,char *[])
{
    bu::quantity<bu::absolute<fahrenheit_type> > q1(212.0 * bu::absolute<fahrenheit_type>());
    bu::quantity<bu::absolute<celsius_type> > q2(0.0 * bu::absolute<celsius_type>());
    bu::quantity<bu::absolute<fahrenheit_type> > q3(q2);
    bu::quantity<fahrenheit_type> q4(q1 - q3);

    BOOST_UNITS_CHECK_CLOSE(q4.value(), 180.0);

    bu::quantity<bu::absolute<kelvin_type> > q5(static_cast<bu::quantity<kelvin_type> >(q4) + static_cast<bu::quantity<bu::absolute<kelvin_type> > >(q2));

    BOOST_UNITS_CHECK_CLOSE(q5.value(), 373.15);

    bu::quantity<bu::absolute<fahrenheit_type> > q6(q5);

    BOOST_UNITS_CHECK_CLOSE(q6.value(), 212.0);

    return(0);
}