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

  Program:   Visualization Toolkit
  Module:    vtkResliceCursorPicker.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 "vtkResliceCursorPicker.h"
#include "vtkObjectFactory.h"

#include "vtkCamera.h"
#include "vtkCommand.h"
#include "vtkGenericCell.h"
#include "vtkLine.h"
#include "vtkMath.h"
#include "vtkMatrix4x4.h"
#include "vtkPlane.h"
#include "vtkPoints.h"
#include "vtkPolyData.h"
#include "vtkRenderWindow.h"
#include "vtkRenderer.h"
#include "vtkResliceCursor.h"
#include "vtkResliceCursorPolyDataAlgorithm.h"
#include "vtkSmartPointer.h"

vtkStandardNewMacro(vtkResliceCursorPicker);
vtkCxxSetObjectMacro(
  vtkResliceCursorPicker, ResliceCursorAlgorithm, vtkResliceCursorPolyDataAlgorithm);
vtkCxxSetObjectMacro(vtkResliceCursorPicker, TransformMatrix, vtkMatrix4x4);

//------------------------------------------------------------------------------
vtkResliceCursorPicker::vtkResliceCursorPicker()
{
  // For polydata picking
  this->Cell = vtkGenericCell::New();

  // Miscellaneous ivars
  this->Tolerance = 1e-6;

  this->PickedAxis1 = this->PickedAxis2 = 0;

  this->ResliceCursorAlgorithm = nullptr;
  this->TransformMatrix = nullptr;

  // Plane on which the cursor lies. This is the picked plane.
  this->Plane = vtkPlane::New();
}

//------------------------------------------------------------------------------
vtkResliceCursorPicker::~vtkResliceCursorPicker()
{
  this->Cell->Delete();
  this->SetResliceCursorAlgorithm(nullptr);
  this->SetTransformMatrix(nullptr);
  this->Plane->Delete();
}

