/*
 * The MIT License

   Conograph (powder auto-indexing program)

Copyright (c) <2012> <Ryoko Oishi-Tomiyasu, KEK>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
 *
 */
#ifdef _OPENMP
 # include <omp.h>
#endif
#include "../utility_func/chToDouble.hh"
#include "../utility_lattice_reduction/super_basis3.hh"
#include "../utility_lattice_reduction/put_Minkowski_reduced_lattice.hh"
#include "../laue_group/LaueGroup.hh"
#include "../point_group/PGNormalSeriesTray.hh"
#include "../model_function/LatticeDistanceModel.hh"
#include "../zlog/zlog.hh"
#include "gather_additional_Q.hh"
#include "LatticeFigureOfMerit.hh"

const Double LatticeFigureOfMerit::m_cv2 = 9.0;

const NRMat<Int4> LatticeFigureOfMerit::m_tmat_prim_to_face = put_transform_matrix_row3to4( transpose( BravaisType::putTransformMatrixFromPrimitiveToFace() ) );
const NRMat<Int4> LatticeFigureOfMerit::m_tmat_prim_to_body = put_transform_matrix_row3to4( BravaisType::putTransformMatrixFromBodyToPrimitive() );
const NRMat<Int4> LatticeFigureOfMerit::m_tmat_prim_to_rhomhex = put_transform_matrix_row3to4( transpose( BravaisType::putTransformMatrixFromPrimitiveToRhomHex() ) );
const NRMat<Int4> LatticeFigureOfMerit::m_tmat_prim_to_base[3] =
		{
				put_transform_matrix_row3to4( transpose( BravaisType::putTransformMatrixFromPrimitiveToBase(BaseA_Axis) ) ),
				put_transform_matrix_row3to4( transpose( BravaisType::putTransformMatrixFromPrimitiveToBase(BaseB_Axis) ) ),
				put_transform_matrix_row3to4( transpose( BravaisType::putTransformMatrixFromPrimitiveToBase(BaseC_Axis) ) )
		};
const NRMat<Int4> LatticeFigureOfMerit::m_tmat_prim_to_prim = put_transform_matrix_row3to4();

LatticeFigureOfMerit::LatticeFigureOfMerit()
	: m_S_optimized( SymMat43_Double( SymMat<Double>(3), NRMat<Int4>(4,3) ) ), m_S_red(3),
	  m_determ_S_red(0.0)
{
}


LatticeFigureOfMerit::LatticeFigureOfMerit(const Double& rhs)
	: m_S_optimized( SymMat43_Double( SymMat<Double>(3), NRMat<Int4>(4,3) ) ), m_S_red(3),
	  m_determ_S_red(rhs)
{
}


LatticeFigureOfMerit::LatticeFigureOfMerit(const BravaisType& brat,
		const SymMat43_Double& S)
	: m_S_optimized( SymMat43_Double( SymMat<Double>(3), NRMat<Int4>(4,3) ) ), m_S_red(3)
{
	this->setLatticeConstants43(brat, S);
}

