/*
 * Copyright (C) 2005-2022 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * 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.
 */

#ifndef otbVectorDataToRightAngleVectorDataFilter_hxx
#define otbVectorDataToRightAngleVectorDataFilter_hxx

#include "otbVectorDataToRightAngleVectorDataFilter.h"
#include "otbVectorData.h"

namespace otb
{
template <class TVectorData>
VectorDataToRightAngleVectorDataFilter<TVectorData>::VectorDataToRightAngleVectorDataFilter()
{
  this->SetNumberOfRequiredInputs(1);
  this->SetNumberOfRequiredOutputs(1);

  m_DistanceThreshold = 20.;
  m_AngleThreshold    = CONST_PI / 30.; // we want a threshold at 6 degrees
}

template <class TVectorData>
void VectorDataToRightAngleVectorDataFilter<TVectorData>::GenerateData()
{
  // Get the input segments
  typename VectorDataType::Pointer vData = const_cast<VectorDataType*>(this->GetInput());

  // Output
  this->GetOutput(0)->SetMetaDataDictionary(vData->GetMetaDataDictionary());

  // Retrieving root node
  typename DataNodeType::Pointer root = this->GetOutput(0)->GetDataTree()->GetRoot()->Get();
  // Create the document node
  typename DataNodeType::Pointer document = DataNodeType::New();
  document->SetNodeType(otb::DOCUMENT);
  // Adding the layer to the data tree
  this->GetOutput(0)->GetDataTree()->Add(document, root);
  // Create the folder node
  typename DataNodeType::Pointer folder = DataNodeType::New();
  folder->SetNodeType(otb::FOLDER);
  this->GetOutput(0)->GetDataTree()->Add(folder, document);
  this->GetOutput(0)->SetProjectionRef(vData->GetProjectionRef());

  // Itterate on the vector data
  TreeIteratorType itVectorRef(vData->GetDataTree()); // Reference
  itVectorRef.GoToBegin();

  while (!itVectorRef.IsAtEnd())
  {
    if (!itVectorRef.Get()->IsLineFeature())
    {
      ++itVectorRef;
      continue; // do not process if it's not a line
    }
    TreeIteratorType itVectorCur = itVectorRef; // Current

    while (!itVectorCur.IsAtEnd())
    {
      if (!itVectorCur.Get()->IsLineFeature())
      {
        ++itVectorCur;
        continue; // do not process if it's not a line
      }
      // Compute the angle formed by the two segments
      double Angle = this->ComputeAngleFormedBySegments(itVectorRef.Get()->GetLine(), itVectorCur.Get()->GetLine());

      // Check if the angle is a right one
      if (std::abs(Angle - CONST_PI_2) <= m_AngleThreshold)
      {
        // Right angle coordinate
        PointType RightAngleCoordinate;
        RightAngleCoordinate = this->ComputeRightAngleCoordinate(itVectorRef.Get()->GetLine(), itVectorCur.Get()->GetLine());

        // Compute the distance between the two segments and the right angle formed by this segments
        double dist1_2 = this->ComputeDistanceFromPointToSegment(RightAngleCoordinate, itVectorRef.Get()->GetLine());
        double dist2_2 = this->ComputeDistanceFromPointToSegment(RightAngleCoordinate, itVectorCur.Get()->GetLine());

        double threshold_2 = m_DistanceThreshold * m_DistanceThreshold;
        if (dist1_2 < threshold_2 && dist2_2 < threshold_2)
        {
          // If Right Angle & not so far from segments: Add to the output
          typename DataNodeType::Pointer CurrentGeometry = DataNodeType::New();
          CurrentGeometry->SetNodeId("FEATURE_POINT");
          CurrentGeometry->SetNodeType(otb::FEATURE_POINT);
          CurrentGeometry->SetPoint(RightAngleCoordinate);
          this->GetOutput(0)->GetDataTree()->Add(CurrentGeometry, folder);
        }
      }

      ++itVectorCur;
    }
    ++itVectorRef;
  }
}

template <class TVectorData>
double VectorDataToRightAngleVectorDataFilter<TVectorData>::ComputeDistanceFromPointToSegment(PointType rAngle, LineType* line)
{
  VertexListType* vertexList = const_cast<VertexListType*>(line->GetVertexList());

  VertexType v0 = vertexList->GetElement(0);
  VertexType v1 = vertexList->GetElement(1);

  // Case the extremities of the segment are the same
  double l2 = v0.SquaredEuclideanDistanceTo(v1);

  // Is the projection of rAngle on the segment inside (0<u<1) or
  // inside the segment bounds
  double u = ((rAngle[0] - v0[0]) * (v1[0] - v0[0]) + (rAngle[1] - v0[1]) * (v1[1] - v0[1])) / l2;

  if (u < 1e-10)
    u = 0.;
  if (u - 1. > 1e-10)
    u = 1.;

  double x = v0[0] + u * (v1[0] - v0[0]);
  double y = v0[1] + u * (v1[1] - v0[1]);

  double dx = x - rAngle[0];
  double dy = y - rAngle[1];

  return dx * dx + dy * dy;
}

template <class TVectorData>
double VectorDataToRightAngleVectorDataFilter<TVectorData>::ComputeAngleFormedBySegments(LineType* lineDst, LineType* lineSrc)
{
  double oriDst = this->ComputeOrientation(lineDst);
  double oriSrc = this->ComputeOrientation(lineSrc);

  return std::abs(oriDst - oriSrc);
}

template <class TVectorData>
double VectorDataToRightAngleVectorDataFilter<TVectorData>::ComputeOrientation(LineType* line)
{
  VertexListType* vertexList = const_cast<VertexListType*>(line->GetVertexList());

  double Xp1 = vertexList->GetElement(0)[0];
  double Yp1 = vertexList->GetElement(0)[1];

  double Xp2 = vertexList->GetElement(1)[0];
  double Yp2 = vertexList->GetElement(1)[1];

  // Compute the orientation
  double dx          = Xp1 - Xp2;
  double dy          = Yp1 - Yp2;
  double orientation = std::atan2(dy, dx);
  if (orientation < 0)
    orientation += CONST_PI;

  return orientation;
}

template <class TVectorData>
typename VectorDataToRightAngleVectorDataFilter<TVectorData>::PointType
VectorDataToRightAngleVectorDataFilter<TVectorData>::ComputeRightAngleCoordinate(LineType* lineDst, LineType* lineSrc)
{
  PointType P;

  VertexListType* vertexListSrc = const_cast<VertexListType*>(lineSrc->GetVertexList());
  double          Xp1           = vertexListSrc->GetElement(0)[0];
  double          Yp1           = vertexListSrc->GetElement(0)[1];
  double          Xp2           = vertexListSrc->GetElement(1)[0];
  double          Yp2           = vertexListSrc->GetElement(1)[1];

  VertexListType* vertexListDst = const_cast<VertexListType*>(lineDst->GetVertexList());
  double          Xq1           = vertexListDst->GetElement(0)[0];
  double          Yq1           = vertexListDst->GetElement(0)[1];
  double          Xq2           = vertexListDst->GetElement(1)[0];
  double          Yq2           = vertexListDst->GetElement(1)[1];

  double numU   = (Xq2 - Xq1) * (Yp1 - Yq1) - (Yq2 - Yq1) * (Xp1 - Xq1);
  double denumU = (Yq2 - Yq1) * (Xp2 - Xp1) - (Xq2 - Xq1) * (Yp2 - Yp1);

  double u = numU / denumU;

  P[0] = Xp1 + u * (Xp2 - Xp1);
  P[1] = Yp1 + u * (Yp2 - Yp1);

  return P;
}

template <class TVectorData>
void VectorDataToRightAngleVectorDataFilter<TVectorData>::PrintSelf(std::ostream& os, itk::Indent indent) const
{
  Superclass::PrintSelf(os, indent);
}

} // end namespace otb

#endif
