/**
 * Stone of Orthanc
 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
 * Department, University Hospital of Liege, Belgium
 * Copyright (C) 2017-2019 Osimis S.A., Belgium
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * This program 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
 * Affero General Public License for more details.
 * 
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 **/


#include "LinearAlgebra.h"

#include "../StoneException.h"

#include <Core/Logging.h>
#include <Core/OrthancException.h>
#include <Core/Toolbox.h>

#include <boost/lexical_cast.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <stdio.h>
#include <iostream>

namespace OrthancStone
{
  namespace LinearAlgebra
  {
    void Print(const Vector& v)
    {
      for (size_t i = 0; i < v.size(); i++)
      {
        printf("%g\n", v[i]);
        //printf("%8.2f\n", v[i]);
      }
      printf("\n");
    }


    void Print(const Matrix& m)
    {
      for (size_t i = 0; i < m.size1(); i++)
      {
        for (size_t j = 0; j < m.size2(); j++)
        {
          printf("%g  ", m(i,j));
          //printf("%8.2f  ", m(i,j));
        }
        printf("\n");        
      }
      printf("\n");        
    }


    bool ParseVector(Vector& target,
                     const std::string& value)
    {
      std::vector<std::string> items;
      Orthanc::Toolbox::TokenizeString(items, Orthanc::Toolbox::StripSpaces(value), '\\');

      target.resize(items.size());

      for (size_t i = 0; i < items.size(); i++)
      {
        /**
         * We try and avoid the use of "boost::lexical_cast<>" here,
         * as it is very slow. As we are parsing many doubles, we
         * prefer to use the standard "std::stod" function if
         * available: http://www.cplusplus.com/reference/string/stod/
         **/
          
#if __cplusplus >= 201103L  // Is C++11 enabled?
        try
        {
          target[i] = std::stod(items[i]);
        }
        catch (std::exception&)
        {
          target.clear();
          return false;
        }
#else  // Fallback implementation using Boost
        try
        {
          target[i] = boost::lexical_cast<double>(items[i]);
        }
        catch (boost::bad_lexical_cast&)
        {
          target.clear();
          return false;
        }
#endif
      }

      return true;
    }


    bool ParseVector(Vector& target,
                     const Orthanc::DicomMap& dataset,
                     const Orthanc::DicomTag& tag)
    {
      std::string value;
      return (dataset.LookupStringValue(value, tag, false) &&
              ParseVector(target, value));
    }


    void NormalizeVector(Vector& u)
    {
      double norm = boost::numeric::ublas::norm_2(u);
      if (!IsCloseToZero(norm))
      {
        u = u / norm;
      }
    }