#ifdef DEBUG
static bool checkInitialLatticeParameters(
		const BravaisType& brat,
		const SymMat<Double>& S_red)
{
	const SymMat<Double> inv_S_red( Inverse3(S_red) );

	if( brat.enumPointGroup() == C2h_Y && brat.enumBravaisLattice() == Prim )
	{
		assert( inv_S_red(0,2) <= 0.0 &&
				inv_S_red(0,0) * 0.9999 < inv_S_red(2,2)
				&& fabs( inv_S_red(0,2) ) * 1.9999 < inv_S_red(2,2)
				&& fabs( inv_S_red(0,2) ) * 1.9999 < inv_S_red(0,0) );
	}
	else if( brat.enumPointGroup() == C2h_Z && brat.enumBravaisLattice() == Prim )
	{
		assert( inv_S_red(0,1) <= 0.0
				&& inv_S_red(0,0) * 0.9999 < inv_S_red(1,1)
				&& fabs( inv_S_red(0,1) ) * 1.9999 < inv_S_red(0,0)
				&& fabs( inv_S_red(0,1) ) * 1.9999 < inv_S_red(1,1) );
	}
	else if( brat.enumPointGroup() == C2h_X && brat.enumBravaisLattice() == Prim )
	{
		assert( inv_S_red(1,2) <= 0.0
				&& inv_S_red(1,1) * 0.9999 < inv_S_red(2,2)
				&& fabs( inv_S_red(1,2) ) * 1.9999 < inv_S_red(1,1)
				&& fabs( inv_S_red(1,2) ) * 1.9999 < inv_S_red(2,2) );
	}
	else if( brat.enumPointGroup() == C2h_Y && brat.enumBravaisLattice() == BaseZ )
	{
		assert( inv_S_red(0,2) <= 0.0
				&& fabs( inv_S_red(0,2) ) * 0.9999 < inv_S_red(2,2)
				&& fabs( inv_S_red(0,2) ) * 1.9999 < inv_S_red(0,0) );
	}
	else if( brat.enumPointGroup() == C2h_Z && brat.enumBravaisLattice() == BaseX )
	{
		assert( inv_S_red(0,1) <= 0.0
				&& fabs( inv_S_red(0,1) ) * 0.9999 < inv_S_red(0,0)
				&& fabs( inv_S_red(0,1) ) * 1.9999 < inv_S_red(1,1) );
	}
	else if( brat.enumPointGroup() == C2h_X && brat.enumBravaisLattice() == BaseY )
	{
		assert( inv_S_red(1,2) <= 0.0
				&& fabs( inv_S_red(1,2) ) * 0.9999 < inv_S_red(1,1)
				&& fabs( inv_S_red(1,2) ) * 1.9999 < inv_S_red(2,2) );
	}
	else if( brat.enumCrystalSystem() == Orthorhombic_C )
	{
		assert( brat.enumBravaisLattice() == BaseZ );
		assert( inv_S_red(0,0) * 0.9999 < inv_S_red(1,1) );
	}
	else if( brat.enumPointGroup() == D2h && brat.enumBravaisLattice() == Prim )
	{
		assert( inv_S_red(0,0) * 0.9999 < inv_S_red(1,1)
				&& inv_S_red(1,1) * 0.9999 < inv_S_red(2,2) );
	}
	return true;
}
#endif

//static Double put_minimum_lattice_point_distance(const SymMat<Double>& S_super)
//{
//	Double ans = S_super(0,0);
//	if( S_super(1,1) < ans ) ans = S_super(1,1);
//	if( S_super(2,2) < ans ) ans = S_super(2,2);
//	if( S_super(3,3) < ans ) ans = S_super(3,3);
//	const Double S_super01 = S_super(0,0)+S_super(1,1)+S_super(0,1)*2.0;
//	if( S_super01 < ans ) ans = S_super01;
//	const Double S_super02 = S_super(0,0)+S_super(2,2)+S_super(0,2)*2.0;
//	if( S_super02 < ans ) ans = S_super02;
//	const Double S_super12 = S_super(1,1)+S_super(2,2)+S_super(1,2)*2.0;
//	if( S_super12 < ans ) ans = S_super12;
//	return ans;
//}


void putTransformMatrixToMinkowskiReduced(const SymMat<Double>& S, NRMat<Int4>& trans_mat)
{
	assert( S.size() == 3 );

	SymMat<Double> S_super_obtuse(4);
	put_super_Gram_matrix_obtuse_angle<Double, SymMat<Double> >(S, S_super_obtuse, trans_mat);
	moveSmallerDiagonalLeftUpper<Double, SymMat<Double> >(S_super_obtuse, trans_mat);

	// S_red = trans_mat * S_super_obtuse * transpose(trans_mat).
	SymMat<Double> S_red(3);
	NRMat<Int4> trans_mat2;
	putMinkowskiReduced(S_super_obtuse, S_red, trans_mat2);
	trans_mat = mprod( trans_mat2, put_transform_matrix_row4to3(trans_mat) );
}


void LatticeFigureOfMerit::setInverseOfMinkowskiReducedForm(NRMat<Int4>& trans_mat)
{
	if( m_brat.enumCrystalSystem() == Triclinic )
	{
		// trans_mat * Inverse(m_S_optimized.first) * transpose(trans_mat) is Minkowski reduced
		// <=> Inverse of transpose(Inverse(trans_mat)) * m_S_optimized.first * Inverse(trans_mat) is Minkowski reduced.
		putTransformMatrixToMinkowskiReduced(Inverse3(m_S_optimized.first), trans_mat);
		transpose_square_matrix(trans_mat);
		m_S_red = transform_sym_matrix(Inverse3(trans_mat), m_S_optimized.first);
	}
	else
	{
		m_S_red = m_S_optimized.first;
		trans_mat = identity_matrix<Int4>(3);
		if( m_brat.enumCrystalSystem() == Monoclinic_P )
		{
			if( m_brat.enumPointGroup() == C2h_X )
			{
				putMinkowskiReducedMonoclinicP(1, 2, m_S_red, trans_mat);
			}
			else if( m_brat.enumPointGroup() == C2h_Y )
			{
				putMinkowskiReducedMonoclinicP(0, 2, m_S_red, trans_mat);
			}
			else //if( m_brat.enumPointGroup() == C2h_Z )
			{
				putMinkowskiReducedMonoclinicP(0, 1, m_S_red, trans_mat);
			}
		}
		else if( m_brat.enumCrystalSystem() == Monoclinic_B )
		{
			m_S_red = m_S_optimized.first;
			putMinkowskiReducedMonoclinicB(m_brat, m_S_red, trans_mat);
		}
		else if( m_brat.enumPointGroup() == D2h )
		{
			m_S_red = m_S_optimized.first;
			putMinkowskiReducedOrthorhombic(m_brat.enumBravaisLattice(), m_S_red, trans_mat);
		}
	}

	assert( checkInitialLatticeParameters(m_brat, m_S_red) );
}


