using System;

namespace Science.Mathematics.LinearAlgebra
{
    public class FactorizationAeqQR
    {
        private Matrix Q;
        private Matrix R;
        private double[,] a;
        public FactorizationAeqQR(Matrix A)
        {
            a = new double[A.SizeOfRow, A.SizeOfColumn];
            for (int i = 0; i < a.GetLength(0); i++)
                for (int j = 0; j < a.GetLength(1); j++)
                    a[i, j] = A[i, j];
            Q = new Matrix(A.SizeOfRow,A.SizeOfColumn);
            R = new Matrix(A.SizeOfRow,A.SizeOfColumn);
        }
        public void Compute()
        {
            QRdcmp obj = new QRdcmp(a);

            for (int i = 0; i < a.GetLength(0); i++)
            {
                for (int j = 0; j < a.GetLength(1); j++)
                {
                    Q[i, j] = obj.QT[j, i];
                    R[i, j] = obj.R[i, j];
                }
            }   
        }
        public Matrix Orthonormal
        {
            get { return Q; }
        }
        public Matrix UpperTriangular
        {
            get { return R; }
        }
    }

    class QRdcmp
    {
        private double[,] qt;
        public double[,] QT
        {
            get { return qt; }  // output
        }
        private double[,] r;
        public double[,] R
        {
            get { return r; }  // output
        }
        private bool sing;
        public bool Singularity
        {
            get { return sing; }
        }
        private int n;
        public QRdcmp()
        {
        }
        ~QRdcmp()
        {
        }
        public QRdcmp(double[,] a)
        {
            n = a.GetLength(0);
            qt = new double[n, n];
            r = new double[n, n];
            int i, j, k;
            for (i = 0; i < n; i++) for (j = 0; j < n; j++) r[i, j] = a[i, j];
            sing = false;
            double[] c = new double[n];
            double[] d = new double[n];
            double scale, sigma, sum, tau;
            for (k = 0; k < n - 1; k++)
            {
                scale = 0.0;
                for (i = k; i < n; i++) scale = Math.Max(scale, Math.Abs(r[i, k]));
                if (scale == 0.0)
                {
                    sing = true;
                    c[k] = d[k] = 0.0;
                }
                else
                {
                    for (i = k; i < n; i++) r[i, k] /= scale;
                    for (sum = 0.0, i = k; i < n; i++) sum += r[i, k] * r[i, k];
                    if (r[k, k] == 0.0) sigma = Math.Sqrt(sum);
                    else sigma = Math.Sign(r[k, k]) * Math.Sqrt(sum);
                    r[k, k] += sigma;
                    c[k] = sigma * r[k, k];
                    d[k] = -scale * sigma;
                    for (j = k + 1; j < n; j++)
                    {
                        for (sum = 0.0, i = k; i < n; i++) sum += r[i, k] * r[i, j];
                        tau = sum / c[k];
                        for (i = k; i < n; i++) r[i, j] -= tau * r[i, k];
                    }
                }
            }
            d[n - 1] = r[n - 1, n - 1];
            if (d[n - 1] == 0.0) sing = true;
            for (i = 0; i < n; i++)
            {
                for (j = 0; j < n; j++) qt[i, j] = 0.0;
                qt[i, i] = 1.0;
            }
            for (k = 0; k < n - 1; k++)
            {
                if (c[k] != 0.0)
                {
                    for (j = 0; j < n; j++)
                    {
                        sum = 0.0;
                        for (i = k; i < n; i++) sum += r[i, k] * qt[i, j];
                        sum /= c[k];
                        for (i = k; i < n; i++) qt[i, j] -= sum * r[i, k];
                    }
                }
            }
            for (i = 0; i < n; i++)
            {
                r[i, i] = d[i];
                for (j = 0; j < i; j++) r[i, j] = 0.0;
            }
        }

        public void solve(double[] b, double[] x)
        {
            qtmult(b, x);
            rsolve(x, x);
        }

        public void qtmult(double[] b, double[] x)
        {
            int i, j;
            double sum;
            for (i = 0; i < n; i++)
            {
                sum = 0.0;
                for (j = 0; j < n; j++) sum += qt[i, j] * b[j];
                x[i] = sum;
            }
        }

        public void rsolve(double[] b, double[] x)
        {
            int i, j;
            double sum;
            if (sing) try { throw new Science.Error(); }
                catch (Science.Error e)
                {
                    e.Write("attempting solve in a singular QR");
                }
            for (i = n - 1; i >= 0; i--)
            {
                sum = b[i];
                for (j = i + 1; j < n; j++) sum -= r[i, j] * x[j];
                x[i] = sum / r[i, i];
            }
        }

        public void update(double[] u, double[] v)
        {
            int i, k;
            double[] w = new double[u.Length];
            for (i = 0; i < u.Length; i++) w[i] = u[i];

            for (k = n - 1; k >= 0; k--)
            {
                if (w[k] != 0.0) break;
            }
            if (k < 0) k = 0;
            for (i = k - 1; i >= 0; i--)
            {
                rotate(i, w[i], -w[i + 1]);
                if (w[i] == 0.0) w[i] = Math.Abs(w[i + 1]);
                else if (Math.Abs(w[i]) > Math.Abs(w[i + 1]))
                    w[i] = Math.Abs(w[i]) * Math.Sqrt(1.0 + (w[i + 1] / w[i]) * (w[i + 1] / w[i]));
                else w[i] = Math.Abs(w[i + 1]) * Math.Sqrt(1.0 + (w[i] / w[i + 1]) * (w[i] / w[i + 1]));
            }
            for (i = 0; i < n; i++) r[0, i] += w[0] * v[i];
            for (i = 0; i < k; i++)
                rotate(i, r[i, i], -r[i + 1, i]);
            for (i = 0; i < n; i++)
                if (r[i, i] == 0.0) sing = true;
        }

        public void rotate(int i, double a, double b)
        {
            int j;
            double c, fact, s, w, y;

            if (a == 0.0)
            {
                c = 0.0;
                s = (b >= 0.0 ? 1.0 : -1.0);
            }
            else if (Math.Abs(a) > Math.Abs(b))
            {
                fact = b / a;
                c = Math.Sign(a) * 1.0 / Math.Sqrt(1.0 + (fact * fact));
                s = fact * c;
            }
            else
            {
                fact = a / b;
                s = Math.Sign(b) * 1.0 / Math.Sqrt(1.0 + (fact * fact));
                c = fact * s;
            }
            for (j = i; j < n; j++)
            {
                y = r[i, j];
                w = r[i + 1, j];
                r[i, j] = c * y - s * w;
                r[i + 1, j] = s * y + c * w;
            }
            for (j = 0; j < n; j++)
            {
                y = qt[i, j];
                w = qt[i + 1, j];
                qt[i, j] = c * y - s * w;
                qt[i + 1, j] = s * y + c * w;
            }
        }
    }
}