/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkSimple3DCirclesStrategy.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm 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 "vtkSimple3DCirclesStrategy.h"

#include "vtkAbstractArray.h"
#include "vtkCharArray.h"       // For temporary store for point ordering
#include "vtkDirectedGraph.h"   // For this->Graph type check
#include "vtkIdTypeArray.h"     // For Ordered array
#include "vtkInEdgeIterator.h"  // For in edge(s) checks
#include "vtkIntArray.h"        // For hierarchy layers
#include "vtkMath.h"            // For cross, outer, norm and dot
#include "vtkObjectFactory.h"   // For VTK ::New() function
#include "vtkOutEdgeIterator.h" // For out edge(s) checks
#include "vtkPoints.h"          // For output target
#include "vtkSmartPointer.h"    // For good memory handling

#include <cmath> // For abs, sin, cos and tan

#include <algorithm> // For min, max, swap, etc.
#include <list>      // For internal store

template <class T>
bool IsZero(T value)
{
  return ((value < VTK_DBL_EPSILON) && (value > (-1.0 * VTK_DBL_EPSILON)));
}

class vtkSimple3DCirclesStrategyInternal
{
public:
  vtkSimple3DCirclesStrategyInternal() = default;
  vtkSimple3DCirclesStrategyInternal(const vtkSimple3DCirclesStrategyInternal& from)
  {
    if (&from != this)
      this->store = from.store;
  };
  vtkSimple3DCirclesStrategyInternal& operator=(const vtkSimple3DCirclesStrategyInternal& from)
  {
    if (&from != this)
      this->store = from.store;
    return *this;
  };
  vtkSimple3DCirclesStrategyInternal& operator=(const std::list<vtkIdType>& from)
  {
    this->store = from;
    return *this;
  };
  vtkIdType front() { return this->store.front(); };
  void pop_front() { this->store.pop_front(); };
  std::size_t size() { return this->store.size(); };
  void push_back(const vtkIdType& value) { this->store.push_back(value); };
  ~vtkSimple3DCirclesStrategyInternal() { this->store.clear(); };

private:
  std::list<vtkIdType> store;
};

vtkStandardNewMacro(vtkSimple3DCirclesStrategy);

void vtkSimple3DCirclesStrategy::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "Radius : " << this->Radius << endl;

  os << indent << "Height : " << this->Height << endl;

  os << indent << "Origin  : (" << this->Origin[0] << "," << this->Origin[1] << ","
     << this->Origin[2] << ")" << endl;

  os << indent << "Direction  : (" << this->Direction[0] << "," << this->Direction[1] << ","
     << this->Direction[2] << ")" << endl;

  os << indent << "Rotate matrix : [[" << this->T[0][0] << ";" << this->T[1][0] << ";"
     << this->T[2][0] << "]";
  os << "[" << this->T[0][1] << ";" << this->T[1][1] << ";" << this->T[2][1] << "]";
  os << "[" << this->T[0][2] << ";" << this->T[1][2] << ";" << this->T[2][2] << "]]" << endl;

  os << indent << "Method : ";
  if (this->Method == FixedRadiusMethod)
    os << "fixed radius method" << endl;
  else if (this->Method == FixedDistanceMethod)
    os << "fixed distance method" << endl;

  os << indent << "MarkValue : " << this->MarkedValue << endl;

  os << indent << "Auto height : ";
  if (this->AutoHeight == 1)
    os << "On" << endl;
  else
    os << "Off" << endl;

  os << indent << "Minimum degree for autoheight : " << this->MinimumRadian << " rad ["
     << vtkMath::DegreesFromRadians(this->MinimumRadian) << " deg]" << endl;

  os << indent << "Registered MarkedStartPoints :";
  if (this->MarkedStartVertices == nullptr)
    os << " (none)" << endl;
  else
  {
    os << endl;
    this->MarkedStartVertices->PrintSelf(os, indent.GetNextIndent());
  }

  os << indent << "Registered HierarchicalLayers :";
  if (this->HierarchicalLayers == nullptr)
    os << " (none)" << endl;
  else
  {
    os << endl;
    this->HierarchicalLayers->PrintSelf(os, indent.GetNextIndent());
  }

  os << indent << "Registered HierarchicalOrder :";
  if (this->HierarchicalOrder == nullptr)
    os << " (none)" << endl;
  else
  {
    os << endl;
    this->HierarchicalOrder->PrintSelf(os, indent.GetNextIndent());
  }

  os << indent
     << "ForceToUseUniversalStartPointsFinder :" << this->ForceToUseUniversalStartPointsFinder
     << endl;
}