// This method assumes that S.second * S.first * Transpose(S.second) is obtuse.
void LatticeFigureOfMerit::setLatticeConstants43(const BravaisType& brat, const SymMat43_Double& S)
{
	m_brat = brat;
	m_S_optimized = S;

	NRMat<Int4> trans_mat;
	setInverseOfMinkowskiReducedForm(trans_mat);	// Set m_S_red from m_S_optimized.

	m_determ_S_red = Determinant3( m_S_optimized.first );
	m_figures_of_merit.reset();
}


ZErrorMessage LatticeFigureOfMerit::setLatticeConstants(const BravaisType& brat, const SymMat<Double>& Sval)
{
	assert( Sval.size()==3 );

	SymMat43_Double S_red_optimized = SymMat43_Double(Sval, NRMat<Int4>(4,3));
	cal_average_crystal_system(brat.enumPointGroup(), S_red_optimized.first);
	if( brat.enumBravaisLattice() == Face )
	{
		S_red_optimized.second = m_tmat_prim_to_face;
	}
	else if( brat.enumBravaisLattice() == Inner )
	{
		S_red_optimized.second = m_tmat_prim_to_body;
	}
	else if( brat.enumBravaisLattice() == BaseX
			|| brat.enumBravaisLattice() == BaseY
			|| brat.enumBravaisLattice() == BaseZ )
	{
		S_red_optimized.second = m_tmat_prim_to_base[ (ArrayIndex)brat.enumBASEaxis() ];
	}
	else if( brat.enumBravaisLattice() == Rhom_hex )
	{
		S_red_optimized.second = m_tmat_prim_to_rhomhex;
	}
	else // if( brat.enumBravaisLattice() == Prim )
	{
		S_red_optimized.second = m_tmat_prim_to_prim;
	}

	// S_super_obtuse = trans_mat * S_red.first * Transpose(trans_mat).
	SymMat<Double> S_super_obtuse = transform_sym_matrix(S_red_optimized.second, S_red_optimized.first);
	Int4 itnum;
	if( !put_super_Gram_matrix_obtuse_angle< Double, SymMat<Double> >(S_red_optimized.second, S_super_obtuse, itnum) )
	{
		return ZErrorMessage(ZErrorArgument, "The argument matrix is not positive definite" __FILE__, __LINE__, __FUNCTION__);
	}
	moveSmallerDiagonalLeftUpper< Double, SymMat<Double> >(S_super_obtuse, S_red_optimized.second);

	setLatticeConstants43(brat, S_red_optimized);

	return ZErrorMessage();
}


inline bool checkIfFirstEntryIsPositive(const VecDat3<Int4>& rhs)
{
	for(Int4 i=0; i<3; i++)
	{
		if( rhs[i] == 0 ) continue;
		if( rhs[i] > 0 ) return true;
		else return false;
	}
	return false;
}