    void CrossProduct(Vector& result,
                      const Vector& u,
                      const Vector& v)
    {
      if (u.size() != 3 ||
          v.size() != 3)
      {
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      result.resize(3);

      result[0] = u[1] * v[2] - u[2] * v[1];
      result[1] = u[2] * v[0] - u[0] * v[2];
      result[2] = u[0] * v[1] - u[1] * v[0];
    }

    double DotProduct(const Vector& u, const Vector& v)
    {
      if (u.size() != 3 ||
        v.size() != 3)
      {
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      return (u[0] * v[0] + u[1] * v[1] + u[2] * v[2]);
    }

    void FillMatrix(Matrix& target,
                    size_t rows,
                    size_t columns,
                    const double values[])
    {
      target.resize(rows, columns);

      size_t index = 0;

      for (size_t y = 0; y < rows; y++)
      {
        for (size_t x = 0; x < columns; x++, index++)
        {
          target(y, x) = values[index];
        }
      }
    }


    void FillVector(Vector& target,
                    size_t size,
                    const double values[])
    {
      target.resize(size);

      for (size_t i = 0; i < size; i++)
      {
        target[i] = values[i];
      }
    }


    void Convert(Matrix& target,
                 const Vector& source)
    {
      const size_t n = source.size();

      target.resize(n, 1);

      for (size_t i = 0; i < n; i++)
      {
        target(i, 0) = source[i];
      }      
    }

    
    double ComputeDeterminant(const Matrix& a)
    {
      if (a.size1() != a.size2())
      {
        LOG(ERROR) << "Determinant only exists for square matrices";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }
    
      // https://en.wikipedia.org/wiki/Rule_of_Sarrus
      if (a.size1() == 1)
      {
        return a(0,0);
      }
      else if (a.size1() == 2)
      {
        return a(0,0) * a(1,1) - a(0,1) * a(1,0);
      }
      else if (a.size1() == 3)
      {
        return (a(0,0) * a(1,1) * a(2,2) + 
                a(0,1) * a(1,2) * a(2,0) +
                a(0,2) * a(1,0) * a(2,1) -
                a(2,0) * a(1,1) * a(0,2) -
                a(2,1) * a(1,2) * a(0,0) -
                a(2,2) * a(1,0) * a(0,1));
      }
      else
      {
        throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
      }
    }
    

    bool IsOrthogonalMatrix(const Matrix& q,
                            double threshold)
    {
      // https://en.wikipedia.org/wiki/Orthogonal_matrix

      if (q.size1() != q.size2())
      {
        LOG(ERROR) << "An orthogonal matrix must be squared";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      using namespace boost::numeric::ublas;

      const Matrix check = prod(trans(q), q) - identity_matrix<double>(q.size1());

      type_traits<double>::real_type norm = norm_inf(check);

      return (norm <= threshold);
    }


    bool IsOrthogonalMatrix(const Matrix& q)
    {
      return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon());
    }


    bool IsRotationMatrix(const Matrix& r,
                          double threshold)
    {
      return (IsOrthogonalMatrix(r, threshold) &&
              IsNear(ComputeDeterminant(r), 1.0, threshold));
    }


    bool IsRotationMatrix(const Matrix& r)
    {
      return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon());
    }


    void InvertUpperTriangularMatrix(Matrix& output,
                                     const Matrix& k)
    {
      if (k.size1() != k.size2())
      {
        LOG(ERROR) << "Determinant only exists for square matrices";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      output.resize(k.size1(), k.size2());

      for (size_t i = 1; i < k.size1(); i++)
      {
        for (size_t j = 0; j < i; j++)
        {
          if (!IsCloseToZero(k(i, j)))
          {
            LOG(ERROR) << "Not an upper triangular matrix";
            throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
          }

          output(i, j) = 0;  // The output is also upper triangular
        }
      }

      if (k.size1() == 3)
      {
        // https://math.stackexchange.com/a/1004181
        double a = k(0, 0);
        double b = k(0, 1);
        double c = k(0, 2);
        double d = k(1, 1);
        double e = k(1, 2);
        double f = k(2, 2);

        if (IsCloseToZero(a) ||
            IsCloseToZero(d) ||
            IsCloseToZero(f))
        {
          LOG(ERROR) << "Singular upper triangular matrix";
          throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
        }
        else
        {
          output(0, 0) = 1.0 / a;
          output(0, 1) = -b / (a * d);
          output(0, 2) = (b * e - c * d) / (a * f * d);
          output(1, 1) = 1.0 / d;
          output(1, 2) = -e / (f * d);
          output(2, 2) = 1.0 / f;
        }
      }
      else
      {
        throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
      }
    }


    static void GetGivensComponent(double& c,
                                   double& s,
                                   const Matrix& a,
                                   size_t i,
                                   size_t j)
    {
      assert(i < 3 && j < 3);

      double x = a(i, i);
      double y = a(i, j);
      double n = sqrt(x * x + y * y);

      if (IsCloseToZero(n))
      {
        c = 1;
        s = 0;
      }
      else
      {
        c = x / n;
        s = -y / n;
      }
    }


    /**
     * This function computes the RQ decomposition of a 3x3 matrix,
     * using Givens rotations. Reference: Algorithm A4.1 (page 579) of
     * "Multiple View Geometry in Computer Vision" (2nd edition).  The
     * output matrix "Q" is a rotation matrix, and "R" is upper
     * triangular.
     **/
    void RQDecomposition3x3(Matrix& r,
                            Matrix& q,
                            const Matrix& a)
    {
      using namespace boost::numeric::ublas;
      
      if (a.size1() != 3 ||
          a.size2() != 3)
      {
        LOG(ERROR) << "Only applicable to a 3x3 matrix";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      r.resize(3, 3);
      q.resize(3, 3);

      r = a;
      q = identity_matrix<double>(3);

      {
        // Set A(2,1) to zero
        double c, s;
        GetGivensComponent(c, s, r, 2, 1);

        double v[9] = { 1, 0, 0, 
                        0, c, -s,
                        0, s, c };

        Matrix g;
        FillMatrix(g, 3, 3, v);

        r = prod(r, g);
        q = prod(trans(g), q);
      }


      {
        // Set A(2,0) to zero
        double c, s;
        GetGivensComponent(c, s, r, 2, 0);

        double v[9] = { c, 0, -s, 
                        0, 1, 0,
                        s, 0, c };

        Matrix g;
        FillMatrix(g, 3, 3, v);

        r = prod(r, g);
        q = prod(trans(g), q);
      }


      {
        // Set A(1,0) to zero
        double c, s;
        GetGivensComponent(c, s, r, 1, 0);

        double v[9] = { c, -s, 0, 
                        s, c, 0,
                        0, 0, 1 };

        Matrix g;
        FillMatrix(g, 3, 3, v);

        r = prod(r, g);
        q = prod(trans(g), q);
      }

      if (!IsCloseToZero(norm_inf(prod(r, q) - a)) ||
          !IsRotationMatrix(q) ||
          !IsCloseToZero(r(1, 0)) ||
          !IsCloseToZero(r(2, 0)) ||
          !IsCloseToZero(r(2, 1)))
      {
        throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
      }
    }


    bool InvertMatrixUnsafe(Matrix& target,
                            const Matrix& source)
    {
      if (source.size1() != source.size2())
      {
        LOG(ERROR) << "Inverse only exists for square matrices";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      if (source.size1() < 4)
      {
        // For matrices with size below 4, use direct computations
        // instead of LU decomposition

        if (source.size1() == 0)
        {
          // By convention, the inverse of the empty matrix, is itself the empty matrix
          target.resize(0, 0);
          return true;
        }

        double determinant = ComputeDeterminant(source);

        if (IsCloseToZero(determinant))
        {
          return false;
        }

        double denominator = 1.0 / determinant;

        target.resize(source.size1(), source.size2());

        if (source.size1() == 1)
        {
          target(0, 0) = denominator;

          return true;
        }
        else if (source.size1() == 2)
        {
          // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices
          target(0, 0) =  source(1, 1) * denominator;
          target(0, 1) = -source(0, 1) * denominator;
          target(1, 0) = -source(1, 0) * denominator;
          target(1, 1) =  source(0, 0) * denominator;

          return true;
        }
        else if (source.size1() == 3)
        {
          // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices
          const double a = source(0, 0);
          const double b = source(0, 1);
          const double c = source(0, 2);
          const double d = source(1, 0);
          const double e = source(1, 1);
          const double f = source(1, 2);
          const double g = source(2, 0);
          const double h = source(2, 1);
          const double i = source(2, 2);
        
          target(0, 0) =  (e * i - f * h) * denominator;
          target(0, 1) = -(b * i - c * h) * denominator;
          target(0, 2) =  (b * f - c * e) * denominator;
          target(1, 0) = -(d * i - f * g) * denominator;
          target(1, 1) =  (a * i - c * g) * denominator;
          target(1, 2) = -(a * f - c * d) * denominator;
          target(2, 0) =  (d * h - e * g) * denominator;
          target(2, 1) = -(a * h - b * g) * denominator;
          target(2, 2) =  (a * e - b * d) * denominator;

          return true;
        }
        else
        {
          throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
        }
      }
      else
      {
        // General case, using LU decomposition

        Matrix a = source;  // Copy the source matrix, as "lu_factorize()" modifies it

        boost::numeric::ublas::permutation_matrix<size_t>  permutation(source.size1());
        
        if (boost::numeric::ublas::lu_factorize(a, permutation) != 0)
        {
          return false;
        }
        else
        {
          target = boost::numeric::ublas::identity_matrix<double>(source.size1());
          lu_substitute(a, permutation, target);
          return true;
        }
      }
    }
    

    
    void InvertMatrix(Matrix& target,
                      const Matrix& source)
    {
      if (!InvertMatrixUnsafe(target, source))
      {
        LOG(ERROR) << "Cannot invert singular matrix";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }
    }

    
    void CreateSkewSymmetric(Matrix& s,
                             const Vector& v)
    {
      if (v.size() != 3)
      {
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      s.resize(3, 3);
      s(0,0) = 0;
      s(0,1) = -v[2];
      s(0,2) = v[1];
      s(1,0) = v[2];
      s(1,1) = 0;
      s(1,2) = -v[0];
      s(2,0) = -v[1];
      s(2,1) = v[0];
      s(2,2) = 0;
    }

  
    Matrix InvertScalingTranslationMatrix(const Matrix& t)
    {
      if (t.size1() != 4 ||
          t.size2() != 4 ||
          !LinearAlgebra::IsCloseToZero(t(0,1)) ||
          !LinearAlgebra::IsCloseToZero(t(0,2)) ||
          !LinearAlgebra::IsCloseToZero(t(1,0)) ||
          !LinearAlgebra::IsCloseToZero(t(1,2)) ||
          !LinearAlgebra::IsCloseToZero(t(2,0)) ||
          !LinearAlgebra::IsCloseToZero(t(2,1)) ||
          !LinearAlgebra::IsCloseToZero(t(3,0)) ||
          !LinearAlgebra::IsCloseToZero(t(3,1)) ||
          !LinearAlgebra::IsCloseToZero(t(3,2)))
      {
        LOG(ERROR) << "This matrix is more than a zoom/translate transform";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
      }

      const double sx = t(0,0);
      const double sy = t(1,1);
      const double sz = t(2,2);
      const double w = t(3,3);

      if (LinearAlgebra::IsCloseToZero(sx) ||
          LinearAlgebra::IsCloseToZero(sy) ||
          LinearAlgebra::IsCloseToZero(sz) ||
          LinearAlgebra::IsCloseToZero(w))
      {
        LOG(ERROR) << "Singular transform";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
      }
    
      const double tx = t(0,3);
      const double ty = t(1,3);
      const double tz = t(2,3);
    
      Matrix m = IdentityMatrix(4);

      m(0,0) = 1.0 / sx;
      m(1,1) = 1.0 / sy;
      m(2,2) = 1.0 / sz;
      m(3,3) = 1.0 / w;

      m(0,3) = -tx / (sx * w);
      m(1,3) = -ty / (sy * w);
      m(2,3) = -tz / (sz * w);

      return m;
    }


    bool IsShearMatrix(const Matrix& shear)
    {
      return (shear.size1() == 4 &&
              shear.size2() == 4 &&
              LinearAlgebra::IsNear(1.0, shear(0,0)) &&
              LinearAlgebra::IsNear(0.0, shear(0,1)) &&
              LinearAlgebra::IsNear(0.0, shear(0,3)) &&
              LinearAlgebra::IsNear(0.0, shear(1,0)) &&
              LinearAlgebra::IsNear(1.0, shear(1,1)) &&
              LinearAlgebra::IsNear(0.0, shear(1,3)) &&
              LinearAlgebra::IsNear(0.0, shear(2,0)) &&
              LinearAlgebra::IsNear(0.0, shear(2,1)) &&
              LinearAlgebra::IsNear(1.0, shear(2,2)) &&
              LinearAlgebra::IsNear(0.0, shear(2,3)) &&
              LinearAlgebra::IsNear(0.0, shear(3,0)) &&
              LinearAlgebra::IsNear(0.0, shear(3,1)) &&
              LinearAlgebra::IsNear(1.0, shear(3,3)));
    }
  

    Matrix InvertShearMatrix(const Matrix& shear)
    {
      if (!IsShearMatrix(shear))
      {
        LOG(ERROR) << "Not a valid shear matrix";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
      }

      Matrix m = IdentityMatrix(4);
      m(0,2) = -shear(0,2);
      m(1,2) = -shear(1,2);
      m(3,2) = -shear(3,2);

      return m;
    }
  }

  std::ostream& operator<<(std::ostream& s, const Vector& vec)
  {
    s << "(";
    for (size_t i = 0; i < vec.size(); ++i)
    {
      s << vec(i);
      if (i < (vec.size() - 1))
        s << ", ";
    }
    s << ")";
    return s;
  }

  std::ostream& operator<<(std::ostream& s, const Matrix& m)
  {
    ORTHANC_ASSERT(m.size1() == m.size2());
    s << "(";
    for (size_t i = 0; i < m.size1(); ++i)
    {
      s << "(";
      for (size_t j = 0; j < m.size2(); ++j)
      {
        s << m(i,j);
        if (j < (m.size2() - 1))
          s << ", ";
      }
      s << ")";
      if (i < (m.size1() - 1))
        s << ", ";
    }
    s << ")";
    return s;
  }
}