// In perspective mode, the warping functions are:
		//	x' = (a0 + a1 x + a2 y) / (c0 x + c1 y + 1)
		//	y' = (b0 + b1 x + b2 y) / (c0 x + c1 y + 1)
		//
		// The following calculates the factors a#, b# and c#.
		// We do this by creating a set of eight equations with a#, b# and c# as unknowns.
		// The equations are derived by:
		// 1. substituting the srcPoints for (x, y);
		// 2. substituting the corresponding destPoints for (x', y');
		// 3. solving the resulting set of equations, with the factors as unknowns.
		//
		// The equations are like these:
		//	a0	x a1	y a2	0		0		0		-xx'c0	-yx'c1	= x'
		//	0	0		0		b0		x b1	y b2	-xy'c0	-yy'c1  = y'
		// The known factors of left hand side ar put in the 8x8 matrix mxLeft for
		// all four point pairs, and the right hand side in the one column matrix mxRight.
		// After solving, m_mxWarpFactors contains a0, a1, a2, b0, b1, b2, c0, c1.
		private void PreCalc(PointD[] destPoints, PointD[] srcPoints)
		{
			var mxLeft = new GeneralMatrix(8, 8); //mxLeft.Null();
			var 	mxRight = new GeneralMatrix(8, 1);

			var row = 0;

			for (int i = 0; i < 4; i++)
			{
				mxLeft.Array[row][0] = 1.0;
				mxLeft.Array[row][1] = srcPoints[i].X;
				mxLeft.Array[row][2] = srcPoints[i].Y;

				mxLeft.Array[row][6] = - srcPoints[i].X * destPoints[i].X;
				mxLeft.Array[row][7] = - srcPoints[i].Y * destPoints[i].X;

				mxRight.Array[row][0] = destPoints[i].X;

				row++;

				mxLeft.Array[row][3] = 1.0f;
				mxLeft.Array[row][4] = srcPoints[i].X;
				mxLeft.Array[row][5] = srcPoints[i].Y;

				mxLeft.Array[row][6] = - srcPoints[i].X * destPoints[i].Y;
				mxLeft.Array[row][7] = - srcPoints[i].Y * destPoints[i].Y;

				mxRight.Array[row][0] = destPoints[i].Y;

				row++;
			}

			_mxWarpFactors = mxLeft.Solve(mxRight);
		}

        public GyroCal(AHRS sensor)
        {
            int i = 0;

            InitializeComponent();

            this.sensor = sensor;

            // Add DataReceived event handler.
            sensor.DataReceivedEvent += new DataReceivedDelegate(DataReceivedEventHandler);
            sensor.confReceivedEvent += new confReceivedDelegate(confReceivedEventHandler);
            confReceivedEventHandler(i);

            data_collection_enabled = false;
            next_data_index = 0;

            loggedData = new double[SAMPLES, 3];
            threshold = 1.5 * 3.14159 / 180;

            bias = new double[3];

            calMat = new double[3, 3];
            calMat[0, 0] = 1.0;
            calMat[1, 1] = 1.0;
            calMat[2, 2] = 1.0;

            D = new GeneralMatrix(SAMPLES, 10);

            for (i = 0; i < SAMPLES; i++)
            {
                loggedData[i,0] = 0;
                loggedData[i, 1] = 0;
                loggedData[i, 2] = 0;
            }
        }

 public void ArrayMultiplyEquals()
 {
     A = R.Copy();
     GeneralMatrix B = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension);
     A.ArrayMultiplyEquals(B);
     Assert.IsTrue(GeneralTests.Check(A.ArrayRightDivideEquals(B), R));
 }

        /**
        * Computes the Moore–Penrose pseudoinverse using the SVD method.
        *
        * Modified version of the original implementation by Kim van der Linde.
        */
        public static GeneralMatrix pinv(GeneralMatrix x)
        {
            if (x.Rank() < 1)
                return null;

            if (x.ColumnDimension > x.RowDimension)
                return pinv(x.Transpose()).Transpose();

            SingularValueDecomposition svdX = new SingularValueDecomposition(x);
            double[] singularValues = svdX.SingularValues;
            double tol = Math.Max(x.ColumnDimension, x.RowDimension)
                    * singularValues[0] * 2E-16;

            double[] singularValueReciprocals = new double[singularValues.Count()];
            for (int i = 0; i < singularValues.Count(); i++)
                singularValueReciprocals[i] = Math.Abs(singularValues[i]) < tol ? 0
                        : (1.0 / singularValues[i]);

            double[][] u = svdX.GetU().Array;
            double[][] v = svdX.GetV().Array;

            int min = Math.Min(x.ColumnDimension, u[0].Count());

            double[][] inverse = new double[x.ColumnDimension][];

            for (int i = 0; i < x.ColumnDimension; i++) {
                inverse[i] = new double[x.RowDimension];

                for (int j = 0; j < u.Count(); j++)
                    for (int k = 0; k < min; k++)
                        inverse[i][j] += v[i][k] * singularValueReciprocals[k] * u[j][k];
            }
            return new GeneralMatrix(inverse);
        }

        public void FindMinimum()
        {
            int i;
            _xCurrent = FirstStep();
            double [] xPrev = new double[_nDim];
            for (i=0;i<_nDim;i++)
                xPrev[i] = _initial[i];

            double [] xNext;
            GeneralMatrix hPrev = GeneralMatrix.Identity(_nDim,_nDim);
            double currEps=1e5;
            _curIter=0;
            while (currEps>_epsilon && _curIter<_itMax)
            {
                _hessian = CalculateNextHessianApproximation(hPrev,xPrev,_xCurrent,_f.GetGradient(xPrev),_f.GetGradient(_xCurrent));
                xNext = CalculateNextPoint(_xCurrent,_f.GetGradient(_xCurrent),_hessian);
                for (i=0;i<_nDim; i++)
                {
                    xPrev[i] = _xCurrent[i];
                    _xCurrent[i] = xNext[i];
                }
                currEps = Diff(_xCurrent,xPrev);
                _curIter++;
            }
        }

 public void InitData()
 {
     A = new GeneralMatrix(columnwise, validld);
     R = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension);
     S = new GeneralMatrix(columnwise, nonconformld);
     O = new GeneralMatrix(A.RowDimension, A.ColumnDimension, 1.0);
 }

        protected override GeneralMatrix CalculateNextHessianApproximation(GeneralMatrix previousH, 
			double[]prevX, double[]curX, double[]prevGrad, double[]curGrad)
        {
            GeneralMatrix currentH = new GeneralMatrix(_nDim,_nDim);
            GeneralMatrix cX = new GeneralMatrix(curX,_nDim);
            GeneralMatrix pX = new GeneralMatrix(prevX,_nDim);
            GeneralMatrix cG = new GeneralMatrix(curGrad,_nDim);
            GeneralMatrix pG = new GeneralMatrix(prevGrad,_nDim);

            GeneralMatrix dX = cX.Subtract(pX);
            GeneralMatrix dG = cG.Subtract(pG);

            double aK1 = 1/(dX.Transpose().Multiply(dG).GetElement(0,0));
            GeneralMatrix aK2 = dX.Multiply(dX.Transpose());

            GeneralMatrix aK = aK2.Multiply(aK1);

            double bK1 = -1/(dG.Transpose().Multiply(previousH).Multiply(dG).GetElement(0,0));
            GeneralMatrix bK2 = previousH.Multiply(dG).Multiply(dG.Transpose()).Multiply(previousH.Transpose());

            GeneralMatrix bK =bK2.Multiply(bK1);

            currentH = previousH.Add(aK).Add(bK);

            return currentH;
        }

 public void CholeskyDecomposition1()
 {
     double[][] pvals = {new double[]{1.0, 1.0, 1.0}, new double[]{1.0, 2.0, 3.0}, new double[]{1.0, 3.0, 6.0}};
     GeneralMatrix A = new GeneralMatrix(pvals);
     CholeskyDecomposition chol = A.chol();
     GeneralMatrix L = chol.GetL();
     Assert.IsTrue(GeneralTests.Check(A, L.Multiply(L.Transpose())));
 }

 public void CholeskyDecomposition2()
 {
     double[][] pvals = {new double[]{1.0, 1.0, 1.0}, new double[]{1.0, 2.0, 3.0}, new double[]{1.0, 3.0, 6.0}};
     GeneralMatrix A = new GeneralMatrix(pvals);
     CholeskyDecomposition chol = A.chol();
     GeneralMatrix X = chol.Solve(GeneralMatrix.Identity(3, 3));
     Assert.IsTrue(GeneralTests.Check(A.Multiply(X), GeneralMatrix.Identity(3, 3)));
 }

 public GeneralMatrix CalculateHessian(double[]x)
 {
     GeneralMatrix hessian = new GeneralMatrix(Dimension,Dimension);
     for (int i=0; i<Dimension; i++)
         for (int j=0; j<Dimension; j++)
             hessian.SetElement(i,j,GetPartialDerivativeVal(i,j,x));
     return hessian;
 }

        public void Negative_BadColumnIndexSetGoodRowIndexSet()
        {
            double[][] avals = {new double[]{1.0, 4.0, 7.0, 10.0}, new double[]{2.0, 5.0, 8.0, 11.0}, new double[]{3.0, 6.0, 9.0, 12.0}};
            int[] rowindexset = new int[]{1, 2};
            int[] badcolumnindexset = new int[]{1, 2, 4};

            GeneralMatrix B = new GeneralMatrix(avals);
            M = B.GetMatrix(badrowindexset, columnindexset);
        }

 public void EigenValueDecomposition1()
 {
     double[][] pvals = {new double[]{1.0, 1.0, 1.0}, new double[]{1.0, 2.0, 3.0}, new double[]{1.0, 3.0, 6.0}};
     GeneralMatrix A = new GeneralMatrix(pvals);
     EigenvalueDecomposition Eig = A.Eigen();
     GeneralMatrix D = Eig.D;
     GeneralMatrix V = Eig.GetV();
     Assert.IsTrue(GeneralTests.Check(A.Multiply(V), V.Multiply(D)));
 }

        public void Negative_BadColumnIndexSet2()
        {
            int ib = 1, ie = 2; /* index ranges for sub GeneralMatrix */
            double[][] avals = {new double[]{1.0, 4.0, 7.0, 10.0}, new double[]{2.0, 5.0, 8.0, 11.0}, new double[]{3.0, 6.0, 9.0, 12.0}};
            int[] badrowindexset = new int[]{1, 3};

            GeneralMatrix B = new GeneralMatrix(avals);

            M = B.GetMatrix(ib, ie + B.RowDimension + 1, columnindexset);
        }

 //display specified matrix
 static void displayMatrix(GeneralMatrix displayMat)
 {
     for (int i = 0; i < displayMat.RowDimension; i++)
     {
         for (int j = 0; j < displayMat.ColumnDimension; j++)
         {
             Console.Write(displayMat.GetElement(i, j) + ",");
         }
         Console.WriteLine();
     }
 }

        public OneDWrapper(IGradientFunction func, GeneralMatrix pX, GeneralMatrix aX)
        {
            if (pX==null || aX==null)
                throw new ArgumentException("Previous x and alfaX may not be null");

            _problemDimension = pX.RowDimension;

            _function = func;

            _previousX = new GeneralMatrix(pX.ArrayCopy);

            _alfaX = new GeneralMatrix(aX.ArrayCopy);
        }

 public void Hessian()
 {
     double eps = 1e-5;
     double[] vector = {0,0,0};
     double[][] result = {new double[]{10.0, 2.0, -2.0},
                             new double[]{ 2.0, 4.0,  2.0},
                             new double[]{-2.0, 2.0,  4.0}};
     GeneralMatrix expectedMatrix = new GeneralMatrix(result);
     TestFunction f = new TestFunction();
     GeneralMatrix hessian = f.CalculateHessian(vector);
     for (int i=0;i<3; i++)
         for (int j=0;j<3; j++)
             Assert.IsTrue(System.Math.Abs(hessian.GetElement(i,j)-expectedMatrix.GetElement(i,j))<eps);
 }

        /// <summary>QR Decomposition, computed by Householder reflections.</summary>
        /// <param name="A">   Rectangular matrix
        /// </param>
        /// <returns>     Structure to access R and the Householder vectors and compute Q.
        /// </returns>
        public QRDecomposition(GeneralMatrix A)
        {
            // Initialize.
            QR = A.ArrayCopy;
            m = A.RowDimension;
            n = A.ColumnDimension;
            Rdiag = new double[n];

            // Main loop.
            for (int k = 0; k < n; k++)
            {
                // Compute 2-norm of k-th column without under/overflow.
                double nrm = 0;
                for (int i = k; i < m; i++)
                {
                    nrm = Maths.Hypot(nrm, QR[i][k]);
                }

                if (nrm != 0.0)
                {
                    // Form k-th Householder vector.
                    if (QR[k][k] < 0)
                    {
                        nrm = - nrm;
                    }
                    for (int i = k; i < m; i++)
                    {
                        QR[i][k] /= nrm;
                    }
                    QR[k][k] += 1.0;

                    // Apply transformation to remaining columns.
                    for (int j = k + 1; j < n; j++)
                    {
                        double s = 0.0;
                        for (int i = k; i < m; i++)
                        {
                            s += QR[i][k] * QR[i][j];
                        }
                        s = (- s) / QR[k][k];
                        for (int i = k; i < m; i++)
                        {
                            QR[i][j] += s * QR[i][k];
                        }
                    }
                }
                Rdiag[k] = - nrm;
            }
        }

 public Optimizer(int dim, double[]initialPar, IGradientFunction f, double step, double epsilon, int itMax)
 {
     _f = f;
     _epsilon = epsilon;
     _itMax = itMax;
     _nDim = dim;
     _xCurrent = new double[_nDim];
     _initial = new double[_nDim];
     _step = step;
     //initiate the first value of alpha to some random value less than 100;
     Random rnd = new Random();
     _alpha = rnd.NextDouble()*100;
     _hessian = new GeneralMatrix(_nDim,_nDim);
     for (int i=0; i<_nDim; i++)
         _initial[i] = initialPar[i];
 }

		// In bilinear mode, the warping functions are:
		//	x' = a0 + a1 x y + a2 x + a3 y
		//	y' = b0 + b1 x y + b2 x + b3 y
		//
		// Here, we have two sets of four equations. In the first set, the a# factors
		// are the unknowns, in the second set the b# factors.
		// The equations are of the form:
		//	a0		+ xy a1		+ x a2		+ y a3	= x'
		// The left hand side is identical for both sets. The right hand side differs.
		// Therefore, we can solve them in one operation.
		// The left hand side factors are put in the 4x4 matrix mxLeft, the right side
		// factors are put in the 4x2 matrix mxRight.
		// After solving, the first column of m_mxWarpFactors contains a0, a1, a2, a3; the
		// second columne contains b0, b1, b2, b3.
		private void PreCalc(PointD[] destPoints, PointD[] srcPoints)
		{
			var mxLeft = new GeneralMatrix(4, 4);
			var 	mxRight = new GeneralMatrix(4, 2);

			for (int row = 0; row < 4; row++)
			{
				mxLeft.Array[row][0] = 1.0;
				mxLeft.Array[row][1] = srcPoints[row].X * srcPoints[row].Y;
				mxLeft.Array[row][2] = srcPoints[row].X;
				mxLeft.Array[row][3] = srcPoints[row].Y;

				mxRight.Array[row][0] = destPoints[row].X;
				mxRight.Array[row][1] = destPoints[row].Y;
			}

			_mxWarpFactors = mxLeft.Solve(mxRight);
		}

 public double[][] decompose(int svCount, double[][] m)
 {
     GeneralMatrix mMat = new GeneralMatrix(m);
     GeneralMatrix resultM = decompose(svCount, mMat);
     double[][] resultD = new double[resultM.RowDimension][];
     for (int i = 0; i < resultM.RowDimension; i++)
     {
         resultD[i] = new double[resultM.ColumnDimension];
         for (int j = 0; j < resultM.ColumnDimension; j++)
         {
             resultD[j][i] = resultM.GetElement(i, j);
             if ((j + 1) % 4 == 0) // To remove messed up byte
             {
                 resultD[j][i] = 0;
             }
         }
     }
     return resultD;
 }