void LatticeFigureOfMerit::putMillerIndicesInRange(const Double& qbegin, const Double& qend,
	vector<HKL_Q>& cal_hkl_tray) const
{
	cal_hkl_tray.clear();

	vector<HKL_Q> cal_hkl_tray2;
	gatherQcal(this->putSellingReducedForm(),
				qbegin, qend,
				40, cal_hkl_tray2);

	set< VecDat3<Int4> > found_hkl_tray;
	vector<MillerIndex3> equiv_hkl_tray;
	VecDat3<Int4> hkl;
	
	PGNormalSeriesTray normal_tray(m_brat.enumPointGroup());
	LaueGroup lg(m_brat.enumPointGroup());
	
	for(vector<HKL_Q>::const_iterator it=upper_bound(cal_hkl_tray2.begin(), cal_hkl_tray2.end(), HKL_Q(0, 0.0));
			it<cal_hkl_tray2.end(); it++)
	{
		hkl = product_hkl(it->HKL(), m_S_optimized.second);
		if( found_hkl_tray.find(hkl) != found_hkl_tray.end() )
		{
			continue;
		}
		if( !checkIfFirstEntryIsPositive(hkl) ) hkl *= -1;
		
		normal_tray.putHKLEquiv(MillerIndex3(hkl[0], hkl[1], hkl[2]), equiv_hkl_tray);
#ifdef DEBUG
		if( (Int4)equiv_hkl_tray.size() != lg->LaueMultiplicity(hkl) )
		{
ZLOG_INFO( num2str(hkl[0]) + " "
		+ num2str(hkl[1]) + " "
		+ num2str(hkl[2]) + "\n"
		+ num2str( equiv_hkl_tray.size() ) + "\n"
		+ num2str( lg->LaueMultiplicity(hkl) ) + "\n" );
		}
#endif

		for(vector<MillerIndex3>::const_iterator ithkl=equiv_hkl_tray.begin(); ithkl<equiv_hkl_tray.end(); ithkl++)
		{
			found_hkl_tray.insert( VecDat3<Int4>( (*ithkl)[0], (*ithkl)[1], (*ithkl)[2] ) );
		}
		
		cal_hkl_tray.push_back( HKL_Q(hkl, it->Q()) );
	}
	sort( cal_hkl_tray.begin(), cal_hkl_tray.end() );
}


void LatticeFigureOfMerit::setFigureOfMerit(const Int4& num_ref_figure_of_merit,
		const vector<QData>& qdata,
		vector< VecDat3<Int4> >& closest_hkl_tray,
		Vec_BOOL& Q_observed_flag)
{
	assert( num_ref_figure_of_merit <= (Int4)qdata.size() );

	// Qdata is sorted into ascended order.
	m_figures_of_merit.reset();
	m_figures_of_merit.putNumberOfReflectionsForFigureOfMerit() = num_ref_figure_of_merit;

	const Int4& num_Q = m_figures_of_merit.putNumberOfReflectionsForFigureOfMerit();
	closest_hkl_tray.clear();
	Q_observed_flag.clear();
	closest_hkl_tray.resize(num_Q, 0);
	Q_observed_flag.resize(num_Q, false);

	if( num_Q <= 0 ) return;

	const Double MinQ = qdata[0].q - sqrt( m_cv2 * qdata[0].q_var );
	const Double MaxQ = qdata[num_Q-1].q + sqrt( m_cv2 * qdata[num_Q-1].q_var );
	const SymMat<Double> S_sup( this->putSellingReducedForm() );

	vector<HKL_Q> cal_hkl_tray;
	gatherQcal(S_sup, MinQ, MaxQ, num_Q+1, cal_hkl_tray);
	if( cal_hkl_tray.empty() ) return;

	vector< vector<HKL_Q>::const_iterator > closest_hkl_q_tray;
	associateQcalWithQobs(cal_hkl_tray, qdata.begin(), qdata.begin()+num_Q, closest_hkl_q_tray);
	const vector<HKL_Q>::const_iterator it_begin = closest_hkl_q_tray[0];
	const vector<HKL_Q>::const_iterator it_end = closest_hkl_q_tray[num_Q-1] + 1;
	assert( it_end <= cal_hkl_tray.end() );
	if( it_begin + 1 >= it_end ) return;

	Double diff;
	Double actually_disc = 0.0;
//	Double norm_actually_disc = 0.0;
	Int4 num_q_observed = 0;
	for(Int4 k=0; k<num_Q; k++)
	{
		closest_hkl_tray[k] = product_hkl( closest_hkl_q_tray[k]->HKL(), m_S_optimized.second);
		diff = qdata[k].q - closest_hkl_q_tray[k]->Q();
		actually_disc += fabs( diff );
//		norm_actually_disc += fabs( sqrt3_2(qdata[k].q) - sqrt3_2((*closest_hkl_q_tray[k].rbegin())->Q()) );
		if( diff * diff <= m_cv2 * qdata[k].q_var )
		{
			Q_observed_flag[k] = true;
			num_q_observed++;
		}
		else Q_observed_flag[k] = false;
	}
	actually_disc /= num_Q;
//	norm_actually_disc /= num_Q;
	m_figures_of_merit.putNumQobsAssociatedWithCloseHKL() = num_q_observed;

	vector< vector<QData>::const_iterator > closest_q_tray;
	associateQobsWithQcal(qdata, it_begin, it_end, closest_q_tray);

	const LaueGroup lg(m_brat.enumPointGroup());

	Double inv_mult = 2.0 / lg->LaueMultiplicity( product_hkl(it_begin->HKL(), m_S_optimized.second) );
	Double num_total_hkl = inv_mult;
	Double rev_actually_disc = fabs( it_begin->Q() - closest_q_tray[0]->q ) * inv_mult;

	Double sum_diff = 0.0;
	Int4 index = 1;
	for(vector<HKL_Q>::const_iterator it=it_begin+1; it<it_end; it++, index++)
	{
		inv_mult = 2.0 / lg->LaueMultiplicity( product_hkl(it->HKL(), m_S_optimized.second) );
		num_total_hkl += inv_mult;
		rev_actually_disc += fabs( it->Q() - closest_q_tray[index]->q ) * inv_mult;

		diff = it->Q() - (it-1)->Q();
		sum_diff += diff * diff;
	}
	m_figures_of_merit.putContinuousNumberOfHKLInRange() = num_total_hkl;
	rev_actually_disc /= num_total_hkl;

//	Double sum_diff = 0.0;
//	Int4 disc_num_total_hkl = 1;
//	for(vector<HKL_Q>::const_iterator it=it_begin+1; it<it_end; it++)
//	{
//		diff = it->Q() - (it-1)->Q();
//		sum_diff += diff * diff;
//
//		if( it->Q() <= (it-1)->Q() ) continue;
//		disc_num_total_hkl += 1;
//	}
//	m_figures_of_merit.putDiscreteNumberOfHKLInRange() = disc_num_total_hkl;
//
//	Double rev_sum_diff = 0.0;
//	for(Int4 k=1; k<num_Q; k++)
//	{
//		diff = qdata[k].q - qdata[k-1].q;
//		rev_sum_diff += diff * diff;
//	}

	// Calculate the symmetric figures of merit by Wolff.
	m_figures_of_merit.putFigureOfMeritWolff() = ( (it_end - 1)->Q() - it_begin->Q() ) / ( 2.0*actually_disc*num_total_hkl );
//	m_figures_of_merit.putNormalizedFigureOfMeritWolff() = ( sqrt3_2((it_end - 1)->Q()) - sqrt3_2(it_begin->Q()) ) / ( 2.0*norm_actually_disc*num_total_hkl );
	m_figures_of_merit.putFigureOfMeritWu() = sum_diff / ( 4.0 * actually_disc * ( (it_end - 1)->Q() - it_begin->Q() ) );
	m_figures_of_merit.putReversedFigureOfMerit() = ( qdata[num_Q-1].q - qdata[0].q ) / ( 2.0*rev_actually_disc*num_Q );

//	sum_diff = 0.0;
//	disc_num_total_hkl = 0;
//	for(vector<HKL_Q>::const_iterator it=cal_hkl_tray.begin()+1; it<it_end; it++)
//	{
//		diff = it->Q() - (it-1)->Q();
//		sum_diff += diff * diff;
//
//		if( (it-1)->Q() < it->Q() ) disc_num_total_hkl++;
//	}
//
//	m_figures_of_merit.putFigureOfMeritWolff_Original() = qdata[num_Q-1].q / ( 2.0*actually_disc*disc_num_total_hkl );
//	m_figures_of_merit.putFigureOfMeritWu_Original() = sum_diff / ( 4.0 * actually_disc * (it_end - 1)->Q() );
}