//------------------------------------------------------------------------------
int vtkResliceCursorPicker::Pick(
  double selectionX, double selectionY, double selectionZ, vtkRenderer* renderer)
{
  int i;
  vtkCamera* camera;
  double p1World[4], p2World[4];
  int winSize[2] = { 1, 1 };
  double x, y;
  double* viewport;
  double cameraPos[4], cameraFP[4];
  double *displayCoords, *worldCoords;
  double* clipRange;
  double ray[3], rayLength;
  double windowLowerLeft[4], windowUpperRight[4];
  double tF, tB;
  double cameraDOP[3];

  //  Initialize picking process
  this->Initialize();
  this->Renderer = renderer;
  this->SelectionPoint[0] = selectionX;
  this->SelectionPoint[1] = selectionY;
  this->SelectionPoint[2] = selectionZ;

  // Invoke start pick method if defined
  this->InvokeEvent(vtkCommand::StartPickEvent, nullptr);

  if (renderer == nullptr)
  {
    vtkErrorMacro(<< "Must specify renderer!");
    return 0;
  }

  // Get camera focal point and position. Convert to display (screen)
  // coordinates. We need a depth value for z-buffer.
  //
  camera = renderer->GetActiveCamera();
  camera->GetPosition(cameraPos);
  cameraPos[3] = 1.0;
  camera->GetFocalPoint(cameraFP);
  cameraFP[3] = 1.0;

  renderer->SetWorldPoint(cameraFP[0], cameraFP[1], cameraFP[2], cameraFP[3]);
  renderer->WorldToDisplay();
  displayCoords = renderer->GetDisplayPoint();
  selectionZ = displayCoords[2];

  // Convert the selection point into world coordinates.
  //
  renderer->SetDisplayPoint(selectionX, selectionY, selectionZ);
  renderer->DisplayToWorld();
  worldCoords = renderer->GetWorldPoint();
  if (worldCoords[3] == 0.0)
  {
    vtkErrorMacro(<< "Bad homogeneous coordinates");
    return 0;
  }
  for (i = 0; i < 3; i++)
  {
    this->PickPosition[i] = worldCoords[i] / worldCoords[3];
  }

  //  Compute the ray endpoints.  The ray is along the line running from
  //  the camera position to the selection point, starting where this line
  //  intersects the front clipping plane, and terminating where this
  //  line intersects the back clipping plane.
  for (i = 0; i < 3; i++)
  {
    ray[i] = this->PickPosition[i] - cameraPos[i];
  }
  for (i = 0; i < 3; i++)
  {
    cameraDOP[i] = cameraFP[i] - cameraPos[i];
  }

  vtkMath::Normalize(cameraDOP);

  if ((rayLength = vtkMath::Dot(cameraDOP, ray)) == 0.0)
  {
    vtkWarningMacro("Cannot process points");
    return 0;
  }

  clipRange = camera->GetClippingRange();

  if (camera->GetParallelProjection())
  {
    tF = clipRange[0] - rayLength;
    tB = clipRange[1] - rayLength;
    for (i = 0; i < 3; i++)
    {
      p1World[i] = this->PickPosition[i] + tF * cameraDOP[i];
      p2World[i] = this->PickPosition[i] + tB * cameraDOP[i];
    }
  }
  else
  {
    tF = clipRange[0] / rayLength;
    tB = clipRange[1] / rayLength;
    for (i = 0; i < 3; i++)
    {
      p1World[i] = cameraPos[i] + tF * ray[i];
      p2World[i] = cameraPos[i] + tB * ray[i];
    }
  }
  p1World[3] = p2World[3] = 1.0;

  // Compute the tolerance in world coordinates.  Do this by
  // determining the world coordinates of the diagonal points of the
  // window, computing the width of the window in world coordinates, and
  // multiplying by the tolerance.
  //
  viewport = renderer->GetViewport();
  if (renderer->GetRenderWindow())
  {
    const int* winSizePtr = renderer->GetRenderWindow()->GetSize();
    if (winSizePtr)
    {
      winSize[0] = winSizePtr[0];
      winSize[1] = winSizePtr[1];
    }
  }
  x = winSize[0] * viewport[0];
  y = winSize[1] * viewport[1];
  renderer->SetDisplayPoint(x, y, selectionZ);
  renderer->DisplayToWorld();
  renderer->GetWorldPoint(windowLowerLeft);

  x = winSize[0] * viewport[2];
  y = winSize[1] * viewport[3];
  renderer->SetDisplayPoint(x, y, selectionZ);
  renderer->DisplayToWorld();
  renderer->GetWorldPoint(windowUpperRight);

  double tol;
  for (tol = 0.0, i = 0; i < 3; i++)
  {
    tol += (windowUpperRight[i] - windowLowerLeft[i]) * (windowUpperRight[i] - windowLowerLeft[i]);
  }

  tol = sqrt(tol) * this->Tolerance;

  vtkResliceCursor* rc = this->ResliceCursorAlgorithm->GetResliceCursor();
  const int axis1 = this->ResliceCursorAlgorithm->GetAxis1();
  const int axis2 = this->ResliceCursorAlgorithm->GetAxis2();
  // const int axis3 = this->ResliceCursorAlgorithm->GetReslicePlaneNormal();

  double center[3];
  rc->GetCenter(center);
  this->PickedCenter = this->IntersectPointWithLine(p1World, p2World, center, tol);
  this->PickedAxis1 =
    this->IntersectPolyDataWithLine(p1World, p2World, rc->GetCenterlineAxisPolyData(axis1), tol);
  this->PickedAxis2 =
    this->IntersectPolyDataWithLine(p1World, p2World, rc->GetCenterlineAxisPolyData(axis2), tol);

  if (this->PickedAxis1 || this->PickedAxis2 || this->PickedCenter)
  {

    // Pick on transformed plane coords and get the real coords back after
    // inverse transformation.

    double t, pickPosT[4], pickPos[4];
    this->TransformPlane();
    this->Plane->IntersectWithLine(p1World, p2World, t, pickPosT);
    pickPosT[3] = 1.0;
    this->InverseTransformPoint(pickPosT, pickPos);

    this->PickPosition[0] = pickPos[0];
    this->PickPosition[1] = pickPos[1];
    this->PickPosition[2] = pickPos[2];
  }

  return this->PickedAxis1 + this->PickedAxis2 + this->PickedCenter;
}

