/***************************************************************************
**
**  This file is part of QGpCoreMath.
**
**  This library is free software; you can redistribute it and/or
**  modify it under the terms of the GNU Lesser General Public
**  License as published by the Free Software Foundation; either
**  version 2.1 of the License, or (at your option) any later version.
**
**  This file is distributed in the hope that it will be useful, but WITHOUT
**  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
**  FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
**  License for more details.
**
**  You should have received a copy of the GNU Lesser General Public
**  License along with this library; if not, write to the Free Software
**  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2008-07-15
**  Copyright: 2008-2019
**    Marc Wathelet
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include "DoubleMatrix.h"
#include "Random.h"

namespace QGpCoreMath {

  /*!
    \class Matrix3x3 DoubleMatrix.h
    \brief Brief description of class still missing

    3x3 matrix with double values
  */

  /*!
    \class DoubleMatrix DoubleMatrix.h
    \brief Brief description of class still missing

    NxM matrix with double values stored in the same way as LAPACK: iCol*nRows+iRow
  */

  /*!
    Sets matrix as a rotation matrix around axis \a ax. The angle is counted using the right hand convention.
    Angle is in radians. The matrix must be set to identity before. Transformation matrix (4x4) can be constructed
    with the same function.
  */
  void DoubleMatrix::rotation(AxisType ax, const Angle& angle)
  {
    TRACE;
    switch (ax) {
    case XAxis:
      at(1, 1)=angle.cos();
      at(1, 2)=-angle.sin();
      at(2, 1)=angle.sin();
      at(2, 2)=angle.cos();
      break;
    case YAxis:
      at(0, 0)=angle.cos();
      at(0, 2)=angle.sin();
      at(2, 0)=-angle.sin();
      at(2, 2)=angle.cos();
      break;
    case ZAxis:
      at(0, 0)=angle.cos();
      at(0, 1)=-angle.sin();
      at(1, 0)=angle.sin();
      at(1, 1)=angle.cos();
      break;
    }
  }

  /*!
    \fn void Matrix4x4::rotation(AxisType ax, const Angle& angle)
    Sets matrix as a rotation matrix. The angle is counted using the right hand convention.
    Angle is in radians.
  */

  /*!
    Returns the upper left submatrix corresponding to the rotation part.
  */
  Matrix3x3 Matrix4x4::rotationOnly() const
  {
    TRACE;
    Matrix3x3 m;
    const double * d1=values();
    double * d2=m.values();
    d2[0]=d1[0];
    d2[1]=d1[1];
    d2[2]=d1[2];
    d2[3]=d1[4];
    d2[4]=d1[5];
    d2[5]=d1[6];
    d2[6]=d1[8];
    d2[7]=d1[9];
    d2[8]=d1[10];
    return m;
  }

  /*!
   \fn void Matrix2x2::rotation(const Angle& angle)
    Sets matrix as a rotation matrix. The angle is counted using the right hand convention.
    Angle is in radians.
  */

  /*!
    \fn void Matrix3x3::rotation(AxisType ax, const Angle& angle)
    Sets matrix as a rotation matrix. The angle is counted using the right hand convention.
    Angle is in radians.
  */

  extern "C" {
    // LAPACK call for double precision matrix inversion
    void dgetrf_(int * m, int * n,
                 double * a, int * lda, int * ipiv, int * info);
    void dgetri_(int * n, double * a, int * lda, int * ipiv,
                 double * work, int * lwork, int * info);
  }

  /*!
    Based on LAPACK LU decomposition, for general matrix type.
  */
  bool DoubleMatrix::invert()
  {
    ASSERT(rowCount()==columnCount());
    double * values=_d->values();
    int m=rowCount();
    if(m==1) {  // Not accepted by dgetrf
      values[0]=1.0/values[0];
      return true;
    }
    int n=columnCount();                // The order of matrix A
    int lda=m;                          // The leading dimension of A
    int * ipiv=new int[n];              // Pivot indices
    int lwork=5*n;                      // Dimension of workspace
    double * work=new double[lwork];    // work space
    int info;                           // dimension of workspaces
    dgetrf_(&m, &n, values, &lda, ipiv, &info);
    if(info!=0) { // singular matrix
      delete [] ipiv;
      delete [] work;
      return false;
    }
    dgetri_(&n, values, &lda, ipiv, work, &lwork, &info);
    if(info!=0) {
      delete [] ipiv;
      delete [] work;
      return false;
    }
    delete [] ipiv;
    delete [] work;
    _d->swapRowColumn();
    return true;
  }

  /*!
    Fills the matrix with random values between \a min and \a max
  */
  void DoubleMatrix::random(double min, double max)
  {
    Random r;
    double * values=_d->values();
    for(int i=columnCount()*rowCount()-1; i>=0;i--) {
      values[i]=r.uniform(min, max);
    }
  }

  extern "C" {
    // LAPACK call for double precision matrix singular value decomposition
    void dgesvd_(char *jobu, char *jobvt, int *m, int *n,
                 double *a, int *lda, double *s, double *u,
                 int *ldu, double *vt, int *ldvt, double *work,
                 int *lwork, int *info);
  }

  /*!
    Computes singular value decomposition U SIGMA Vt
  */
  void DoubleMatrix::singularValues(QVector<double>& values, DoubleMatrix * u, DoubleMatrix * vt) const
  {
    int m=rowCount();
    int n=columnCount();
    int minDim=m;
    if(n<minDim) {
      minDim=n;
    }
    values.resize(minDim);

    double * a=copyOfValues();  // A copy of matrix values because its content will be destroyed

    double * um;                          // left-singular vectors
    if(u) {
      u->resize(m, m);
      um=u->values();
    } else {
      um=new double[m*m];
    }

    double * vtm;                         // right-singular vectors
    if(vt) {
      vt->resize(n, n);
      vtm=vt->values();
    } else {
      vtm=new double[n*n];
    }

    double * s=values.data();             // singular values

    char jobu='A';            // flags to tell what to do in dgesvd_
    char jobvt='A';
    int lda=m;
    int ldu=m;
    int ldvt=n;
    int info=0;
    double * work=nullptr;
    int lwork=-1; // First query the optimal size for work
    dgesvd_(&jobu, &jobvt, &m, &n, a, &lda, s, um, &ldu, vtm, &ldvt, work, &lwork, &info);
    work=new double[lwork];  // work space
    dgesvd_(&jobu, &jobvt, &m, &n, a, &lda, s, um, &ldu, vtm, &ldvt, work, &lwork, &info);
    // Output is A=U * SIGMA * VT
    delete [] work;
    if(!vt) {
      delete [] vtm;
    }
    if(!u) {
      delete [] u;
    }
    delete [] a;
  }

  extern "C" {
    // LAPACK call to reduce a symetric double precision matrix to a tridiagonal form
    void dsytrd_(char * uplo, int * n, double * a, int * lda, double * d,
                double * e, double * tau, double * work, int * lwork, int *info);
    // LAPACK call to eigen values of tridiagonal matrix
    void dsteqr_(char * compz, int * n, double * d, double * e, double * z,
                 int * ldz, double * work, int * info);
  }

  /*!
    Compute eigen values of a symetric matrix
  */
  QVector<double> DoubleMatrix::symetricEigenValues(bool& ok) const
  {
    int m=rowCount();
    int n=columnCount();
    int minDim=m;
    if(n<minDim) {
      minDim=n;
    }

    double * a=new double[m*n];           // A copy of matrix values because its content will be destroyed
    const double * aValues=values();
    for(int i=m*n-1; i>=0; i--) {
      a[i]=aValues[i];
    }

    QVector<double> values(minDim);
    double * d=values.data();
    double * e=new double[minDim-1];
    double * tau=new double[minDim-1];
    char uplo='U';            // Values supposed to be in upper triangular part
    int lda=m;
    int info=0;
    int lwork=32*minDim; // Query the optimal size for work in the case of a 2x2, 3x3 and 4x4
                         // matrices returns 64, 96 and 128, respectively
    double * work=new double[lwork];
    dsytrd_(&uplo, &minDim, a, &lda, d, e, tau, work, &lwork, &info);
    delete [] work;
    delete [] a;
    delete [] tau;
    if(info==0) {
      char compz='N';
      dsteqr_(&compz, &minDim, d, e, nullptr, &lda, nullptr, &info);
      if(info!=0) {
        ok=false;
      }
    } else {
      ok=false;
    }
    delete [] e;
    return values;
  }

  extern "C" {
    // LAPACK call to QR factorization of a real M-byN matrix A
    void dgeqrf_(int * m, int * n,
                 double * a, int * lda, double * tau,
                 double * work, int * lwork, int * info);
    // LAPACK call to multiplication of a matrix by Q or Q^T
    void dormqr_(char * side, char * trans,
                 int  * m, int * n, int * k,
                 double * a, int * lda, double * tau,
                 double * c, int * ldc,
                 double * work, int * lwork, int * info);
    // LAPACK call to solve a triangular sytem
    void dtrtrs_(char * uplo, char * trans, char * diag,
                 int * n, int * nrhs,
                 double * a, int * lda,
                 double * b, int * ldb,
                 int * info);
  }

  /*!
    Solve overdetermined system Ax=b where this is A. Return optimal vector x
  */
  DoubleMatrix DoubleMatrix::leastSquare(const DoubleMatrix& b) const
  {
    int m=rowCount();
    int n=columnCount();
    ASSERT(m>=n);

    // Calculate Q and R
    double * a=copyOfValues();  // A copy of matrix values because its content will be destroyed
    double * tau=new double[n];
    int lwork=-1;
    double iwork;
    int info=0;
    dgeqrf_(&m, &n, a, &m, tau, &iwork, &lwork, &info);  // Dry run to query work size
    lwork=static_cast<int>(iwork);
    double * work=new double[lwork];
    dgeqrf_(&m, &n, a, &m, tau, work, &lwork, &info);
    delete [] work;

    // Calculate Q^T*b
    double * bval=b.copyOfValues();  // A copy of matrix values because its content will be destroyed
    int unit=1;
    char side='L';
    char trans='T';
    lwork=-1;
    dormqr_(&side, &trans, &m, &unit, &n, a, &m, tau, bval, &m, &iwork, &lwork, &info);  // Dry run to query work size
    lwork=static_cast<int>(iwork);
    work=new double[lwork];
    dormqr_(&side, &trans, &m, &unit, &n, a, &m, tau, bval, &m, work, &lwork, &info);
    delete [] work;
    delete [] tau;

    // Solve triangular system A*x=b
    char uplo='U';
    char diag='N';
    dtrtrs_(&uplo, &diag, &diag, &n, &unit, a, &m, bval, &m, &info);
    delete [] a;

    DoubleMatrix x(n, 1);
    double * xval=x.values();
    for(int i=n-1; i>=0; i--) {
      xval[i]=bval[i];
    }
    delete [] bval;
    return x;
  }

  double Matrix2x2::determinant() const
  {
    return at(0, 0)*at(1, 1)-at(0, 1)*at(0, 1);
  }

  Segment Matrix3x3::operator*(const Segment& s) const
  {
    Point p1=(*this)*s.point(0);
    Point p2=(*this)*s.point(1);
    return Segment(p1, p2);
  }

  Segment Matrix4x4::operator*(const Segment& s) const
  {
    Point p1=(*this)*s.point(0);
    Point p2=(*this)*s.point(1);
    return Segment(p1, p2);
  }

} // namespace QGpCoreMath