void vtkSimple3DCirclesStrategy::SetDirection(double dx, double dy, double dz)
{
  vtkDebugMacro(<< this->GetClassName() << " (" << this << "): setting Direction to (" << dx << ","
                << dy << "," << dz << ")");

  if ((this->Direction[0] != dx) || (this->Direction[1] != dy) || (this->Direction[2] != dz))
  {
    double global[3], local[3];
    global[0] = dx;
    global[1] = dy;
    global[2] = dz;

    local[0] = 0.0;
    local[1] = 1.0;
    local[2] = 0.0;

    double length_global = vtkMath::Norm(global);

    if (IsZero(length_global))
    {
      vtkWarningMacro(<< "The length of direction vector is zero! Direction has not been changed!");
      return;
    }

    double cosfi, n[3], E[3][3], U[3][3], u[3][3], number;

    global[0] = global[0] / length_global;
    global[1] = global[1] / length_global;
    global[2] = global[2] / length_global;

    // http://en.citizendium.org/wiki/Rotation_matrix
    // we are going from local to global.
    // cos(fi) = local.global -> cosfi, because |local|=1 and |global|=1
    cosfi = vtkMath::Dot(local, global);
    // if fi == 2*Pi -> cosfi = -1
    if (IsZero(cosfi + 1.0))
    {
      // if "local" is on "z" axes
      if (IsZero(local[2] + 1.0) || IsZero(local[2] - 1.0))
      {
        this->T[0][0] = this->T[2][2] = -1.0;
        this->T[1][1] = 1.0;
        this->T[0][1] = this->T[1][0] = this->T[0][2] = this->T[2][0] = this->T[1][2] =
          this->T[2][1] = 0.0;
      }
      // if local != ( (0,0,1) or (0,0,-1) )
      else
      {
        // n vector
        n[0] = 1.0 / (1.0 - local[2] * local[2]) * local[1];
        n[1] = -1.0 / (1.0 - local[2] * local[2]) * local[0];
        n[2] = 0.0;
        // u = n X n
        vtkMath::Outer(n, n, u);

        // -E
        E[0][0] = E[1][1] = E[2][2] = -1.0;
        E[0][1] = E[1][0] = E[0][2] = E[2][0] = E[1][2] = E[2][1] = 0.0;

        // T = -E + 2*u
        int i, j;
        for (i = 0; i < 3; ++i)
          for (j = 0; j < 3; ++j)
            this->T[i][j] = E[i][j] + (u[i][j] * 2.0);
      }
    }
    // fi < 2*Pi
    else
    {
      // n = local x global -> n(nx,ny,nz)
      vtkMath::Cross(local, global, n);
      //
      // cos(fi)*E
      //
      E[0][0] = E[1][1] = E[2][2] = cosfi;
      E[0][1] = E[1][0] = E[0][2] = E[2][0] = E[1][2] = E[2][1] = 0.0;
      //                 |  0  -nz  ny |
      // U = sin(fi)*N = |  nz  0  -nx |
      //                 | -ny  nx  0  |
      U[0][0] = U[1][1] = U[2][2] = 0.0;
      U[0][1] = -1.0 * n[2];
      U[1][0] = n[2];
      U[0][2] = n[1];
      U[2][0] = -1.0 * n[1];
      U[1][2] = -1.0 * n[0];
      U[2][1] = n[0];

      // u = n X n
      vtkMath::Outer(n, n, u);

      int i, j;
      // T = cos(fi)*E + U + 1/(1+cos(fi))*[n X n]
      // [ number = 1/(1+cos(fi)) ]
      number = 1.0 / (1.0 + cosfi);
      for (i = 0; i < 3; ++i)
        for (j = 0; j < 3; ++j)
          this->T[i][j] = E[i][j] + U[i][j] + (u[i][j] * number);
    }

    this->Direction[0] = dx;
    this->Direction[1] = dy;
    this->Direction[2] = dz;

    vtkDebugMacro(<< "Transformation matrix : [[" << this->T[0][0] << "," << this->T[1][0] << ","
                  << this->T[2][0] << "][" << this->T[0][1] << "," << this->T[1][1] << ","
                  << this->T[2][1] << "][" << this->T[0][2] << "," << this->T[1][2] << ","
                  << this->T[2][2] << "]]");

    this->Modified();
  }
}

