private void TestGradient()
        {
            currentWinner = new CurrentBestSolution()
            {
                curBestFunction = double.MaxValue
            };
            //pParameters = new double[] { .001013, .1628 };
            List <double> grad = new List <double>(2);

            CalculateZExpectationsScaled();
            grad.Add(0.0);
            grad.Add(0.0);

            double ss = GetDerivativesScaled(pParameters, grad);

            double[] op      = pParameters.ToArray();
            double[] difs    = new double[2];
            double[] obsGrad = grad.ToArray();
            for (int i = 0; i < 2; i++)
            {
                double eps = 1e-8;
                pParameters    = op.ToArray();
                pParameters[i] = op[i] + eps;
                double predict1 = GetDerivativesScaled(pParameters, grad);
                pParameters[i] = op[i] - eps;
                double predict2 = GetDerivativesScaled(pParameters, grad);
                double est      = (predict1 - predict2) / (2 * eps);
                double dif      = est - obsGrad[i];
                difs[i] = dif;
            }
            double q = difs.Sum();

            q = q / 1.0;
        }
        //The M Step - Much better numeric properties if A and R are put on the same scale at the start
        //of the process
        private void MaximizeGivenZExpectationsScaled()
        {
            //Recreating as I didn't know if it remembered the Hessian
            QN = new QuasiNewton();
            QN.MaxIterations = MaxIterations;
            QN.Tolerance     = TerminationTolerance;
            currentWinner    = new CurrentBestSolution()
            {
                curBestFunction = double.MaxValue
            };

            // results = QN.MinimizeDetail(new DiffFunc(GetDerivatives), new double[] {InitialPopSize,GrowthRate});
            results = QN.MinimizeDetail(new DiffFunc(GetDerivativesScaled), startParams);

            pParameters = results.solution;
            curVal      = results.funcValue;
            if (results.quality != Microsoft.SolverFoundation.Solvers.CompactQuasiNewtonSolutionQuality.LocalOptima)
            {
                //Sometimes it went to the local optimum, but didn't seem to think so.
                if (results.quality == Microsoft.SolverFoundation.Solvers.CompactQuasiNewtonSolutionQuality.UserCalculationError &&
                    currentWinner.GradientL2NormAtBest < .002)
                {
                    pParameters = currentWinner.curBestParameters;
                    curVal      = currentWinner.curBestFunction;
                }
                else
                {
                    throw new Exception("M Step Failed to Converge during Solving");
                }
            }
        }
        //Log transform forces growth rate to be positive, as well as init pop size
        private double GetDerivativesLOG(IList <double> value, IList <double> grad)
        {
            double origA = value[(int)ParametersIndex.P0Index];
            double A     = Math.Exp(origA);
            double origr = value[(int)ParametersIndex.rIndex];
            double r     = Math.Exp(origr);

            //First the R Gradient
            double rGradient = 0.0;
            double AGradient = 0.0;
            double ss        = 0.0;

            for (int i = 0; i < xs.Length; i++)
            {
                for (int j = 0; j < 2; j++)
                {
                    double cy   = ys[i];
                    double cx   = xs[i];
                    double cz   = zs[i, j];
                    double csd  = sds[j];
                    double cvd  = Math.Pow(csd, 2.0);
                    double ercx = Math.Exp(r * cx);
                    //ss += -cz*(Math.Log(Passignments[j])-Math.Log(csd)-.5*Math.Pow(cy - A * ercx, 2)/(2*cvd));
                    ss += cz * Math.Pow(cy - A * ercx, 2) / (2 * cvd);
                    double val = (-cz * (cy - A * ercx) * A * cx * ercx * r) / cvd;
                    val       += 1;
                    rGradient += (-cz * (cy - A * ercx) * A * cx * ercx * r) / cvd;
                    AGradient += (-cz * (cy - A * ercx) * A * ercx) / cvd;
                }
            }
            grad[(int)ParametersIndex.P0Index] = AGradient;
            grad[(int)ParametersIndex.rIndex]  = rGradient;
            if (ss > currentWinner.curBestFunction)
            {
                //Qualities.Add(ss);
                var b = grad.ToList();
                b.ForEach(x => Math.Pow(x, 2));
                double norm = (b.Sum() / 2);
                currentWinner = new CurrentBestSolution()
                {
                    curBestFunction = ss, curBestParameters = value.ToArray(), GradientL2NormAtBest = norm
                };
            }
            return(ss);
        }
        private double GetDerivativesScaled(IList <double> value, IList <double> grad)
        {
            double A = value[(int)ParametersIndex.P0Index] / AConditioningScale;
            double r = value[(int)ParametersIndex.rIndex];

            //First the R Gradient
            double rGradient = 0.0;
            double AGradient = 0.0;
            double ss        = 0.0;

            for (int i = 0; i < xs.Length; i++)
            {
                for (int j = 0; j < 2; j++)
                {
                    double cy   = ys[i];
                    double cx   = xs[i];
                    double cz   = zs[i, j];
                    double csd  = sds[j];
                    double cvd  = Math.Pow(csd, 2.0);
                    double ercx = Math.Exp(r * cx);
                    ss        += cz * Math.Pow(cy - A * ercx, 2) / (2 * cvd);
                    cvd       *= AConditioningScale;
                    rGradient += (-cz * (cy - A * ercx) * A * AConditioningScale * cx * ercx) / cvd;
                    AGradient += (-cz * (cy - A * ercx) * ercx) / cvd;
                }
            }
            grad[(int)ParametersIndex.P0Index] = AGradient;
            grad[(int)ParametersIndex.rIndex]  = rGradient;
            if (currentWinner == null || ss < currentWinner.curBestFunction)
            {
                var b  = grad.ToList();
                var qq = from x in b select Math.Pow(x, 2);

                double norm = (qq.Sum() / 2);
                currentWinner = new CurrentBestSolution()
                {
                    curBestFunction = ss, curBestParameters = value.ToArray(), GradientL2NormAtBest = norm
                };
            }
            return(ss);
        }
        //The M Step
        private void MaximizeGivenZExpectations()
        {
            //Solver doesn't work well, but is here.
            //LBFGS solver = new LBFGS(2);
            //FunctionEval ff = new FunctionEval(GetDerivativesLBFGS);
            //Vector x0 = Vector.FromArray(new double[] { InitialPopSize, GrowthRate });
            //solver.debug = true;
            //solver.linesearchDebug = true;
            //solver.Run(x0,1, ff);
            //pParameters = x0.ToArray();
            //var b = solver.convergenceCriteria;

            //Recreating as I didn't know if it remembered the Hessian
            QN = new QuasiNewton();
            QN.MaxIterations = MaxIterations;
            QN.Tolerance     = TerminationTolerance;
            currentWinner    = new CurrentBestSolution()
            {
                curBestFunction = double.MaxValue
            };

            // results = QN.MinimizeDetail(new DiffFunc(GetDerivatives), new double[] {InitialPopSize,GrowthRate});
            results = QN.MinimizeDetail(new DiffFunc(GetDerivatives), startParams);

            pParameters = results.solution;
            curVal      = results.funcValue;
            if (results.quality != Microsoft.SolverFoundation.Solvers.CompactQuasiNewtonSolutionQuality.LocalOptima)
            {
                //Sometimes it went to the local optimum, but didn't seem to think so.
                if (results.quality == Microsoft.SolverFoundation.Solvers.CompactQuasiNewtonSolutionQuality.UserCalculationError &&
                    currentWinner.GradientL2NormAtBest < .002)
                {
                    pParameters = currentWinner.curBestParameters;
                    curVal      = currentWinner.curBestFunction;
                }
                else
                {
                    throw new Exception("M Step Failed to Converge during Solving");
                }
            }
        }