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

  Program:   Visualization Toolkit
  Module:    UnitTestLine.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 <cmath>
#include <limits>

#include "vtkLine.h"
#include "vtkMath.h"
#include "vtkMinimalStandardRandomSequence.h"
#include "vtkSmartPointer.h"

namespace
{
const double EPSILON = 1.e-6;

void GenerateIntersectingLineSegments(vtkMinimalStandardRandomSequence* seq, double* a1, double* a2,
  double* b1, double* b2, double& u, double& v)
{
  // Generate two line segments ((a1,a2) and (b1,b2)) that intersect, and set
  // u and v as the parametric points of intersection on the two respective
  // lines. First, an intersection is generated. Two lines are then generated
  // that cross through this intersection.

  double intersection[3];
  for (unsigned i = 0; i < 3; i++)
  {
    intersection[i] = seq->GetValue();
    seq->Next();
    a1[i] = seq->GetValue();
    seq->Next();
    b1[i] = seq->GetValue();
    seq->Next();
  }

  intersection[0] = 1.;
  intersection[1] = intersection[2] = 0.;
  a1[0] = a1[1] = a1[2] = 0.;
  b1[0] = b1[1] = 1.;
  b1[2] = 0.;

  double t1 = seq->GetValue();
  seq->Next();
  double t2 = seq->GetValue();
  seq->Next();

  double lenA = 0.;
  double lenB = 0.;
  double lenA1toIntersection = 0.;
  double lenB1toIntersection = 0.;

  for (unsigned i = 0; i < 3; i++)
  {
    a2[i] = a1[i] + (intersection[i] - a1[i]) * (1. + t1);
    b2[i] = b1[i] + (intersection[i] - b1[i]) * (1. + t2);
    lenA += (a2[i] - a1[i]) * (a2[i] - a1[i]);
    lenB += (b2[i] - b1[i]) * (b2[i] - b1[i]);
    lenA1toIntersection += (a1[i] - intersection[i]) * (a1[i] - intersection[i]);
    lenB1toIntersection += (b1[i] - intersection[i]) * (b1[i] - intersection[i]);
  }

  lenA = sqrt(lenA);
  lenB = sqrt(lenB);
  lenA1toIntersection = sqrt(lenA1toIntersection);
  lenB1toIntersection = sqrt(lenB1toIntersection);

  u = lenA1toIntersection / lenA;
  v = lenB1toIntersection / lenB;
}

void RandomSphere(
  vtkMinimalStandardRandomSequence* seq, const double radius, const double* offset, double* value)
{
  // Generate a point within a sphere.

  double theta = 2. * vtkMath::Pi() * seq->GetValue();
  seq->Next();
  double phi = vtkMath::Pi() * seq->GetValue();
  seq->Next();
  value[0] = radius * cos(theta) * sin(phi) + offset[0];
  value[1] = radius * sin(theta) * sin(phi) + offset[1];
  value[2] = radius * cos(phi) + offset[2];
}

void GenerateNonintersectingLineSegments(
  vtkMinimalStandardRandomSequence* seq, double* a1, double* a2, double* b1, double* b2)
{
  // Generate two line segments ((a1,a2) and (b1,b2)) that do not intersect.
  // The endpoints of each line segment are generated from two non-overlapping
  // spheres, and the two spheres for each line segment are physically displaced
  // as well.

  static const double center[4][3] = { { 0., 0., 0. }, { 1., 0., 0. }, { 0., 1., 0. },
    { 1., 1., 0. } };

  static const double radius = 0.5 - 1.e-6;

  RandomSphere(seq, radius, center[0], a1);
  RandomSphere(seq, radius, center[1], a2);
  RandomSphere(seq, radius, center[2], b1);
  RandomSphere(seq, radius, center[3], b2);
}

void GenerateColinearLineSegments(
  vtkMinimalStandardRandomSequence* seq, double* a1, double* a2, double* b1, double* b2)
{
  // Generate two line segments ((a1,a2) and (b1,b2)) that are colinear.

  for (unsigned i = 0; i < 3; i++)
  {
    a1[i] = seq->GetValue();
    seq->Next();
    a2[i] = seq->GetValue();
    seq->Next();
    double tmp = seq->GetValue();
    seq->Next();
    b1[i] = a1[i] + tmp;
    b2[i] = a2[i] + tmp;
  }
}

void GenerateLinesAtKnownDistance(vtkMinimalStandardRandomSequence* seq, double& lineDist,
  double* a1, double* a2, double* b1, double* b2, double* a12, double* b12, double& u, double& v)
{
  // Generate two lines ((a1,a2) and (b1,b2)) set a known distance (lineDist)
  // apart. the parameter and value of the closest points for lines a and b are
  // a12, u and b12, v, respectively.

  double v1[3], v2[3], v3[3];
  for (unsigned i = 0; i < 3; i++)
  {
    v1[i] = seq->GetValue();
    seq->Next();
    v2[i] = seq->GetValue();
    seq->Next();
  }
  vtkMath::Cross(v1, v2, v3);
  vtkMath::Normalize(v1);
  vtkMath::Normalize(v2);
  vtkMath::Normalize(v3);

  double a1_to_a12 = .1 + seq->GetValue();
  seq->Next();
  double a12_to_a2 = .1 + seq->GetValue();
  seq->Next();
  double b1_to_b12 = .1 + seq->GetValue();
  seq->Next();
  double b12_to_b2 = .1 + seq->GetValue();
  seq->Next();

  lineDist = seq->GetValue();
  seq->Next();

  for (unsigned i = 0; i < 3; i++)
  {
    a12[i] = seq->GetValue();
    seq->Next();
    b12[i] = a12[i] + lineDist * v3[i];
    a1[i] = a12[i] - a1_to_a12 * v1[i];
    a2[i] = a12[i] + a12_to_a2 * v1[i];
    b1[i] = b12[i] - b1_to_b12 * v2[i];
    b2[i] = b12[i] + b12_to_b2 * v2[i];
  }
  u = a1_to_a12 / (a1_to_a12 + a12_to_a2);
  v = b1_to_b12 / (b1_to_b12 + b12_to_b2);
}

void GenerateLineAtKnownDistance(
  vtkMinimalStandardRandomSequence* seq, double* a1, double* a2, double* p, double& dist)
{
  // Generate a line (a1,a2) set a known distance (dist) from a generated point
  // p.

  double v1[3], v2[3], v3[3];
  for (unsigned i = 0; i < 3; i++)
  {
    v1[i] = seq->GetValue();
    seq->Next();
    v2[i] = seq->GetValue();
    seq->Next();
  }
  vtkMath::Cross(v1, v2, v3);
  vtkMath::Normalize(v1);
  vtkMath::Normalize(v2);
  vtkMath::Normalize(v3);

  double a1_to_a12 = .1 + seq->GetValue();
  seq->Next();
  double a12_to_a2 = .1 + seq->GetValue();
  seq->Next();

  dist = seq->GetValue();
  seq->Next();

  double nearest[3];
  for (unsigned i = 0; i < 3; i++)
  {
    nearest[i] = seq->GetValue();
    seq->Next();
    p[i] = nearest[i] + dist * v3[i];
    a1[i] = nearest[i] - a1_to_a12 * v1[i];
    a2[i] = nearest[i] + a12_to_a2 * v1[i];
  }
}

double PointToLineSegment(double* p1, double* p2, double* p, double* pn, double& u)
{
  // Helper function that computes the distance from a point to a line segment.
  // It is not used to actually test the vtkLine function for the same purpose,
  // but rather to determine correct values for these test functions.

  u = 0.;
  double dist2 = 0.;
  for (unsigned i = 0; i < 3; i++)
  {
    u += (p[i] - p1[i]) * (p2[i] - p1[i]);
    dist2 += (p2[i] - p1[i]) * (p2[i] - p1[i]);
  }
  u /= dist2;

  if (u <= 0.)
  {
    for (unsigned i = 0; i < 3; i++)
    {
      u = 0.;
      pn[i] = p1[i];
    }
  }
  else if (u >= 1.)
  {
    u = 1.;
    for (unsigned i = 0; i < 3; i++)
    {
      pn[i] = p2[i];
    }
  }
  else
  {
    for (unsigned i = 0; i < 3; i++)
    {
      pn[i] = p1[i] + u * (p2[i] - p1[i]);
    }
  }

  double dist = 0.;
  for (unsigned i = 0; i < 3; i++)
  {
    dist += (p[i] - pn[i]) * (p[i] - pn[i]);
  }
  return std::sqrt(dist);
}

void GenerateLineSegmentsAtKnownDistance(vtkMinimalStandardRandomSequence* seq, double& lineDist,
  double* a1, double* a2, double* b1, double* b2, double* a12, double* b12, double& u, double& v)
{
  // Generate two line segments ((a1,a2) and (b1,b2)) set a known distance
  // (lineDist) apart. the parameter and value of the closest points for lines
  // a and b are a12, u and b12, v, respectively.

  GenerateLinesAtKnownDistance(seq, lineDist, a1, a2, b1, b2, a12, b12, u, v);

  double modify = seq->GetValue();
  seq->Next();

  if (modify < 0.25)
  {
    double t = seq->GetValue();
    seq->Next();

    for (unsigned i = 0; i < 3; i++)
    {
      a12[i] = a2[i] = a1[i] + (a12[i] - a1[i]) * t;
    }

    u = 1.;
    lineDist = PointToLineSegment(b1, b2, a2, b12, v);
  }
  else if (modify < 0.5)
  {
    double t = seq->GetValue();
    seq->Next();

    for (unsigned i = 0; i < 3; i++)
    {
      b12[i] = b2[i] = b1[i] + (b12[i] - b1[i]) * t;
    }

    lineDist = PointToLineSegment(a1, a2, b2, a12, u);
    v = 1.;
  }
}

int TestLineIntersection_PositiveResult(vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  double a1[3], a2[3], b1[3], b2[3], u, v;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateIntersectingLineSegments(seq, a1, a2, b1, b2, u, v);

    double uv[2];
    int returnValue = vtkLine::Intersection(a1, a2, b1, b2, uv[0], uv[1]);

    if (returnValue != vtkLine::Intersect)
    {
      return EXIT_FAILURE;
    }

    if (std::fabs(u - uv[0]) > EPSILON && std::fabs(v - uv[1]) > EPSILON)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}

int TestLineIntersection_NegativeResult(vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  double a1[3], a2[3], b1[3], b2[3], u, v;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateNonintersectingLineSegments(seq, a1, a2, b1, b2);

    int returnValue = vtkLine::Intersection(a1, a2, b1, b2, u, v);

    if (returnValue != vtkLine::NoIntersect)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}

int TestLineIntersectionAbsTol_PositiveResult(
  vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  double a1[3], a2[3], b1[3], b2[3], u, v;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateIntersectingLineSegments(seq, a1, a2, b1, b2, u, v);

    double uv[2];
    int returnValue =
      vtkLine::Intersection(a1, a2, b1, b2, uv[0], uv[1], 1.0e-06, vtkLine::Absolute);

    if (returnValue != vtkLine::Intersect)
    {
      return EXIT_FAILURE;
    }

    if (std::fabs(u - uv[0]) > EPSILON && std::fabs(v - uv[1]) > EPSILON)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}

int TestLineIntersectionAbsTol_NegativeResult(
  vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  double a1[3], a2[3], b1[3], b2[3], u, v;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateNonintersectingLineSegments(seq, a1, a2, b1, b2);

    int returnValue = vtkLine::Intersection(a1, a2, b1, b2, u, v, 1.0e-06, vtkLine::Absolute);

    if (returnValue != vtkLine::NoIntersect)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}

int TestLineIntersection_ColinearResult(vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  double a1[3], a2[3], b1[3], b2[3], u, v;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateColinearLineSegments(seq, a1, a2, b1, b2);

    int returnValue = vtkLine::Intersection(a1, a2, b1, b2, u, v);

    if (returnValue != vtkLine::OnLine)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}

int TestLineIntersection(vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  if (TestLineIntersection_PositiveResult(seq, nTests) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  if (TestLineIntersection_NegativeResult(seq, nTests) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  if (TestLineIntersectionAbsTol_PositiveResult(seq, nTests) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  if (TestLineIntersectionAbsTol_NegativeResult(seq, nTests) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  if (TestLineIntersection_ColinearResult(seq, nTests) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}

int TestDistanceBetweenLines(vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  double a1[3], a2[3], b1[3], b2[3], a12[3], b12[3], u, v, dist;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateLinesAtKnownDistance(seq, dist, a1, a2, b1, b2, a12, b12, u, v);

    double p1[3], p2[3], t1, t2;
    double d = vtkLine::DistanceBetweenLines(a1, a2, b1, b2, p1, p2, t1, t2);

    if (std::fabs(dist * dist - d) > EPSILON)
    {
      return EXIT_FAILURE;
    }

    for (unsigned j = 0; j < 3; j++)
    {
      if (std::fabs(a12[j] - p1[j]) > EPSILON || std::fabs(b12[j] - p2[j]) > EPSILON)
      {
        return EXIT_FAILURE;
      }
    }

    if (std::fabs(u - t1) > EPSILON || std::fabs(v - t2) > EPSILON)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}

int TestDistanceBetweenLineSegments(vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  double a1[3], a2[3], b1[3], b2[3], a12[3], b12[3], u, v, dist;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateLineSegmentsAtKnownDistance(seq, dist, a1, a2, b1, b2, a12, b12, u, v);

    double p1[3], p2[3], t1, t2;
    double d = vtkLine::DistanceBetweenLineSegments(a1, a2, b1, b2, p1, p2, t1, t2);

    if (std::fabs(dist * dist - d) > EPSILON)
    {
      return EXIT_FAILURE;
    }

    for (unsigned j = 0; j < 3; j++)
    {
      if (std::fabs(a12[j] - p1[j]) > EPSILON || std::fabs(b12[j] - p2[j]) > EPSILON)
      {
        return EXIT_FAILURE;
      }
    }

    if (std::fabs(u - t1) > EPSILON || std::fabs(v - t2) > EPSILON)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}

int TestDistanceToLine(vtkMinimalStandardRandomSequence* seq, unsigned nTests)
{
  const double epsilon = 256 * std::numeric_limits<double>::epsilon();

  double a1[3], a2[3], p[3], dist;

  for (unsigned i = 0; i < nTests; i++)
  {
    GenerateLineAtKnownDistance(seq, a1, a2, p, dist);

    double d = vtkLine::DistanceToLine(p, a1, a2);

    if (std::fabs(dist * dist - d) > epsilon)
    {
      return EXIT_FAILURE;
    }
  }

  return EXIT_SUCCESS;
}
}

int UnitTestLine(int, char*[])
{
  vtkMinimalStandardRandomSequence* sequence = vtkMinimalStandardRandomSequence::New();

  sequence->SetSeed(1);

  const unsigned nTest = 1.e4;

  std::cout << "Testing vtkLine::vtkLine::Intersection" << std::endl;
  if (TestLineIntersection(sequence, nTest) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  std::cout << "Testing vtkLine::DistanceBetweenLines" << std::endl;
  if (TestDistanceBetweenLines(sequence, nTest) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  std::cout << "Testing vtkLine::DistanceBetweenLineSegments" << std::endl;
  if (TestDistanceBetweenLineSegments(sequence, nTest) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }
  std::cout << "Testing vtkLine::DistanceToLine" << std::endl;
  if (TestDistanceToLine(sequence, nTest) == EXIT_FAILURE)
  {
    return EXIT_FAILURE;
  }

  sequence->Delete();
  return EXIT_SUCCESS;
}