void vtkSimple3DCirclesStrategy::SetDirection(double d[3])
{
  this->SetDirection(d[0], d[1], d[2]);
}

vtkCxxSetObjectMacro(vtkSimple3DCirclesStrategy, MarkedStartVertices, vtkAbstractArray);

void vtkSimple3DCirclesStrategy::SetMarkedValue(vtkVariant val)
{
  if (!this->MarkedValue.IsEqual(val))
  {
    this->MarkedValue = val;
    vtkDebugMacro(<< "Setting MarkedValue : " << this->MarkedValue);
    this->Modified();
  }
}

vtkVariant vtkSimple3DCirclesStrategy::GetMarkedValue()
{
  return this->MarkedValue;
}

void vtkSimple3DCirclesStrategy::SetMinimumDegree(double degree)
{
  this->SetMinimumRadian(vtkMath::RadiansFromDegrees(degree));
}

double vtkSimple3DCirclesStrategy::GetMinimumDegree()
{
  return vtkMath::DegreesFromRadians(this->GetMinimumRadian());
}

vtkCxxSetObjectMacro(vtkSimple3DCirclesStrategy, HierarchicalLayers, vtkIntArray);
vtkCxxSetObjectMacro(vtkSimple3DCirclesStrategy, HierarchicalOrder, vtkIdTypeArray);

vtkSimple3DCirclesStrategy::vtkSimple3DCirclesStrategy()
  : Radius(1)
  , Height(1)
  , Method(FixedRadiusMethod)
  , MarkedStartVertices(nullptr)
  , ForceToUseUniversalStartPointsFinder(0)
  , AutoHeight(0)
  , MinimumRadian(vtkMath::Pi() / 6.0)
  , HierarchicalLayers(nullptr)
  , HierarchicalOrder(nullptr)
{
  this->Direction[0] = this->Direction[1] = 0.0;
  this->Direction[2] = 1.0;
  this->T[0][1] = this->T[0][2] = this->T[1][2] = 0.0;
  this->T[1][0] = this->T[2][0] = this->T[2][1] = 0.0;
  this->T[0][0] = this->T[1][1] = this->T[2][2] = 1.0;
  this->Origin[0] = this->Origin[1] = this->Origin[2] = 0.0;
}

vtkSimple3DCirclesStrategy::~vtkSimple3DCirclesStrategy()
{
  this->SetMarkedStartVertices(nullptr);
  this->SetHierarchicalLayers(nullptr);
  this->SetHierarchicalOrder(nullptr);
}

