void Optimizer::FitRest(mat& alpha,
	mat& beta,
	mat& rho,
	mat& lamda,
	InputPtr input,
	ModelPtr model,
	MeshPtr mesh,
	ShapePtr shape,
	TexturePtr texture)
{

	mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
	mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);

	mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
	mat beta_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);


	Rho rho_para(rho, input, model, mesh, shape, texture);
	Lamda lamda_para(lamda, model, mesh, shape, texture);


	// 2500 1000 700 500 300 200

	model->InitialRandomGenerator(ModelImage::REST);

	double weight = 1.0 / 200;


	for (int l = 0; l < 1000; ++l)
	{

		Alpha alpha_para(alpha, input, model, mesh, shape, texture);
		Beta beta_para(beta, model, mesh, shape, texture);

		// Generate random points  
		GenerateRandomPoints(model, GradientRandomNum);

		double function_value = 0;


		for (int i = 0; i < PrincipalNum; ++i)
		{
			double variance = shape->GetVariance(i);
			alpha_gradient[i] = weight * ComputeIntensityGradient(input, &alpha_para, i) + 2 * alpha[i] / variance;
		}

		for (int i = 0; i < PrincipalNum; ++i)
		{
			double variance = texture->GetVariance(i);
			beta_gradient[i] = weight * ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
		}

		alpha -= alpha_para.Step*alpha_gradient;
		beta -= beta_para.Step*beta_gradient;

	}

}
void Optimizer::FitAll(mat& alpha,
	mat& beta,
	mat& rho,
	mat& lamda,
	InputPtr input,
	ModelPtr model,
	MeshPtr mesh,
	ShapePtr shape,
	TexturePtr texture)
{

	mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
	mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);

	mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
	mat beta_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);

	mat rho_gradient = mat(RhoNum, 1, fill::zeros);
	mat rho_hessian_inv = mat(RhoNum, RhoNum, fill::zeros);

	mat lamda_gradient = mat(LamdaNum, 1, fill::zeros);
	mat lamda_hessian_inv = mat(LamdaNum, LamdaNum, fill::zeros);


	vec step(RhoNum, 1);
	step.rows(0, 2).fill(0.00000006);
	step.rows(3, 5).fill(0.0002);
	step.rows(6, 6).fill(2.0);
	mat step_mat = diagmat(step);


	SetStartingValue(rho, lamda);

	// 2500 1000 700 500 300 200
	array<double, 6> weight_intensity = { { 1.0 / 1000, 1.0 / 900, 1.0 / 800, 1.0 / 700, 1.0 / 500, 1.0 / 300 } };
	array<int, 6> para_num = { { 10, 15, 25, 35, 55, 99 } };   // 20 40
	array<int, 6> iterations = { { 1000, 1000, 1000, 1000, 1000, 1000 } };


	//array<double, 4> weight_intensity = { { 1.0 / 900, 1.0 / 700, 1.0 / 500, 1.0 / 400 } };
	//array<int, 4> para_num = { { 10, 20, 40, 99 } };   // 20 40
	//array<int, 4> iterations = { { 1000, 1000, 800, 600 } };


	int counter = 0;
	for (int c = 0; c < 6; ++c)
	{
		for (int l = 0; l < iterations[c]; ++l)
		{

			if (counter == 0 || counter % 1000 == 0)
			{
				Face3dModel face3d_model(shape, texture);
				mesh = face3d_model.Construction(alpha, beta);
				TwoPassZbuffer(rho, lamda, mesh, model);
				model->EnableIterator();
				model->InitialRandomGenerator();

				//GenerateRandomPoints(model, HessianRandomNum);

				//for (int i = 0; i < para_num[c]; ++i)
				//{
				//	double variance = shape->GetVariance(i);

				//	mat alpha1 = alpha;
				//	alpha1[i] -= H;
				//	Alpha alpha_para1(alpha1, input, model, mesh, shape, texture);

				//	double first_derivative1 = weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para1, i) +
				//		weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para1, i);


				//	mat alpha2 = alpha;
				//	alpha2[i] += H;
				//	Alpha alpha_para2(alpha2, input, model, mesh, shape, texture);
				//	double first_derivative2 = weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para2, i) +
				//		weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para2, i);

				//	double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
				//	alpha_hessian_inv(i, i) = 1 / (second_derivative +2 / variance);

				//}


				/*	for (int i = 0; i < para_num[c]; ++i)
					{
					double variance = texture->GetVariance(i);

					mat beta1 = beta;
					beta1[i] -= H;
					Beta beta_para1(beta1, model, mesh, shape, texture);
					double first_derivative1 = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para1, i);

					mat beta2 = beta;
					beta2[i] += H;
					Beta beta_para2(beta2, model, mesh, shape, texture);
					double first_derivative2 = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para2, i);

					double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
					beta_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);

					}*/


				//for (int i = 0; i < RhoNum; ++i)
				//{
				//	double variance = GetRhoVariance(i);

				//	mat rho1 = rho;
				//	rho1[i] -= H;
				//	Rho rho_para1(rho1, input, model, mesh, shape, texture);
				//	double first_derivative1 =/* weight_feature[c] * ComputeLandmarkGradient(input, &rho_para1, i) +*/
				//		weight_intensity[c] * ComputeIntensityGradient(input, &rho_para1, i);

				//	mat rho2 = rho;
				//	rho2[i] += H;
				//	Rho rho_para2(rho2, input, model, mesh, shape, texture);

				//	double first_derivative2 = /*weight_feature[c] * ComputeLandmarkGradient(input, &rho_para2, i) +*/
				//		weight_intensity[c] * ComputeIntensityGradient(input, &rho_para2, i);

				//	double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
				//	rho_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);

				//}


				//for (int i = 0; i < LamdaNum - 7; ++i)
				//{

				//	double variance = GetLamdaVariance(i);
				//	mat lamda1 = lamda;
				//	lamda1[i] -= H;
				//	Lamda lamda_para1(lamda1, model, mesh, shape, texture);
				//	double first_derivative1 = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para1, i);


				//	mat lamda2 = lamda;
				//	lamda2[i] += H;
				//	Lamda lamda_para2(lamda2, model, mesh, shape, texture);
				//	double first_derivative2 = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para2, i);
				//	double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);

				//	lamda_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);

				//}

			}

			Alpha alpha_para(alpha, input, model, mesh, shape, texture);
			Beta beta_para(beta, model, mesh, shape, texture);
			Rho rho_para(rho, input, model, mesh, shape, texture);
			Lamda lamda_para(lamda, model, mesh, shape, texture);

			GenerateRandomPoints(model, GradientRandomNum);
			//double function_value = 0;
			//function_value = ComputeCost(input, model, alpha_para);
			//ofstream cost;
			//cost.open("all_cost", ios::app);
			//cost << function_value << "\n";
			//cost.close();
		

			for (int i = 0; i < para_num[c]; ++i)
			{
				double variance = shape->GetVariance(i);
				alpha_gradient[i] = /*weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para, i) +*/
					weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para, i) +2 * alpha[i] / variance;
			}


			for (int i = 0; i < para_num[c]; ++i)
			{
				double variance = texture->GetVariance(i);
				beta_gradient[i] = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
			}


			for (int i = 0; i < RhoNum; ++i)
			{
				//double variance = GetRhoVariance(i);
				//double mean = GetRhoMean(i);
				rho_gradient[i] = /*weight_feature[c] * ComputeLandmarkGradient(input, &rho_para, i) +*/
					weight_intensity[c] * ComputeIntensityGradient(input, &rho_para, i);// +2 * (rho[i] - mean) / variance;
			}


			for (int i = 0; i < LamdaNum; ++i)
			{
				double variance = GetLamdaVariance(i);
				double mean = GetLamdaMean(i);
				lamda_gradient[i] = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para, i) +2 * (lamda[i] - mean) / variance;
			}


			//alpha -= 0.00003*alpha_hessian_inv*alpha_gradient;  // 0.01  0.00003
			//beta -= 0.01*beta_hessian_inv*beta_gradient;
			//lamda -= 0.03*lamda_hessian_inv*lamda_gradient;
			//rho -= 0.0001*rho_hessian_inv*rho_gradient;

			alpha -= alpha_para.Step*alpha_gradient;
		    beta -= beta_para.Step*beta_gradient;
			rho -= step_mat*rho_gradient;   // gradient descent
			lamda -= lamda_para.Step*lamda_gradient;

			++counter;
		}

		Face3dModel face3d_model(shape, texture);
		mesh = face3d_model.Construction(alpha, beta);
		VisualizeResult(rho, lamda, input, mesh, "segmented", c);
	}
	 
}