//------------------------------------------------------------------------------
// Pick a display coordinate and return the picked world coordinates
//
void vtkResliceCursorPicker::Pick(double displayPos[2], double world[3], vtkRenderer* ren)
{
  // First compute the equivalent of this display point on the focal plane

  double fp[4], tmp1[4], camPos[4], eventFPpos[4];
  ren->GetActiveCamera()->GetFocalPoint(fp);
  ren->GetActiveCamera()->GetPosition(camPos);
  fp[3] = 1.0;
  ren->SetWorldPoint(fp);
  ren->WorldToDisplay();
  ren->GetDisplayPoint(tmp1);

  tmp1[0] = displayPos[0];
  tmp1[1] = displayPos[1];

  this->Renderer->SetDisplayPoint(tmp1);
  this->Renderer->DisplayToWorld();

  // This is the world coordinates of the point on the focal plane.

  this->Renderer->GetWorldPoint(eventFPpos);

  // Now construct the pick ray

  double cameraDOP[3];
  for (int i = 0; i < 3; i++)
  {
    cameraDOP[i] = fp[i] - camPos[i];
  }

  double otherPoint[3] = { eventFPpos[0] + cameraDOP[0], eventFPpos[1] + cameraDOP[1],
    eventFPpos[2] + cameraDOP[2] };

  double t, pickPosT[4], pickPos[4];

  // Transform the plane into one that lies on the resliced plane
  this->TransformPlane();

  // Pick it
  this->Plane->IntersectWithLine(eventFPpos, otherPoint, t, pickPosT);

  // Transform it back from the resliced plane coords to actual world coords
  pickPosT[3] = 1.0;
  this->InverseTransformPoint(pickPosT, pickPos);

  // Copy the result
  for (int i = 0; i < 3; i++)
  {
    world[i] = pickPos[i];
  }
}

//------------------------------------------------------------------------------
// Helper function for sanity check - to see if one point is different from
// another.
//
static bool vtkResliceCursorPickerIsDifferentSanityCheck(const double a[3], const double b[3])
{
  // Tolerance of 0.0001 needs some fluff..

  return (fabs(a[0] - b[0]) > 0.0001 || fabs(a[1] - b[1]) > 0.0001 || fabs(a[2] - b[2]) > 0.0001);
}

//------------------------------------------------------------------------------
// Transform the reslice plane onto the co-ordinate system its displayed in.
//
void vtkResliceCursorPicker::TransformPlane()
{
  vtkResliceCursor* rc = this->ResliceCursorAlgorithm->GetResliceCursor();
  const int axis3 = this->ResliceCursorAlgorithm->GetReslicePlaneNormal();

  double origin[4] = { 0, 0, 0, 1 }, originT[4];
  double normal[3];
  rc->GetPlane(axis3)->GetOrigin(origin);
  rc->GetPlane(axis3)->GetNormal(normal);

  if (!this->TransformMatrix)
  {
    this->Plane->SetOrigin(origin);
    this->Plane->SetNormal(normal);

    // The origin of the reslice cursor will remain untransformed.
    double center[3];
    rc->GetCenter(center);

    // Sanity check
    if (vtkResliceCursorPickerIsDifferentSanityCheck(origin, center))
    {
      vtkErrorMacro("Reslice cursor center of ("
        << center[0] << "," << center[1] << "," << center[2] << ") is not equal to plane "
        << "origin along axis " << axis3 << " of (" << origin[0] << "," << origin[1] << ","
        << origin[2] << ").");
    }

    return;
  }

  double normalPoint[4] = { origin[0] + normal[0], origin[1] + normal[1], origin[2] + normal[2],
    1.0 };
  double normalPointT[4];

  this->TransformPoint(origin, originT);

  // Sanity check
  if (vtkResliceCursorPickerIsDifferentSanityCheck(origin, originT))
  {
    vtkErrorMacro("Reslice cursor after transformation ("
      << originT[0] << "," << originT[1] << "," << originT[2] << ") is not equal to before "
      << "transformation along axis " << axis3 << " of (" << origin[0] << "," << origin[1] << ","
      << origin[2] << ").");
  }

  this->TransformPoint(normalPoint, normalPointT);

  double normalT[3];
  vtkMath::Subtract(normalPointT, originT, normalT);
  vtkMath::Normalize(normalT);

  // The origin of the reslice cursor will remain untransformed.
  double center[3];
  rc->GetCenter(center);

  // Sanity check
  if (vtkResliceCursorPickerIsDifferentSanityCheck(origin, center))
  {
    vtkErrorMacro("Reslice cursor center of ("
      << center[0] << "," << center[1] << "," << center[2] << ") is not equal to plane "
      << "origin along axis " << axis3 << " of (" << origin[0] << "," << origin[1] << ","
      << origin[2] << ").");
  }

  this->Plane->SetOrigin(originT);
  this->Plane->SetNormal(normalT);
}

//------------------------------------------------------------------------------
void vtkResliceCursorPicker::TransformPoint(double pIn[4], double pOut[4])
{
  this->TransformMatrix->MultiplyPoint(pIn, pOut);
}

