SVD.hpp

#pragma once
#include <NDEVR/Buffer.h>
#include <NDEVR/Exception.h>
#include <NDEVR/Translator.h>
namespace NDEVR
{
    typedef Buffer<Buffer<double>> NdArray;
    class SVD
    {
    public:
        // =============================================================================
        // Description:
        explicit SVD(const NdArray& inMatrix) 
            : m_(inMatrix.size())
            , n_(inMatrix[0].size())
            , u_(inMatrix)
            , eps_(std::numeric_limits<double>::epsilon())
        {
            v_.setSize(n_);
            for (uint04 i = 0; i < n_; i++)
                v_[i].setSize(n_);
            s_.setSize(n_);
            decompose();
            reorder();
            tsh_ = 0.5 * std::sqrt(m_ + n_ + 1.) * s_[0] * eps_;
        }
 
        // =============================================================================
        // Description:
        const NdArray& u() noexcept
        {
            return u_;
        }
 
        // =============================================================================
        // Description:
        const NdArray& v() noexcept
        {
            return v_;
        }
 
        // =============================================================================
        // Description:
        const Buffer<double>& s() noexcept
        {
            return s_;
        }
 
        // =============================================================================
        // Description:
        Buffer<double> solve(const Buffer<double>& inInput, double inThresh = -1.)
        {
            double ss{};
 
            if (inInput.size() != m_)
            {
                throw Exception(_t("bad sizes."));
            }
 
            Buffer<double> returnArray;
            returnArray.setSize(n_);
 
            Buffer<double> tmp;
            tmp.setSize(n_);
 
            tsh_ = (inThresh >= 0. ? inThresh : 0.5 * std::sqrt(m_ + n_ + 1.) * s_[0] * eps_);
 
            for (uint04 j = 0; j < n_; j++)
            {
                ss = 0.;
                if (s_[j] > tsh_)
                {
                    for (uint04 i = 0; i < m_; i++)
                        ss += u_[i][j] * inInput[i];
                    ss /= s_[j];
                }
                tmp[j] = ss;
            }
 
            for (uint04 j = 0; j < n_; j++)
            {
                ss = 0.;
                for (uint04 jj = 0; jj < n_; jj++)
                {
                    ss += v_[j][jj] * tmp[jj];
                }
 
                returnArray[j] = ss;
            }
 
            return returnArray;
        }
 
    private:
        // =============================================================================
        // Description:
        static double SIGN(double inA, double inB) noexcept
        {
            return inB >= 0 ? (inA >= 0 ? inA : -inA) : (inA >= 0 ? -inA : inA);
        }
 