void vtkSimple3DCirclesStrategy::Layout()
{
  if (this->Graph == nullptr)
  {
    vtkErrorMacro(<< "Graph is null!");
    return;
  }
  if (this->Graph->GetNumberOfVertices() == 0)
  {
    vtkDebugMacro(<< "Graph is empty (no no vertices)!");
    return;
  }

  vtkSmartPointer<vtkDirectedGraph> target = vtkSmartPointer<vtkDirectedGraph>::New();
  if (!target->CheckedShallowCopy(this->Graph))
  {
    vtkErrorMacro(<< "Graph must be directed graph!");
    return;
  }

  vtkSimple3DCirclesStrategyInternal start_points, order_points, stand_alones;

  // Layers begin
  vtkSmartPointer<vtkIntArray> layers = nullptr;
  if (this->HierarchicalLayers != nullptr)
  {
    if ((this->HierarchicalLayers->GetMaxId() + 1) == target->GetNumberOfVertices())
    {
      layers = this->HierarchicalLayers;
    }
  }
  if (layers == nullptr)
  {
    layers = vtkSmartPointer<vtkIntArray>::New();
    if (this->HierarchicalLayers != nullptr)
      this->HierarchicalLayers->UnRegister(this);
    this->HierarchicalLayers = layers;
    this->HierarchicalLayers->Register(this);

    layers->SetNumberOfValues(target->GetNumberOfVertices());
    for (vtkIdType i = 0; i <= layers->GetMaxId(); ++i)
      layers->SetValue(i, -1);

    if (this->UniversalStartPoints(target, &start_points, &stand_alones, layers) == -1)
    {
      vtkErrorMacro(<< "There is no start point!");
      return;
    }
    order_points = start_points;
    this->BuildLayers(target, &start_points, layers);
  }
  else
  {
    for (vtkIdType i = 0; i <= layers->GetMaxId(); ++i)
    {
      if (layers->GetValue(i) == 0)
        order_points.push_back(i);
      else if (layers->GetValue(i) == -2)
        stand_alones.push_back(i);
    }
  }
  // Layers end

  // Order begin
  vtkSmartPointer<vtkIdTypeArray> order = nullptr;
  if (this->HierarchicalOrder != nullptr)
  {
    if ((this->HierarchicalOrder->GetMaxId() + 1) == target->GetNumberOfVertices())
    {
      order = this->HierarchicalOrder;
    }
  }

  if (order == nullptr)
  {
    order = vtkSmartPointer<vtkIdTypeArray>::New();
    if (this->HierarchicalOrder != nullptr)
      this->HierarchicalOrder->UnRegister(this);
    this->HierarchicalOrder = order;
    this->HierarchicalOrder->Register(this);
    order->SetNumberOfValues(target->GetNumberOfVertices());
    for (vtkIdType i = 0; i <= order->GetMaxId(); ++i)
      order->SetValue(i, -1);

    this->BuildPointOrder(target, &order_points, &stand_alones, layers, order);
  }
  // Order end

  if (order->GetValue(order->GetMaxId()) == -1)
  {
    vtkErrorMacro(<< "Not all parts of the graph is accessible. There may be a loop.");
    return;
  }

  int index = 0;
  int layer = 0;
  int start = 0;
  double R = this->Radius;
  double Rprev = 0.0;
  double localXYZ[3], globalXYZ[3], localHeight = this->Height;
  double alfa = 0.0;
  double tangent = tan(vtkMath::Pi() / 2 - this->MinimumRadian);
  int ind = 0;

  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
  points->SetNumberOfPoints(target->GetNumberOfVertices());

  while (index <= order->GetMaxId())
  {
    start = index;
    R = this->Radius;
    layer = layers->GetValue(order->GetValue(index));
    while (index <= order->GetMaxId())
    {
      if (layers->GetValue(order->GetValue(index)) == layer)
        ++index;
      else
        break;
    }

    alfa = 2.0 * vtkMath::Pi() / double(index - start);

    if (this->Method == FixedDistanceMethod)
    {
      R = double(index - start - 1) * this->Radius / vtkMath::Pi();
    }
    else if (this->Method == FixedRadiusMethod)
    {
      if ((index - start) == 1)
        R = 0.0;
    }
    else
    {
      vtkErrorMacro(<< "Method must be FixedRadiusMethod or FixedDistanceMethod!");
      return;
    }

    if ((this->AutoHeight == 1) && (this->Method == FixedDistanceMethod))
    {
      if (fabs(tangent * (R - Rprev)) > this->Height)
        localHeight = fabs(tangent * (R - Rprev));
      else
        localHeight = this->Height;
    }

    if (layer != 0)
      localXYZ[2] = localXYZ[2] + localHeight;
    else
      localXYZ[2] = 0.0;

    for (ind = start; ind < index; ++ind)
    {
      localXYZ[0] = R * cos(double(ind - start) * alfa);
      localXYZ[1] = R * sin(double(ind - start) * alfa);
      this->Transform(localXYZ, globalXYZ);
      points->SetPoint(order->GetValue(ind), globalXYZ);
    }

    Rprev = R;
  }

  this->Graph->SetPoints(points);
  vtkDebugMacro(<< "vtkPoints is added to the graph. Vertex layout is ready.");
}

