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

namespace Azimuth.UnitTests.DenseLinearAlgebra
{
    public class ConstrainedLeastSquaresTests : UnitTestSharp.TestFixture
    {
        public class RemovePivotTests : UnitTestSharp.TestFixture
        {
            public void Empty()
            {
                var matrix = new RectangularMatrix(0, 0);
                DenseVector diagonal = new Scalar[] { };
                DenseVector b = new Scalar[] { };
                int[] rowPermutations = new int[] { };
                List<int> columnsOfR = new List<int> { };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 0);
            }

            public void x11()
            {
                var matrix = new RectangularMatrix(1, 1);
                DenseVector diagonal = new Scalar[] { 1.0 };
                DenseVector b = new Scalar[] { 1.5 };
                int[] rowPermutations = new int[] { 0, };
                List<int> columnsOfR = new List<int> { 0, };

                var expectedRowPermutations = new int[] { 0, };
                var expectedColumnsOfR = new List<int> { };
                var expectedDiagonal = new Scalar[] { 1.0 };
                var expectedMatrix = new RectangularMatrix(1, 1);
                var expectedB = new Scalar[] { 1.5 };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 0);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void RemoveLastColumnDoesNothingElse()
            {
                var matrix = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, 2, 3, 4, }, 
                    { 5, 6, 7, 8, }, 
                    { 9, 0, 11, 12, }, 
                    { 13, 14, 15, 16, },                   
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5 };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, };

                var expectedRowPermutations = new int[] { 0, 1, 2, 3, };
                var expectedColumnsOfR = new List<int> { 0, 1, 2, };
                var expectedDiagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0 };
                var expectedMatrix = matrix.Clone();
                var expectedB = new Scalar[] { 1.5, 2.5, 3.5, 4.5, };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 3);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void AscendingDiagonalSquare()
            {
                var matrix = new RectangularMatrix(4, 4);
                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5 };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, };

                var expectedRowPermutations = new int[] { 0, 1, 3, 2, };
                var expectedColumnsOfR = new List<int> { 0, 1, 3 };
                var expectedDiagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0 };
                var expectedMatrix = new RectangularMatrix(4, 4);
                var expectedB = new Scalar[] { 1.5, 2.5, 4.5, 3.5 };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void x55()
            {
                // R = 
                // 1    2    3    4    5
                // 0    2    8    9   10
                // 0    0    3   14   15
                // 0    0    0    4   20
                // 0    0    0    0    5

                // Final matrix = (P * R * P') * G3 * G4
                // 1.000000000000000    2.000000000000000   -2.095290887308735    0.168406498460618    6.751399510385769
                // 0.000000000000000    2.000000000000000   -5.936657514041415   -0.673625993842474   14.467284665112363
                // 0.000000000000000    0.000000000000000    0.838116354923494   10.980103699632327   17.167844469266672
                // 0.000000000000000    0.000000000000000    0.000000000000000    3.452333218442680    3.616821166278091
                // 0.000000000000000    0.000000000000000   -0.000000000000000    0.000000000000000   20.736441353327720

                // G3:
                // 1 0 0 0 0
                // 0 1 0 0 0
                // 0 0 0.209529088730873 0 0.977802414077410
                // 0 0 0 1 0
                // 0 0 -0.977802414077410 0 0.209529088730873
                
                // G4:
                // 1 0 0 0 0
                // 0 1 0 0 0
                // 0 0 1 0 0
                // 0 0 0 0.690466643688536 0.723364233255618
                // 0 0 0 -0.723364233255618   0.690466643688536
                
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                var expectedRowPermutations = new int[] { 0, 1, 3, 4, 2, };
                var expectedColumnsOfR = new List<int> { 0, 1, 3, 4, };
                var expectedDiagonal = new Scalar[] { 1, 2, 3.0, 0.838116354923494, 3.452333218442680, };
                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, -2.095290887308738, 0.168406498460620 },
                    { 6, 7, 8, -5.936657514041423, -0.673625993842471 },
                    { 11, 12, 13, 14, 10.980103699632329 },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                var expectedB = new Scalar[]
                {
                    1.5000000000000000,
                    2.5000000000000000,
                    -2.4794275499820064,
                    0.0842032492303099,
                    7.5229880258584299,                  
                };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void x55_PermutationsStack()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 4, 2, 1, 0, 3, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                var expectedRowPermutations = new int[] { 4, 2, 0, 3, 1, };
                var expectedColumnsOfR = new List<int> { 0, 1, 3, 4, };
                var expectedDiagonal = new Scalar[] { 1, 2, 3.0, 0.838116354923494, 3.452333218442680, };
                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, -2.095290887308738, 0.168406498460620 },
                    { 6, 7, 8, -5.936657514041423, -0.673625993842471 },
                    { 11, 12, 13, 14, 10.980103699632329 },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                var expectedB = new Scalar[]
                {
                    1.5000000000000000,
                    2.5000000000000000,
                    -2.4794275499820064,
                    0.0842032492303099,
                    7.5229880258584299,                  
                };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void TooSmallB()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);
            }

            public void TooLargeB()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, 6, 7, 8, 9, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                var expectedRowPermutations = new int[] { 0, 1, 3, 4, 2, };
                var expectedColumnsOfR = new List<int> { 0, 1, 3, 4, };
                var expectedDiagonal = new Scalar[] { 1, 2, 3.0, 0.838116354923494, 3.452333218442680, };
                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, -2.095290887308738, 0.168406498460620 },
                    { 6, 7, 8, -5.936657514041423, -0.673625993842471 },
                    { 11, 12, 13, 14, 10.980103699632329 },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                var expectedB = new Scalar[]
                {
                    1.5000000000000000,
                    2.5000000000000000,
                    -2.4794275499820064,
                    0.0842032492303099,
                    7.5229880258584299, 
                    6,
                    7,
                    8,
                    9,
                };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void TooSmallRowPermutations()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);
            }

            public void TooLargeRowPermutations()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, -1, -1, -1, -1, -1, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                var expectedRowPermutations = new int[] { 0, 1, 3, 4, 2, -1, -1, -1, -1, -1, };
                var expectedColumnsOfR = new List<int> { 0, 1, 3, 4, };
                var expectedDiagonal = new Scalar[] { 1, 2, 3.0, 0.838116354923494, 3.452333218442680, };
                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, -2.095290887308738, 0.168406498460620 },
                    { 6, 7, 8, -5.936657514041423, -0.673625993842471 },
                    { 11, 12, 13, 14, 10.980103699632329 },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                var expectedB = new Scalar[]
                {
                    1.5000000000000000,
                    2.5000000000000000,
                    -2.4794275499820064,
                    0.0842032492303099,
                    7.5229880258584299,                  
                };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void TooSmallDiagonal()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);
            }

            public void TooLargeDiagonal()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0, -1, -1, -1, -1, -1, -1, };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                var expectedRowPermutations = new int[] { 0, 1, 3, 4, 2, };
                var expectedColumnsOfR = new List<int> { 0, 1, 3, 4, };
                var expectedDiagonal = new Scalar[] { 1, 2, 3.0, 0.838116354923494, 3.452333218442680, -1, -1, -1, -1, -1, -1, };
                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, -2.095290887308738, 0.168406498460620 },
                    { 6, 7, 8, -5.936657514041423, -0.673625993842471 },
                    { 11, 12, 13, 14, 10.980103699632329 },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                var expectedB = new Scalar[]
                {
                    1.5000000000000000,
                    2.5000000000000000,
                    -2.4794275499820064,
                    0.0842032492303099,
                    7.5229880258584299,                  
                };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void Diagonal0()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);
            }

            public void TooSmallMatrix_NumColumns()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, },
                    { 6, 7, 8, 9, },
                    { 11, 12, 13, 15, },
                    { 16, 17, 18, 20, },
                    { 21, 22, 23, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);
            }

            public void TooSmallMatrix_NumRows()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 15, 14, },
                    { 16, 17, 18, 20, 19, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);
            }

            public void NegativeRemoveIndex()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, -1);
            }

            public void TooLargeRemoveIndex()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 4, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 5);
            }

            public void EmbeddedInLargerMatrix()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, -1, 4, 5, },
                    { 6, 7, 8, -1, 9, 10, },
                    { 11, 12, 13, -1, 14, 15, },
                    { 16, 17, 18, -1, 19, 20, },
                    { 21, 22, 23, -1, 24, 25, },
                    { -1, -1, -1, -1, -1, -1, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, -1.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 4, 5, };

                var expectedRowPermutations = new int[] { 0, 1, 3, 4, 2, };
                var expectedColumnsOfR = new List<int> { 0, 1, 4, 5, };
                DenseVector expectedDiagonal = new Scalar[] { 1, 2, 3.0, -1.0, 0.838116354923494, 3.452333218442680, };
                var expectedMatrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, -1, -2.095290887308738, 0.168406498460620 },
                    { 6, 7, 8, -1, -5.936657514041423, -0.673625993842471 },
                    { 11, 12, 13, -1, 14, 10.980103699632329 },
                    { 16, 17, 18, -1, 19, 20, },
                    { 21, 22, 23, -1, 24, 25, },
                    { -1, -1, -1, -1, -1, -1, },
                });

                var expectedB = new Scalar[]
                {
                    1.5000000000000000,
                    2.5000000000000000,
                    -2.4794275499820064,
                    0.0842032492303099,
                    7.5229880258584299,
                    6.5,
                };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 2);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }

            public void ColumnsOfRDuplicate()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 0, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 1);
            }

            public void ColumnsOfRNegative()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, -1, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 1);
            }

            public void ColumnsOfR_IndexTooLarge()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 2, 3, 4, 5, },
                    { 6, 7, 8, 9, 10, },
                    { 11, 12, 13, 14, 15, },
                    { 16, 17, 18, 19, 20, },
                    { 21, 22, 23, 24, 25, },
                });

                DenseVector diagonal = new Scalar[] { 1.0, 2.0, 3.0, 4.0, 5.0 };
                DenseVector b = new Scalar[] { 1.5, 2.5, 3.5, 4.5, 5.5, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, 4, };
                List<int> columnsOfR = new List<int> { 0, 1, 2, 3, 5, };

                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 1);
            }

            [UnitTestSharp.IgnoreTest]
            public void LotsOfNegatives_x22()
            {
                // I'm not sure if this ever worked; it might show why the RemovePivot is fundamentally flawed.

                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 2, 0, -1, 0 },
                    { 0, 1, 0, -2 },
                });

                DenseVector diagonal = new Scalar[] { -1, 0, 0, 1 };
                DenseVector b = new Scalar[] { -1, 1, -1, -3, };
                int[] rowPermutations = new int[] { 0, 1, 2, 3, };
                List<int> columnsOfR = new List<int> { 0, 3, };

                var expectedRowPermutations = new int[] { 1, 0, 2, 3, };
                var expectedColumnsOfR = new List<int> { };
                DenseVector expectedDiagonal = new Scalar[] { -1, 0, 0, 1, };
                var expectedMatrix = matrix.Clone();
                var expectedB = new Scalar[] { -1, 1, -1, -3, };

                ConstrainedLeastSquares.RemovePivot(ref matrix, ref diagonal, ref b, ref rowPermutations, ref columnsOfR, 0);

                CheckEqual(expectedMatrix, matrix);
                CheckEqual(expectedDiagonal, diagonal);
                CheckEqual(expectedB, b);
                CheckEqual(expectedColumnsOfR, columnsOfR);
                CheckEqual(expectedRowPermutations, rowPermutations);
            }
        }

        public class LISTests : UnitTestSharp.TestFixture
        {
            public void Empty()
            {
                DenseVector solution = new DenseVector();
                var A = new RectangularMatrix(0, 0);
                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector b = new DenseVector();
                var GT = new RectangularMatrix(0, 0);
                var h = new DenseVector();
                int progress = 0;

                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                var expectedSolution = new DenseVector();

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_ConstraintsSatisfiedAlready()
            {
                var A = new RectangularMatrix(new Scalar[,] { { 1 } });
                DenseVector b = new Scalar[] { 1 };
                var GT = new RectangularMatrix(new Scalar[,] { { 1 } });
                DenseVector h = new Scalar[] { 0.0, };
                
                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { 1 };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_NN()
            {
                var A = new RectangularMatrix(new Scalar[,] { { 1 } });
                DenseVector b = new Scalar[] { -1 };

                var GT = new RectangularMatrix(new Scalar[,] { { 1 } });
                DenseVector h = new Scalar[] { 0 };

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { 0 };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_NN_RedundantConstraints()
            {
                var GT = new RectangularMatrix(new Scalar[,] { { 1, 2, 3, } });
                DenseVector h = new Scalar[] { 0, 0, 0, };
                
                var A = new RectangularMatrix(new Scalar[,] { { 1 } });
                DenseVector b = new Scalar[] { -1 };

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { 0 };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_NonIdentity_NP()
            {
                var A = new RectangularMatrix(new Scalar[,] { { -1 } });
                DenseVector b = new Scalar[] { 7 };

                var GT = new RectangularMatrix(new Scalar[,] { { -1, -2, -3, } });
                DenseVector h = new Scalar[] { 0, 0, 0, };

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { -7 };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_NN_ParallelConstraints()
            {
                var GT = new RectangularMatrix(new Scalar[,] { { 1, 2, 3, } });
                DenseVector h = new Scalar[] { 0, 1, 2, };
                DenseVector b = new Scalar[] { -1 };
                var A = new RectangularMatrix(new Scalar[,] { { 1 } });
                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { 2.0/3.0 };
                
                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_ConstraintsAlwaysSatisfied_NotInInitialSet()
            {
                var GT = new RectangularMatrix(new Scalar[,] { { 0, } });
                DenseVector h = new Scalar[] { 0, };
                DenseVector b = new Scalar[] { -1 };
                var A = new RectangularMatrix(new Scalar[,] { { 1 } });
                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { -1 };
                
                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_A0()
            {
                var GT = new RectangularMatrix(new Scalar[,] { { 1, } });
                DenseVector h = new Scalar[] { 1, };
                DenseVector b = new Scalar[] { -1 };
                var A = new RectangularMatrix(new Scalar[,] { { 0 } });
                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { 1 };
                
                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x11_SolutionTooSmall()
            {
                var GT = new RectangularMatrix(new Scalar[,] { { 0, } });
                DenseVector h = new Scalar[] { 0, };
                DenseVector b = new Scalar[] { -1 };
                var A = new RectangularMatrix(new Scalar[,] { { 1 } });
                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(0);

                int progress = 0;
                CheckThrow(typeof(Exception));
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);
            }

            public void x11_MultipleAdds()
            {
                var GT = new RectangularMatrix(new Scalar[,] { { 1, 2, 3, -1, } });
                DenseVector h = new Scalar[] { 1, 2, 3, -4, };
                DenseVector b = new Scalar[] { 0 };
                var A = new RectangularMatrix(new Scalar[,] { { 1 } });
                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(1);

                DenseVector expectedSolution = new Scalar[] { 1 };
                
                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

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

                DenseVector h = new Scalar[]
                {
                    0, 0,
                };

                DenseVector b = new Scalar[] 
                {
                    -1, -2,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, 0, },
                    { 0, 1, },
                });

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(2);

                DenseVector expectedSolution = new Scalar[]
                {
                    0, 0,
                };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

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

                DenseVector h = new Scalar[]
                {
                    0, 0,
                };

                DenseVector b = new Scalar[] 
                {
                    1, -2,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, 0, },
                    { 0, 1, },
                });

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(2);

                DenseVector expectedSolution = new Scalar[]
                {
                    1, 0,
                };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

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

                DenseVector h = new Scalar[]
                {
                    0, 0,
                };

                DenseVector b = new Scalar[] 
                {
                    1, 2,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, 0, },
                    { 0, 1, },
                });

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(2);

                DenseVector expectedSolution = new Scalar[]
                {
                    1, 2,
                };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x22_LeastNormSolution()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 0, -1, 0, },
                    { 0, 1, 0, -1, },

                });

                DenseVector h = new Scalar[]
                {
                    -1, 1, -1, -3, 
                };

                DenseVector b = new Scalar[] 
                {
                    3,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, 0, },
                });

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(2);

                DenseVector expectedSolution = new Scalar[]
                {
                    1, 1,
                };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x22_LeastNormSolution_MixedXY()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 0, -1, 0, },
                    { 0, 1, 0, -1, },

                });

                DenseVector h = new Scalar[]
                {
                    -1, 1, -1, -3, 
                };

                DenseVector b = new Scalar[] 
                {
                    -3,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, 1, },
                });

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                DenseVector solution = new DenseVector(2);

                DenseVector expectedSolution = new Scalar[]
                {
                    -1, 1,
                };

                int progress = 0;
                ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress);

                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }
        }

        public class MixedLSILSETests : UnitTestSharp.TestFixture
        {
            public void x21_HigherInequality()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 1, },
                });

                DenseVector h = new Scalar[]
                {
                    1,
                    2,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, },
                });

                DenseVector b = new Scalar[] 
                {
                    3,
                };

                var decomp = new CompleteOrthogonalDecomposition(ref A);
                
                DenseVector expectedSolution = new Scalar[]
                {
                    0,
                };
                DenseVector solution = new DenseVector(expectedSolution.Length);

                int progress = 0;
                bool success = ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress, 1);

                CheckFalse(success);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x21_EqualInequality()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 1, },
                });

                DenseVector h = new Scalar[]
                {
                    1,
                    1,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, },
                });

                DenseVector b = new Scalar[] 
                {
                    3,
                };

                var decomp = new CompleteOrthogonalDecomposition(ref A);

                DenseVector expectedSolution = new Scalar[]
                {
                    1,
                };
                DenseVector solution = new DenseVector(expectedSolution.Length);

                int progress = 0;
                bool success = ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress, 1);

                Check(success);
                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x21_LowerInequality()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 1, },
                });

                DenseVector h = new Scalar[]
                {
                    1,
                    0,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, },
                });

                DenseVector b = new Scalar[] 
                {
                    3,
                };

                var decomp = new CompleteOrthogonalDecomposition(ref A);

                DenseVector expectedSolution = new Scalar[]
                {
                    1,
                };
                DenseVector solution = new DenseVector(expectedSolution.Length);

                int progress = 0;
                bool success = ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress, 1);

                Check(success);
                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x21_InconsistentEqualities()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 1, },
                });

                DenseVector h = new Scalar[]
                {
                    1,
                    0,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, },
                });

                DenseVector b = new Scalar[] 
                {
                    3,
                };

                var decomp = new CompleteOrthogonalDecomposition(ref A);

                DenseVector expectedSolution = new Scalar[]
                {
                    1,
                };
                DenseVector solution = new DenseVector(expectedSolution.Length);

                int progress = 0;
                bool success = ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress, 2);

                CheckFalse(success);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x21_ConsistentEqualities()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 1, },
                });

                DenseVector h = new Scalar[]
                {
                    1,
                    1,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 1, },
                });

                DenseVector b = new Scalar[] 
                {
                    3,
                };

                var decomp = new CompleteOrthogonalDecomposition(ref A);

                DenseVector expectedSolution = new Scalar[]
                {
                    1,
                };
                DenseVector solution = new DenseVector(expectedSolution.Length);

                int progress = 0;
                bool success = ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress, 2);

                Check(success);
                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }

            public void x22_Mixed()
            {
                var GT = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 1, },
                    { 0, 2, },
                });

                DenseVector h = new Scalar[]
                {
                    -1,
                    3,
                };

                var A = new RectangularMatrix(new Scalar[,] 
                {
                    { 0, 1, },
                });

                DenseVector b = new Scalar[] 
                {
                    1.5,
                };

                var decomp = new CompleteOrthogonalDecomposition(ref A);

                DenseVector expectedSolution = new Scalar[]
                {
                    -1,
                    2,
                };
                DenseVector solution = new DenseVector(expectedSolution.Length);

                int progress = 0;
                bool success = ConstrainedLeastSquares.LSI(ref solution, ref b, ref h, ref GT, decomp, ref progress, 1);

                Check(success);
                CheckEqual(expectedSolution, solution);
                CheckEqual(GT.ColumnCount + 1, progress);
            }
        }
    }
}