// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010-2011 Hauke Heibel <heibel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <unsupported/Eigen/Splines>

namespace Eigen {

// lets do some explicit instantiations and thus
// force the compilation of all spline functions...
template class Spline<double, 2, Dynamic>;
template class Spline<double, 3, Dynamic>;

template class Spline<double, 2, 2>;
template class Spline<double, 2, 3>;
template class Spline<double, 2, 4>;
template class Spline<double, 2, 5>;

template class Spline<float, 2, Dynamic>;
template class Spline<float, 3, Dynamic>;

template class Spline<float, 3, 2>;
template class Spline<float, 3, 3>;
template class Spline<float, 3, 4>;
template class Spline<float, 3, 5>;

}  // namespace Eigen

Spline<double, 2, Dynamic> closed_spline2d() {
  RowVectorXd knots(12);
  knots << 0, 0, 0, 0, 0.867193179093898, 1.660330955342408, 2.605084834823134, 3.484154586374428, 4.252699478956276,
      4.252699478956276, 4.252699478956276, 4.252699478956276;

  MatrixXd ctrls(8, 2);
  ctrls << -0.370967741935484, 0.236842105263158, -0.231401860693277, 0.442245185027632, 0.344361228532831,
      0.773369994120753, 0.828990216203802, 0.106550882647595, 0.407270163678382, -1.043452922172848,
      -0.488467813584053, -0.390098582530090, -0.494657189446427, 0.054804824897884, -0.370967741935484,
      0.236842105263158;
  ctrls.transposeInPlace();

  return Spline<double, 2, Dynamic>(knots, ctrls);
}

/* create a reference spline */
Spline<double, 3, Dynamic> spline3d() {
  RowVectorXd knots(11);
  knots << 0, 0, 0, 0.118997681558377, 0.162611735194631, 0.498364051982143, 0.655098003973841, 0.679702676853675,
      1.000000000000000, 1.000000000000000, 1.000000000000000;

  MatrixXd ctrls(8, 3);
  ctrls << 0.959743958516081, 0.340385726666133, 0.585267750979777, 0.223811939491137, 0.751267059305653,
      0.255095115459269, 0.505957051665142, 0.699076722656686, 0.890903252535799, 0.959291425205444, 0.547215529963803,
      0.138624442828679, 0.149294005559057, 0.257508254123736, 0.840717255983663, 0.254282178971531, 0.814284826068816,
      0.243524968724989, 0.929263623187228, 0.349983765984809, 0.196595250431208, 0.251083857976031, 0.616044676146639,
      0.473288848902729;
  ctrls.transposeInPlace();

  return Spline<double, 3, Dynamic>(knots, ctrls);
}

/* compares evaluations against known results */
void eval_spline3d() {
  Spline3d spline = spline3d();

  RowVectorXd u(10);
  u << 0.351659507062997, 0.830828627896291, 0.585264091152724, 0.549723608291140, 0.917193663829810, 0.285839018820374,
      0.757200229110721, 0.753729094278495, 0.380445846975357, 0.567821640725221;

  MatrixXd pts(10, 3);
  pts << 0.707620811535916, 0.510258911240815, 0.417485437023409, 0.603422256426978, 0.529498282727551,
      0.270351549348981, 0.228364197569334, 0.423745615677815, 0.637687289287490, 0.275556796335168, 0.350856706427970,
      0.684295784598905, 0.514519311047655, 0.525077224890754, 0.351628308305896, 0.724152914315666, 0.574461155457304,
      0.469860285484058, 0.529365063753288, 0.613328702656816, 0.237837040141739, 0.522469395136878, 0.619099658652895,
      0.237139665242069, 0.677357023849552, 0.480655768435853, 0.422227610314397, 0.247046593173758, 0.380604672404750,
      0.670065791405019;
  pts.transposeInPlace();

  for (int i = 0; i < u.size(); ++i) {
    Vector3d pt = spline(u(i));
    VERIFY((pt - pts.col(i)).norm() < 1e-14);
  }
}

/* compares evaluations on corner cases */
void eval_spline3d_onbrks() {
  Spline3d spline = spline3d();

  RowVectorXd u = spline.knots();

  MatrixXd pts(11, 3);
  pts << 0.959743958516081, 0.340385726666133, 0.585267750979777, 0.959743958516081, 0.340385726666133,
      0.585267750979777, 0.959743958516081, 0.340385726666133, 0.585267750979777, 0.430282980289940, 0.713074680056118,
      0.720373307943349, 0.558074875553060, 0.681617921034459, 0.804417124839942, 0.407076008291750, 0.349707710518163,
      0.617275937419545, 0.240037008286602, 0.738739390398014, 0.324554153129411, 0.302434111480572, 0.781162443963899,
      0.240177089094644, 0.251083857976031, 0.616044676146639, 0.473288848902729, 0.251083857976031, 0.616044676146639,
      0.473288848902729, 0.251083857976031, 0.616044676146639, 0.473288848902729;
  pts.transposeInPlace();

  for (int i = 0; i < u.size(); ++i) {
    Vector3d pt = spline(u(i));
    VERIFY((pt - pts.col(i)).norm() < 1e-14);
  }
}