void LatticeFigureOfMerit::setWuFigureOfMerit(const Int4& num_ref_figure_of_merit,
		const vector<QData>& qdata,
		const Double& min_thred_num_hkl,
		const Double& max_thred_num_hkl)
{
	m_figures_of_merit.reset();
	m_figures_of_merit.putNumberOfReflectionsForFigureOfMerit() = min( num_ref_figure_of_merit, (Int4)qdata.size() );
	const Int4& num_Q = m_figures_of_merit.putNumberOfReflectionsForFigureOfMerit();
	if( num_Q <= 0 ) return;

	const Double MinQ = qdata[0].q - sqrt( m_cv2 * qdata[0].q_var );
	const Double MaxQ = qdata[num_Q-1].q + sqrt( m_cv2 * qdata[num_Q-1].q_var );

	const SymMat<Double> S_sup( this->putSellingReducedForm() );

	vector<HKL_Q> cal_hkl_tray;
	gatherQcal(S_sup, MinQ, MaxQ, num_Q+1, cal_hkl_tray);
	if( (Double)cal_hkl_tray.size() < num_Q * min_thred_num_hkl ) return;
	if( (Double)cal_hkl_tray.size() > num_Q * max_thred_num_hkl ) return;

	vector< vector<HKL_Q>::const_iterator > closest_hkl_q_tray;
	associateQcalWithQobs(cal_hkl_tray, qdata.begin(), qdata.begin()+num_Q, closest_hkl_q_tray);
	const vector<HKL_Q>::const_iterator it_begin = closest_hkl_q_tray[0];
	const vector<HKL_Q>::const_iterator it_end = closest_hkl_q_tray[num_Q-1] + 1;
	assert( it_end <= cal_hkl_tray.end() );
	if( it_begin + 1 >= it_end ) return;

	Double actually_disc = 0.0;
	for(Int4 k=0; k<num_Q; k++)
	{
		actually_disc += fabs( qdata[k].q - closest_hkl_q_tray[k]->Q() );
	}
	actually_disc /= num_Q;

	Double sum_diff = 0.0, diff;
	for(vector<HKL_Q>::const_iterator it=it_begin+1; it<it_end; it++)
	{
		diff = it->Q() - (it-1)->Q();
		sum_diff += diff * diff;
	}

	// Calculate the figure of merit by Wu.
	m_figures_of_merit.putFigureOfMeritWu() = sum_diff / ( 4.0 * actually_disc * ( (it_end - 1)->Q() - it_begin->Q() ) );
}


