#ifndef Magnum_Math_Algorithms_Svd_h
#define Magnum_Math_Algorithms_Svd_h
/*
    Copyright © 2010, 2011, 2012 Vladimír Vondruš <mosra@centrum.cz>

    This file is part of Magnum.

    Magnum is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License version 3
    only, as published by the Free Software Foundation.

    Magnum 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 version 3 for more details.
*/

/** @file
 * @brief Function Magnum::Math::Algorithms::svd()
 */

#include <tuple>

#include "Math/Functions.h"
#include "Math/Matrix.h"

namespace Magnum { namespace Math { namespace Algorithms {

#ifndef DOXYGEN_GENERATING_OUTPUT
namespace Implementation {

template<class T> T pythagoras(T a, T b) {
    T absa = std::abs(a);
    T absb = std::abs(b);
    if(absa > absb)
        return absa*std::sqrt(T(1) + Math::pow<2>(absb/absa));
    else if(absb == T(0)) /** @todo epsilon comparison? */
        return T(0);
    else
        return absb*std::sqrt(T(1) + Math::pow<2>(absa/absb));
}

template<class T> T smallestDelta();
template<> inline constexpr float smallestDelta<float>() { return 1.0e-32; }
template<> inline constexpr double smallestDelta<double>() { return 1.0e-64; }

}
#endif

/**
@brief Singular Value Decomposition

Performs Thin SVD on given matrix where @p rows >= @p cols:
@f[
    M = U \Sigma V^*
@f]
Returns first @p cols column vectors of @f$ U @f$, diagonal of @f$ \Sigma @f$
and non-transposed @f$ V @f$. If the solution doesn't converge, returns
zero matrices.

Full @f$ U @f$, @f$ \Sigma @f$ matrices and original @f$ M @f$ matrix can be
reconstructed from the values as following:
@code
RectangularMatrix<cols, rows, double> m;

RectangularMatrix<cols, rows, double> uPart;
Vector<cols, double> wDiagonal;
Matrix<cols, double> v;

std::tie(uPart, wDiagonal, v) = Math::Algorithms::svd(m);

// Extend U
Matrix<rows, double> u(Matrix<rows, double>::Zero);
for(std::size_t i = 0; i != rows; ++i)
    u[i] = uPart[i];

// Diagonal W
RectangularMatrix<cols, rows, double> w =
    RectangularMatrix<cols, rows, double>::fromDiagonal(wDiagonal);

// u*w*v.transposed() == m
@endcode

Implementation based on *Golub, G. H.; Reinsch, C. (1970). "Singular value
decomposition and least squares solutions"*.
*/
/* The matrix is passed by value because it is changed inside */
template<std::size_t cols, std::size_t rows, class T> std::tuple<RectangularMatrix<cols, rows, T>, Vector<cols, T>, Matrix<cols, T>> svd(RectangularMatrix<cols, rows, T> m) {
    static_assert(rows >= cols, "Unsupported matrix aspect ratio");
    static_assert(T(1)+MathTypeTraits<T>::epsilon() > T(1), "Epsilon too small");
    constexpr T tol = Implementation::smallestDelta<T>()/MathTypeTraits<T>::epsilon();
    static_assert(tol > T(0), "Tol too small");
    constexpr std::size_t maxIterations = 50;

    Matrix<cols, T> v(Matrix<cols, T>::Zero);
    Vector<cols, T> e, q;

    /* Householder's reduction to bidiagonal form */
    T g = T(0);
    T epsilonX = T(0);
    for(std::size_t i = 0; i != cols; ++i) {
        const std::size_t l = i+1;

        e[i] = g;
        T s1 = T(0);
        for(std::size_t j = i; j != rows; ++j)
            s1 += Math::pow<2>(m[i][j]);
        if(s1 > tol) {
            const T f = m[i][i];
            g = (f < T(0) ? std::sqrt(s1) : -std::sqrt(s1));
            const T h = f*g - s1;
            m[i][i] = f - g;

            for(std::size_t j = l; j != cols; ++j) {
                T s = T(0);
                for(std::size_t k = i; k != rows; ++k)
                    s += m[i][k]*m[j][k];
                const T f = s/h;
                for(std::size_t k = i; k != rows; ++k)
                    m[j][k] += f*m[i][k];
            }
        } else g = T(0);

        q[i] = g;
        T s2 = T(0);
        for(std::size_t j = l; j != cols; ++j)
            s2 += Math::pow<2>(m[j][i]);
        if(s2 > tol) {
            const T f = m[i+1][i];
            g = (f < T(0) ? std::sqrt(s2) : -std::sqrt(s2));
            const T h = f*g - s2;
            m[i+1][i] = f - g;

            for(std::size_t j = l; j != cols; ++j)
                e[j] = m[j][i]/h;

            for(std::size_t j = l; j != rows; ++j) {
                T s = T(0);
                for(std::size_t k = l; k != cols; ++k)
                    s += m[k][j]*m[k][i];
                for(std::size_t k = l; k != cols; ++k)
                    m[k][j] += s*e[k];
            }

        } else g = T(0);

        const T y = std::abs(q[i]) + std::abs(e[i]);
        if(y > epsilonX) epsilonX = y;
    }

    /* Accumulation of right hand transformations */
    for(std::size_t l = cols; l != 0; --l) {
        const std::size_t i = l-1;

        if(g != T(0)) { /** @todo epsilon check? */
            const T h = g*m[i+1][i];

            for(std::size_t j = l; j != cols; ++j)
                v[i][j] = m[j][i]/h;

            for(std::size_t j = l; j != cols; ++j) {
                T s = T(0);
                for(std::size_t k = l; k != cols; ++k)
                    s += m[k][i]*v[j][k];
                for(std::size_t k = l; k != cols; ++k)
                    v[j][k] += s*v[i][k];
            }
        }

        for(std::size_t j = l; j != cols; ++j)
            v[j][i] = v[i][j] = T(0);

        v[i][i] = T(1);
        g = e[i];
    }

    /* Accumulation of left hand transformations */
    for(std::size_t l = cols; l != 0; --l) {
        const std::size_t i = l-1;

        for(std::size_t j = l; j != cols; ++j)
            m[j][i] = T(0);

        const T d = q[i];
        if(d != T(0)) { /** @todo epsilon check? */
            const T h = m[i][i]*d;
            for(std::size_t j = l; j != cols; ++j) {
                T s = T(0);
                for(std::size_t k = l; k != rows; ++k)
                    s += m[i][k]*m[j][k];
                const T f = s/h;
                for(std::size_t k = i; k != rows; ++k)
                    m[j][k] += f*m[i][k];
            }

            for(std::size_t j = i; j != rows; ++j)
                m[i][j] /= d;

        } else for(std::size_t j = i; j != rows; ++j)
            m[i][j] = T(0);

        m[i][i] += T(1);
    }

    /* Diagonalization of the bidiagonal form */
    const T epsilon = MathTypeTraits<T>::epsilon()*epsilonX;
    for(std::size_t k2 = cols; k2 != 0; --k2) {
        const std::size_t k = k2 - 1;

        for(std::size_t iteration = 0; iteration != maxIterations; ++iteration) {
            /* Test for splitting */
            bool doCancellation = true;
            std::size_t l = 0;
            for(std::size_t l2 = k2; l2 != 0; --l2) {
                l = l2 - 1;
                if(std::abs(e[l]) <= epsilon) {
                    doCancellation = false;
                    break;
                } else if(std::abs(q[l-1]) <= epsilon) {
                    break;
                }
            }

            /* Cancellation */
            if(doCancellation) {
                const std::size_t l1 = l - 1;
                T c = T(0);
                T s = T(1);
                for(std::size_t i = l; i != k+1; ++i) {
                    CORRADE_INTERNAL_ASSERT(i <= k+1);

                    const T f = s*e[i];
                    e[i] = c*e[i];
                    if(std::abs(f) <= epsilon) break;

                    const T g = q[i];
                    const T h = Implementation::pythagoras(f, g);
                    q[i] = h;
                    c = g/h;
                    s = -f/h;

                    const Vector<rows, T> a = m[l1];
                    const Vector<rows, T> b = m[i];
                    m[l1] = a*c+b*s;
                    m[i] = -a*s+b*c;
                }
            }

            /* Test for convergence */
            const T z = q[k];
            if(l == k) {
                /* Invert to non-negative */
                if(z < T(0)) {
                    q[k] = -z;
                    v[k] = -v[k];
                }

                break;

            /* Exceeded iteration count, done */
            } else if(iteration >= maxIterations-1) {
                Corrade::Utility::Error() << "Magnum::Math::Algorithms::svd(): no convergence";
                return std::make_tuple(RectangularMatrix<cols, rows, T>(), Vector<cols, T>(), Matrix<cols, T>(Matrix<cols, T>::Zero));
            }

            /* Shift from bottom 2x2 minor */
            const T y = q[k-1];
            const T h = e[k];
            const T d = e[k-1];
            T x = q[l];
            T f = ((y - z)*(y + z) + (d - h)*(d + h))/(T(2)*h*y);
            const T b = Implementation::pythagoras(f, T(1));
            if(f < T(0))
                f = ((x - z)*(x + z) + h*(y/(f - b) - h))/x;
            else
                f = ((x - z)*(x + z) + h*(y/(f + b) - h))/x;

            /* Next QR transformation */
            /** @todo isn't this extractable elsewhere? */
            T c = T(1);
            T s = T(1);
            for(std::size_t i = l+1; i != k+1; ++i) {
                CORRADE_INTERNAL_ASSERT(i <= k+1);

                const T g1 = c*e[i];
                const T h1 = s*e[i];
                const T y1 = q[i];

                const T z1 = Implementation::pythagoras(f, h1);
                e[i-1] = z1;
                c = f/z1;
                s = h1/z1;
                f = x*c + g1*s;

                const T g2 = -x*s + g1*c;
                const T h2 = y1*s;
                const T y2 = y1*c;

                const Vector<cols, T> a1 = v[i-1];
                const Vector<cols, T> b1 = v[i];
                v[i-1] = a1*c+b1*s;
                v[i] = -a1*s+b1*c;

                const T z2 = Implementation::pythagoras(f, h2);
                q[i-1] = z2;
                c = f/z2;
                s = h2/z2;
                f = c*g2 + s*y2;
                x = -s*g2 + c*y2;

                const Vector<rows, T> a2 = m[i-1];
                const Vector<rows, T> b2 = m[i];
                m[i-1] = a2*c+b2*s;
                m[i] = -a2*s+b2*c;
            }

            e[l] = T(0);
            e[k] = f;
            q[k] = x;
        }
    }

    return std::make_tuple(m, q, v);
}

}}}

#endif