//=========================================================================
//  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 "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkLine.h"
#include "vtkMergePoints.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkPolyData.h"
#include "vtkTransform.h"
#include "vtkTriangle.h"
#include "vtkIncrementalPointLocator.h"
#include "vtkPiecewiseFunction.h"
#include <vtkPolyDataNormals.h>
#include "vtkSmartPointer.h"
#include "vtkKochanekSpline.h"
#include "vtkTetra.h"
#include "vtkIntersectionPolyDataFilter.h"
#include "vtkPlaneSource.h"

#include "smtk/extension/vtk/filter/vtkCMBApplyBathymetryFilter.h"

#include <vtkTriangleFilter.h>

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

#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();
}