pair<bool, ZErrorMessage> LatticeFigureOfMerit::fitLatticeParameterLinear(
		const vector<QData>& qdata,
		const vector< VecDat3<Int4> >& hkl_to_fit,
		const vector<bool>& fix_or_fit_flag, const bool& output_view_flag)
{
	const ArrayIndex isize = hkl_to_fit.size();
	
	assert( hkl_to_fit.size() == fix_or_fit_flag.size() );
	assert( hkl_to_fit.size() <= qdata.size() );

	Vec_DP ydata(isize), ydata_err(isize);
	Vec_BOOL nxfit(isize);
	Int4 data_num=0;
	
	for(ArrayIndex i=0; i<isize; i++)
	{
		ydata[i] = qdata[i].q;
		ydata_err[i] = sqrt_d( qdata[i].q_var );
		if( ydata_err[i] <= 0.0 )
		{
			nxfit[i] = false;
		}
		else
		{
			nxfit[i] = fix_or_fit_flag[i];
			if( nxfit[i] ) data_num++;
		}
	}
	
	LaueGroup lg(m_brat.enumPointGroup());
	Mat_DP_constr cmat;
	lg->putLatticeConstantFlag(cmat);
	if( data_num <= countNumberOfIndependentParam(cmat.begin(),cmat.end()) )
	{
		return pair<bool, ZErrorMessage>(false, ZErrorMessage("NUMBER OF DATA IS TOO SMALL", __FILE__, __LINE__, __FUNCTION__));
	}
	setIndex(cmat);

	vector<Double> init_param(6);
	const SymMat<Double>& S_val = this->putOptimizedForm().first;
	init_param[0] = S_val(0,0);
	init_param[1] = S_val(1,1);
	init_param[2] = S_val(2,2);
	init_param[3] = S_val(1,2);
	init_param[4] = S_val(0,2);
	init_param[5] = S_val(0,1);
	
	LatticeDistanceModel latModel;
	latModel.setConstraint(&cmat[0]);
	Double chisq_all;
	pair<bool, ZErrorMessage> ans = latModel.setFittedParam(hkl_to_fit, ydata, ydata_err, nxfit,
														output_view_flag, 0.0, 1, init_param, chisq_all);
	if( !(ans.first)) return ans;

	LatticeFigureOfMerit new_lat(*this);
	SymMat<Double> S_red_optimized(3);
	latModel.putResult(S_red_optimized);
	new_lat.setLatticeConstants(m_brat, S_red_optimized);
	new_lat.setFigureOfMerit( m_figures_of_merit.putNumberOfReflectionsForFigureOfMerit(), qdata );

	if( cmpFOMdeWolff(new_lat, *this) )
	{
		*this = new_lat;
		return pair<bool, ZErrorMessage>(true, ZErrorMessage());
	}
	else return pair<bool, ZErrorMessage>(false, ZErrorMessage());
}


