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

namespace Azimuth.UnitTests.DenseLinearAlgebra
{
    public class LinearLeastSquaresTests_Solve : UnitTestSharp.TestFixture
    {
        public class FullRankSquare : UnitTestSharp.TestFixture
        {
            public void x11()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 7 },
                });

                var input = new DenseVector(new Scalar[] {
                    28,
                });

                var expected = new DenseVector(new Scalar[] {
                    4,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

            public void x22_Triangular()
            {

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

                var input = new DenseVector(new Scalar[] {
                    4,
                    2,
                });

                var expected = new DenseVector(new Scalar[] {
                    1,
                    2,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }
            
            public void x22()
            {                
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 3, 4, },
                    { 7, 10, },
                });

                var input = new DenseVector(new Scalar[] {
                    7,
                    17,
                });

                var expected = new DenseVector(new Scalar[] {
                    1,
                    1,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

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

                var input = new DenseVector(new Scalar[] {
                     1,
                     1,
                     1,
                });

                var expected = new DenseVector(new Scalar[] {
                     0,
                     0,
                     1,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

            public void x33_A()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 2,  1,  3, },
                    {-1,  0,  7, },
                    { 0, -1, -1, },
                });

                var input = new DenseVector(new Scalar[] {
                     1,
                     1,
                     1,
                });

                var expected = new DenseVector(new Scalar[] {
                       .75,
                     -1.25,
                       .25,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

            public void x33()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3, },                    
                    { 1, 2, 1, },
                    { 3, 2, 1, },
                });

                var input = new DenseVector(new Scalar[] {
                     4,
                     2,
                     0,
                });

                var expected = new DenseVector(new Scalar[] {
                    -1,
                     1,
                     1,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

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

                var input = new DenseVector(new Scalar[] {
                     1,
                     0,
                     1,
                     0,
                });

                var expected = new DenseVector(new Scalar[] {
                    1,
                    0,
                    1,
                    0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }
        }

        public class RankDeficientSquare : UnitTestSharp.TestFixture
        {
            public void x22()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, },
                    { 2, 4, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    .12,
                    .24,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

            public void x33()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3, },
                    { 3, 2, 1, },
                    { 1, 1, 1, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1.0/5.4,
                    1.0/5.4,
                    1.0/5.4,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }
        }

        public class FullRankFat : UnitTestSharp.TestFixture
        {
            public void x24()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3, 4, },
                    { 4, 3, 2, 1, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    .1,
                    .1,
                    .1,
                    .1,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

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

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    -6.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                    -6.0/28.0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }
        }

        public class FullRankTall : UnitTestSharp.TestFixture
        {
            public void x42()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 4, },
                    { 2, 3, },
                    { 3, 2, },
                    { 4, 1, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    .2,
                    .2,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

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

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1.0/7.0,
                    1.0/7.0,
                    0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }
        }

        public class RankDeficientFat : UnitTestSharp.TestFixture
        {
            public void x24()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3, 4, },
                    { 2, 4, 6, 8, },
                });

                var input = new DenseVector(new Scalar[] {
                    100,
                    100,
                });

                var expected = new DenseVector(new Scalar[] {
                    2,
                    4,
                    6,
                    8,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }

            public void x36()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3,  4,  5,  6, },
                    { 2, 4, 6,  8, 10, 12, },
                    { 3, 6, 9, 12, 15, 18, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1 * 3.0/637.0,
                    2 * 3.0/637.0,
                    3 * 3.0/637.0,
                    4 * 3.0/637.0,
                    5 * 3.0/637.0,
                    6 * 3.0/637.0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }
        }

        public class RankDeficientTall : UnitTestSharp.TestFixture
        {
            public void x42()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2 },
                    { 2, 4 },
                    { 3, 6 },
                    { 4, 8 },
                });

                var input = new DenseVector(new Scalar[] {
                    4,
                    3,
                    2,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1.0/7.5,
                    1.0/3.75,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);
                
               CheckEqual(expected, output);
            }

            public void x63()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1,  2,  3, },
                    { 2,  4,  6, },
                    { 3,  6,  9, },
                    { 4,  8, 12, },
                    { 5, 10, 15, },
                    { 6, 12, 18, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1 * 3.0/182,
                    2 * 3.0/182,
                    3 * 3.0/182,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystem(out output, input);

                CheckEqual(expected, output);
            }
        }
    }

    public class LinearLeastSquaresTests_SolveWithPreallocated : UnitTestSharp.TestFixture
    {
        public class FullRankSquare : UnitTestSharp.TestFixture
        {
            public void x11()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 7 },
                });

                var input = new DenseVector(new Scalar[] {
                    28,
                });

                var expected = new DenseVector(new Scalar[] {
                    4,
                });

                DenseVector output = new DenseVector(size: 1);

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystemToPreallocated(ref output, input);

                CheckEqual(expected, output);
            }

            public void x33()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3, },                    
                    { 1, 2, 1, },
                    { 3, 2, 1, },
                });

                var input = new DenseVector(new Scalar[] {
                     4,
                     2,
                     0,
                });

                var expected = new DenseVector(new Scalar[] {
                    -1,
                     1,
                     1,
                });

                DenseVector output = new DenseVector(size: 3);

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystemToPreallocated(ref output, input);

                CheckEqual(expected, output);
            }

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

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    -6.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                    -6.0/28.0,
                });

                DenseVector output = new DenseVector(size: 6);

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystemToPreallocated(ref output, input);

                CheckEqual(expected, output);
            }

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

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    -6.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                    -6.0/28.0,
                });

                DenseVector output = new DenseVector(size: 5);

                LinearLeastSquares lls = new LinearLeastSquares(matrix);

                CheckThrow(typeof(Exception));
                lls.SolveSystemToPreallocated(ref output, input);
            }

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

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    -6.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                     5.0/28.0,
                    -6.0/28.0,
                });

                DenseVector output = new DenseVector(size: 7);

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystemToPreallocated(ref output, input);
                CheckEqual(expected, output);
            }

            public void x63()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1,  2,  3, },
                    { 2,  4,  6, },
                    { 3,  6,  9, },
                    { 4,  8, 12, },
                    { 5, 10, 15, },
                    { 6, 12, 18, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1 * 3.0/182,
                    2 * 3.0/182,
                    3 * 3.0/182,
                });

                DenseVector output = new DenseVector(size: 6);

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                lls.SolveSystemToPreallocated(ref output, input);

                CheckEqual(expected, output);
            }

            public void x63_SizedTooSmall()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1,  2,  3, },
                    { 2,  4,  6, },
                    { 3,  6,  9, },
                    { 4,  8, 12, },
                    { 5, 10, 15, },
                    { 6, 12, 18, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1 * 3.0/182,
                    2 * 3.0/182,
                    3 * 3.0/182,
                });

                DenseVector output = new DenseVector(size: 3);

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                CheckThrow(typeof(Exception));
                lls.SolveSystemToPreallocated(ref output, input);
            }
        }
    }
    
    public class LinearLeastSquaresTests_SolveWithInequalityConstraints : UnitTestSharp.TestFixture
    {
        public class FakeIllConditioned : UnitTestSharp.TestFixture
        {
            public void x1()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 400000.0, },
                });

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

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

                var input = new DenseVector(new Scalar[] {
                    -4000000,
                });

                var expected = new DenseVector(new Scalar[] {
                    0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }
        }

        public class NoConstraintsActually : UnitTestSharp.TestFixture
        {
            public void x63()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1,  2,  3, },
                    { 2,  4,  6, },
                    { 3,  6,  9, },
                    { 4,  8, 12, },
                    { 5, 10, 15, },
                    { 6, 12, 18, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1 * 3.0/182,
                    2 * 3.0/182,
                    3 * 3.0/182,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, 
                    new RectangularMatrix(0,matrix.ColumnCount), new DenseVector(0)));

                CheckEqual(expected, output);
            }

            public void x36()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3,  4,  5,  6, },
                    { 2, 4, 6,  8, 10, 12, },
                    { 3, 6, 9, 12, 15, 18, },
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1 * 3.0/637.0,
                    2 * 3.0/637.0,
                    3 * 3.0/637.0,
                    4 * 3.0/637.0,
                    5 * 3.0/637.0,
                    6 * 3.0/637.0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, new RectangularMatrix(0, matrix.ColumnCount), new DenseVector(0)));

                CheckEqual(expected, output);
            }
        }

        
        public class RedundantConstraints : UnitTestSharp.TestFixture
        {
            public void x11()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, },
                });

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

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

                var input = new DenseVector(new Scalar[] {
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }

            public void x36()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 2, 3,  4,  5,  6, },
                    { 2, 4, 6,  8, 10, 12, },
                    { 3, 6, 9, 12, 15, 18, },
                });

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

                var h = new DenseVector(new Scalar[] {
                    -1,
                    -1,
                });

                var input = new DenseVector(new Scalar[] {
                    1,
                    1,
                    1,
                });

                var expected = new DenseVector(new Scalar[] {
                    1 * 3.0/637.0,
                    2 * 3.0/637.0,
                    3 * 3.0/637.0,
                    4 * 3.0/637.0,
                    5 * 3.0/637.0,
                    6 * 3.0/637.0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }
        }

        
        public class NonredundantConstraints : UnitTestSharp.TestFixture
        {
            public void x11()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, },
                });

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

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

                var input = new DenseVector(new Scalar[] {
                    3,
                });

                var expected = new DenseVector(new Scalar[] {
                    2,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }

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

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

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

                var input = new DenseVector(new Scalar[] {
                    3,
                    -3,
                });

                var expected = new DenseVector(new Scalar[] {
                    2,
                    0,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }

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

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

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

                var input = new DenseVector(new Scalar[] {
                    3,
                    -3,
                    3,
                });

                var expected = new DenseVector(new Scalar[] {
                    2,
                    0,
                    3,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }
        }

        
        public class BadSizes : UnitTestSharp.TestFixture
        {
            public void BadSizeForG()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 0, 0, 1, },
                    { 0, 1, 0, },
                    { 1, 0, 0, },
                });

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

                var h = new DenseVector(new Scalar[] {
                    -2,
                    -2,
                });

                var input = new DenseVector(new Scalar[] {
                    3,
                    -3,
                    3,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);

                CheckThrow(typeof(Exception));
                lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h);
            }

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

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

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

                var input = new DenseVector(new Scalar[] {
                    3,
                    -3,
                    3,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);

                CheckThrow(typeof(Exception));
                lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h);
            }

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

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

                var h = new DenseVector(new Scalar[] {
                    -2,
                    -2,
                });

                var input = new DenseVector(new Scalar[] {
                    3,
                    -3,
                });

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);

                CheckThrow(typeof(Exception));
                lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h);
            }
        }

        
        public class InconsistentConstraints : UnitTestSharp.TestFixture
        {
            public void x11()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, },
                });

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

                var h = new DenseVector(new Scalar[] {
                    2,
                    0,
                });

                var input = new DenseVector(new Scalar[] {
                    3,
                });

                DenseVector expected = new DenseVector();
                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                CheckFalse(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }

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

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

                var h = new DenseVector(new Scalar[] {
                    2,
                    0,
                });

                var input = new DenseVector(new Scalar[] {
                    3,
                    2,
                });

                DenseVector expected = new DenseVector();
                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                CheckFalse(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }

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

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

                var h = new DenseVector(new Scalar[] {
                    2,
                    0,
                });

                var input = new DenseVector(new Scalar[] {
                    3,
                    2,
                    1,
                });

                DenseVector expected = new DenseVector();
                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                CheckFalse(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }
        }

        public class NonSimpleConstraints : UnitTestSharp.TestFixture
        {
            public void x22()
            {
                var matrix = new RectangularMatrix(new Scalar[,] {
                    { 1, 1, },
                    { -1, 1, },
                });

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

                var h = new DenseVector(new Scalar[] {
                    10,
                    10,
                });

                var input = new DenseVector(new Scalar[] {
                    -100,
                    -100,
                });

                DenseVector expected = new Scalar[] {
                    0,
                    10,
                };

                DenseVector output;

                LinearLeastSquares lls = new LinearLeastSquares(matrix);
                Check(lls.SolveSystemWithInequalityConstraints(out output, input, constraints, h));

                CheckEqual(expected, output);
            }
        }

        public class LazyColumnGrabber : UnitTestSharp.TestFixture
        {
            public void Identity()
            {
                var A = new SquareMatrix(new Scalar[,] 
                {
                    { 1, 0, 0, },
                    { 0, 1, 0, },
                    { 0, 0, 1, },
                });

                var G = new SquareMatrix(new Scalar[,]
                {
                    { 1, 0, 0, },
                    { 0, 1, 0, },
                    { 0, 0, 1, },
                });

                var qr_left = new QRDecomposition(A);
                var qr_right = new QRDecomposition(qr_left.R.Transpose());

                Func<int, DenseVector> grabber = 
                    (new LinearLeastSquares.LazyColumnGrabber(G, qr_left, qr_right)).ColumnGetter;

                var expected = new SquareMatrix(new Scalar[,]
                {
                    { 1, 0, 0, },
                    { 0, 1, 0, },
                    { 0, 0, 1, },
                });

                var actual = new SquareMatrix(new DenseVector[] 
                {
                    grabber(0),
                    grabber(1),
                    grabber(2),
                });

                CheckEqual(expected, actual);
            }

            public void Basic_x33()
            {
                var A = new SquareMatrix(new Scalar[,] 
                {
                    { 1, 2, 3, },
                    { 3, 2, 1, },
                    { 1, 0, 0, },
                });

                var G = new SquareMatrix(new Scalar[,]
                {
                    { 0, 0, 1, },
                    { 1, 1, 0, },
                    { 1, 0, 1, },
                });

                var qr_left = new QRDecomposition(A);
                var qr_right = new QRDecomposition(qr_left.R.Transpose());

                var lazyColumnGrabber = new LinearLeastSquares.LazyColumnGrabber(G, qr_left, qr_right);
                Func<int, DenseVector> grabber = lazyColumnGrabber.ColumnGetter;

                var expected = new SquareMatrix(new Scalar[,]
                {
                    { 1.5, -1.5, 2.5, },
                    { -1.5, 1.625, -2.5, },
                    { 2.5, -2.5, 4.5 },
                });

                grabber(0).Scale(1.0 / lazyColumnGrabber.columnMagnitudes[0]);
                grabber(1).Scale(1.0 / lazyColumnGrabber.columnMagnitudes[1]);
                grabber(2).Scale(1.0 / lazyColumnGrabber.columnMagnitudes[2]);

                var actual = new SquareMatrix(new DenseVector[] 
                {
                    grabber(0),
                    grabber(1),
                    grabber(2),
                });

                CheckEqual(expected, actual);
            }
        }

        public class x16x16 : UnitTestSharp.TestFixture
        {
            public void GammaIs0()
            {
                var matrix = new RectangularMatrix(new Scalar[,]
                {
                    { 0 , 0 , 0 , 0 , 0 , 0 , 0 , -0.1775367111 , -0.3159118593 , -0.5115574598 , -0.892641902 , -1.7015343904 , -3.5179440975 , -7.0986309052 , -9.9009904861 , -38.4615249634 },
                    { 0 , 38.461540222 , 9.9009904861 , 4.4247789383 , 2.4937655926 , 1.597443819 , 1.109877944 , 1.0652204752 , 0.9477356672 , 1.27889359 , 1.7852838039 , 2.5523018837 , 3.5179440975 , 3.5493140221 , 0 , 0 },
                    { 0 , 0 , 0 , 0 , 0 , 0 , 0 , -0.5693323612 , -0.8926418424 , -1.7015340328 , -3.5179440975 , -7.0986280441 , -9.9009904861 , -7.0986251831 , -3.5179440975 , -3.5493147373 },
                    { -9.9009904861 , -38.4615402222 , 0 , 38.461540222 , 9.9009904861 , 4.4247775078 , 2.4937655926 , 2.2773296833 , 1.7852838039 , 2.5523011684 , 3.5179443359 , 3.5493140221 , 0 , -3.5493137836 , -3.5179440975 , -7.0986280441 },
                    { 0 , 0 , 0 , 0 , 0 , 0 , 0 , -3.5493147373 , -3.517945528 , -7.0986280441 , -9.9009904861 , -7.0986280441 , -3.5179440975 , -1.7015340328 , -0.892641902 , -0.56933254 },
                    { -2.4937655926 , -4.4247789383 , -9.9009904861 , -38.4615402222 , 0 , 38.461509705 , 9.9009904861 , 7.0986309052 , 3.5179457664 , 3.5493147373 , 0.0000009442 , -3.5493140221 , -3.5179440975 , -2.5523014069 , -1.7852838039 , -2.2773296833 },
                    { 0 , 0 , 0 , 0 , 0 , 0 , 0 , -38.4615516663 , -9.9009914398 , -7.0986309052 , -3.5179457664 , -1.7015343904 , -0.892641902 , -0.5115574002 , -0.3159118891 , -0.1775367707 },
                    { -1.109877944 , -1.5974440575 , -2.4937655926 , -4.4247789383 , -9.9009904861 , -38.461566925 , 0 , 0 , 0 , -3.5493147373 , -3.517945528 , -2.5523018837 , -1.7852838039 , -1.27889359 , -0.9477356672 , -1.0652204752 },
                    { 0.3159118593 , 0.5115574002 , 0.8926418424 , 1.7015342712 , 3.517945528 , 7.0986318588 , 9.9009914398 , 38.461540222 , 0 , -0.000007336 , -0.0000009442 , -0.0000002813 , -0.0000001189 , -0.0000000609 , -0.0000000353 , 0.1775366664 },
                    { -0.9477356672 , -1.27889359 , -1.7852838039 , -2.5523018837 , -3.5179457664 , -3.5493149757 , 0 , 0 , 0 , -38.4615516663 , -9.9009914398 , -4.4247789383 , -2.4937655926 , -1.597443819 , -1.109877944 , -1.0652204752 },
                    { 0.892641902 , 1.7015340328 , 3.5179440975 , 7.0986280441 , 9.9009904861 , 7.0986280441 , 3.5179457664 , 3.5493159294 , 0.0000009442 , 0 , 0 , 0 , 0 , 0 , 0 , 0.5693323612 },
                    { -1.7852838039 , -2.5523011684 , -3.5179443359 , -3.5493147373 , -0.0000009442 , 3.5493147373 , 3.517945528 , 7.0986299515 , 9.9009914398 , 38.461540222 , 0 , -38.4615249634 , -9.9009876251 , -4.4247770309 , -2.4937655926 , -2.2773296833 },
                    { 3.5179440975 , 7.0986280441 , 9.9009904861 , 7.0986280441 , 3.5179440975 , 1.7015340328 , 0.892641902 , 0.56933254 , 0.0000001189 , 0 , 0 , 0 , 0 , 0 , 0 , 3.5493147373 },
                    { -3.5179440975 , -3.5493140221 , 0 , 3.5493140221 , 3.5179440975 , 2.5523014069 , 1.7852838039 , 2.2773296833 , 2.4937655926 , 4.4247779846 , 9.9009876251 , 38.461540222 , 0 , -38.4615097046 , -9.9009904861 , -7.0986309052 },
                    { 9.9009904861 , 7.0986280441 , 3.5179440975 , 1.7015343904 , 0.892641902 , 0.5115574002 , 0.3159118891 , 0.1775367707 , 0.0000000353 , 0 , 0 , 0 , 0 , 0 , 0 , 38.461551666 },
                    { 0 , 3.5493140221 , 3.5179440975 , 2.5523018837 , 1.7852838039 , 1.27889359 , 0.9477356672 , 1.0652204752 , 1.109877944 , 1.5974440575 , 2.4937655926 , 4.4247789383 , 9.9009904861 , 38.461566925 , 0 , 0 }
                });

                LinearLeastSquares squares = new LinearLeastSquares(matrix.Clone());
            }
        }

        public class FailingIllConditioned : UnitTestSharp.TestFixture
        {
            public void Simple()
            {
                var M = new RectangularMatrix(new Scalar[,]
                {
                    { 1, 1, },
                    { 1, 1, },
                });

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

                var h = new DenseVector(2);

                var b = new DenseVector(new Scalar[]
                {
                    -1000,
                    -1000,
                });

                LinearLeastSquares lls = new LinearLeastSquares(M);
                DenseVector output;

                Check(lls.SolveSystemWithInequalityConstraints(out output, b, G, h));
                var expected = new DenseVector(2);

                CheckEqual(expected, output);
            }

            public void Reference()
            {
                var M = new RectangularMatrix(new Scalar[,]
                {
                    { 40, 40, },
                    { 40, 40, },
                });

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

                var h = new DenseVector(2);

                var b = new DenseVector(new Scalar[]
                {
                    -40000,
                    -40000,
                });

                LinearLeastSquares lls = new LinearLeastSquares(M);
                DenseVector output;
                
                Check(lls.SolveSystemWithInequalityConstraints(out output, b, G, h));
                var expected = new DenseVector(2);

                CheckEqual(expected, output);
            }

            public void Failing()
            {
                var M = new RectangularMatrix(new Scalar[,]
                {
                    { 40.1500003750009, 39.8499996249991, },
                    { 39.8499996249991, 40.150000375001, },
                });

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

                var h = new DenseVector(2);

                var b = new DenseVector(new Scalar[]
                {
                    -39597.9797464467,
                    -39597.9797464467,
                });

                LinearLeastSquares lls = new LinearLeastSquares(M);
                DenseVector output;
                int progress = 0;
                // Maybe iteratively drop a rank and try again?
                Check(lls.SolveSystemWithInequalityConstraints(out output, b, G, h, ref progress));
                var expected = new DenseVector(2);

                CheckEqual(expected, output);
            }
        }
    }

    public class LinearLeastSquaresTests_SolveWithEqualityConstraints : UnitTestSharp.TestFixture
    {
        public void Identity_Constrained_Positive()
        {
            var A = new RectangularMatrix(new Scalar[,]
            {
                { 1, 0, },
                { 0, 1, },
            });

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

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

            var lls = new LinearLeastSquares(A);

            var x = lls.SolveSystemWithEqualityConstraints(b, C, d);

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

            CheckEqual(expected, x);
        }

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

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

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

            var lls = new LinearLeastSquares(A);

            var x = lls.SolveSystemWithEqualityConstraints(b, C, d);

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

            CheckEqual(expected, x);
        }

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

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

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

            var lls = new LinearLeastSquares(A);

            var x = lls.SolveSystemWithEqualityConstraints(b, C, d);

            DenseVector expected = new Scalar[] { 1, 100, };

            CheckEqual(expected, x);
        }

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

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

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

            var lls = new LinearLeastSquares(A);

            var x = lls.SolveSystemWithEqualityConstraints(b, C, d);

            DenseVector expected = new Scalar[] { 0, 100, };

            CheckEqual(expected, x);
        }

        public void RankDeficientA()
        {
            var A = new RectangularMatrix(new Scalar[,]
            {
                { 2, 1, },
                { 4, 2, },
            });

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

            DenseVector b = new Scalar[] { 100, 100, };
            DenseVector d = new Scalar[] { 17, };

            var lls = new LinearLeastSquares(A);

            var x = lls.SolveSystemWithEqualityConstraints(b, C, d);

            DenseVector expected = new Scalar[] { 43, -26 };

            CheckEqual(expected, x);
        }
    }
}