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

  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 "vtkCommand.h"
#include "vtkMath.h"
#include "vtkPoints.h"
#include "vtkGenericCell.h"
#include "vtkRenderer.h"
#include "vtkCamera.h"
#include "vtkLine.h"
#include "vtkPolyData.h"
#include "vtkRenderWindow.h"
#include "vtkResliceCursor.h"
#include "vtkResliceCursorPolyDataAlgorithm.h"
#include "vtkPlane.h"
#include "vtkMatrix4x4.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())
  {
    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;
}