// Copyright (c) 2007, 2008_WIN32 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
//
// Matrix and vector classes, based on Eigen2.
//
// Avoid using Eigen2 classes directly; instead typedef them here.

#ifndef LIBMV_NUMERIC_NUMERIC_H
#define LIBMV_NUMERIC_NUMERIC_H

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>

#if !defined(__MINGW64__)
#  if defined(_WIN32) || defined(__APPLE__) || defined(__FreeBSD__) ||         \
      defined(__NetBSD__) || defined(__HAIKU__)
inline void sincos(double x, double* sinx, double* cosx) {
  *sinx = sin(x);
  *cosx = cos(x);
}
#  endif
#endif  // !__MINGW64__

#if (defined(WIN32) || defined(WIN64)) && !defined(__MINGW32__)
inline long lround(double d) {
  return (long)(d > 0 ? d + 0.5 : ceil(d - 0.5));
}
#  if _MSC_VER < 1800
inline int round(double d) {
  return (d > 0) ? int(d + 0.5) : int(d - 0.5);
}
#  endif  // _MSC_VER < 1800
typedef unsigned int uint;
#endif  // _WIN32

namespace libmv {

typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd Vec;

typedef Eigen::MatrixXf Matf;
typedef Eigen::VectorXf Vecf;

typedef Eigen::Matrix<unsigned int, Eigen::Dynamic, Eigen::Dynamic> Matu;
typedef Eigen::Matrix<unsigned int, Eigen::Dynamic, 1> Vecu;
typedef Eigen::Matrix<unsigned int, 2, 1> Vec2u;

typedef Eigen::Matrix<double, 2, 2> Mat2;
typedef Eigen::Matrix<double, 2, 3> Mat23;
typedef Eigen::Matrix<double, 3, 3> Mat3;
typedef Eigen::Matrix<double, 3, 4> Mat34;
typedef Eigen::Matrix<double, 3, 5> Mat35;
typedef Eigen::Matrix<double, 4, 1> Mat41;
typedef Eigen::Matrix<double, 4, 3> Mat43;
typedef Eigen::Matrix<double, 4, 4> Mat4;
typedef Eigen::Matrix<double, 4, 6> Mat46;
typedef Eigen::Matrix<float, 2, 2> Mat2f;
typedef Eigen::Matrix<float, 2, 3> Mat23f;
typedef Eigen::Matrix<float, 3, 3> Mat3f;
typedef Eigen::Matrix<float, 3, 4> Mat34f;
typedef Eigen::Matrix<float, 3, 5> Mat35f;
typedef Eigen::Matrix<float, 4, 3> Mat43f;
typedef Eigen::Matrix<float, 4, 4> Mat4f;
typedef Eigen::Matrix<float, 4, 6> Mat46f;

typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> RMat3;
typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor> RMat4;

typedef Eigen::Matrix<double, 2, Eigen::Dynamic> Mat2X;
typedef Eigen::Matrix<double, 3, Eigen::Dynamic> Mat3X;
typedef Eigen::Matrix<double, 4, Eigen::Dynamic> Mat4X;
typedef Eigen::Matrix<double, Eigen::Dynamic, 2> MatX2;
typedef Eigen::Matrix<double, Eigen::Dynamic, 3> MatX3;
typedef Eigen::Matrix<double, Eigen::Dynamic, 4> MatX4;
typedef Eigen::Matrix<double, Eigen::Dynamic, 5> MatX5;
typedef Eigen::Matrix<double, Eigen::Dynamic, 6> MatX6;
typedef Eigen::Matrix<double, Eigen::Dynamic, 7> MatX7;
typedef Eigen::Matrix<double, Eigen::Dynamic, 8> MatX8;
typedef Eigen::Matrix<double, Eigen::Dynamic, 9> MatX9;
typedef Eigen::Matrix<double, Eigen::Dynamic, 15> MatX15;
typedef Eigen::Matrix<double, Eigen::Dynamic, 16> MatX16;

typedef Eigen::Vector2d Vec2;
typedef Eigen::Vector3d Vec3;
typedef Eigen::Vector4d Vec4;
typedef Eigen::Matrix<double, 5, 1> Vec5;
typedef Eigen::Matrix<double, 6, 1> Vec6;
typedef Eigen::Matrix<double, 7, 1> Vec7;
typedef Eigen::Matrix<double, 8, 1> Vec8;
typedef Eigen::Matrix<double, 9, 1> Vec9;
typedef Eigen::Matrix<double, 10, 1> Vec10;
typedef Eigen::Matrix<double, 11, 1> Vec11;
typedef Eigen::Matrix<double, 12, 1> Vec12;
typedef Eigen::Matrix<double, 13, 1> Vec13;
typedef Eigen::Matrix<double, 14, 1> Vec14;
typedef Eigen::Matrix<double, 15, 1> Vec15;
typedef Eigen::Matrix<double, 16, 1> Vec16;
typedef Eigen::Matrix<double, 17, 1> Vec17;
typedef Eigen::Matrix<double, 18, 1> Vec18;
typedef Eigen::Matrix<double, 19, 1> Vec19;
typedef Eigen::Matrix<double, 20, 1> Vec20;

typedef Eigen::Vector2f Vec2f;
typedef Eigen::Vector3f Vec3f;
typedef Eigen::Vector4f Vec4f;

typedef Eigen::VectorXi VecXi;

typedef Eigen::Vector2i Vec2i;
typedef Eigen::Vector3i Vec3i;
typedef Eigen::Vector4i Vec4i;

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    RMatf;

typedef Eigen::NumTraits<double> EigenDouble;

using Eigen::Dynamic;
using Eigen::Map;
using Eigen::Matrix;

// Find U, s, and VT such that
//
//   A = U * diag(s) * VT
//
template <typename TMat, typename TVec>
inline void SVD(TMat* /*A*/, Vec* /*s*/, Mat* /*U*/, Mat* /*VT*/) {
  assert(0);
}

// Solve the linear system Ax = 0 via SVD. Store the solution in x, such that
// ||x|| = 1.0. Return the singluar value corresponding to the solution.
// Destroys A and resizes x if necessary.
// TODO(maclean): Take the SVD of the transpose instead of this zero padding.
template <typename TMat, typename TVec>
double Nullspace(TMat* A, TVec* nullspace) {
  Eigen::JacobiSVD<TMat> svd(*A, Eigen::ComputeFullV);
  (*nullspace) = svd.matrixV().col(A->cols() - 1);
  if (A->rows() >= A->cols())
    return svd.singularValues()(A->cols() - 1);
  else
    return 0.0;
}

// Solve the linear system Ax = 0 via SVD. Finds two solutions, x1 and x2, such
// that x1 is the best solution and x2 is the next best solution (in the L2
// norm sense). Store the solution in x1 and x2, such that ||x|| = 1.0. Return
// the singluar value corresponding to the solution x1.  Destroys A and resizes
// x if necessary.
template <typename TMat, typename TVec1, typename TVec2>
double Nullspace2(TMat* A, TVec1* x1, TVec2* x2) {
  Eigen::JacobiSVD<TMat> svd(*A, Eigen::ComputeFullV);
  *x1 = svd.matrixV().col(A->cols() - 1);
  *x2 = svd.matrixV().col(A->cols() - 2);
  if (A->rows() >= A->cols())
    return svd.singularValues()(A->cols() - 1);
  else
    return 0.0;
}

// In place transpose for square matrices.
template <class TA>
inline void TransposeInPlace(TA* A) {
  *A = A->transpose().eval();
}

template <typename TVec>
inline double NormL1(const TVec& x) {
  return x.array().abs().sum();
}

template <typename TVec>
inline double NormL2(const TVec& x) {
  return x.norm();
}

template <typename TVec>
inline double NormLInfinity(const TVec& x) {
  return x.array().abs().maxCoeff();
}

template <typename TVec>
inline double DistanceL1(const TVec& x, const TVec& y) {
  return (x - y).array().abs().sum();
}

template <typename TVec>
inline double DistanceL2(const TVec& x, const TVec& y) {
  return (x - y).norm();
}
template <typename TVec>
inline double DistanceLInfinity(const TVec& x, const TVec& y) {
  return (x - y).array().abs().maxCoeff();
}

// Normalize a vector with the L1 norm, and return the norm before it was
// normalized.
template <typename TVec>
inline double NormalizeL1(TVec* x) {
  double norm = NormL1(*x);
  *x /= norm;
  return norm;
}

// Normalize a vector with the L2 norm, and return the norm before it was
// normalized.
template <typename TVec>
inline double NormalizeL2(TVec* x) {
  double norm = NormL2(*x);
  *x /= norm;
  return norm;
}

// Normalize a vector with the L^Infinity norm, and return the norm before it
// was normalized.
template <typename TVec>
inline double NormalizeLInfinity(TVec* x) {
  double norm = NormLInfinity(*x);
  *x /= norm;
  return norm;
}

// Return the square of a number.
template <typename T>
inline T Square(T x) {
  return x * x;
}

Mat3 RotationAroundX(double angle);
Mat3 RotationAroundY(double angle);
Mat3 RotationAroundZ(double angle);

// Returns the rotation matrix of a rotation of angle |axis| around axis.
// This is computed using the Rodrigues formula, see:
//  http://mathworld.wolfram.com/RodriguesRotationFormula.html
Mat3 RotationRodrigues(const Vec3& axis);

// Make a rotation matrix such that center becomes the direction of the
// positive z-axis, and y is oriented close to up.
Mat3 LookAt(Vec3 center);

// Return a diagonal matrix from a vector containg the diagonal values.
template <typename TVec>
inline Mat Diag(const TVec& x) {
  return x.asDiagonal();
}

template <typename TMat>
inline double FrobeniusNorm(const TMat& A) {
  return sqrt(A.array().abs2().sum());
}

template <typename TMat>
inline double FrobeniusDistance(const TMat& A, const TMat& B) {
  return FrobeniusNorm(A - B);
}

inline Vec3 CrossProduct(const Vec3& x, const Vec3& y) {
  return x.cross(y);
}

Mat3 CrossProductMatrix(const Vec3& x);

void MeanAndVarianceAlongRows(const Mat& A,
                              Vec* mean_pointer,
                              Vec* variance_pointer);

#if defined(_WIN32)
// TODO(bomboze): un-#if this for both platforms once tested under Windows
/* This solution was extensively discussed here
   http://forum.kde.org/viewtopic.php?f=74&t=61940 */
#  define SUM_OR_DYNAMIC(x, y)                                                 \
    (x == Eigen::Dynamic || y == Eigen::Dynamic) ? Eigen::Dynamic : (x + y)

template <typename Derived1, typename Derived2>
struct hstack_return {
  typedef typename Derived1::Scalar Scalar;
  enum {
    RowsAtCompileTime = Derived1::RowsAtCompileTime,
    ColsAtCompileTime = SUM_OR_DYNAMIC(Derived1::ColsAtCompileTime,
                                       Derived2::ColsAtCompileTime),
    Options = Derived1::Flags & Eigen::RowMajorBit ? Eigen::RowMajor : 0,
    MaxRowsAtCompileTime = Derived1::MaxRowsAtCompileTime,
    MaxColsAtCompileTime = SUM_OR_DYNAMIC(Derived1::MaxColsAtCompileTime,
                                          Derived2::MaxColsAtCompileTime)
  };
  typedef Eigen::Matrix<Scalar,
                        RowsAtCompileTime,
                        ColsAtCompileTime,
                        Options,
                        MaxRowsAtCompileTime,
                        MaxColsAtCompileTime>
      type;
};

template <typename Derived1, typename Derived2>
typename hstack_return<Derived1, Derived2>::type HStack(
    const Eigen::MatrixBase<Derived1>& lhs,
    const Eigen::MatrixBase<Derived2>& rhs) {
  typename hstack_return<Derived1, Derived2>::type res;
  res.resize(lhs.rows(), lhs.cols() + rhs.cols());
  res << lhs, rhs;
  return res;
};

template <typename Derived1, typename Derived2>
struct vstack_return {
  typedef typename Derived1::Scalar Scalar;
  enum {
    RowsAtCompileTime = SUM_OR_DYNAMIC(Derived1::RowsAtCompileTime,
                                       Derived2::RowsAtCompileTime),
    ColsAtCompileTime = Derived1::ColsAtCompileTime,
    Options = Derived1::Flags & Eigen::RowMajorBit ? Eigen::RowMajor : 0,
    MaxRowsAtCompileTime = SUM_OR_DYNAMIC(Derived1::MaxRowsAtCompileTime,
                                          Derived2::MaxRowsAtCompileTime),
    MaxColsAtCompileTime = Derived1::MaxColsAtCompileTime
  };
  typedef Eigen::Matrix<Scalar,
                        RowsAtCompileTime,
                        ColsAtCompileTime,
                        Options,
                        MaxRowsAtCompileTime,
                        MaxColsAtCompileTime>
      type;
};

template <typename Derived1, typename Derived2>
typename vstack_return<Derived1, Derived2>::type VStack(
    const Eigen::MatrixBase<Derived1>& lhs,
    const Eigen::MatrixBase<Derived2>& rhs) {
  typename vstack_return<Derived1, Derived2>::type res;
  res.resize(lhs.rows() + rhs.rows(), lhs.cols());
  res << lhs, rhs;
  return res;
};

#else  // _WIN32

// Since it is not possible to typedef privately here, use a macro.
// Always take dynamic columns if either side is dynamic.
#  define COLS                                                                 \
    ((ColsLeft == Eigen::Dynamic || ColsRight == Eigen::Dynamic)               \
         ? Eigen::Dynamic                                                      \
         : (ColsLeft + ColsRight))

// Same as above, except that prefer fixed size if either is fixed.
#  define ROWS                                                                 \
    ((RowsLeft == Eigen::Dynamic && RowsRight == Eigen::Dynamic)               \
         ? Eigen::Dynamic                                                      \
         : ((RowsLeft == Eigen::Dynamic) ? RowsRight : RowsLeft))

// TODO(keir): Add a static assert if both rows are at compiletime.
template <typename T, int RowsLeft, int RowsRight, int ColsLeft, int ColsRight>
Eigen::Matrix<T, ROWS, COLS> HStack(
    const Eigen::Matrix<T, RowsLeft, ColsLeft>& left,
    const Eigen::Matrix<T, RowsRight, ColsRight>& right) {
  assert(left.rows() == right.rows());
  int n = left.rows();
  int m1 = left.cols();
  int m2 = right.cols();

  Eigen::Matrix<T, ROWS, COLS> stacked(n, m1 + m2);
  stacked.block(0, 0, n, m1) = left;
  stacked.block(0, m1, n, m2) = right;
  return stacked;
}

// Reuse the above macros by swapping the order of Rows and Cols. Nasty, but
// the duplication is worse.
// TODO(keir): Add a static assert if both rows are at compiletime.
// TODO(keir): Mail eigen list about making this work for general expressions
// rather than only matrix types.
template <typename T, int RowsLeft, int RowsRight, int ColsLeft, int ColsRight>
Eigen::Matrix<T, COLS, ROWS> VStack(
    const Eigen::Matrix<T, ColsLeft, RowsLeft>& top,
    const Eigen::Matrix<T, ColsRight, RowsRight>& bottom) {
  assert(top.cols() == bottom.cols());
  int n1 = top.rows();
  int n2 = bottom.rows();
  int m = top.cols();

  Eigen::Matrix<T, COLS, ROWS> stacked(n1 + n2, m);
  stacked.block(0, 0, n1, m) = top;
  stacked.block(n1, 0, n2, m) = bottom;
  return stacked;
}
#  undef COLS
#  undef ROWS
#endif  // _WIN32

void HorizontalStack(const Mat& left, const Mat& right, Mat* stacked);

template <typename TTop, typename TBot, typename TStacked>
void VerticalStack(const TTop& top, const TBot& bottom, TStacked* stacked) {
  assert(top.cols() == bottom.cols());
  int n1 = top.rows();
  int n2 = bottom.rows();
  int m = top.cols();

  stacked->resize(n1 + n2, m);
  stacked->block(0, 0, n1, m) = top;
  stacked->block(n1, 0, n2, m) = bottom;
}

void MatrixColumn(const Mat& A, int i, Vec2* v);
void MatrixColumn(const Mat& A, int i, Vec3* v);
void MatrixColumn(const Mat& A, int i, Vec4* v);

template <typename TMat, typename TCols>
TMat ExtractColumns(const TMat& A, const TCols& columns) {
  TMat compressed(A.rows(), columns.size());
  for (int i = 0; i < columns.size(); ++i) {
    compressed.col(i) = A.col(columns[i]);
  }
  return compressed;
}

template <typename TMat, typename TDest>
void reshape(const TMat& a, int rows, int cols, TDest* b) {
  assert(a.rows() * a.cols() == rows * cols);
  b->resize(rows, cols);
  for (int i = 0; i < rows; i++) {
    for (int j = 0; j < cols; j++) {
      (*b)(i, j) = a[cols * i + j];
    }
  }
}

inline bool isnan(double i) {
#ifdef WIN32
  return _isnan(i) > 0;
#else
  return std::isnan(i);
#endif
}

/// Ceil function that has the same behavior for positive
/// and negative values
template <typename FloatType>
FloatType ceil0(const FloatType& value) {
  FloatType result = std::ceil(std::fabs(value));
  return (value < 0.0) ? -result : result;
}

/// Returns the skew anti-symmetric matrix of a vector
inline Mat3 SkewMat(const Vec3& x) {
  Mat3 skew;
  skew << 0, -x(2), x(1), x(2), 0, -x(0), -x(1), x(0), 0;
  return skew;
}
/// Returns the skew anti-symmetric matrix of a vector with only
/// the first two (independent) lines
inline Mat23 SkewMatMinimal(const Vec2& x) {
  Mat23 skew;
  skew << 0, -1, x(1), 1, 0, -x(0);
  return skew;
}

/// Returns the rotaiton matrix built from given vector of euler angles
inline Mat3 RotationFromEulerVector(Vec3 euler_vector) {
  double theta = euler_vector.norm();
  if (theta == 0.0) {
    return Mat3::Identity();
  }
  Vec3 w = euler_vector / theta;
  Mat3 w_hat = CrossProductMatrix(w);
  return Mat3::Identity() + w_hat * sin(theta) +
         w_hat * w_hat * (1 - cos(theta));
}
}  // namespace libmv

#endif  // LIBMV_NUMERIC_NUMERIC_H