//=========================================================================
//  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 "vtkClosestPointFilter.h"

#include "vtkCellLocator.h"
#include "vtkErrorCode.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkMultiBlockDataSet.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkPolyData.h"
#include "vtkSmartPointer.h"
#include "vtkTransform.h"
#
#include <sys/stat.h>
#include <sys/types.h>
#include <vtksys/SystemTools.hxx>

vtkStandardNewMacro(vtkClosestPointFilter);

vtkClosestPointFilter::vtkClosestPointFilter()
{
  this->Translation[0] = this->Translation[1] = this->Translation[2] = 0.0;
  this->Orientation[0] = this->Orientation[1] = this->Orientation[2] = 0.0;
  this->Scale[0] = this->Scale[1] = this->Scale[2] = 1.0;
  this->Transform = vtkTransform::New();
  this->Locator = vtkCellLocator::New();
  this->SetNumberOfInputPorts(1);
  this->PointMode = true;
  this->TestPoint[0] = this->TestPoint[1] = this->TestPoint[2] = 0.0;
  this->ClosestPoint[0] = this->ClosestPoint[1] = this->ClosestPoint[2] = 0.0;
  this->TransformInverse = NULL;
}

vtkClosestPointFilter::~vtkClosestPointFilter()
{
  this->Locator->Delete();
  this->Transform->Delete();
}

int vtkClosestPointFilter::RequestData(vtkInformation* vtkNotUsed(request),
  vtkInformationVector** inputVector, vtkInformationVector* vtkNotUsed(outputVector))
{
  // get the info and input data
  vtkInformation* inInfo = inputVector[0]->GetInformationObject(0);
  vtkPolyData* input = vtkPolyData::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));

  this->Locator->SetDataSet(input);
  this->Locator->BuildLocator();

  //   // See if we need to update the transform
  //   if (this->BuildTime < this->MTime)
  //     {
  this->Transform->Identity();
  this->Transform->PreMultiply();
  this->Transform->Translate(this->Translation);
  this->Transform->RotateZ(this->Orientation[2]);
  this->Transform->RotateX(this->Orientation[0]);
  this->Transform->RotateY(this->Orientation[1]);
  this->Transform->Scale(this->Scale[0], this->Scale[1], this->Scale[2]);

  this->TransformInverse = this->Transform->GetInverse();
  this->BuildTime.Modified();
  //    }

  double tp[3], ctp[3];
  int subId;
  double d2;
  vtkIdType cellId;

  if (this->PointMode)
  {
    // Transform the test point to the data's coordinate system
    this->TransformInverse->TransformPoint(this->TestPoint, tp);

    // Now calculate the closest point
    this->Locator->FindClosestPoint(tp, ctp, cellId, subId, d2);
  }
  else
  {
    double tp2[3], t, pcoords[3];
    // Transform the test line to the data's coordinate system
    this->TransformInverse->TransformPoint(this->TestLine, tp);
    this->TransformInverse->TransformPoint(&(this->TestLine[3]), tp2);

    // Now calculate the closest point
    if (!this->Locator->IntersectWithLine(tp, tp2, 0.0, t, ctp, pcoords, subId))
    {
      // Well the intersection failed try closestpoint
      this->Locator->FindClosestPoint(tp, ctp, cellId, subId, d2);
    }
  }

  // Transform the point back to world space
  this->Transform->TransformPoint(ctp, this->ClosestPoint);

  return VTK_OK;
}

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

  if (this->PointMode)
  {
    os << indent << "Test Point: (" << this->TestPoint[0] << ", " << this->TestPoint[1] << ", "
       << this->TestPoint[2] << ")\n";
    os << indent << "Test Line: (Not Set)\n";
  }
  else
  {
    os << indent << "Test Line: ((" << this->TestLine[0] << ", " << this->TestLine[1] << ", "
       << this->TestLine[2] << ") (" << this->TestLine[3] << ", " << this->TestLine[4] << ", "
       << this->TestLine[5] << "))\n";
    os << indent << "Test Point: (Not Set)\n";
  }

  os << indent << "Closest Point: (" << this->ClosestPoint[0] << ", " << this->ClosestPoint[1]
     << ", " << this->ClosestPoint[2] << ")\n";
  os << indent << "Translation: (" << this->Translation[0] << ", " << this->Translation[1] << ", "
     << this->Translation[2] << ")\n";
  os << indent << "Orientation: (" << this->Orientation[0] << ", " << this->Orientation[1] << ", "
     << this->Orientation[2] << ")\n";
  os << indent << "Scale: (" << this->Scale[0] << ", " << this->Scale[1] << ", " << this->Scale[2]
     << ")\n";
}