void LatticeFigureOfMerit::printLatticeInformation(
					const eABCaxis& abc_axis,
					const eRHaxis& rh_axis,
					const Int4& label_start0,
					ostream* os) const
{
	Int4 label_start = label_start0;
	os->width(label_start);
  	*os << "" << "<CrystalSystem>";
	os->width(17);
	*os << put_cs_name(this->enumCrystalSystem(), abc_axis);
  	*os << " </CrystalSystem>\n\n";

	os->width(label_start); *os << "";
  	*os << "<!-- a, b, c(angstrom), alpha, beta, gamma(deg.)-->\n";

	VecDat3<Double> length_axis, angle_axis;
	if( this->enumCrystalSystem() == Rhombohedral )
	{
		this->putReducedLatticeConstantsDegree(abc_axis, Rho_Axis, length_axis, angle_axis);

		os->width(label_start); *os << "";
		*os << "<ReducedLatticeParameters axis=\"Rhombohedral\">";
	  	os->width(14);
	  	*os << length_axis[0];
	  	os->width(14);
	   	*os << length_axis[1];
	  	os->width(14);
	   	*os << length_axis[2];
	 	os->width(14);
	   	*os << angle_axis[0];
	  	os->width(14);
	   	*os << angle_axis[1];
	  	os->width(14);
	   	*os << angle_axis [2];
	  	*os << " </ReducedLatticeParameters>\n";

		this->putReducedLatticeConstantsDegree(abc_axis, Hex_Axis, length_axis, angle_axis);

		os->width(label_start); *os << "";
	  	*os << "<ReducedLatticeParameters axis=\"Hexagonal\">";
	  	os->width(14);
	  	*os << length_axis[0];
	  	os->width(14);
	   	*os << length_axis[1];
	  	os->width(14);
	   	*os << length_axis[2];
	 	os->width(14);
	   	*os << angle_axis[0];
	  	os->width(14);
	   	*os << angle_axis[1];
	  	os->width(14);
	   	*os << angle_axis[2];
	  	*os << " </ReducedLatticeParameters>\n\n";
	}
	else
	{
		this->putReducedLatticeConstantsDegree(abc_axis, Rho_Axis, length_axis, angle_axis);

		os->width(label_start); *os << "";
		*os << "<ReducedLatticeParameters>";
	  	os->width(14);
	  	*os << length_axis[0];
	  	os->width(14);
	   	*os << length_axis[1];
	  	os->width(14);
	   	*os << length_axis[2];
	 	os->width(14);
	   	*os << angle_axis[0];
	  	os->width(14);
	   	*os << angle_axis[1];
	  	os->width(14);
	   	*os << angle_axis[2];
	  	*os << " </ReducedLatticeParameters>\n";
	}

	this->putOptimizedLatticeConstantsDegree(abc_axis, rh_axis, length_axis, angle_axis);

	os->width(label_start); *os << "";
	*os << "<OptimizedLatticeParameters>";
  	os->width(14);
  	*os << length_axis[0];
  	os->width(14);
   	*os << length_axis[1];
  	os->width(14);
   	*os << length_axis[2];
 	os->width(14);
   	*os << angle_axis[0];
  	os->width(14);
   	*os << angle_axis[1];
  	os->width(14);
   	*os << angle_axis[2];
  	*os << " </OptimizedLatticeParameters>\n\n";

	os->width(label_start); *os << "";
  	if( this->enumCrystalSystem() == Rhombohedral )
  	{
		if( rh_axis == Hex_Axis )
		{
			*os << "<VolumeOfUnitCell axis=\"Hexagonal\">";
	  	  	os->width(14);
	  	  	*os << this->putLatticeVolume();
		}
		else // if( rh_axis == Rho_Axis )
		{
			*os << "<VolumeOfUnitCell axis=\"Rhombohedral\">";
	  	  	os->width(14);
	  	  	*os << this->putLatticeVolume() / 3.0;
		}
  	}
  	else{
  		*os << "<VolumeOfUnitCell>";
  	  	os->width(14);
  	  	*os << this->putLatticeVolume();
  	}
  	*os << " </VolumeOfUnitCell>\n";

  	const SetOfFigureOfMerit& critical_value = this->putFiguresOfMerit();

	os->width(label_start); *os << "";
   	*os << "<FigureOfMeritWolff name=\"" << critical_value.putLabel_FigureOfMeritWolff() << "\">";
  	os->width(14);
   	*os << critical_value.putFigureOfMeritWolff();
//   	*os << " (" << critical_value.putFigureOfMeritWolff_Original() << ")";
	*os << " </FigureOfMeritWolff>\n";

	os->width(label_start);
   	*os << "" << "<FigureOfMeritWu name=\"" << critical_value.putLabel_FigureOfMeritWu() << "\">";
  	os->width(14);
   	*os << critical_value.putFigureOfMeritWu();
//   	*os << " (" << critical_value.putFigureOfMeritWu_Original() << ")";
	*os << " </FigureOfMeritWu>\n";

//	os->width(label_start); *os << "";
//   	*os << "<NormalizedFigureOfMeritWolff name=\"" << critical_value.putLabel_NormalizedFigureOfMeritWolff() << "\">";
//  	os->width(14);
//   	*os << critical_value.putNormalizedFigureOfMeritWolff();
//	*os << " </NormalizedFigureOfMeritWolff>\n";

	os->width(label_start);
   	*os << "" << "<ReversedFigureOfMeritWolff name=\"" << critical_value.putLabel_ReversedFigureOfMeritWolff() << "\">";
  	os->width(14);
   	*os << critical_value.putReversedFigureOfMerit();
	*os << " </ReversedFigureOfMeritWolff>\n";

	os->width(label_start);
   	*os << "" << "<SymmetricFigureOfMeritWolff name=\"" << critical_value.putLabel_SymmetricFigureOfMeritWolff() << "\">";
  	os->width(14);
   	*os << critical_value.putSymmetricFigureOfMerit();
	*os << " </SymmetricFigureOfMeritWolff>\n";

	os->width(label_start);
   	*os << "" << "<!-- Number of reflections up to the ";
   	*os << critical_value.putNumberOfReflectionsForFigureOfMerit() << "th reflection. (The multiplicity of peaks is considered.)-->\n";
	os->width(label_start);
   	*os << "" << "<NumberOfMillerIndicesInRange>";
  	os->width(14);
   	*os << critical_value.putContinuousNumberOfHKLInRange();
   	*os << " </NumberOfMillerIndicesInRange>\n";

   	os->width(label_start);
   	*os << "" << "<NumberOfIndexedPeaks>";
  	os->width(14);
   	*os << critical_value.putNumQobsAssociatedWithCloseHKL();
   	*os << " </NumberOfIndexedPeaks>\n";

   	os->width(label_start);
   	*os << "" << "<NumberOfPeaksNecessaryToResolveDominantZone>";
  	os->width(14);
   	*os << this->checkDominantZone();
   	*os << " </NumberOfPeaksNecessaryToResolveDominantZone>\n\n";
}