void eval_closed_spline2d() {
  Spline2d spline = closed_spline2d();

  RowVectorXd u(12);
  u << 0, 0.332457030395796, 0.356467130532952, 0.453562180176215, 0.648017921874804, 0.973770235555003,
      1.882577647219307, 2.289408593930498, 3.511951429883045, 3.884149321369450, 4.236261590369414, 4.252699478956276;

  MatrixXd pts(12, 2);
  pts << -0.370967741935484, 0.236842105263158, -0.152576775123250, 0.448975001279334, -0.133417538277668,
      0.461615613865667, -0.053199060826740, 0.507630360006299, 0.114249591147281, 0.570414135097409, 0.377810316891987,
      0.560497102875315, 0.665052120135908, -0.157557441109611, 0.516006487053228, -0.559763292174825,
      -0.379486035348887, -0.331959640488223, -0.462034726249078, -0.039105670080824, -0.378730600917982,
      0.225127015099919, -0.370967741935484, 0.236842105263158;
  pts.transposeInPlace();

  for (int i = 0; i < u.size(); ++i) {
    Vector2d pt = spline(u(i));
    VERIFY((pt - pts.col(i)).norm() < 1e-14);
  }
}

void check_global_interpolation2d() {
  typedef Spline2d::PointType PointType;
  typedef Spline2d::KnotVectorType KnotVectorType;
  typedef Spline2d::ControlPointVectorType ControlPointVectorType;

  ControlPointVectorType points = ControlPointVectorType::Random(2, 100);

  KnotVectorType chord_lengths;  // knot parameters
  Eigen::ChordLengths(points, chord_lengths);

  // interpolation without knot parameters
  {
    const Spline2d spline = SplineFitting<Spline2d>::Interpolate(points, 3);

    for (Eigen::DenseIndex i = 0; i < points.cols(); ++i) {
      PointType pt = spline(chord_lengths(i));
      PointType ref = points.col(i);
      VERIFY((pt - ref).matrix().norm() < 1e-14);
    }
  }

  // interpolation with given knot parameters
  {
    const Spline2d spline = SplineFitting<Spline2d>::Interpolate(points, 3, chord_lengths);

    for (Eigen::DenseIndex i = 0; i < points.cols(); ++i) {
      PointType pt = spline(chord_lengths(i));
      PointType ref = points.col(i);
      VERIFY((pt - ref).matrix().norm() < 1e-14);
    }
  }
}

void check_global_interpolation_with_derivatives2d() {
  typedef Spline2d::PointType PointType;
  typedef Spline2d::KnotVectorType KnotVectorType;

  const Eigen::DenseIndex numPoints = 100;
  const unsigned int dimension = 2;
  const unsigned int degree = 3;

  ArrayXXd points = ArrayXXd::Random(dimension, numPoints);

  KnotVectorType knots;
  Eigen::ChordLengths(points, knots);

  ArrayXXd derivatives = ArrayXXd::Random(dimension, numPoints);
  VectorXd derivativeIndices(numPoints);

  for (Eigen::DenseIndex i = 0; i < numPoints; ++i) derivativeIndices(i) = static_cast<double>(i);

  const Spline2d spline =
      SplineFitting<Spline2d>::InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree);

  for (Eigen::DenseIndex i = 0; i < points.cols(); ++i) {
    PointType point = spline(knots(i));
    PointType referencePoint = points.col(i);
    VERIFY_IS_APPROX(point, referencePoint);
    PointType derivative = spline.derivatives(knots(i), 1).col(1);
    PointType referenceDerivative = derivatives.col(i);
    VERIFY_IS_APPROX(derivative, referenceDerivative);
  }
}

EIGEN_DECLARE_TEST(splines) {
  for (int i = 0; i < g_repeat; ++i) {
    CALL_SUBTEST(eval_spline3d());
    CALL_SUBTEST(eval_spline3d_onbrks());
    CALL_SUBTEST(eval_closed_spline2d());
    CALL_SUBTEST(check_global_interpolation2d());
    CALL_SUBTEST(check_global_interpolation_with_derivatives2d());
  }
}