// Right now, we only take positive semi-definite matrices
//#define LEMKE

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

#if LEMKE
namespace Azimuth.UnitTests.DenseLinearAlgebra
{
    public class LemkesAlgorithmTests : UnitTestSharp.TestFixture
    {
        public void ReturnIsRightSize()
        {
            var M = new SquareMatrix(new Scalar[,] {
                { 2, 1,},
                { 1, 2,},
            });

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

            var z = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(z.Length, M.ColumnCount);
        }

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

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

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

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        public void Example7_2()
        {
            // Taken from Example 7.2 in GAME PHYSICS
            // by David H Eberly ISBN 1-55860-740-4
            // Chapter 7 page 409

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

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

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

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

#if ROBUST_LEMKE
        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }
#endif

        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

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

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

            // unbounded.  z2 can take on the values of all real numbers >= 0
            DenseVector correctAnswer = null;

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        public void Example7_3()
        {
            // Taken from Example 7.3 in GAME PHYSICS
            // by David H Eberly ISBN 1-55860-740-4
            // Chapter 7 page 410

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

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

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

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        public void Example7_4()
        {
            // Taken from Example 7.4 in GAME PHYSICS
            // by David H Eberly ISBN 1-55860-740-4
            // Chapter 7 page 414

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

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

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

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

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

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

            DenseVector correctAnswer = null;

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        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
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }

        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,
            });

            var suppliedAnswer = LemkesAlgorithm.SolveLCP(M, q);

            CheckEqual(correctAnswer, suppliedAnswer);
        }
    }
}
#endif