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

//http://ftp.cs.wisc.edu/math-prog/matlab/lemke.m
// Take that, put it somewhere, addPath it in Octave,
// and run to verify correct answers.
//
// Note it's a weird implementation, so it can give answers
// where none exist, and has other weird problems like that.

namespace Azimuth.UnitTests.DenseLinearAlgebra
{
    public class DantzigsAlgorithmTests : UnitTestSharp.TestFixture
    {
        public class PivotTests : UnitTestSharp.TestFixture
        {
            public void Basic()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1, 0, },
                    { 0, 1, },
                });

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

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

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

                DantzigsAlgorithm.PrincipalPivot(ref M, ref q, 0);

                CheckEqual(expectedM, M);
                CheckEqual(expectedQ, q);
            }

            public void x33_01()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 11,  8,  8, },
                    {  8,  9, 10, },
                    {  8, 10, 14, },
                });

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

                var expectedM = new SquareMatrix(new Scalar[,] {
                    {  9.0/35, -8.0/35,   8.0/35, },
                    { -8.0/35, 11.0/35, -46.0/35, },
                    { -8.0/35, 46.0/35,  94.0/35, },
                });

                DantzigsAlgorithm.PrincipalPivot(ref M, ref q, 0);
                DantzigsAlgorithm.PrincipalPivot(ref M, ref q, 1, new HashSet<int>(new int[] { 0 }));
                
                CheckEqual(expectedM, M);
            }

            public void x22_00()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1, 2, },
                    { 2, 1, },
                });

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

                var expectedM = new SquareMatrix(new Scalar[,] {
                    { 1, -2, },
                    { 2, -3, },
                });

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

                DantzigsAlgorithm.PrincipalPivot(ref M, ref q, 0);

                CheckEqual(expectedM, M);
                CheckEqual(expectedQ, q);
            }

            public void maintains_PSD()
            {
                // This gave a bad answer once:
                SquareMatrix M = new SquareMatrix(new Scalar[,] {
                    { 12, 13,  4,  9, },
                    { 13, 15,  3,  9, },
                    {  4,  3,  4,  3, },
                    {  9,  9,  3, 13, },
                });

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

                var subtractant = new SquareMatrix(new Scalar[,] {
                    { 0, 0, 0, 0 },
                    { 0, 169, 52, 117 },
                    { 0, 52, 16, 36 },
                    { 0, 117, 36, 81 },
                });

                for (int i = 0; i < subtractant.Size; ++i)
                {
                    for (int j = 0; j < subtractant.Size; ++j)
                    {
                        subtractant[i,j] /= -12;
                    }
                }

                for (int i = 0; i < subtractant.Size; ++i)
                {
                    for (int j = 0; j < subtractant.Size; ++j)
                    {
                        subtractant[i,j] += M[i,j];
                    }
                }

                subtractant[0, 0] = 1.0 / 12.0;

                for (int i = 1; i < M.Size; ++i)
                {
                    subtractant[0, i] = -M[0, i] / 12;
                    subtractant[i, 0] = M[0, i] / 12;
                }

                DantzigsAlgorithm.PrincipalPivot(ref M, ref q, 0);

                // If this succeeds, the matrix remained PSD
                
                for (int i = 0; i < M.Size; ++i)
                {
                    for (int j = 0; j < M.Size; ++j)
                    {
                        CheckEqual(subtractant[i, j], M[i, j]);
                    }
                }
            }

            public void pivot_HasInverse()
            {
                var M1 = new SquareMatrix(new Scalar[,] {
                    { 1, 2, 3, },
                    { 2, 1, 7, },
                    { 3, 7, 10, },
                });

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

                var M2 = M1.Clone();

                var q2 = q1.Clone();

                DantzigsAlgorithm.PrincipalPivot(ref M1, ref q1, 0);
                DantzigsAlgorithm.PrincipalPivot(ref M1, ref q1, 1, new HashSet<int>(new int[] { 0 }));
                DantzigsAlgorithm.PrincipalPivot(ref M1, ref q1, 0, new HashSet<int>(new int[] { 0, 1 }));

                DantzigsAlgorithm.PrincipalPivot(ref M2, ref q2, 1);

                CheckEqual(M2, M1);
                CheckEqual(q2, q1);
            }

            public void pivoting_IsCommutative()
            {
                var M1 = new SquareMatrix(new Scalar[,] {
                    { 1, 2, 3, },
                    { 2, 1, 7, },
                    { 3, 7, 10, },
                });

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

                var M2 = M1.Clone();

                var q2 = q1.Clone();

                DantzigsAlgorithm.PrincipalPivot(ref M1, ref q1, 0);
                DantzigsAlgorithm.PrincipalPivot(ref M1, ref q1, 1, new HashSet<int>(new int[] { 0 }));
                DantzigsAlgorithm.PrincipalPivot(ref M1, ref q1, 2, new HashSet<int>(new int[] { 0, 1 }));

                DantzigsAlgorithm.PrincipalPivot(ref M2, ref q2, 1);
                DantzigsAlgorithm.PrincipalPivot(ref M2, ref q2, 0, new HashSet<int>(new int[] { 1 }));
                DantzigsAlgorithm.PrincipalPivot(ref M2, ref q2, 2, new HashSet<int>(new int[] { 1, 0 }));

                CheckEqual(M2, M1);
                CheckEqual(q2, q1);
            }

            public void AllTheSame()
            {
                var M1 = new SquareMatrix(new Scalar[,] {
                    {  3364, 3364, },
                    {  3364, 3364, },
                });

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

                var M2 = M1.Clone();

                var q2 = q1.Clone();

                DantzigsAlgorithm.PrincipalPivot(ref M1, ref q1, 0);
            }
        }

        public class PivotHelperClassTests : UnitTestSharp.TestFixture
        {
            public void Basic_x22_OnePivot()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1, 2, },
                    { 2, 1, },
                });

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

                SquareMatrix expected_M = M.Clone();
                DenseVector expected_q = q.Clone();

                DantzigsAlgorithm.PrincipalPivot(ref expected_M, ref expected_q, 0);

                var pivots = new DantzigsAlgorithm.Pivots(M.Size);
                pivots.AddPivot(new DantzigsAlgorithm.PivotReflector(M.ExtractColumn(0).Clone(), 0));

                var col0 = M.ExtractColumn(0).Clone();
                var col1 = M.ExtractColumn(1).Clone();

                pivots.TransformColumn(ref col0, 0);
                pivots.TransformColumn(ref col1, 1);

                var actual_M = new SquareMatrix(new DenseVector[] { col0, col1, });

                CheckEqual(expected_M, actual_M);
            }

            public void Basic_x22_TwoPivot()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1, 2, },
                    { 2, 1, },
                });

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

                SquareMatrix expected_M = M.Clone();
                DenseVector expected_q = q.Clone();

                DantzigsAlgorithm.PrincipalPivot(ref expected_M, ref expected_q, 0, new HashSet<int>(new int[] { } ));
                DantzigsAlgorithm.PrincipalPivot(ref expected_M, ref expected_q, 1, new HashSet<int>(new int[] { 0 }));

                var pivots = new DantzigsAlgorithm.Pivots(M.Size);
                {
                    pivots.AddPivot(new DantzigsAlgorithm.PivotReflector(M.ExtractColumn(0).Clone(), 0));

                    var col = M.ExtractColumn(1).Clone();
                    pivots.TransformColumn(ref col, 1);
                    pivots.AddPivot(new DantzigsAlgorithm.PivotReflector(col, 1));
                }

                var col0 = M.ExtractColumn(0).Clone();
                var col1 = M.ExtractColumn(1).Clone();

                pivots.TransformColumn(ref col0, 0);
                pivots.TransformColumn(ref col1, 1);

                var actual_M = new SquareMatrix(new DenseVector[] { col0, col1, });

                CheckEqual(expected_M, actual_M);
            }

            public void Unpivot()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 5, 2, 3, },
                    { 2, 5, 3, },
                    { 3, 3, 6, },
                });

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

                var pivots = new DantzigsAlgorithm.Pivots(M.Size);
                {
                    pivots.AddPivot(new DantzigsAlgorithm.PivotReflector(M.ExtractColumn(0).Clone(), 0));

                    var col = M.ExtractColumn(1).Clone();
                    pivots.TransformColumn(ref col, 1);
                    pivots.AddPivot(new DantzigsAlgorithm.PivotReflector(col, 1));

                    col = M.ExtractColumn(2).Clone();
                    pivots.TransformColumn(ref col, 2);
                    pivots.AddPivot(new DantzigsAlgorithm.PivotReflector(col, 2));
                }

                pivots.RemovePivot(0, M.ExtractColumn);
                {
                    SquareMatrix expected_M = M.Clone();
                    DenseVector expected_q = q.Clone();

                    DantzigsAlgorithm.PrincipalPivot(
                        ref expected_M, ref expected_q, 1, new HashSet<int>(new int[] { }));

                    DantzigsAlgorithm.PrincipalPivot(
                        ref expected_M, ref expected_q, 2, new HashSet<int>(new int[] { 1 }));

                    var col0 = M.ExtractColumn(0).Clone();
                    var col1 = M.ExtractColumn(1).Clone();
                    var col2 = M.ExtractColumn(2).Clone();

                    pivots.TransformColumn(ref col0, 0);
                    pivots.TransformColumn(ref col1, 1);
                    pivots.TransformColumn(ref col2, 2);

                    var actual_M = new SquareMatrix(new DenseVector[] { col0, col1, col2 });

                    CheckEqual(expected_M, actual_M);
                }

                pivots.RemovePivot(2, M.ExtractColumn);
                {
                    SquareMatrix expected_M = M.Clone();
                    DenseVector expected_q = q.Clone();

                    DantzigsAlgorithm.PrincipalPivot(
                        ref expected_M, ref expected_q, 1, new HashSet<int>(new int[] { }));

                    var col0 = M.ExtractColumn(0).Clone();
                    var col1 = M.ExtractColumn(1).Clone();
                    var col2 = M.ExtractColumn(2).Clone();

                    pivots.TransformColumn(ref col0, 0);
                    pivots.TransformColumn(ref col1, 1);
                    pivots.TransformColumn(ref col2, 2);

                    var actual_M = new SquareMatrix(new DenseVector[] { col0, col1, col2 });

                    CheckEqual(expected_M, actual_M);
                }                
            }
        }

        public class MRTTests : UnitTestSharp.TestFixture
        {
            public void Problem1_1()
            {
                int r = 0;

                var q = new DenseVector(new Scalar[]
                {
                    -1, -1, -1,
                });
                
                var column = new DenseVector(new Scalar[]
                {
                    20, 10, 10,
                });

                var alpha = new HashSet<int>();                

                CheckEqual(0, DantzigsAlgorithm.MinimumRatioTest(q, column, alpha, r));
            }

            public void Problem1_2()
            {
                int r = 1;

                var q = new DenseVector(new Scalar[]
                {
                    1.0/20, -.5, -.5,
                });

                var column = new DenseVector(new Scalar[]
                {
                    -.5, 49, 35,
                });

                var alpha = new HashSet<int>(new int[] { 0 });                

                CheckEqual(1, DantzigsAlgorithm.MinimumRatioTest(q, column, alpha, r));
            }

            public void Problem1_3()
            {
                int r = 2;

                var q = new DenseVector(new Scalar[]
                {
                    11.0/245, 1.0/98, -1.0/7,
                });

                var column = new DenseVector(new Scalar[]
                {
                    -1.0/7, -5.0/7, 0,
                });

                var alpha = new HashSet<int>(new int[] { 0, 1 });                

                CheckEqual(1, DantzigsAlgorithm.MinimumRatioTest(q, column, alpha, r));
            }

            public void Problem1_4()
            {
                int r = 2;

                var q = new DenseVector(new Scalar[]
                {
                    1.0/20, -.5, -.5,
                });

                var column = new DenseVector(new Scalar[]
                {
                    -1.0/2, 35, 25,
                });

                var alpha = new HashSet<int>(new int[] { 0 });

                CheckEqual(2, DantzigsAlgorithm.MinimumRatioTest(q, column, alpha, r));
            }

            public void Problem2_1()
            {
                int r = 0;

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

                var column = new DenseVector(new Scalar[]
                {
                    240, 90, 125,
                });

                var alpha = new HashSet<int>(new int[] { });                

                CheckEqual(0, DantzigsAlgorithm.MinimumRatioTest(q, column, alpha, r));
            }

            // I need to double check that this is really the right expected answer
            [UnitTestSharp.IgnoreTest]
            public void Problem2_2()
            {
                int r = 1;

                var q = new DenseVector(new Scalar[]
                {
                    1.0/240, -5.0/8, -23.0/48,
                });

                var column = new DenseVector(new Scalar[]
                {
                    -3.0/8, 165.0/4, 233.0/8
                });               

                var alpha = new HashSet<int>(new int[] { 0 });

                CheckEqual(0, DantzigsAlgorithm.MinimumRatioTest(q, column, alpha, r));
            }

            public void Problem2_3()
            {
                int r = 1;

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

                var column = new DenseVector(new Scalar[]
                {
                    90, 75, 76
                });

                var alpha = new HashSet<int>(new int[] { });

                CheckEqual(1, DantzigsAlgorithm.MinimumRatioTest(q, column, alpha, r));
            }
        }

        public class LCPTests : UnitTestSharp.TestFixture
        {
            [UnitTestSharp.IgnoreTest]
            public void TestLCP(SquareMatrix M)
            {
                var q = new DenseVector(M.Size);

                for (int i = 0; i < q.Length; ++i)
                {
                    q[i] = -1;
                }

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);

                Check(success);
                if (success)
                {
                    var w = new DenseVector(M.ColumnCount);
                    w.AssignmentPreMatrixMultiply(M, z);
                    w.Add(q);

                    Check(success);
                    for (int i = 0; i < M.Size; ++i)
                    {
                        Scalar.FuzzyEquality(0, w[i] * z[i], 1e-5f);
                        Check(w[i].Value > -1e-5);
                        Check(z[i].Value >= 0);
                    }
                }
            }

            public void x33_AllPivoted_MultiplePostPivots()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 240, 90, 125 },
                    {  90, 75,  76 },
                    { 125, 76,  86 },
                });

                TestLCP(M);
            }

            public void x33_RankDeficient()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 20, 10, 10 },
                    { 10, 54, 40 },
                    { 10, 40, 30 },
                });

                TestLCP(M);
            }

            public void Basic()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1, 0, },
                    { 0, 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);

                CheckTrue(success);

                CheckEqual(expectedZ, z);
            }
            public void IdentityMatrix_x11_Neg()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void IdentityMatrix_x11_Pos()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  1, },
                });

                var q = new DenseVector(new Scalar[] {
                    5,
                });

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void IdentityMatrix_x22_SimpleDegeneracy()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  1, 0, },
                    {  0, 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void IdentityMatrix_x22_Pos()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  1, 0, },
                    {  0, 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void IdentityMatrix_x22_Mixed()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  1, 0, },
                    {  0, 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void IdentityMatrix_x33()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  1, 0, 0, },
                    {  0, 1, 0, }, 
                    {  0, 0, 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void IdentityMatrix_x55()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  1, 0, 0, 0, 0, },
                    {  0, 1, 0, 0, 0, },
                    {  0, 0, 1, 0, 0, },
                    {  0, 0, 0, 1, 0, },
                    {  0, 0, 0, 0, 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void x22Precision()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  15689, -13473, },
                    {  -13473, 11570, },
                });

                TestLCP(M);
            }

            public void x22InfiniteLoop()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  3364, 3364, },
                    {  3364, 3364, },
                });

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

                TestLCP(M);
            }

            // M wasn't PSD
            //
            //
            //
            //public void Identity_x11_NoSolution()
            //{
            //    var M = new SquareMatrix(new Scalar[,] {
            //        { -1, },
            //    });

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

            //    DenseVector correctAnswer = null;

            //    DenseVector z;
            //    bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
            //    CheckFalse(success);

            //    CheckEqual(correctAnswer, z);
            //}

            public void Identity_x11_Solution()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void Identity_x22_Degenerate()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1, 0, },
                    { 0, 1, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            SquareMatrix M_22 = new SquareMatrix(new Scalar[,] {
                {  5, 11, },
                { 11, 25, },
            });

            public void Basic_x22_NN()
            {
                var q = new DenseVector(new Scalar[] {
                    -1,
                    -1,
                });

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M_22, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void Basic_x22_N0()
            {
                var q = new DenseVector(new Scalar[] {
                    -10,
                    0,
                });

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M_22, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void Basic_x22_NN_2()
            {
                var q = new DenseVector(new Scalar[] {
                    -10,
                    -100,
                });

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M_22, q);
                Check(success);

                CheckEqual(correctAnswer, z);
            }

            public void Basic_x22_ZeroDiag()
            {
                SquareMatrix M_22 = new SquareMatrix(new Scalar[,] {
                    {  0, 11, },
                    { 11, 25, },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M_22, q);
                CheckFalse(success);
            }

            public void Basic_x44()
            {
                // This gave a bad answer once:
                SquareMatrix M = new SquareMatrix(new Scalar[,] {
                    { 12, 13,  4,  9, },
                    { 13, 15,  3,  9, },
                    {  4,  3,  4,  3, },
                    {  9,  9,  3, 13, },
                });

                TestLCP(M);
            }

            public void Basic_x44_Notcomplementary()
            {
                // This gave a bad answer once:
                SquareMatrix M = new SquareMatrix(new Scalar[,] {
                    { 1250, 1335, 0400, 0900, },
                    { 1335, 1510, 0339, 0990, },
                    { 0400, 0339, 0420, 0350, },
                    { 0900, 0990, 0350, 1300, },
                });

                TestLCP(M);
            }

            public void Basic_x33_AllPivoted()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 150, 095, 100, },
                    { 095, 165, 110, },
                    { 100, 110, 090, },
                });

                TestLCP(M);
            }

            public void x33_AllPivoted_2StillNegative()
            {
                var M = new SquareMatrix(new Scalar[,] {
	                { 135, 114, 076, },
	                { 114, 101, 079, },
	                { 076, 079, 123, },
                });

                TestLCP(M);
            }

            public void x33_AllPivoted_2StillNegative_PivotLargestOne()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 083, 047, 043},
                    { 047, 069, 112},
                    { 043, 112, 200},
                });

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

                DantzigsAlgorithm.PrincipalPivot(ref M, ref q, 2, new HashSet<int>(new int[] { }));
                DantzigsAlgorithm.PrincipalPivot(ref M, ref q, 0, new HashSet<int>(new int[] { 2 }));

                TestLCP(M);
            }

            public void x33_AllPivoted_2StillNegative_PivotLargestOne_2()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 083, 047, 043},
                    { 047, 069, 106},
                    { 043, 106, 200},
                });

                TestLCP(M);
            }

            public void x33_AllPivot_AbortedMinorCycle()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 60, 26, 37},
                    { 26, 36, 29},
                    { 37, 29, 30},
                });

                TestLCP(M);
            }

            public void x33_CyclesAndSubcycles()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 60, 41, 54 },
                    { 41, 31, 37 },
                    { 54, 37, 52 },
                });

                TestLCP(M);
            }

            public void x33_WasFailing()
            {
                // This pushes numerical accuracy of calculating w after you get z back
                var M = new SquareMatrix(new Scalar[,] {
                    { 1839191,  837547.23478, 1073122.8473 },
                    { 0837547.23478, 689434, 0218298.21525 },
                    { 1073122.8473,  218298.21525, 0863495.00897 },
                });

                TestLCP(M);
            }

            // We'll handle precision issues later
            public void x33_Precision()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1599856, 522217, 995407 },
                    {  522217, 522260, 579742 },
                    {  995407, 579742, 844512 },
                });

                TestLCP(M);
            }

            public void x33_ShouldHaveAnAnswer()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 70, 24, 76 },
                    { 24, 18, 30 },
                    { 76, 30, 117 },
                });

                TestLCP(M);
            }

            public void x33_NoSolution()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  120, -108, 108 },
                    { -108, 104, -104 },
                    {  108,-104,  104 },
                });

                var q = new DenseVector(M.Size);

                for (int i = 0; i < q.Length; ++i)
                {
                    q[i] = -1;
                }

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);

                CheckFalse(success);
            }

            public void x33_Squared()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    {  120, -108, 108 },
                    { -108, 104, -104 },
                    {  108,-104,  104 },
                });

                M = new SquareMatrix(M.MatrixMultiply(M));

                var q = new DenseVector(M.Size);

                for (int i = 0; i < q.Length; ++i)
                {
                    q[i] = -1;
                }

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);

                CheckFalse(success);
            }

            public void x44_RankDegenerate()
            {
                var M = new SquareMatrix(new Scalar[,]
                {
                    { 126, -12, -60, -102, },
                    { -12, 75, -68, 38, },
                    { -60, -68, 104, 28, },
                    { -102, 38, 28, 125, },
                });

                // answer: pivots { 0, 1, 2 }
                TestLCP(M);
            }

            public void x44_FullRankCycle()
            {
                var M = new SquareMatrix(new Scalar[,]
                {
                    { 1, -.3, -92108, 173608, },
                    { -.3, -.00001, 0.5, -2 },
                    { -92108, -.5, 23840, -44932, },
                    { 173608, -2, -44932, -84688, },
                });

                var q = new DenseVector(M.Size);

                for (int i = 0; i < q.Length; ++i)
                {
                    q[i] = -1;
                }

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);

                CheckFalse(success);
            }

            public void x33_Fail_Rank2()
            {
                var M = new SquareMatrix(new Scalar[,]
                {
                    { 50, 19, 27 },
                    { 19, 97, 25, },
                    { 27, 25, 17 },
                });

                TestLCP(M);
            }

            public void x55_FullRankNoSolution()
            {
                var M = new SquareMatrix(new Scalar[,]
                {
                    {  158, -123,  -80,  -48,   69 },
                    { -123,  303,  128,   -3, -264 },
                    {  -80,  128,   14,   22, -112 },
                    {  -48,   -3,   22,  140,  -53 },
                    {   69, -264, -112,  -53,  289 },
                });

                var q = new DenseVector(M.Size);

                for (int i = 0; i < q.Length; ++i)
                {
                    q[i] = -1;
                }

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);

                CheckFalse(success);
            }

            [UnitTestSharp.IgnoreTest]
            private void EnsureNoAnswer(SquareMatrix M)
            {
                var q = new DenseVector(M.Size);

                for (int i = 0; i < q.Length; ++i)
                {
                    q[i] = -1;
                }

                for (int i = 0; i < (1 << M.Size); ++i)
                {
                    var pivots = new HashSet<int>();
                    var pivotsForReal = new HashSet<int>();

                    var Mclone = M.Clone();
                    var qclone = q.Clone();

                    for (int j = 0; j < M.Size; ++j)
                    {
                        bool notPivoting = (i & (1 << j)) == 0;

                        if (notPivoting)
                        {
                            continue;
                        }
                        else
                        {
                            try
                            {
                                pivotsForReal.Add(j);
                                Azimuth.DenseLinearAlgebra.DantzigsAlgorithm.PrincipalPivot(ref Mclone, ref qclone, j, pivots);
                            }
                            catch (Exception)
                            {
                                break;
                            }
                        }
                    }

                    if (qclone.All(x => x >= 0))
                    {
                        // Found an answer
                        throw new Exception("Found an answer");
                    }
                }
            }

            public void x55_NoSolution_NumericalRobustness()
            {
                var M = new SquareMatrix(new Scalar[,]
                {
                    { 128, -98,  -57,   24, -19 },
                    { -98, 258,   40,  -92,  85 },
                    { -57,  40,  230, -141, -32 },
                    {  24, -92, -141,  197, -87 },
                    { -19,  85,  -32,  -87, 120 },
                });

                var q = new DenseVector(M.Size);

                for (int i = 0; i < q.Length; ++i)
                {
                    q[i] = -1;
                }

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);

                CheckFalse(success);
            }

            public void x11_AlreadySolved_MSmall()
            {
                var M = new SquareMatrix(new Scalar[,] {
                    { 1.0/(400000.0*400000.0), },
                });

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

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

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                Check(success);
                CheckEqual(correctAnswer, z);
            }

            public void x22_NumericalUnstable()
            {
                var M = new SquareMatrix(new Scalar[,]
                {
                    {  0.707116724855459, -0.707096837377802, },
                    { -0.707096837377803,  0.707116724855458, },
                });

                var q = new DenseVector(new Scalar[] 
                {
                    -494.974746830666,
                    -494.974746830500,
                });

                DenseVector z;
                bool success = DantzigsAlgorithm.SolveLCP(out z, M, q);
                CheckFalse(success);
            }
        }
    }
}