void vtkSimple3DCirclesStrategy::SetGraph(vtkGraph* graph)
{
  if (this->Graph != graph)
  {
    this->Superclass::SetGraph(graph);
    if (this->HierarchicalLayers != nullptr)
    {
      this->HierarchicalLayers->UnRegister(this);
      this->HierarchicalLayers = nullptr;
    }
    if (this->HierarchicalOrder != nullptr)
    {
      this->HierarchicalOrder->UnRegister(this);
      this->HierarchicalOrder = nullptr;
    }
  }
}

int vtkSimple3DCirclesStrategy::UniversalStartPoints(vtkDirectedGraph* input,
  vtkSimple3DCirclesStrategyInternal* target, vtkSimple3DCirclesStrategyInternal* StandAlones,
  vtkIntArray* layers)
{
  if ((this->MarkedStartVertices != nullptr) && (this->ForceToUseUniversalStartPointsFinder == 0))
  {
    if (this->MarkedStartVertices->GetMaxId() == layers->GetMaxId())
    {
      for (vtkIdType i = 0; i < input->GetNumberOfVertices(); ++i)
      {
        if ((input->GetInDegree(i) == 0) && (input->GetOutDegree(i) > 0))
        {
          target->push_back(i);
          layers->SetValue(i, 0);
        }
        else if ((input->GetInDegree(i) == 0) && (input->GetOutDegree(i) == 0))
        {
          layers->SetValue(i, -2);
          StandAlones->push_back(i);
        }
        else if ((this->MarkedStartVertices->GetVariantValue(i) == this->MarkedValue) &&
          (input->GetOutDegree(i) > 0))
        {
          target->push_back(i);
          layers->SetValue(i, 0);
        }
      }

      vtkDebugMacro(
        << "StartPoint finder: Universal start point finder was used. Number of start point(s): "
        << target->size() << "; Number of stand alone point(s): " << StandAlones->size());
      return static_cast<int>(target->size());
    }
    else
    {
      vtkErrorMacro(<< "MarkedStartPoints number is NOT equal number of vertices!");
      return -1;
    }
  }

  for (vtkIdType i = 0; i < input->GetNumberOfVertices(); ++i)
  {
    if ((input->GetInDegree(i) == 0) && (input->GetOutDegree(i) > 0))
    {
      target->push_back(i);
      layers->SetValue(i, 0);
    }
    else if ((input->GetInDegree(i) == 0) && (input->GetOutDegree(i) == 0))
    {
      layers->SetValue(i, -2);
      StandAlones->push_back(i);
    }
  }

  vtkDebugMacro(
    << "StartPoint finder: Universal start point finder was used. Number of start points: "
    << target->size() << "; Number of stand alone point(s): " << StandAlones->size());
  return static_cast<int>(target->size());
}

