/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/
#ifndef itkTriangleMeshToSimplexMeshFilter_hxx
#define itkTriangleMeshToSimplexMeshFilter_hxx
#include "itkTriangleMeshToSimplexMeshFilter.h"

namespace itk
{
template <typename TInputMesh, typename TOutputMesh>
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::TriangleMeshToSimplexMeshFilter()
  : m_FaceSet(nullptr)
  , m_Edges(nullptr)
  , m_EdgeNeighborList(nullptr)
  , m_VertexNeighborList(nullptr)
  , m_LineCellIndices(nullptr)
  , m_CellIdxOffset(0)
  , m_IdOffset(0)
  , m_EdgeCellId(0)
  , m_HandledEdgeIds(IdVectorType::New())
{
  OutputMeshPointer output = TOutputMesh::New();

  this->ProcessObject::SetNumberOfRequiredOutputs(1);
  this->ProcessObject::SetNthOutput(0, output.GetPointer());
}

template <typename TInputMesh, typename TOutputMesh>
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::~TriangleMeshToSimplexMeshFilter()
{
  delete m_FaceSet;
}

template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::GenerateData()
{
  this->Initialize();
  this->CreateSimplexPoints();
  this->CreateSimplexNeighbors();
  this->CreateCells();
  this->GetOutput()->BuildCellLinks();
}

template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::Initialize()
{

  const InputMeshType * input = this->GetInput(0);

  m_IdOffset = input->GetNumberOfCells();

  m_EdgeCellId = 0;
  m_EdgeNeighborList = EdgeNeighborListType::New();
  m_VertexNeighborList = VertexNeighborListType::New();
  m_LineCellIndices = LineCellIndexType::New();
  m_Edges = EdgeMapType::New();
  delete m_FaceSet;

  m_FaceSet = new IndexSetType();

  InputPointType v1, v2, v3;

  for (unsigned int idx1 = 0; idx1 < m_IdOffset; idx1++)
  {
    m_FaceSet->insert(idx1);

    CellAutoPointer triCellPointer;
    input->GetCell(idx1, triCellPointer);
    const PointIdentifier * tp = triCellPointer->GetPointIds();
    if (!input->GetPoint(tp[0], &v1))
    {
      itkExceptionMacro("Point with id " << tp[0] << " does not exist in the input mesh");
    }
    if (!input->GetPoint(tp[1], &v2))
    {
      itkExceptionMacro("Point with id " << tp[1] << " does not exist in the input mesh");
    }
    if (!input->GetPoint(tp[2], &v3))
    {
      itkExceptionMacro("Point with id " << tp[2] << " does not exist in the input mesh");
    }

    this->CreateNewEdge(idx1, 0, tp[0], tp[1], input);
    this->CreateNewEdge(idx1, 1, tp[1], tp[2], input);
    this->CreateNewEdge(idx1, 2, tp[2], tp[0], input);
  }
}

template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::CreateSimplexPoints()
{
  // create the points of the simplex mesh
  auto faceIterator = m_FaceSet->begin();

  const InputMeshType * input = this->GetInput(0);
  TOutputMesh *         output = this->GetOutput();

  while (faceIterator != m_FaceSet->end())
  {
    InputPointType  newPoint = ComputeFaceCenter(*faceIterator, input);
    OutputPointType copyPoint;
    copyPoint.CastFrom(newPoint);

    unsigned int id = *faceIterator;
    output->SetPoint(id, copyPoint);
    output->SetGeometryData(id, new itk::SimplexMeshGeometry());
    faceIterator++;
  }
}

template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::CreateEdgeForTrianglePair(CellIdentifier pointIndex,
                                                                                    CellIdentifier boundaryId,
                                                                                    TOutputMesh *  outputMesh)
{
  EdgeIdentifierType facePair = m_EdgeNeighborList->GetElement(boundaryId);

  if (facePair.first == pointIndex)
  {
    outputMesh->AddNeighbor(pointIndex, facePair.second);
  }
  else
  {
    outputMesh->AddNeighbor(pointIndex, facePair.first);
  }

  if (!m_HandledEdgeIds->IndexExists(boundaryId))
  {
    CellIdentifier edgeId = outputMesh->AddEdge(facePair.first, facePair.second);
    m_LineCellIndices->InsertElement(facePair, edgeId);
    m_HandledEdgeIds->InsertElement(boundaryId, edgeId);
  }
}

template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::CreateSimplexNeighbors()
{
  TOutputMesh * output = this->GetOutput(0);

  // add neighbor vertices
  OutputPointsContainerPointer  outputPointsContainer = output->GetPoints();
  OutputPointsContainerIterator points = outputPointsContainer->Begin();

  CellIdentifier tp0, tp1, tp2;

  InputBoundaryAssignmentsContainerPointer cntlines = this->GetInput(0)->GetBoundaryAssignments(1);

  while (points != outputPointsContainer->End())
  {
    PointIdentifier                    idx = points.Index();
    InputBoundnaryAssignmentIdentifier key0(idx, 0);
    InputBoundnaryAssignmentIdentifier key1(idx, 1);
    InputBoundnaryAssignmentIdentifier key2(idx, 2);

    tp0 = cntlines->GetElement(key0);
    tp1 = cntlines->GetElement(key1);
    tp2 = cntlines->GetElement(key2);

    CreateEdgeForTrianglePair(idx, tp0, output);
    CreateEdgeForTrianglePair(idx, tp1, output);
    CreateEdgeForTrianglePair(idx, tp2, output);

    ++points;
  }
}

template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::CreateNewEdge(CellIdentifier        currentCellId,
                                                                        CellFeatureIdentifier featureId,
                                                                        PointIdentifier       startPointId,
                                                                        PointIdentifier       endPointId,
                                                                        const InputMeshType * input)
{
  CellIdentifier boundaryId;

  // The filter shouldn't modify the input...
  auto * nonConstInput = const_cast<InputMeshType *>(input);

  EdgeIdentifierType edge = std::make_pair(startPointId, endPointId);
  EdgeIdentifierType edgeInv = std::make_pair(endPointId, startPointId);

  if (!m_Edges->IndexExists(edge) && !m_Edges->IndexExists(edgeInv))
  {
    boundaryId = m_IdOffset + m_EdgeCellId;
    m_Edges->InsertElement(edge, boundaryId);
    m_NewInputMeshCellPointer.TakeOwnership(new LineType);
    m_NewInputMeshCellPointer->SetPointId(0, startPointId);
    m_NewInputMeshCellPointer->SetPointId(1, endPointId);
    nonConstInput->SetCell(boundaryId, m_NewInputMeshCellPointer);
    m_EdgeCellId++;
  }
  else
  {
    if (m_Edges->IndexExists(edge))
    {
      boundaryId = m_Edges->GetElement(edge);
    }
    else
    {
      boundaryId = m_Edges->GetElement(edgeInv);
    }
  }
  nonConstInput->SetBoundaryAssignment(1, currentCellId, featureId, boundaryId);

  if (!m_EdgeNeighborList->IndexExists(boundaryId))
  {
    EdgeIdentifierType neighboringCells =
      std::make_pair(currentCellId, (CellIdentifier)NumericTraits<CellIdentifier>::max());
    m_EdgeNeighborList->InsertElement(boundaryId, neighboringCells);
  }
  else
  {
    EdgeIdentifierType neighboringCells = m_EdgeNeighborList->GetElement(boundaryId);

    if (neighboringCells.second == NumericTraits<CellIdentifier>::max())
    {
      neighboringCells.second = currentCellId;
    }
    m_EdgeNeighborList->InsertElement(boundaryId, neighboringCells);
  }

  if (!m_VertexNeighborList->IndexExists(startPointId))
  {
    IndexSetType neighborEdges;
    neighborEdges.insert(boundaryId);
    m_VertexNeighborList->InsertElement(startPointId, neighborEdges);
  }
  else
  {
    IndexSetType neighborEdges = m_VertexNeighborList->GetElement(startPointId);
    neighborEdges.insert(boundaryId);
    m_VertexNeighborList->InsertElement(startPointId, neighborEdges);
  }

  if (!m_VertexNeighborList->IndexExists(endPointId))
  {
    IndexSetType neighborEdges;
    neighborEdges.insert(boundaryId);
    m_VertexNeighborList->InsertElement(endPointId, neighborEdges);
  }
  else
  {
    IndexSetType neighborEdges = m_VertexNeighborList->GetElement(endPointId);
    neighborEdges.insert(boundaryId);
    m_VertexNeighborList->InsertElement(endPointId, neighborEdges);
  }
}

/* PrintSelf. */
template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "ToDo: implement PrinSelf!!!";
}

