using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Azimuth.DenseLinearAlgebra;

namespace Azimuth.UnitTests.DenseLinearAlgebra
{
    public class RQDecompositionTests : UnitTestSharp.TestFixture
    {
        public class DecompositionTests : UnitTestSharp.TestFixture
        {
            public void EmptyMatrix()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {

                });

                int rank = 0;

                var expectedMatrix = matrix.Clone();

                var pivots = new int[] { };
                var oldDiagonal = new DenseVector(new Scalar[] { });
                var newDiagonal = new DenseVector(new Scalar[] { });

                int progress = 0;

                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);

                CheckEqual(new DenseVector(new Scalar[] { }), newDiagonal);
                CheckEqual(new DenseVector(new Scalar[] { }), oldDiagonal);
                CheckEqual(rank, progress);
                CheckEqual(expectedMatrix, matrix);
            }

            public void Identity()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 0, 0, },
                    { 0, 1, 0, },
                    { 0, 0, 1, },
                });

                int rank = matrix.ColumnCount;

                var expectedMatrix = matrix.Clone();

                var pivots = new int[] { 0, 1, 2, };
                var oldDiagonal = new DenseVector(new Scalar[] { 1, 1, 1, });
                var newDiagonal = new DenseVector(new Scalar[] { 1, 1, 1, });

                int progress = 0;

                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);

                CheckEqual(new DenseVector(new Scalar[] { 1, 1, 1, }), newDiagonal);
                CheckEqual(new DenseVector(new Scalar[] { 1, 1, 1, }), oldDiagonal);
                CheckEqual(rank, progress);
                CheckEqual(expectedMatrix, matrix);
            }

            public void ZeroMatrix()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 0, 0, 0, },
                    { 0, 0, 0, },
                    { 0, 0, 0, },
                });

                int rank = 0;

                var expectedMatrix = matrix.Clone();

                var pivots = new int[] { 0, 1, 2, };
                var oldDiagonal = new DenseVector(new Scalar[] { 0, 0, 0, });
                var newDiagonal = new DenseVector(new Scalar[] { 0, 0, 0, });

                var expectedOldDiagonal = oldDiagonal.Clone();
                var expectedNewDiagonal = newDiagonal.Clone();

                int progress = 0;

                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);

                CheckEqual(expectedNewDiagonal, newDiagonal);
                CheckEqual(expectedOldDiagonal, oldDiagonal);
                CheckEqual(rank, progress);
                CheckEqual(expectedMatrix, matrix);
            }

            public void Fat_All1s()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 8, 1, 1, 1, },
                    { 8, 8, 1, 1, },
                    { 8, 8, 8, 1, },
                    { 8, 8, 8, 8, },
                });

                int rank = 3;

                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 8, -1, -Math.Sqrt(2), 0, },
                    { 8, 8, -Math.Sqrt(2), 0, },
                    { 8, 8, 8, 0, },
                    { 8, 8, 8, 1, },
                });

                var pivots = new int[] { 0, 1, 2, 3, };
                var oldDiagonal = new DenseVector(new Scalar[] { 1, 1, 1, });
                var newDiagonal = new DenseVector(new Scalar[] { -1, -1, -Math.Sqrt(2), });

                var expectedOldDiagonal = oldDiagonal.Clone();
                var expectedNewDiagonal = newDiagonal.Clone();

                var expectedK = new RectangularMatrix(new Scalar[,]
                {
                    { -1, 0, 0, 0, },
                    { 0, -1, 0, 0, },
                    { 0, 0, -Math.Sqrt(2)/2, -Math.Sqrt(2)/2, },
                });

                var expectedR = new RectangularMatrix(new Scalar[,] 
                {
                    { -1, -1, -Math.Sqrt(2), },
                    { 0, -1, -Math.Sqrt(2), },
                    { 0, 0, -Math.Sqrt(2), },
                });

                int progress = 0;

                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);

                var K = new RectangularMatrix(rank, matrix.ColumnCount);

                RQDecomposition.ComputeKExplicitly(ref K, matrix, oldDiagonal, newDiagonal, pivots, rank);

                var R = new QRDecomposition.UpperTriangularMatrix(matrix, newDiagonal, pivots.Take(3).ToList(), rank);

                CheckEqual(expectedNewDiagonal, newDiagonal);
                CheckEqual(expectedOldDiagonal, oldDiagonal);
                CheckEqual(rank, progress);
                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedK, K);
                CheckEqual(expectedR, R);
            }

            public void Fat_All2s()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 8, 2, 2, 2, },
                    { 8, 8, 2, 2, },
                    { 8, 8, 8, 2, },
                    { 8, 8, 8, 8, },
                });

                int rank = 3;

                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 8, -2, -2 * Math.Sqrt(2), 0, },
                    { 8, 8, -2 * Math.Sqrt(2), 0, },
                    { 8, 8, 8, 0, },
                    { 8, 8, 8, 2, },
                });

                var pivots = new int[] { 0, 1, 2, 3, };
                var oldDiagonal = new DenseVector(new Scalar[] { 2, 2, 2, });
                var newDiagonal = new DenseVector(new Scalar[] { -2, -2, -2 * Math.Sqrt(2), });

                var expectedOldDiagonal = oldDiagonal.Clone();
                var expectedNewDiagonal = newDiagonal.Clone();

                var expectedK = new RectangularMatrix(new Scalar[,]
                {
                    { -1, 0, 0, 0, },
                    { 0, -1, 0, 0, },
                    { 0, 0, -Math.Sqrt(2)/2, -Math.Sqrt(2)/2, },
                });

                var expectedR = new RectangularMatrix(new Scalar[,] 
                {
                    { -2, -2, -2 * Math.Sqrt(2), },
                    { 0, -2, -2 * Math.Sqrt(2), },
                    { 0, 0, -2 * Math.Sqrt(2), },
                });

                int progress = 0;

                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);

                var K = new RectangularMatrix(rank, matrix.ColumnCount);

                RQDecomposition.ComputeKExplicitly(ref K, matrix, oldDiagonal, newDiagonal, pivots, rank);

                var R = new QRDecomposition.UpperTriangularMatrix(matrix, newDiagonal, pivots.Take(3).ToList(), rank);

                CheckEqual(expectedNewDiagonal, newDiagonal);
                CheckEqual(expectedOldDiagonal, oldDiagonal);
                CheckEqual(rank, progress);
                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedK, K);
                CheckEqual(expectedR, R);
            }

            public void Fat_ArbitraryMatrix()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 8, 2, 3, 4, 5, 6, 7, },
                    { 8, 8, 6, 2, 1, 0, 8, },
                    { 8, 8, 8, 1, 1, 2, 7, },
                    { 8, 8, 8, 8, 0, 0, 0, },
                    { 8, 8, 8, 8, 0, 0, 0, },
                    { 8, 8, 8, 8, 0, 0, 0, },
                    { 8, 8, 8, 8, 0, 0, 0, },
                });

                int rank = 3;

                var pivots = new int[] { 0, 1, 2, 3, 4, 5, 6, };
                var oldDiagonal = new DenseVector(new Scalar[] { 2, 2, 2, });
                var newDiagonal = new DenseVector(new Scalar[] { -6.7151177023, -4.8537946015, -Math.Sqrt(59), });

                var expectedOldDiagonal = oldDiagonal.Clone();
                var expectedNewDiagonal = newDiagonal.Clone();

                var expectedK = new RectangularMatrix(new Scalar[,]
                {
                    { -0.2978354347, -0.3036207273, -0.0734946432, -0.4061489687, -0.5521740398, -0.5028919175, 0.3015851614 },
                    { 0, -0.4120487503, -0.7402909752, -0.1641211124, 0.0419032627, 0.4958552758, 0.0872984641 },
                    { 0, 0, -0.260377822, -0.130188911, -0.130188911, -0.260377822, -0.9113223769 },
                });

                var expectedR = new RectangularMatrix(new Scalar[,] 
                {
                    { -6.7151177023, 0.0942823412, -9.8943572345 },
                    { 0, -4.8537946015, -9.2434126796 },
                    { 0, 0, -7.6811457479 },
                });

                int progress = 0;

                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);

                var K = new RectangularMatrix(rank, matrix.ColumnCount);

                RQDecomposition.ComputeKExplicitly(ref K, matrix, oldDiagonal, newDiagonal, pivots, rank);

                var R = new QRDecomposition.UpperTriangularMatrix(matrix, newDiagonal, pivots.Take(rank).ToList(), rank);

                CheckEqual(expectedNewDiagonal, newDiagonal);
                CheckEqual(expectedOldDiagonal, oldDiagonal);
                CheckEqual(rank, progress);
                CheckEqual(expectedK, K);
                CheckEqual(expectedR, R);
            }

            public void Permutations()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 5, 3, 8, 2, 4, 6, 7, },
                    { 1, 6, 8, 8, 2, 0, 8, },
                    { 1, 8, 8, 8, 1, 2, 7, },
                    { 0, 8, 8, 8, 8, 0, 0, },
                    { 0, 8, 8, 8, 8, 0, 0, },
                    { 0, 8, 8, 8, 8, 0, 0, },
                    { 0, 8, 8, 8, 8, 0, 0, },
                });

                int rank = 3;

                var pivots = new int[] { 2, 3, 1, 4, 0, 5, 6, };
                var oldDiagonal = new DenseVector(new Scalar[] { 2, 2, 2, });
                var newDiagonal = new DenseVector(new Scalar[] { -6.7151177023, -4.8537946015, -Math.Sqrt(59), });

                var expectedOldDiagonal = oldDiagonal.Clone();
                var expectedNewDiagonal = newDiagonal.Clone();

                var expectedK = new RectangularMatrix(new Scalar[,]
                {
                    { -0.5521740398, -0.0734946432, -0.2978354347, -0.3036207273, -0.4061489687, -0.5028919175, 0.3015851614 },
                    { 0.0419032627, -0.7402909752, 0, -0.4120487503, -0.1641211124, 0.4958552758, 0.0872984641 },
                    { -0.130188911, -0.260377822, 0, 0, -0.130188911, -0.260377822, -0.9113223769 },
                });

                var expectedR = new RectangularMatrix(new Scalar[,] 
                {
                    { -6.7151177023, 0.0942823412, -9.8943572345 },
                    { 0, -4.8537946015, -9.2434126796 },
                    { 0, 0, -7.6811457479 },
                });

                int progress = 0;

                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);

                var K = new RectangularMatrix(rank, matrix.ColumnCount);

                RQDecomposition.ComputeKExplicitly(ref K, matrix, oldDiagonal, newDiagonal, pivots, rank);

                var R = new QRDecomposition.UpperTriangularMatrix(matrix, newDiagonal, pivots.Take(rank).ToList(), rank);

                CheckEqual(expectedNewDiagonal, newDiagonal);
                CheckEqual(expectedOldDiagonal, oldDiagonal);
                CheckEqual(rank, progress);
                CheckEqual(expectedK, K);
                CheckEqual(expectedR, R);
            }

            public void BadlySizedMatrix()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 8, 1, 1, 1, },
                    { 8, 8, 1, 1, },
                    { 8, 8, 8, 1, },
                });

                int rank = 3;

                var pivots = new int[] { 0, 1, 2, 3, };
                var oldDiagonal = new DenseVector(new Scalar[] { 1, 1, 1, });
                var newDiagonal = new DenseVector(new Scalar[] { -1, -1, -Math.Sqrt(2), });

                int progress = 0;

                CheckThrow(typeof(Exception));
                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);
            }

            public void OldDiagonal_0()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 8, 1, 1, 1, },
                    { 8, 8, 1, 1, },
                    { 8, 8, 8, 1, },
                    { 8, 8, 8, 8, },
                });

                int rank = 3;

                var pivots = new int[] { 0, 1, 2, 3, };
                var oldDiagonal = new DenseVector(new Scalar[] { 1, Scalar.FuzzyEqualityEpsilon / 2, 1, });
                var newDiagonal = new DenseVector(new Scalar[] { -1, -1, -Math.Sqrt(2), });

                int progress = 0;

                CheckThrow(typeof(Exception));
                RQDecomposition.DecomposeUpperTrapezoidal(ref matrix, ref newDiagonal, pivots, oldDiagonal, rank, ref progress);
            }
        }

        // Most other things are tested by the tests in CompleteOrthogonalDecomposition, and I'm feeling lazy :D
    }
}