//------------------------------------------------------------------------------
void vtkResliceCursorPicker::InverseTransformPoint(double pIn[4], double pOut[4])
{
  if (!this->TransformMatrix)
  {
    for (int i = 0; i < 3; i++)
    {
      pOut[i] = pIn[i];
    }
    return;
  }

  // Maintain a copy of the existing elements.
  double elements[4][4];
  for (int i = 0; i < 4; i++)
  {
    for (int j = 0; j < 4; j++)
    {
      elements[i][j] = this->TransformMatrix->Element[i][j];
    }
  }

  // Invert matrix
  this->TransformMatrix->Invert();

  // Transform point
  this->TransformPoint(pIn, pOut);

  // Copy back the elements.
  for (int i = 0; i < 4; i++)
  {
    for (int j = 0; j < 4; j++)
    {
      this->TransformMatrix->Element[i][j] = elements[i][j];
    }
  }
}

//------------------------------------------------------------------------------
int vtkResliceCursorPicker::IntersectPolyDataWithLine(
  double p1[3], double p2[3], vtkPolyData* data, double tol)
{
  vtkIdType numCells = data->GetNumberOfCells();

  for (vtkIdType cellId = 0; cellId < numCells; cellId++)
  {
    double t;
    double x[3];
    double pcoords[3];
    pcoords[0] = pcoords[1] = pcoords[2] = 0;
    int newSubId = -1;
    int numSubIds = 1;

    // This will only loop once unless we need to deal with a strip
    for (int subId = 0; subId < numSubIds; subId++)
    {
      data->GetCell(cellId, this->Cell);

      // Transform the points using any transform matrix that may be set.

      for (int i = 0; i < this->Cell->GetPoints()->GetNumberOfPoints(); i++)
      {
        if (this->TransformMatrix)
        {
          double pIn[4] = { 0, 0, 0, 1 }, pOut[4];
          this->Cell->GetPoints()->GetPoint(i, pIn);
          this->TransformMatrix->MultiplyPoint(pIn, pOut);
          this->Cell->GetPoints()->SetPoint(i, pOut);
        }
      }

      int cellPicked = 0;
      cellPicked = this->Cell->IntersectWithLine(
        const_cast<double*>(p1), const_cast<double*>(p2), tol, t, x, pcoords, newSubId);

      if (cellPicked)
      {
        return cellPicked;
      }

    } // if a close cell
  }   // for all cells

  return 0;
}

//------------------------------------------------------------------------------
int vtkResliceCursorPicker::IntersectPointWithLine(
  double p1[3], double p2[3], double x[3], double tol)
{

  double X[4] = { x[0], x[1], x[2], 1 };
  if (this->TransformMatrix)
  {
    double pIn[4] = { x[0], x[1], x[2], 1 };
    this->TransformMatrix->MultiplyPoint(pIn, X);
  }

  int i;
  double ray[3], rayFactor, projXYZ[3];

  for (i = 0; i < 3; i++)
  {
    ray[i] = p2[i] - p1[i];
  }
  if ((rayFactor = vtkMath::Dot(ray, ray)) == 0.0)
  {
    return 0;
  }
  //
  //  Project each point onto ray. Determine if point is within tolerance.
  //
  const double t =
    (ray[0] * (X[0] - p1[0]) + ray[1] * (X[1] - p1[1]) + ray[2] * (X[2] - p1[2])) / rayFactor;

  if (t >= 0.0 && t <= 1.0)
  {
    for (i = 0; i < 3; i++)
    {
      projXYZ[i] = p1[i] + t * ray[i];
      if (fabs(X[i] - projXYZ[i]) > tol)
      {
        break;
      }
    }

    if (i > 2) // within tolerance
    {
      return 1;
    }
  }

  return 0;
}

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

  os << indent << "PickedAxis1: " << this->PickedAxis1 << endl;
  os << indent << "PickedAxis2: " << this->PickedAxis2 << endl;
  os << indent << "PickedCenter: " << this->PickedCenter << endl;
  os << indent << "ResliceCursorAlgorithm: " << this->ResliceCursorAlgorithm << "\n";
  if (this->ResliceCursorAlgorithm)
  {
    this->ResliceCursorAlgorithm->PrintSelf(os, indent);
  }
  os << indent << "TransformMatrix: " << this->TransformMatrix << "\n";
  if (this->TransformMatrix)
  {
    this->TransformMatrix->PrintSelf(os, indent);
  }
  // this->PointIds;
  // this->Plane;
  // this->Cell;
}