        // =============================================================================
        // Description:
        //
        static bool essentiallyEqual(double a, double b)
        {
            if (abs(a - b) < std::numeric_limits<double>::epsilon())
                return true;
            return false;
        }
        void decompose()
        {
            bool   flag{};
            uint04 i{};
            uint04 its{};
            uint04 j{};
            uint04 jj{};
            uint04 k{};
            uint04 l{};
            uint04 nm{};
 
            double anorm{};
            double c{};
            double f{};
            double g{};
            double h{};
            double ss{};
            double scale{};
            double x{};
            double y{};
            double z{};
 
            Buffer<double> rv1;
            rv1.setSize(n_);
 
            for (i = 0; i < n_; ++i)
            {
                l = i + 2;
                rv1[i] = scale * g;
                g = ss = scale = 0.;
 
                if (i < m_)
                {
                    for (k = i; k < m_; ++k)
                    {
                        scale += std::abs(u_[k][i]);
                    }
 
                    if (!essentiallyEqual(scale, 0.))
                    {
                        for (k = i; k < m_; ++k)
                        {
                            u_[k][i] /= scale;
                            ss += u_[k][i] * u_[k][i];
                        }
 
                        f = u_[i][i];
                        g = -SIGN(std::sqrt(ss), f);
                        h = f * g - ss;
                        u_[i][i] = f - g;
 
                        for (j = l - 1; j < n_; ++j)
                        {
                            for (ss = 0., k = i; k < m_; ++k)
                            {
                                ss += u_[k][i] * u_[k][j];
                            }
 
                            f = ss / h;
 
                            for (k = i; k < m_; ++k)
                            {
                                u_[k][j] += f * u_[k][i];
                            }
                        }
 
                        for (k = i; k < m_; ++k)
                        {
                            u_[k][i] *= scale;
                        }
                    }
                }
 
                s_[i] = scale * g;
                g = ss = scale = 0.;
 
                if (i + 1 <= m_ && i + 1 != n_)
                {
                    for (k = l - 1; k < n_; ++k)
                    {
                        scale += std::abs(u_[i][k]);
                    }
 
                    if (!essentiallyEqual(scale, 0.))
                    {
                        for (k = l - 1; k < n_; ++k)
                        {
                            u_[i][k] /= scale;
                            ss += u_[i][k] * u_[i][k];
                        }
 
                        f = u_[i][l - 1];
                        g = -SIGN(std::sqrt(ss), f);
                        h = f * g - ss;
                        u_[i][l - 1] = f - g;
 
                        for (k = l - 1; k < n_; ++k)
                        {
                            rv1[k] = u_[i][k] / h;
                        }
 
                        for (j = l - 1; j < m_; ++j)
                        {
                            for (ss = 0., k = l - 1; k < n_; ++k)
                            {
                                ss += u_[j][k] * u_[i][k];
                            }
 
                            for (k = l - 1; k < n_; ++k)
                            {
                                u_[j][k] += ss * rv1[k];
                            }
                        }
 
                        for (k = l - 1; k < n_; ++k)
                        {
                            u_[i][k] *= scale;
                        }
                    }
                }
 
                anorm = std::max(anorm, (std::abs(s_[i]) + std::abs(rv1[i])));
            }
 
            for (i = n_ - 1; i != static_cast<uint04>(-1); --i)
            {
                if (i < n_ - 1)
                {
                    if (!essentiallyEqual(g, 0.))
                    {
                        for (j = l; j < n_; ++j)
                        {
                            v_[j][i] = (u_[i][j] / u_[i][l]) / g;
                        }
 
                        for (j = l; j < n_; ++j)
                        {
                            for (ss = 0., k = l; k < n_; ++k)
                            {
                                ss += u_[i][k] * v_[k][j];
                            }
 
                            for (k = l; k < n_; ++k)
                            {
                                v_[k][j] += ss * v_[k][i];
                            }
                        }
                    }
 
                    for (j = l; j < n_; ++j)
                    {
                        v_[i][j] = v_[j][i] = 0.;
                    }
                }
 
                v_[i][i] = 1.;
                g = rv1[i];
                l = i;
            }
 
            for (i = std::min(m_, n_) - 1; i != static_cast<uint04>(-1); --i)
            {
                l = i + 1;
                g = s_[i];
 
                for (j = l; j < n_; ++j)
                {
                    u_[i][j] = 0.;
                }
 
                if (!essentiallyEqual(g, 0.))
                {
                    g = 1. / g;
 
                    for (j = l; j < n_; ++j)
                    {
                        for (ss = 0., k = l; k < m_; ++k)
                        {
                            ss += u_[k][i] * u_[k][j];
                        }
 
                        f = (ss / u_[i][i]) * g;
 
                        for (k = i; k < m_; ++k)
                        {
                            u_[k][j] += f * u_[k][i];
                        }
                    }
 
                    for (j = i; j < m_; ++j)
                    {
                        u_[j][i] *= g;
                    }
                }
                else
                {
                    for (j = i; j < m_; ++j)
                    {
                        u_[j][i] = 0.;
                    }
                }
 
                ++u_[i][i];
            }
 
            for (k = n_ - 1; k != static_cast<uint04>(-1); --k)
            {
                for (its = 0; its < 30; ++its)
                {
                    flag = true;
                    for (l = k; l != static_cast<uint04>(-1); --l)
                    {
                        nm = l - 1;
                        if (l == 0 || std::abs(rv1[l]) <= eps_ * anorm)
                        {
                            flag = false;
                            break;
                        }
 
                        if (std::abs(s_[nm]) <= eps_ * anorm)
                        {
                            break;
                        }
                    }
 
                    if (flag)
                    {
                        c = 0.;
                        ss = 1.;
                        for (i = l; i < k + 1; ++i)
                        {
                            f = ss * rv1[i];
                            rv1[i] = c * rv1[i];
 
                            if (std::abs(f) <= eps_ * anorm)
                            {
                                break;
                            }
 
                            g = s_[i];
                            h = pythag(f, g);
                            s_[i] = h;
                            h = 1. / h;
                            c = g * h;
                            ss = -f * h;
 
                            for (j = 0; j < m_; ++j)
                            {
                                y = u_[j][nm];
                                z = u_[j][i];
                                u_[j][nm] = y * c + z * ss;
                                u_[j][i] = z * c - y * ss;
                            }
                        }
                    }
 
                    z = s_[k];
                    if (l == k)
                    {
                        if (z < 0.)
                        {
                            s_[k] = -z;
                            for (j = 0; j < n_; ++j)
                            {
                                v_[j][k] = -v_[j][k];
                            }
                        }
                        break;
                    }
 
                    if (its == 29)
                    {
                        throw Exception(_t("no convergence in 30 svdcmp iterations"));
                    }
 
                    x = s_[l];
                    nm = k - 1;
                    y = s_[nm];
                    g = rv1[nm];
                    h = rv1[k];
                    f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2. * h * y);
                    g = pythag(f, 1.);
                    f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
                    c = ss = 1.;
 
                    for (j = l; j <= nm; j++)
                    {
                        i = j + 1;
                        g = rv1[i];
                        y = s_[i];
                        h = ss * g;
                        g = c * g;
                        z = pythag(f, h);
                        rv1[j] = z;
                        c = f / z;
                        ss = h / z;
                        f = x * c + g * ss;
                        g = g * c - x * ss;
                        h = y * ss;
                        y *= c;
 
                        for (jj = 0; jj < n_; ++jj)
                        {
                            x = v_[jj][j];
                            z = v_[jj][i];
                            v_[jj][j] = x * c + z * ss;
                            v_[jj][i] = z * c - x * ss;
                        }
 
                        z = pythag(f, h);
                        s_[j] = z;
 
                        if (!essentiallyEqual(z, 0.))
                        {
                            z = 1. / z;
                            c = f * z;
                            ss = h * z;
                        }
 
                        f = c * g + ss * y;
                        x = c * y - ss * g;
 
                        for (jj = 0; jj < m_; ++jj)
                        {
                            y = u_[jj][j];
                            z = u_[jj][i];
                            u_[jj][j] = y * c + z * ss;
                            u_[jj][i] = z * c - y * ss;
                        }
                    }
                    rv1[l] = 0.;
                    rv1[k] = f;
                    s_[k] = x;
                }
            }
        }
 