void LatticeFigureOfMerit::putLatticeConstantsDegree(const BravaisType& brat, const SymMat<Double>& S0,
		const eABCaxis& axis1,
		const eRHaxis& axis2, VecDat3<Double>& length_axis, VecDat3<Double>& angle_axis)
{
	SymMat<Double> S = S0;
	if( brat.enumCrystalSystem() == Rhombohedral && axis2 != brat.enumRHaxis() )
	{
		if( axis2 == Hex_Axis ) // Rho -> Hex.
		{
			static const FracMat matrix_rho2hex = FInverse3( transpose(BravaisType::putTransformMatrixFromPrimitiveToRhomHex() ) );
			S = transform_sym_matrix(matrix_rho2hex.mat, S)/(matrix_rho2hex.denom*matrix_rho2hex.denom);
		}
		else // if( axis2 == RhoAxis ) // Hex -> Rho.
		{
			static const NRMat<Int4> matrix_hex2rho = transpose( BravaisType::putTransformMatrixFromPrimitiveToRhomHex() );
			S = transform_sym_matrix(matrix_hex2rho, S);
		}
	}
	else if( brat.enumCrystalSystem() == Monoclinic_B )
	{
		const NRMat<Int4> this2output = put_transform_matrix_monoclinic_b(brat.enumABCaxis(), axis1);
		S = transform_sym_matrix(this2output, S);
	}

	calLatticeConstant( S, length_axis, angle_axis );
}



Int4 LatticeFigureOfMerit::checkDominantZone() const
{
	const vector<QData> qdata = VCData::putPeakQData();
	if( qdata.empty() )
	{
		if( this->enumPointGroup() == Ci ) return 6;
		else if( this->enumPointGroup() == C2h_X ||  this->enumPointGroup() == C2h_Y ||  this->enumPointGroup() == C2h_Z ) return 4;
		else if( this->enumPointGroup() == D2h ) return 3;
		else if( this->enumPointGroup() == D4h_Z || this->enumPointGroup() == D31d_rho || this->enumPointGroup() == D6h ) return 2;
		else if( this->enumPointGroup() == Oh ) return 1;
		assert(false);
	}

	const SymMat<Double> S_super = this->putSellingReducedForm();
	const Double max_q = max(
						  max( max( S_super(0,0), S_super(1,1) ), max( S_super(2,2), S_super(3,3) ) ),
						  max( max( S_super(0,0) + S_super(1,1) + S_super(0,1)*2.0,
								 	 S_super(0,0) + S_super(2,2) + S_super(0,2)*2.0 ),
								 	 S_super(1,1) + S_super(2,2) + S_super(1,2)*2.0 ) );

	return distance( qdata.begin(), lower_bound( qdata.begin(), qdata.end(), QData( qdata.begin()->q + max_q, 0.0 ) ) ) + 1;
}