template <typename TInputMesh, typename TOutputMesh>
void
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::CreateCells()
{
  const InputPointsContainer *      pointsContainer = this->GetInput(0)->GetPoints();
  InputPointsContainerConstIterator points = pointsContainer->Begin();
  TOutputMesh *                     outputMesh = this->GetOutput();
  PointIdentifier                   idx;

  using MapType = itk::MapContainer<CellIdentifier, CellIdentifier>;

  while (points != pointsContainer->End())
  {
    idx = points.Index();
    IndexSetType vertexNeighbors = m_VertexNeighborList->GetElement(idx);

    auto iterator1 = vertexNeighbors.begin();

    typename MapType::Pointer tmpMap = MapType::New();
    CellIdentifier            startIdx = NumericTraits<CellIdentifier>::max(), lastIdx = 0, wrongIdx = 0;

    while (lastIdx != startIdx)
    {
      if (startIdx == NumericTraits<CellIdentifier>::max())
      {
        EdgeIdentifierType neighboringCells = m_EdgeNeighborList->GetElement(*iterator1);
        startIdx = neighboringCells.first;
        tmpMap->InsertElement(neighboringCells.first, neighboringCells.second);

        lastIdx = neighboringCells.second;
        wrongIdx = neighboringCells.first;
      }
      else
      {
        auto iterator2 = vertexNeighbors.begin();
        while (iterator2 != vertexNeighbors.end())
        {
          EdgeIdentifierType compare = m_EdgeNeighborList->GetElement(*iterator2);
          if (compare.first == lastIdx && compare.second != wrongIdx)
          {
            tmpMap->InsertElement(compare.first, compare.second);
            lastIdx = compare.second;
            wrongIdx = compare.first;
            break;
          }
          else if (compare.second == lastIdx && compare.first != wrongIdx)
          {
            tmpMap->InsertElement(compare.second, compare.first);
            lastIdx = compare.first;
            wrongIdx = compare.second;
            break;
          }
          iterator2++;
        }
      }
      iterator1++;
    }

    // create a new cell
    m_NewSimplexCellPointer.TakeOwnership(new OutputPolygonType);
    PointIdentifier       vertexIdx = NumericTraits<PointIdentifier>::ZeroValue();
    CellIdentifier        nextIdx = startIdx;
    CellFeatureIdentifier featureId = 0;

    CellIdentifier faceIndex = outputMesh->AddFace(m_NewSimplexCellPointer);

    while (tmpMap->IndexExists(nextIdx))
    {
      m_NewSimplexCellPointer->SetPointId(vertexIdx++, nextIdx);
      CellIdentifier newIdx = tmpMap->GetElement(nextIdx);

      EdgeIdentifierType line = std::make_pair(nextIdx, newIdx);
      EdgeIdentifierType lineInv = std::make_pair(newIdx, nextIdx);

      CellIdentifier edgeIdx = NumericTraits<CellIdentifier>::ZeroValue();

      if (m_LineCellIndices->IndexExists(line))
      {
        edgeIdx = m_LineCellIndices->GetElement(line);
      }
      else if (m_LineCellIndices->IndexExists(lineInv))
      {
        edgeIdx = m_LineCellIndices->GetElement(lineInv);
      }
      else
      {
        std::cout << "error!!! " << std::endl;
      }
      outputMesh->SetBoundaryAssignment(1, faceIndex, featureId++, edgeIdx);

      if (newIdx == startIdx)
      {
        break;
      }
      else
      {
        nextIdx = newIdx;
      }
      featureId++;
    }
    ++points;
  }
}