int vtkSimple3DCirclesStrategy::BuildLayers(
  vtkDirectedGraph* input, vtkSimple3DCirclesStrategyInternal* source, vtkIntArray* layers)
{
  vtkSmartPointer<vtkOutEdgeIterator> edge_out_iterator =
    vtkSmartPointer<vtkOutEdgeIterator>::New();
  vtkSmartPointer<vtkInEdgeIterator> edge_in_iterator = vtkSmartPointer<vtkInEdgeIterator>::New();
  int layer = 0, flayer = 0;
  vtkInEdgeType in_edge;
  vtkOutEdgeType out_edge;
  bool HasAllInput = true;
  vtkIdType ID = 0;
  int max_layer_id = -1;

  while (source->size() > 0)
  {
    ID = source->front();
    source->pop_front();

    input->GetOutEdges(ID, edge_out_iterator);

    while (edge_out_iterator->HasNext())
    {
      out_edge = edge_out_iterator->Next();
      if (layers->GetValue(out_edge.Target) == -1)
      {
        input->GetInEdges(out_edge.Target, edge_in_iterator);
        layer = layers->GetValue(ID);
        HasAllInput = true;

        while (edge_in_iterator->HasNext() && HasAllInput)
        {
          in_edge = edge_in_iterator->Next();
          flayer = layers->GetValue(in_edge.Source);
          if (flayer == -1)
            HasAllInput = false;
          layer = std::max(layer, flayer);
        }

        if (HasAllInput)
        {
          source->push_back(out_edge.Target);
          layers->SetValue(out_edge.Target, layer + 1);
          max_layer_id = std::max(max_layer_id, layer + 1);
        }
      }
    }
  }

  vtkDebugMacro(<< "Layer building is successful.");
  return max_layer_id;
}

void vtkSimple3DCirclesStrategy::BuildPointOrder(vtkDirectedGraph* input,
  vtkSimple3DCirclesStrategyInternal* source, vtkSimple3DCirclesStrategyInternal* StandAlones,
  vtkIntArray* layers, vtkIdTypeArray* order)
{
  vtkSmartPointer<vtkOutEdgeIterator> edge_out_iterator =
    vtkSmartPointer<vtkOutEdgeIterator>::New();
  vtkSmartPointer<vtkCharArray> mark = vtkSmartPointer<vtkCharArray>::New();
  vtkOutEdgeType out_edge;
  int step = 0;
  int layer = 0;
  vtkIdType ID = 0;

  mark->SetNumberOfValues(input->GetNumberOfVertices());
  for (vtkIdType i = 0; i <= mark->GetMaxId(); ++i)
    mark->SetValue(i, 0);

  while (source->size() > 0)
  {
    ID = source->front();
    source->pop_front();

    order->SetValue(step, ID);
    input->GetOutEdges(ID, edge_out_iterator);
    layer = layers->GetValue(ID) + 1;

    while (edge_out_iterator->HasNext())
    {
      out_edge = edge_out_iterator->Next();
      if (mark->GetValue(out_edge.Target) == 0)
      {
        if (layers->GetValue(out_edge.Target) == layer)
        {
          mark->SetValue(out_edge.Target, 1);
          source->push_back(out_edge.Target);
        }
      }
    }

    ++step;
  }

  while (StandAlones->size() > 0)
  {
    order->SetValue(step, StandAlones->front());
    StandAlones->pop_front();
    ++step;
  }

  vtkDebugMacro(<< "Vertex order building is successful.");
}

void vtkSimple3DCirclesStrategy::Transform(double Local[], double Global[])
{
  vtkMath::Multiply3x3(this->T, Local, Global);
  Global[0] = this->Origin[0] + Global[0];
  Global[1] = this->Origin[1] + Global[1];
  Global[2] = this->Origin[2] + Global[2];
}