        // =============================================================================
        // Description:
        void reorder()
        {
            uint04 i = 0;
            uint04 j = 0;
            uint04 k = 0;
            uint04 ss = 0;
            uint04 inc = 1;
 
            double          sw{};
            Buffer<double> su;
            su.setSize(m_);
            Buffer<double> sv;
            sv.setSize(n_);
 
            do
            {
                inc *= 3;
                ++inc;
            } while (inc <= n_);
 
            do
            {
                inc /= 3;
                for (i = inc; i < n_; ++i)
                {
                    sw = s_[i];
                    for (k = 0; k < m_; ++k)
                    {
                        su[k] = u_[k][i];
                    }
 
                    for (k = 0; k < n_; ++k)
                    {
                        sv[k] = v_[k][i];
                    }
 
                    j = i;
                    while (s_[j - inc] < sw)
                    {
                        s_[j] = s_[j - inc];
 
                        for (k = 0; k < m_; ++k)
                        {
                            u_[k][j] = u_[k][j - inc];
                        }
 
                        for (k = 0; k < n_; ++k)
                        {
                            v_[k][j] = v_[k][j - inc];
                        }
 
                        j -= inc;
 
                        if (j < inc)
                        {
                            break;
                        }
                    }
 
                    s_[j] = sw;
 
                    for (k = 0; k < m_; ++k)
                    {
                        u_[k][j] = su[k];
                    }
 
                    for (k = 0; k < n_; ++k)
                    {
                        v_[k][j] = sv[k];
                    }
                }
            } while (inc > 1);
 
            for (k = 0; k < n_; ++k)
            {
                ss = 0;
 
                for (i = 0; i < m_; i++)
                {
                    if (u_[i][k] < 0.)
                    {
                        ss++;
                    }
                }
 
                for (j = 0; j < n_; ++j)
                {
                    if (v_[j][k] < 0.)
                    {
                        ss++;
                    }
                }
 
                if (ss > (m_ + n_) / 2)
                {
                    for (i = 0; i < m_; ++i)
                    {
                        u_[i][k] = -u_[i][k];
                    }
 
                    for (j = 0; j < n_; ++j)
                    {
                        v_[j][k] = -v_[j][k];
                    }
                }
            }
        }
 
        // =============================================================================
        // Description:
        static double pythag(double inA, double inB) noexcept
        {
            const double absa = std::abs(inA);
            const double absb = std::abs(inB);
            return (absa > absb
                ? absa * std::sqrt(1. + (absb / absa) * (absb / absa))
                : (essentiallyEqual(absb, 0.) ? 0. : absb * std::sqrt(1. + (absb / absa) * (absb / absa))));
        }
        static Buffer<double> lstsq(const NdArray& inA, const Buffer<double>& inB, double inTolerance = 1e-12)
        {
            SVD svdSolver(inA);
            const double threshold = inTolerance * svdSolver.s()[0];
 
            return svdSolver.solve(inB, threshold);
        }
    private:
        // ===============================Attributes====================================
        const uint04    m_{};
        const uint04    n_{};
        NdArray u_{};
        NdArray v_{};
        Buffer<double> s_;
        double eps_{};
        double tsh_{};
    };
 
    
}