template <typename TInputMesh, typename TOutputMesh>
typename TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::InputPointType
TriangleMeshToSimplexMeshFilter<TInputMesh, TOutputMesh>::ComputeFaceCenter(CellIdentifier        faceId,
                                                                            const InputMeshType * inputMesh)
{
  InputPointType v1, v2, v3;

  CellAutoPointer cellPointer;

  inputMesh->GetCell(faceId, cellPointer);
  const PointIdentifier * tp = cellPointer->GetPointIds();
  if (!inputMesh->GetPoint(tp[0], &v1))
  {
    itkExceptionMacro("Point with id " << tp[0] << " does not exist in the input mesh");
  }
  if (!inputMesh->GetPoint(tp[1], &v2))
  {
    itkExceptionMacro("Point with id " << tp[1] << " does not exist in the input mesh");
  }
  if (!inputMesh->GetPoint(tp[2], &v3))
  {
    itkExceptionMacro("Point with id " << tp[2] << " does not exist in the input mesh");
  }

  InputPointType result;
  result[0] = ((double)(v1[0] + v2[0] + v3[0])) / 3.0;
  result[1] = ((double)(v1[1] + v2[1] + v3[1])) / 3.0;
  result[2] = ((double)(v1[2] + v2[2] + v3[2])) / 3.0;

  return result;
}
} // namespace itk

#endif //_TRIANGLE_MESH_TO_SIMPLEX_MESH_FILTER_TXX
