public void HeapSortWithDecreasingDoubleArray()
        {
            var sortedIndices = new int[10];
            var values = new DenseVector(10);
            values[0] = 9;
            values[1] = 8;
            values[2] = 7;
            values[3] = 6;
            values[4] = 5;
            values[5] = 4;
            values[6] = 3;
            values[7] = 2;
            values[8] = 1;
            values[9] = 0;
            for (var i = 0; i < sortedIndices.Length; i++)
            {
                sortedIndices[i] = i;
            }

            ILUTPElementSorter.SortDoubleIndicesDecreasing(0, sortedIndices.Length - 1, sortedIndices, values);
            for (var i = 0; i < sortedIndices.Length; i++)
            {
                Assert.AreEqual(i, sortedIndices[i], "#01-" + i);
            }
        }

        public void SolveWideMatrixThrowsArgumentException()
        {
            var matrix = new SparseMatrix(2, 3);
            var input = new DenseVector(2);

            var solver = new MlkBiCgStab();
            Assert.That(() => matrix.SolveIterative(input, solver), Throws.ArgumentException);
        }

        public void SolveLongMatrixThrowsArgumentException()
        {
            var matrix = new SparseMatrix(3, 2);
            var input = new DenseVector(3);

            var solver = new GpBiCg();
            Assert.That(() => matrix.SolveIterative(input, solver), Throws.ArgumentException);
        }

 public void ApproximateWithNonInitializedPreconditionerThrowsArgumentException()
 {
     const int Size = 10;
     var vector = CreateStandardBcVector(Size);
     var preconditioner = CreatePreconditioner();
     var result = new DenseVector(vector.Count);
     Assert.Throws<ArgumentException>(() => preconditioner.Approximate(vector, result));
 }

 private float ComputeMecanumDriveMotorSpeed(DenseVector desiredMovement, VectorComponent wheelForceVectorInput)
 {
     var wheelForceVector = new DenseVector(new[] { wheelForceVectorInput.X, wheelForceVectorInput.Y });
      var wheelForceVectorUnit = wheelForceVector.Normalize(2);
      var scale = wheelForceVectorUnit.DotProduct(desiredMovement);
     //         Console.WriteLine(wheelForceVector + " " + wheelForceVectorUnit + " " + desiredMovement + " " + scale);
      return scale;
 }

        public void SolveWideMatrixThrowsArgumentException()
        {
            var matrix = new SparseMatrix(2, 3);
            var input = new DenseVector(2);

            var solver = new TFQMR();
            Assert.Throws<ArgumentException>(() => matrix.SolveIterative(input, solver));
        }

        public void SolveLongMatrixThrowsArgumentException()
        {
            var matrix = new SparseMatrix(3, 2);
            var input = new DenseVector(3);

            var solver = new MlkBiCgStab();
            Assert.Throws<ArgumentException>(() => matrix.SolveIterative(input, solver));
        }

        /// <summary>
        /// Create standard vector.
        /// </summary>
        /// <param name="size">Size of the vector.</param>
        /// <returns>New vector.</returns>
        protected DenseVector CreateStandardBcVector(int size)
        {
            var vector = new DenseVector(size);
            for (var i = 0; i < size; i++)
            {
                vector[i] = i + 1;
            }

            return vector;
        }

        /// <summary>
        /// Creates a new instance of the Vector class.
        /// </summary>
        /// <param name="data">The array to create this vector from.</param>
        /// <returns>The new <c>Vector</c>.</returns>
        protected override Vector<float> CreateVector(IList<float> data)
        {
            var vector = new DenseVector(data.Count);
            for (var index = 0; index < data.Count; index++)
            {
                vector[index] = data[index];
            }

            return vector;
        }

        public void DetermineStatus()
        {
            var criterion = new FailureStopCriterion<float>();
            Assert.IsNotNull(criterion, "There should be a criterion");

            var solution = new DenseVector(new[] { 3.0f, 2.0f, 1.0f });
            var source = new DenseVector(new[] { 1001.0f, 0.0f, 2003.0f });
            var residual = new DenseVector(new[] { 1.0f, 2.0f, 3.0f });

            var status = criterion.DetermineStatus(5, solution, source, residual);
            Assert.AreEqual(IterationStatus.Continue, status, "Should be running");
        }

        /// <summary>
        /// Check the result.
        /// </summary>
        /// <param name="preconditioner">Specific preconditioner.</param>
        /// <param name="matrix">Source matrix.</param>
        /// <param name="vector">Initial vector.</param>
        /// <param name="result">Result vector.</param>
        protected override void CheckResult(IPreconditioner<float> preconditioner, SparseMatrix matrix, Vector<float> vector, Vector<float> result)
        {
            Assert.AreEqual(typeof (DiagonalPreconditioner), preconditioner.GetType(), "#01");

            // Compute M * result = product
            // compare vector and product. Should be equal
            var product = new DenseVector(result.Count);
            matrix.Multiply(result, product);

            for (var i = 0; i < product.Count; i++)
            {
                Assert.IsTrue(((double) vector[i]).AlmostEqualNumbersBetween(product[i], -Epsilon.Magnitude()), "#02-" + i);
            }
        }

        public void ApproximateReturningOldVector()
        {
            const int Size = 10;
            var newMatrix = CreateUnitMatrix(Size);
            var vector = CreateStandardBcVector(Size);

            var preconditioner = CreatePreconditioner();
            preconditioner.Initialize(newMatrix);

            var result = new DenseVector(vector.Count);
            preconditioner.Approximate(vector, result);

            CheckResult(preconditioner, newMatrix, vector, result);
        }

        public void CanCreateDenseVectorFromArray()
        {
            var data = new float[Data.Length];
            Array.Copy(Data, data, Data.Length);
            var vector = new DenseVector(data);

            for (var i = 0; i < data.Length; i++)
            {
                Assert.AreEqual(data[i], vector[i]);
            }

            vector[0] = 100.0f;
            Assert.AreEqual(100.0f, data[0]);
        }

        public HolonomicDriveValues MecanumDrive(HolonomicDriveTrain driveTrain, float x, float y, bool inputsSquared = false)
        {
            x = InputUtilities.TransformWithWarning(x, square: inputsSquared);
             y = InputUtilities.TransformWithWarning(y, square: inputsSquared);

             var desiredMovement = new DenseVector(new [] { x, y });
            //         var desiredMovement = new DenseVector(new [] { 1f, 0f });
             HolonomicDriveValues result = new HolonomicDriveValues();
             result.FrontLeft = ComputeMecanumDriveMotorSpeed(desiredMovement, driveTrain.FrontLeft.GetComponent<VectorComponent>(DeviceComponentType.DriveWheelForceVector));
             result.FrontRight = ComputeMecanumDriveMotorSpeed(desiredMovement, driveTrain.FrontRight.GetComponent<VectorComponent>(DeviceComponentType.DriveWheelForceVector));
             result.RearLeft = ComputeMecanumDriveMotorSpeed(desiredMovement, driveTrain.RearLeft.GetComponent<VectorComponent>(DeviceComponentType.DriveWheelForceVector));
             result.RearRight = ComputeMecanumDriveMotorSpeed(desiredMovement, driveTrain.RearRight.GetComponent<VectorComponent>(DeviceComponentType.DriveWheelForceVector));
             return result;
        }

        public void CanMultiplyWithVector()
        {
            var matrix = TestMatrices["Singular3x3"];
            var x = new DenseVector(new[] { 1.0f, 2.0f, 3.0f });
            var y = matrix * x;

            Assert.AreEqual(matrix.RowCount, y.Count);

            for (var i = 0; i < matrix.RowCount; i++)
            {
                var ar = matrix.Row(i);
                var dot = ar * x;
                Assert.AreEqual(dot, y[i]);
            }
        }

        public void DetermineStatus()
        {
            var criterion = new ResidualStopCriterion<float>(1e-3f, 10);

            // the solution vector isn't actually being used so ...
            var solution = new DenseVector(new[] { float.NaN, float.NaN, float.NaN });

            // Set the source values
            var source = new DenseVector(new[] { 1.000f, 1.000f, 2.001f });

            // Set the residual values
            var residual = new DenseVector(new[] { 0.001f, 0.001f, 0.002f });

            var status = criterion.DetermineStatus(5, solution, source, residual);
            Assert.AreEqual(IterationStatus.Continue, status, "Should still be running");

            var status2 = criterion.DetermineStatus(16, solution, source, residual);
            Assert.AreEqual(IterationStatus.Converged, status2, "Should be done");
        }

 /// <summary>
 /// Outer product of this and another vector.
 /// </summary>
 /// <param name="v">The vector to operate on.</param>
 /// <returns>
 /// Matrix M[i,j] = this[i] * v[j].
 /// </returns>
 /// <seealso cref="OuterProduct(DenseVector, DenseVector)"/>
 public Matrix<float> OuterProduct(DenseVector v)
 {
     return OuterProduct(this, v);
 }

        /// <summary>
        /// Creates a vector containing specified elements.
        /// </summary>
        /// <param name="index">The first element to begin copying from.</param>
        /// <param name="length">The number of elements to copy.</param>
        /// <returns>A vector containing a copy of the specified elements.</returns>
        /// <exception cref="ArgumentOutOfRangeException"><list><item>If <paramref name="index"/> is not positive or
        /// greater than or equal to the size of the vector.</item>
        /// <item>If <paramref name="index"/> + <paramref name="length"/> is greater than or equal to the size of the vector.</item>
        /// </list></exception>
        /// <exception cref="ArgumentException">If <paramref name="length"/> is not positive.</exception>
        public override Vector<float> SubVector(int index, int length)
        {
            if (index < 0 || index >= _length)
            {
                throw new ArgumentOutOfRangeException("index");
            }

            if (length <= 0)
            {
                throw new ArgumentOutOfRangeException("length");
            }

            if (index + length > _length)
            {
                throw new ArgumentOutOfRangeException("length");
            }

            var result = new DenseVector(length);

            CommonParallel.For(
                index, 
                index + length,
                i => result._values[i - index] = _values[i]);
            return result;
        }

        public void ResetCalculationState()
        {
            var criterion = new ResidualStopCriterion<float>(1e-3f, 10);

            var solution = new DenseVector(new[] { 0.001f, 0.001f, 0.002f });
            var source = new DenseVector(new[] { 0.001f, 0.001f, 0.002f });
            var residual = new DenseVector(new[] { 1.000f, 1.000f, 2.001f });

            var status = criterion.DetermineStatus(5, solution, source, residual);
            Assert.AreEqual(IterationStatus.Continue, status, "Should be running");

            criterion.Reset();
            Assert.AreEqual(IterationStatus.Continue, criterion.Status, "Should not have started");
        }

        public void CanSolveForRandomVectorWhenResultVectorGiven(int row, int column)
        {
            var matrixA = MatrixLoader.GenerateRandomDenseMatrix(row, column);
            var matrixACopy = matrixA.Clone();
            var factorSvd = matrixA.Svd(true);
            var vectorb = MatrixLoader.GenerateRandomDenseVector(row);
            var vectorbCopy = vectorb.Clone();
            var resultx = new DenseVector(column);
            factorSvd.Solve(vectorb, resultx);

            var matrixBReconstruct = matrixA * resultx;

            // Check the reconstruction.
            for (var i = 0; i < vectorb.Count; i++)
            {
                Assert.AreEqual(vectorb[i], matrixBReconstruct[i], 1e-4);
            }

            // Make sure A didn't change.
            for (var i = 0; i < matrixA.RowCount; i++)
            {
                for (var j = 0; j < matrixA.ColumnCount; j++)
                {
                    Assert.AreEqual(matrixACopy[i, j], matrixA[i, j]);
                }
            }

            // Make sure b didn't change.
            for (var i = 0; i < vectorb.Count; i++)
            {
                Assert.AreEqual(vectorbCopy[i], vectorb[i]);
            }
        }